Unverified Commit 2d15c96b authored by Daniel Agar's avatar Daniel Agar Committed by GitHub
Browse files

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
parent 6a4c589c
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment