Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -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;
// set the variances using the reported GPS speed accuracy
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) {
// use external nav data as the 2nd preference
stateStruct.velocity = extNavVelDelayed.vel;
P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err);
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else {
stateStruct.velocity.x = 0.0f;
stateStruct.velocity.y = 0.0f;
// set the variances using the likely speed range
P[4][4] = P[3][3] = sq(25.0f);
}
// clear the timeout flags and counters
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
}
}
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].velocity.x = stateStruct.velocity.x;
storedOutput[i].velocity.y = stateStruct.velocity.y;
@ -565,9 +560,9 @@ void NavEKF2_core::SelectVelPosFusion()
void NavEKF2_core::FuseVelPosNED()
{
// health is set bad until test passed
velHealth = false;
posHealth = false;
hgtHealth = false;
bool velHealth = false; // boolean true if velocity measurements have passed innovation consistency check
bool posHealth = false; // boolean true if position measurements have passed innovation consistency check
bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check
// declare variables used to check measurement errors
Vector3f velInnov;
@ -696,8 +691,6 @@ void NavEKF2_core::FuseVelPosNED()
posTestRatio = 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
velTestRatio = 0.0f;
}
} else {
velHealth = false;
}
}

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -809,9 +809,6 @@ private:
// Variables
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 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