aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Improved in-air reset behaviour

Browse Source
This commit is contained in:
Paul Riseborough 2014-01-21 06:52:27 +11:00 committed by Andrew Tridgell
parent 1651980b9f
commit b123e1bb16
2 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -383,7 +383,7 @@ void NavEKF::ResetHeight(void)
states[9] = -hgtMea;
}
void NavEKF::InitialiseFilter(void)
void NavEKF::InitialiseFilterDynamic(void)
{
// Set re-used variables to zero
ZeroVariables();
@ -420,6 +420,9 @@ void NavEKF::InitialiseFilter(void)
// write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
for (uint8_t j=10; j<=15; j++) states[j] = 0.0; // dAngBias, dVelBias, windVel
ResetVelocity();
ResetPosition();
ResetHeight();
states[16] = initMagNED.x; // Magnetic Field North
states[17] = initMagNED.y; // Magnetic Field East
states[18] = initMagNED.z; // Magnetic Field Down
@ -553,7 +556,7 @@ void NavEKF::UpdateFilter()
if (dtIMU > 0.2f) {
// we have stalled for far too long - reset from DCM
InitialiseFilter();
InitialiseFilterDynamic();
perf_end(_perf_UpdateFilter);
return;
}

6
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -75,9 +75,11 @@ public:
NavEKF(const AP_AHRS *ahrs, AP_Baro &baro);
// Initialise the filter states from the AHRS and magnetometer data (if present)
void InitialiseFilter(void);
// This method can be used when the vehicle is moving
void InitialiseFilterDynamic(void);
// Initialise the attitude from accelerometer and magnetometer data (if present)
// Initialise the states from accelerometer and magnetometer data (if present)
// This method can only be used when the vehicle is static
void InitialiseFilterBootstrap(void);
// inhibits position and velocity attitude corrections when set to true