Revert "AP_NavEKF: Modifed Vel Pos fusion to fuse height data whenever baro reading has changed"

This reverts commit d1e1be192a0e7ab0771edc9f2c03547da25697cf.
This commit is contained in:
Paul Riseborough 2014-01-05 20:02:07 +11:00 committed by Andrew Tridgell
parent 32cc427ff4
commit da2c341914
2 changed files with 23 additions and 11 deletions

View File

@ -71,8 +71,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
hgtRate = 0.0f;
mag_state.q0 = 1;
mag_state.DCM.identity();
hgtMeaPrev = 0.0f;
hgtMea = 0.0f;
}
bool NavEKF::healthy(void) const
@ -264,12 +262,31 @@ void NavEKF::SelectVelPosFusion()
{
fuseVelData = true;
fusePosData = true;
fuseHgtData = true;
}
// Don't wait longer than HGTmsecMax msec between height updates
// if no GPS
if (newDataHgt)
else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax)
{
fuseHgtData = true;
// 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
@ -277,7 +294,6 @@ void NavEKF::SelectVelPosFusion()
{
FuseVelPosNED();
newDataGps = false;
newDataHgt = false;
posVelFuseStep = true;
}
else
@ -1942,7 +1958,7 @@ void NavEKF::readGpsData()
void NavEKF::readHgtData()
{
// ToDo - better check for new height data
// 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) {

View File

@ -201,7 +201,7 @@ private:
ftype _windStateNoise; // rate of change of wind : m/s^2
ftype _wndVarHgtRateScale; // scale factor applied to wind process noise from height rate
ftype _gyrNoise; // gyro process noise : rad/s
ftype _accNoise; // accelerometer process noise : m/s^2hgtHealth = (sq(hgtInnov) < maxf((100.0f * varInnovVelPos[5])) , 5.0f);
ftype _accNoise; // accelerometer process noise : m/s^2
ftype _dAngBiasNoise; // gyro bias state noise : rad/s^2
ftype _magEarthNoise; // earth magnetic field process noise : gauss/sec
ftype _magBodyNoise; // earth magnetic field process noise : gauss/sec
@ -297,10 +297,6 @@ private:
// TAS input variables
bool newDataTas;
// HGT input variables
float hgtMeaPrev;
bool newDataHgt;
// Time stamp when vel, pos or height measurements last failed checks
uint32_t velFailTime;
uint32_t posFailTime;