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

49
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -804,8 +804,17 @@ void NavEKF::SelectVelPosFusion()
StoreStatesReset(); StoreStatesReset();
} }
} else { } else {
if (!holdVelocity) {
fuseVelData = false; fuseVelData = false;
fusePosData = false; fusePosData = false;
} else {
fuseVelData = true;
fusePosData = false;
statesAtVelTime = state;
velNED.x = heldVelNE.x;
velNED.y = heldVelNE.y;
velNED.z = 0.0f;
}
} }
} else { } else {
@ -902,6 +911,24 @@ void NavEKF::SelectMagFusion()
// select fusion of optical flow measurements // select fusion of optical flow measurements
void NavEKF::SelectFlowFusion() 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 // Apply tilt check
bool tiltOK = (Tnb_flow.c.z > DCM33FlowMin); 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 // 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 // * and + operators have been overloaded
// blended IMU calc // blended IMU calc
delVelNav = Tbn_temp*correctedDelVel12 + gravityNED*dtIMU; 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 // single IMU calcs
delVelNav1 = Tbn_temp*correctedDelVel1 + gravityNED*dtIMU; delVelNav1 = Tbn_temp*correctedDelVel1 + gravityNED*dtIMU;
delVelNav2 = Tbn_temp*correctedDelVel2 + gravityNED*dtIMU; delVelNav2 = Tbn_temp*correctedDelVel2 + gravityNED*dtIMU;
@ -4026,19 +4044,6 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
} else { } else {
newDataRng = false; 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 // 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 float flowUpdateCountMaxInv; // floating point inverse of flowUpdateCountMax
bool newDataRng; // true when new valid range finder data has arrived. 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 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 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 // states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps // optical flow X,Y motion compensated rate measurements are fused across two time steps