commit 2e2a560c4b60c24258e0eaadc1189eb9dcc1a0b4
author Mathias Agopian <mathias@google.com> Thu May 30 14:18:23 2013 -0700
committer Mathias Agopian <mathias@google.com> Thu May 30 14:18:23 2013 -0700
tree fcfe63aea9d06305d61fae3332aa7b659969c99a
parent 9e3cb55b8f6f007906872decfe3b8a2541e94dd2

use gyro timestamp directly in fusion

we used to estimate the gyro rate and deduce the period from that
but it turns out this is causing problems.

Bug: 5192288
Change-Id: I8ca826d0e11e488587bcaa1720de99e92b82f191

services/sensorservice/SensorFusion.cpp

diff --git a/services/sensorservice/SensorFusion.cpp b/services/sensorservice/SensorFusion.cpp
index 4837b97..a0a17da 100644
--- a/services/sensorservice/SensorFusion.cpp
+++ b/services/sensorservice/SensorFusion.cpp
@@ -53,8 +53,8 @@
 
         // 200 Hz for gyro events is a good compromise between precision
         // and power/cpu usage.
-        mGyroRate = 200;
-        mTargetDelayNs = 1000000000LL/mGyroRate;
+        mEstimatedGyroRate = 200;
+        mTargetDelayNs = 1000000000LL/mEstimatedGyroRate;
         mFusion.init();
     }
 }
@@ -63,14 +63,15 @@
     if (event.type == mGyro.getType()) {
         if (mGyroTime != 0) {
             const float dT = (event.timestamp - mGyroTime) / 1000000000.0f;
+            mFusion.handleGyro(vec3_t(event.data), dT);
+            // here we estimate the gyro rate (useful for debugging)
             const float freq = 1 / dT;
             if (freq >= 100 && freq<1000) { // filter values obviously wrong
                 const float alpha = 1 / (1 + dT); // 1s time-constant
-                mGyroRate = freq + (mGyroRate - freq)*alpha;
+                mEstimatedGyroRate = freq + (mEstimatedGyroRate - freq)*alpha;
             }
         }
         mGyroTime = event.timestamp;
-        mFusion.handleGyro(vec3_t(event.data), 1.0f/mGyroRate);
     } else if (event.type == SENSOR_TYPE_MAGNETIC_FIELD) {
         const vec3_t mag(event.data);
         mFusion.handleMag(mag);
@@ -142,7 +143,7 @@
             "b=< %g, %g, %g >\n",
             mEnabled ? "enabled" : "disabled",
             mClients.size(),
-            mGyroRate,
+            mEstimatedGyroRate,
             fusion.getAttitude().x,
             fusion.getAttitude().y,
             fusion.getAttitude().z,

services/sensorservice/SensorFusion.h

diff --git a/services/sensorservice/SensorFusion.h b/services/sensorservice/SensorFusion.h
index 4c99bcb..3c2244e 100644
--- a/services/sensorservice/SensorFusion.h
+++ b/services/sensorservice/SensorFusion.h
@@ -44,7 +44,7 @@
     Sensor mGyro;
     Fusion mFusion;
     bool mEnabled;
-    float mGyroRate;
+    float mEstimatedGyroRate;
     nsecs_t mTargetDelayNs;
     nsecs_t mGyroTime;
     vec4_t mAttitude;
@@ -60,7 +60,7 @@
     mat33_t getRotationMatrix() const { return mFusion.getRotationMatrix(); }
     vec4_t getAttitude() const { return mAttitude; }
     vec3_t getGyroBias() const { return mFusion.getBias(); }
-    float getEstimatedRate() const { return mGyroRate; }
+    float getEstimatedRate() const { return mEstimatedGyroRate; }
 
     status_t activate(void* ident, bool enabled);
     status_t setDelay(void* ident, int64_t ns);