diff --git a/msg/FailureDetectorStatus.msg b/msg/FailureDetectorStatus.msg index 923ceb36da..5aa9cb18cb 100644 --- a/msg/FailureDetectorStatus.msg +++ b/msg/FailureDetectorStatus.msg @@ -9,6 +9,8 @@ bool fd_arm_escs bool fd_battery bool fd_imbalanced_prop bool fd_motor +bool fd_servo 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 servo_failure_mask # Bit-mask with servo indices, indicating critical servo failures diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 76bb1c9677..857b5c3aa6 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -98,15 +98,6 @@ public: */ virtual void allocate() = 0; - /** - * Set actuator failure flag - * This prevents a change of the scaling in the matrix normalization step - * in case of a motor failure. - * - * @param failure Motor failure flag - */ - void setHadActuatorFailure(bool failure) { _had_actuator_failure = failure; } - /** * Set the control effectiveness matrix * @@ -244,6 +235,5 @@ protected: matrix::Vector _control_trim; ///< Control at trim actuator values int _num_actuators{0}; bool _normalize_matrix_as_planar_mc{false}; ///< if true, normalize roll, pitch and yaw columns optimized for planar MC - bool _had_actuator_failure{false}; int _airmode{0}; ///< 0: disabled, 1: RP airmode, 2: RPY airmode }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 38c7b4d3d4..47a1e9ec7c 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -59,7 +59,7 @@ ControlAllocationPseudoInverse::updatePseudoInverse() if (_mix_update_needed) { matrix::geninv(_effectiveness, _mix); - if (_normalization_needs_update && !_had_actuator_failure) { + if (_normalization_needs_update) { updateControlAllocationMatrixScale(); _normalization_needs_update = false; } diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 84423a742b..7ebc8e5562 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -419,6 +419,14 @@ ControlAllocator::Run() fw_dthr_scale[0] = fw_dthr_scale[2]; } + for (int i = 0; i < 3; i++) { + if (_handled_servo_failure_bitmask && _param_ca_fw_dthr_fb_en.get() && !_has_control_authority[1][i]) { + // If fallback is enabled and it is discovered that an axis doesn't have any + // control authority, then the differential thrust weight for that axis is set to 1. + fw_dthr_weight[i] = 1.f; + } + } + // Set control setpoint vector(s) matrix::Vector c[ActuatorEffectiveness::MAX_NUM_MATRICES]; c[0](0) = _torque_sp(0); @@ -451,11 +459,8 @@ ControlAllocator::Run() c[1](5) = vehicle_thrust_setpoint.xyz[2]; } - const bool has_non_zero_dthr_weight = fw_dthr_weight[0] > FLT_EPSILON || fw_dthr_weight[1] > FLT_EPSILON - || fw_dthr_weight[2] > FLT_EPSILON; - // VTOL differential thrust in FW - if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT && has_non_zero_dthr_weight) { + if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT) { /* CA_FW_DTHR_SC_R etc are scales to tune response of differential thrust around each axis. The scaling factor is applied to the (normalized) torque setpoint from the rate controller going to the motors for rate control in fixed-wing flight using differential thrust instead of @@ -469,10 +474,16 @@ ControlAllocator::Run() c[0](1) = vehicle_torque_setpoint_matrix_1(1) * fw_dthr_scale[1] * fw_dthr_weight[1]; c[0](2) = vehicle_torque_setpoint_matrix_1(2) * fw_dthr_scale[2] * fw_dthr_weight[2]; - _actuator_effectiveness->setEnableAuxiliaryMotors(true); + const bool has_non_zero_dthr_weight = fw_dthr_weight[0] > FLT_EPSILON || fw_dthr_weight[1] > FLT_EPSILON + || fw_dthr_weight[2] > FLT_EPSILON; - } else if (_flight_phase == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT && !has_non_zero_dthr_weight) { - _actuator_effectiveness->setEnableAuxiliaryMotors(false); + if (has_non_zero_dthr_weight) { + _actuator_effectiveness->setEnableAuxiliaryMotors(true); // Currently only does something for Standard VTOL + + } else { + + _actuator_effectiveness->setEnableAuxiliaryMotors(false); + } } } @@ -612,6 +623,27 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso } } + // Handle failed servos + if (_handled_servo_failure_bitmask) { + actuator_idx = 0; + memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix)); + + for (int servos_idx = 0; servos_idx < _num_actuators[0] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { + const int selected_matrix = 1; // matrix 1 (only works for VTOL atm) + + if (_handled_servo_failure_bitmask & (1 << servos_idx)) { + ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix]; + + for (int i = 0; i < NUM_AXES; i++) { + matrix(i, actuator_idx_matrix[selected_matrix]) = 0.f; + } + } + + ++actuator_idx_matrix[selected_matrix]; + ++actuator_idx; + } + } + for (int i = 0; i < _num_control_allocation; ++i) { _control_allocation[i]->setActuatorMin(minimum[i]); _control_allocation[i]->setActuatorMax(maximum[i]); @@ -634,6 +666,10 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso if (all_entries_small) { matrix.row(n) = 0.f; + _has_control_authority[i][n] = false; + + } else { + _has_control_authority[i][n] = true; } } @@ -773,8 +809,10 @@ ControlAllocator::check_for_actuator_activation_update() failure_detector_status_s failure_detector_status; + const bool status_updated = _failure_detector_status_sub.update(&failure_detector_status); + if ((FailureMode)_param_ca_failure_mode.get() > FailureMode::IGNORE - && _failure_detector_status_sub.update(&failure_detector_status)) { + && status_updated) { if (failure_detector_status.fd_motor) { if (_handled_motor_failure_bitmask != failure_detector_status.motor_failure_mask) { @@ -789,10 +827,6 @@ ControlAllocator::check_for_actuator_activation_update() _handled_motor_failure_bitmask = failure_detector_status.motor_failure_mask; PX4_WARN("Removing motor from allocation (0x%x)", _handled_motor_failure_bitmask); activation_updated = true; - - for (int i = 0; i < _num_control_allocation; ++i) { - _control_allocation[i]->setHadActuatorFailure(true); - } } } break; @@ -808,10 +842,22 @@ ControlAllocator::check_for_actuator_activation_update() PX4_INFO("Restoring all motors"); _handled_motor_failure_bitmask = 0; - for (int i = 0; i < _num_control_allocation; ++i) { - _control_allocation[i]->setHadActuatorFailure(false); + activation_updated = true; + } + + if (failure_detector_status.fd_servo) { + if (_handled_servo_failure_bitmask != failure_detector_status.servo_failure_mask) { + // servo failure bitmask changed + _handled_servo_failure_bitmask = failure_detector_status.servo_failure_mask; + PX4_WARN("Removing servo nr. %d from allocation", _handled_servo_failure_bitmask); + activation_updated = true; } + } else if (_handled_servo_failure_bitmask != 0) { + // Clear bitmask completely + PX4_INFO("Restoring all servos"); + _handled_servo_failure_bitmask = 0; + activation_updated = true; } } diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index c5cb8bfeb9..4609f9943f 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -198,6 +198,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 _handled_servo_failure_bitmask{0}; uint16_t _handled_motor_disabled_bitmask{0}; @@ -213,6 +214,8 @@ private: bool _has_slew_rate{false}; ActuatorEffectiveness::FlightPhase _flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT}; + bool _has_control_authority[2][6] = {false}; + DEFINE_PARAMETERS( (ParamInt) _param_ca_airframe, (ParamInt) _param_ca_method, @@ -225,7 +228,8 @@ private: (ParamFloat) _param_ca_fw_dthr_wgt_p, (ParamFloat) _param_ca_fw_dthr_wgt_y, (ParamInt) _param_mc_airmode, - (ParamInt) _param_ca_fw_dthr_airmd + (ParamInt) _param_ca_fw_dthr_airmd, + (ParamBool) _param_ca_fw_dthr_fb_en ) }; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 3c50047909..5fdb22f75b 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -619,6 +619,20 @@ parameters: 1: Roll/Pitch 2: Roll/Pitch/Yaw default: 0 + + CA_FW_DTHR_FB_EN: + description: + short: Enable fallback to differential thrust for control in fixed-wing + long: | + Enable fallback to differential thrust for rate control around axes that have + 0 effectiveness in the servo matrix after the detected servo failure. + type: boolean + values: + 0: Disabled + 1: Enabled + default: 1 + + # Mixer mixer: actuator_types: