From 6613bf0afa7b5d59a7f6a6f923a73447d0325f23 Mon Sep 17 00:00:00 2001
From: Ben Fennema <fennema@google.com>
Date: Tue, 10 Apr 2018 22:06:52 -0700
Subject: [PATCH] sensorhal: allow accel bias updates while accel enabled

Set mAccelEnabledBias to mAccelBias when the first accel
sample is received after calibrated accel is enabled.

It is set based on the first accel sample received so that if
there is a bias update after enabling but prior to outputing
any accel samples, it is used instead of the old bias value.

Update calibrated accel samples by adding mAccelBias and
subtracting mAccelEnabledBias.

As long as there is an active accel client, use mAccelEnabledBias
for uncalibrated accel bias as well.

Bug: 77729211
Test: verify updating accel bias doesn't cause jumps in accel values
Change-Id: I7ead05a1dfef48659538543322e984174bc8dde0
Signed-off-by: Ben Fennema <fennema@google.com>
---
 sensorhal/hubconnection.cpp | 37 ++++++++++++++++++++++++++++++++++---
 sensorhal/hubconnection.h   |  3 ++-
 2 files changed, 36 insertions(+), 4 deletions(-)

diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index 3af5a604..e43305a6 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -132,6 +132,8 @@ HubConnection::HubConnection()
     mMagAccuracyRestore = SENSOR_STATUS_UNRELIABLE;
     mGyroBias[0] = mGyroBias[1] = mGyroBias[2] = 0.0f;
     mAccelBias[0] = mAccelBias[1] = mAccelBias[2] = 0.0f;
+    mAccelEnabledBias[0] = mAccelEnabledBias[1] = mAccelEnabledBias[2] = 0.0f;
+    mAccelEnabledBiasStored = true;
     memset(&mGyroOtcData, 0, sizeof(mGyroOtcData));
 
     mLefty.accel = false;
@@ -732,6 +734,22 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
 
         sendDirectReportEvent(&nev[cnt], 1);
         if (mSensorState[sensor].enable && isSampleIntervalSatisfied(sensor, timestamp)) {
+            if (!mAccelEnabledBiasStored) {
+                // accel is enabled, but no enabled bias. Store latest bias and use
+                // for accel and uncalibrated accel due to:
+                // https://source.android.com/devices/sensors/sensor-types.html
+                // "The bias and scale calibration must only be updated while the sensor is deactivated,
+                // so as to avoid causing jumps in values during streaming."
+                mAccelEnabledBiasStored = true;
+                mAccelEnabledBias[0] = mAccelBias[0];
+                mAccelEnabledBias[1] = mAccelBias[1];
+                mAccelEnabledBias[2] = mAccelBias[2];
+            }
+            // samples arrive using latest bias
+            // adjust for enabled bias being different from lastest bias
+            sv->x += mAccelBias[0] - mAccelEnabledBias[0];
+            sv->y += mAccelBias[1] - mAccelEnabledBias[1];
+            sv->z += mAccelBias[2] - mAccelEnabledBias[2];
             ++cnt;
         }
 
@@ -741,9 +759,17 @@ void HubConnection::processSample(uint64_t timestamp, uint32_t type, uint32_t se
         ue->x_uncalib = sample->ix * mScaleAccel + mAccelBias[0];
         ue->y_uncalib = sample->iy * mScaleAccel + mAccelBias[1];
         ue->z_uncalib = sample->iz * mScaleAccel + mAccelBias[2];
-        ue->x_bias = mAccelBias[0];
-        ue->y_bias = mAccelBias[1];
-        ue->z_bias = mAccelBias[2];
+        if (!mAccelEnabledBiasStored) {
+            // No enabled bias (which means accel is disabled). Use latest bias.
+            ue->x_bias = mAccelBias[0];
+            ue->y_bias = mAccelBias[1];
+            ue->z_bias = mAccelBias[2];
+        } else {
+            // enabled bias is valid, so use it
+            ue->x_bias = mAccelEnabledBias[0];
+            ue->y_bias = mAccelEnabledBias[1];
+            ue->z_bias = mAccelEnabledBias[2];
+        }
 
         sendDirectReportEvent(&nev[cnt], 1);
         if (mSensorState[COMMS_SENSOR_ACCEL_UNCALIBRATED].enable
@@ -1665,6 +1691,11 @@ void HubConnection::queueActivate(int handle, bool enable)
     Mutex::Autolock autoLock(mLock);
 
     if (isValidHandle(handle)) {
+        // disabling accel, so no longer need to use the bias from when
+        // accel was first enabled
+        if (handle == COMMS_SENSOR_ACCEL && !enable)
+            mAccelEnabledBiasStored = false;
+
         mSensorState[handle].enable = enable;
 
         initConfigCmd(&cmd, handle);
diff --git a/sensorhal/hubconnection.h b/sensorhal/hubconnection.h
index 558ece34..63bd22d3 100644
--- a/sensorhal/hubconnection.h
+++ b/sensorhal/hubconnection.h
@@ -247,7 +247,8 @@ private:
     uint8_t mMagAccuracy;
     uint8_t mMagAccuracyRestore;
 
-    float mGyroBias[3], mAccelBias[3];
+    float mGyroBias[3], mAccelBias[3], mAccelEnabledBias[3];
+    bool mAccelEnabledBiasStored;
     GyroOtcData mGyroOtcData;
 
     float mScaleAccel, mScaleMag;