diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index c73dadff34..97a0ea7d2a 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -920,14 +920,19 @@ bool VotedSensorsUpdate::check_failover(SensorData &sensor, const char *sensor_n } } else { + int failover_index = sensor.voter.failover_index(); + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!", sensor_name, - sensor.voter.failover_index(), + failover_index, ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""), ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""), ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : "")); + + // reduce priority of failed sensor to the minimum + sensor.priority[failover_index] = 1; } sensor.last_failover_count = sensor.voter.failover_count(); @@ -1258,4 +1263,4 @@ void VotedSensorsUpdate::calc_mag_inconsistency(sensor_preflight_s &preflt) // get the vector length of the largest difference and write to the combined sensor struct preflt.mag_inconsistency_ga = sqrtf(mag_diff_sum_max_sq); } -} \ No newline at end of file +}