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