dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
Daniel Agar 2022-06-27 11:37:02 -04:00
parent e5be0e776e
commit 2f3cb97872
1 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -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 {