diff --git a/src/modules/commander/failure_detector/CMakeLists.txt b/src/modules/commander/failure_detector/CMakeLists.txt index 8c68bc6d71..e6cd9d57c8 100644 --- a/src/modules/commander/failure_detector/CMakeLists.txt +++ b/src/modules/commander/failure_detector/CMakeLists.txt @@ -33,4 +33,5 @@ px4_add_library(failure_detector FailureDetector.cpp + FailureInjector.cpp ) diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index e475328566..34eb92089a 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -42,123 +42,6 @@ using namespace time_literals; -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) { - continue; - } - - bool handled = false; - 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)); - } - } - - else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { - PX4_WARN("CMD_INJECT_FAILURE, motors off"); - supported = true; - - // 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"); - supported = true; - - // 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); - } - } - } - - 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); - } - } - -} - -void FailureInjector::manipulateEscStatus(esc_status_s &status) -{ - if (_esc_blocked != 0 || _esc_wrong != 0) { - unsigned offline = 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)) { - 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; - - } else if (_esc_wrong & (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; - } -} - FailureDetector::FailureDetector(ModuleParams *parent) : ModuleParams(parent) { diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index e89719d2aa..67b70f1bf5 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -1,4 +1,3 @@ - /**************************************************************************** * * Copyright (c) 2018 PX4 Development Team. All rights reserved. @@ -43,6 +42,8 @@ #pragma once +#include "FailureInjector.hpp" + #include #include #include @@ -56,12 +57,9 @@ #include #include #include -#include -#include #include #include #include -#include #include union failure_detector_status_u { @@ -80,20 +78,6 @@ union failure_detector_status_u { using uORB::SubscriptionData; -class FailureInjector -{ -public: - void update(); - - void manipulateEscStatus(esc_status_s &status); -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{}; -}; - class FailureDetector : public ModuleParams { public: diff --git a/src/modules/commander/failure_detector/FailureInjector.cpp b/src/modules/commander/failure_detector/FailureInjector.cpp new file mode 100644 index 0000000000..f2960d9492 --- /dev/null +++ b/src/modules/commander/failure_detector/FailureInjector.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "FailureInjector.hpp" + +#include + +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) { + continue; + } + + bool handled = false; + 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)); + } + } + + else if (failure_type == vehicle_command_s::FAILURE_TYPE_OFF) { + PX4_WARN("CMD_INJECT_FAILURE, motors off"); + supported = true; + + // 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"); + supported = true; + + // 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); + } + } + } + + 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); + } + } + +} + +void FailureInjector::manipulateEscStatus(esc_status_s &status) +{ + if (_esc_blocked != 0 || _esc_wrong != 0) { + unsigned offline = 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)) { + 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; + + } else if (_esc_wrong & (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 new file mode 100644 index 0000000000..6d4b9bef56 --- /dev/null +++ b/src/modules/commander/failure_detector/FailureInjector.hpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +class FailureInjector +{ +public: + void update(); + + void manipulateEscStatus(esc_status_s &status); +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{}; +};