mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 21:23:34 +08:00
EKF: remove un-used magnetometer health class variable
replaced by _sensor_health_status
This commit is contained in:
@@ -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;
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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;
|
||||
|
||||
+3
-6
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user