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

uavcan: cleanup

Browse Source
This commit is contained in:
ttechnick 2026-02-09 13:27:07 +01:00 committed by Matthias Grob
parent b2ea7ffab6
commit 15f5a18629
3 changed files with 81 additions and 82 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -104,17 +104,21 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
{
const NavModes required_modes = _param_com_arm_chk_escs.get() ? NavModes::All : NavModes::None;
if (esc_status.esc_count > 0) {
// Check if one or more the ESCs are offline
char esc_fail_msg[50];
esc_fail_msg[0] = '\0';
if (esc_status.esc_count <= 0) {
return;
}
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
const bool mapped = math::isInRange(esc_status.esc[i].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1));
const bool offline = (esc_status.esc_online_flags & (1 << i)) == 0;
char esc_fail_msg[50];
esc_fail_msg[0] = '\0';
if (mapped && offline) {
int online_bitmask = (1 << esc_status.esc_count) - 1;
// Check if one or more the ESCs are offline
if (online_bitmask != esc_status.esc_online_flags) {
for (int index = 0; index < esc_status.esc_count; index++) {
if ((esc_status.esc_online_flags & (1 << index)) == 0) {
uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1;
/* EVENT
* @description
* <profile name="dev">
@ -122,99 +126,98 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
* </profile>
*/
reporter.healthFailure<uint8_t>(required_modes, health_component_t::motors_escs, events::ID("check_escs_offline"),
events::Log::Critical, "ESC {1} offline", i + 1);
snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", i + 1);
events::Log::Critical, "ESC {1} offline", motor_index);
snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", motor_index);
esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
}
}
if ((esc_fail_msg[0] != '\0') && reporter.mavlink_log_pub()) {
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
}
}
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) {
const bool esc_failure = (esc_status.esc[index].failures != 0);
reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
const bool motor_failure = (reporter.failsafeFlags().fd_motor_failure);
const bool esc_failure = (esc_status.esc[index].failures != 0);
reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
const bool motor_failure = (reporter.failsafeFlags().fd_motor_failure);
if (esc_failure) {
if (esc_failure) {
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max); fault_index++) {
if (esc_status.esc[index].failures & (1 << fault_index)) {
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max); fault_index++) {
if (esc_status.esc[index].failures & (1 << fault_index)) {
esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
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;
const char *user_action = "";
events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
if (context.isArmed()) {
if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
|| fault_reason_index == esc_fault_reason_t::esc_warn_temp
|| fault_reason_index == esc_fault_reason_t::over_rpm) {
user_action = "Reduce throttle";
action = events::px4::enums::suggested_action_t::reduce_throttle;
if (context.isArmed()) {
if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
|| fault_reason_index == esc_fault_reason_t::esc_warn_temp
|| fault_reason_index == esc_fault_reason_t::over_rpm) {
user_action = "Reduce throttle";
action = events::px4::enums::suggested_action_t::reduce_throttle;
} else {
user_action = "Land now!";
action = events::px4::enums::suggested_action_t::land;
}
} else {
user_action = "Land now!";
action = events::px4::enums::suggested_action_t::land;
}
}
uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1;
uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1;
/* EVENT
* @description
* {3}
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>(
required_modes, health_component_t::motors_escs, events::ID("check_escs_fault"),
events::Log::Critical, "ESC {1}: {2}", motor_index, fault_reason_index, action);
/* EVENT
* @description
* {3}
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>(
required_modes, health_component_t::motors_escs, events::ID("check_escs_fault"),
events::Log::Critical, "ESC {1}: {2}", motor_index, fault_reason_index, action);
if (reporter.mavlink_log_pub()) {
mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", motor_index,
esc_fault_reason_str(fault_reason_index), user_action);
}
if (reporter.mavlink_log_pub()) {
mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", motor_index,
esc_fault_reason_str(fault_reason_index), user_action);
}
}
}
}
if (motor_failure) {
reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
if (motor_failure) {
reporter.failsafeFlags().fd_motor_failure = context.status().failure_detector_status & vehicle_status_s::FAILURE_MOTOR;
if (reporter.failsafeFlags().fd_motor_failure) {
// Get the failure detector status to check which motor(s) failed
failure_detector_status_s failure_detector_status{};
bool have_motor_mask = _failure_detector_status_sub.copy(&failure_detector_status);
if (reporter.failsafeFlags().fd_motor_failure) {
// Get the failure detector status to check which motor(s) failed
failure_detector_status_s failure_detector_status{};
bool have_motor_mask = _failure_detector_status_sub.copy(&failure_detector_status);
if (have_motor_mask && failure_detector_status.motor_failure_mask != 0) {
for (uint8_t motor_index = 0; motor_index < esc_status_s::CONNECTED_ESC_MAX; motor_index++) {
if (failure_detector_status.motor_failure_mask & (1 << motor_index)) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>FD_ACT_EN</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs,
events::ID("check_failure_detector_motor"),
events::Log::Critical, "Motor {1} failure detected", motor_index);
if (have_motor_mask && failure_detector_status.motor_failure_mask != 0) {
for (uint8_t motor_index = 0; motor_index < esc_status_s::CONNECTED_ESC_MAX; motor_index++) {
if (failure_detector_status.motor_failure_mask & (1 << motor_index)) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>FD_ACT_EN</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(NavModes::All, health_component_t::motors_escs,
events::ID("check_failure_detector_motor"),
events::Log::Critical, "Motor {1} failure detected", motor_index);
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor %d failure detected",
motor_index);
}
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Motor %d failure detected",
motor_index);
}
}
}
}
}
}
}
}

17
src/modules/commander/failure_detector/FailureDetector.cpp
View File

@ -191,7 +191,7 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
}
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(!is_all_escs_armed, now);
_esc_failure_hysteresis.set_state_and_update(!is_all_escs_armed, time_now);
_failure_detector_status.flags.arm_escs = _esc_failure_hysteresis.get_state();
@ -286,8 +286,8 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
// Check individual ESC reports
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
const bool mapped = math::isInRange(esc_status.esc[i].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1));
const bool mapped = math::isInRange((int)esc_status.esc[i].actuator_function, (int)OutputFunction::Motor1,
(int)OutputFunction::MotorMax);
// Skip if ESC is not actually connected
if (!mapped) {
@ -296,8 +296,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
// 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;
esc_status.esc[i].actuator_function - (int)OutputFunction::Motor1;
if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) {
continue; // Invalid mapping
@ -332,10 +331,6 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
_esc_has_reported_current[i] = true;
}
if (!_esc_has_reported_current[i]) {
continue;
}
// Current limits
float thrust = 0.f;
@ -351,12 +346,12 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
_esc_undercurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
_esc_overcurrent_hysteresis[i].set_hysteresis_time_from(false, _param_fd_act_mot_tout.get() * 1_ms);
if (!_esc_undercurrent_hysteresis[i].get_state()) {
if (_esc_has_reported_current[i] && !_esc_undercurrent_hysteresis[i].get_state()) {
// Do not clear because a reaction could be to stop the motor and that would be conidered healty again
_esc_undercurrent_hysteresis[i].set_state_and_update(thrust_above_threshold && current_too_low && !timeout, now);
}
if (!_esc_overcurrent_hysteresis[i].get_state()) {
if (_esc_has_reported_current[i] && !_esc_overcurrent_hysteresis[i].get_state()) {
// Do not clear because a reaction could be to stop the motor and that would be conidered healty again
_esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high && !timeout, now);
}

1
src/modules/commander/failure_detector/FailureDetector.hpp
View File

@ -48,6 +48,7 @@
#include <lib/mathlib/mathlib.h>
#include <lib/mathlib/math/filter/AlphaFilter.hpp>
#include <matrix/matrix/math.hpp>
#include <mixer_module/output_functions.hpp>
#include <px4_platform_common/module_params.h>
// subscriptions