aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: second attempt at higher rate baro fusion

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-05 20:54:49 +11:00 committed by Andrew Tridgell
parent cdbc5a3f35
commit 2f3b2a7111
2 changed files with 17 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -257,37 +257,17 @@ void NavEKF::SelectVelPosFusion()
{ {
// Command fusion of GPS measurements if new ones available // Command fusion of GPS measurements if new ones available
readGpsData(); readGpsData();
readHgtData();
if (newDataGps) if (newDataGps)
{ {
fuseVelData = true; fuseVelData = true;
fusePosData = true; fusePosData = true;
fuseHgtData = true;
} }
// Don't wait longer than HGTmsecMax msec between height updates readHgtData();
// if no GPS if (newDataHgt || ((IMUmsec - HGTmsecPrev) >= HGTmsecMax))
else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)
{ {
// Static mode is used for pre-arm and bench testing and allows operation
// without GPS
if (staticMode) {
fuseVelData = true;
fusePosData = true;
fuseHgtData = true; fuseHgtData = true;
}
else {
fuseVelData = false;
fusePosData = false;
fuseHgtData = true;
}
HGTmsecPrev = IMUmsec; HGTmsecPrev = IMUmsec;
} }
else
{
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
// set flag to let other processes know that GPS and/or height fusion has // set flag to let other processes know that GPS and/or height fusion has
// ocurred in this frame // ocurred in this frame
if (fuseVelData || fusePosData || fuseHgtData) if (fuseVelData || fusePosData || fuseHgtData)
@ -1958,18 +1938,15 @@ void NavEKF::readGpsData()
void NavEKF::readHgtData() void NavEKF::readHgtData()
{ {
// ToDo do we check for new height data or grab a height measurement? if (_baro.get_last_update() != lastHgtUpdate) {
// Best to do this at the same time as GPS measurement fusion for efficiency lastHgtUpdate = _baro.get_last_update();
hgtMea = _baro.get_altitude(); hgtMea = _baro.get_altitude();
if (hgtMeaPrev != hgtMea) { newDataHgt = true;
newDataHgt == true;
hgtMeaPrev = hgtMea;
}
else {
newDataHgt == false;
}
// recall states from compass measurement time // recall states from compass measurement time
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
} else {
newDataHgt = false;
}
} }
void NavEKF::readMagData() void NavEKF::readMagData()

6
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -267,7 +267,7 @@ private:
uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step uint32_t HGTmsecPrev; // time stamp of last height measurement fusion step
const uint32_t HGTmsecMax; // maximum allowed interval between height measurement fusion steps const uint32_t HGTmsecMax; // maximum allowed interval between height measurement fusion steps
const bool fuseMeNow; // boolean to force fusion whenever data arrives const bool fuseMeNow; // boolean to force fusion whenever data arrives
bool staticMode; // boolean to force positio and velocity measurements to zero for pre-arm or bench testing bool staticMode; // boolean to force position and velocity measurements to zero for pre-arm or bench testing
// last time compass was updated // last time compass was updated
uint32_t lastMagUpdate; uint32_t lastMagUpdate;
@ -297,6 +297,10 @@ private:
// TAS input variables // TAS input variables
bool newDataTas; bool newDataTas;
// HGT input variables
bool newDataHgt;
uint32_t lastHgtUpdate;
// Time stamp when vel, pos or height measurements last failed checks // Time stamp when vel, pos or height measurements last failed checks
uint32_t velFailTime; uint32_t velFailTime;
uint32_t posFailTime; uint32_t posFailTime;