Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 17:53:59 -04:00
Code Issues 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; states[9] = -hgtMea;
} }
void NavEKF::InitialiseFilter(void) void NavEKF::InitialiseFilterDynamic(void)
{ {
// Set re-used variables to zero // Set re-used variables to zero
ZeroVariables(); ZeroVariables();
@ -420,6 +420,9 @@ void NavEKF::InitialiseFilter(void)
// write to state vector // write to state vector
for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions 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 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[16] = initMagNED.x; // Magnetic Field North
states[17] = initMagNED.y; // Magnetic Field East states[17] = initMagNED.y; // Magnetic Field East
states[18] = initMagNED.z; // Magnetic Field Down states[18] = initMagNED.z; // Magnetic Field Down
@ -553,7 +556,7 @@ void NavEKF::UpdateFilter()
if (dtIMU > 0.2f) { if (dtIMU > 0.2f) {
// we have stalled for far too long - reset from DCM // we have stalled for far too long - reset from DCM
InitialiseFilter(); InitialiseFilterDynamic();
perf_end(_perf_UpdateFilter); perf_end(_perf_UpdateFilter);
return; return;
} }

6
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -75,9 +75,11 @@ public:
NavEKF(const AP_AHRS *ahrs, AP_Baro &baro); NavEKF(const AP_AHRS *ahrs, AP_Baro &baro);
// Initialise the filter states from the AHRS and magnetometer data (if present) // 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); void InitialiseFilterBootstrap(void);
// inhibits position and velocity attitude corrections when set to true // inhibits position and velocity attitude corrections when set to true