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

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

Browse Source 
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.
This commit is contained in:
Balduin
2026-01-27 11:15:24 +01:00
committed by GitHub
parent 50bb31491b
commit 0676647d8a
6 changed files with 101 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+49 -3
View File
@@ -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];