diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index ed78b0a818..e449ba1f0a 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -827,7 +827,7 @@ VotedSensorsUpdate::calcAccelInconsistency(sensor_preflight_s &preflt) for (int sensor_index = 0; sensor_index < _accel.subscription_count; sensor_index++) { // check that the sensor we are checking against is not the same as the primary - if ((_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) { + if (_accel.enabled[sensor_index] && (_accel.priority[sensor_index] > 0) && (sensor_index != _accel.last_best_vote)) { float accel_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison agains the primary @@ -876,7 +876,7 @@ void VotedSensorsUpdate::calcGyroInconsistency(sensor_preflight_s &preflt) for (int sensor_index = 0; sensor_index < _gyro.subscription_count; sensor_index++) { // check that the sensor we are checking against is not the same as the primary - if ((_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) { + if (_gyro.enabled[sensor_index] && (_gyro.priority[sensor_index] > 0) && (sensor_index != _gyro.last_best_vote)) { float gyro_diff_sum_sq = 0.0f; // sum of differences squared for a single sensor comparison against the primary @@ -925,7 +925,7 @@ void VotedSensorsUpdate::calcMagInconsistency(sensor_preflight_s &preflt) // Check each sensor against the primary for (int i = 0; i < _mag.subscription_count; i++) { // check that the sensor we are checking against is not the same as the primary - if ((_mag.priority[i] > 0) && (i != _mag.last_best_vote)) { + if (_mag.enabled[i] && (_mag.priority[i] > 0) && (i != _mag.last_best_vote)) { // calculate angle to 3D magnetic field vector of the primary sensor Vector3f current_mag(_last_magnetometer[i].magnetometer_ga); float angle_error = AxisAnglef(Quatf(current_mag, primary_mag)).angle();