mirror of https://github.com/ArduPilot/ardupilot
AP_AHRS: delay EKF start until logging ready
for replay to work reliably we need all the parameters output before we init the EKF. Co-authored-by: Peter Barker <pbarker@barker.dropbear.id.au>
This commit is contained in:
parent
096aab9388
commit
31fbb59384
|
@ -26,6 +26,7 @@
|
|||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_InternalError/AP_InternalError.h>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
#include <AP_Notify/AP_Notify.h>
|
||||
#include <AP_Vehicle/AP_Vehicle_Type.h>
|
||||
|
||||
|
@ -194,13 +195,20 @@ void AP_AHRS_NavEKF::update_EKF2(void)
|
|||
if (start_time_ms == 0) {
|
||||
start_time_ms = AP_HAL::millis();
|
||||
}
|
||||
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) {
|
||||
_ekf2_started = EKF2.InitialiseFilter();
|
||||
if (_force_ekf) {
|
||||
// if we're doing Replay logging then don't allow any data
|
||||
// into the EKF yet. Don't allow it to block us for long.
|
||||
if (!hal.util->was_watchdog_reset()) {
|
||||
if (AP_HAL::millis() - start_time_ms < 5000) {
|
||||
if (!AP::logger().allow_start_ekf()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
|
||||
_ekf2_started = EKF2.InitialiseFilter();
|
||||
}
|
||||
}
|
||||
if (_ekf2_started) {
|
||||
EKF2.UpdateFilter();
|
||||
if (active_EKF_type() == EKFType::TWO) {
|
||||
|
@ -268,13 +276,19 @@ void AP_AHRS_NavEKF::update_EKF3(void)
|
|||
if (start_time_ms == 0) {
|
||||
start_time_ms = AP_HAL::millis();
|
||||
}
|
||||
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) {
|
||||
_ekf3_started = EKF3.InitialiseFilter();
|
||||
if (_force_ekf) {
|
||||
// if we're doing Replay logging then don't allow any data
|
||||
// into the EKF yet. Don't allow it to block us for long.
|
||||
if (!hal.util->was_watchdog_reset()) {
|
||||
if (AP_HAL::millis() - start_time_ms < 5000) {
|
||||
if (!AP::logger().allow_start_ekf()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (AP_HAL::millis() - start_time_ms > startup_delay_ms) {
|
||||
_ekf3_started = EKF3.InitialiseFilter();
|
||||
}
|
||||
}
|
||||
if (_ekf3_started) {
|
||||
EKF3.UpdateFilter();
|
||||
if (active_EKF_type() == EKFType::THREE) {
|
||||
|
|
|
@ -277,9 +277,6 @@ public:
|
|||
|
||||
bool getGpsGlitchStatus() const;
|
||||
|
||||
// used by Replay to force start at right timestamp
|
||||
void force_ekf_start(void) { _force_ekf = true; }
|
||||
|
||||
// is the EKF backend doing its own sensor logging?
|
||||
bool have_ekf_logging(void) const override;
|
||||
|
||||
|
@ -348,7 +345,6 @@ private:
|
|||
bool _ekf3_started;
|
||||
void update_EKF3(void);
|
||||
#endif
|
||||
bool _force_ekf;
|
||||
|
||||
// rotation from vehicle body to NED frame
|
||||
Matrix3f _dcm_matrix;
|
||||
|
|
Loading…
Reference in New Issue