diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp index 92d8dad6d3..e8150f1b1f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp @@ -33,9 +33,6 @@ void NavEKF2_core::FuseAirspeed() Vector24 H_TAS; float VtasPred; - // health is set bad until test passed - tasHealth = false; - // copy required states to local variable names vn = stateStruct.velocity.x; ve = stateStruct.velocity.y; @@ -113,7 +110,7 @@ void NavEKF2_core::FuseAirspeed() tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); // fail if the ratio is > 1, but don't fail if bad IMU data - tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); + bool tasHealth = ((tasTestRatio < 1.0f) || badIMUdata); tasTimeout = (imuSampleTime_ms - lastTasPassTime_ms) > frontend->tasRetryTime_ms; // test the ratio before fusing data, forcing fusion if airspeed and position are timed out as we have no choice but to try and use airspeed to constrain error growth diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 360878f218..203057a44e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -810,7 +810,6 @@ private: // Variables bool statesInitialised; // boolean true when filter states have been initialised bool magHealth; // boolean true if magnetometer has passed innovation consistency check - bool tasHealth; // boolean true if true airspeed has passed innovation consistency check bool velTimeout; // boolean true if velocity measurements have failed innovation consistency check and timed out bool posTimeout; // boolean true if position measurements have failed innovation consistency check and timed out bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out