diff --git a/msg/estimator_sensor_bias.msg b/msg/estimator_sensor_bias.msg index 0ede805560..1f4e92ca1a 100644 --- a/msg/estimator_sensor_bias.msg +++ b/msg/estimator_sensor_bias.msg @@ -11,11 +11,14 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s) float32[3] gyro_bias_variance +bool gyro_bias_valid uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2) float32[3] accel_bias_variance +bool accel_bias_valid uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss) float32[3] mag_bias_variance +bool mag_bias_valid diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp index b836a14532..94ded6cc6f 100644 --- a/src/lib/sensor_calibration/Accelerometer.cpp +++ b/src/lib/sensor_calibration/Accelerometer.cpp @@ -123,6 +123,30 @@ void Accelerometer::SensorCorrectionsUpdate(bool force) } } +bool Accelerometer::set_offset(const Vector3f &offset) +{ + if (Vector3f(_offset - offset).longerThan(0.001f)) { + _offset = offset; + + _calibration_count++; + return true; + } + + return false; +} + +bool Accelerometer::set_scale(const Vector3f &scale) +{ + if (Vector3f(_scale - scale).longerThan(0.001f)) { + _scale = scale; + + _calibration_count++; + return true; + } + + return false; +} + void Accelerometer::set_rotation(Rotation rotation) { _rotation_enum = rotation; @@ -183,27 +207,11 @@ void Accelerometer::ParametersUpdate() _priority = new_priority; } - bool calibration_changed = false; - // CAL_ACCx_OFF{X,Y,Z} - const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index); - - if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) { - calibration_changed = true; - _offset = offset; - } + set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); // CAL_ACCx_SCALE{X,Y,Z} - const Vector3f scale = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index); - - if (Vector3f(_scale - scale).norm_squared() > 0.001f * 0.001f) { - calibration_changed = true; - _scale = scale; - } - - if (calibration_changed) { - _calibration_count++; - } + set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index)); } else { Reset(); diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index 5d95101b70..c0a4ca667e 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -62,17 +62,19 @@ public: void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } void set_device_id(uint32_t device_id, bool external = false); void set_external(bool external = true); - void set_offset(const matrix::Vector3f &offset) { _offset = offset; } - void set_scale(const matrix::Vector3f &scale) { _scale = scale; } + bool set_offset(const matrix::Vector3f &offset); + bool set_scale(const matrix::Vector3f &scale); void set_rotation(Rotation rotation); uint8_t calibration_count() const { return _calibration_count; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } bool external() const { return _external; } + const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } const matrix::Dcmf &rotation() const { return _rotation; } const Rotation &rotation_enum() const { return _rotation_enum; } + const matrix::Vector3f &scale() const { return _scale; } // apply offsets and scale // rotate corrected measurements from sensor to body frame diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp index fbb3725011..9baba5b694 100644 --- a/src/lib/sensor_calibration/Gyroscope.cpp +++ b/src/lib/sensor_calibration/Gyroscope.cpp @@ -123,6 +123,18 @@ void Gyroscope::SensorCorrectionsUpdate(bool force) } } +bool Gyroscope::set_offset(const Vector3f &offset) +{ + if (Vector3f(_offset - offset).longerThan(0.001f)) { + _offset = offset; + + _calibration_count++; + return true; + } + + return false; +} + void Gyroscope::set_rotation(Rotation rotation) { _rotation_enum = rotation; @@ -183,19 +195,8 @@ void Gyroscope::ParametersUpdate() _priority = new_priority; } - bool calibration_changed = false; - // CAL_GYROx_OFF{X,Y,Z} - const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index); - - if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) { - calibration_changed = true; - _offset = offset; - } - - if (calibration_changed) { - _calibration_count++; - } + set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); } else { Reset(); diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp index 0827b90bc6..2d6e6d010d 100644 --- a/src/lib/sensor_calibration/Gyroscope.hpp +++ b/src/lib/sensor_calibration/Gyroscope.hpp @@ -62,13 +62,14 @@ public: void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } void set_device_id(uint32_t device_id, bool external = false); void set_external(bool external = true); - void set_offset(const matrix::Vector3f &offset) { _offset = offset; } + bool set_offset(const matrix::Vector3f &offset); void set_rotation(Rotation rotation); uint8_t calibration_count() const { return _calibration_count; } uint32_t device_id() const { return _device_id; } bool enabled() const { return (_priority > 0); } bool external() const { return _external; } + const matrix::Vector3f &offset() const { return _offset; } const int32_t &priority() const { return _priority; } const matrix::Dcmf &rotation() const { return _rotation; } const Rotation &rotation_enum() const { return _rotation_enum; } diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index 02d0e2bfb7..b5b7a6b38a 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -83,23 +83,49 @@ void Magnetometer::set_external(bool external) _external = external; } -void Magnetometer::set_scale(const Vector3f &scale) +bool Magnetometer::set_offset(const Vector3f &offset) { - _scale(0, 0) = scale(0); - _scale(1, 1) = scale(1); - _scale(2, 2) = scale(2); + if (Vector3f(_offset - offset).longerThan(0.001f)) { + _offset = offset; + + _calibration_count++; + return true; + } + + return false; } -void Magnetometer::set_offdiagonal(const Vector3f &offdiagonal) +bool Magnetometer::set_scale(const Vector3f &scale) { - _scale(0, 1) = offdiagonal(0); - _scale(1, 0) = offdiagonal(0); + if (Vector3f(_scale.diag() - scale).longerThan(0.001f)) { + _scale(0, 0) = scale(0); + _scale(1, 1) = scale(1); + _scale(2, 2) = scale(2); - _scale(0, 2) = offdiagonal(1); - _scale(2, 0) = offdiagonal(1); + _calibration_count++; + return true; + } - _scale(1, 2) = offdiagonal(2); - _scale(2, 1) = offdiagonal(2); + return false; +} + +bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal) +{ + if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiagonal).longerThan(0.001f)) { + _scale(0, 1) = offdiagonal(0); + _scale(1, 0) = offdiagonal(0); + + _scale(0, 2) = offdiagonal(1); + _scale(2, 0) = offdiagonal(1); + + _scale(1, 2) = offdiagonal(2); + _scale(2, 1) = offdiagonal(2); + + _calibration_count++; + return true; + } + + return false; } void Magnetometer::set_rotation(Rotation rotation) @@ -162,42 +188,14 @@ void Magnetometer::ParametersUpdate() _priority = new_priority; } - bool calibration_changed = false; - // CAL_MAGx_OFF{X,Y,Z} - const Vector3f offset = GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index); - - if (Vector3f(_offset - offset).norm_squared() > 0.001f * 0.001f) { - calibration_changed = true; - _offset = offset; - } + set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index)); // CAL_MAGx_SCALE{X,Y,Z} - const Vector3f diag = GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index); - - if (Vector3f(_scale.diag() - diag).norm_squared() > 0.001f * 0.001f) { - calibration_changed = true; - } + set_scale(GetCalibrationParamsVector3f(SensorString(), "SCALE", _calibration_index)); // CAL_MAGx_ODIAG{X,Y,Z} - const Vector3f offdiag = GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index); - - if (Vector3f(Vector3f{_scale(0, 1), _scale(0, 2), _scale(1, 2)} - offdiag).norm_squared() > 0.001f * 0.001f) { - calibration_changed = true; - } - - if (calibration_changed) { - - float scale[9] { - diag(0), offdiag(0), offdiag(1), - offdiag(0), diag(1), offdiag(2), - offdiag(1), offdiag(2), diag(2) - }; - _scale = Matrix3f{scale}; - - _calibration_count++; - } - + set_offdiagonal(GetCalibrationParamsVector3f(SensorString(), "ODIAG", _calibration_index)); // CAL_MAGx_COMP{X,Y,Z} _power_compensation = GetCalibrationParamsVector3f(SensorString(), "COMP", _calibration_index); diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 13ddc83eff..ce3ddb5f0f 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -63,9 +63,9 @@ public: void set_calibration_index(uint8_t calibration_index) { _calibration_index = calibration_index; } void set_device_id(uint32_t device_id, bool external = false); void set_external(bool external = true); - void set_offset(const matrix::Vector3f &offset) { _offset = offset; } - void set_scale(const matrix::Vector3f &scale); - void set_offdiagonal(const matrix::Vector3f &offdiagonal); + bool set_offset(const matrix::Vector3f &offset); + bool set_scale(const matrix::Vector3f &scale); + bool set_offdiagonal(const matrix::Vector3f &offdiagonal); void set_rotation(Rotation rotation); uint8_t calibration_count() const { return _calibration_count; } @@ -85,6 +85,12 @@ public: return _rotation * (_scale * ((data + _power * _power_compensation) - _offset)); } + // Compute sensor offset from bias (board frame) + matrix::Vector3f BiasCorrectedSensorOffset(const matrix::Vector3f &bias) const + { + return _scale.I() * _rotation.I() * bias + _offset; + } + bool ParametersSave(); void ParametersUpdate(); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index f2b5a1d981..71f538a4e3 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -903,16 +903,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } if (param_save) { - // reset the learned EKF mag in-flight bias offsets which have been learned for the previous - // sensor calibration and will be invalidated by a new sensor calibration - for (int axis = 0; axis < 3; axis++) { - char axis_char = 'X' + axis; - char str[20] {}; - sprintf(str, "EKF2_MAGBIAS_%c", axis_char); - float offset = 0.f; - param_set_no_notification(param_find(str), &offset); - } - param_notify_changes(); } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 1f8d0e8bea..869c645e36 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -202,24 +202,6 @@ int EKF2::print_status() return 0; } -template -void EKF2::update_mag_bias(Param &mag_bias_param, int axis_index) -{ - // calculate weighting using ratio of variances and update stored bias values - const float weighting = constrain(_param_ekf2_magb_vref.get() / (_param_ekf2_magb_vref.get() + - _last_valid_variance(axis_index)), 0.0f, _param_ekf2_magb_k.get()); - const float mag_bias_saved = mag_bias_param.get(); - - _last_valid_mag_cal(axis_index) = weighting * _last_valid_mag_cal(axis_index) + mag_bias_saved; - - mag_bias_param.set(_last_valid_mag_cal(axis_index)); - - // save new parameters unless in multi-instance mode - if (!_multi_mode) { - mag_bias_param.commit_no_notification(); - } -} - void EKF2::Run() { if (should_exit()) { @@ -874,39 +856,39 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) const Vector3f gyro_bias{_ekf.getGyroBias()}; const Vector3f accel_bias{_ekf.getAccelBias()}; - - // the saved mag bias EKF2_MAGBIAS_{X,Y,Z} is subtracted from sensor mag before going into ecl/EKF - const Vector3f mag_cal{_param_ekf2_magbias_x.get(), _param_ekf2_magbias_y.get(), _param_ekf2_magbias_z.get()}; - const Vector3f mag_bias{_ekf.getMagBias() + mag_cal}; + const Vector3f mag_bias{_mag_cal_last_bias}; // only publish on change - if ((gyro_bias - _last_gyro_bias).longerThan(0.001f) - || (accel_bias - _last_accel_bias).longerThan(0.001f) - || (mag_bias - _last_mag_bias).longerThan(0.001f)) { + if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f) + || (accel_bias - _last_accel_bias_published).longerThan(0.001f) + || (mag_bias - _last_mag_bias_published).longerThan(0.001f)) { // take device ids from sensor_selection_s if not using specific vehicle_imu_s if (_device_id_gyro != 0) { bias.gyro_device_id = _device_id_gyro; gyro_bias.copyTo(bias.gyro_bias); _ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance); + bias.gyro_bias_valid = true; - _last_gyro_bias = gyro_bias; + _last_gyro_bias_published = gyro_bias; } if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) { bias.accel_device_id = _device_id_accel; accel_bias.copyTo(bias.accel_bias); _ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance); + bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias; - _last_accel_bias = accel_bias; + _last_accel_bias_published = accel_bias; } if (_device_id_mag != 0) { bias.mag_device_id = _device_id_mag; mag_bias.copyTo(bias.mag_bias); - _ekf.getMagBiasVariance().copyTo(bias.mag_bias_variance); + _mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance); + bias.mag_bias_valid = _mag_cal_available; - _last_mag_bias = mag_bias; + _last_mag_bias_published = mag_bias; } bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); @@ -1389,47 +1371,35 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) vehicle_magnetometer_s magnetometer; if (_magnetometer_sub.update(&magnetometer)) { - // Reset learned bias parameters if there has been a persistant change in magnetometer ID - // Do not reset parmameters when armed to prevent potential time slips casued by parameter set - // and notification events - // Check if there has been a persistant change in magnetometer ID - if (magnetometer.device_id != 0 - && (magnetometer.device_id != (uint32_t)_param_ekf2_magbias_id.get())) { - if (_invalid_mag_id_count < 200) { - _invalid_mag_id_count++; + bool reset = false; + + // check if magnetometer has changed + if (magnetometer.device_id != _device_id_mag) { + if (_device_id_mag != 0) { + PX4_WARN("%d - mag sensor ID changed %d -> %d", _instance, _device_id_mag, magnetometer.device_id); } - } else { - if (_invalid_mag_id_count > 0) { - _invalid_mag_id_count--; - } + reset = true; + + } else if (magnetometer.calibration_count > _mag_calibration_count) { + // existing calibration has changed, reset saved mag bias + PX4_DEBUG("%d - mag %d calibration updated, resetting bias", _instance, _device_id_mag); + reset = true; } - _device_id_mag = magnetometer.device_id; + if (reset) { + _ekf.resetMagBias(); + _device_id_mag = magnetometer.device_id; + _mag_calibration_count = magnetometer.calibration_count; - if (!_armed && (_invalid_mag_id_count > 100)) { - // the sensor ID used for the last saved mag bias is not confirmed to be the same as the current sensor ID - // this means we need to reset the learned bias values to zero - _param_ekf2_magbias_x.set(0.f); - _param_ekf2_magbias_y.set(0.f); - _param_ekf2_magbias_z.set(0.f); - _param_ekf2_magbias_id.set(magnetometer.device_id); - - if (!_multi_mode) { - _param_ekf2_magbias_x.reset(); - _param_ekf2_magbias_y.reset(); - _param_ekf2_magbias_z.reset(); - _param_ekf2_magbias_id.commit(); - PX4_INFO("%d - mag sensor ID changed to %i", _instance, _param_ekf2_magbias_id.get()); - } - - _invalid_mag_id_count = 0; + // reset magnetometer bias learning + _mag_cal_total_time_us = 0; + _mag_cal_last_us = 0; + _mag_cal_available = false; } - const Vector3f mag_cal_bias{_param_ekf2_magbias_x.get(), _param_ekf2_magbias_y.get(), _param_ekf2_magbias_z.get()}; - - _ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga} - mag_cal_bias}); + _ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}}); ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); @@ -1484,56 +1454,32 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) // the EKF is operating in the correct mode and there are no filter faults if (_ekf.control_status_flags().in_air && _ekf.control_status_flags().mag_3D && (_ekf.fault_status().value == 0)) { - if (_last_magcal_us == 0) { - _last_magcal_us = timestamp; + if (_mag_cal_last_us != 0) { + _mag_cal_total_time_us += timestamp - _mag_cal_last_us; - } else { - _total_cal_time_us += timestamp - _last_magcal_us; - _last_magcal_us = timestamp; - } - - // Start checking mag bias estimates when we have accumulated sufficient calibration time - if (_total_cal_time_us > 30_s) { - // we have sufficient accumulated valid flight time to form a reliable bias estimate - // check that the state variance for each axis is within a range indicating filter convergence - const float max_var_allowed = 100.0f * _param_ekf2_magb_vref.get(); - const float min_var_allowed = 0.01f * _param_ekf2_magb_vref.get(); - - // Declare all bias estimates invalid if any variances are out of range - const Vector3f mag_bias_variance{_ekf.getMagBiasVariance()}; - - // Store valid estimates and their associated variances - if ((mag_bias_variance.min() >= min_var_allowed) && (mag_bias_variance.max() <= max_var_allowed)) { - _last_valid_mag_cal = _ekf.getMagBias(); - _last_valid_variance = mag_bias_variance; - _valid_cal_available = true; + // Start checking mag bias estimates when we have accumulated sufficient calibration time + if (_mag_cal_total_time_us > 30_s) { + _mag_cal_last_bias = _ekf.getMagBias(); + _mag_cal_last_bias_variance = _ekf.getMagBiasVariance(); + _mag_cal_available = true; } } - } else if (_ekf.fault_status().value != 0) { - // if a filter fault has occurred, assume previous learning was invalid and do not - // count it towards total learning time. - _total_cal_time_us = 0; - _valid_cal_available = false; + _mag_cal_last_us = timestamp; } else { // conditions are NOT OK for learning magnetometer bias, reset timestamp // but keep the accumulated calibration time - _last_magcal_us = timestamp; + _mag_cal_last_us = 0; + + if (_ekf.fault_status().value != 0) { + // if a filter fault has occurred, assume previous learning was invalid and do not + // count it towards total learning time. + _mag_cal_total_time_us = 0; + } } - // Check and save the last valid calibration when we are disarmed if (!_armed) { - if (_valid_cal_available) { - update_mag_bias(_param_ekf2_magbias_x, 0); - update_mag_bias(_param_ekf2_magbias_y, 1); - update_mag_bias(_param_ekf2_magbias_z, 2); - - // reset to prevent data being saved too frequently - _total_cal_time_us = 0; - _valid_cal_available = false; - } - // update stored declination value if (!_mag_decl_saved) { float declination_deg; diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 97aff60ea1..b4575b280f 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -121,9 +121,6 @@ public: private: void Run() override; - template - void update_mag_bias(Param &mag_bias_param, int axis_index); - void PublishAttitude(const hrt_abstime ×tamp); void PublishEkfDriftMetrics(const hrt_abstime ×tamp); void PublishGlobalPosition(const hrt_abstime ×tamp); @@ -173,16 +170,13 @@ private: perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; - // Initialise time stamps used to send sensor data to the EKF and for logging - uint8_t _invalid_mag_id_count = 0; ///< number of times an invalid magnetomer device ID has been detected - // Used to check, save and use learned magnetometer biases - hrt_abstime _last_magcal_us = 0; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec) - hrt_abstime _total_cal_time_us = 0; ///< accumulated calibration time since the last save + hrt_abstime _mag_cal_last_us{0}; ///< last time the EKF was operating a mode that estimates magnetomer biases (uSec) + hrt_abstime _mag_cal_total_time_us{0}; ///< accumulated calibration time since the last save - Vector3f _last_valid_mag_cal{}; ///< last valid XYZ magnetometer bias estimates (Gauss) - Vector3f _last_valid_variance{}; ///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2) - bool _valid_cal_available{false}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available + Vector3f _mag_cal_last_bias{}; ///< last valid XYZ magnetometer bias estimates (Gauss) + Vector3f _mag_cal_last_bias_variance{}; ///< variances for the last valid magnetometer XYZ bias estimates (Gauss**2) + bool _mag_cal_available{false}; ///< true when an unsaved valid calibration for the XYZ magnetometer bias is available // Used to control saving of mag declination to be used on next startup bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved @@ -195,6 +189,7 @@ private: float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 uint8_t _imu_calibration_count{0}; + uint8_t _mag_calibration_count{0}; uint32_t _device_id_accel{0}; uint32_t _device_id_baro{0}; @@ -203,9 +198,9 @@ private: Vector3f _last_local_position_for_gpos{}; - Vector3f _last_accel_bias{}; - Vector3f _last_gyro_bias{}; - Vector3f _last_mag_bias{}; + Vector3f _last_accel_bias_published{}; + Vector3f _last_gyro_bias_published{}; + Vector3f _last_mag_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements @@ -457,17 +452,6 @@ private: (ParamExtFloat) _param_ekf2_angerr_init, ///< 1-sigma tilt error after initial alignment using gravity vector (rad) - // EKF saved XYZ magnetometer bias values - (ParamFloat) _param_ekf2_magbias_x, ///< X magnetometer bias (Gauss) - (ParamFloat) _param_ekf2_magbias_y, ///< Y magnetometer bias (Gauss) - (ParamFloat) _param_ekf2_magbias_z, ///< Z magnetometer bias (Gauss) - (ParamInt) - _param_ekf2_magbias_id, ///< ID of the magnetometer sensor used to learn the bias values - (ParamFloat) - _param_ekf2_magb_vref, ///< Assumed error variance of previously saved magnetometer bias estimates (Gauss**2) - (ParamFloat) - _param_ekf2_magb_k, ///< maximum fraction of the learned magnetometer bias that is saved at each disarm - // EKF accel bias learning control (ParamExtFloat) _param_ekf2_abl_lim, ///< Accelerometer bias learning limit (m/s**2) (ParamExtFloat) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 98e2c8f1e3..f2b86c3173 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -428,17 +428,6 @@ PARAM_DEFINE_FLOAT(EKF2_BETA_GATE, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_BETA_NOISE, 0.3f); -/** - * Magnetic declination - * - * @group EKF2 - * @volatile - * @category system - * @unit deg - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0); - /** * Gate size for magnetic heading fusion * @@ -1049,82 +1038,6 @@ PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f); */ PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f); -/** - * Learned value of magnetometer X axis bias. - * This is the amount of X-axis magnetometer bias learned by the EKF and saved from the last flight. It must be set to zero if the ground based magnetometer calibration is repeated. - * - * @group EKF2 - * @min -0.5 - * @max 0.5 - * @reboot_required true - * @volatile - * @category system - * @unit gauss - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_MAGBIAS_X, 0.0f); - -/** - * Learned value of magnetometer Y axis bias. - * This is the amount of Y-axis magnetometer bias learned by the EKF and saved from the last flight. It must be set to zero if the ground based magnetometer calibration is repeated. - * - * @group EKF2 - * @min -0.5 - * @max 0.5 - * @reboot_required true - * @volatile - * @category system - * @unit gauss - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_MAGBIAS_Y, 0.0f); - -/** - * Learned value of magnetometer Z axis bias. - * This is the amount of Z-axis magnetometer bias learned by the EKF and saved from the last flight. It must be set to zero if the ground based magnetometer calibration is repeated. - * - * @group EKF2 - * @min -0.5 - * @max 0.5 - * @reboot_required true - * @volatile - * @category system - * @unit gauss - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_MAGBIAS_Z, 0.0f); - -/** - * ID of Magnetometer the learned bias is for. - * - * @group EKF2 - * @reboot_required true - * @category system - */ -PARAM_DEFINE_INT32(EKF2_MAGBIAS_ID, 0); - -/** - * 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. Smaller values will make the stored bias data adjust more slowly from flight to flight. Larger values will make it adjust faster. - * - * @group EKF2 - * @reboot_required true - * @unit gauss^2 - * @decimal 8 - */ -PARAM_DEFINE_FLOAT(EKF2_MAGB_VREF, 2.5E-7f); - -/** - * Maximum fraction of learned mag bias saved at each disarm. - * Smaller values make the saved mag bias learn slower from flight to flight. Larger values make it learn faster. Must be > 0.0 and <= 1.0. - * - * @group EKF2 - * @min 0.0 - * @max 1.0 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_MAGB_K, 0.2f); - /** * Range sensor aid. * diff --git a/src/modules/ekf2/ekf2_multi_params.c b/src/modules/ekf2/ekf2_params_multi.c similarity index 100% rename from src/modules/ekf2/ekf2_multi_params.c rename to src/modules/ekf2/ekf2_params_multi.c diff --git a/src/modules/ekf2/ekf2_selector_params.c b/src/modules/ekf2/ekf2_params_selector.c similarity index 100% rename from src/modules/ekf2/ekf2_selector_params.c rename to src/modules/ekf2/ekf2_params_selector.c diff --git a/src/modules/ekf2/ekf2_params_volatile.c b/src/modules/ekf2/ekf2_params_volatile.c new file mode 100644 index 0000000000..5b947a5cf8 --- /dev/null +++ b/src/modules/ekf2/ekf2_params_volatile.c @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Magnetic declination + * + * @group EKF2 + * @volatile + * @category system + * @unit deg + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0); diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index f5c3932164..46d561ad3b 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -125,7 +125,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("estimator_status", 500, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); @@ -143,6 +143,7 @@ void LoggedTopics::add_default_topics() add_topic_multi("sensor_mag", 1000, 4); add_topic_multi("vehicle_imu", 500, 4); add_topic_multi("vehicle_imu_status", 1000, 4); + add_topic_multi("vehicle_magnetometer", 500, 4); #ifdef CONFIG_ARCH_BOARD_PX4_SITL add_topic("actuator_controls_virtual_fw"); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 91cd59724e..7fae7a7945 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -129,6 +129,114 @@ void VehicleMagnetometer::ParametersUpdate(bool force) } } +void VehicleMagnetometer::MagCalibrationUpdate() +{ + // 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 + && (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) { + + const auto old_offset = _mag_cal[i].mag_offset; + + _mag_cal[i].device_id = estimator_sensor_bias.mag_device_id; + _mag_cal[i].mag_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias); + _mag_cal[i].mag_bias_variance = bias_variance; + + _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])", + mag_index, _mag_cal[i].device_id, + (double)_mag_cal[i].mag_offset(0), (double)_mag_cal[i].mag_offset(1), (double)_mag_cal[i].mag_offset(2), + (double)bias(0), (double)bias(1), (double)bias(2)); + } + + break; + } + } + } + } + } + + } else if (_mag_cal_available) { + // 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++) { + // 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()) + && _mag_cal[i].mag_offset.longerThan(0.01f)) { + + 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); + } + + PX4_INFO("%d (%d) est. offset %d committed: [%.3f %.3f %3f]->[%.3f %.3f %.3f] (full [%.3f %.3f %.3f])", + mag_index, _calibration[mag_index].device_id(), i, + (double)_calibration[mag_index].offset()(0), (double)_calibration[mag_index].offset()(1), + (double)_calibration[mag_index].offset()(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[mag_index].set_offset(mag_cal_offset); + 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_param_save_needed) { + for (int mag_index = 0; mag_index < MAX_SENSOR_COUNT; mag_index++) { + if (_calibration[mag_index].device_id() != 0) { + _calibration[mag_index].ParametersSave(); + } + } + + _mag_cal_available = false; + } + } +} + void VehicleMagnetometer::Run() { perf_begin(_cycle_perf); @@ -136,11 +244,11 @@ void VehicleMagnetometer::Run() ParametersUpdate(); // check vehicle status for changes to armed state - if (_vcontrol_mode_sub.updated()) { - vehicle_control_mode_s vcontrol_mode; + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode; - if (_vcontrol_mode_sub.copy(&vcontrol_mode)) { - _armed = vcontrol_mode.flag_armed; + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { + _armed = vehicle_control_mode.flag_armed; } } @@ -325,6 +433,8 @@ void VehicleMagnetometer::Run() calcMagInconsistency(); } + MagCalibrationUpdate(); + // reschedule timeout ScheduleDelayed(20_ms); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index 6fafd70c54..8fdbf4000e 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -48,9 +48,11 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -82,6 +84,7 @@ private: * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers */ void calcMagInconsistency(); + void MagCalibrationUpdate(); static constexpr int MAX_SENSOR_COUNT = 4; @@ -98,7 +101,18 @@ private: uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0}; uORB::Subscription _params_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; + 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}; + + struct MagCal { + uint32_t device_id{0}; + matrix::Vector3f mag_offset{}; + matrix::Vector3f mag_bias_variance{}; + } _mag_cal[ORB_MULTI_MAX_INSTANCES] {}; uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_mag), 0}, @@ -140,7 +154,7 @@ private: int8_t _selected_sensor_sub_index{-1}; - bool _armed{false}; /**< arming status of the vehicle */ + bool _armed{false}; DEFINE_PARAMETERS( (ParamInt) _param_mag_comp_typ,