sensor: unify accel/gyro/mag estimated bias -> cal update

src/lib/sensor_calibration/Accelerometer.hpp

@@ -69,7 +69,7 @@ public:
 	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 enabled() const { return (_device_id != 0) && (_priority > 0); }
 	bool external() const { return _external; }
 	const matrix::Vector3f &offset() const { return _offset; }
 	const int32_t &priority() const { return _priority; }

src/lib/sensor_calibration/Gyroscope.hpp

@@ -68,7 +68,7 @@ public:
 	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 enabled() const { return (_device_id != 0) && (_priority > 0); }
 	bool external() const { return _external; }
 	const matrix::Vector3f &offset() const { return _offset; }
 	const int32_t &priority() const { return _priority; }

src/lib/sensor_calibration/Magnetometer.hpp

@@ -71,7 +71,7 @@ public:
 	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 enabled() const { return (_device_id != 0) && (_priority > 0); }
 	bool external() const { return _external; }
 	const matrix::Vector3f &offset() const { return _offset; }
 	const int32_t &priority() const { return _priority; }

src/lib/sensor_calibration/Utilities.hpp

@@ -149,4 +149,77 @@ matrix::Dcmf GetBoardRotationMatrix();
  */
 bool DeviceExternal(uint32_t device_id);
 
+
+struct InFlightCalibration {
+	uint32_t device_id{0};
+	matrix::Vector3f offset{};
+	matrix::Vector3f variance{};
+	uint8_t estimator_instance{0};
+};
+
+template<typename T, int MAX_LEARNED = 3>
+bool SensorCalibrationApplyLearned(T &calibration, int sensor_index,
+				   InFlightCalibration(&learned_calibrations)[MAX_LEARNED])
+{
+	bool updated = false;
+
+	// State variance assumed for bias storage.
+	// This is a reference variance used to calculate the fraction of learned bias that will be used to update the stored value.
+	// Larger values cause a larger fraction of the learned biases to be used.
+	static constexpr float BIAS_VAR_REF = 0.000'001f;
+	matrix::Vector3f state_variance{BIAS_VAR_REF, BIAS_VAR_REF, BIAS_VAR_REF};
+
+	// apply all valid saved offsets
+	// iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme
+	for (auto &inflight_cal : learned_calibrations) {
+		if ((inflight_cal.device_id != 0) && (inflight_cal.device_id == calibration.device_id()) && calibration.enabled()) {
+
+			const matrix::Vector3f offset_orig{calibration.offset()};
+
+			// new offset
+			matrix::Vector3f offset_new{};
+
+			if (calibration.calibrated()) {
+				// calculate weighting using ratio of variances and update stored bias values
+				const matrix::Vector3f &obs{inflight_cal.offset};            // observation
+				const matrix::Vector3f &obs_variance{inflight_cal.variance}; // observation variance
+
+				for (int axis_index = 0; axis_index < 3; axis_index++) {
+					float innovation = offset_orig(axis_index) - obs(axis_index);
+					float innovation_variance = state_variance(axis_index) + obs_variance(axis_index);
+
+					float kalman_gain = state_variance(axis_index) / innovation_variance;
+
+					offset_new(axis_index) = offset_orig(axis_index) - (innovation * kalman_gain);
+
+					state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f);
+				}
+
+			} else {
+				// sensor wasn't calibrated, use full offset
+				offset_new = inflight_cal.offset;
+			}
+
+			if (calibration.set_offset(offset_new)) {
+				PX4_INFO("%s %d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])",
+					 calibration.SensorString(), sensor_index, calibration.device_id(), inflight_cal.estimator_instance,
+					 (double)offset_orig(0), (double)offset_orig(1), (double)offset_orig(2),
+					 (double)offset_new(0), (double)offset_new(1), (double)offset_new(2),
+					 (double)inflight_cal.offset(0), (double)inflight_cal.offset(1), (double)inflight_cal.offset(2));
+
+				updated = true;
+			}
+
+			// clear
+			inflight_cal = {};
+		}
+	}
+
+	if (updated) {
+		calibration.ParametersSave(sensor_index);
+	}
+
+	return updated;
+}
+
 } // namespace calibration

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -125,24 +125,20 @@ bool VehicleIMU::ParametersUpdate(bool force)
 		_accel_calibration.ParametersUpdate();
 		_gyro_calibration.ParametersUpdate();
 
-		if (accel_calibration_count != _accel_calibration.calibration_count()) {
+		if ((accel_calibration_count != _accel_calibration.calibration_count())
+		    || (gyro_calibration_count != _gyro_calibration.calibration_count())
+		   ) {
 			// if calibration changed reset any existing learned calibration
-			_accel_cal_available = false;
-			_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
-
 			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;
-			_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
 
 			for (auto &learned_cal : _gyro_learned_calibration) {
 				learned_cal = {};
 			}
+
+			_in_flight_cal_available = false;
+			_last_calibration_update = hrt_absolute_time();
 		}
 
 		// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
@@ -165,9 +161,94 @@ bool VehicleIMU::ParametersUpdate(bool force)
 	return updated;
 }
 
+void VehicleIMU::SensorCalibrationUpdate()
+{
+	int accel_cal_index = 0;
+	int gyro_cal_index = 0;
+
+	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)
+		    && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 10_s)
+		    && (estimator_sensor_bias.timestamp > _last_calibration_update)) {
+
+			// find corresponding accel bias
+			if ((estimator_sensor_bias.accel_device_id != 0)
+			    && (estimator_sensor_bias.accel_device_id == _accel_calibration.device_id())
+			    && estimator_sensor_bias.accel_bias_valid
+			    && estimator_sensor_bias.accel_bias_stable
+			   ) {
+
+				if (accel_cal_index < IN_FLIGHT_CAL_COUNT) {
+
+					const Vector3f bias{estimator_sensor_bias.accel_bias};
+					const Vector3f bias_variance{estimator_sensor_bias.accel_bias_variance};
+
+					_accel_learned_calibration[accel_cal_index].device_id = estimator_sensor_bias.accel_device_id;
+					_accel_learned_calibration[accel_cal_index].offset = _accel_calibration.BiasCorrectedSensorOffset(bias);
+					_accel_learned_calibration[accel_cal_index].variance = bias_variance;
+					_accel_learned_calibration[accel_cal_index].estimator_instance = i;
+
+					_in_flight_cal_available = true;
+				}
+
+				accel_cal_index++;
+			}
+
+			// find corresponding gyro calibration
+			if ((estimator_sensor_bias.gyro_device_id != 0)
+			    && (estimator_sensor_bias.gyro_device_id == _gyro_calibration.device_id())
+			    && estimator_sensor_bias.gyro_bias_valid
+			    && estimator_sensor_bias.gyro_bias_stable
+			   ) {
+
+				if (gyro_cal_index < IN_FLIGHT_CAL_COUNT) {
+
+					const Vector3f bias{estimator_sensor_bias.gyro_bias};
+					const Vector3f bias_variance{estimator_sensor_bias.gyro_bias_variance};
+
+					_gyro_learned_calibration[gyro_cal_index].device_id = estimator_sensor_bias.gyro_device_id;
+					_gyro_learned_calibration[gyro_cal_index].offset = _gyro_calibration.BiasCorrectedSensorOffset(bias);
+					_gyro_learned_calibration[gyro_cal_index].variance = bias_variance;
+					_gyro_learned_calibration[gyro_cal_index].estimator_instance = i;
+
+					_in_flight_cal_available = true;
+				}
+
+				gyro_cal_index++;
+			}
+		}
+	}
+}
+
+void VehicleIMU::SensorCalibrationApplyAll()
+{
+	if (_in_flight_cal_available) {
+		bool calibration_param_save_needed = false;
+
+		if (calibration::SensorCalibrationApplyLearned<calibration::Accelerometer, IN_FLIGHT_CAL_COUNT>(_accel_calibration,
+				_instance, _accel_learned_calibration)) {
+			calibration_param_save_needed = true;
+		}
+
+		if (calibration::SensorCalibrationApplyLearned<calibration::Gyroscope, IN_FLIGHT_CAL_COUNT>(_gyro_calibration,
+				_instance, _gyro_learned_calibration)) {
+			calibration_param_save_needed = true;
+		}
+
+		if (calibration_param_save_needed) {
+			_last_calibration_update = hrt_absolute_time();
+			param_notify_changes();
+		}
+
+		_in_flight_cal_available = false;
+	}
+}
+
 void VehicleIMU::Run()
 {
-	const hrt_abstime now_us = hrt_absolute_time();
+	const hrt_abstime time_now_us = hrt_absolute_time();
 
 	const bool parameters_updated = ParametersUpdate();
 
@@ -245,7 +326,7 @@ void VehicleIMU::Run()
 					_gyro_update_latency_mean.reset();
 				}
 
-				const float time_run_s = now_us * 1e-6f;
+				const float time_run_s = time_now_us * 1e-6f;
 
 				_gyro_update_latency_mean.update(Vector2f{time_run_s - _gyro_timestamp_sample_last * 1e-6f, time_run_s - _gyro_timestamp_last * 1e-6f});
 
@@ -259,16 +340,19 @@ void VehicleIMU::Run()
 		}
 	}
 
-	if (_param_sens_imu_autocal.get() && !parameters_updated) {
-		if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
-		    && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) {
+	if (_param_sens_imu_autocal.get() && !parameters_updated
+	    && (time_now_us > _last_calibration_update + IN_FLIGHT_CALIBRATION_QUIET_PERIOD_US)) {
+
+		bool uncalibrated = !_accel_calibration.calibrated() || !_gyro_calibration.calibrated();
+
+		if ((_armed || uncalibrated)
+		    && (time_now_us > _in_flight_calibration_check_timestamp_last + 1_s)) {
 
 			SensorCalibrationUpdate();
-			_in_flight_calibration_check_timestamp_last = now_us;
+			_in_flight_calibration_check_timestamp_last = time_now_us;
 
 		} else if (!_armed) {
-			SensorCalibrationSaveAccel();
-			SensorCalibrationSaveGyro();
+			SensorCalibrationApplyAll();
 		}
 	}
 }
@@ -771,145 +855,4 @@ void VehicleIMU::PrintStatus()
 	_gyro_calibration.PrintStatus();
 }
 
-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)
-		    && (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)) {
-
-			// find corresponding accel bias
-			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};
-
-				_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_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};
-
-				_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;
-			}
-		}
-	}
-}
-
-void VehicleIMU::SensorCalibrationSaveAccel()
-{
-	if (_accel_cal_available) {
-		const Vector3f cal_orig{_accel_calibration.offset()};
-		bool initialised = false;
-		Vector3f offset_estimate{};
-		Vector3f bias_variance{};
-
-		// apply all valid saved offsets
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-			if (_accel_learned_calibration[i].valid) {
-				if (!initialised) {
-					bias_variance = _accel_learned_calibration[i].bias_variance;
-					offset_estimate = _accel_learned_calibration[i].offset;
-					initialised = true;
-
-				} else {
-					for (int axis_index = 0; axis_index < 3; axis_index++) {
-						const float sum_of_variances = _accel_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index);
-						const float k1 = bias_variance(axis_index) / sum_of_variances;
-						const float k2 = _accel_learned_calibration[i].bias_variance(axis_index) / sum_of_variances;
-						offset_estimate(axis_index) = k2 * offset_estimate(axis_index) + k1 * _accel_learned_calibration[i].offset(axis_index);
-						bias_variance(axis_index) *= k2;
-					}
-				}
-
-				// reset
-				_accel_learned_calibration[i] = {};
-			}
-		}
-
-		if (initialised && ((cal_orig - offset_estimate).longerThan(0.05f) || !_accel_calibration.calibrated())) {
-			if (_accel_calibration.set_offset(offset_estimate)) {
-				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
-					 _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(),
-					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
-					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
-
-				// save parameters with preferred calibration slot to current sensor index
-				if (_accel_calibration.ParametersSave(_sensor_accel_sub.get_instance())) {
-					param_notify_changes();
-				}
-
-				_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
-			}
-		}
-
-		// reset
-		_accel_cal_available = false;
-	}
-}
-
-void VehicleIMU::SensorCalibrationSaveGyro()
-{
-	if (_gyro_cal_available) {
-		const Vector3f cal_orig{_gyro_calibration.offset()};
-		bool initialised = false;
-		Vector3f offset_estimate{};
-		Vector3f bias_variance{};
-
-		// apply all valid saved offsets
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-			if (_gyro_learned_calibration[i].valid) {
-				if (!initialised) {
-					bias_variance = _gyro_learned_calibration[i].bias_variance;
-					offset_estimate = _gyro_learned_calibration[i].offset;
-					initialised = true;
-
-				} else {
-					for (int axis_index = 0; axis_index < 3; axis_index++) {
-						const float sum_of_variances = _gyro_learned_calibration[i].bias_variance(axis_index) + bias_variance(axis_index);
-						const float k1 = bias_variance(axis_index) / sum_of_variances;
-						const float k2 = _gyro_learned_calibration[i].bias_variance(axis_index) / sum_of_variances;
-						offset_estimate(axis_index) = k2 * offset_estimate(axis_index) + k1 * _gyro_learned_calibration[i].offset(axis_index);
-						bias_variance(axis_index) *= k2;
-					}
-				}
-
-				// reset
-				_gyro_learned_calibration[i] = {};
-			}
-		}
-
-		if (initialised && ((cal_orig - offset_estimate).longerThan(0.01f) || !_gyro_calibration.calibrated())) {
-			if (_gyro_calibration.set_offset(offset_estimate)) {
-				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
-					 _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(),
-					 (double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
-					 (double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
-
-				// save parameters with preferred calibration slot to current sensor index
-				if (_gyro_calibration.ParametersSave(_sensor_gyro_sub.get_instance())) {
-					param_notify_changes();
-				}
-
-				_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
-			}
-		}
-
-		// reset
-		_gyro_cal_available = false;
-	}
-}
-
 } // namespace sensors

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -41,6 +41,7 @@
 #include <lib/perf/perf_counter.h>
 #include <lib/sensor_calibration/Accelerometer.hpp>
 #include <lib/sensor_calibration/Gyroscope.hpp>
+#include <lib/sensor_calibration/Utilities.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -89,8 +90,7 @@ private:
 	inline void UpdateGyroVibrationMetrics(const matrix::Vector3f &angular_velocity);
 
 	void SensorCalibrationUpdate();
-	void SensorCalibrationSaveAccel();
-	void SensorCalibrationSaveGyro();
+	void SensorCalibrationApplyAll();
 
 	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
 	uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
@@ -170,21 +170,15 @@ private:
 
 	bool _armed{false};
 
-	bool _accel_cal_available{false};
-	bool _gyro_cal_available{false};
+	static constexpr int IN_FLIGHT_CAL_COUNT = 3;
+	calibration::InFlightCalibration _accel_learned_calibration[IN_FLIGHT_CAL_COUNT] {};
+	calibration::InFlightCalibration _gyro_learned_calibration[IN_FLIGHT_CAL_COUNT] {};
 
-	struct InFlightCalibration {
-		matrix::Vector3f offset{};
-		matrix::Vector3f bias_variance{};
-		bool valid{false};
-	};
-
-	InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
-	InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
-
-	static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s};
+	static constexpr hrt_abstime IN_FLIGHT_CALIBRATION_QUIET_PERIOD_US{10_s};
 
 	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
+	hrt_abstime _last_calibration_update{0};
+	bool _in_flight_cal_available{false};
 
 	perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
 	perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -135,11 +135,13 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 		_mag_comp_type = mag_comp_typ;
 
 
-		if (!_armed) {
-			bool calibration_updated = false;
+		bool calibration_updated = false;
+
+		// update mag priority (CAL_MAGx_PRIO)
+		for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) {
+
+			if (_calibration[mag].device_id() != 0) {
 
-			// update mag priority (CAL_MAGx_PRIO)
-			for (int mag = 0; mag < MAX_SENSOR_COUNT; mag++) {
 				bool clear_online_mag_cal = false;
 
 				const auto calibration_count = _calibration[mag].calibration_count();
@@ -173,7 +175,7 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 					// clear any mag bias estimate
 					_calibration_estimator_bias[mag].zero();
 
-					for (auto &cal : _mag_cal) {
+					for (auto &cal : _learned_calibration) {
 						// clear any in flight mag calibration
 						if (cal.device_id == _calibration[mag].device_id()) {
 							cal = {};
@@ -181,10 +183,10 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 					}
 				}
 			}
+		}
 
-			if (calibration_updated) {
-				_last_calibration_update = hrt_absolute_time();
-			}
+		if (calibration_updated) {
+			_last_calibration_update = hrt_absolute_time();
 		}
 
 		return true;
@@ -195,161 +197,118 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 
 void VehicleMagnetometer::UpdateMagBiasEstimate()
 {
-	if (_magnetometer_bias_estimate_sub.updated()) {
-		// Continuous mag calibration is running when not armed
-		magnetometer_bias_estimate_s mag_bias_est;
+	// Continuous mag calibration is running when not armed
+	magnetometer_bias_estimate_s mag_bias_est;
 
-		if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) {
-			bool parameters_notify = false;
+	if (_magnetometer_bias_estimate_sub.update(&mag_bias_est) && !_armed) {
+		bool parameters_notify = false;
 
-			for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-				if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) {
+		for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+			if (mag_bias_est.valid[mag_index] && (mag_bias_est.timestamp > _last_calibration_update)) {
 
-					const Vector3f bias{mag_bias_est.bias_x[mag_index],
-							    mag_bias_est.bias_y[mag_index],
-							    mag_bias_est.bias_z[mag_index]};
+				const Vector3f bias{mag_bias_est.bias_x[mag_index],
+						    mag_bias_est.bias_y[mag_index],
+						    mag_bias_est.bias_z[mag_index]};
 
-					_calibration_estimator_bias[mag_index] = bias;
+				_calibration_estimator_bias[mag_index] = bias;
 
-					// set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available
-					if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index]
-					    && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) {
+				// set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available
+				if (_param_sens_mag_autocal.get() && mag_bias_est.stable[mag_index]
+				    && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) {
 
-						const Vector3f old_offset = _calibration[mag_index].offset();
+					// set initial mag calibration
+					const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
 
-						// set initial mag calibration
-						const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
+					if (_calibration[mag_index].set_offset(offset)) {
+						// save parameters with preferred calibration slot to current sensor index
+						_calibration[mag_index].ParametersSave(mag_index);
 
-						if (_calibration[mag_index].set_offset(offset)) {
-							// save parameters with preferred calibration slot to current sensor index
-							_calibration[mag_index].ParametersSave(mag_index);
+						PX4_INFO("mag %d (%" PRIu32 ") setting offset: [%.3f, %.3f, %.3f]",
+							 mag_index, _calibration[mag_index].device_id(),
+							 (double)_calibration[mag_index].offset()(0),
+							 (double)_calibration[mag_index].offset()(1),
+							 (double)_calibration[mag_index].offset()(2));
 
-							PX4_INFO("mag %d (%" PRIu32 ") setting offsets [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f]",
-								 mag_index, _calibration[mag_index].device_id(),
-								 (double)old_offset(0), (double)old_offset(1), (double)old_offset(2),
-								 (double)_calibration[mag_index].offset()(0),
-								 (double)_calibration[mag_index].offset()(1),
-								 (double)_calibration[mag_index].offset()(2));
+						_calibration_estimator_bias[mag_index].zero();
 
-							_calibration_estimator_bias[mag_index].zero();
-
-							parameters_notify = true;
+						// clear any learned mag bias
+						for (auto &cal : _learned_calibration) {
+							if (cal.device_id == _calibration[mag_index].device_id()) {
+								cal = {};
+							}
 						}
+
+						parameters_notify = true;
 					}
 				}
 			}
+		}
 
-			if (parameters_notify) {
-				param_notify_changes();
-				_last_calibration_update = hrt_absolute_time();
+		if (parameters_notify) {
+			_last_calibration_update = hrt_absolute_time();
+			param_notify_changes();
+		}
+	}
+}
+
+void VehicleMagnetometer::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 ((hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 10_s)
+			    && (estimator_sensor_bias.timestamp > _last_calibration_update)
+			    && estimator_sensor_bias.mag_bias_valid
+			    && estimator_sensor_bias.mag_bias_stable
+			    && (estimator_sensor_bias.mag_device_id != 0)) {
+
+				// find corresponding mag calibration
+				for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+					if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) {
+
+						const Vector3f bias{estimator_sensor_bias.mag_bias};
+						const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance};
+
+						_learned_calibration[i].device_id = estimator_sensor_bias.mag_device_id;
+
+						// readd estimated bias that was removed before publishing vehicle_magnetometer
+						_learned_calibration[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) +
+										 _calibration_estimator_bias[mag_index];
+
+						_learned_calibration[i].variance = bias_variance;
+						_learned_calibration[i].estimator_instance = i;
+
+						_in_flight_cal_available = true;
+						break;
+					}
+				}
 			}
 		}
 	}
 }
 
-void VehicleMagnetometer::UpdateMagCalibration()
+void VehicleMagnetometer::SensorCalibrationApplyAll()
 {
-	// State variance assumed for magnetometer bias storage.
-	// This is a reference variance used to calculate the fraction of learned magnetometer bias that will be used to update the stored value.
-	// Larger values cause a larger fraction of the learned biases to be used.
-	static constexpr float magb_vref = 2.5e-7f;
-	static constexpr float min_var_allowed = magb_vref * 0.01f;
-	static constexpr float max_var_allowed = magb_vref * 100.f;
-
-	if (_armed) {
-		static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]);
-
-		for (int i = 0; i < math::min(_estimator_sensor_bias_subs.size(), mag_cal_size); i++) {
-			estimator_sensor_bias_s estimator_sensor_bias;
-
-			if (_estimator_sensor_bias_subs[i].update(&estimator_sensor_bias)) {
-
-				const Vector3f bias{estimator_sensor_bias.mag_bias};
-				const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance};
-
-				const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
-						   && (estimator_sensor_bias.mag_device_id != 0) &&
-						   estimator_sensor_bias.mag_bias_valid &&
-						   estimator_sensor_bias.mag_bias_stable &&
-						   (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
-
-				if (valid) {
-					// find corresponding mag calibration
-					for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-						if (_calibration[mag_index].device_id() == estimator_sensor_bias.mag_device_id) {
-
-							_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;
-
-							// readd estimated bias that was removed before publishing vehicle_magnetometer
-							_mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) +
-									     _calibration_estimator_bias[mag_index];
-
-							_mag_cal[i].variance = bias_variance;
-
-							_in_flight_mag_cal_available = true;
-							break;
-						}
-					}
-				}
-			}
-		}
-
-	} else if (_in_flight_mag_cal_available) {
-		// not armed and mag cal available
+	if (_in_flight_cal_available) {
 		bool calibration_param_save_needed = false;
-		// iterate through available bias estimates and fuse them sequentially using a Kalman Filter scheme
-		Vector3f state_variance{magb_vref, magb_vref, magb_vref};
 
 		for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
-			// apply all valid saved offsets
-			for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-				if ((_calibration[mag_index].device_id() != 0) && (_mag_cal[i].device_id == _calibration[mag_index].device_id())) {
-					const Vector3f mag_cal_orig{_calibration[mag_index].offset()};
 
-					// calculate weighting using ratio of variances and update stored bias values
-					const Vector3f &observation{_mag_cal[i].offset};
-					const Vector3f &obs_variance{_mag_cal[i].variance};
-
-					const Vector3f innovation{mag_cal_orig - observation};
-					const Vector3f innovation_variance{state_variance + obs_variance};
-					const Vector3f kalman_gain{state_variance.edivide(innovation_variance)};
-
-					// new offset
-					const Vector3f mag_cal_offset{mag_cal_orig - innovation.emult(kalman_gain)};
-
-					for (int axis_index = 0; axis_index < 3; axis_index++) {
-						state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain(axis_index)), 0.f);
-					}
-
-					if (_calibration[mag_index].set_offset(mag_cal_offset)) {
-
-						PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])",
-							 mag_index, _calibration[mag_index].device_id(), i,
-							 (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2),
-							 (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
-							 (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2));
-
-						_calibration[mag_index].ParametersSave();
-
-						_calibration_estimator_bias[mag_index].zero();
-
-						calibration_param_save_needed = true;
-					}
-				}
+			if (calibration::SensorCalibrationApplyLearned<calibration::Magnetometer, ORB_MULTI_MAX_INSTANCES>
+			    (_calibration[mag_index], mag_index, _learned_calibration)) {
+				calibration_param_save_needed = true;
+				_calibration_estimator_bias[mag_index].zero();
 			}
 		}
 
 		if (calibration_param_save_needed) {
-			param_notify_changes();
 			_last_calibration_update = hrt_absolute_time();
+			param_notify_changes();
 		}
 
-		// clear all
-		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
-			_mag_cal[i] = {};
-		}
-
-		_in_flight_mag_cal_available = false;
+		_in_flight_cal_available = false;
 	}
 }
 
@@ -393,7 +352,7 @@ void VehicleMagnetometer::Run()
 
 	const hrt_abstime time_now_us = hrt_absolute_time();
 
-	const bool parameter_update = ParametersUpdate();
+	const bool parameters_updated = ParametersUpdate();
 
 	// check vehicle status for changes to armed state
 	if (_vehicle_control_mode_sub.updated()) {
@@ -478,7 +437,7 @@ void VehicleMagnetometer::Run()
 
 	if (best_index >= 0) {
 		// handle selection change (don't process on same iteration as parameter update)
-		if ((_selected_sensor_sub_index != best_index) && !parameter_update) {
+		if ((_selected_sensor_sub_index != best_index) && !parameters_updated) {
 			// clear all registered callbacks
 			for (auto &sub : _sensor_sub) {
 				sub.unregisterCallback();
@@ -557,7 +516,7 @@ void VehicleMagnetometer::Run()
 		}
 	}
 
-	if (!parameter_update && _param_sens_mag_mode.get()) {
+	if (!parameters_updated && _param_sens_mag_mode.get()) {
 		CheckFailover(time_now_us);
 	}
 
@@ -565,7 +524,27 @@ void VehicleMagnetometer::Run()
 		calcMagInconsistency();
 	}
 
-	UpdateMagCalibration();
+	if (_param_sens_mag_autocal.get() && !parameters_updated
+	    && (time_now_us > _last_calibration_update + IN_FLIGHT_CALIBRATION_QUIET_PERIOD_US)) {
+
+		bool uncalibrated = false;
+
+		for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+			if (_calibration[i].enabled() && !_calibration[i].calibrated()) {
+				uncalibrated = true;
+			}
+		}
+
+		if ((_armed || uncalibrated)
+		    && (time_now_us > _in_flight_calibration_check_timestamp_last + 1_s)) {
+
+			SensorCalibrationUpdate();
+			_in_flight_calibration_check_timestamp_last = time_now_us;
+
+		} else if (!_armed) {
+			SensorCalibrationApplyAll();
+		}
+	}
 
 	UpdateStatus();
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -36,6 +36,7 @@
 #include "data_validator/DataValidatorGroup.hpp"
 
 #include <lib/sensor_calibration/Magnetometer.hpp>
+#include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/conversion/rotation.h>
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
@@ -91,8 +92,10 @@ private:
 	 */
 	void calcMagInconsistency();
 
+	void SensorCalibrationApplyAll();
+	void SensorCalibrationUpdate();
+
 	void UpdateMagBiasEstimate();
-	void UpdateMagCalibration();
 	void UpdatePowerCompensation();
 
 	static constexpr int MAX_SENSOR_COUNT = 4;
@@ -118,13 +121,10 @@ private:
 	// Used to check, save and use learned magnetometer biases
 	uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
 
-	bool _in_flight_mag_cal_available{false}; ///< from navigation filter
-
-	struct MagCal {
-		uint32_t device_id{0};
-		matrix::Vector3f offset{};
-		matrix::Vector3f variance{};
-	} _mag_cal[ORB_MULTI_MAX_INSTANCES] {};
+	static constexpr hrt_abstime IN_FLIGHT_CALIBRATION_QUIET_PERIOD_US{10_s};
+	calibration::InFlightCalibration _learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
+	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
+	bool _in_flight_cal_available{false}; ///< from navigation filter
 
 	uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {
 		{this, ORB_ID(sensor_mag), 0},