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

2 files changed, 8 insertions, 7 deletions

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 @@ SensorFusion::SensorFusion()
 
         // 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 @@ void SensorFusion::process(const sensors_event_t& event) {
     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 @@ void SensorFusion::dump(String8& result, char* buffer, size_t SIZE) {
             "b=< %g, %g, %g >\n",
             mEnabled ? "enabled" : "disabled",
             mClients.size(),
-            mGyroRate,
+            mEstimatedGyroRate,
             fusion.getAttitude().x,
             fusion.getAttitude().y,
             fusion.getAttitude().z,
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 @@ class SensorFusion : public Singleton<SensorFusion> {
     Sensor mGyro;
     Fusion mFusion;
     bool mEnabled;
-    float mGyroRate;
+    float mEstimatedGyroRate;
     nsecs_t mTargetDelayNs;
     nsecs_t mGyroTime;
     vec4_t mAttitude;
@@ -60,7 +60,7 @@ public:
     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);