fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 06:23:34 +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-03-12 18:30:51 +01:00
parent 8bb82c70ee
commit 6361b4cd7e
7 changed files with 17 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
+1 -1
View File
@@ -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
src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
+9 -10
View File
@@ -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<uint8_t>(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<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
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<uint8_t>(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.
src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
-1
View File
@@ -40,7 +40,6 @@
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/failure_detector_status.h>
#include <mixer_module/output_functions.hpp>
class EscChecks : public HealthAndArmingCheckBase
{
src/modules/mavlink/streams/ESC_INFO.hpp
+2 -2
View File
@@ -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
src/modules/mavlink/streams/ESC_STATUS.hpp
+2 -2
View File
@@ -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