From 2f3cb97872fdfdede519f0ce4bfca35e9f4e16ab Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 27 Jun 2022 11:37:02 -0400 Subject: [PATCH] sensors/vehicle_magnetometer: don't advertise vehicle_magnetometer instance if mag has been disable - if using multi-EKF across all magnetometers then an instance of vehicle_magnetometer is advertised immediately for every sensor_mag instance - this can become problematic if EKF2 multi-mag is enabled, but with only 1 IMU (EKF2_MULTI_MAG) because you will be stuck with no magnetometer data --- .../VehicleMagnetometer.cpp | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 157413c4d5..6ea7d8e617 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -437,13 +437,6 @@ void VehicleMagnetometer::Run() } } - // advertise outputs in order if publishing all - if (!_param_sens_mag_mode.get()) { - for (int instance = 0; instance < uorb_index; instance++) { - _vehicle_magnetometer_pub[instance].advertise(); - } - } - if (_selected_sensor_sub_index < 0) { _sensor_sub[uorb_index].registerCallback(); } @@ -524,6 +517,16 @@ void VehicleMagnetometer::Run() out.timestamp = hrt_absolute_time(); if (multi_mode) { + + if (!_vehicle_magnetometer_pub[instance].advertised()) { + // prefer to maintain vehicle_magneometer instance numbering in sensor order + for (int mag_instance = 0; mag_instance < instance; mag_instance++) { + if (_calibration[instance].enabled()) { + _vehicle_magnetometer_pub[mag_instance].advertise(); + } + } + } + _vehicle_magnetometer_pub[instance].publish(out); } else {