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
 109         /* Private methods */
 110         private void tellMain(CMD.Response cmd, Object value)
 111         {
 112                 try {
 113                         android.os.Message msg = android.os.Message.obtain();
 114                         msg.what = cmd.ordinal();
 115                         msg.obj  = value;
 116                         this.messenger.send(msg);
 117                 } catch (Exception e) {
 118                         Util.debug("Sensors: error sending message", e);
 119                 }
 120         }
 121
 122         private void notify(String text, int icon)
 123         {
 124                 // Notify Main
 125                 this.tellMain(CMD.Response.NOTIFY, text);
 126
 127                 // Notification bar
 128                 //Notification  note   = new Notification(icon, null, 0);
 129                 //Intent        intent = new Intent(this, Main.class);
 130                 //PendingIntent pend   = PendingIntent.getActivity(this, 0, intent, 0);
 131                 //note.setLatestEventInfo(this, "iBeaconNav!", text, pend);
 132                 //PendingIntent pend = PendingIntent.getActivity(this, 0, intent, 0);
 133
 134                 Notification  note = new Notification.Builder(this)
 135                         .setContentTitle("iBeaconNav!")
 136                         .setContentText("iBeaconNav!")
 137                         .setSmallIcon(icon)
 138                         .build();
 139
 140                 this.startForeground(1, note);
 141         }
 142
 143         private void handle(CMD.Command cmd, Messenger mgr)
 144         {
 145                 // Validate messenger
 146                 if (cmd != CMD.Command.REGISTER && mgr != null && mgr != this.messenger)
 147                         Util.debug("Sensors: handle - invalid messenger");
 148
 149                 // Handle the command
 150                 switch (cmd) {
 151                         // Setup communication with Main
 152                         case REGISTER:
 153                                 Util.debug("Sensors: handle - register");
 154                                 this.messenger = mgr;
 155                                 break;
 156
 157                         // Create client thread
 158                         case CONNECT:
 159                                 Util.debug("Sensors: handle - connect");
 160                                 sensorManager.registerListener(this, accSensor, SensorManager.SENSOR_DELAY_FASTEST);
 161                                 sensorManager.registerListener(this, grvSensor, SensorManager.SENSOR_DELAY_FASTEST);
 162                                 sensorManager.registerListener(this, magSensor, SensorManager.SENSOR_DELAY_FASTEST);
 163                                 sensorManager.registerListener(this, gyrSensor, SensorManager.SENSOR_DELAY_FASTEST);
 164                                 break;
 165
 166                         // Stop client thread
 167                         case DISCONNECT:
 168                                 Util.debug("Sensors: handle - register");
 169                                 // TODO
 170                                 break;
 171
 172                         // Reset heading
 173                         case RSTHEAD:
 174                                 Util.debug("Sensors: handle - reset heading");
 175                                 currentHeading = 0;
 176                                 displayData(CMD.Data.HEADING);
 177                                 break;
 178
 179                         // Reset distance
 180                         case RSTDST:
 181                                 Util.debug("Sensors: handle - reset distance");
 182                                 currentPosX = 0;
 183                                 currentPosY = 0;
 184                                 displayData(CMD.Data.POSITION);
 185                                 break;
 186                 }
 187         }
 188
 189         /* Public methods */
 190         public boolean isRunning()
 191         {
 192                 return true; // TODO
 193         }
 194
 195         /* Service Methods */
 196         @Override
 197         public void onCreate()
 198         {
 199                 Util.debug("Sensors: onCreate");
 200                 super.onCreate();
 201                 sensorManager =  (SensorManager) getSystemService(Context.SENSOR_SERVICE);
 202                 accSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
 203                 grvSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
 204                 magSensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
 205                 gyrSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
 206         }
 207
 208         @Override
 209         public void onDestroy()
 210         {
 211                 Util.debug("Sensors: onDestroy");
 212                 //this.handle(CMD.Response.DISCONNECT, null);
 213         }
 214
 215         @Override
 216         public int onStartCommand(Intent intent, int flags, int startId)
 217         {
 218                 Util.debug("Sensors: onStartCommand");
 219                 int         rval = super.onStartCommand(intent, flags, startId);
 220                 CMD.Command cmd  = (CMD.Command)intent.getExtras().get("Command");
 221                 Messenger   mgr  = (Messenger)intent.getExtras().get("Messenger");
 222                 this.handle(cmd, mgr);
 223                 return rval;
 224         }
 225
 226         @Override
 227         public IBinder onBind(Intent intent)
 228         {
 229                 Util.debug("Sensors: onBind");
 230                 return messenger.getBinder();
 231         }
 232         class IncomingHandler extends Handler{
 233                 @Override
 234                 public void handleMessage(Message msg) {
 235                         //Util.debug("Sensors: MSG HANDLERRRRRRRRR");
 236                         super.handleMessage(msg);
 237                 }
 238         }
 239
 240         @Override
 241         public void onAccuracyChanged(Sensor sensor, int accuracy) {
 242                 // TODO Auto-generated method stub
 243                 Util.debug("Sensors: onAccuracyChanged");
 244
 245         }
 246
 247         @Override
 248         public void onSensorChanged(SensorEvent event) {
 249                 // TODO Auto-generated method stub
 250                 switch(event.sensor.getType()){
 251                 case Sensor.TYPE_ACCELEROMETER:
 252                         accCnt++;
 253                         System.arraycopy(event.values, 0, accValues, 0, 3);
 254                         //accCnt = everyAveragedBuffer(accBuffer, accValues, 3, accCnt, cycle, CMD.Data.ACC);
 255                         forwardAveragedBuffer(accBuffers, accSum, accAvg, accValues, 3, accCnt, cycle, CMD.Data.ACC);
 256                         toWorldCoordinates(accWorldCoord, accValues, rotationMatrix_R, accCnt, CMD.Data.WRDACC);
 257                         accValueTotal = accWorldCoord[2];
 258                         if (ifSetGrvRef && !ifGrvOffset && Math.abs((accValueTotal-gravityRef)/gravityRef)<0.05){
 259                                 rtMatrixStable_R  = rotationMatrix_R;
 260                                 calculatedGravity = accValueTotal;
 261                                 ifGrvOffset = true;
 262                                 ifGyrOffset = false;
 263                         }
 264                         else if(!ifSetGrvRef && accCnt>400){
 265                                 ifSetGrvRef = true;
 266                                 gravityRef  = accValueTotal;
 267                         }
 268                         updateOrientation();
 269                         break;
 270                 case Sensor.TYPE_MAGNETIC_FIELD:
 271                         magCnt++;
 272                         System.arraycopy(event.values, 0, magValues, 0, 3);
 273                         //magCnt = everyAveragedBuffer(magBuffer, magValues, 3, magCnt, cycle, CMD.Data.MAG);
 274                         forwardAveragedBuffer(magBuffers, magSum, null, magValues, 3, magCnt, cycle, CMD.Data.MAG);
 275                         updateOrientation();       // This maybe useless
 276                         break;
 277                 case Sensor.TYPE_GYROSCOPE:
 278                         gyrCnt++;
 279                         float gyrTemp[] = new float[3];
 280                         long currentTime_ns = event.timestamp;
 281                         snsInterval_ns = currentTime_ns-lastTime_ns;
 282                         lastTime_ns    = currentTime_ns;
 283
 284                         int chkIntCnt = 8;                              //check Interval Count
 285                         if (gyrCnt%chkIntCnt==0){
 286                                 //long currentTime_ms = SystemClock.elapsedRealtime();
 287                                 //snsInterval_ms = (float)(currentTime_ms-lastTime_ms)/chkIntCnt;
 288                                 if(ifSetGrvRef){
 289                                         ifStable = isStable(accBuffers, 3, cycle/2) && isStable(gyrBuffers, 3, cycle/2);
 290                                 }
 291                                 if (ifStable){
 292                                         ifGrvOffset = false;
 293                                 }
 294                                 //lastTime_ms = SystemClock.elapsedRealtime();
 295                                 //Util.debug("Interal in ms: "+Float.toString(snsInterval_ms));
 296                         }
 297
 298                         System.arraycopy(event.values, 0, gyrValues, 0, 3);
 299                         System.arraycopy(gyrValues, 0, gyrTemp, 0, 3);
 300                         forwardAveragedBuffer(gyrBuffers, gyrSum, null, gyrValues, 3, gyrCnt, 2*cycle, CMD.Data.GYR);
 301                         System.arraycopy(gyrTemp, 0, gyrValues, 0, 3);
 302                         if (!ifGyrOffset && ifGrvOffset){
 303                                 //Util.debug("[GYR1] Reset  "+Float.toString(gyrValues[2]));
 304                                 gyrOffset[0] = gyrValues[0];                    //x
 305                                 gyrOffset[1] = gyrValues[1];                    //y
 306                                 gyrOffset[2] = gyrValues[2];                    //z
 307                                 ifGyrOffset = true;
 308                         }
 309                         depleteOffset(gyrValues, gyrOffset, 3);
 310                         //cycleFloatArray(gyrValues, 3);
 311                         toWorldCoordinates(gyrWorldCoord, gyrValues, rtMatrixStable_R, gyrCnt, CMD.Data.WRDGYR);
 312
 313                         float gyrZ = gyrWorldCoord[2];
 314                         //if (!ifStable && ifGyrOffset && Math.abs(gyrZ)>0.05){
 315                         if (ifGyrOffset){
 316                                 //currentHeading+=gyrZ*snsInterval_ms/1000*180/Math.PI;
 317                                 currentHeading += gyrZ*snsInterval_ns/1000000000*180/Math.PI;
 318                                 currentHeading%=360;
 319                                 displayData(CMD.Data.HEADING);
 320                         }
 321                         break;
 322
 323
 324                 }
 325                 processSensorInfo();
 326         }
 327
 328         private void cycleFloatArray(float[] array, int length){
 329                 float temp = array[length-1];
 330                 for(int i=1; i<length; i++){
 331                         array[i] = array[i-1];
 332                 }
 333                 array[0]=temp;
 334         }
 335
 336         private void depleteOffset(float[] values, float[] offset, int len){
 337                 for(int i=0; i<len; i++){
 338                         values[i]-=offset[i];
 339                 }
 340         }
 341
 342         private boolean isStable(LinkedList<Float> buffer,  int length, int cycle){
 343                 int len = buffer.size();
 344                 float[] avrg   = new float[length];
 345                 float[] devSum = new float[length];
 346                 float   ttDev  = 0;
 347                 for(int i=0; i<length; i++){
 348                         avrg[length-1-i]=0;
 349                         for (int j=0; j<cycle; j++){
 350                                 avrg[length-1-i]+=buffer.get(len-1-j*length-i);
 351                         }
 352                         avrg[length-1-i]/=cycle;
 353                 }
 354                 for (int i=0; i<length; i++){
 355                         devSum[i] = 0;
 356                         for(int j=0; j<cycle; j++){
 357                                 devSum[length-1-i]+=(float) Math.pow(buffer.get(len-1-j*length-i)-avrg[length-1-i],2);
 358                         }
 359                         devSum[length-1-i] = (float) Math.sqrt(devSum[length-1-i]/cycle);
 360                 }
 361
 362                 for (int i=0; i<length; i++){
 363                         ttDev+=devSum[i];
 364                 }
 365
 366                 if (ttDev>0.1){
 367                         //Util.debug("[DEV] unStable");
 368                         displayData(CMD.Data.STABLE);
 369                         return false;
 370                 }else{
 371                         //Util.debug("[DEV] Stable");
 372                         displayData(CMD.Data.STABLE);
 373                         return true;
 374                 }
 375         }
 376
 377
 378
 379         private void processSensorInfo(){
 380                 displayData(CMD.Data.STPCNT);
 381                 float epsl = (float)0.6;
 382                 // on step count????
 383                 if (stepStart){
 384                         //judge if stop
 385                         if(accValueTotal-calculatedGravity>epsl){
 386                                 stepStart = false;
 387                                 stepCount++;
 388                                 currentPosX += stepLength*Math.sin(currentHeading);
 389                                 currentPosY += stepLength*Math.cos(currentHeading);
 390                                 displayData(CMD.Data.POSITION);
 391                         }
 392
 393                 }else{
 394                         //judge if start
 395                         if (calculatedGravity-accValueTotal>epsl ){
 396                                 stepStart = true;
 397                         }
 398                 }
 399         }
 400
 401
 402
 403         private void updateOrientation(){
 404                 if (accValues == null || magValues == null){
 405                         return;
 406                 }
 407                 float R[] = new float[9];
 408                 float I[] = new float[9];
 409                 boolean success;
 410                 success = SensorManager.getRotationMatrix(R, I, accValues, magValues);
 411                 if(success){
 412                         orientCnt++;
 413                         rtMtrxCnt++;
 414                         rotationMatrix_R = R;
 415                         rotationMatrix_I = I;
 416                         SensorManager.getOrientation(rotationMatrix_R, orientValues_rd);
 417                 }
 418                 // to degree
 419                 orientValues[0] = (float) (orientValues_rd[0]*180/Math.PI);
 420                 orientValues[1] = (float) (orientValues_rd[1]*180/Math.PI);
 421                 orientValues[2] = (float) (orientValues_rd[2]*180/Math.PI);
 422
 423                 //orientCnt=everyAveragedBuffer(orientBuffer, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
 424                 forwardAveragedBuffer(rtMtrxBuffers,rtMtrxSum, null, rotationMatrix_R, 9, rtMtrxCnt, 2*cycle, null);
 425                 forwardAveragedBuffer(orientBuffers,orientSum, null, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
 426                 //displayData(CMD.Data.ORIENT);
 427         }
 428
 429         private void toWorldCoordinates(float[] worldCoord, float[] values, float[] rotationMatrix, int cnt, CMD.Data cmd){
 430                 float result[] = new float[3];
 431                 result = new Matrix(rotationMatrix).multipleV(values);
 432                 worldCoord[0] = result[0];
 433                 worldCoord[1] = result[1];
 434                 worldCoord[2] = result[2];
 435                 if(cnt%32==0){
 436                         displayData(cmd);
 437                 }
 438         }
 439
 440         private void displayData(CMD.Data dataType){
 441                 //Util.debug("Sensors: displayData");
 442                 float data[] = new float[4];
 443                 data[0] = dataType.ordinal();
 444                 switch(dataType) {
 445                 case ACC:
 446                         data[1] = accValues[0];
 447                         data[2] = accValues[1];
 448                         data[3] = accValues[2];
 449                         break;
 450                 case MAG:
 451                         data[1] = magValues[0];
 452                         data[2] = magValues[1];
 453                         data[3] = magValues[2];
 454                         break;
 455                 case ORIENT:
 456                         data[1] = orientValues[0];
 457                         data[2] = orientValues[1];
 458                         data[3] = orientValues[2];
 459                         break;
 460                 case WRDACC:
 461                         data[1] = accWorldCoord[0];
 462                         data[2] = accWorldCoord[1];
 463                         data[3] = accWorldCoord[2];
 464                         break;
 465                 case STPCNT:
 466                         data[1] = stepCount;
 467                         data[2] = calculatedGravity;
 468                         break;
 469                 case POSITION:
 470                         data[1] = currentPosX;
 471                         data[2] = currentPosY;
 472                         break;
 473                 case GYR:
 474                         data[1] = gyrValues[0];
 475                         data[2] = gyrValues[1];
 476                         data[3] = gyrValues[2];
 477                         break;
 478                 case HEADING:
 479                         data[1] = currentHeading;
 480                         break;
 481                 case WRDGYR:
 482                         data[1] = gyrWorldCoord[0];
 483                         break;
 484                 case STABLE:
 485                         data[1] = ifStable?1:0;
 486                         break;
 487                 default:
 488                         Util.debug("Bad Data Sending Command!");
 489                         return;
 490                 }
 491
 492                 this.tellMain(CMD.Response.SHOWDATA, data);
 493         }
 494
 495         private int everyAveragedBuffer(float[] buffer, float[] values, int length, int cnt, int cycle, CMD.Data cmd){
 496                 for(int i=0; i<length; i++){
 497                         buffer[i]+=values[i];
 498                 }
 499                 if (cnt == cycle){
 500                         cnt = 0;
 501                         for(int i=0; i<length; i++){
 502                                 values[i] = buffer[i]/cycle;
 503                                 buffer[i] = 0;
 504                         }
 505                         displayData(cmd);
 506                 }
 507                 return cnt;
 508         }
 509
 510         private void forwardAveragedBuffer(LinkedList<Float> buffer, float[] sum, float[] avg,
 511                         float[] values, int length, int cnt, int cycle, CMD.Data cmd){
 512                 if (buffer==null||buffer.size()<cycle*length){
 513                         for(int i=0; i<length; i++){
 514                                 buffer.addLast(values[i]);
 515                                 sum[i]+=values[i];
 516                                 values[i] = sum[i]/buffer.size();
 517                         }
 518                 }
 519                 else{
 520                         float[] discarded = new float[length];
 521                         for(int i=0; i<length; i++){
 522                                 discarded[i]= buffer.removeFirst();
 523                                 buffer.addLast(values[i]);
 524                                 sum[i]-=discarded[i];
 525                                 sum[i]+=values[i];
 526                                 values[i]=sum[i]/cycle;
 527                         }
 528                 }
 529                 if(avg!=null){
 530                         for(int i=0; i<length; i++){
 531                                 avg[i]=sum[i]/cycle;
 532                         }
 533                 }
 534                 if (cnt%32==0 && cmd != null){
 535                         displayData(cmd);
 536                 }
 537         }
 538
 539         public void printMatrix(String s, Matrix m){
 540                 Util.debug("Sensor: ["+s+"] "+m.mValue[0]+" "+m.mValue[1]+" "+m.mValue[2]);
 541                 Util.debug("Sensor:       "  +m.mValue[3]+" "+m.mValue[4]+" "+m.mValue[5]);
 542                 Util.debug("Sensor:       "  +m.mValue[6]+" "+m.mValue[7]+" "+m.mValue[8]);
 543         }
 544
 545         public void printVector(String s, float[] v){
 546                 Util.debug("Sensor: ["+s+"] "+v[0]+" "+v[1]+" "+v[2]);
 547         }
 548 }