From 2d15c96b9b3aad71b9fcb1333aa30cb280177c62 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 13 Apr 2021 14:24:46 -0400 Subject: [PATCH] ekf2: Multi-EKF let new instance schedule itself immediately on successful init - this is to minimize missed sensor data (and unnecessary error output) between EKF2 construction and once it actually starts running --- src/modules/ekf2/EKF2.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 441c3f1b22..6e7dabc63e 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -205,6 +205,8 @@ bool EKF2::multi_init(int imu, int mag) && (_global_position_pub.get_instance() == status_instance)) { _instance = status_instance; + + ScheduleNow(); return true; } @@ -1766,16 +1768,21 @@ int EKF2::task_spawn(int argc, char *argv[]) if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) { _objects[actual_instance].store(ekf2_inst); - ekf2_inst->ScheduleNow(); success = true; multi_instances_allocated++; ekf2_instance_created[imu][mag] = true; + if (actual_instance == 0) { + // force selector to run immediately if first instance started + _ekf2_selector.load()->ScheduleNow(); + } + PX4_INFO("starting instance %d, IMU:%d (%d), MAG:%d (%d)", actual_instance, imu, vehicle_imu_sub.get().accel_device_id, mag, vehicle_mag_sub.get().device_id); - _ekf2_selector.load()->ScheduleNow(); + // sleep briefly before starting more instances + px4_usleep(10000); } else { PX4_ERR("instance numbering problem instance: %d", actual_instance);