diff --git a/msg/ControlAllocatorStatus.msg b/msg/ControlAllocatorStatus.msg index 2d7b088322..3eed13b27d 100644 --- a/msg/ControlAllocatorStatus.msg +++ b/msg/ControlAllocatorStatus.msg @@ -19,3 +19,4 @@ int8[16] actuator_saturation # Indicates actuator saturation status. # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value. uint16 handled_motor_failure_mask # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector +uint16 motor_stop_mask # Bitmaks of motors stopped by failure injection diff --git a/msg/FailureDetectorStatus.msg b/msg/FailureDetectorStatus.msg index 923ceb36da..092848932e 100644 --- a/msg/FailureDetectorStatus.msg +++ b/msg/FailureDetectorStatus.msg @@ -12,3 +12,4 @@ bool fd_motor float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed) uint16 motor_failure_mask # Bit-mask with motor indices, indicating critical motor failures +uint16 motor_stop_mask # Bitmaks of motors stopped by failure injection diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 606445ebb6..663223c9bf 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1956,6 +1956,7 @@ void Commander::run() fd_status.fd_motor = _failure_detector.getStatusFlags().motor; fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric(); fd_status.motor_failure_mask = _failure_detector.getMotorFailures(); + fd_status.motor_stop_mask = _failure_detector.getMotorStopMask(); fd_status.timestamp = hrt_absolute_time(); _failure_detector_status_pub.publish(fd_status); } diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp index 67b70f1bf5..d84581ca5e 100644 --- a/src/modules/commander/failure_detector/FailureDetector.hpp +++ b/src/modules/commander/failure_detector/FailureDetector.hpp @@ -89,6 +89,7 @@ public: 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; } + uint16_t getMotorStopMask() { return _failure_injector.getMotorStopMask(); } private: void updateAttitudeStatus(const vehicle_status_s &vehicle_status); diff --git a/src/modules/commander/failure_detector/FailureInjector.cpp b/src/modules/commander/failure_detector/FailureInjector.cpp index 38bccdf5d2..e1633500c3 100644 --- a/src/modules/commander/failure_detector/FailureInjector.cpp +++ b/src/modules/commander/failure_detector/FailureInjector.cpp @@ -33,10 +33,23 @@ #include "FailureInjector.hpp" +#include #include +FailureInjector::FailureInjector() +{ + int32_t param_sys_failure_en = 0; + + if ((param_get(param_find("SYS_FAILURE_EN"), ¶m_sys_failure_en) == PX4_OK) + && (param_sys_failure_en == 1)) { + _failure_injection_enabled = true; + } +} + void FailureInjector::update() { + if (!_failure_injection_enabled) { return; } + vehicle_command_s vehicle_command; while (_vehicle_command_sub.update(&vehicle_command)) { @@ -59,14 +72,21 @@ void FailureInjector::update() switch (failure_type) { case vehicle_command_s::FAILURE_TYPE_OK: supported = true; - PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i); + PX4_INFO("CMD_INJECT_FAILURE, motor %d ok", i + 1); + _motor_stop_mask &= ~(1 << i); _esc_telemetry_blocked_mask &= ~(1 << i); _esc_telemetry_wrong_mask &= ~(1 << i); break; case vehicle_command_s::FAILURE_TYPE_OFF: supported = true; - PX4_INFO("CMD_INJECT_FAILURE, motor %d no esc telemetry", i); + PX4_INFO("CMD_INJECT_FAILURE, motor %d off", i + 1); + _motor_stop_mask |= 1 << i; + break; + + case vehicle_command_s::FAILURE_TYPE_STUCK: + supported = true; + PX4_INFO("CMD_INJECT_FAILURE, motor %d no esc telemetry", i + 1); _esc_telemetry_blocked_mask |= 1 << i; break; @@ -91,6 +111,8 @@ void FailureInjector::update() void FailureInjector::manipulateEscStatus(esc_status_s &status) { + if (!_failure_injection_enabled) { return; } + if (_esc_telemetry_blocked_mask != 0 || _esc_telemetry_wrong_mask != 0) { for (int i = 0; i < status.esc_count; i++) { const unsigned i_esc = status.esc[i].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1; diff --git a/src/modules/commander/failure_detector/FailureInjector.hpp b/src/modules/commander/failure_detector/FailureInjector.hpp index b74897e551..86f2199723 100644 --- a/src/modules/commander/failure_detector/FailureInjector.hpp +++ b/src/modules/commander/failure_detector/FailureInjector.hpp @@ -42,13 +42,18 @@ class FailureInjector { public: + FailureInjector(); + void update(); void manipulateEscStatus(esc_status_s &status); + uint32_t getMotorStopMask() { return _motor_stop_mask; } private: uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + bool _failure_injection_enabled = false; + uint32_t _motor_stop_mask{}; uint32_t _esc_telemetry_blocked_mask{}; uint32_t _esc_telemetry_wrong_mask{}; }; diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index e0dc14bcbe..f70eb1d8df 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -641,6 +641,7 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) // Handled motor failures control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask; + control_allocator_status.motor_stop_mask = _motor_stop_mask; _control_allocator_status_pub[matrix_index].publish(control_allocator_status); } @@ -665,7 +666,9 @@ ControlAllocator::publish_actuator_controls() int actuator_idx = 0; int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; - uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() | _handled_motor_failure_bitmask; + uint32_t stopped_motors = _actuator_effectiveness->getStoppedMotors() + | _handled_motor_failure_bitmask + | _motor_stop_mask; // motors int motors_idx; @@ -716,8 +719,13 @@ ControlAllocator::check_for_motor_failures() if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE && _failure_detector_status_sub.update(&failure_detector_status)) { - if (failure_detector_status.fd_motor) { + if (_motor_stop_mask != failure_detector_status.motor_stop_mask) { + _motor_stop_mask = failure_detector_status.motor_stop_mask; + PX4_WARN("Stopping motors (%d)", _motor_stop_mask); + } + + if (failure_detector_status.fd_motor) { if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) { // motor failure bitmask changed switch ((FailureMode)_param_ca_failure_mode.get()) { diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 14c30026d1..376171ea23 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -201,6 +201,7 @@ private: // Reflects motor failures that are currently handled, not motor failures that are reported. // For example, the system might report two motor failures, but only the first one is handled by CA uint16_t _handled_motor_failure_bitmask{0}; + uint16_t _motor_stop_mask{0}; perf_counter_t _loop_perf; /**< loop duration performance counter */