From 6166fc7c8120f223d4518cc860f96ddacf430580 Mon Sep 17 00:00:00 2001 From: Dado Mista Date: Tue, 25 Jun 2024 13:27:21 -0700 Subject: [PATCH] Factor out smooth ramping into function Signed-off-by: Dado Mista --- src/atr.c | 31 +------------------------------ src/torque_tilt.c | 31 +------------------------------ src/utils.c | 34 ++++++++++++++++++++++++++++++++++ src/utils.h | 9 +++++++++ 4 files changed, 45 insertions(+), 60 deletions(-) diff --git a/src/atr.c b/src/atr.c index ffca5a6..1240c4c 100644 --- a/src/atr.c +++ b/src/atr.c @@ -200,36 +200,7 @@ static void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *co } // Smoothen changes in tilt angle by ramping the step size - if (config->inputtilt_smoothing_factor > 0) { - float smoothing_factor = 0.05; - // Sets the angle away from Target that step size begins ramping down - float smooth_center_window = 1.5; - float tiltback_target_diff = atr->target_offset - atr->offset; - - // Within X degrees of Target Angle, start ramping down step size - if (fabsf(tiltback_target_diff) < smooth_center_window) { - // Target step size is reduced the closer to center you are (needed for smoothly - // transitioning away from center) - atr->ramped_step_size = (smoothing_factor * step_size * (tiltback_target_diff / 2)) + - ((1 - smoothing_factor) * atr->ramped_step_size); - // Linearly ramped down step size is provided as minimum to prevent overshoot - float centering_step_size = - fminf(fabsf(atr->ramped_step_size), fabsf(tiltback_target_diff / 2) * step_size) * - sign(tiltback_target_diff); - if (fabsf(tiltback_target_diff) < fabsf(centering_step_size)) { - atr->offset = atr->target_offset; - } else { - atr->offset += centering_step_size; - } - } else { - // Ramp up step size until the configured tilt speed is reached - atr->ramped_step_size = (smoothing_factor * step_size * sign(tiltback_target_diff)) + - ((1 - smoothing_factor) * atr->ramped_step_size); - atr->offset += atr->ramped_step_size; - } - } else { - rate_limitf(&atr->offset, atr->target_offset, step_size); - } + smooth_rampf(&atr->offset, &atr->ramped_step_size, atr->target_offset, step_size, 0.05, 1.5); } static void braketilt_update( diff --git a/src/torque_tilt.c b/src/torque_tilt.c index 59c52ea..af0acab 100644 --- a/src/torque_tilt.c +++ b/src/torque_tilt.c @@ -62,36 +62,7 @@ void torque_tilt_update(TorqueTilt *tt, const MotorData *motor, const RefloatCon } // Smoothen changes in tilt angle by ramping the step size - if (config->inputtilt_smoothing_factor > 0) { - float smoothing_factor = 0.04; - // Sets the angle away from Target that step size begins ramping down - float smooth_center_window = 1.5; - float tiltback_target_diff = target_offset - tt->offset; - - // Within X degrees of Target Angle, start ramping down step size - if (fabsf(tiltback_target_diff) < smooth_center_window) { - // Target step size is reduced the closer to center you are (needed for smoothly - // transitioning away from center) - tt->ramped_step_size = (smoothing_factor * step_size * (tiltback_target_diff / 2)) + - ((1 - smoothing_factor) * tt->ramped_step_size); - // Linearly ramped down step size is provided as minimum to prevent overshoot - float centering_step_size = - fminf(fabsf(tt->ramped_step_size), fabsf(tiltback_target_diff / 2) * step_size) * - sign(tiltback_target_diff); - if (fabsf(tiltback_target_diff) < fabsf(centering_step_size)) { - tt->offset = target_offset; - } else { - tt->offset += centering_step_size; - } - } else { - // Ramp up step size until the configured tilt speed is reached - tt->ramped_step_size = (smoothing_factor * step_size * sign(tiltback_target_diff)) + - ((1 - smoothing_factor) * tt->ramped_step_size); - tt->offset += tt->ramped_step_size; - } - } else { - rate_limitf(&tt->offset, target_offset, step_size); - } + smooth_rampf(&tt->offset, &tt->ramped_step_size, target_offset, step_size, 0.04, 1.5); } void torque_tilt_winddown(TorqueTilt *tt) { diff --git a/src/utils.c b/src/utils.c index 1537384..9cda310 100644 --- a/src/utils.c +++ b/src/utils.c @@ -33,6 +33,40 @@ void rate_limitf(float *value, float target, float step) { } } +// Smoothen changes in tilt angle by ramping the step size +// smooth_center_window: Sets the angle away from Target that step size begins ramping down +void smooth_rampf( + float *value, + float *ramped_step, + float target, + float step, + float smoothing_factor, + float smooth_center_window +) { + float tiltback_target_diff = target - *value; + + // Within X degrees of Target Angle, start ramping down step size + if (fabsf(tiltback_target_diff) < smooth_center_window) { + // Target step size is reduced the closer to center you are (needed for smoothly + // transitioning away from center) + *ramped_step = (smoothing_factor * step * (tiltback_target_diff / 2)) + + ((1 - smoothing_factor) * *ramped_step); + // Linearly ramped down step size is provided as minimum to prevent overshoot + float centering_step = fminf(fabsf(*ramped_step), fabsf(tiltback_target_diff / 2) * step) * + sign(tiltback_target_diff); + if (fabsf(tiltback_target_diff) < fabsf(centering_step)) { + *value = target; + } else { + *value += centering_step; + } + } else { + // Ramp up step size until the configured tilt speed is reached + *ramped_step = (smoothing_factor * step * sign(tiltback_target_diff)) + + ((1 - smoothing_factor) * *ramped_step); + *value += *ramped_step; + } +} + float clampf(float value, float min, float max) { const float m = value < min ? min : value; return m > max ? max : m; diff --git a/src/utils.h b/src/utils.h index 9bca854..1c795b4 100644 --- a/src/utils.h +++ b/src/utils.h @@ -118,3 +118,12 @@ float clampf(float value, float min, float max); * @param step A maximum unit of change of @p value. */ void rate_limitf(float *value, float target, float step); + +void smooth_rampf( + float *value, + float *ramped_step_size, + float target, + float step, + float smoothing_factor, + float smooth_center_window +);