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

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; }

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;
 	}

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