From bb2ea574aa82f8c18bf6eeb0a37a3d5a7cd497a0 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 10 Apr 2022 11:07:01 -0400
Subject: [PATCH] ekf2: properly reset IMU biases on calibration change
 (non-multi-EKF)

 - this was working in the multi-EKF case using vehicle_imu, but missing
in sensor_combined
---
 msg/sensor_combined.msg                      |  3 +++
 src/modules/ekf2/EKF2.cpp                    | 22 ++++++++++++++++++++
 src/modules/sensors/voted_sensors_update.cpp |  5 ++++-
 3 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg
index 42440213cf..26ea318a83 100644
--- a/msg/sensor_combined.msg
+++ b/msg/sensor_combined.msg
@@ -18,3 +18,6 @@ uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
 uint8 CLIPPING_Z = 4
 uint8 accelerometer_clipping            # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period
+
+uint8 accel_calibration_count  	# Calibration changed counter. Monotonically increases whenever accelermeter calibration changes.
+uint8 gyro_calibration_count   	# Calibration changed counter. Monotonically increases whenever rate gyro calibration changes.
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 8637e6e2ba..b7c41fc343 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -446,6 +446,28 @@ void EKF2::Run()
 			}
 
 			imu_dt = sensor_combined.gyro_integral_dt;
+
+			if (sensor_combined.accel_calibration_count != _accel_calibration_count) {
+
+				PX4_DEBUG("%d - resetting accelerometer bias", _instance);
+
+				_ekf.resetAccelBias();
+				_accel_calibration_count = sensor_combined.accel_calibration_count;
+
+				// reset bias learning
+				_accel_cal = {};
+			}
+
+			if (sensor_combined.gyro_calibration_count != _gyro_calibration_count) {
+
+				PX4_DEBUG("%d - resetting rate gyro bias", _instance);
+
+				_ekf.resetGyroBias();
+				_gyro_calibration_count = sensor_combined.gyro_calibration_count;
+
+				// reset bias learning
+				_gyro_cal = {};
+			}
 		}
 
 		if (_sensor_selection_sub.updated() || (_device_id_accel == 0 || _device_id_gyro == 0)) {
diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp
index 7e4f4f07a4..d363ba11de 100644
--- a/src/modules/sensors/voted_sensors_update.cpp
+++ b/src/modules/sensors/voted_sensors_update.cpp
@@ -172,7 +172,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 			_last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1);
 			_last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2);
 			_last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt;
-
+			_last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count;
+			_last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count;
 
 			_last_accel_timestamp[uorb_index] = imu_report.timestamp_sample;
 
@@ -231,6 +232,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw)
 		raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt;
 		raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt;
 		raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping;
+		raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count;
+		raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count;
 
 		if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) {
 			_accel.last_best_vote = (uint8_t)accel_best_index;