diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index aa3d47f682..b6f67dc05e 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -544,7 +544,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) bool accel_updated; orb_check(_accel.subscription[uorb_index], &accel_updated); - if (accel_updated && _accel.enabled[uorb_index]) { + if (accel_updated) { struct accel_report accel_report; int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report); @@ -553,6 +553,10 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) continue; //ignore invalid data } + if (!_accel.enabled[uorb_index]) { + continue; + } + // First publication with data if (_accel.priority[uorb_index] == 0) { int32_t priority = 0; @@ -649,7 +653,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) bool gyro_updated; orb_check(_gyro.subscription[uorb_index], &gyro_updated); - if (gyro_updated && _gyro.enabled[uorb_index]) { + if (gyro_updated) { struct gyro_report gyro_report; int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report); @@ -658,6 +662,10 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) continue; //ignore invalid data } + if (!_gyro.enabled[uorb_index]) { + continue; + } + // First publication with data if (_gyro.priority[uorb_index] == 0) { int32_t priority = 0; @@ -752,7 +760,7 @@ void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer) bool mag_updated; orb_check(_mag.subscription[uorb_index], &mag_updated); - if (mag_updated && _mag.enabled[uorb_index]) { + if (mag_updated) { struct mag_report mag_report; int ret = orb_copy(ORB_ID(sensor_mag), _mag.subscription[uorb_index], &mag_report); @@ -761,6 +769,10 @@ void VotedSensorsUpdate::mag_poll(vehicle_magnetometer_s &magnetometer) continue; //ignore invalid data } + if (!_mag.enabled[uorb_index]) { + continue; + } + // First publication with data if (_mag.priority[uorb_index] == 0) { int32_t priority = 0;