Integrate quaternions

[~andy/iBeaconNav] / src / edu / ucla / iBeaconNav / Sensors.java

diff --git a/src/edu/ucla/iBeaconNav/Sensors.java b/src/edu/ucla/iBeaconNav/Sensors.java
index e3d788a3067c04996a05bcf06c7ba0662a7fdc91..745c6e6bfbcb4ced82cb0dc7bde916c666ceb320 100644 (file)
--- a/src/edu/ucla/iBeaconNav/Sensors.java
+++ b/src/edu/ucla/iBeaconNav/Sensors.java
@@ -105,6 +105,13 @@ public class Sensors extends Service implements SensorEventListener
        private boolean stepStart      = false;
 
 
+       /* Integrated positions */
+       private Vect    position       = new Vect();
+       private Quat    rotation       = new Quat();
+
+       /* Test data */
+       private long    time1hz        = 0;
+       private Vect    gyr1hz         = new Vect();
 
        /* Private methods */
        private void tellMain(CMD.Response cmd, Object value)
@@ -172,6 +179,7 @@ public class Sensors extends Service implements SensorEventListener
                        // Reset heading
                        case RSTHEAD:
                                Util.debug("Sensors: handle - reset heading");
+                               this.rotation = new Quat();
                                currentHeading = 0;
                                displayData(CMD.Data.HEADING);
                                break;
@@ -318,6 +326,22 @@ public class Sensors extends Service implements SensorEventListener
                                currentHeading%=360;
                                displayData(CMD.Data.HEADING);
                        }
+
+                       // Integrate rotations
+                       Vect rpy = new Vect(event.values[1], event.values[0], -event.values[2]);
+                       rpy.mul((double)snsInterval_ns/1E9);
+                       Quat rot = new Quat(rpy.x, rpy.y, rpy.z);
+                       this.rotation.mul(rot);
+                       displayData(CMD.Data.ROTATION);
+
+                       // Calculate 1 second gryo data
+                       this.gyr1hz.add(rpy);
+                       if (currentTime_ns > time1hz+1E9) {
+                               displayData(CMD.Data.GYR1HZ);
+                               this.gyr1hz.set(0, 0, 0);
+                               time1hz = currentTime_ns;
+                       }
+
                        break;
 
 
@@ -470,11 +494,23 @@ public class Sensors extends Service implements SensorEventListener
                        data[1] = currentPosX;
                        data[2] = currentPosY;
                        break;
+               case ROTATION:
+                       Vect rpy = new Vect();
+                       this.rotation.get(rpy);
+                       data[1] = (float)(rpy.x * (180/Math.PI));
+                       data[2] = (float)(rpy.y * (180/Math.PI));
+                       data[3] = (float)(rpy.z * (180/Math.PI));
+                       break;
                case GYR:
                        data[1] = gyrValues[0];
                        data[2] = gyrValues[1];
                        data[3] = gyrValues[2];
                        break;
+               case GYR1HZ:
+                       data[1] = (float)(gyr1hz.x * (180/Math.PI) * 60 * 60);
+                       data[2] = (float)(gyr1hz.y * (180/Math.PI) * 60 * 60);
+                       data[3] = (float)(gyr1hz.z * (180/Math.PI) * 60 * 60);
+                       break;
                case HEADING:
                        data[1] = currentHeading;
                        break;