Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: move posvel fusion health booleans to be on the stack

Browse Source
This commit is contained in:
Peter Barker 2020-11-22 14:48:23 +11:00 committed by Peter Barker
parent 2d25149e2e
commit 3a98d51427
2 changed files with 6 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -36,25 +36,20 @@ void NavEKF2_core::ResetVelocity(void)
stateStruct.velocity.y = gps_corrected.vel.y; stateStruct.velocity.y = gps_corrected.vel.y;
// set the variances using the reported GPS speed accuracy // set the variances using the reported GPS speed accuracy
P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy)); P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
// clear the timeout flags and counters
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else if (imuSampleTime_ms - extNavVelMeasTime_ms < 250) { } else if (imuSampleTime_ms - extNavVelMeasTime_ms < 250) {
// use external nav data as the 2nd preference // use external nav data as the 2nd preference
stateStruct.velocity = extNavVelDelayed.vel; stateStruct.velocity = extNavVelDelayed.vel;
P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err); P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err);
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else { } else {
stateStruct.velocity.x = 0.0f; stateStruct.velocity.x = 0.0f;
stateStruct.velocity.y = 0.0f; stateStruct.velocity.y = 0.0f;
// set the variances using the likely speed range // set the variances using the likely speed range
P[4][4] = P[3][3] = sq(25.0f); P[4][4] = P[3][3] = sq(25.0f);
}
// clear the timeout flags and counters // clear the timeout flags and counters
velTimeout = false; velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms;
} }
}
for (uint8_t i=0; i<imu_buffer_length; i++) { for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].velocity.x = stateStruct.velocity.x; storedOutput[i].velocity.x = stateStruct.velocity.x;
storedOutput[i].velocity.y = stateStruct.velocity.y; storedOutput[i].velocity.y = stateStruct.velocity.y;
@ -565,9 +560,9 @@ void NavEKF2_core::SelectVelPosFusion()
void NavEKF2_core::FuseVelPosNED() void NavEKF2_core::FuseVelPosNED()
{ {
// health is set bad until test passed // health is set bad until test passed
velHealth = false; bool velHealth = false; // boolean true if velocity measurements have passed innovation consistency check
posHealth = false; bool posHealth = false; // boolean true if position measurements have passed innovation consistency check
hgtHealth = false; bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check
// declare variables used to check measurement errors // declare variables used to check measurement errors
Vector3f velInnov; Vector3f velInnov;
@ -696,8 +691,6 @@ void NavEKF2_core::FuseVelPosNED()
posTestRatio = 0.0f; posTestRatio = 0.0f;
velTestRatio = 0.0f; velTestRatio = 0.0f;
} }
} else {
posHealth = false;
} }
} }
@ -742,8 +735,6 @@ void NavEKF2_core::FuseVelPosNED()
// Reset the normalised innovation to avoid failing the bad fusion tests // Reset the normalised innovation to avoid failing the bad fusion tests
velTestRatio = 0.0f; velTestRatio = 0.0f;
} }
} else {
velHealth = false;
} }
} }

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -809,9 +809,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 velHealth; // boolean true if velocity measurements have passed innovation consistency check
bool posHealth; // boolean true if position measurements have passed innovation consistency check
bool hgtHealth; // boolean true if height measurements have passed innovation consistency check
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 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