mbe: integrate pre-takeoff mag bias estimate in VehicleMagnetometer

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -174,7 +174,7 @@ void VehicleMagnetometer::MagCalibrationUpdate()
 							_mag_cal[i].mag_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias);
 							_mag_cal[i].mag_bias_variance = bias_variance;
 
-							_mag_cal_available = true;
+							_in_flight_mag_cal_available = true;
 
 							if ((old_offset - _mag_cal[i].mag_offset).longerThan(0.01f)) {
 								PX4_DEBUG("Mag %d (%d) est. offset saved: [% 05.3f % 05.3f % 05.3f] (bias [% 05.3f % 05.3f % 05.3f])",
@@ -190,47 +190,62 @@ void VehicleMagnetometer::MagCalibrationUpdate()
 			}
 		}
 
-	} else if (_mag_cal_available) {
+		if (_in_flight_mag_cal_available || _on_ground_mag_bias_estimate_available) {
+			_should_save_on_disarm = true;
+		}
+
+	} else if (_should_save_on_disarm) {
 		// not armed and mag 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++) {
+			// first, apply the calibration from the mag bias estimator (not EKF2)
+			if (_on_ground_mag_bias_estimate_available) {
+				const Vector3f new_offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
+
+				if (_calibration[mag_index].set_offset(new_offset)) {
+					calibration_param_save_needed = true;
+					_calibration_estimator_bias[mag_index].zero();
+				}
+			}
+
 			// 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()};
-					Vector3f mag_cal_offset{_calibration[mag_index].offset()};
-
-					// calculate weighting using ratio of variances and update stored bias values
-					const Vector3f &observation = _mag_cal[i].mag_offset;
-					const Vector3f &obs_variance = _mag_cal[i].mag_bias_variance;
-
-					for (int axis_index = 0; axis_index < 3; axis_index++) {
-						const float innovation_variance = state_variance(axis_index) + obs_variance(axis_index);
-						const float innovation = mag_cal_offset(axis_index) - observation(axis_index);
-						const float kalman_gain = state_variance(axis_index) / innovation_variance;
-						mag_cal_offset(axis_index) -= innovation * kalman_gain;
-						state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f);
-					}
+			if (_in_flight_mag_cal_available) {
+				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()};
+						Vector3f mag_cal_offset{_calibration[mag_index].offset()};
+
+						// calculate weighting using ratio of variances and update stored bias values
+						const Vector3f &observation = _mag_cal[i].mag_offset;
+						const Vector3f &obs_variance = _mag_cal[i].mag_bias_variance;
+
+						for (int axis_index = 0; axis_index < 3; axis_index++) {
+							const float innovation_variance = state_variance(axis_index) + obs_variance(axis_index);
+							const float innovation = mag_cal_offset(axis_index) - observation(axis_index);
+							const float kalman_gain = state_variance(axis_index) / innovation_variance;
+							mag_cal_offset(axis_index) -= innovation * kalman_gain;
+							state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain), 0.f);
+						}
 
-					if (_calibration[mag_index].set_offset(mag_cal_offset)) {
+						if (_calibration[mag_index].set_offset(mag_cal_offset)) {
 
-						PX4_INFO("%d (%" PRIu32 ") EST:%d offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
-							 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].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2));
+							PX4_INFO("%d (%" PRIu32 ") EST:%d offset committed: [%.2f %.2f %.2f]->[%.2f %.2f %.2f] (full [%.2f %.2f %.2f])",
+								 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].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2));
 
-						calibration_param_save_needed = true;
-					}
+							calibration_param_save_needed = true;
+						}
 
-					// clear
-					_mag_cal[i].device_id = 0;
-					_mag_cal[i].mag_offset.zero();
-					_mag_cal[i].mag_bias_variance.zero();
+						// clear
+						_mag_cal[i].device_id = 0;
+						_mag_cal[i].mag_offset.zero();
+						_mag_cal[i].mag_bias_variance.zero();
+					}
 				}
 			}
 		}
@@ -242,7 +257,29 @@ void VehicleMagnetometer::MagCalibrationUpdate()
 				}
 			}
 
-			_mag_cal_available = false;
+			param_notify_changes();
+		}
+
+		_should_save_on_disarm = false;
+		_on_ground_mag_bias_estimate_available = false;
+		_in_flight_mag_cal_available = false;
+
+	} else {
+		// Continuous mag calibration is running when not armed
+		if (_magnetometer_bias_estimate_sub.updated()) {
+			magnetometer_bias_estimate_s mag_bias_est;
+
+			if (_magnetometer_bias_estimate_sub.copy(&mag_bias_est)) {
+				for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) {
+					if (mag_bias_est.valid[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;
+						_on_ground_mag_bias_estimate_available = true;
+					}
+				}
+			}
 		}
 	}
 }
@@ -322,6 +359,7 @@ void VehicleMagnetometer::Run()
 				}
 
 				if (_calibration[uorb_index].enabled()) {
+
 					if (!was_advertised) {
 						if (uorb_index > 0) {
 							/* the first always exists, but for each further sensor, add a new validator */
@@ -342,7 +380,7 @@ void VehicleMagnetometer::Run()
 						}
 					}
 
-					const Vector3f vect{_calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z})};
+					const Vector3f vect{_calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z}) - _calibration_estimator_bias[uorb_index]};
 
 					float mag_array[3] {vect(0), vect(1), vect(2)};
 					_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -53,6 +53,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/estimator_sensor_bias.h>
+#include <uORB/topics/magnetometer_bias_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/sensor_preflight_mag.h>
@@ -103,12 +104,15 @@ private:
 
 	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
+	uORB::Subscription _magnetometer_bias_estimate_sub{ORB_ID(magnetometer_bias_estimate)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 
 	// 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 _mag_cal_available{false};
+	bool _in_flight_mag_cal_available{false}; ///< from navigation filter
+	bool _on_ground_mag_bias_estimate_available{false}; ///< from pre-takeoff mag_bias_estimator
+	bool _should_save_on_disarm{false};
 
 	struct MagCal {
 		uint32_t device_id{0};
@@ -123,6 +127,8 @@ private:
 		{this, ORB_ID(sensor_mag), 3}
 	};
 
+	matrix::Vector3f _calibration_estimator_bias[MAX_SENSOR_COUNT] {};
+
 	calibration::Magnetometer _calibration[MAX_SENSOR_COUNT];
 
 	// Magnetometer interference compensation