From f8b52c6a672268c709fdf8b73b4a8d243512d804 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2016 08:34:32 +1100 Subject: [PATCH] AP_AHRS: try to start EKF2 slightly before EKF2 this gives priority to EKF2 on memory --- libraries/AP_AHRS/AP_AHRS.h | 5 +++++ libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 3 ++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e23ce96830..0d5ad7dcc6 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -412,6 +412,11 @@ public: // time that the AHRS has been up virtual uint32_t uptime_ms(void) const = 0; + // get the selected ekf type, for allocation decisions + int8_t get_ekf_type(void) const { + return _ekf_type; + } + protected: AHRS_VehicleClass _vehicle_class; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 06abbeed65..bd3c5e6088 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -109,7 +109,8 @@ void AP_AHRS_NavEKF::update_EKF1(void) if (start_time_ms == 0) { start_time_ms = AP_HAL::millis(); } - if (AP_HAL::millis() - start_time_ms > startup_delay_ms) { + // slight extra delay on EKF1 to prioritise EKF2 for memory + if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100) { ekf1_started = EKF1.InitialiseFilterDynamic(); } }