From b139f48fef2581a923395e920a508df4458f2d87 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 1 Jun 2020 11:25:12 -0400 Subject: [PATCH] sensors: throttle mavlink emergency log messages - in rare situations sending these messages multiple times in close succession can actually cause a loop of additional sensor timeouts --- .../vehicle_air_data/VehicleAirData.cpp | 19 ++++++++------ .../vehicle_air_data/VehicleAirData.hpp | 1 + src/modules/sensors/voted_sensors_update.cpp | 25 ++++++++++++------- src/modules/sensors/voted_sensors_update.h | 1 + 4 files changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index ec27e568f3..6650225dcf 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -252,13 +252,18 @@ void VehicleAirData::Run() } else { if (failover_index != -1) { - mavlink_log_emergency(&_mavlink_log_pub, "sensor_baro:#%i failed: %s%s%s%s%s!, reconfiguring priorities", - failover_index, - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_error_message > 3_s) { + mavlink_log_emergency(&_mavlink_log_pub, "sensor_baro:#%i failed: %s%s%s%s%s!, reconfiguring priorities", + failover_index, + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + _last_error_message = now; + } // reduce priority of failed sensor to the minimum _priority[failover_index] = ORB_PRIO_MIN; diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 1d5c052317..d31dc204b2 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -87,6 +87,7 @@ private: perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + hrt_abstime _last_error_message{0}; orb_advert_t _mavlink_log_pub{nullptr}; DataValidatorGroup _voter{1}; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index fbaf8e958e..a8e5328e9e 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -50,6 +50,7 @@ using namespace sensors; using namespace matrix; +using namespace time_literals; using math::radians; VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled, @@ -615,20 +616,26 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na if (flags == DataValidator::ERROR_FLAG_NO_ERROR) { if (failover_index != -1) { - //we switched due to a non-critical reason. No need to panic. + // we switched due to a non-critical reason. No need to panic. PX4_INFO("%s sensor switch from #%i", sensor_name, failover_index); } } else { if (failover_index != -1) { - mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!", - sensor_name, - failover_index, - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_error_message > 3_s) { + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!", + sensor_name, + failover_index, + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + _last_error_message = now; + } // reduce priority of failed sensor to the minimum sensor.priority[failover_index] = ORB_PRIO_MIN; diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 6e27762e80..c7f613e7f4 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -185,6 +185,7 @@ private: SensorData _gyro{ORB_ID::sensor_gyro}; SensorData _mag{ORB_ID::sensor_mag}; + hrt_abstime _last_error_message{0}; orb_advert_t _mavlink_log_pub{nullptr}; uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; /**< handle to the sensor selection uORB topic */