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(); ResetVelocity();
StoreStatesReset(); StoreStatesReset();
} }
} } else if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
// Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same // Timeout fusion of GPS data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives // measurement until the next one arrives to provide a smoother output
if (hal.scheduler->millis() > lastFixTime_ms + _msecGpsAvg + 40) {
fuseVelData = false; fuseVelData = false;
fusePosData = false; fusePosData = false;
} }
// Read height data
readHgtData();
// Command fusion of height measurements if new ones available // Command fusion of height measurements if new ones available
if (newDataHgt) if (newDataHgt)
{ {
@ -589,14 +590,12 @@ void NavEKF::SelectVelPosFusion()
newDataHgt = false; newDataHgt = false;
// enable fusion // enable fusion
fuseHgtData = true; 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 // Timeout fusion of height data if stale. Needed because we repeatedly fuse the same
// measurement until the next one arrives // measurement until the next one arrives to provide a smoother output
readHgtData();
if (hal.scheduler->millis() > lastHgtUpdate + _msecHgtAvg + 40) {
fuseHgtData = false; fuseHgtData = false;
} }
} else { } else {
// we only fuse position and height in static mode // we only fuse position and height in static mode
fuseVelData = false; fuseVelData = false;
@ -2473,11 +2472,14 @@ void NavEKF::readGpsData()
void NavEKF::readHgtData() void NavEKF::readHgtData()
{ {
if (_baro.get_last_update() != lastHgtUpdate) { if (_baro.get_last_update() != lastHgtMeasTime) {
lastHgtUpdate = _baro.get_last_update(); // 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(); hgtMea = _baro.get_altitude();
newDataHgt = true; newDataHgt = true;
// recall states from compass measurement time // recall states at effective measurement time
RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay)); RecallStates(statesAtHgtTime, (IMUmsec - _msecHgtDelay));
} else { } else {
newDataHgt = false; newDataHgt = false;
@ -2489,10 +2491,9 @@ void NavEKF::readMagData()
// scale compass data to improve numerical conditioning // scale compass data to improve numerical conditioning
if (_ahrs->get_compass()->last_update != lastMagUpdate) { if (_ahrs->get_compass()->last_update != lastMagUpdate) {
lastMagUpdate = _ahrs->get_compass()->last_update; 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; magBias = -_ahrs->get_compass()->get_offsets() * 0.001f;
magData = _ahrs->get_compass()->get_field() * 0.001f + magBias; magData = _ahrs->get_compass()->get_field() * 0.001f + magBias;
// Recall states from compass measurement time // Recall states from compass measurement time
RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay)); RecallStates(statesAtMagMeasTime, (IMUmsec - _msecMagDelay));
newDataMag = true; newDataMag = true;
@ -2620,7 +2621,7 @@ void NavEKF::ZeroVariables()
HGTmsecPrev = 0; HGTmsecPrev = 0;
lastMagUpdate = 0; lastMagUpdate = 0;
lastAirspeedUpdate = 0; lastAirspeedUpdate = 0;
lastHgtUpdate = 0; lastHgtMeasTime = 0;
dtIMU = 0; dtIMU = 0;
dt = 0; dt = 0;
hgtMea = 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 newDataTas; // true when new airspeed data has arrived
bool tasDataWaiting; // true when new airspeed data is waiting to be fused bool tasDataWaiting; // true when new airspeed data is waiting to be fused
bool newDataHgt; // true when new height data has arrived 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 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 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) uint32_t posFailTime; // time stamp when GPS position measurement last failed covaraiance consistency check (msec)