sensors: minor IMU bias saving updates

- sensors/vehicle_imu: reset learned cal on any calibration change
during parameter update
 - sensors/vehicle_imu: cleanup logic estimated bias -> calibration offset
saving
   - don't invalidate saved calibration (the point is to keep the last valid)
   - remove old debug code, etc
 - sensors/vehicle_imu: notify parameter changes if accel or gyro
calibration has changed
 - lib/sensor_calibration: add calibrated() and calibration_index()
getters, keep Accelerometer/Gyroscope/Magnetometer in sync

src/lib/sensor_calibration/Accelerometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -60,10 +60,6 @@ void Accelerometer::set_device_id(uint32_t device_id, bool external)
 		set_external(external);
 		_device_id = device_id;
 
-		if (_device_id != 0) {
-			_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-		}
-
 		ParametersUpdate();
 		SensorCorrectionsUpdate(true);
 	}
@@ -166,6 +162,10 @@ void Accelerometer::set_rotation(Rotation rotation)
 
 void Accelerometer::ParametersUpdate()
 {
+	if (_device_id != 0) {
+		_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+	}
+
 	if (_calibration_index >= 0) {
 
 		// CAL_ACCx_ROT

src/lib/sensor_calibration/Accelerometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -67,7 +67,9 @@ public:
 	void set_rotation(Rotation rotation);
 	void set_temperature(float temperature) { _temperature = temperature; };
 
+	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
+	int8_t calibration_index() const { return _calibration_index; }
 	uint32_t device_id() const { return _device_id; }
 	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }

src/lib/sensor_calibration/Gyroscope.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -60,10 +60,6 @@ void Gyroscope::set_device_id(uint32_t device_id, bool external)
 		set_external(external);
 		_device_id = device_id;
 
-		if (_device_id != 0) {
-			_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
-		}
-
 		ParametersUpdate();
 		SensorCorrectionsUpdate(true);
 	}
@@ -151,6 +147,10 @@ void Gyroscope::set_rotation(Rotation rotation)
 
 void Gyroscope::ParametersUpdate()
 {
+	if (_device_id != 0) {
+		_calibration_index = FindCalibrationIndex(SensorString(), _device_id);
+	}
+
 	if (_calibration_index >= 0) {
 
 		// CAL_GYROx_ROT

src/lib/sensor_calibration/Gyroscope.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -66,7 +66,9 @@ public:
 	void set_rotation(Rotation rotation);
 	void set_temperature(float temperature) { _temperature = temperature; };
 
+	bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); }
 	uint8_t calibration_count() const { return _calibration_count; }
+	int8_t calibration_index() const { return _calibration_index; }
 	uint32_t device_id() const { return _device_id; }
 	bool enabled() const { return (_priority > 0); }
 	bool external() const { return _external; }

src/lib/sensor_calibration/Magnetometer.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/lib/sensor_calibration/Magnetometer.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -115,9 +115,29 @@ void VehicleIMU::ParametersUpdate(bool force)
 
 		updateParams();
 
+		const auto accel_calibration_count = _accel_calibration.calibration_count();
+		const auto gyro_calibration_count = _gyro_calibration.calibration_count();
 		_accel_calibration.ParametersUpdate();
 		_gyro_calibration.ParametersUpdate();
 
+		if (accel_calibration_count != _accel_calibration.calibration_count()) {
+			// if calibration changed reset any existing learned calibration
+			_accel_cal_available = false;
+
+			for (auto &learned_cal : _accel_learned_calibration) {
+				learned_cal = {};
+			}
+		}
+
+		if (gyro_calibration_count != _gyro_calibration.calibration_count()) {
+			// if calibration changed reset any existing learned calibration
+			_gyro_cal_available = false;
+
+			for (auto &learned_cal : _gyro_learned_calibration) {
+				learned_cal = {};
+			}
+		}
+
 		// constrain IMU integration time 1-10 milliseconds (100-1000 Hz)
 		int32_t imu_integration_rate_hz = constrain(_param_imu_integ_rate.get(),
 						  (int32_t)100, math::max(_param_imu_gyro_ratemax.get(), (int32_t) 1000));
@@ -687,75 +707,38 @@ void VehicleIMU::SensorCalibrationUpdate()
 		for (int i = 0; i < _estimator_sensor_bias_subs.size(); i++) {
 			estimator_sensor_bias_s estimator_sensor_bias;
 
-			if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) {
+			if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)
+			    && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) {
+
 				// find corresponding accel bias
-				if ((estimator_sensor_bias.accel_device_id != 0)
-				    && (_accel_calibration.device_id() == estimator_sensor_bias.accel_device_id)) {
+				if (estimator_sensor_bias.accel_bias_valid && estimator_sensor_bias.accel_bias_stable
+				    && (estimator_sensor_bias.accel_device_id != 0)
+				    && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())) {
 
 					const Vector3f bias{estimator_sensor_bias.accel_bias};
-					const Vector3f bias_variance{estimator_sensor_bias.accel_bias_variance};
-
-					const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) && estimator_sensor_bias.accel_bias_valid
-							   && estimator_sensor_bias.accel_bias_stable;
-
-					if (valid) {
-						const Vector3f offset_old{_accel_learned_calibration[i].offset};
-
-						_accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
-						_accel_learned_calibration[i].bias_variance = bias_variance;
-						_accel_learned_calibration[i].valid = true;
-						_accel_cal_available = true;
-
-						if ((offset_old - _accel_learned_calibration[i].offset).longerThan(0.05f)) {
-							PX4_DEBUG("accel %d (%" PRIu32 ") new offset: [%.2f %.2f %.2f] (full bias [%.2f %.2f %.2f])",
-								  _instance, _accel_calibration.device_id(),
-								  (double)_accel_learned_calibration[i].offset(0),
-								  (double)_accel_learned_calibration[i].offset(1),
-								  (double)_accel_learned_calibration[i].offset(2),
-								  (double)bias(0), (double)bias(1), (double)bias(2));
-						}
 
-					} else {
-						_accel_learned_calibration[i].valid = false;
-					}
+					_accel_learned_calibration[i].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
+					_accel_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.accel_bias_variance};
+					_accel_learned_calibration[i].valid = true;
+					_accel_cal_available = true;
 				}
 
 				// find corresponding gyro calibration
-				if ((estimator_sensor_bias.gyro_device_id != 0)
-				    && (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_device_id)) {
+				if (estimator_sensor_bias.gyro_bias_valid && estimator_sensor_bias.gyro_bias_stable
+				    && (estimator_sensor_bias.gyro_device_id != 0)
+				    && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())) {
+
 					const Vector3f bias{estimator_sensor_bias.gyro_bias};
-					const Vector3f bias_variance{estimator_sensor_bias.gyro_bias_variance};
-
-					const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) && estimator_sensor_bias.gyro_bias_valid
-							   && estimator_sensor_bias.gyro_bias_stable;
-
-					if (valid) {
-						const Vector3f offset_old{_gyro_learned_calibration[i].offset};
-
-						_gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
-						_gyro_learned_calibration[i].bias_variance = bias_variance;
-						_gyro_learned_calibration[i].valid = true;
-						_gyro_cal_available = true;
-
-						if ((offset_old - _gyro_learned_calibration[i].offset).longerThan(0.01f)) {
-							PX4_DEBUG("gyro %d (%" PRIu32 ") new offset: [%.2f %.2f %.2f] (full bias [%.2f %.2f %.2f])",
-								  _instance, _gyro_calibration.device_id(),
-								  (double)_gyro_learned_calibration[i].offset(0),
-								  (double)_gyro_learned_calibration[i].offset(1),
-								  (double)_gyro_learned_calibration[i].offset(2),
-								  (double)bias(0), (double)bias(1), (double)bias(2));
-						}
 
-					} else {
-						_gyro_learned_calibration[i].valid = false;
-					}
+					_gyro_learned_calibration[i].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
+					_gyro_learned_calibration[i].bias_variance = Vector3f{estimator_sensor_bias.gyro_bias_variance};
+					_gyro_learned_calibration[i].valid = true;
+					_gyro_cal_available = true;
 				}
 			}
 		}
-
 	}
 
-
 	// not armed and accel cal available
 	if (!_armed && _accel_cal_available) {
 		const Vector3f accel_cal_orig{_accel_calibration.offset()};
@@ -785,12 +768,14 @@ void VehicleIMU::SensorCalibrationUpdate()
 
 		if (initialised && (accel_cal_orig - offset_estimate).longerThan(0.05f)) {
 			if (_accel_calibration.set_offset(offset_estimate)) {
-				PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f])",
+				PX4_INFO("accel %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
 					 _instance, _accel_calibration.device_id(),
 					 (double)accel_cal_orig(0), (double)accel_cal_orig(1), (double)accel_cal_orig(2),
 					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
 
-				_accel_calibration.ParametersSave();
+				if (_accel_calibration.ParametersSave()) {
+					param_notify_changes();
+				}
 			}
 		}
 
@@ -832,12 +817,14 @@ void VehicleIMU::SensorCalibrationUpdate()
 
 		if (initialised && (gyro_cal_orig - offset_estimate).longerThan(0.01f)) {
 			if (_gyro_calibration.set_offset(offset_estimate)) {
-				PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f])",
+				PX4_INFO("gyro %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
 					 _instance, _gyro_calibration.device_id(),
 					 (double)gyro_cal_orig(0), (double)gyro_cal_orig(1), (double)gyro_cal_orig(2),
 					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
 
-				_gyro_calibration.ParametersSave();
+				if (_gyro_calibration.ParametersSave()) {
+					param_notify_changes();
+				}
 			}
 		}
 

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions