|
|
|
@ -77,8 +77,7 @@ void AP_AHRS_NavEKF::update(void)
@@ -77,8 +77,7 @@ void AP_AHRS_NavEKF::update(void)
|
|
|
|
|
_dcm_attitude(roll, pitch, yaw); |
|
|
|
|
|
|
|
|
|
if (!ekf_started) { |
|
|
|
|
// if we have a GPS lock and more than 6 satellites, we can start the EKF
|
|
|
|
|
if (get_gps().status() >= AP_GPS::GPS_OK_FIX_3D && get_gps().num_sats() >= _gps_minsats) { |
|
|
|
|
// wait 10 seconds
|
|
|
|
|
if (start_time_ms == 0) { |
|
|
|
|
start_time_ms = hal.scheduler->millis(); |
|
|
|
|
} |
|
|
|
@ -87,7 +86,6 @@ void AP_AHRS_NavEKF::update(void)
@@ -87,7 +86,6 @@ void AP_AHRS_NavEKF::update(void)
|
|
|
|
|
EKF.InitialiseFilterDynamic(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
if (ekf_started) { |
|
|
|
|
EKF.UpdateFilter(); |
|
|
|
|
EKF.getRotationBodyToNED(_dcm_matrix); |
|
|
|
|