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.SystemClock;
  19 import android.widget.EditText;
  20 import edu.ucla.iBeaconNav.R;
  21
  22 public class Sensors implements SensorEventListener
  23 {
  24         /* Private data */
  25         private Task          task;
  26
  27         private SensorManager sensorManager;
  28         private Sensor        accSensor;
  29         private Sensor        grvSensor;
  30         private Sensor        magSensor;
  31         private Sensor        gyrSensor;
  32
  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         /* Position Related Stuff */
  95         //private float   startPosX      = 0;
  96         //private float   startPos       = 0;
  97         private float   currentPosX    = 0;
  98         private float   currentPosY    = 0;
  99         // rotate around gravity direction, positive when counterclock-wise, initially align with user's first step
 100         private float   currentHeading = 0;
 101         private float   headingOffset  = 0;
 102         private float   stepLength     = (float)0.5;    // in m
 103         private int     stepCount      = 0;
 104         private boolean stepStart      = false;
 105
 106
 107         /* Integrated positions */
 108         private Vect    position       = new Vect();
 109         private Quat    rotation       = new Quat();
 110
 111         /* Test data */
 112         private long    time1hz        = 0;
 113         private Vect    gyr1hz         = new Vect();
 114
 115         /* Public methods */
 116         public Sensors(Task task)
 117         {
 118                 Util.debug("Sensors: constructor");
 119                 this.task          = task;
 120                 this.sensorManager = (SensorManager)task.getSystemService(Context.SENSOR_SERVICE);
 121
 122                 // Get sensors
 123                 this.accSensor = sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
 124                 this.grvSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GRAVITY);
 125                 this.magSensor = sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
 126                 this.gyrSensor = sensorManager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);
 127         }
 128
 129         public void connect() {
 130                 // Connect sensor listeners
 131                 sensorManager.registerListener(this, accSensor, SensorManager.SENSOR_DELAY_FASTEST);
 132                 sensorManager.registerListener(this, grvSensor, SensorManager.SENSOR_DELAY_FASTEST);
 133                 sensorManager.registerListener(this, magSensor, SensorManager.SENSOR_DELAY_FASTEST);
 134                 sensorManager.registerListener(this, gyrSensor, SensorManager.SENSOR_DELAY_FASTEST);
 135         }
 136
 137         public void disconnect() {
 138                 // Remove sensor listeners
 139                 sensorManager.unregisterListener(this, accSensor);
 140                 sensorManager.unregisterListener(this, grvSensor);
 141                 sensorManager.unregisterListener(this, magSensor);
 142                 sensorManager.unregisterListener(this, gyrSensor);
 143         }
 144
 145         public void reset_heading() {
 146                 Util.debug("Sensors: handle - reset heading");
 147                 this.rotation = new Quat();
 148                 currentHeading = 0;
 149                 displayData(CMD.Data.HEADING);
 150         }
 151
 152         public void reset_distance() {
 153                 Util.debug("Sensors: handle - reset distance");
 154                 currentPosX = 0;
 155                 currentPosY = 0;
 156                 displayData(CMD.Data.POSITION);
 157         }
 158
 159         /* Sensor Event Listener functions */
 160         @Override
 161         public void onAccuracyChanged(Sensor sensor, int accuracy) {
 162                 Util.debug("Sensors: onAccuracyChanged");
 163         }
 164
 165         @Override
 166         public void onSensorChanged(SensorEvent event) {
 167                 switch(event.sensor.getType()){
 168                 case Sensor.TYPE_ACCELEROMETER:
 169                         accCnt++;
 170                         System.arraycopy(event.values, 0, accValues, 0, 3);
 171                         //accCnt = everyAveragedBuffer(accBuffer, accValues, 3, accCnt, cycle, CMD.Data.ACC);
 172                         forwardAveragedBuffer(accBuffers, accSum, accAvg, accValues, 3, accCnt, cycle, CMD.Data.ACC);
 173                         toWorldCoordinates(accWorldCoord, accValues, rotationMatrix_R, accCnt, CMD.Data.WRDACC);
 174                         accValueTotal = accWorldCoord[2];
 175                         if (ifSetGrvRef && !ifGrvOffset && Math.abs((accValueTotal-gravityRef)/gravityRef)<0.05){
 176                                 rtMatrixStable_R  = rotationMatrix_R;
 177                                 calculatedGravity = accValueTotal;
 178                                 ifGrvOffset = true;
 179                                 ifGyrOffset = false;
 180                         }
 181                         else if(!ifSetGrvRef && accCnt>400){
 182                                 ifSetGrvRef = true;
 183                                 gravityRef  = accValueTotal;
 184                         }
 185                         updateOrientation();
 186                         break;
 187                 case Sensor.TYPE_MAGNETIC_FIELD:
 188                         magCnt++;
 189                         System.arraycopy(event.values, 0, magValues, 0, 3);
 190                         //magCnt = everyAveragedBuffer(magBuffer, magValues, 3, magCnt, cycle, CMD.Data.MAG);
 191                         forwardAveragedBuffer(magBuffers, magSum, null, magValues, 3, magCnt, cycle, CMD.Data.MAG);
 192                         updateOrientation();       // This maybe useless
 193                         break;
 194                 case Sensor.TYPE_GYROSCOPE:
 195                         gyrCnt++;
 196                         float gyrTemp[] = new float[3];
 197                         long currentTime_ns = event.timestamp;
 198                         snsInterval_ns = currentTime_ns-lastTime_ns;
 199                         lastTime_ns    = currentTime_ns;
 200
 201                         int chkIntCnt = 8;                              //check Interval Count
 202                         if (gyrCnt%chkIntCnt==0){
 203                                 //long currentTime_ms = SystemClock.elapsedRealtime();
 204                                 //snsInterval_ms = (float)(currentTime_ms-lastTime_ms)/chkIntCnt;
 205                                 if(ifSetGrvRef){
 206                                         ifStable = isStable(accBuffers, 3, cycle/2) && isStable(gyrBuffers, 3, cycle/2);
 207                                 }
 208                                 if (ifStable){
 209                                         ifGrvOffset = false;
 210                                 }
 211                                 //lastTime_ms = SystemClock.elapsedRealtime();
 212                                 //Util.debug("Interal in ms: "+Float.toString(snsInterval_ms));
 213                         }
 214
 215                         System.arraycopy(event.values, 0, gyrValues, 0, 3);
 216                         System.arraycopy(gyrValues, 0, gyrTemp, 0, 3);
 217                         forwardAveragedBuffer(gyrBuffers, gyrSum, null, gyrValues, 3, gyrCnt, 2*cycle, CMD.Data.GYR);
 218                         System.arraycopy(gyrTemp, 0, gyrValues, 0, 3);
 219                         if (!ifGyrOffset && ifGrvOffset){
 220                                 //Util.debug("[GYR1] Reset  "+Float.toString(gyrValues[2]));
 221                                 gyrOffset[0] = gyrValues[0];                    //x
 222                                 gyrOffset[1] = gyrValues[1];                    //y
 223                                 gyrOffset[2] = gyrValues[2];                    //z
 224                                 ifGyrOffset = true;
 225                         }
 226                         depleteOffset(gyrValues, gyrOffset, 3);
 227                         //cycleFloatArray(gyrValues, 3);
 228                         toWorldCoordinates(gyrWorldCoord, gyrValues, rtMatrixStable_R, gyrCnt, CMD.Data.WRDGYR);
 229
 230                         float gyrZ = gyrWorldCoord[2];
 231                         //if (!ifStable && ifGyrOffset && Math.abs(gyrZ)>0.05){
 232                         if (ifGyrOffset){
 233                                 //currentHeading+=gyrZ*snsInterval_ms/1000*180/Math.PI;
 234                                 currentHeading += gyrZ*snsInterval_ns/1000000000*180/Math.PI;
 235                                 currentHeading%=360;
 236                                 displayData(CMD.Data.HEADING);
 237                         }
 238
 239                         // Integrate rotations
 240                         Vect rpy = new Vect(event.values[1], event.values[0], -event.values[2]);
 241                         rpy.mul((double)snsInterval_ns/1E9);
 242                         Quat rot = new Quat(rpy.x, rpy.y, rpy.z);
 243                         this.rotation.mul(rot);
 244                         displayData(CMD.Data.ROTATION);
 245
 246                         // Calculate 1 second gryo data
 247                         this.gyr1hz.add(rpy);
 248                         if (currentTime_ns > time1hz+1E9) {
 249                                 displayData(CMD.Data.GYR1HZ);
 250                                 this.gyr1hz.set(0, 0, 0);
 251                                 time1hz = currentTime_ns;
 252                         }
 253
 254                         break;
 255
 256
 257                 }
 258                 processSensorInfo();
 259         }
 260
 261         /* Private helper functions */
 262         private void cycleFloatArray(float[] array, int length){
 263                 float temp = array[length-1];
 264                 for(int i=1; i<length; i++){
 265                         array[i] = array[i-1];
 266                 }
 267                 array[0]=temp;
 268         }
 269
 270         private void depleteOffset(float[] values, float[] offset, int len){
 271                 for(int i=0; i<len; i++){
 272                         values[i]-=offset[i];
 273                 }
 274         }
 275
 276         private boolean isStable(LinkedList<Float> buffer,  int length, int cycle){
 277                 int len = buffer.size();
 278                 float[] avrg   = new float[length];
 279                 float[] devSum = new float[length];
 280                 float   ttDev  = 0;
 281                 for(int i=0; i<length; i++){
 282                         avrg[length-1-i]=0;
 283                         for (int j=0; j<cycle; j++){
 284                                 avrg[length-1-i]+=buffer.get(len-1-j*length-i);
 285                         }
 286                         avrg[length-1-i]/=cycle;
 287                 }
 288                 for (int i=0; i<length; i++){
 289                         devSum[i] = 0;
 290                         for(int j=0; j<cycle; j++){
 291                                 devSum[length-1-i]+=(float) Math.pow(buffer.get(len-1-j*length-i)-avrg[length-1-i],2);
 292                         }
 293                         devSum[length-1-i] = (float) Math.sqrt(devSum[length-1-i]/cycle);
 294                 }
 295
 296                 for (int i=0; i<length; i++){
 297                         ttDev+=devSum[i];
 298                 }
 299
 300                 if (ttDev>0.1){
 301                         //Util.debug("[DEV] unStable");
 302                         displayData(CMD.Data.STABLE);
 303                         return false;
 304                 }else{
 305                         //Util.debug("[DEV] Stable");
 306                         displayData(CMD.Data.STABLE);
 307                         return true;
 308                 }
 309         }
 310
 311         private void processSensorInfo(){
 312                 displayData(CMD.Data.STPCNT);
 313                 float epsl = (float)0.6;
 314                 // on step count????
 315                 if (stepStart){
 316                         //judge if stop
 317                         if(accValueTotal-calculatedGravity>epsl){
 318                                 stepStart = false;
 319                                 stepCount++;
 320                                 currentPosX += stepLength*Math.sin(currentHeading);
 321                                 currentPosY += stepLength*Math.cos(currentHeading);
 322                                 displayData(CMD.Data.POSITION);
 323                         }
 324
 325                 }else{
 326                         //judge if start
 327                         if (calculatedGravity-accValueTotal>epsl ){
 328                                 stepStart = true;
 329                         }
 330                 }
 331         }
 332
 333         private void updateOrientation(){
 334                 if (accValues == null || magValues == null){
 335                         return;
 336                 }
 337                 float R[] = new float[9];
 338                 float I[] = new float[9];
 339                 boolean success;
 340                 success = SensorManager.getRotationMatrix(R, I, accValues, magValues);
 341                 if(success){
 342                         orientCnt++;
 343                         rtMtrxCnt++;
 344                         rotationMatrix_R = R;
 345                         rotationMatrix_I = I;
 346                         SensorManager.getOrientation(rotationMatrix_R, orientValues_rd);
 347                 }
 348                 // to degree
 349                 orientValues[0] = (float) (orientValues_rd[0]*180/Math.PI);
 350                 orientValues[1] = (float) (orientValues_rd[1]*180/Math.PI);
 351                 orientValues[2] = (float) (orientValues_rd[2]*180/Math.PI);
 352
 353                 //orientCnt=everyAveragedBuffer(orientBuffer, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
 354                 forwardAveragedBuffer(rtMtrxBuffers,rtMtrxSum, null, rotationMatrix_R, 9, rtMtrxCnt, 2*cycle, null);
 355                 forwardAveragedBuffer(orientBuffers,orientSum, null, orientValues, 3, orientCnt, 2*cycle, CMD.Data.ORIENT);
 356                 //displayData(CMD.Data.ORIENT);
 357         }
 358
 359         private void toWorldCoordinates(float[] worldCoord, float[] values, float[] rotationMatrix, int cnt, CMD.Data cmd){
 360                 float result[] = new float[3];
 361                 result = new Matrix(rotationMatrix).multipleV(values);
 362                 worldCoord[0] = result[0];
 363                 worldCoord[1] = result[1];
 364                 worldCoord[2] = result[2];
 365                 if(cnt%32==0){
 366                         displayData(cmd);
 367                 }
 368         }
 369
 370         private void displayData(CMD.Data dataType){
 371                 //Util.debug("Sensors: displayData");
 372                 float data[] = new float[4];
 373                 data[0] = dataType.ordinal();
 374                 switch(dataType) {
 375                 case ACC:
 376                         data[1] = accValues[0];
 377                         data[2] = accValues[1];
 378                         data[3] = accValues[2];
 379                         break;
 380                 case MAG:
 381                         data[1] = magValues[0];
 382                         data[2] = magValues[1];
 383                         data[3] = magValues[2];
 384                         break;
 385                 case ORIENT:
 386                         data[1] = orientValues[0];
 387                         data[2] = orientValues[1];
 388                         data[3] = orientValues[2];
 389                         break;
 390                 case WRDACC:
 391                         data[1] = accWorldCoord[0];
 392                         data[2] = accWorldCoord[1];
 393                         data[3] = accWorldCoord[2];
 394                         break;
 395                 case STPCNT:
 396                         data[1] = stepCount;
 397                         data[2] = calculatedGravity;
 398                         break;
 399                 case POSITION:
 400                         data[1] = currentPosX;
 401                         data[2] = currentPosY;
 402                         break;
 403                 case ROTATION:
 404                         Vect rpy = new Vect();
 405                         this.rotation.get(rpy);
 406                         data[1] = (float)(rpy.x * (180/Math.PI));
 407                         data[2] = (float)(rpy.y * (180/Math.PI));
 408                         data[3] = (float)(rpy.z * (180/Math.PI));
 409                         break;
 410                 case GYR:
 411                         data[1] = gyrValues[0];
 412                         data[2] = gyrValues[1];
 413                         data[3] = gyrValues[2];
 414                         break;
 415                 case GYR1HZ:
 416                         data[1] = (float)(gyr1hz.x * (180/Math.PI) * 60 * 60);
 417                         data[2] = (float)(gyr1hz.y * (180/Math.PI) * 60 * 60);
 418                         data[3] = (float)(gyr1hz.z * (180/Math.PI) * 60 * 60);
 419                         break;
 420                 case HEADING:
 421                         data[1] = currentHeading;
 422                         break;
 423                 case WRDGYR:
 424                         data[1] = gyrWorldCoord[0];
 425                         break;
 426                 case STABLE:
 427                         data[1] = ifStable?1:0;
 428                         break;
 429                 default:
 430                         Util.debug("Bad Data Sending Command!");
 431                         return;
 432                 }
 433
 434                 this.task.tellMain(CMD.Response.SHOWDATA, data);
 435         }
 436
 437         private int everyAveragedBuffer(float[] buffer, float[] values, int length, int cnt, int cycle, CMD.Data cmd){
 438                 for(int i=0; i<length; i++){
 439                         buffer[i]+=values[i];
 440                 }
 441                 if (cnt == cycle){
 442                         cnt = 0;
 443                         for(int i=0; i<length; i++){
 444                                 values[i] = buffer[i]/cycle;
 445                                 buffer[i] = 0;
 446                         }
 447                         displayData(cmd);
 448                 }
 449                 return cnt;
 450         }
 451
 452         private void forwardAveragedBuffer(LinkedList<Float> buffer, float[] sum, float[] avg,
 453                         float[] values, int length, int cnt, int cycle, CMD.Data cmd){
 454                 if (buffer==null||buffer.size()<cycle*length){
 455                         for(int i=0; i<length; i++){
 456                                 buffer.addLast(values[i]);
 457                                 sum[i]+=values[i];
 458                                 values[i] = sum[i]/buffer.size();
 459                         }
 460                 }
 461                 else{
 462                         float[] discarded = new float[length];
 463                         for(int i=0; i<length; i++){
 464                                 discarded[i]= buffer.removeFirst();
 465                                 buffer.addLast(values[i]);
 466                                 sum[i]-=discarded[i];
 467                                 sum[i]+=values[i];
 468                                 values[i]=sum[i]/cycle;
 469                         }
 470                 }
 471                 if(avg!=null){
 472                         for(int i=0; i<length; i++){
 473                                 avg[i]=sum[i]/cycle;
 474                         }
 475                 }
 476                 if (cnt%32==0 && cmd != null){
 477                         displayData(cmd);
 478                 }
 479         }
 480
 481         private void printMatrix(String s, Matrix m){
 482                 Util.debug("Sensor: ["+s+"] "+m.mValue[0]+" "+m.mValue[1]+" "+m.mValue[2]);
 483                 Util.debug("Sensor:       "  +m.mValue[3]+" "+m.mValue[4]+" "+m.mValue[5]);
 484                 Util.debug("Sensor:       "  +m.mValue[6]+" "+m.mValue[7]+" "+m.mValue[8]);
 485         }
 486
 487         private void printVector(String s, float[] v){
 488                 Util.debug("Sensor: ["+s+"] "+v[0]+" "+v[1]+" "+v[2]);
 489         }
 490 }