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

AP_NavEKF : Rework logic to cope with bad flow data

Browse Source
This commit is contained in:
priseborough 2014-10-29 11:56:39 +11:00 committed by Andrew Tridgell
parent 44e1695d5a
commit 8faeb190de
2 changed files with 31 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -804,8 +804,17 @@ void NavEKF::SelectVelPosFusion()
StoreStatesReset();
}
} else {
fuseVelData = false;
fusePosData = false;
if (!holdVelocity) {
fuseVelData = false;
fusePosData = false;
} else {
fuseVelData = true;
fusePosData = false;
statesAtVelTime = state;
velNED.x = heldVelNE.x;
velNED.y = heldVelNE.y;
velNED.z = 0.0f;
}
}
} else {
@ -902,6 +911,24 @@ void NavEKF::SelectMagFusion()
// select fusion of optical flow measurements
void NavEKF::SelectFlowFusion()
{
// if we don't have flow measurements we use GPS velocity if available or else
// dead reckon using current velocity vector
if (imuSampleTime_ms - flowMeaTime_ms > 1000) {
forceUseGPS = true;
if (imuSampleTime_ms - lastFixTime_ms > 1000) {
holdVelocity = true;
} else {
holdVelocity = false;
}
} else {
forceUseGPS = false;
holdVelocity = false;
}
if (holdVelocity && !lastHoldVelocity) {
heldVelNE.x = state.velocity.x;
heldVelNE.y = state.velocity.y;
}
lastHoldVelocity = holdVelocity;
// Apply tilt check
bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin);
// if we have waited too long, set a timeout flag which will force fusion to occur regardless of load spreading
@ -1068,15 +1095,6 @@ void NavEKF::UpdateStrapdownEquationsNED()
// * and + operators have been overloaded
// blended IMU calc
delVelNav = Tbn_temp*correctedDelVel12 + gravityNED*dtIMU;
// used to hold constant velocity if aiding is unavailable
if (holdVelocity) {
delVelNav.x = 0.0f;
delVelNav.y = 0.0f;
delVelNav1.x = 0.0f;
delVelNav1.y = 0.0f;
delVelNav2.x = 0.0f;
delVelNav2.y = 0.0f;
}
// single IMU calcs
delVelNav1 = Tbn_temp*correctedDelVel1 + gravityNED*dtIMU;
delVelNav2 = Tbn_temp*correctedDelVel2 + gravityNED*dtIMU;
@ -4026,19 +4044,6 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
} else {
newDataRng = false;
}
// if we don't have flow measurements we use GPS velocity if available or else
// dead reckon using current velocity vector
if (imuSampleTime_ms - flowMeaTime_ms > 1000) {
forceUseGPS = true;
if (imuSampleTime_ms - lastFixTime_ms > 1000) {
holdVelocity = true;
} else {
holdVelocity = false;
}
} else {
forceUseGPS = false;
holdVelocity = false;
}
}
// calculate the NED earth spin vector in rad/sec

2
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -589,7 +589,9 @@ private:
float flowUpdateCountMaxInv; // floating point inverse of flowUpdateCountMax
bool newDataRng; // true when new valid range finder data has arrived.
bool holdVelocity; // true when holding velocity in optical flow mode when no flow measurements are available
bool lastHoldVelocity; // last value of holdVelocity
bool forceUseGPS; // true when lack of optical flow data forces us to use GPS
Vector2f heldVelNE; // velocity held when no aiding is available
// states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps