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: unify timeout handling

Browse Source
This commit is contained in:
ttechnick 2026-01-30 10:34:18 +01:00 committed by Matthias Grob
parent aa998d88b8
commit 9f978b05f3
3 changed files with 96 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -146,7 +146,8 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e
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::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;

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

@ -72,14 +72,17 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
if (_esc_status_sub.update(&esc_status)) {
_failure_injector.manipulateEscStatus(esc_status);
_last_esc_status = esc_status;
_esc_status_received = true;
if (_param_escs_en.get()) {
updateEscsStatus(vehicle_status, esc_status);
}
}
if (_param_fd_act_en.get()) {
updateMotorStatus(vehicle_status, esc_status);
}
// Run motor status checks even when no new ESC data arrives (to detect timeouts)
if (_param_fd_act_en.get() && _esc_status_received) {
updateMotorStatus(vehicle_status, _last_esc_status);
}
if (_param_fd_imb_prop_thr.get() > 0) {
@ -188,11 +191,9 @@ 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_esc_failure, now);
_esc_failure_hysteresis.set_state_and_update(!is_all_escs_armed, now);
if (_esc_failure_hysteresis.get_state()) {
_failure_detector_status.flags.arm_escs = true;
}
_failure_detector_status.flags.arm_escs = _esc_failure_hysteresis.get_state();
} else {
// reset ESC bitfield
@ -266,74 +267,101 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status,
// Overvoltage, overcurrent do not have checks yet esc_report.failures are handled separately
const hrt_abstime now = hrt_absolute_time();
const bool is_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
// Only check while armed
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
actuator_motors_s actuator_motors{};
_actuator_motors_sub.copy(&actuator_motors);
// Clear the failure mask at the start --> Can recover when issue is resolved!!
_motor_failure_mask = 0;
// 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 - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) {
continue; // Invalid mapping
}
// Check telemetry timeout always, current checks only when armed
actuator_motors_s actuator_motors{};
_actuator_motors_sub.copy(&actuator_motors);
const bool timeout = now > esc_status.esc[i].timestamp + 300_ms;
const float current = esc_status.esc[i].esc_current;
// 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));
// First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.
if (current > FLT_EPSILON) {
_esc_has_reported_current[i] = true;
}
if (!_esc_has_reported_current[i]) {
continue;
}
_motor_failure_mask &= ~(1u << actuator_function_index); // Reset bit in mask to accumulate failures again
_motor_failure_mask |= (static_cast<uint16_t>(timeout) << actuator_function_index); // Telemetry timeout
// Current limits
float thrust = 0.f;
if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) {
// Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust
thrust = fabsf(actuator_motors.control[actuator_function_index]);
}
bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get();
bool current_too_low = current < (thrust * _param_fd_act_mot_c2t.get()) - _param_fd_act_low_off.get();
bool current_too_high = current > (thrust * _param_fd_act_mot_c2t.get()) + _param_fd_act_high_off.get();
_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()) {
// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
_esc_undercurrent_hysteresis[i].set_state_and_update(thrust_above_threshold && current_too_low && !timeout, now);
}
if (!_esc_overcurrent_hysteresis[i].get_state()) {
// Do not clear mid operation because a reaction could be to stop the motor and that would be conidered healthy again
_esc_overcurrent_hysteresis[i].set_state_and_update(current_too_high && !timeout, now);
}
_motor_failure_mask |= (static_cast<uint16_t>(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index);
_motor_failure_mask |= (static_cast<uint16_t>(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index);
// Skip if ESC is not actually connected
if (!mapped) {
continue;
}
_failure_detector_status.flags.motor = (_motor_failure_mask != 0u);
// 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;
} else { // Disarmed
if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) {
continue; // Invalid mapping
}
const bool timeout = now > esc_status.esc[i].timestamp + 300_ms;
const bool is_offline = (esc_status.esc_online_flags & (1 << i)) == 0;
const float current = esc_status.esc[i].esc_current;
// Set failure bits for this motor
if (timeout) {
_motor_failure_mask |= (1u << actuator_function_index);
}
if (is_offline) {
_motor_failure_mask |= (1u << actuator_function_index);
}
// Current checks only when armed
if (!is_armed) {
continue;
}
// First wait for ESC telemetry reporting non-zero current. Before that happens, don't check it.
if (current > FLT_EPSILON) {
_esc_has_reported_current[i] = true;
}
if (!_esc_has_reported_current[i]) {
continue;
}
// Current limits
float thrust = 0.f;
if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) {
// Normalized motor thrust commands before thrust model factor is applied, NAN means motor is turned off -> 0 thrust
thrust = fabsf(actuator_motors.control[actuator_function_index]);
}
bool thrust_above_threshold = thrust > _param_fd_act_mot_thr.get();
bool current_too_low = current < (thrust * _param_fd_act_mot_c2t.get()) - _param_fd_act_low_off.get();
bool current_too_high = current > (thrust * _param_fd_act_mot_c2t.get()) + _param_fd_act_high_off.get();
_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()) {
// 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()) {
// 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);
}
_motor_failure_mask |= (static_cast<uint16_t>(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index);
_motor_failure_mask |= (static_cast<uint16_t>(_esc_overcurrent_hysteresis[i].get_state()) << actuator_function_index);
}
// Clear current-related hysteresis when disarmed
if (!is_armed) {
for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
_esc_undercurrent_hysteresis[i].set_state_and_update(false, now);
_esc_overcurrent_hysteresis[i].set_state_and_update(false, now);
}
_failure_detector_status.flags.motor = false;
}
_failure_detector_status.flags.motor = (_motor_failure_mask != 0u);
}

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

@ -115,6 +115,8 @@ private:
systemlib::Hysteresis _esc_undercurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
systemlib::Hysteresis _esc_overcurrent_hysteresis[esc_status_s::CONNECTED_ESC_MAX];
uint16_t _motor_failure_mask = 0; // actuator function indexed
esc_status_s _last_esc_status{}; // Store last received ESC status
bool _esc_status_received{false}; // Track if we've ever received ESC data
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance