From 2a1ecaa13eb7b6fbad2b2e3629801150a6ed5df5 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 26 Oct 2017 15:37:59 -0400 Subject: [PATCH] sensors reduce priority of a failed sensor --- src/modules/sensors/voted_sensors_update.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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 +}