Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 16:23:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: 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 3a98d51427
commit b79a26135c
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_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -35,27 +35,22 @@ void NavEKF3_core::ResetVelocity(resetDataSource velResetSource)
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[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy)); P[5][5] = P[4][4] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
// clear the timeout flags and counters
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::EXTNAV) { } else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == resetDataSource::DEFAULT) || velResetSource == resetDataSource::EXTNAV) {
// use external nav data as the 2nd preference // use external nav data as the 2nd preference
// already corrected for sensor position // already corrected for sensor position
stateStruct.velocity.x = extNavVelDelayed.vel.x; stateStruct.velocity.x = extNavVelDelayed.vel.x;
stateStruct.velocity.y = extNavVelDelayed.vel.y; stateStruct.velocity.y = extNavVelDelayed.vel.y;
P[5][5] = P[4][4] = sq(extNavVelDelayed.err); P[5][5] = P[4][4] = 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[5][5] = P[4][4] = sq(25.0f); P[5][5] = P[4][4] = 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;
@ -576,9 +571,9 @@ void NavEKF3_core::SelectVelPosFusion()
void NavEKF3_core::FuseVelPosNED() void NavEKF3_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;
@ -711,8 +706,6 @@ void NavEKF3_core::FuseVelPosNED()
velTestRatio = 0.0f; velTestRatio = 0.0f;
} }
} }
} else {
posHealth = false;
} }
} }
@ -757,8 +750,6 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_core.h
View File

@ -937,9 +937,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