mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF : Bug fix for height drift due to timer wrap-around
This commit is contained in:
parent
939a32a7bb
commit
89e0b48320
|
@ -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 to provide a smoother output
|
||||||
// measurement until the next one arrives
|
|
||||||
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 to provide a smoother output
|
||||||
// measurement until the next one arrives
|
|
||||||
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;
|
||||||
|
|
|
@ -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)
|
||||||
|
|
Loading…
Reference in New Issue