ControlAllocator: Periodically spin MC motors to shed ice (#26322)

This feature periodically spins the unused motors on VTOL airframes, to
break off ice that has built up in the motor while it is still feasible
to do so. Each ice shedding event sends a motor ouput of 0.01 to the
unused motors for 2 seconds.

Configured by `CA_ICE_PERIOD`, the duration of the entire cycle. Set to
0 to disable.

src/modules/control_allocator/ControlAllocator.cpp

@@ -74,6 +74,8 @@ ControlAllocator::ControlAllocator() :
 	}
 
 	parameters_updated();
+
+	_slew_limited_ice_shedding_output.setSlewRate(ICE_SHEDDING_MAX_SLEWRATE);
 }
 
 ControlAllocator::~ControlAllocator()
@@ -339,6 +341,7 @@ ControlAllocator::Run()
 		if (_vehicle_status_sub.update(&vehicle_status)) {
 
 			_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+			_is_vtol = vehicle_status.is_vtol;
 
 			ActuatorEffectiveness::FlightPhase flight_phase{ActuatorEffectiveness::FlightPhase::HOVER_FLIGHT};
 
@@ -646,6 +649,39 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)
 	_control_allocator_status_pub[matrix_index].publish(control_allocator_status);
 }
 
+float
+ControlAllocator::get_ice_shedding_output(hrt_abstime now, bool any_stopped_motor_failed)
+{
+	const float period_sec = _param_ice_shedding_period.get();
+
+	const bool feature_disabled_by_param = period_sec <= FLT_EPSILON;
+	const bool in_forward_flight = _actuator_effectiveness->getFlightPhase() == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT;
+
+	// 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;
+
+	if (feature_disabled_by_param || !apply_shedding) {
+		// Bypass slew limit and immediately set zero, to not
+		// interfere with backtransition in any way
+		_slew_limited_ice_shedding_output.setForcedValue(0.0f);
+
+	} else {
+		// Raw square wave output
+		const float elapsed_in_period = fmodf(static_cast<float>(now) / 1_s, period_sec);
+		const float raw_ice_shedding_output = elapsed_in_period < ICE_SHEDDING_ON_SEC ? ICE_SHEDDING_OUTPUT : 0.0f;
+
+		// Apply slew rate limit
+		const float dt = static_cast<float>(now - _last_ice_shedding_update) / 1_s;
+		_slew_limited_ice_shedding_output.update(raw_ice_shedding_output, dt);
+	}
+
+	_last_ice_shedding_update = now;
+
+	return _slew_limited_ice_shedding_output.getState();
+}
+
 void
 ControlAllocator::publish_actuator_controls()
 {
@@ -666,9 +702,15 @@ 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
-				  | _motor_stop_mask;
+	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;
@@ -680,6 +722,10 @@ ControlAllocator::publish_actuator_controls()
 
 		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];

src/modules/control_allocator/ControlAllocator.hpp

@@ -89,6 +89,11 @@ public:
 	static constexpr int MAX_NUM_MOTORS = actuator_motors_s::NUM_CONTROLS;
 	static constexpr int MAX_NUM_SERVOS = actuator_servos_s::NUM_CONTROLS;
 
+	static constexpr float ICE_SHEDDING_MAX_SLEWRATE = 0.1f;
+	static constexpr float ICE_SHEDDING_ON_SEC = 2.0f;
+	static constexpr float ICE_SHEDDING_OUTPUT = 0.01f;
+
+
 	using ActuatorVector = ActuatorEffectiveness::ActuatorVector;
 
 	ControlAllocator();
@@ -139,6 +144,8 @@ private:
 
 	void publish_actuator_controls();
 
+	float get_ice_shedding_output(hrt_abstime now, bool any_stopped_motor_failed);
+
 	AllocationMethod _allocation_method_id{AllocationMethod::NONE};
 	ControlAllocation *_control_allocation[ActuatorEffectiveness::MAX_NUM_MATRICES] {}; 	///< class for control allocation calculations
 	int _num_control_allocation{0};
@@ -206,6 +213,7 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
 
 	bool _armed{false};
+	bool _is_vtol{false};
 	hrt_abstime _last_run{0};
 	hrt_abstime _timestamp_sample{0};
 	hrt_abstime _last_status_pub{0};
@@ -214,11 +222,16 @@ private:
 	Params _params{};
 	bool _has_slew_rate{false};
 
+
+	SlewRate<float> _slew_limited_ice_shedding_output;
+	hrt_abstime _last_ice_shedding_update{};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe,
 		(ParamInt<px4::params::CA_METHOD>) _param_ca_method,
 		(ParamInt<px4::params::CA_FAILURE_MODE>) _param_ca_failure_mode,
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev,
+		(ParamFloat<px4::params::CA_ICE_PERIOD>) _param_ice_shedding_period
 	)
 
 };

src/modules/control_allocator/module.yaml

@@ -605,6 +605,22 @@ parameters:
                 1: Remove first failed motor from effectiveness
             default: 0
 
+        CA_ICE_PERIOD:
+            description:
+                short: Ice shedding cycle period
+                long: |
+                    Ice shedding prevents ice buildup in VTOL aircraft motors by
+                    periodically spinning inactive rotors. When enabled (period
+                    > 0), every cycle lasts for the defined period and includes
+                    a 2‑second spin at 0.01 motor output. If period <= 0, the
+                    feature is disabled.
+            type: float
+            decimal: 1
+            unit: s
+            increment: 0.1
+            min: 0.0
+            default: 0.0
+
 # Mixer
 mixer:
     actuator_types: