diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp index d5696625..ef95ae68 100644 --- a/sensorhal/hubconnection.cpp +++ b/sensorhal/hubconnection.cpp @@ -126,6 +126,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; @@ -688,6 +690,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; } @@ -697,9 +715,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 @@ -1552,6 +1578,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 e8b0e1db..26f85128 100644 --- a/sensorhal/hubconnection.h +++ b/sensorhal/hubconnection.h @@ -242,7 +242,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;