|
|
|
@ -59,7 +59,7 @@ void AP_AHRS_NavEKF::update(void)
@@ -59,7 +59,7 @@ void AP_AHRS_NavEKF::update(void)
|
|
|
|
|
} |
|
|
|
|
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) { |
|
|
|
|
ekf_started = true; |
|
|
|
|
EKF.InitialiseFilter(); |
|
|
|
|
EKF.InitialiseFilterBootstrap(); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -85,7 +85,7 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers)
@@ -85,7 +85,7 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers)
|
|
|
|
|
{ |
|
|
|
|
AP_AHRS_DCM::reset(recover_eulers); |
|
|
|
|
if (ekf_started) { |
|
|
|
|
EKF.InitialiseFilter();
|
|
|
|
|
EKF.InitialiseFilterBootstrap();
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -100,7 +100,7 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con
@@ -100,7 +100,7 @@ void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, con
|
|
|
|
|
{ |
|
|
|
|
AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw); |
|
|
|
|
if (ekf_started) { |
|
|
|
|
EKF.InitialiseFilter();
|
|
|
|
|
EKF.InitialiseFilterBootstrap();
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|