fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 09:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

FailureDetector: implement upper and lower current limit with offset

Browse Source
This commit is contained in:
Matthias Grob
2025-12-18 17:10:51 +01:00
parent f8329ff80d
commit 61a8ae80a6
3 changed files with 73 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/FailureDetector.cpp
+28 -21
View File
@@ -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<uint16_t>(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<uint16_t>(_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<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);
@@ -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;
src/modules/commander/failure_detector/FailureDetector.hpp
+5 -2
View File
@@ -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<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
)
};
src/modules/commander/failure_detector/failure_detector_params.c
+40 -10
View File
@@ -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);