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) @@ -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;

1
EKF/estimator_interface.cpp

@ -57,7 +57,6 @@ EstimatorInterface::EstimatorInterface(): @@ -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),

5
EKF/estimator_interface.h

@ -259,12 +259,11 @@ protected: @@ -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<imuSample> _imu_buffer;

9
EKF/mag_fusion.cpp

@ -176,20 +176,19 @@ void Ekf::fuseMag() @@ -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() @@ -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() @@ -622,7 +620,6 @@ void Ekf::fuseHeading()
}
} else {
_mag_healthy = true;
_sensor_fault_status.flags.reject_yaw = false;
}

Loading…
Cancel
Save