Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -71,8 +71,6 @@ NavEKF::NavEKF(const AP_AHRS *ahrs, AP_Baro &baro) :
hgtRate = 0.0f; hgtRate = 0.0f;
mag_state.q0 = 1; mag_state.q0 = 1;
mag_state.DCM.identity(); mag_state.DCM.identity();
hgtMeaPrev = 0.0f;
hgtMea = 0.0f;
} }
bool NavEKF::healthy(void) const bool NavEKF::healthy(void) const
@ -264,12 +262,31 @@ void NavEKF::SelectVelPosFusion()
{ {
fuseVelData = true; fuseVelData = true;
fusePosData = true; fusePosData = true;
fuseHgtData = true;
} }
// Don't wait longer than HGTmsecMax msec between height updates // Don't wait longer than HGTmsecMax msec between height updates
// if no GPS // 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 // set flag to let other processes know that GPS and/or height fusion has
// ocurred in this frame // ocurred in this frame
@ -277,7 +294,6 @@ void NavEKF::SelectVelPosFusion()
{ {
FuseVelPosNED(); FuseVelPosNED();
newDataGps = false; newDataGps = false;
newDataHgt = false;
posVelFuseStep = true; posVelFuseStep = true;
} }
else else
@ -1942,7 +1958,7 @@ void NavEKF::readGpsData()
void NavEKF::readHgtData() 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 // Best to do this at the same time as GPS measurement fusion for efficiency
hgtMea = _baro.get_altitude(); hgtMea = _baro.get_altitude();
if (hgtMeaPrev != hgtMea) { if (hgtMeaPrev != hgtMea) {

6
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -201,7 +201,7 @@ private:
ftype _windStateNoise; // rate of change of wind : m/s^2 ftype _windStateNoise; // rate of change of wind : m/s^2
ftype _wndVarHgtRateScale; // scale factor applied to wind process noise from height rate ftype _wndVarHgtRateScale; // scale factor applied to wind process noise from height rate
ftype _gyrNoise; // gyro process noise : rad/s 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 _dAngBiasNoise; // gyro bias state noise : rad/s^2
ftype _magEarthNoise; // earth magnetic field process noise : gauss/sec ftype _magEarthNoise; // earth magnetic field process noise : gauss/sec
ftype _magBodyNoise; // earth magnetic field process noise : gauss/sec ftype _magBodyNoise; // earth magnetic field process noise : gauss/sec
@ -297,10 +297,6 @@ private:
// TAS input variables // TAS input variables
bool newDataTas; bool newDataTas;
// HGT input variables
float hgtMeaPrev;
bool newDataHgt;
// 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;