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

AP_AHRS: start EKF 5 seconds after getting GPS lock

This commit is contained in:
Andrew Tridgell 2014-01-04 17:04:34 +11:00
parent df271fbd59
commit 574946f0aa
1 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -52,14 +52,15 @@ void AP_AHRS_NavEKF::update(void)
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}
// if we have GPS lock and a compass set we can start the EKF
if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D &&
hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = true;
EKF.InitialiseFilter();
if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D) {
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = true;
EKF.InitialiseFilter();
}
}
}
if (ekf_started) {