diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 5b099953d3..7f1cbe977c 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -548,7 +548,7 @@ union warning_event_status_u { bool gps_data_stopped_using_alternate : 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation bool height_sensor_timeout : 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period bool stopping_navigation : 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation - bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements + bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements bool bad_yaw_using_gps_course : 1; ///< 7 - true when the fiter has detected an invalid yaw esitmate and has reset the yaw angle to the GPS ground course bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetomer data bool vision_data_stopped : 1; ///< 9 - true when the vision system data has stopped for a significant time period diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f1234617de..4c4f9d1ce2 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -244,6 +244,8 @@ public: Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2 Vector3f getMagBiasVariance() const { return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; } + bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } + // get GPS check status void get_gps_check_status(uint16_t *val) const { *val = _gps_check_fail_status.value; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 84c1d7c98a..999131d742 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1057,17 +1057,18 @@ void EKF2::PublishOdometryAligned(const hrt_abstime ×tamp, const vehicle_od void EKF2::PublishSensorBias(const hrt_abstime ×tamp) { // estimator_sensor_bias - estimator_sensor_bias_s bias{}; - bias.timestamp_sample = _ekf.get_imu_sample_delayed().time_us; - const Vector3f gyro_bias{_ekf.getGyroBias()}; const Vector3f accel_bias{_ekf.getAccelBias()}; const Vector3f mag_bias{_ekf.getMagBias()}; - // only publish on change + // publish at ~1 Hz, or sooner if there's a change 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)) { + || (mag_bias - _last_mag_bias_published).longerThan(0.001f) + || (timestamp >= _last_sensor_bias_published + 1_s)) { + + estimator_sensor_bias_s bias{}; + bias.timestamp_sample = timestamp; // take device ids from sensor_selection_s if not using specific vehicle_imu_s if (_device_id_gyro != 0) { @@ -1102,6 +1103,8 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_sensor_bias_pub.publish(bias); + + _last_sensor_bias_published = bias.timestamp; } } @@ -1779,18 +1782,40 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) { + if (_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS) { + _accel_cal.cal_available = false; + return; + } + + // State variance assumed for accelerometer bias storage. + // This is a reference variance used to calculate the fraction of learned accelerometer 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 max_var_allowed = 1e-3f; + static constexpr float max_var_ratio = 1e2f; + + const Vector3f bias_variance{_ekf.getAccelBiasVariance()}; + // Check if conditions are OK for learning of accelerometer bias values // the EKF is operating in the correct mode and there are no filter faults - if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0) - && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) { + if ((_ekf.fault_status().value == 0) + && !_ekf.accel_bias_inhibited() + && !_preflt_checker.hasHorizFailed() && !_preflt_checker.hasVertFailed() + && (_ekf.control_status_flags().baro_hgt || _ekf.control_status_flags().rng_hgt + || _ekf.control_status_flags().gps_hgt || _ekf.control_status_flags().ev_hgt) + && !_ekf.warning_event_flags().height_sensor_timeout && !_ekf.warning_event_flags().invalid_accel_bias_cov_reset + && !_ekf.innov_check_fail_status_flags().reject_ver_pos && !_ekf.innov_check_fail_status_flags().reject_ver_vel + && (bias_variance.max() < max_var_allowed) && (bias_variance.max() < max_var_ratio * bias_variance.min()) + ) { if (_accel_cal.last_us != 0) { _accel_cal.total_time_us += timestamp - _accel_cal.last_us; // Start checking accel bias estimates when we have accumulated sufficient calibration time if (_accel_cal.total_time_us > 30_s) { - _accel_cal.last_bias = _ekf.getAccelBias(); - _accel_cal.last_bias_variance = _ekf.getAccelBiasVariance(); + if (!_accel_cal.cal_available) { + PX4_DEBUG("%d accel bias now stable", _instance); + } + _accel_cal.cal_available = true; } } @@ -1798,31 +1823,40 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) _accel_cal.last_us = timestamp; } else { - // conditions are NOT OK for learning accelerometer bias, reset timestamp - // but keep the accumulated calibration time - _accel_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. - _accel_cal.total_time_us = 0; + // conditions are NOT OK for learning accelerometer bias, reset + if (_accel_cal.total_time_us > 0) { + PX4_DEBUG("%d, clearing learned accel bias", _instance); } + + _accel_cal = {}; } } void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) { + // State variance assumed for accelerometer bias storage. + // This is a reference variance used to calculate the fraction of learned accelerometer 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 max_var_allowed = 1e-3f; + static constexpr float max_var_ratio = 1e2f; + + const Vector3f bias_variance{_ekf.getGyroBiasVariance()}; + // Check if conditions are OK for learning of gyro bias values // the EKF is operating in the correct mode and there are no filter faults - if (_ekf.control_status_flags().in_air && (_ekf.fault_status().value == 0)) { + if ((_ekf.fault_status().value == 0) + && (bias_variance.max() < max_var_allowed) && (bias_variance.max() < max_var_ratio * bias_variance.min()) + ) { if (_gyro_cal.last_us != 0) { _gyro_cal.total_time_us += timestamp - _gyro_cal.last_us; // Start checking gyro bias estimates when we have accumulated sufficient calibration time if (_gyro_cal.total_time_us > 30_s) { - _gyro_cal.last_bias = _ekf.getGyroBias(); - _gyro_cal.last_bias_variance = _ekf.getGyroBiasVariance(); + if (!_gyro_cal.cal_available) { + PX4_DEBUG("%d gyro bias now stable", _instance); + } + _gyro_cal.cal_available = true; } } @@ -1830,15 +1864,12 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) _gyro_cal.last_us = timestamp; } else { - // conditions are NOT OK for learning gyro bias, reset timestamp - // but keep the accumulated calibration time - _gyro_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. - _gyro_cal.total_time_us = 0; + // conditions are NOT OK for learning gyro bias, reset + if (_gyro_cal.total_time_us > 0) { + PX4_DEBUG("%d, clearing learned gyro bias", _instance); } + + _gyro_cal = {}; } } @@ -1853,8 +1884,6 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) // 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.cal_available = true; } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 47ea4f7254..55e9754c00 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -208,8 +208,6 @@ private: struct InFlightCalibration { hrt_abstime last_us{0}; ///< last time the EKF was operating a mode that estimates accelerometer biases (uSec) hrt_abstime total_time_us{0}; ///< accumulated calibration time since the last save - Vector3f last_bias{}; ///< last valid XYZ accelerometer bias estimates (Gauss) - Vector3f last_bias_variance{}; ///< variances for the last valid accelerometer XYZ bias estimates (m/s**2)**2 bool cal_available{false}; ///< true when an unsaved valid calibration for the XYZ accelerometer bias is available }; @@ -241,6 +239,8 @@ private: Vector3f _last_gyro_calibration_published{}; Vector3f _last_mag_calibration_published{}; + hrt_abstime _last_sensor_bias_published{0}; + float _last_baro_bias_published{}; float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index a3626e7d43..07d8d40c33 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -683,27 +683,20 @@ void VehicleIMU::PrintStatus() void VehicleIMU::SensorCalibrationUpdate() { - // State variance assumed for accelerometer bias storage. - // This is a reference variance used to calculate the fraction of learned accelerometer 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 max_var_allowed = 1e-3f; - static constexpr float max_var_ratio = 1e2f; - if (_armed) { 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)) { // find corresponding accel bias - if (_accel_calibration.device_id() == estimator_sensor_bias.accel_device_id) { + if ((estimator_sensor_bias.accel_device_id != 0) + && (_accel_calibration.device_id() == estimator_sensor_bias.accel_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_device_id != 0) && - estimator_sensor_bias.accel_bias_stable && - (bias_variance.max() < max_var_allowed) && - (bias_variance.max() < max_var_ratio * bias_variance.min()); + 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}; @@ -728,16 +721,13 @@ void VehicleIMU::SensorCalibrationUpdate() } // find corresponding gyro calibration - if (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_device_id) { + if ((estimator_sensor_bias.gyro_device_id != 0) + && (_gyro_calibration.device_id() == estimator_sensor_bias.gyro_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_device_id != 0) && - estimator_sensor_bias.gyro_bias_valid && - estimator_sensor_bias.gyro_bias_stable && - (bias_variance.max() < max_var_allowed) && - (bias_variance.max() < max_var_ratio * bias_variance.min()); + 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};