From 786e0a12c20bb3035b905dc7fafec609e4607afe Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 27 Aug 2025 16:54:15 +0200 Subject: [PATCH] FailureInjector simplification rework --- .../failure_detector/FailureInjector.cpp | 113 ++++++------------ .../failure_detector/FailureInjector.hpp | 4 +- 2 files changed, 38 insertions(+), 79 deletions(-) diff --git a/src/modules/commander/failure_detector/FailureInjector.cpp b/src/modules/commander/failure_detector/FailureInjector.cpp index f2960d9492..38bccdf5d2 100644 --- a/src/modules/commander/failure_detector/FailureInjector.cpp +++ b/src/modules/commander/failure_detector/FailureInjector.cpp @@ -40,114 +40,73 @@ void FailureInjector::update() vehicle_command_s vehicle_command; while (_vehicle_command_sub.update(&vehicle_command)) { - if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE) { + const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); + const int failure_type = static_cast(vehicle_command.param2 + 0.5f); + + if (vehicle_command.command != vehicle_command_s::VEHICLE_CMD_INJECT_FAILURE + || failure_unit != vehicle_command_s::FAILURE_UNIT_SYSTEM_MOTOR) { continue; } - bool handled = false; + const int instance = static_cast(vehicle_command.param3 + 0.5f); bool supported = false; - const int failure_unit = static_cast(vehicle_command.param1 + 0.5f); - const int failure_type = static_cast(vehicle_command.param2 + 0.5f); - const int instance = static_cast(vehicle_command.param3 + 0.5f); - - if (failure_unit == vehicle_command_s::FAILURE_UNIT_SYSTEM_MOTOR) { - handled = true; - - if (failure_type == vehicle_command_s::FAILURE_TYPE_OK) { - PX4_INFO("CMD_INJECT_FAILURE, motors ok"); - supported = false; - - // 0 to signal all - if (instance == 0) { - supported = true; - - for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { - PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i); - _esc_blocked &= ~(1 << i); - _esc_wrong &= ~(1 << i); - } - - } else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) { - supported = true; - - PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", instance - 1); - _esc_blocked &= ~(1 << (instance - 1)); - _esc_wrong &= ~(1 << (instance - 1)); - } + for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { + if (instance != 0 && i != (instance - 1)) { + continue; } - else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, motors off"); + switch (failure_type) { + case vehicle_command_s::FAILURE_TYPE_OK: supported = true; + PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i); + _esc_telemetry_blocked_mask &= ~(1 << i); + _esc_telemetry_wrong_mask &= ~(1 << i); + break; - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { - PX4_INFO("CMD_INJECT_FAILURE, motor %d off", i); - _esc_blocked |= 1 << i; - } - - } else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) { - PX4_INFO("CMD_INJECT_FAILURE, motor %d off", instance - 1); - _esc_blocked |= 1 << (instance - 1); - } - } - - else if (failure_type == vehicle_command_s::FAILURE_TYPE_WRONG) { - PX4_INFO("CMD_INJECT_FAILURE, motors wrong"); + case vehicle_command_s::FAILURE_TYPE_OFF: supported = true; + PX4_INFO("CMD_INJECT_FAILURE, motor %d no esc telemetry", i); + _esc_telemetry_blocked_mask |= 1 << i; + break; - // 0 to signal all - if (instance == 0) { - for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { - PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", i); - _esc_wrong |= 1 << i; - } - - } else if (instance >= 1 && instance <= esc_status_s::CONNECTED_ESC_MAX) { - PX4_INFO("CMD_INJECT_FAILURE, motor %d wrong", instance - 1); - _esc_wrong |= 1 << (instance - 1); - } + case vehicle_command_s::FAILURE_TYPE_WRONG: + supported = true; + PX4_INFO("CMD_INJECT_FAILURE, motor %d esc telemetry wrong", i); + _esc_telemetry_wrong_mask |= 1 << i; + break; } } - if (handled) { - vehicle_command_ack_s ack{}; - ack.command = vehicle_command.command; - ack.from_external = false; - ack.result = supported ? - vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : - vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; - ack.timestamp = hrt_absolute_time(); - _command_ack_pub.publish(ack); - } + vehicle_command_ack_s ack{}; + ack.command = vehicle_command.command; + ack.from_external = false; + ack.result = supported ? + vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : + vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; + ack.timestamp = hrt_absolute_time(); + _command_ack_pub.publish(ack); } - } void FailureInjector::manipulateEscStatus(esc_status_s &status) { - if (_esc_blocked != 0 || _esc_wrong != 0) { - unsigned offline = 0; - + if (_esc_telemetry_blocked_mask != 0 || _esc_telemetry_wrong_mask != 0) { for (int i = 0; i < status.esc_count; i++) { const unsigned i_esc = status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1; - if (_esc_blocked & (1 << i_esc)) { + if (_esc_telemetry_blocked_mask & (1 << i_esc)) { unsigned function = status.esc[i].actuator_function; memset(&status.esc[i], 0, sizeof(status.esc[i])); status.esc[i].actuator_function = function; - offline |= 1 << i; + status.esc_online_flags &= ~(1 << i); - } else if (_esc_wrong & (1 << i_esc)) { + } else if (_esc_telemetry_wrong_mask & (1 << i_esc)) { // Create wrong rerport for this motor by scaling key values up and down status.esc[i].esc_voltage *= 0.1f; status.esc[i].esc_current *= 0.1f; status.esc[i].esc_rpm *= 10.0f; } } - - status.esc_online_flags &= ~offline; } } diff --git a/src/modules/commander/failure_detector/FailureInjector.hpp b/src/modules/commander/failure_detector/FailureInjector.hpp index 6d4b9bef56..b74897e551 100644 --- a/src/modules/commander/failure_detector/FailureInjector.hpp +++ b/src/modules/commander/failure_detector/FailureInjector.hpp @@ -49,6 +49,6 @@ private: uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; - uint32_t _esc_blocked{}; - uint32_t _esc_wrong{}; + uint32_t _esc_telemetry_blocked_mask{}; + uint32_t _esc_telemetry_wrong_mask{}; };