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);