diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 73f5a2bb39..fabf5e33b8 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -170,7 +170,6 @@ bool Ekf::init(uint64_t timestamp) _imu_updated = false; _NED_origin_initialised = false; _gps_speed_valid = false; - _mag_healthy = false; _filter_initialised = false; _terrain_initialised = false; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index f615e4ab86..161e74c6a9 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -57,7 +57,6 @@ EstimatorInterface::EstimatorInterface(): _gps_speed_valid(false), _gps_origin_eph(0.0f), _gps_origin_epv(0.0f), - _mag_healthy(false), _airspeed_healthy(false), _yaw_test_ratio(0.0f), _time_last_imu(0), diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 0babecb623..8cc9baa5fb 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -259,12 +259,11 @@ protected: float _gps_origin_epv; // vertical position uncertainty of the GPS origin struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians) - bool _mag_healthy; // computed by mag innovation test - bool _airspeed_healthy; // computed by airspeed innovation test + bool _airspeed_healthy; // computed by airspeed innovation test float _yaw_test_ratio; // yaw innovation consistency check ratio float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios float _vel_pos_test_ratio[6]; // velocity and position innovation consistency check ratios - float _tas_test_ratio; // tas innovation consistency check ratio + float _tas_test_ratio; // tas innovation consistency check ratio // data buffer instances RingBuffer _imu_buffer; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 9c41a1eb35..e019c70a71 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -176,20 +176,19 @@ void Ekf::fuseMag() // Perform an innovation consistency check on each measurement and if one axis fails // do not fuse any data from the sensor because the most common errors affect multiple axes. - _mag_healthy = true; - + bool mag_fail = false; for (uint8_t index = 0; index <= 2; index++) { _mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]); if (_mag_test_ratio[index] > 1.0f) { - _mag_healthy = false; + mag_fail = false; _sensor_fault_status.value |= (1 << (index + 3)); } else { _sensor_fault_status.value &= !(1 << (index + 3)); } } - if (!_mag_healthy) { + if (mag_fail) { return; } @@ -606,7 +605,6 @@ void Ekf::fuseHeading() // set the magnetometer unhealthy if the test fails if (_yaw_test_ratio > 1.0f) { - _mag_healthy = false; _sensor_fault_status.flags.reject_yaw = true; // if we are in air we don't want to fuse the measurement @@ -622,7 +620,6 @@ void Ekf::fuseHeading() } } else { - _mag_healthy = true; _sensor_fault_status.flags.reject_yaw = false; }