diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 4125dc8b36..e7791e033c 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -649,19 +649,19 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) float ControlAllocator::get_ice_shedding_output(hrt_abstime now) { + const float period_sec = _param_ice_shedding_period.get(); + const float on_sec = _param_ice_shedding_on_time.get(); + const float max_ice_shedding_output = _param_ice_shedding_output.get(); - // Feature config - const bool spin_to_shed_enabled = true; // TODO replace by param - - const float period_sec = 10.0f; - const float on_sec = 5.0f; - const float slew_sec = 1.0f; // Time to reach full shed_ice_thrust - const float max_ice_shedding_output = 0.1f; + if (period_sec <= FLT_EPSILON || on_sec <= FLT_EPSILON || max_ice_shedding_output <= FLT_EPSILON) { + // The user has not configured the feature to be turned on + return 0.0f; + } const bool has_unused_upwards_rotors = _effectiveness_source_id == EffectivenessSource::STANDARD_VTOL || _effectiveness_source_id == EffectivenessSource::TILTROTOR_VTOL; const bool in_forward_flight = _actuator_effectiveness->getFlightPhase() == ActuatorEffectiveness::FlightPhase::FORWARD_FLIGHT; - const bool apply_shedding = has_unused_upwards_rotors && spin_to_shed_enabled && in_forward_flight; + const bool apply_shedding = has_unused_upwards_rotors && in_forward_flight; if (!apply_shedding) { return 0.0f; @@ -671,6 +671,16 @@ ControlAllocator::get_ice_shedding_output(hrt_abstime now) float current_ice_shedding_output = 0.0f; + const float slew_sec = 1.0f; // Time to reach full shed_ice_thrust + + // TOOD ensure this works (fails predictably...) when on_sec < 2 * slew_sec + // TODO if period_sec > on_sec, always output thrust? still fade it in? + // TODO when the params are changed in flight we essentially enter the + // cycle in a completely random phase, bypassing the slew limit. + // - find out if this is actually dangerous + // - if so, put some offset that is updated on param changes to stay in the same phase + // - or redo the entire implementation with a state machine rather than this pure functional version + if (elapsed_in_period < on_sec) { // We are in the "active" window, calculate ramping diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index a412759cda..480cfabd46 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -220,7 +220,10 @@ private: (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, + (ParamFloat) _param_ice_shedding_on_time, + (ParamFloat) _param_ice_shedding_output ) }; diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 23405466a6..8adf5448a7 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -605,6 +605,53 @@ parameters: 1: Remove first failed motor from effectiveness default: 0 + CA_ICE_PERIOD: + description: + short: Ice shedding cycle period + long: | + Ice shedding is a feature that periodically spins unused + upwards rotors on VTOL aircraft to remove ice buildup within + the motors. This period defines the total duration of the + ice shedding cycle. Feature is turned off if <= 0. + type: float + decimal: 1 + unit: s + increment: 0.1 + min: 0.0 + max: 300.0 + default: 60.0 + + CA_ICE_ON_TIME: + description: + short: Ice shedding active duration + long: | + This parameter defines the active duration within each ice + shedding cycle during which rotors will spin. Feature is + turned off if <= 0. + type: float + decimal: 1 + unit: s + increment: 0.1 + min: 0.0 + max: 300.0 + default: 0.0 + + CA_ICE_OUTPUT: + description: + short: Maximum motor output for ice shedding + long: | + This parameter determines the maximum motor output commanded + during ice shedding. Warning: If this output is too high, + the feature disturbs with fixed wing rate control, + especially on tiltrotorp. Start with very low values, only + increase in small steps. Feature is turned off if <= 0. + type: float + decimal: 2 + increment: 0.01 + min: 0.0 + max: 0.2 + default: 0.0 + # Mixer mixer: actuator_types: