diff --git a/docs/en/SUMMARY.md b/docs/en/SUMMARY.md index afd55db247..cafc85b353 100644 --- a/docs/en/SUMMARY.md +++ b/docs/en/SUMMARY.md @@ -93,6 +93,7 @@ - [Back-transition Tuning](config_vtol/vtol_back_transition_tuning.md) - [VTOL w/o Airspeed Sensor](config_vtol/vtol_without_airspeed_sensor.md) - [VTOL Weather Vane](config_vtol/vtol_weathervane.md) + - [VTOL Ice Shedding](config_vtol/vtol_ice_shedding.md) - [Flight Modes](flight_modes_vtol/index.md) - [Mission Mode (VTOL)](flight_modes_vtol/mission.md) - [Return Mode (VTOL)](flight_modes_vtol/return.md) diff --git a/docs/en/config_vtol/index.md b/docs/en/config_vtol/index.md index b819080b95..38ee15f2f9 100644 --- a/docs/en/config_vtol/index.md +++ b/docs/en/config_vtol/index.md @@ -9,3 +9,4 @@ Then perform VTOL-specific configuration and tuning: - [Back-transition Tuning](../config_vtol/vtol_back_transition_tuning.md) - [VTOL w/o Airspeed Sensor](../config_vtol/vtol_without_airspeed_sensor.md) - [VTOL Weather Vane](../config_vtol/vtol_weathervane.md) +- [Ice Shedding](../config_vtol/vtol_ice_shedding.md) diff --git a/docs/en/config_vtol/vtol_ice_shedding.md b/docs/en/config_vtol/vtol_ice_shedding.md new file mode 100644 index 0000000000..629a599f93 --- /dev/null +++ b/docs/en/config_vtol/vtol_ice_shedding.md @@ -0,0 +1,20 @@ +# VTOL Ice Shedding feature + +## Overview + +Ice shedding is a feature that periodically spins unused motors in fixed-wing +flight, to break off any ice that is starting to build up in the motors while it +is still feasible to do so. + +It is configured by the paramter `CA_ICE_PERIOD`. When it is 0, the feature is +disabled, when it is above 0, it sets the duration of the ice shedding cycle in +seconds. In each cycle, the rotors are spun for two seconds at a motor output of +0.01. + +:::warning +When enabling the feature on a new airframe, there is the risk of producing +torques that disturb the fixed-wing rate controller. To mitigate this risk: + - Set your `PWM_MIN` values correctly, so that the motor output 0.01 actually + produces 1% thrust + - Be prepared to take control and switch back to multicopter +::: diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 28baf752ee..17385a117c 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/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(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(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]; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 376171ea23..02fe81529f 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/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 _slew_limited_ice_shedding_output; + hrt_abstime _last_ice_shedding_update{}; + DEFINE_PARAMETERS( (ParamInt) _param_ca_airframe, (ParamInt) _param_ca_method, (ParamInt) _param_ca_failure_mode, - (ParamInt) _param_r_rev + (ParamInt) _param_r_rev, + (ParamFloat) _param_ice_shedding_period ) }; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 23405466a6..1790484347 100644 --- a/src/modules/control_allocator/module.yaml +++ b/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: