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

@@ -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];
+        if (!mAccelEnabledBiasStored) {
-        ue->y_bias = mAccelBias[1];
+            // No enabled bias (which means accel is disabled). Use latest bias.
-        ue->z_bias = mAccelBias[2];
+            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);

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;