Browse Source

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
main
Daniel Agar 3 years ago
parent
commit
2f3cb97872
  1. 17
      src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

17
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) { if (_selected_sensor_sub_index < 0) {
_sensor_sub[uorb_index].registerCallback(); _sensor_sub[uorb_index].registerCallback();
} }
@ -524,6 +517,16 @@ void VehicleMagnetometer::Run()
out.timestamp = hrt_absolute_time(); out.timestamp = hrt_absolute_time();
if (multi_mode) { 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); _vehicle_magnetometer_pub[instance].publish(out);
} else { } else {

Loading…
Cancel
Save