Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: fix initialization of the SmallEKF

Browse Source 
The constructor 'states' variable was not being called. To make sure other variables where also zeroed now
This commit is contained in:
Arthur Benemann 2015-03-20 12:56:49 -07:00 committed by Randy Mackay
parent 0189f80462
commit 4ad3e786a5
1 changed files with 10 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF/AP_SmallEKF.cpp
View File

@ -31,11 +31,17 @@ const AP_Param::GroupInfo SmallEKF::var_info[] PROGMEM = {
SmallEKF::SmallEKF(const AP_AHRS_NavEKF &ahrs) : SmallEKF::SmallEKF(const AP_AHRS_NavEKF &ahrs) :
_ahrs(ahrs), _ahrs(ahrs),
_main_ekf(ahrs.get_NavEKF_const()), _main_ekf(ahrs.get_NavEKF_const()),
states(),
state(*reinterpret_cast<struct state_elements *>(&states)), state(*reinterpret_cast<struct state_elements *>(&states)),
TiltCorrection(0),
StartTime_ms(0),
FiltInit(false), FiltInit(false),
lastMagUpdate(0) lastMagUpdate(0),
dtIMU(0)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
memset(&gSense,0,sizeof(gSense));
memset(&Cov,0,sizeof(Cov));
} }
// run a 9-state EKF used to calculate orientation // run a 9-state EKF used to calculate orientation
@ -82,6 +88,7 @@ void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vect
// fuse SmallEKF velocity data // fuse SmallEKF velocity data
fuseVelocity(YawAligned); fuseVelocity(YawAligned);
// Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold // Align the heading once there has been enough time for the filter to settle and the tilt corrections have dropped below a threshold
// Force it to align if too much time has lapsed // Force it to align if too much time has lapsed
if (((((imuSampleTime_ms - StartTime_ms) > 5000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) { if (((((imuSampleTime_ms - StartTime_ms) > 5000 && TiltCorrection < 1e-4f) || (imuSampleTime_ms - StartTime_ms) > 30000)) && !YawAligned) {
@ -92,12 +99,13 @@ void SmallEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const Vect
} }
// Fuse magnetometer data if we have new measurements and an aligned heading // Fuse magnetometer data if we have new measurements and an aligned heading
readMagData(); readMagData();
if (newDataMag && YawAligned) { if (newDataMag && YawAligned) {
fuseCompass(); fuseCompass();
newDataMag = false; newDataMag = false;
} }
} }
// state prediction // state prediction