From af3cfaea25e5b904f0fbafeb3ec895c3f7a0d6d0 Mon Sep 17 00:00:00 2001 From: Balduin Date: Fri, 6 Mar 2026 11:22:44 +0100 Subject: [PATCH] fix(control_allocator): Apply stopped motors before slew So transitions between stopped/not stopped respect the slew rate. - Remove previous stopped motor handling in publish_actuator_controls - Replace with handle_stopped_motors, called in Run() before slew - Introduce ApplyNanToActuators in ControlAllocation to stop - Refactor ice shedding slightly to fit new structure --- .../control_allocation/ControlAllocation.hpp | 18 +++++++ .../control_allocator/ControlAllocator.cpp | 54 +++++++++++-------- .../control_allocator/ControlAllocator.hpp | 4 +- 3 files changed, 54 insertions(+), 22 deletions(-) diff --git a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp index a106120026..b8d647f834 100644 --- a/src/lib/control_allocation/control_allocation/ControlAllocation.hpp +++ b/src/lib/control_allocation/control_allocation/ControlAllocation.hpp @@ -222,6 +222,24 @@ public: ActuatorVector normalizeActuatorSetpoint(const ActuatorVector &actuator) const; + /** + * Apply a mask of actuators to be set to NaN. + * + * A NaN value in _actuator_sp represents a disabled or stopped actuator. + * This mask is typically used to stop motors in specific flight phases or when certain thrust components are NaN. + * + * @param nan_actuators_mask Bitmask indicating which actuators to set to NaN. + * If (nan_actuators_mask & (1 << i)), _actuator_sp(i) becomes NaN. + */ + void applyNanToActuators(uint32_t nan_actuators_mask) + { + for (int i = 0; i < _num_actuators; i++) { + if (nan_actuators_mask & (1u << i)) { + _actuator_sp(i) = NAN; + } + } + } + virtual void updateParameters() {} int numConfiguredActuators() const { return _num_actuators; } diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 4d962a835e..26ad512c56 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -437,6 +437,10 @@ ControlAllocator::Run() _actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp, _control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax()); + if (i == 0) { + handle_stopped_motors(now); + } + if (_has_slew_rate) { _control_allocation[i]->applySlewRateLimit(dt); } @@ -596,6 +600,33 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso } } + +void +ControlAllocator::handle_stopped_motors(const hrt_abstime now) +{ + const uint32_t stopped_motors_due_to_effectiveness = _actuator_effectiveness->getStoppedMotors(); + + const uint32_t stopped_motors = stopped_motors_due_to_effectiveness + | _handled_motor_failure_bitmask + | _motor_stop_mask; + + // Handle stopped motors by setting NaN + const unsigned int allocation_index = 0; + _control_allocation[allocation_index]->applyNanToActuators(stopped_motors); + + // Apply ice shedding, which applies _only_ to stopped motors + const bool any_stopped_motor_failed = 0 != (stopped_motors_due_to_effectiveness & (_handled_motor_failure_bitmask | _motor_stop_mask)); + const float ice_shedding_output = get_ice_shedding_output(now); + + if (ice_shedding_output > FLT_EPSILON && !any_stopped_motor_failed) { + for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) { + if (stopped_motors & 1u << motors_idx) { + _control_allocation[allocation_index]->_actuator_sp(motors_idx) = ice_shedding_output; + } + } + } +} + void ControlAllocator::publish_control_allocator_status(int matrix_index) { @@ -650,7 +681,7 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) } float -ControlAllocator::get_ice_shedding_output(hrt_abstime now, bool any_stopped_motor_failed) +ControlAllocator::get_ice_shedding_output(hrt_abstime now) { const float period_sec = _param_ice_shedding_period.get(); @@ -660,7 +691,7 @@ ControlAllocator::get_ice_shedding_output(hrt_abstime now, bool any_stopped_moto // If any stopped motor has failed, the feature will create much more // torque than in the nominal case, and becomes pointless anyway as we // cannot go back to multicopter - const bool apply_shedding = _is_vtol && in_forward_flight && !any_stopped_motor_failed; + const bool apply_shedding = _is_vtol && in_forward_flight; if (feature_disabled_by_param || !apply_shedding) { return 0.0f; @@ -694,16 +725,6 @@ ControlAllocator::publish_actuator_controls() int actuator_idx = 0; int actuator_idx_matrix[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; - const uint32_t stopped_motors_due_to_effectiveness = _actuator_effectiveness->getStoppedMotors(); - - const uint32_t stopped_motors = stopped_motors_due_to_effectiveness - | _handled_motor_failure_bitmask - | _motor_stop_mask; - - const bool any_stopped_motor_failed = 0 != (stopped_motors_due_to_effectiveness & (_handled_motor_failure_bitmask | _motor_stop_mask)); - - const float ice_shedding_output = get_ice_shedding_output(actuator_motors.timestamp, any_stopped_motor_failed); - // motors int motors_idx; @@ -711,15 +732,6 @@ ControlAllocator::publish_actuator_controls() int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; - - if (stopped_motors & (1u << motors_idx)) { - actuator_motors.control[motors_idx] = NAN; - - if (ice_shedding_output > FLT_EPSILON) { - actuator_motors.control[motors_idx] = ice_shedding_output; - } - } - ++actuator_idx_matrix[selected_matrix]; ++actuator_idx; } diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index f31a888021..4ed6e5f0d0 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -145,7 +145,9 @@ private: void publish_actuator_controls(); - float get_ice_shedding_output(hrt_abstime now, bool any_stopped_motor_failed); + void handle_stopped_motors(const hrt_abstime now); + + float get_ice_shedding_output(hrt_abstime now); AllocationMethod _allocation_method_id{AllocationMethod::NONE}; ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; ///< class for control allocation calculations