fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: Multi-EKF let new instance schedule itself immediately on successful init

Browse Source 
- this is to minimize missed sensor data (and unnecessary error output) between EKF2 construction and once it actually starts running
This commit is contained in:
Daniel Agar 2021-04-13 14:24:46 -04:00 committed by GitHub
parent 6a4c589c62
commit 2d15c96b9b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/ekf2/EKF2.cpp
View File

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