Browse Source

AP_NavEKF3: move tasHealth to be a stack variable

c415-sdk
Peter Barker 4 years ago committed by Peter Barker
parent
commit
2885026bba
  1. 5
      libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
  2. 1
      libraries/AP_NavEKF3/AP_NavEKF3_core.h

5
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp

@ -32,9 +32,6 @@ void NavEKF3_core::FuseAirspeed()
Vector24 H_TAS = {}; Vector24 H_TAS = {};
float VtasPred; float VtasPred;
// health is set bad until test passed
tasHealth = false;
// copy required states to local variable names // copy required states to local variable names
vn = stateStruct.velocity.x; vn = stateStruct.velocity.x;
ve = stateStruct.velocity.y; ve = stateStruct.velocity.y;
@ -129,7 +126,7 @@ void NavEKF3_core::FuseAirspeed()
tasTestRatio = sq(innovVtas) / (sq(MAX(0.01f * (float)frontend->_tasInnovGate, 1.0f)) * varInnovVtas); 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 // 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; 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 // 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

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h

@ -938,7 +938,6 @@ private:
// Variables // Variables
bool statesInitialised; // boolean true when filter states have been initialised bool statesInitialised; // boolean true when filter states have been initialised
bool magHealth; // boolean true if magnetometer has passed innovation consistency check 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 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 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 bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out

Loading…
Cancel
Save