fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Unify motor function mapping checks to only depend on the interface

Browse Source
This commit is contained in:
Matthias Grob 2026-02-23 14:02:56 +01:00
parent 8bb82c70ee
commit 6361b4cd7e
7 changed files with 17 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
msg/EscReport.msg
View File

@ -11,19 +11,8 @@ uint8 esc_cmdcount # Counter of number of commands
uint8 esc_state # State of ESC - depend on Vendor uint8 esc_state # State of ESC - depend on Vendor
uint8 actuator_function # actuator output function (one of Motor1...MotorN) uint8 actuator_function # actuator output function (one of Motor1...MotorN)
uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 uint8 ACTUATOR_FUNCTION_MOTOR1 = 101
uint8 ACTUATOR_FUNCTION_MOTOR2 = 102 uint8 ACTUATOR_FUNCTION_MOTOR_MAX = 112 # output_functions.yaml Motor.start + Motor.count - 1
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
uint16 failures # Bitmask to indicate the internal ESC faults uint16 failures # Bitmask to indicate the internal ESC faults
int8 esc_power # Applied power 0-100 in % (negative values reserved) int8 esc_power # Applied power 0-100 in % (negative values reserved)

4
msg/versioned/ActuatorMotors.msg
View File

@ -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 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) 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)

2
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
View File

@ -71,7 +71,7 @@ px4_add_library(health_and_arming_checks
checks/externalChecks.cpp 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 px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
LINKLIBS health_and_arming_checks mode_util LINKLIBS health_and_arming_checks mode_util

19
src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
View File

@ -122,8 +122,8 @@ uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, con
esc_fail_msg[0] = '\0'; esc_fail_msg[0] = '\0';
for (int esc_index = 0; esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) { 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,
if (!math::isInRange(esc_status.esc[esc_index].actuator_function, (uint8_t)OutputFunction::Motor1, (uint8_t)OutputFunction::MotorMax)) { esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) {
continue; // Skip unmapped ESC status entries continue; // Skip unmapped ESC status entries
} }
@ -160,18 +160,18 @@ uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, con
uint16_t mask = 0; uint16_t mask = 0;
for (int esc_index = 0; esc_index < esc_status_s::CONNECTED_ESC_MAX; esc_index++) { 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 continue; // Skip unmapped ESC status entries
} }
const uint8_t act_function_index = const uint8_t actuator_function_index = esc_status.esc[esc_index].actuator_function - esc_report_s::ACTUATOR_FUNCTION_MOTOR1;
esc_status.esc[esc_index].actuator_function - static_cast<uint8_t>(OutputFunction::Motor1);
if (esc_status.esc[esc_index].failures == 0) { if (esc_status.esc[esc_index].failures == 0) {
continue; continue;
} else { } 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<uint8_t>(esc_fault_reason_t::_max); fault_index++) { for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(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 // Check individual ESC reports
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) {
// Map the esc status index to the actuator function index if (!math::isInRange(esc_status.esc[i].actuator_function,
const uint8_t actuator_function_index = esc_status.esc[i].actuator_function - static_cast<uint8_t>(OutputFunction::Motor1); esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX)) {
if (!math::isInRange(esc_status.esc[i].actuator_function, (uint8_t)OutputFunction::Motor1, (uint8_t)OutputFunction::MotorMax)) {
continue; // Skip unmapped ESC status entries 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; 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. // First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.

1
src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
View File

@ -40,7 +40,6 @@
#include <uORB/topics/actuator_motors.h> #include <uORB/topics/actuator_motors.h>
#include <uORB/topics/esc_status.h> #include <uORB/topics/esc_status.h>
#include <uORB/topics/failure_detector_status.h> #include <uORB/topics/failure_detector_status.h>
#include <mixer_module/output_functions.hpp>
class EscChecks : public HealthAndArmingCheckBase class EscChecks : public HealthAndArmingCheckBase
{ {

4
src/modules/mavlink/streams/ESC_INFO.hpp
View File

@ -102,8 +102,8 @@ private:
_interface[i].esc_online_flags = 0; _interface[i].esc_online_flags = 0;
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) { 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) && bool is_motor = math::isInRange(esc.esc[j].actuator_function,
((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12); esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX);
if (is_motor) { if (is_motor) {
// Map OutputFunction number to index // Map OutputFunction number to index

4
src/modules/mavlink/streams/ESC_STATUS.hpp
View File

@ -84,8 +84,8 @@ private:
if (_esc_status_subs[i].update(&esc)) { if (_esc_status_subs[i].update(&esc)) {
for (int j = 0; j < esc_status_s::CONNECTED_ESC_MAX; j++) { 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) && const bool is_motor = math::isInRange(esc.esc[j].actuator_function,
((int)esc.esc[j].actuator_function <= esc_report_s::ACTUATOR_FUNCTION_MOTOR12); esc_report_s::ACTUATOR_FUNCTION_MOTOR1, esc_report_s::ACTUATOR_FUNCTION_MOTOR_MAX);
if (is_motor) { if (is_motor) {
// Map OutputFunction number to index // Map OutputFunction number to index