Skip to content

Improvement iteration for under- and new overcurrent motor failure detection #26262

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
MaEtUgR merged 5 commits into from
Feb 9, 2026
Merged

Improvement iteration for under- and new overcurrent motor failure detection #26262

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b8d7afa
escCheck: rework online check to properly report offline ESCs
MaEtUgR Dec 16, 2025
583e240
FailureDetector: consistent timestamp naming
MaEtUgR Dec 15, 2025
1661185
FailureDetector: rework motor status check
MaEtUgR Dec 16, 2025
34fb985
FailureDetector: implement upper and lower current limit with offset
MaEtUgR Dec 18, 2025
612a160
Update src/modules/commander/failure_detector/FailureDetector.cpp
ttechnick Feb 9, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 23 additions & 26 deletions src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
checkEscStatus(context, reporter, esc_status);
reporter.setIsPresent(health_component_t::motors_escs);

} else if (_param_escs_checks_required.get()
} else if (_param_com_arm_chk_escs.get()
&& now - _start_time > 5_s) { // Wait a bit after startup to allow esc's to init

/* EVENT
Expand All @@ -102,40 +102,37 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)

void EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
{
const NavModes required_modes = _param_escs_checks_required.get() ? NavModes::All : NavModes::None;
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';

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">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(required_modes, health_component_t::motors_escs, events::ID("check_escs_offline"),
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';
}
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,
Copy link
Copy Markdown
Contributor

@dakejahl dakejahl Jan 14, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
const bool mapped = math::isInRange(esc_status.esc[i].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
const bool is_motor = math::isInRange(esc_status.esc[i].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,

Copy link
Copy Markdown
Member Author

@MaEtUgR MaEtUgR Jan 15, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is mapped as motor? I'll check 🤔 👍

Copy link
Copy Markdown
Member Author

@MaEtUgR MaEtUgR Feb 9, 2026 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think you look at the check and see it checks for being mapped to a motor and I looked at what's interesting, is it mapped at all. Both are valid. I think we should combine tha names in the next iteration, I want to unblock @ttechnick 's work.

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;

if (mapped && offline) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </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);
esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
}
}

if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
}
if ((esc_fail_msg[0] != '\0') && 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.esc_count; index++) {
for (int index = 0; index < math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX); ++index) {
Copy link
Copy Markdown
Contributor

@dakejahl dakejahl Jan 14, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
for (int index = 0; index < math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX); ++index) {
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; ++index) {

you should still iterate over esc_status_s::CONNECTED_ESC_MAX. Technically you could configure Motor1 and Motor2 on actuator channels 1&2 and Motor3 and Motor4 on channels 5&6. The EscStatus.esc[8] message is in actuator order.

Copy link
Copy Markdown
Member

@ttechnick ttechnick Feb 9, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can we keep it the proposed way, as it is more conservative and change it to your suggested way in the next iteration?

Copy link
Copy Markdown
Member Author

@MaEtUgR MaEtUgR Feb 9, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We need to document more clearly how the array and esc_count works in the esc_status message. This implementation is based on the assumption the count gives the number of valid array entries from the beginning. I thought that's currently also the case but we need to check the details.

Copy link
Copy Markdown
Contributor

@dakejahl dakejahl Feb 9, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

EscStatus.esc_count is the total number of valid entries but they are not necessarily sequential. The EscReport[8] esc is in actuator order not motor order, hence the EscReport.actuator_function


if (esc_status.esc[index].failures != 0) {

Expand Down
2 changes: 1 addition & 1 deletion src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,6 @@ class EscChecks : public HealthAndArmingCheckBase
const hrt_abstime _start_time{hrt_absolute_time()};

DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs
)
};
135 changes: 51 additions & 84 deletions src/modules/commander/failure_detector/FailureDetector.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ void FailureDetector::publishStatus()
failure_detector_status.fd_imbalanced_prop = _failure_detector_status.flags.imbalanced_prop;
failure_detector_status.fd_motor = _failure_detector_status.flags.motor;
failure_detector_status.imbalanced_prop_metric = _imbalanced_prop_lpf.getState();
failure_detector_status.motor_failure_mask = _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask;
failure_detector_status.motor_failure_mask = _motor_failure_mask;
failure_detector_status.motor_stop_mask = _failure_injector.getMotorStopMask();
failure_detector_status.timestamp = hrt_absolute_time();
_failure_detector_status_pub.publish(failure_detector_status);
Expand Down Expand Up @@ -141,13 +141,13 @@ void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_statu
const bool roll_status = (max_roll > FLT_EPSILON) && (fabsf(roll) > max_roll);
const bool pitch_status = (max_pitch > FLT_EPSILON) && (fabsf(pitch) > max_pitch);

hrt_abstime time_now = hrt_absolute_time();
hrt_abstime now = hrt_absolute_time();

// Update hysteresis
_roll_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_r_ttri.get()));
_pitch_failure_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(1_s * _param_fd_fail_p_ttri.get()));
_roll_failure_hysteresis.set_state_and_update(roll_status, time_now);
_pitch_failure_hysteresis.set_state_and_update(pitch_status, time_now);
_roll_failure_hysteresis.set_state_and_update(roll_status, now);
_pitch_failure_hysteresis.set_state_and_update(pitch_status, now);

// Update status
_failure_detector_status.flags.roll = _roll_failure_hysteresis.get_state();
Expand All @@ -164,19 +164,17 @@ void FailureDetector::updateExternalAtsStatus()
uint32_t pulse_width = pwm_input.pulse_width;
bool ats_trigger_status = (pulse_width >= (uint32_t)_param_fd_ext_ats_trig.get()) && (pulse_width < 3_ms);

hrt_abstime time_now = hrt_absolute_time();

// Update hysteresis
_ext_ats_failure_hysteresis.set_hysteresis_time_from(false, 100_ms); // 5 consecutive pulses at 50hz
_ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, time_now);
_ext_ats_failure_hysteresis.set_state_and_update(ats_trigger_status, hrt_absolute_time());

_failure_detector_status.flags.ext = _ext_ats_failure_hysteresis.get_state();
}
}

void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
{
hrt_abstime time_now = hrt_absolute_time();
hrt_abstime now = hrt_absolute_time();

if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
Expand All @@ -190,15 +188,15 @@ 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, time_now);
_esc_failure_hysteresis.set_state_and_update(is_esc_failure, now);

if (_esc_failure_hysteresis.get_state()) {
_failure_detector_status.flags.arm_escs = true;
}

} else {
// reset ESC bitfield
_esc_failure_hysteresis.set_state_and_update(false, time_now);
_esc_failure_hysteresis.set_state_and_update(false, now);
_failure_detector_status.flags.arm_escs = false;
}
}
Expand Down Expand Up @@ -263,110 +261,79 @@ void FailureDetector::updateImbalancedPropStatus()

void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
{
// What need to be checked:
//
// 1. ESC telemetry disappears completely -> dead ESC or power loss on that ESC
// 2. ESC failures like overvoltage, overcurrent etc. But DShot driver for example is not populating the field 'esc_report.failures'
// 3. Motor current too low. Compare drawn motor current to expected value from a parameter
// -- ESC voltage does not really make sense and is highly dependent on the setup
// 1. Telemetry times out -> communication or power lost on that ESC
// 2. Too low current draw compared to commanded thrust
// Overvoltage, overcurrent do not have checks yet esc_report.failures are handled separately

// First wait for some ESC telemetry that has the required fields. Before that happens, don't check this ESC
// Then check
const hrt_abstime now = hrt_absolute_time();

// Only check while armed
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
const hrt_abstime now = hrt_absolute_time();
const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);

actuator_motors_s actuator_motors{};
_actuator_motors_sub.copy(&actuator_motors);

// Check individual ESC reports
for (int esc_status_idx = 0; esc_status_idx < limited_esc_count; esc_status_idx++) {

const esc_report_s &cur_esc_report = esc_status.esc[esc_status_idx];

for (uint8_t i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
// Map the esc status index to the actuator function index
const unsigned i_esc = cur_esc_report.actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;
const uint8_t actuator_function_index =
esc_status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1;

if (i_esc >= actuator_motors_s::NUM_CONTROLS) {
continue;
}

// Check if ESC telemetry was available and valid at some point. This is a prerequisite for the failure detection.
if (!(_motor_failure_esc_valid_current_mask & (1 << i_esc)) && cur_esc_report.esc_current > 0.0f) {
_motor_failure_esc_valid_current_mask |= (1 << i_esc);
if (actuator_function_index >= actuator_motors_s::NUM_CONTROLS) {
continue; // Invalid mapping
}

// Check for telemetry timeout
const bool esc_timed_out = now > cur_esc_report.timestamp + 300_ms;
const bool esc_was_valid = _motor_failure_esc_valid_current_mask & (1 << i_esc);
const bool esc_timeout_currently_flagged = _motor_failure_esc_timed_out_mask & (1 << i_esc);

if (esc_was_valid && esc_timed_out && !esc_timeout_currently_flagged) {
// Set flag
_motor_failure_esc_timed_out_mask |= (1 << i_esc);
const bool timeout = now > esc_status.esc[i].timestamp + 300_ms;
Comment thread
ttechnick marked this conversation as resolved.
const float current = esc_status.esc[i].esc_current;

} else if (!esc_timed_out && esc_timeout_currently_flagged) {
// Reset flag
_motor_failure_esc_timed_out_mask &= ~(1 << i_esc);
// 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;
}

// Check if ESC current is too low
if (cur_esc_report.esc_current > FLT_EPSILON) {
_motor_failure_esc_has_current[i_esc] = true;
if (!_esc_has_reported_current[i]) {
continue;
}

if (_motor_failure_esc_has_current[i_esc]) {
float esc_throttle = 0.f;

if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
esc_throttle = fabsf(actuator_motors.control[i_esc]);
}
_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

const bool throttle_above_threshold = esc_throttle > _param_fd_act_mot_thr.get();
const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
_param_fd_act_mot_c2t.get();
// Current limits
float thrust = 0.f;

if (throttle_above_threshold && current_too_low && !esc_timed_out) {
if (_motor_failure_undercurrent_start_time[i_esc] == 0) {
_motor_failure_undercurrent_start_time[i_esc] = now;
}
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]);
}

} else {
if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
_motor_failure_undercurrent_start_time[i_esc] = 0;
}
}
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();

if (_motor_failure_undercurrent_start_time[i_esc] != 0
&& now > (_motor_failure_undercurrent_start_time[i_esc] + (_param_fd_act_mot_tout.get() * 1_ms))
&& (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
// Set flag
_motor_failure_esc_under_current_mask |= (1 << i_esc);
_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);

} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
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);
}
}

bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);

if (critical_esc_failure && !(_failure_detector_status.flags.motor)) {
// Add motor failure flag to bitfield
_failure_detector_status.flags.motor = true;
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);
}

} else if (!critical_esc_failure && _failure_detector_status.flags.motor) {
// Reset motor failure flag
_failure_detector_status.flags.motor = false;
_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);
}

_failure_detector_status.flags.motor = (_motor_failure_mask != 0u);

} else { // Disarmed
// reset ESC bitfield
for (int i_esc = 0; i_esc < actuator_motors_s::NUM_CONTROLS; i_esc++) {
_motor_failure_undercurrent_start_time[i_esc] = 0;
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);
}

_motor_failure_esc_under_current_mask = 0;
_failure_detector_status.flags.motor = false;
}
}
13 changes: 7 additions & 6 deletions src/modules/commander/failure_detector/FailureDetector.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,11 +111,10 @@ class FailureDetector : public ModuleParams
hrt_abstime _imu_status_timestamp_prev{0};

// Motor failure check
uint8_t _motor_failure_esc_valid_current_mask{}; // ESC 1-8, true if ESC telemetry was valid at some point
uint8_t _motor_failure_esc_timed_out_mask{}; // ESC telemetry no longer available -> failure
uint8_t _motor_failure_esc_under_current_mask{}; // ESC drawing too little current -> failure
bool _motor_failure_esc_has_current[actuator_motors_s::NUM_CONTROLS] {false}; // true if some ESC had non-zero current (some don't support it)
hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};
bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; // true if ESC reported non-zero current before (some never report any)
Comment thread
MaEtUgR marked this conversation as resolved.
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

uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance
Expand All @@ -142,6 +141,8 @@ class FailureDetector : public ModuleParams
(ParamBool<px4::params::FD_ACT_EN>) _param_fd_act_en,
(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_act_mot_thr,
(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_act_mot_c2t,
(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_act_mot_tout
(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_act_mot_tout,
(ParamFloat<px4::params::FD_ACT_LOW_OFF>) _param_fd_act_low_off,
(ParamFloat<px4::params::FD_ACT_HIGH_OFF>) _param_fd_act_high_off
)
};
Loading
Loading