Browse Source

AP_NavEKF: second attempt at higher rate baro fusion

master
Paul Riseborough 11 years ago committed by Andrew Tridgell
parent
commit
2f3b2a7111
  1. 45
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 6
      libraries/AP_NavEKF/AP_NavEKF.h

45
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -257,37 +257,17 @@ void NavEKF::SelectVelPosFusion() @@ -257,37 +257,17 @@ void NavEKF::SelectVelPosFusion()
{
// Command fusion of GPS measurements if new ones available
readGpsData();
readHgtData();
if (newDataGps)
{
fuseVelData = true;
fusePosData = true;
fuseHgtData = true;
}
// Don't wait longer than HGTmsecMax msec between height updates
// if no GPS
else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)
readHgtData();
if (newDataHgt || ((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;
}
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() @@ -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;
}
else {
newDataHgt == false;
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;
}
// recall states from compass measurement time
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
}
void NavEKF::readMagData()

6
libraries/AP_NavEKF/AP_NavEKF.h

@ -267,7 +267,7 @@ private: @@ -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: @@ -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;

Loading…
Cancel
Save