aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: move tasHealth to be a stack variable

Browse Source
This commit is contained in:
Peter Barker 2020-11-22 14:58:01 +11:00 committed by Peter Barker
parent 0aa10d8281
commit 2885026bba
2 changed files with 1 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF3/AP_NavEKF3_AirDataFusion.cpp
View File

@ -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
View File

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