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

Enable directly injecting motor failures using e.g. failure motor off -i 1

Browse Source 
Only if SYS_FAILURE_EN is enabled and CA_FAILURE_MODE is > 0.
This commit is contained in:
Matthias Grob
2025-08-28 16:14:51 +02:00
parent 786e0a12c2
commit e59afce5db
8 changed files with 44 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+10 -2
View File
@@ -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()) {
src/modules/control_allocator/ControlAllocator.hpp
+1
View File
@@ -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 */