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

AP_NavEKF : Bug fix for height drift due to timer wrap-around

This commit is contained in:
Paul Riseborough 2014-02-23 14:27:08 +11:00 committed by priseborough
parent 939a32a7bb
commit 89e0b48320
2 changed files with 20 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -573,15 +573,16 @@ void NavEKF::SelectVelPosFusion()
ResetVelocity();
StoreStatesReset();
}
}
} else if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
// Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives
if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
// measurement until the next one arrives to provide a smoother output
fuseVelData = false;
fusePosData = false;
}
// Read height data
readHgtData();
// Command fusion of height measurements if new ones available
if (newDataHgt)
{
@ -589,14 +590,12 @@ void NavEKF::SelectVelPosFusion()
newDataHgt = false;
// enable fusion
fuseHgtData = true;
}
} else if (hal.scheduler->millis() > lastHgtTime_ms + _msecHgtAvg + 40) {
// Timeout fusion of height data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives
readHgtData();
if (hal.scheduler->millis() > lastHgtUpdate + _msecHgtAvg + 40) {
// measurement until the next one arrives to provide a smoother output
fuseHgtData = false;
}
} else {
// we only fuse position and height in static mode
fuseVelData = false;
@ -2473,11 +2472,14 @@ void NavEKF::readGpsData()
void NavEKF::readHgtData()
{
if (_baro.get_last_update() != lastHgtUpdate) {
lastHgtUpdate = _baro.get_last_update();
if (_baro.get_last_update() != lastHgtMeasTime) {
// time stamp used to check for new measurement
lastHgtMeasTime = _baro.get_last_update();
// time stamp used to check for timeout
lastHgtTime_ms = hal.scheduler->millis();
hgtMea = _baro.get_altitude();
newDataHgt = true;
// recall states from compass measurement time
// recall states at effective measurement time
RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay));
} else {
newDataHgt = false;
@ -2489,10 +2491,9 @@ void NavEKF::readMagData()
// scale compass data to improve numerical conditioning
if (_ahrs->get_compass()->last_update != lastMagUpdate) {
lastMagUpdate = _ahrs->get_compass()->last_update;
// Body fixed magnetic bias is opposite sign to APM compass offsets
magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
// Recall states from compass measurement time
RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay));
newDataMag = true;
@ -2620,7 +2621,7 @@ void NavEKF::ZeroVariables()
HGTmsecPrev = 0;
lastMagUpdate = 0;
lastAirspeedUpdate = 0;
lastHgtUpdate = 0;
lastHgtMeasTime = 0;
dtIMU = 0;
dt = 0;
hgtMea = 0;

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -397,7 +397,8 @@ private:
bool newDataTas; // true when new airspeed data has arrived
bool tasDataWaiting; // true when new airspeed data is waiting to be fused
bool newDataHgt; // true when new height data has arrived
uint32_t lastHgtUpdate; // time of last height measurement received (msec)
uint32_t lastHgtMeasTime; // time of last height measurement used to determine if new data has arrived
uint32_t lastHgtTime_ms; // time of last height update (msec) used to calculate timeout
float hgtVarScaler; // scaler applied to height measurement variance to allow for oversampling
uint32_t velFailTime; // time stamp when GPS velocity measurement last failed covaraiance consistency check (msec)
uint32_t posFailTime; // time stamp when GPS position measurement last failed covaraiance consistency check (msec)