Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Consolidate constant velocity mode decision logic 

The decision to switch to constant velocity mode during optical flow operation and te decision to switch back were previously being made in two different places in code. Both decisions are now made in the one place which makes the code easier to follow and maintain.
This commit is contained in:
priseborough 2015-01-01 09:39:19 +11:00
parent 9caf2ac895
commit f1dfa282dc
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -909,6 +909,11 @@ void NavEKF::SelectFlowFusion()
// if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in positon hold mode
if (!flowDataValid && !constPosMode && PV_AidingMode == RELATIVE) {
constVelMode = true;
constPosMode = false; // always clear constant position mode if constant velocity mode is active
} else if (flowDataValid) {
// clear the constant velocity mode now we have good data
constVelMode = false;
lastConstVelMode = false;
}
// Apply tilt check
bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin);
@ -4094,9 +4099,6 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
flowRadXYcomp[1] = flowStates[0]*flowRadXY[1] + omegaAcrossFlowTime.y;
// set flag that will trigger observations
newDataFlow = true;
// clear the constant velocity mode now we have good data
constVelMode = false;
lastConstVelMode = false;
flowValidMeaTime_ms = imuSampleTime_ms;
} else {
newDataFlow = false;