1 package edu.ucla.iBeaconNav;
3 import java.util.LinkedList;
4 import java.util.Queue;
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;
23 public class Sensors extends Service implements SensorEventListener
26 private SensorManager sensorManager;
27 private Sensor accSensor;
28 private Sensor grvSensor;
29 private Sensor magSensor;
30 private Sensor gyrSensor;
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;
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};
53 /* Auxiliary Variables for Sensor Processing */
55 private final int cycle = 32;
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,
72 private float[] accBuffer = {0,0,0};
73 private float[] magBuffer = {0,0,0};
74 private float[] orientBuffer= {0,0,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;
82 private boolean ifGyrOffset = false;
83 private boolean ifGrvOffset = false;
84 private boolean ifStable = false;
86 private final float EPSILON = (float)0.01;
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;
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;
108 /* Integrated positions */
109 private Vect position = new Vect();
110 private Quat rotation = new Quat();
113 private long time1hz = 0;
114 private Vect gyr1hz = new Vect();
116 /* Private methods */
117 private void tellMain(CMD.Response cmd, Object value)
120 android.os.Message msg = android.os.Message.obtain();
121 msg.what = cmd.ordinal();
123 this.messenger.send(msg);
124 } catch (Exception e) {
125 Util.debug("Sensors: error sending message", e);
129 private void notify(String text, int icon)
132 this.tellMain(CMD.Response.NOTIFY, text);
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);
141 Notification note = new Notification.Builder(this)
142 .setContentTitle("iBeaconNav!")
143 .setContentText("iBeaconNav!")
147 this.startForeground(1, note);
150 private void handle(CMD.Command cmd, Messenger mgr)
152 // Validate messenger
153 if (cmd != CMD.Command.REGISTER && mgr != null && mgr != this.messenger)
154 Util.debug("Sensors: handle - invalid messenger");
156 // Handle the command
158 // Setup communication with Main
160 Util.debug("Sensors: handle - register");
161 this.messenger = mgr;
164 // Create client thread
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);
173 // Stop client thread
175 Util.debug("Sensors: handle - register");
181 Util.debug("Sensors: handle - reset heading");
182 this.rotation = new Quat();
184 displayData(CMD.Data.HEADING);
189 Util.debug("Sensors: handle - reset distance");
192 displayData(CMD.Data.POSITION);
198 public boolean isRunning()
203 /* Service Methods */
205 public void onCreate()
207 Util.debug("Sensors: 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);
217 public void onDestroy()
219 Util.debug("Sensors: onDestroy");
220 //this.handle(CMD.Response.DISCONNECT, null);
224 public int onStartCommand(Intent intent, int flags, int startId)
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);
235 public IBinder onBind(Intent intent)
237 Util.debug("Sensors: onBind");
238 return messenger.getBinder();
240 class IncomingHandler extends Handler{
242 public void handleMessage(Message msg) {
243 //Util.debug("Sensors: MSG HANDLERRRRRRRRR");
244 super.handleMessage(msg);
249 public void onAccuracyChanged(Sensor sensor, int accuracy) {
250 // TODO Auto-generated method stub
251 Util.debug("Sensors: onAccuracyChanged");
256 public void onSensorChanged(SensorEvent event) {
257 // TODO Auto-generated method stub
258 switch(event.sensor.getType()){
259 case Sensor.TYPE_ACCELEROMETER:
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;
272 else if(!ifSetGrvRef && accCnt>400){
274 gravityRef = accValueTotal;
278 case Sensor.TYPE_MAGNETIC_FIELD:
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
285 case Sensor.TYPE_GYROSCOPE:
287 float gyrTemp[] = new float[3];
288 long currentTime_ns = event.timestamp;
289 snsInterval_ns = currentTime_ns-lastTime_ns;
290 lastTime_ns = currentTime_ns;
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;
297 ifStable = isStable(accBuffers, 3, cycle/2) && isStable(gyrBuffers, 3, cycle/2);
302 //lastTime_ms = SystemClock.elapsedRealtime();
303 //Util.debug("Interal in ms: "+Float.toString(snsInterval_ms));
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
317 depleteOffset(gyrValues, gyrOffset, 3);
318 //cycleFloatArray(gyrValues, 3);
319 toWorldCoordinates(gyrWorldCoord, gyrValues, rtMatrixStable_R, gyrCnt, CMD.Data.WRDGYR);
321 float gyrZ = gyrWorldCoord[2];
322 //if (!ifStable && ifGyrOffset && Math.abs(gyrZ)>0.05){
324 //currentHeading+=gyrZ*snsInterval_ms/1000*180/Math.PI;
325 currentHeading += gyrZ*snsInterval_ns/1000000000*180/Math.PI;
327 displayData(CMD.Data.HEADING);
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);
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;
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];
360 private void depleteOffset(float[] values, float[] offset, int len){
361 for(int i=0; i<len; i++){
362 values[i]-=offset[i];
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];
371 for(int i=0; i<length; i++){
373 for (int j=0; j<cycle; j++){
374 avrg[length-1-i]+=buffer.get(len-1-j*length-i);
376 avrg[length-1-i]/=cycle;
378 for (int i=0; i<length; i++){
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);
383 devSum[length-1-i] = (float) Math.sqrt(devSum[length-1-i]/cycle);
386 for (int i=0; i<length; i++){
391 //Util.debug("[DEV] unStable");
392 displayData(CMD.Data.STABLE);
395 //Util.debug("[DEV] Stable");
396 displayData(CMD.Data.STABLE);
403 private void processSensorInfo(){
404 displayData(CMD.Data.STPCNT);
405 float epsl = (float)0.6;
409 if(accValueTotal-calculatedGravity>epsl){
412 currentPosX += stepLength*Math.sin(currentHeading);
413 currentPosY += stepLength*Math.cos(currentHeading);
414 displayData(CMD.Data.POSITION);
419 if (calculatedGravity-accValueTotal>epsl ){
427 private void updateOrientation(){
428 if (accValues == null || magValues == null){
431 float R[] = new float[9];
432 float I[] = new float[9];
434 success = SensorManager.getRotationMatrix(R, I, accValues, magValues);
438 rotationMatrix_R = R;
439 rotationMatrix_I = I;
440 SensorManager.getOrientation(rotationMatrix_R, orientValues_rd);
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);
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);
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];
464 private void displayData(CMD.Data dataType){
465 //Util.debug("Sensors: displayData");
466 float data[] = new float[4];
467 data[0] = dataType.ordinal();
470 data[1] = accValues[0];
471 data[2] = accValues[1];
472 data[3] = accValues[2];
475 data[1] = magValues[0];
476 data[2] = magValues[1];
477 data[3] = magValues[2];
480 data[1] = orientValues[0];
481 data[2] = orientValues[1];
482 data[3] = orientValues[2];
485 data[1] = accWorldCoord[0];
486 data[2] = accWorldCoord[1];
487 data[3] = accWorldCoord[2];
491 data[2] = calculatedGravity;
494 data[1] = currentPosX;
495 data[2] = currentPosY;
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));
505 data[1] = gyrValues[0];
506 data[2] = gyrValues[1];
507 data[3] = gyrValues[2];
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);
515 data[1] = currentHeading;
518 data[1] = gyrWorldCoord[0];
521 data[1] = ifStable?1:0;
524 Util.debug("Bad Data Sending Command!");
528 this.tellMain(CMD.Response.SHOWDATA, data);
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];
537 for(int i=0; i<length; i++){
538 values[i] = buffer[i]/cycle;
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]);
552 values[i] = sum[i]/buffer.size();
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];
562 values[i]=sum[i]/cycle;
566 for(int i=0; i<length; i++){
570 if (cnt%32==0 && cmd != null){
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]);
581 public void printVector(String s, float[] v){
582 Util.debug("Sensor: ["+s+"] "+v[0]+" "+v[1]+" "+v[2]);