Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-05 18:15:13 +11:00 committed by Andrew Tridgell
parent 2998aa1a6a
commit 32cc427ff4
2 changed files with 18 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -71,6 +71,8 @@ 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
@ -262,38 +264,20 @@ 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
else if ((IMUmsec - HGTmsecPrev) >= HGTmsecMax) if (newDataHgt)
{ {
// 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;
}
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)
{ {
FuseVelPosNED(); FuseVelPosNED();
newDataGps = false; newDataGps = false;
newDataHgt = false;
posVelFuseStep = true; posVelFuseStep = true;
} }
else else
@ -1958,14 +1942,16 @@ void NavEKF::readGpsData()
void NavEKF::readHgtData() void NavEKF::readHgtData()
{ {
// ToDo do we check for new height data or grab a height measurement? // ToDo - better check for new height data
// 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 0 if (hgtMeaPrev != hgtMea) {
Vector3f pos; newDataHgt == true;
getPosNED(pos); hgtMeaPrev = hgtMea;
//::printf("BARO.Alt=%.2f REL.z=%.2f\n", hgtMea, -pos.z); }
#endif else {
newDataHgt == false;
}
// recall states from compass measurement time // recall states from compass measurement time
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay)); RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
} }

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^2 ftype _accNoise; // accelerometer process noise : m/s^2hgtHealth = (sq(hgtInnov) < maxf((100.0f * varInnovVelPos[5])) , 5.0f);
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,6 +297,10 @@ 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;