diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
index 9b35c80992f..7bc47dc8550 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
@@ -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
@@ -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
- *
- * This check can be configured via COM_ARM_CHK_ESCS parameter.
- *
- */
- reporter.healthFailure(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,
+ 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
+ *
+ * This check can be configured via COM_ARM_CHK_ESCS parameter.
+ *
+ */
+ reporter.healthFailure(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) {
if (esc_status.esc[index].failures != 0) {
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
index 7e32563e468..a96c147234e 100644
--- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
@@ -54,6 +54,6 @@ class EscChecks : public HealthAndArmingCheckBase
const hrt_abstime _start_time{hrt_absolute_time()};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
- (ParamBool) _param_escs_checks_required
+ (ParamBool) _param_com_arm_chk_escs
)
};
diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp
index 0c93f303a72..cbba4c68e0a 100644
--- a/src/modules/commander/failure_detector/FailureDetector.cpp
+++ b/src/modules/commander/failure_detector/FailureDetector.cpp
@@ -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);
@@ -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();
@@ -164,11 +164,9 @@ 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();
}
@@ -176,7 +174,7 @@ void FailureDetector::updateExternalAtsStatus()
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);
@@ -190,7 +188,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_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;
@@ -198,7 +196,7 @@ void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, c
} 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;
}
}
@@ -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;
+ 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(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(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index);
+ _motor_failure_mask |= (static_cast(_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;
}
}
diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp
index 78b4861a7af..26182f4c3cc 100644
--- a/src/modules/commander/failure_detector/FailureDetector.hpp
+++ b/src/modules/commander/failure_detector/FailureDetector.hpp
@@ -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)
+ 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
@@ -142,6 +141,8 @@ class FailureDetector : public ModuleParams
(ParamBool) _param_fd_act_en,
(ParamFloat) _param_fd_act_mot_thr,
(ParamFloat) _param_fd_act_mot_c2t,
- (ParamInt) _param_fd_act_mot_tout
+ (ParamInt) _param_fd_act_mot_tout,
+ (ParamFloat) _param_fd_act_low_off,
+ (ParamFloat) _param_fd_act_high_off
)
};
diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c
index e472c304b86..93248204720 100644
--- a/src/modules/commander/failure_detector/failure_detector_params.c
+++ b/src/modules/commander/failure_detector/failure_detector_params.c
@@ -169,12 +169,13 @@ PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
*
* @group Failure Detector
*/
-PARAM_DEFINE_INT32(FD_ACT_EN, 1);
+PARAM_DEFINE_INT32(FD_ACT_EN, 0);
/**
- * Motor Failure Throttle Threshold
+ * Motor Failure Thrust Threshold
*
- * Motor failure triggers only above this throttle value.
+ * Failure detection per motor only triggers above this thrust value.
+ * Set to 1 to disable the detection.
*
* @group Failure Detector
* @unit norm
@@ -186,9 +187,11 @@ PARAM_DEFINE_INT32(FD_ACT_EN, 1);
PARAM_DEFINE_FLOAT(FD_ACT_MOT_THR, 0.2f);
/**
- * Motor Failure Current/Throttle Threshold
+ * Motor Failure Current/Throttle Scale
*
- * Motor failure triggers only below this current value
+ * Determines the slope between expected steady state current and linearized, normalized thrust command.
+ * E.g. FD_ACT_MOT_C2T A represents the expected steady state current at 100%.
+ * FD_ACT_LOW_OFF and FD_ACT_HIGH_OFF offset the threshold from that slope.
*
* @group Failure Detector
* @min 0.0
@@ -197,13 +200,12 @@ PARAM_DEFINE_FLOAT(FD_ACT_MOT_THR, 0.2f);
* @decimal 2
* @increment 1
*/
-PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 2.0f);
+PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 35.f);
/**
- * Motor Failure Time Threshold
+ * Motor Failure Hysteresis Time
*
- * Motor failure triggers only if the throttle threshold and the
- * current to throttle threshold are violated for this time.
+ * Motor failure only triggers after current thresholds are exceeded for this time.
*
* @group Failure Detector
* @unit ms
@@ -211,4 +213,32 @@ PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 2.0f);
* @max 10000
* @increment 100
*/
-PARAM_DEFINE_INT32(FD_ACT_MOT_TOUT, 100);
+PARAM_DEFINE_INT32(FD_ACT_MOT_TOUT, 1000);
+
+/**
+ * Undercurrent motor failure limit offset
+ *
+ * threshold = FD_ACT_MOT_C2T * thrust - FD_ACT_LOW_OFF
+ *
+ * @group Failure Detector
+ * @min 0
+ * @max 30
+ * @unit A
+ * @decimal 2
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(FD_ACT_LOW_OFF, 10.f);
+
+/**
+ * Overcurrent motor failure limit offset
+ *
+ * threshold = FD_ACT_MOT_C2T * thrust + FD_ACT_HIGH_OFF
+ *
+ * @group Failure Detector
+ * @min 0
+ * @max 30
+ * @unit A
+ * @decimal 2
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(FD_ACT_HIGH_OFF, 10.f);