From 7bb123c65ed50e5389533ea0a359c57cb09209af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 21 Apr 2021 11:09:43 +0200 Subject: [PATCH] voted_sensors_update: switch to events --- src/modules/sensors/voted_sensors_update.cpp | 30 +++++++++++++++++--- src/modules/sensors/voted_sensors_update.h | 3 +- 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 11bb5fcc55..794fb796c5 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -216,8 +216,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) _accel.voter.get_best(hrt_absolute_time(), &accel_best_index); _gyro.voter.get_best(hrt_absolute_time(), &gyro_best_index); - checkFailover(_accel, "Accel"); - checkFailover(_gyro, "Gyro"); + checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel); + checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro); } // write data for the best sensor to output variables @@ -276,7 +276,8 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) } } -bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name) +bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name, + events::px4::enums::sensor_type_t sensor_type) { if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) { @@ -295,7 +296,7 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na 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!", + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i fail: %s%s%s%s%s!\t", sensor_name, failover_index, ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), @@ -303,6 +304,27 @@ bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_na ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""), ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""), ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : "")); + + events::px4::enums::sensor_failover_reason_t failover_reason{}; + + if (flags & DataValidator::ERROR_FLAG_NO_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::no_data; } + + if (flags & DataValidator::ERROR_FLAG_STALE_DATA) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::stale_data; } + + if (flags & DataValidator::ERROR_FLAG_TIMEOUT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::timeout; } + + if (flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_count; } + + if (flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) { failover_reason = failover_reason | events::px4::enums::sensor_failover_reason_t::high_error_density; } + + /* EVENT + * @description + * Land immediately and check the system. + */ + events::send( + events::ID("sensor_failover"), events::Log::Emergency, "{1} sensor #{2} failure: {3}", sensor_type, failover_index, + failover_reason); + _last_error_message = now; } diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 750a3d5a2c..4ae773a867 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -42,6 +42,7 @@ #include "data_validator/DataValidator.hpp" #include "data_validator/DataValidatorGroup.hpp" +#include #include #include #include @@ -139,7 +140,7 @@ private: * Check & handle failover of a sensor * @return true if a switch occured (could be for a non-critical reason) */ - bool checkFailover(SensorData &sensor, const char *sensor_name); + bool checkFailover(SensorData &sensor, const char *sensor_name, events::px4::enums::sensor_type_t sensor_type); /** * Calculates the magnitude in m/s/s of the largest difference between each accelerometer vector and the mean of all vectors