Smoothened ATR ramping (like inputtilt ramping)

Following the example of inputtilt, ATR setpoints are now ramped
slower when near the current setpoint. This allows for much higher
ramping speeds without causing a jerking effect.

Instead of using 5deg/sec and 3deg/sec one can now easily run 20 and 12
or even more without any perceived downsides.

Feature: Smoothened/faster setpoint ramping for ATR

Signed-off-by: Dado Mista <dadomista@gmail.com>

Author: surfdado

src/atr.c

@@ -27,6 +27,7 @@ void atr_reset(ATR *atr) {
     atr->speed_boost = 0;
     atr->target = 0;
     atr->setpoint = 0;
+    atr->ramped_step_size = 0;
 
     smooth_target_reset(&atr->smooth_target, 0.0f);
     ema_filter_reset(&atr->ema_target, 0.0f, 0.0f);
@@ -203,9 +204,14 @@ void atr_update(ATR *atr, const MotorData *motor, const RefloatConfig *config, f
     } else if (config->target_filter.type == SFT_EMA3) {
         ema_filter_update(&atr->ema_target, atr->target, dt);
         atr->setpoint = atr->ema_target.value;
-    } else {
+    } else if (config->target_filter.type == SFT_THREE_STAGE) {
         smooth_target_update(&atr->smooth_target, atr->target);
         atr->setpoint = atr->smooth_target.value;
+    } else {
+        // Smoothen changes in tilt angle by ramping the step size
+        smooth_rampf(
+            &atr->setpoint, &atr->ramped_step_size, atr->target, atr_step_size, 0.05, 1.5
+        );
     }
 }
 

src/atr.h

@@ -26,6 +26,7 @@
 typedef struct {
     float on_step_size;
     float off_step_size;
+    float ramped_step_size;
 
     float accel_diff;
     float speed_boost;

src/conf/datatypes.h

@@ -182,7 +182,8 @@ typedef struct {
 typedef enum {
     SFT_NONE = 0,
     SFT_THREE_STAGE,
-    SFT_EMA3
+    SFT_EMA3,
+    SFT_NICO
 } TargetFilterType;
 
 typedef struct {

src/conf/settings.xml

@@ -248,12 +248,16 @@ p, li { white-space: pre-wrap; }
 <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p>
 <p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">It's defined by the EMA Half Time config option. You can use the EMA Return Multiplier to speed up transitions returning to zero (e.g. a value of 2 means the returning to zero transitions will be two times faster).</p>
 <p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p>
+<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-weight:600;">Nico's</span></p>
+<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">A smoothing filter written by Nico originally for Inputtilt, it was later proposed to be used tor ATR and Torquetilt as well. Although this filter works and is reported to work very well, it has a mathematical flaw, and hence is only included for reference. The other filters are, in fact, an attempt to have correct filter(s) that work the same or better.</p>
+<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-weight:600;"><br /></p>
 &lt;p style=&quot; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;&quot;&gt;&lt;span style=&quot; font-weight:600;&quot;&gt;Note: The 3-Stage and 3rd Order EMA ignore Tiltback Response Boost and Tiltback Transition Boost for ATR.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</description>
             <cDefine>CFG_DFLT_TARGET_FILTER_TYPE</cDefine>
             <valInt>1</valInt>
             <enumNames>None</enumNames>
             <enumNames>3-Stage</enumNames>
             <enumNames>3rd Order EMA</enumNames>
+            <enumNames>Nico's</enumNames>
         </target_filter.type>
         <target_filter.tt_type>
             <longName>Filter Type (config on ATR tab)</longName>
@@ -271,6 +275,7 @@ p, li { white-space: pre-wrap; }
             <enumNames>None</enumNames>
             <enumNames>3-Stage</enumNames>
             <enumNames>3rd Order EMA</enumNames>
+            <enumNames>Nico's</enumNames>
         </target_filter.tt_type>
         <target_filter.it_type>
             <longName>Filter Type (config on ATR tab)</longName>

src/torque_tilt.c

@@ -24,6 +24,7 @@
 
 void torque_tilt_reset(TorqueTilt *tt) {
     tt->setpoint = 0;
+    tt->ramped_step_size = 0;
 
     smooth_target_reset(&tt->smooth_target, 0.0f);
     ema_filter_reset(&tt->ema_target, 0.0f, 0.0f);
@@ -82,9 +83,12 @@ void torque_tilt_update(
     } else if (config->target_filter.tt_type == SFT_EMA3) {
         ema_filter_update(&tt->ema_target, target, dt);
         tt->setpoint = tt->ema_target.value;
-    } else {
+    } else if (config->target_filter.tt_type == SFT_THREE_STAGE) {
         smooth_target_update(&tt->smooth_target, target);
         tt->setpoint = tt->smooth_target.value;
+    } else {
+        // Smoothen changes in tilt angle by ramping the step size
+        smooth_rampf(&tt->setpoint, &tt->ramped_step_size, target, step_size, 0.04, 1.5);
     }
 }
 

src/torque_tilt.h

@@ -26,6 +26,7 @@
 typedef struct {
     float on_step_size;
     float off_step_size;
+    float ramped_step_size;
 
     float setpoint;
 

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

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