From 6e99ebd9286c5b52585d1809ae3ae2a3f00e6bd5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 26 Oct 2020 10:31:11 -0400 Subject: [PATCH] EKF: add fault status bit for bad vertical accel data --- EKF/common.h | 3 ++- EKF/control.cpp | 6 +++--- EKF/covariance.cpp | 4 ++-- EKF/ekf.h | 1 - EKF/ekf_helper.cpp | 2 +- 5 files changed, 8 insertions(+), 8 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 5c84fca8a1..d0a0d5bc4e 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -401,7 +401,8 @@ union fault_status_u { bool bad_pos_E: 1; ///< 13 - true if fusion of the East position has encountered a numerical error bool bad_pos_D: 1; ///< 14 - true if fusion of the Down position has encountered a numerical error bool bad_acc_bias: 1; ///< 15 - true if bad delta velocity bias estimates have been detected - bool bad_acc_clipping: 1; ///< 16 - true if delta velocity data contains clipping (asymmetric railing) + bool bad_acc_vertical: 1; ///< 16 - true if bad vertical accelerometer data has been detected + bool bad_acc_clipping: 1; ///< 17 - true if delta velocity data contains clipping (asymmetric railing) } flags; uint16_t value; diff --git a/EKF/control.cpp b/EKF/control.cpp index 662c275edb..a1e9cceac0 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -898,11 +898,11 @@ void Ekf::checkVerticalAccelerationHealth() // declare a bad vertical acceleration measurement and make the declaration persist // for a minimum of BADACC_PROBATION seconds - if (_bad_vert_accel_detected) { - _bad_vert_accel_detected = isRecent(_time_bad_vert_accel, BADACC_PROBATION); + if (_fault_status.flags.bad_acc_vertical) { + _fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION); } else { - _bad_vert_accel_detected = bad_vert_accel; + _fault_status.flags.bad_acc_vertical = bad_vert_accel; } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 3d5e9bdf37..6645108a73 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -160,7 +160,7 @@ void Ekf::predictCovariance() const bool do_inhibit_all_axes = (_params.fusion_mode & MASK_INHIBIT_ACC_BIAS) || is_manoeuvre_level_high - || _bad_vert_accel_detected; + || _fault_status.flags.bad_acc_vertical; for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) { const unsigned index = stateIndex - 13; @@ -245,7 +245,7 @@ void Ekf::predictCovariance() float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); - if (_bad_vert_accel_detected) { + if (_fault_status.flags.bad_acc_vertical) { // Increase accelerometer process noise if bad accel data is detected. Measurement errors due to // vibration induced clipping commonly reach an equivalent 0.5g offset. accel_noise = BADACC_BIAS_PNOISE; diff --git a/EKF/ekf.h b/EKF/ekf.h index c345e2f24b..baf6cdcc6e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -518,7 +518,6 @@ class Ekf : public EstimatorInterface // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec) - bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected // variables used to control range aid functionality bool _is_range_aid_suitable{false}; ///< true when range finder can be used in flight as the height reference instead of the primary height sensor diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6547dc8bce..57ccb69738 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1092,7 +1092,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const bool gps_pos_innov_bad = (_gps_pos_test_ratio(0) > 1.0f); const bool mag_innov_good = (_mag_test_ratio.max() < 1.0f) && (_yaw_test_ratio < 1.0f); soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; - soln_status.flags.accel_error = _bad_vert_accel_detected; + soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical; *status = soln_status.value; }