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;