|
|
|
@ -93,9 +93,9 @@ void AP_AHRS_NavEKF::update_EKF1(void)
@@ -93,9 +93,9 @@ void AP_AHRS_NavEKF::update_EKF1(void)
|
|
|
|
|
if (!ekf1_started) { |
|
|
|
|
// wait 1 second for DCM to output a valid tilt error estimate
|
|
|
|
|
if (start_time_ms == 0) { |
|
|
|
|
start_time_ms = hal.scheduler->millis(); |
|
|
|
|
start_time_ms = AP_HAL::millis(); |
|
|
|
|
} |
|
|
|
|
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) { |
|
|
|
|
if (AP_HAL::millis() - start_time_ms > startup_delay_ms) { |
|
|
|
|
ekf1_started = EKF1.InitialiseFilterDynamic(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -163,9 +163,9 @@ void AP_AHRS_NavEKF::update_EKF2(void)
@@ -163,9 +163,9 @@ void AP_AHRS_NavEKF::update_EKF2(void)
|
|
|
|
|
if (!ekf2_started) { |
|
|
|
|
// wait 1 second for DCM to output a valid tilt error estimate
|
|
|
|
|
if (start_time_ms == 0) { |
|
|
|
|
start_time_ms = hal.scheduler->millis(); |
|
|
|
|
start_time_ms = AP_HAL::millis(); |
|
|
|
|
} |
|
|
|
|
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) { |
|
|
|
|
if (AP_HAL::millis() - start_time_ms > startup_delay_ms) { |
|
|
|
|
ekf2_started = EKF2.InitialiseFilter(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -633,11 +633,11 @@ bool AP_AHRS_NavEKF::initialised(void) const
@@ -633,11 +633,11 @@ bool AP_AHRS_NavEKF::initialised(void) const
|
|
|
|
|
case 1: |
|
|
|
|
default: |
|
|
|
|
// initialisation complete 10sec after ekf has started
|
|
|
|
|
return (ekf1_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); |
|
|
|
|
return (ekf1_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); |
|
|
|
|
|
|
|
|
|
case 2: |
|
|
|
|
// initialisation complete 10sec after ekf has started
|
|
|
|
|
return (ekf2_started && (hal.scheduler->millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); |
|
|
|
|
return (ekf2_started && (AP_HAL::millis() - start_time_ms > AP_AHRS_NAVEKF_SETTLE_TIME_MS)); |
|
|
|
|
} |
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|