Pileus Git - ~andy/iBeaconNav/blob - src/edu/ucla/iBeaconNav/Sensors.java

   1 package edu.ucla.iBeaconNav;
   2
   3 import java.util.LinkedList;
   4 import java.util.Queue;
   5
   6 import android.app.Notification;
   7 import android.app.Activity;
   8 import android.app.Service;
   9 import android.content.Context;
  10 import android.content.Intent;
  11 import android.hardware.Sensor;
  12 import android.hardware.SensorEvent;
  13 import android.hardware.SensorEventListener;
  14 import android.hardware.SensorManager;
  15 import android.os.Handler;
  16 import android.os.IBinder;
  17 import android.os.Message;
  18 import android.os.Messenger;
  19 import android.os.SystemClock;
  20 import android.widget.EditText;
  21 import edu.ucla.iBeaconNav.R;
  22
  23 public class Sensors extends Service implements SensorEventListener
  24 {
  25         /* Private data */
  26         private SensorManager sensorManager;
  27         private Sensor        accSensor;
  28         private Sensor        grvSensor;
  29         private Sensor        magSensor;
  30         private Sensor        gyrSensor;
  31
  32         private Messenger     messenger;
  33         private boolean       active;
  34         private long          lastTime_ms    = 0;
  35         private float         snsInterval_ms = 0;
  36         private long          lastTime_ns    = 0;
  37         private long          snsInterval_ns = 0;
  38
  39
  40         /* Sensor data */
  41         private float[] accValues        = new float[3];
  42         private float[] magValues        = new float[3];
  43         private float[] gyrValues        = new float[3];
  44         private float[] rotationMatrix_R = new float[9];
  45         private float[] rtMatrixStable_R = new float[9];
  46         private float[] rotationMatrix_I = new float[9];
  47         private float[] orientValues_rd  = new float[3];
  48         private float[] orientValues     = new float[3];
  49         private float[] accWorldCoord    = {0,0,0};
  50         private float[] gyrWorldCoord    = {0,0,0};
  51
  52
  53         /* Auxiliary Variables for Sensor Processing */
  54
  55         private final int cycle = 32;
  56
  57         private LinkedList<Float> accBuffers    = new LinkedList<Float>();
  58         private LinkedList<Float> magBuffers    = new LinkedList<Float>();
  59         private LinkedList<Float> gyrBuffers    = new LinkedList<Float>();
  60         private LinkedList<Float> orientBuffers = new LinkedList<Float>();
  61         private LinkedList<Float> rtMtrxBuffers = new LinkedList<Float>();
  62         private float[] data        = new float[4];
  63         private float[] accSum      = {0,0,0};
  64     private float[] accAvg      = {0,0,0};
  65         private float[] magSum      = {0,0,0};
  66         private float[] gyrSum      = {0,0,0};
  67         private float[] gyrOffset   = {0,0,0};
  68         private float[] orientSum   = {0,0,0};
  69         private float[] rtMtrxSum   = {0,0,0,
  70                                                0,0,0,
  71                                                0,0,0};
  72         private float[] accBuffer   = {0,0,0};
  73         private float[] magBuffer   = {0,0,0};
  74         private float[] orientBuffer= {0,0,0};
  75         private int     cnt         = 0;
  76         private int     accCnt      = 0;
  77         private int     magCnt      = 0;
  78         private int     gyrCnt      = 0;
  79         private int     orientCnt   = 0;
  80         private int     rtMtrxCnt   = 0;
  81
  82         private boolean ifGyrOffset = false;
  83         private boolean ifGrvOffset = false;
  84         private boolean ifStable    = false;
  85
  86         private final float EPSILON       = (float)0.01;
  87
  88         private float   accValueTotal     = 0;
  89         private float   calculatedGravity = SensorManager.GRAVITY_EARTH;
  90         private float   gravityRef        = 0;
  91         private float   gyroscopeRef      = 0;
  92         private boolean ifSetGrvRef       = false;
  93
  94
  95         /* Position Related Stuff */
  96         //private float   startPosX      = 0;
  97         //private float   startPos       = 0;
  98         private float   currentPosX    = 0;
  99         private float   currentPosY    = 0;
 100         // rotate around gravity direction, positive when counterclock-wise, initially align with user's first step
 101         private float   currentHeading = 0;
 102         private float   headingOffset  = 0;
 103         private float   stepLength     = (float)0.5;    // in m
 104         private int     stepCount      = 0;
 105         private boolean stepStart      = false;
 106
 107
 108         /* Integrated positions */
 109         private Vect    position       = new Vect();
 110         private Quat    rotation       = new Quat();
 111
 112         /* Test data */
 113         private long    time1hz        = 0;
 114         private Vect    gyr1hz         = new Vect();
 115
 116         /* Private methods */
 117         private void tellMain(CMD.Response cmd, Object value)
 118         {
 119                 try {
 120                         android.os.Message msg = android.os.Message.obtain();
 121                         msg.what = cmd.ordinal();
 122                         msg.obj  = value;
 123                         this.messenger.send(msg);
 124                 } catch (Exception e) {
 125                         Util.debug("Sensors: error sending message", e);
 126                 }
 127         }
 128
 129         private void notify(String text, int icon)
 130         {
 131                 // Notify Main
 132                 this.tellMain(CMD.Response.NOTIFY, text);
 133
 134                 // Notification bar
 135                 //Notification  note   = new Notification(icon, null, 0);
 136                 //Intent        intent = new Intent(this, Main.class);
 137                 //PendingIntent pend   = PendingIntent.getActivity(this, 0, intent, 0);
 138                 //note.setLatestEventInfo(this, "iBeaconNav!", text, pend);
 139                 //PendingIntent pend = PendingIntent.getActivity(this, 0, intent, 0);
 140
 141                 Notification  note = new Notification.Builder(this)
 142                         .setContentTitle("iBeaconNav!")
 143                         .setContentText("iBeaconNav!")
 144                         .setSmallIcon(icon)
 145                         .build();
 146
 147                 this.startForeground(1, note);
 148         }
 149
 150         private void handle(CMD.Command cmd, Messenger mgr)
 151         {
 152                 // Validate messenger
 153                 if (cmd != CMD.Command.REGISTER && mgr != null && mgr != this.messenger)
 154                         Util.debug("Sensors: handle - invalid messenger");
 155
 156                 // Handle the command
 157                 switch (cmd) {
 158                         // Setup communication with Main
 159                         case REGISTER:
 160                                 Util.debug("Sensors: handle - register");
 161                                 this.messenger = mgr;
 162                                 break;
 163
 164                         // Create client thread
 165                         case CONNECT:
 166                                 Util.debug("Sensors: handle - connect");
 167                                 sensorManager.registerListener(this, accSensor, SensorManager.SENSOR_DELAY_FASTEST);
 168                                 sensorManager.registerListener(this, grvSensor, SensorManager.SENSOR_DELAY_FASTEST);
 169                                 sensorManager.registerListener(this, magSensor, SensorManager.SENSOR_DELAY_FASTEST);
 170                                 sensorManager.registerListener(this, gyrSensor, SensorManager.SENSOR_DELAY_FASTEST);
 171                                 break;
 172
 173                         // Stop client thread
 174                         case DISCONNECT:
 175                                 Util.debug("Sensors: handle - register");
 176                                 // TODO
 177                                 break;
 178
 179                         // Reset heading
 180                         case RSTHEAD:
 181                                 Util.debug("Sensors: handle - reset heading");
 182                                 this.rotation = new Quat();
 183                                 currentHeading = 0;
 184                                 displayData(CMD.Data.HEADING);
 185                                 break;
 186
 187                         // Reset distance
 188                         case RSTDST:
 189                                 Util.debug("Sensors: handle - reset distance");
 190                                 currentPosX = 0;
 191                                 currentPosY = 0;
 192                                 displayData(CMD.Data.POSITION);
 193                                 break;
 194                 }
 195         }
 196
 197         /* Public methods */
 198         public boolean isRunning()
 199         {
 200                 return true; // TODO
 201         }
 202
 203         /* Service Methods */
 204         @Override
 205         public void onCreate()
 206         {
 207                 Util.debug("Sensors: onCreate");
 208                 super.onCreate();
 209                 sensorManager =  (SensorManager) getSystemService(Context.SENSOR_SERVICE);
 210                 accSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
 211                 grvSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
 212                 magSensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
 213                 gyrSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
 214         }
 215
 216         @Override
 217         public void onDestroy()
 218         {
 219                 Util.debug("Sensors: onDestroy");
 220                 //this.handle(CMD.Response.DISCONNECT, null);
 221         }
 222
 223         @Override
 224         public int onStartCommand(Intent intent, int flags, int startId)
 225         {
 226                 Util.debug("Sensors: onStartCommand");
 227                 int         rval = super.onStartCommand(intent, flags, startId);
 228                 CMD.Command cmd  = (CMD.Command)intent.getExtras().get("Command");
 229                 Messenger   mgr  = (Messenger)intent.getExtras().get("Messenger");
 230                 this.handle(cmd, mgr);
 231                 return rval;
 232         }
 233
 234         @Override
 235         public IBinder onBind(Intent intent)
 236         {
 237                 Util.debug("Sensors: onBind");
 238                 return messenger.getBinder();
 239         }
 240         class IncomingHandler extends Handler{
 241                 @Override
 242                 public void handleMessage(Message msg) {
 243                         //Util.debug("Sensors: MSG HANDLERRRRRRRRR");
 244                         super.handleMessage(msg);
 245                 }
 246         }
 247
 248         @Override
 249         public void onAccuracyChanged(Sensor sensor, int accuracy) {
 250                 // TODO Auto-generated method stub
 251                 Util.debug("Sensors: onAccuracyChanged");
 252
 253         }
 254
 255         @Override
 256         public void onSensorChanged(SensorEvent event) {
 257                 // TODO Auto-generated method stub
 258                 switch(event.sensor.getType()){
 259                 case Sensor.TYPE_ACCELEROMETER:
 260                         accCnt++;
 261                         System.arraycopy(event.values, 0, accValues, 0, 3);
 262                         //accCnt = everyAveragedBuffer(accBuffer, accValues, 3, accCnt, cycle, CMD.Data.ACC);
 263                         forwardAveragedBuffer(accBuffers, accSum, accAvg, accValues, 3, accCnt, cycle, CMD.Data.ACC);
 264                         toWorldCoordinates(accWorldCoord, accValues, rotationMatrix_R, accCnt, CMD.Data.WRDACC);
 265                         accValueTotal = accWorldCoord[2];
 266                         if (ifSetGrvRef && !ifGrvOffset && Math.abs((accValueTotal-gravityRef)/gravityRef)<0.05){
 267                                 rtMatrixStable_R  = rotationMatrix_R;
 268                                 calculatedGravity = accValueTotal;
 269                                 ifGrvOffset = true;
 270                                 ifGyrOffset = false;
 271                         }
 272                         else if(!ifSetGrvRef && accCnt>400){
 273                                 ifSetGrvRef = true;
 274                                 gravityRef  = accValueTotal;
 275                         }
 276                         updateOrientation();
 277                         break;
 278                 case Sensor.TYPE_MAGNETIC_FIELD:
 279                         magCnt++;
 280                         System.arraycopy(event.values, 0, magValues, 0, 3);
 281                         //magCnt = everyAveragedBuffer(magBuffer, magValues, 3, magCnt, cycle, CMD.Data.MAG);
 282                         forwardAveragedBuffer(magBuffers, magSum, null, magValues, 3, magCnt, cycle, CMD.Data.MAG);
 283                         updateOrientation();       // This maybe useless
 284                         break;
 285                 case Sensor.TYPE_GYROSCOPE:
 286                         gyrCnt++;
 287                         float gyrTemp[] = new float[3];
 288                         long currentTime_ns = event.timestamp;
 289                         snsInterval_ns = currentTime_ns-lastTime_ns;
 290                         lastTime_ns    = currentTime_ns;
 291
 292                         int chkIntCnt = 8;                              //check Interval Count
 293                         if (gyrCnt%chkIntCnt==0){
 294                                 //long currentTime_ms = SystemClock.elapsedRealtime();
 295                                 //snsInterval_ms = (float)(currentTime_ms-lastTime_ms)/chkIntCnt;
 296                                 if(ifSetGrvRef){
 297                                         ifStable = isStable(accBuffers, 3, cycle/2) && isStable(gyrBuffers, 3, cycle/2);
 298                                 }
 299                                 if (ifStable){
 300                                         ifGrvOffset = false;
 301                                 }
 302                                 //lastTime_ms = SystemClock.elapsedRealtime();
 303                                 //Util.debug("Interal in ms: "+Float.toString(snsInterval_ms));
 304                         }
 305
 306                         System.arraycopy(event.values, 0, gyrValues, 0, 3);
 307                         System.arraycopy(gyrValues, 0, gyrTemp, 0, 3);
 308                         forwardAveragedBuffer(gyrBuffers, gyrSum, null, gyrValues, 3, gyrCnt, 2*cycle, CMD.Data.GYR);
 309                         System.arraycopy(gyrTemp, 0, gyrValues, 0, 3);
 310                         if (!ifGyrOffset && ifGrvOffset){
 311                                 //Util.debug("[GYR1] Reset  "+Float.toString(gyrValues[2]));
 312                                 gyrOffset[0] = gyrValues[0];                    //x
 313                                 gyrOffset[1] = gyrValues[1];                    //y
 314                                 gyrOffset[2] = gyrValues[2];                    //z
 315                                 ifGyrOffset = true;
 316                         }
 317                         depleteOffset(gyrValues, gyrOffset, 3);
 318                         //cycleFloatArray(gyrValues, 3);
 319                         toWorldCoordinates(gyrWorldCoord, gyrValues, rtMatrixStable_R, gyrCnt, CMD.Data.WRDGYR);
 320
 321                         float gyrZ = gyrWorldCoord[2];
 322                         //if (!ifStable && ifGyrOffset && Math.abs(gyrZ)>0.05){
 323                         if (ifGyrOffset){
 324                                 //currentHeading+=gyrZ*snsInterval_ms/1000*180/Math.PI;
 325                                 currentHeading += gyrZ*snsInterval_ns/1000000000*180/Math.PI;
 326                                 currentHeading%=360;
 327                                 displayData(CMD.Data.HEADING);
 328                         }
 329
 330                         // Integrate rotations
 331                         Vect rpy = new Vect(event.values[1], event.values[0], -event.values[2]);
 332                         rpy.mul((double)snsInterval_ns/1E9);
 333                         Quat rot = new Quat(rpy.x, rpy.y, rpy.z);
 334                         this.rotation.mul(rot);
 335                         displayData(CMD.Data.ROTATION);
 336
 337                         // Calculate 1 second gryo data
 338                         this.gyr1hz.add(rpy);
 339                         if (currentTime_ns > time1hz+1E9) {
 340                                 displayData(CMD.Data.GYR1HZ);
 341                                 this.gyr1hz.set(0, 0, 0);
 342                                 time1hz = currentTime_ns;
 343                         }
 344
 345                         break;
 346
 347
 348                 }
 349                 processSensorInfo();
 350         }
 351
 352         private void cycleFloatArray(float[] array, int length){
 353                 float temp = array[length-1];
 354                 for(int i=1; i<length; i++){
 355                         array[i] = array[i-1];
 356                 }
 357                 array[0]=temp;
 358         }
 359
 360         private void depleteOffset(float[] values, float[] offset, int len){
 361                 for(int i=0; i<len; i++){
 362                         values[i]-=offset[i];
 363                 }
 364         }
 365
 366         private boolean isStable(LinkedList<Float> buffer,  int length, int cycle){
 367                 int len = buffer.size();
 368                 float[] avrg   = new float[length];
 369                 float[] devSum = new float[length];
 370                 float   ttDev  = 0;
 371                 for(int i=0; i<length; i++){
 372                         avrg[length-1-i]=0;
 373                         for (int j=0; j<cycle; j++){
 374                                 avrg[length-1-i]+=buffer.get(len-1-j*length-i);
 375                         }
 376                         avrg[length-1-i]/=cycle;
 377                 }
 378                 for (int i=0; i<length; i++){
 379                         devSum[i] = 0;
 380                         for(int j=0; j<cycle; j++){
 381                                 devSum[length-1-i]+=(float) Math.pow(buffer.get(len-1-j*length-i)-avrg[length-1-i],2);
 382                         }
 383                         devSum[length-1-i] = (float) Math.sqrt(devSum[length-1-i]/cycle);
 384                 }
 385
 386                 for (int i=0; i<length; i++){
 387                         ttDev+=devSum[i];
 388                 }
 389
 390                 if (ttDev>0.1){
 391                         //Util.debug("[DEV] unStable");
 392                         displayData(CMD.Data.STABLE);
 393                         return false;
 394                 }else{
 395                         //Util.debug("[DEV] Stable");
 396                         displayData(CMD.Data.STABLE);
 397                         return true;
 398                 }
 399         }
 400
 401
 402
 403         private void processSensorInfo(){
 404                 displayData(CMD.Data.STPCNT);
 405                 float epsl = (float)0.6;
 406                 // on step count????
 407                 if (stepStart){
 408                         //judge if stop
 409                         if(accValueTotal-calculatedGravity>epsl){
 410                                 stepStart = false;
 411                                 stepCount++;
 412                                 currentPosX += stepLength*Math.sin(currentHeading);
 413                                 currentPosY += stepLength*Math.cos(currentHeading);
 414                                 displayData(CMD.Data.POSITION);
 415                         }
 416
 417                 }else{
 418                         //judge if start
 419                         if (calculatedGravity-accValueTotal>epsl ){
 420                                 stepStart = true;
 421                         }
 422                 }
 423         }
 424
 425
 426
 427         private void updateOrientation(){
 428                 if (accValues == null || magValues == null){
 429                         return;
 430                 }
 431                 float R[] = new float[9];
 432                 float I[] = new float[9];
 433                 boolean success;
 434                 success = SensorManager.getRotationMatrix(R, I, accValues, magValues);
 435                 if(success){
 436                         orientCnt++;
 437                         rtMtrxCnt++;
 438                         rotationMatrix_R = R;
 439                         rotationMatrix_I = I;
 440                         SensorManager.getOrientation(rotationMatrix_R, orientValues_rd);
 441                 }
 442                 // to degree
 443                 orientValues[0] = (float) (orientValues_rd[0]*180/Math.PI);
 444                 orientValues[1] = (float) (orientValues_rd[1]*180/Math.PI);
 445                 orientValues[2] = (float) (orientValues_rd[2]*180/Math.PI);
 446
 447                 //orientCnt=everyAveragedBuffer(orientBuffer, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
 448                 forwardAveragedBuffer(rtMtrxBuffers,rtMtrxSum, null, rotationMatrix_R, 9, rtMtrxCnt, 2*cycle, null);
 449                 forwardAveragedBuffer(orientBuffers,orientSum, null, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
 450                 //displayData(CMD.Data.ORIENT);
 451         }
 452
 453         private void toWorldCoordinates(float[] worldCoord, float[] values, float[] rotationMatrix, int cnt, CMD.Data cmd){
 454                 float result[] = new float[3];
 455                 result = new Matrix(rotationMatrix).multipleV(values);
 456                 worldCoord[0] = result[0];
 457                 worldCoord[1] = result[1];
 458                 worldCoord[2] = result[2];
 459                 if(cnt%32==0){
 460                         displayData(cmd);
 461                 }
 462         }
 463
 464         private void displayData(CMD.Data dataType){
 465                 //Util.debug("Sensors: displayData");
 466                 float data[] = new float[4];
 467                 data[0] = dataType.ordinal();
 468                 switch(dataType) {
 469                 case ACC:
 470                         data[1] = accValues[0];
 471                         data[2] = accValues[1];
 472                         data[3] = accValues[2];
 473                         break;
 474                 case MAG:
 475                         data[1] = magValues[0];
 476                         data[2] = magValues[1];
 477                         data[3] = magValues[2];
 478                         break;
 479                 case ORIENT:
 480                         data[1] = orientValues[0];
 481                         data[2] = orientValues[1];
 482                         data[3] = orientValues[2];
 483                         break;
 484                 case WRDACC:
 485                         data[1] = accWorldCoord[0];
 486                         data[2] = accWorldCoord[1];
 487                         data[3] = accWorldCoord[2];
 488                         break;
 489                 case STPCNT:
 490                         data[1] = stepCount;
 491                         data[2] = calculatedGravity;
 492                         break;
 493                 case POSITION:
 494                         data[1] = currentPosX;
 495                         data[2] = currentPosY;
 496                         break;
 497                 case ROTATION:
 498                         Vect rpy = new Vect();
 499                         this.rotation.get(rpy);
 500                         data[1] = (float)(rpy.x * (180/Math.PI));
 501                         data[2] = (float)(rpy.y * (180/Math.PI));
 502                         data[3] = (float)(rpy.z * (180/Math.PI));
 503                         break;
 504                 case GYR:
 505                         data[1] = gyrValues[0];
 506                         data[2] = gyrValues[1];
 507                         data[3] = gyrValues[2];
 508                         break;
 509                 case GYR1HZ:
 510                         data[1] = (float)(gyr1hz.x * (180/Math.PI) * 60 * 60);
 511                         data[2] = (float)(gyr1hz.y * (180/Math.PI) * 60 * 60);
 512                         data[3] = (float)(gyr1hz.z * (180/Math.PI) * 60 * 60);
 513                         break;
 514                 case HEADING:
 515                         data[1] = currentHeading;
 516                         break;
 517                 case WRDGYR:
 518                         data[1] = gyrWorldCoord[0];
 519                         break;
 520                 case STABLE:
 521                         data[1] = ifStable?1:0;
 522                         break;
 523                 default:
 524                         Util.debug("Bad Data Sending Command!");
 525                         return;
 526                 }
 527
 528                 this.tellMain(CMD.Response.SHOWDATA, data);
 529         }
 530
 531         private int everyAveragedBuffer(float[] buffer, float[] values, int length, int cnt, int cycle, CMD.Data cmd){
 532                 for(int i=0; i<length; i++){
 533                         buffer[i]+=values[i];
 534                 }
 535                 if (cnt == cycle){
 536                         cnt = 0;
 537                         for(int i=0; i<length; i++){
 538                                 values[i] = buffer[i]/cycle;
 539                                 buffer[i] = 0;
 540                         }
 541                         displayData(cmd);
 542                 }
 543                 return cnt;
 544         }
 545
 546         private void forwardAveragedBuffer(LinkedList<Float> buffer, float[] sum, float[] avg,
 547                         float[] values, int length, int cnt, int cycle, CMD.Data cmd){
 548                 if (buffer==null||buffer.size()<cycle*length){
 549                         for(int i=0; i<length; i++){
 550                                 buffer.addLast(values[i]);
 551                                 sum[i]+=values[i];
 552                                 values[i] = sum[i]/buffer.size();
 553                         }
 554                 }
 555                 else{
 556                         float[] discarded = new float[length];
 557                         for(int i=0; i<length; i++){
 558                                 discarded[i]= buffer.removeFirst();
 559                                 buffer.addLast(values[i]);
 560                                 sum[i]-=discarded[i];
 561                                 sum[i]+=values[i];
 562                                 values[i]=sum[i]/cycle;
 563                         }
 564                 }
 565                 if(avg!=null){
 566                         for(int i=0; i<length; i++){
 567                                 avg[i]=sum[i]/cycle;
 568                         }
 569                 }
 570                 if (cnt%32==0 && cmd != null){
 571                         displayData(cmd);
 572                 }
 573         }
 574
 575         public void printMatrix(String s, Matrix m){
 576                 Util.debug("Sensor: ["+s+"] "+m.mValue[0]+" "+m.mValue[1]+" "+m.mValue[2]);
 577                 Util.debug("Sensor:       "  +m.mValue[3]+" "+m.mValue[4]+" "+m.mValue[5]);
 578                 Util.debug("Sensor:       "  +m.mValue[6]+" "+m.mValue[7]+" "+m.mValue[8]);
 579         }
 580
 581         public void printVector(String s, float[] v){
 582                 Util.debug("Sensor: ["+s+"] "+v[0]+" "+v[1]+" "+v[2]);
 583         }
 584 }