From 0f256718d33f555f2e8abff423a931c8e3c19875 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 9 Feb 2023 18:02:23 +0100 Subject: [PATCH] mixer_module: correct pwm ramp Before: When the mixed throttle for the motor was exactly zero the ramp went from the disarmed PWM value to the minimum PWM value. When the throttle was even just slightly different from zero the ramp made a jump up to the commanded throttle scaled between disarmed PWM and maximum PWM, then ramped between disarmed PWM and minimum PWM and at the end jumped up again to the commanded throttle scaled between minimum PWM and maximum PWM. After: The ramp goes from disarmed PWM value to the the commanded throttle scaled between minimum PWM and maximum PWM. If the commanded throttle changes during the ramp then the scale and hence also end value of the ramp changes. --- src/lib/mixer_module/mixer_module.cpp | 49 ++++----------------------- 1 file changed, 6 insertions(+), 43 deletions(-) diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 29a07ba690..7fe50d8e44 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -598,53 +598,16 @@ MixingOutput::output_limit_calc(const bool armed, const int num_channels, const case OutputLimitState::RAMP: { hrt_abstime diff = hrt_elapsed_time(&_output_time_armed); + float progress = static_cast(diff) / RAMP_TIME_US; - static constexpr int PROGRESS_INT_SCALING = 10000; - int progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; - - if (progress > PROGRESS_INT_SCALING) { - progress = PROGRESS_INT_SCALING; + if (progress > 1.f) { + progress = 1.f; } for (int i = 0; i < num_channels; i++) { - - float control_value = output[i]; - - /* check for invalid / disabled channels */ - if (!PX4_ISFINITE(control_value)) { - _current_output_value[i] = _disarmed_value[i]; - continue; - } - - uint16_t ramp_min_output; - - /* if a disarmed output value was set, blend between disarmed and min */ - if (_disarmed_value[i] > 0) { - - /* safeguard against overflows */ - auto disarmed = _disarmed_value[i]; - - if (disarmed > _min_value[i]) { - disarmed = _min_value[i]; - } - - int disarmed_min_diff = _min_value[i] - disarmed; - ramp_min_output = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; - - } else { - /* no disarmed output value set, choose min output */ - ramp_min_output = _min_value[i]; - } - - if (_reverse_output_mask & (1 << i)) { - control_value = -1.f * control_value; - } - - _current_output_value[i] = control_value * (_max_value[i] - ramp_min_output) / 2 + (_max_value[i] + ramp_min_output) / - 2; - - /* last line of defense against invalid inputs */ - _current_output_value[i] = math::constrain(_current_output_value[i], ramp_min_output, _max_value[i]); + // Ramp from disarmed value to currently desired output that would apply without ramp + uint16_t desired_output = output_limit_calc_single(i, output[i]); + _current_output_value[i] = _disarmed_value[i] + progress * (desired_output - _disarmed_value[i]); } } break;