From 6361b4cd7e1c504aa0d0e667444d4135ed0a148e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 23 Feb 2026 14:02:56 +0100 Subject: [PATCH] Unify motor function mapping checks to only depend on the interface --- msg/EscReport.msg | 13 +------------ msg/versioned/ActuatorMotors.msg | 4 ++-- .../HealthAndArmingChecks/CMakeLists.txt | 2 +- .../HealthAndArmingChecks/checks/escCheck.cpp | 19 +++++++++---------- .../HealthAndArmingChecks/checks/escCheck.hpp | 1 - src/modules/mavlink/streams/ESC_INFO.hpp | 4 ++-- src/modules/mavlink/streams/ESC_STATUS.hpp | 4 ++-- 7 files changed, 17 insertions(+), 30 deletions(-) diff --git a/msg/EscReport.msg b/msg/EscReport.msg index 25331d951f..e84f9f46de 100644 --- a/msg/EscReport.msg +++ b/msg/EscReport.msg @@ -11,19 +11,8 @@ uint8 esc_cmdcount # Counter of number of commands uint8 esc_state # State of ESC - depend on Vendor uint8 actuator_function # actuator output function (one of Motor1...MotorN) - uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 -uint8 ACTUATOR_FUNCTION_MOTOR2 = 102 -uint8 ACTUATOR_FUNCTION_MOTOR3 = 103 -uint8 ACTUATOR_FUNCTION_MOTOR4 = 104 -uint8 ACTUATOR_FUNCTION_MOTOR5 = 105 -uint8 ACTUATOR_FUNCTION_MOTOR6 = 106 -uint8 ACTUATOR_FUNCTION_MOTOR7 = 107 -uint8 ACTUATOR_FUNCTION_MOTOR8 = 108 -uint8 ACTUATOR_FUNCTION_MOTOR9 = 109 -uint8 ACTUATOR_FUNCTION_MOTOR10 = 110 -uint8 ACTUATOR_FUNCTION_MOTOR11 = 111 -uint8 ACTUATOR_FUNCTION_MOTOR12 = 112 +uint8 ACTUATOR_FUNCTION_MOTOR_MAX = 112 # output_functions.yaml Motor.start + Motor.count - 1 uint16 failures # Bitmask to indicate the internal ESC faults int8 esc_power # Applied power 0-100 in % (negative values reserved) diff --git a/msg/versioned/ActuatorMotors.msg b/msg/versioned/ActuatorMotors.msg index 4b0b4df888..09b3820ec6 100644 --- a/msg/versioned/ActuatorMotors.msg +++ b/msg/versioned/ActuatorMotors.msg @@ -10,7 +10,7 @@ uint64 timestamp_sample # [us] Sampling timestamp of the data this control respo uint16 reversible_flags # [-] Bitset indicating which motors are configured to be reversible -uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 +uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 # output_functions.yaml Motor.start -uint8 NUM_CONTROLS = 12 +uint8 NUM_CONTROLS = 12 # output_functions.yaml Motor.count float32[12] control # [@range -1, 1] Normalized thrust. Where 1 means maximum positive thrust, -1 maximum negative (if not supported by the output, <0 maps to NaN). NaN maps to disarmed (stop the motors) diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index dcc5b7c1b4..b170d1d2ca 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -71,7 +71,7 @@ px4_add_library(health_and_arming_checks checks/externalChecks.cpp ) -add_dependencies(health_and_arming_checks mode_util output_functions_header) +add_dependencies(health_and_arming_checks mode_util) px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp LINKLIBS health_and_arming_checks mode_util diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 678b1143ed..946afa035f 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -122,8 +122,8 @@ uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, con esc_fail_msg[0] = '\0'; for (int esc_index = 0; esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) { - // check if mapped - if (!math::isInRange(esc_status.esc[esc_index].actuator_function, (uint8_t)OutputFunction::Motor1, (uint8_t)OutputFunction::MotorMax)) { + if (!math::isInRange(esc_status.esc[esc_index].actuator_function, + esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) { continue; // Skip unmapped ESC status entries } @@ -160,18 +160,18 @@ uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, con uint16_t mask = 0; for (int esc_index = 0; esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) { - if (!math::isInRange(esc_status.esc[esc_index].actuator_function, (uint8_t)OutputFunction::Motor1, (uint8_t)OutputFunction::MotorMax)) { + if (!math::isInRange(esc_status.esc[esc_index].actuator_function, + esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) { continue; // Skip unmapped ESC status entries } - const uint8_t act_function_index = - esc_status.esc[esc_index].actuator_function - static_cast(OutputFunction::Motor1); + const uint8_t actuator_function_index = esc_status.esc[esc_index].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1; if (esc_status.esc[esc_index].failures == 0) { continue; } else { - mask |= (1u << act_function_index); // Set bit in mask + mask |= (1u << actuator_function_index); // Set bit in mask } for (uint8_t fault_index = 0; fault_index <= static_cast(esc_fault_reason_t::_max); fault_index++) { @@ -229,13 +229,12 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c // Check individual ESC reports for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { - // Map the esc status index to the actuator function index - const uint8_t actuator_function_index = esc_status.esc[i].actuator_function - static_cast(OutputFunction::Motor1); - - if (!math::isInRange(esc_status.esc[i].actuator_function, (uint8_t)OutputFunction::Motor1, (uint8_t)OutputFunction::MotorMax)) { + if (!math::isInRange(esc_status.esc[i].actuator_function, + esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) { continue; // Skip unmapped ESC status entries } + const uint8_t actuator_function_index = esc_status.esc[i].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1; const float current = esc_status.esc[i].esc_current; // First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it. diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp index 25888a8c27..0e9b47361e 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp @@ -40,7 +40,6 @@ #include #include #include -#include class EscChecks : public HealthAndArmingCheckBase { diff --git a/src/modules/mavlink/streams/ESC_INFO.hpp b/src/modules/mavlink/streams/ESC_INFO.hpp index d4066a2332..8cdfbecdaa 100644 --- a/src/modules/mavlink/streams/ESC_INFO.hpp +++ b/src/modules/mavlink/streams/ESC_INFO.hpp @@ -102,8 +102,8 @@ private: _interface[i].esc_online_flags = 0; for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) { - bool is_motor = ((int)esc.esc[j].actuator_function >= esc_report_s::ACTUATOR_FUNCTION_MOTOR1) && - ((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12); + bool is_motor = math::isInRange(esc.esc[j].actuator_function, + esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX); if (is_motor) { // Map OutputFunction number to index diff --git a/src/modules/mavlink/streams/ESC_STATUS.hpp b/src/modules/mavlink/streams/ESC_STATUS.hpp index 925f77ab81..e316946a91 100644 --- a/src/modules/mavlink/streams/ESC_STATUS.hpp +++ b/src/modules/mavlink/streams/ESC_STATUS.hpp @@ -84,8 +84,8 @@ private: if (_esc_status_subs[i].update(&esc)) { for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) { - bool is_motor = ((int)esc.esc[j].actuator_function >= esc_report_s::ACTUATOR_FUNCTION_MOTOR1) && - ((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12); + const bool is_motor = math::isInRange(esc.esc[j].actuator_function, + esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX); if (is_motor) { // Map OutputFunction number to index