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

47
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -257,37 +257,17 @@ void NavEKF::SelectVelPosFusion()
{
// Command fusion of GPS measurements if new ones available
readGpsData();
readHgtData();
if (newDataGps)
{
fuseVelData = true;
fusePosData = true;
}
readHgtData();
if (newDataHgt || ((IMUmsec - HGTmsecPrev) >= HGTmsecMax))
{
fuseHgtData = true;
}
// Don't wait longer than HGTmsecMax msec between height updates
// if no GPS
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;
}
else {
fuseVelData = false;
fusePosData = false;
fuseHgtData = true;
}
HGTmsecPrev = IMUmsec;
}
else
{
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
// set flag to let other processes know that GPS and/or height fusion has
// ocurred in this frame
if (fuseVelData || fusePosData || fuseHgtData)
@ -1958,18 +1938,15 @@ void NavEKF::readGpsData()
void NavEKF::readHgtData()
{
// ToDo do we check for new height data or grab a height measurement?
// Best to do this at the same time as GPS measurement fusion for efficiency
hgtMea = _baro.get_altitude();
if (hgtMeaPrev != hgtMea) {
newDataHgt == true;
hgtMeaPrev = hgtMea;
if (_baro.get_last_update() != lastHgtUpdate) {
lastHgtUpdate = _baro.get_last_update();
hgtMea = _baro.get_altitude();
newDataHgt = true;
// recall states from compass measurement time
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
} else {
newDataHgt = false;
}
else {
newDataHgt == false;
}
// recall states from compass measurement time
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
}
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
const uint32_t HGTmsecMax; // maximum allowed interval between height measurement fusion steps
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
uint32_t lastMagUpdate;
@ -297,6 +297,10 @@ private:
// TAS input variables
bool newDataTas;
// HGT input variables
bool newDataHgt;
uint32_t lastHgtUpdate;
// Time stamp when vel, pos or height measurements last failed checks
uint32_t velFailTime;
uint32_t posFailTime;