From 79a0edafb5f538c9c48e0b51c9405c65aadf21e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 9 Sep 2021 22:03:12 +0200 Subject: [PATCH] vehicle_air_data: switch to events --- .../vehicle_air_data/VehicleAirData.cpp | 24 ++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index dc515971c7..83c1a75a10 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -34,6 +34,7 @@ #include "VehicleAirData.hpp" #include +#include #include namespace sensors @@ -288,7 +289,7 @@ void VehicleAirData::Run() const hrt_abstime now = hrt_absolute_time(); if (now - _last_error_message > 3_s) { - mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!", + mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!\t", "BARO", failover_index, ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), @@ -296,6 +297,27 @@ void VehicleAirData::Run() ((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_baro"), events::Log::Emergency, "Baro sensor #{1} failure: {2}", failover_index, + failover_reason); + _last_error_message = now; }