Browse Source

EKF: remove un-used magnetometer health class variable

replaced by _sensor_health_status
master
Paul Riseborough 9 years ago
parent
commit
388e500180
  1. 1
      EKF/ekf.cpp
  2. 1
      EKF/estimator_interface.cpp
  3. 5
      EKF/estimator_interface.h
  4. 9
      EKF/mag_fusion.cpp

1
EKF/ekf.cpp

@ -170,7 +170,6 @@ bool Ekf::init(uint64_t timestamp)
_imu_updated = false; _imu_updated = false;
_NED_origin_initialised = false; _NED_origin_initialised = false;
_gps_speed_valid = false; _gps_speed_valid = false;
_mag_healthy = false;
_filter_initialised = false; _filter_initialised = false;
_terrain_initialised = false; _terrain_initialised = false;

1
EKF/estimator_interface.cpp

@ -57,7 +57,6 @@ EstimatorInterface::EstimatorInterface():
_gps_speed_valid(false), _gps_speed_valid(false),
_gps_origin_eph(0.0f), _gps_origin_eph(0.0f),
_gps_origin_epv(0.0f), _gps_origin_epv(0.0f),
_mag_healthy(false),
_airspeed_healthy(false), _airspeed_healthy(false),
_yaw_test_ratio(0.0f), _yaw_test_ratio(0.0f),
_time_last_imu(0), _time_last_imu(0),

5
EKF/estimator_interface.h

@ -259,12 +259,11 @@ protected:
float _gps_origin_epv; // vertical position uncertainty of the GPS origin 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) 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 _yaw_test_ratio; // yaw innovation consistency check ratio
float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios 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 _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 // data buffer instances
RingBuffer<imuSample> _imu_buffer; RingBuffer<imuSample> _imu_buffer;

9
EKF/mag_fusion.cpp

@ -176,20 +176,19 @@ void Ekf::fuseMag()
// Perform an innovation consistency check on each measurement and if one axis fails // 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. // 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++) { 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]); _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) { if (_mag_test_ratio[index] > 1.0f) {
_mag_healthy = false; mag_fail = false;
_sensor_fault_status.value |= (1 << (index + 3)); _sensor_fault_status.value |= (1 << (index + 3));
} else { } else {
_sensor_fault_status.value &= !(1 << (index + 3)); _sensor_fault_status.value &= !(1 << (index + 3));
} }
} }
if (!_mag_healthy) { if (mag_fail) {
return; return;
} }
@ -606,7 +605,6 @@ void Ekf::fuseHeading()
// set the magnetometer unhealthy if the test fails // set the magnetometer unhealthy if the test fails
if (_yaw_test_ratio > 1.0f) { if (_yaw_test_ratio > 1.0f) {
_mag_healthy = false;
_sensor_fault_status.flags.reject_yaw = true; _sensor_fault_status.flags.reject_yaw = true;
// if we are in air we don't want to fuse the measurement // if we are in air we don't want to fuse the measurement
@ -622,7 +620,6 @@ void Ekf::fuseHeading()
} }
} else { } else {
_mag_healthy = true;
_sensor_fault_status.flags.reject_yaw = false; _sensor_fault_status.flags.reject_yaw = false;
} }

Loading…
Cancel
Save