mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 02:43:33 +08:00
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:
@@ -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 {
|
||||||
|
|||||||
Reference in New Issue
Block a user