Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-03-24 02:24:09 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

uavcan: optimization and edge cases

Browse Source
This commit is contained in:
ttechnick
2026-02-17 11:56:36 +01:00
committed by Matthias Grob
parent c5652b2084
commit 4e279b16c2
4 changed files with 16 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -71,7 +71,7 @@ px4_add_library(health_and_arming_checks
checks/externalChecks.cpp
)
add_dependencies(health_and_arming_checks mode_util)
add_dependencies(health_and_arming_checks mode_util output_functions_header)
px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp
LINKLIBS health_and_arming_checks mode_util

8
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks_params.c
View File

@@ -31,14 +31,6 @@
*
****************************************************************************/
/**
* @file health_and_arming_checks_params.c
*
* Parameters used by the Health and Arming Checks module.
*
* @author Nick Truttmann <nick.truttmann@icloud.com>
*/
#include <px4_platform_common/px4_config.h>
#include <parameters/param.h>

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

@@ -72,8 +72,7 @@ static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_r
void EscChecks::checkAndReport(const Context &context, Report &reporter)
{
const hrt_abstime now = hrt_absolute_time();
const hrt_abstime esc_telemetry_timeout =
2_s; // Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
const hrt_abstime esc_telemetry_timeout = 700_ms; // Some DShot ESCs are unresponsive for ~550ms during their initialization
esc_status_s esc_status;
@@ -90,7 +89,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
mask |= checkEscStatus(context, reporter, esc_status);
updateEscsStatus(context, reporter, esc_status);
if (!_param_fd_act_en.get()) {
if (_param_fd_act_en.get() > 0) {
mask |= checkMotorStatus(context, reporter, esc_status);
}
@@ -98,8 +97,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
reporter.setIsPresent(health_component_t::motors_escs);
reporter.failsafeFlags().fd_motor_failure = mask != 0;
} else if (_param_com_arm_chk_escs.get()
&& now - _start_time > esc_telemetry_timeout) { // Wait a bit after startup to allow esc's to init
} else if ((_param_com_arm_chk_escs.get() > 0) && now - _start_time > esc_telemetry_timeout) { // Allow esc's to init
/* EVENT
* @description
@@ -119,19 +117,17 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, const esc_status_s &esc_status)
{
// Check if one or more the ESCs are offline
if (!_param_com_arm_chk_escs.get()) {
if (_param_com_arm_chk_escs.get() == 0) {
return 0;
}
uint16_t mask = 0;
char esc_fail_msg[50];
char esc_fail_msg[esc_status_s::CONNECTED_ESC_MAX * 6 + 1] {};
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, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) {
if (!math::isInRange(esc_status.esc[esc_index].actuator_function, (uint8_t)OutputFunction::Motor1, (uint8_t)OutputFunction::MotorMax)) {
continue; // Skip unmapped ESC status entries
}
@@ -158,16 +154,14 @@ uint16_t EscChecks::checkEscOnline(const Context &context, Report &reporter, con
if (reporter.mavlink_log_pub() && esc_fail_msg[0] != '\0') {
mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
}
return mask;
}
uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
{
if (!_param_com_arm_chk_escs.get()) {
if (_param_com_arm_chk_escs.get() == 0) {
return 0;
}
@@ -175,13 +169,12 @@ uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, con
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, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1))) {
if (!math::isInRange(esc_status.esc[esc_index].actuator_function, (uint8_t)OutputFunction::Motor1, (uint8_t)OutputFunction::MotorMax)) {
continue; // Skip unmapped ESC status entries
}
const uint8_t act_function_index =
esc_status.esc[esc_index].actuator_function - actuator_motors_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) {
continue;
@@ -190,14 +183,12 @@ uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, con
mask |= (1u << act_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++) {
if (!(esc_status.esc[esc_index].failures & (1 << fault_index))) {
continue;
}
esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
const char *user_action = "";
events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
@@ -238,7 +229,7 @@ uint16_t EscChecks::checkEscStatus(const Context &context, Report &reporter, con
uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
{
if (!_param_fd_act_en.get()) {
if (_param_fd_act_en.get() == 0) {
return 0;
}
@@ -251,13 +242,11 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c
_actuator_motors_sub.copy(&actuator_motors);
// 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
const uint8_t actuator_function_index =
esc_status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
const uint8_t actuator_function_index = esc_status.esc[i].actuator_function - static_cast<uint8_t>(OutputFunction::Motor1);
// check if mapped
if (!math::isInRange(actuator_function_index, uint8_t(0), uint8_t(actuator_motors_s::NUM_CONTROLS - 1))) {
if (!math::isInRange(esc_status.esc[i].actuator_function, (uint8_t)OutputFunction::Motor1, (uint8_t)OutputFunction::MotorMax)) {
continue; // Skip unmapped ESC status entries
}
@@ -340,16 +329,14 @@ uint16_t EscChecks::checkMotorStatus(const Context &context, Report &reporter, c
_esc_undercurrent_hysteresis[i].set_state_and_update(false, now);
_esc_overcurrent_hysteresis[i].set_state_and_update(false, now);
}
}
return mask;
}
void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
{
if (!_param_com_arm_chk_escs.get()) {
if (_param_com_arm_chk_escs.get() == 0) {
return;
}
@@ -374,13 +361,11 @@ void EscChecks::updateEscsStatus(const Context &context, Report &reporter, const
events::ID("check_escs_not_all_armed"),
events::Log::Critical, "Not all ESCs are armed");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "ESC failure: Not all ESCs are armed. Land now!");
}
reporter.failsafeFlags().fd_esc_arming_failure = true;
}
} else {

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

@@ -40,6 +40,7 @@
#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
{
@@ -61,7 +62,6 @@ private:
uint16_t checkMotorStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
void updateEscsStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
@@ -83,5 +83,4 @@ private:
(ParamInt<px4::params::MOTFAIL_TOUT>) _param_motfail_tout,
(ParamFloat<px4::params::MOTFAIL_LOW_OFF>) _param_motfail_low_off,
(ParamFloat<px4::params::MOTFAIL_HIGH_OFF>) _param_motfail_high_off);
};