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

AP_NavEKF : stop velocity change when flow measurements drop out

Browse Source
This commit is contained in:
priseborough 2014-10-17 07:04:13 +11:00 committed by Andrew Tridgell
parent 9132fcfe43
commit 283811edcb
2 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -1057,6 +1057,15 @@ 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;
@ -3990,8 +3999,10 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
if (rawFlowQuality > 100){
// set flag that will trigger observations
newDataFlow = true;
holdVelocity = false;
} else {
newDataFlow = false;
holdVelocity = true;
}
// Use range finder if 3 or more consecutive good samples. This reduces likelihood of using bad data.
if (rangeHealth >= 3) {
@ -4257,6 +4268,7 @@ void NavEKF::ZeroVariables()
prevFlowFusionTime_ms = imuSampleTime_ms; // time the last flow measurement was fused
flowGyroBias.x = 0;
flowGyroBias.y = 0;
holdVelocity = false;
}
// return true if we should use the airspeed sensor

1
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -587,6 +587,7 @@ private:
uint8_t flowUpdateCountMax; // limit on the number of minor state corrections using optical flow data
float flowUpdateCountMaxInv; // floating point inverse of flowUpdateCountMax
bool newDataRng; // true when new valid range finder data has arrived.
bool holdVelocity; // true wehn holding velocity in optical flow mode when no flow measurements are available
// states held by optical flow fusion across time steps
// optical flow X,Y motion compensated rate measurements are fused across two time steps