From e0bcc17f5b1e0dcbe422dc79875dc599f655dafa Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 1 Oct 2021 14:19:10 +0200 Subject: [PATCH] mbe: integrate pre-takeoff mag bias estimate in VehicleMagnetometer --- .../VehicleMagnetometer.cpp | 104 ++++++++++++------ .../VehicleMagnetometer.hpp | 8 +- 2 files changed, 78 insertions(+), 34 deletions(-) diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 08d5dd51f2..9b6500cd77 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/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())) { + 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()}; - 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; - // 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); + } - 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)) { + + 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; + } + + // clear + _mag_cal[i].device_id = 0; + _mag_cal[i].mag_offset.zero(); + _mag_cal[i].mag_bias_variance.zero(); } - - 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)); - - 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(); } } } @@ -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]); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index ca62cf8b9b..5e32caa323 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -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_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