diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp index 7b74a1b869..b2eed03290 100644 --- a/src/modules/commander/failure_detector/FailureDetector.cpp +++ b/src/modules/commander/failure_detector/FailureDetector.cpp @@ -269,7 +269,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, // Only check while armed if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - actuator_motors_s actuator_motors{}; // Normalized motor thrust commands before thrust model factor is applied + actuator_motors_s actuator_motors{}; _actuator_motors_sub.copy(&actuator_motors); // Check individual ESC reports @@ -294,30 +294,36 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, continue; } - _motor_failure_mask &= ~(1u << actuator_function_index); // Reset bit in mask to be able to accumulate failures + _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 - // Current too low - if (_esc_has_reported_current[i]) { - float thrust = 0.f; + // Current limits + float thrust = 0.f; - if (PX4_ISFINITE(actuator_motors.control[actuator_function_index])) { - // 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(); - - _esc_undercurrent_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); - } - - _motor_failure_mask |= (static_cast(_esc_undercurrent_hysteresis[i].get_state()) << actuator_function_index); + 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(_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); @@ -325,6 +331,7 @@ void FailureDetector::updateMotorStatus(const vehicle_status_s &vehicle_status, } else { // Disarmed for (uint8_t i = 0; i < actuator_motors_s::NUM_CONTROLS; ++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; diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 26ae114abb..26182f4c3c 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -111,8 +111,9 @@ private: hrt_abstime _imu_status_timestamp_prev{0}; // Motor failure check - bool _esc_has_reported_current[esc_status_s::CONNECTED_ESC_MAX] {}; // true if some ESC had non-zero current (some don't support it) + 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)}; @@ -140,6 +141,8 @@ private: (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 e472c304b8..9324820472 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);