Factor out smooth ramping into function

Signed-off-by: Dado Mista <[email protected]>

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(

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) {

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;

src/utils.h

@@ -97,3 +97,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
+);