From 38c1ec6a9c0cdf9b783096f6902fac5fea0cea26 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Tue, 27 Apr 2021 10:51:17 +0200 Subject: [PATCH] ekf2: check if ChangeInstance succeeded --- src/modules/ekf2/EKF2.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 6e7dabc63e..b0a55289be 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -194,12 +194,11 @@ bool EKF2::multi_init(int imu, int mag) _estimator_visual_odometry_aligned_pub.advertised(); _yaw_est_pub.advertise(); - _vehicle_imu_sub.ChangeInstance(imu); - _magnetometer_sub.ChangeInstance(mag); + bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); const int status_instance = _estimator_states_pub.get_instance(); - if ((status_instance >= 0) + if ((status_instance >= 0) && changed_instance && (_attitude_pub.get_instance() == status_instance) && (_local_position_pub.get_instance() == status_instance) && (_global_position_pub.get_instance() == status_instance)) {