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_AHRS: fixed EKF startup bug

Browse Source 
This fixes the EKF for when GPS lock takes more than 10 seconds

fixes issue #2010
This commit is contained in:
Andrew Tridgell 2015-03-28 10:52:22 -07:00
parent 3289d38339
commit 8ba043e593
2 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -81,9 +81,9 @@ void AP_AHRS_NavEKF::update(void)
if (start_time_ms == 0) { if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis(); start_time_ms = hal.scheduler->millis();
} }
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) { if (hal.scheduler->millis() - start_time_ms > startup_delay_ms &&
EKF.InitialiseFilterDynamic()) {
ekf_started = true; ekf_started = true;
EKF.InitialiseFilterDynamic();
} }
} }
if (ekf_started) { if (ekf_started) {
@ -152,7 +152,7 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers)
{ {
AP_AHRS_DCM::reset(recover_eulers); AP_AHRS_DCM::reset(recover_eulers);
if (ekf_started) { if (ekf_started) {
EKF.InitialiseFilterBootstrap(); ekf_started = EKF.InitialiseFilterBootstrap();
} }
} }
@ -161,7 +161,7 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con
{ {
AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw); AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw);
if (ekf_started) { if (ekf_started) {
EKF.InitialiseFilterBootstrap(); ekf_started = EKF.InitialiseFilterBootstrap();
} }
} }

3
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -38,7 +38,8 @@ public:
AP_AHRS_DCM(ins, baro, gps), AP_AHRS_DCM(ins, baro, gps),
EKF(this, baro), EKF(this, baro),
ekf_started(false), ekf_started(false),
startup_delay_ms(10000) startup_delay_ms(10000),
start_time_ms(0)
{ {
} }