fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 01:43:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

failure_detector: add motor/ESC failure detection

Browse Source
This commit is contained in:
Alessandro Simovic
2022-05-12 07:58:56 +02:00
committed by Beat Küng
parent dfd934fbdb
commit fb71e7587c
25 changed files with 338 additions and 441 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/failure_detector/FailureDetector.cpp
+133 -12
View File
@@ -65,8 +65,17 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
_status.flags.ext = false;
}
if (_param_escs_en.get()) {
updateEscsStatus(vehicle_status);
// esc_status subscriber is shared between subroutines
esc_status_s esc_status;
if (_esc_status_sub.update(&esc_status)) {
if (_param_escs_en.get()) {
updateEscsStatus(vehicle_status, esc_status);
}
if (_param_fd_actuator_en.get()) {
updateMotorStatus(vehicle_status, esc_status);
}
}
if (_param_fd_imb_prop_thr.get() > 0) {
@@ -80,7 +89,7 @@ void FailureDetector::updateAttitudeStatus()
{
vehicle_attitude_s attitude;
if (_vehicule_attitude_sub.update(&attitude)) {
if (_vehicle_attitude_sub.update(&attitude)) {
const matrix::Eulerf euler(matrix::Quatf(attitude.q));
const float roll(euler.phi());
@@ -127,22 +136,26 @@ void FailureDetector::updateExternalAtsStatus()
}
}
void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status)
void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
{
hrt_abstime time_now = hrt_absolute_time();
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
esc_status_s esc_status;
const int limited_esc_count = math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX);
const int all_escs_armed_mask = (1 << limited_esc_count) - 1;
const bool is_all_escs_armed = (all_escs_armed_mask == esc_status.esc_armed_flags);
if (_esc_status_sub.update(&esc_status)) {
int all_escs_armed = (1 << esc_status.esc_count) - 1;
bool is_esc_failure = !is_all_escs_armed;
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(all_escs_armed != esc_status.esc_armed_flags, time_now);
for (int i = 0; i < limited_esc_count; i++) {
is_esc_failure = is_esc_failure | (esc_status.esc[i].failures > 0);
}
if (_esc_failure_hysteresis.get_state()) {
_status.flags.arm_escs = true;
}
_esc_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
_esc_failure_hysteresis.set_state_and_update(is_esc_failure, time_now);
if (_esc_failure_hysteresis.get_state()) {
_status.flags.arm_escs = true;
}
} else {
@@ -209,3 +222,111 @@ 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
// First wait for some ESC telemetry that has the required fields. Before that happens, don't check this ESC
// Then check
const hrt_abstime time_now = hrt_absolute_time();
// Only check while armed
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);
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];
// 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;
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);
}
// Check for telemetry timeout
const hrt_abstime telemetry_age = time_now - cur_esc_report.timestamp;
const bool esc_timed_out = telemetry_age > 100_ms; // TODO: magic number
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);
} else if (!esc_timed_out && esc_timeout_currently_flagged) {
// Reset flag
_motor_failure_esc_timed_out_mask &= ~(1 << i_esc);
}
// Check if ESC current is too low
float esc_throttle = 0.f;
if (PX4_ISFINITE(actuator_motors.control[i_esc])) {
esc_throttle = fabsf(actuator_motors.control[i_esc]);
}
const bool throttle_above_threshold = esc_throttle > _param_fd_motor_throttle_thres.get();
const bool current_too_low = cur_esc_report.esc_current < esc_throttle *
_param_fd_motor_current2throttle_thres.get();
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] = time_now;
}
} else {
if (_motor_failure_undercurrent_start_time[i_esc] != 0) {
_motor_failure_undercurrent_start_time[i_esc] = 0;
}
}
if (_motor_failure_undercurrent_start_time[i_esc] != 0
&& (time_now - _motor_failure_undercurrent_start_time[i_esc]) > _param_fd_motor_time_thres.get() * 1_ms
&& (_motor_failure_esc_under_current_mask & (1 << i_esc)) == 0) {
// Set flag
_motor_failure_esc_under_current_mask |= (1 << i_esc);
} // else: this flag is never cleared, as the motor is stopped, so throttle < threshold
}
bool critical_esc_failure = (_motor_failure_esc_timed_out_mask != 0 || _motor_failure_esc_under_current_mask != 0);
if (critical_esc_failure && !(_status.flags.motor)) {
// Add motor failure flag to bitfield
_status.flags.motor = true;
} else if (!critical_esc_failure && _status.flags.motor) {
// Reset motor failure flag
_status.flags.motor = false;
}
} 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;
}
_motor_failure_esc_under_current_mask = 0;
_status.flags.motor = false;
}
}
src/modules/commander/failure_detector/FailureDetector.hpp
+22 -4
View File
@@ -51,6 +51,7 @@
// subscriptions
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -69,6 +70,7 @@ union failure_detector_status_u {
uint16_t arm_escs : 1;
uint16_t battery : 1;
uint16_t imbalanced_prop : 1;
uint16_t motor : 1;
} flags;
uint16_t value {0};
};
@@ -79,16 +81,19 @@ class FailureDetector : public ModuleParams
{
public:
FailureDetector(ModuleParams *parent);
~FailureDetector() = default;
bool update(const vehicle_status_s &vehicle_status, const vehicle_control_mode_s &vehicle_control_mode);
const failure_detector_status_u &getStatus() const { return _status; }
const decltype(failure_detector_status_u::flags) &getStatusFlags() const { return _status.flags; }
float getImbalancedPropMetric() const { return _imbalanced_prop_lpf.getState(); }
uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; }
private:
void updateAttitudeStatus();
void updateExternalAtsStatus();
void updateEscsStatus(const vehicle_status_s &vehicle_status);
void updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
void updateImbalancedPropStatus();
failure_detector_status_u _status{};
@@ -103,11 +108,18 @@ private:
uint32_t _selected_accel_device_id{0};
hrt_abstime _imu_status_timestamp_prev{0};
uORB::Subscription _vehicule_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
// 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
hrt_abstime _motor_failure_undercurrent_start_time[actuator_motors_s::NUM_CONTROLS] {};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)}; // TODO: multi-instance
uORB::Subscription _pwm_input_sub{ORB_ID(pwm_input)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::FD_FAIL_P>) _param_fd_fail_p,
@@ -117,6 +129,12 @@ private:
(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr
(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
// Actuator failure
(ParamBool<px4::params::FD_ACT_EN>) _param_fd_actuator_en,
(ParamFloat<px4::params::FD_ACT_MOT_THR>) _param_fd_motor_throttle_thres,
(ParamFloat<px4::params::FD_ACT_MOT_C2T>) _param_fd_motor_current2throttle_thres,
(ParamInt<px4::params::FD_ACT_MOT_TOUT>) _param_fd_motor_time_thres
)
};
src/modules/commander/failure_detector/failure_detector_params.c
+56
View File
@@ -156,3 +156,59 @@ PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_IMB_PROP_THR, 30);
/**
* Enable Actuator Failure check
*
* If enabled, failure detector will verify that for motors, a minimum amount of ESC current per throttle
* level is being consumed.
* Otherwise this indicates an motor failure.
*
* @boolean
* @reboot_required true
*
* @group Failure Detector
*/
PARAM_DEFINE_INT32(FD_ACT_EN, 1);
/**
* Motor Failure Throttle Threshold
*
* Motor failure triggers only above this throttle value.
*
* @group Failure Detector
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FD_ACT_MOT_THR, 0.2f);
/**
* Motor Failure Current/Throttle Threshold
*
* Motor failure triggers only below this current value
*
* @group Failure Detector
* @min 0.0
* @max 50.0
* @unit A/%
* @decimal 2
* @increment 1
*/
PARAM_DEFINE_FLOAT(FD_ACT_MOT_C2T, 2.0f);
/**
* Motor Failure Time Threshold
*
* Motor failure triggers only if the throttle threshold and the
* current to throttle threshold are violated for this time.
*
* @group Failure Detector
* @unit ms
* @min 10
* @max 10000
* @increment 100
*/
PARAM_DEFINE_INT32(FD_ACT_MOT_TOUT, 100);