Browse Source

AP_AHRS: try to start EKF2 slightly before EKF2

this gives priority to EKF2 on memory
mission-4.1.18
Andrew Tridgell 9 years ago
parent
commit
f8b52c6a67
  1. 5
      libraries/AP_AHRS/AP_AHRS.h
  2. 3
      libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

5
libraries/AP_AHRS/AP_AHRS.h

@ -412,6 +412,11 @@ public:
// time that the AHRS has been up // time that the AHRS has been up
virtual uint32_t uptime_ms(void) const = 0; 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: protected:
AHRS_VehicleClass _vehicle_class; AHRS_VehicleClass _vehicle_class;

3
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@ -109,7 +109,8 @@ void AP_AHRS_NavEKF::update_EKF1(void)
if (start_time_ms == 0) { if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis(); 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(); ekf1_started = EKF1.InitialiseFilterDynamic();
} }
} }

Loading…
Cancel
Save