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

7
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -52,16 +52,17 @@ void AP_AHRS_NavEKF::update(void)
_dcm_attitude(roll, pitch, yaw); _dcm_attitude(roll, pitch, yaw);
if (!ekf_started) { if (!ekf_started) {
// 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) {
if (start_time_ms == 0) { if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis(); start_time_ms = hal.scheduler->millis();
} }
// if we have GPS lock and a compass set we can start the EKF if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
if (get_compass() && _gps && _gps->status() >= GPS::GPS_OK_FIX_3D &&
hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = true; ekf_started = true;
EKF.InitialiseFilter(); EKF.InitialiseFilter();
} }
} }
}
if (ekf_started) { if (ekf_started) {
EKF.UpdateFilter(); EKF.UpdateFilter();
EKF.getRotationBodyToNED(_dcm_matrix); EKF.getRotationBodyToNED(_dcm_matrix);