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

AP_NavEKF: Improved behaviour following loss and regaining of GPS

This commit is contained in:
Paul Riseborough 2014-01-21 06:41:41 +11:00 committed by Andrew Tridgell
parent 14eb63e7c9
commit 1651980b9f
2 changed files with 36 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -263,7 +263,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: HGT_RETRY1
// @DisplayName: Height retry time with vertical velocity measurement (msec)
// @Description: This is the number of msec that the the filter will wait if height measurements are failing the innovation consistency check before forcing us of the measurements. This value applies when GPS vertical velocity measurements are being used (EKF_GPS_TYPE = 0).
// @Description: This is the number of msec that the the filter will wait if height measurements are failing the innovation consistency check before forcing use of the measurements. This value applies when GPS vertical velocity measurements are being used (EKF_GPS_TYPE = 0).
// @Range: 1000 - 30000
// @Increment: 1000
// @User: advanced
@ -271,7 +271,7 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: HGT_RETRY2
// @DisplayName: Height retry time without vertical velocity measurement (msec)
// @Description: This is the number of msec that the the filter will wait if height measurements are failing the innovation consistency check before forcing us of the measurements. This value applies when GPS vertical velocity measurements are NOT being used (EKF_GPS_TYPE = 1 or 2).
// @Description: This is the number of msec that the the filter will wait if height measurements are failing the innovation consistency check before forcing use of the measurements. This value applies when GPS vertical velocity measurements are NOT being used (EKF_GPS_TYPE = 1 or 2).
// @Range: 1000 - 30000
// @Increment: 1000
// @User: advanced
@ -356,10 +356,30 @@ void NavEKF::ResetPosition(void)
{
// read the GPS
readGpsData();
readHgtData();
// write to state vector
states[7] = posNE[0];
states[8] = posNE[1];
}
void NavEKF::ResetVelocity(void)
{
// read the GPS
readGpsData();
// write to state vector
if (_fusionModeGPS <= 1) {
states[4] = velNED[0];
states[5] = velNED[1];
}
if (_fusionModeGPS <= 0) {
states[6] = velNED[2];
}
}
void NavEKF::ResetHeight(void)
{
// read the altimeter
readHgtData();
// write to state vector
states[9] = -hgtMea;
}
@ -1539,6 +1559,11 @@ void NavEKF::FuseVelPosNED()
{
velHealth = true;
velFailTime = hal.scheduler->millis();
if (velTimeout)
{
ResetVelocity();
}
}
else
{
@ -1588,7 +1613,7 @@ void NavEKF::FuseVelPosNED()
hgtFailTime = hal.scheduler->millis();
if (hgtTimeout)
{
ResetPosition();
ResetHeight();
}
}
else

10
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -80,9 +80,6 @@ public:
// Initialise the attitude from accelerometer and magnetometer data (if present)
void InitialiseFilterBootstrap(void);
// Reset the position and height states to the last GPS and barometer measurement
void ResetPosition(void);
// inhibits position and velocity attitude corrections when set to true
// setting to true has same effect as ahrs.set_correct_centrifugal(false)
void SetStaticMode(bool setting);
@ -217,6 +214,13 @@ private:
void ZeroVariables();
void ResetPosition(void);
void ResetVelocity(void);
void ResetHeight(void);
private:
// EKF Mavlink Tuneable Parameters
AP_Float _gpsHorizVelNoise; // GPS horizontal velocity measurement noise : m/s