From ab0791b03dc83cde632a286bc30205af3c69bd66 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Luk=C3=A1=C5=A1=20Hr=C3=A1zk=C3=BD?= <lukkash@email.cz>
Date: Sun, 20 Oct 2024 12:04:01 +0200
Subject: [PATCH] Add out alpha smoothing

---
 src/conf/datatypes.h  |  1 +
 src/conf/settings.xml | 24 ++++++++++++++++++++++++
 src/smooth_target.c   | 20 ++++++++++++--------
 src/smooth_target.h   |  3 ++-
 4 files changed, 39 insertions(+), 9 deletions(-)

diff --git a/src/conf/datatypes.h b/src/conf/datatypes.h
index f770027..84a8885 100644
--- a/src/conf/datatypes.h
+++ b/src/conf/datatypes.h
@@ -182,6 +182,7 @@ typedef struct {
     float alpha;
     float in_alpha_away;
     float in_alpha_back;
+    float smooth_out_alpha;
 } CfgSmoothTarget;
 
 typedef struct {
diff --git a/src/conf/settings.xml b/src/conf/settings.xml
index 6a051c4..1d024cc 100644
--- a/src/conf/settings.xml
+++ b/src/conf/settings.xml
@@ -333,6 +333,28 @@ p, li { white-space: pre-wrap; }
             <suffix></suffix>
             <vTx>7</vTx>
         </setpoint_filter.in_alpha_back>
+        <setpoint_filter.smooth_out_alpha>
+            <longName>3-Stage Smooth Out Alpha</longName>
+            <type>1</type>
+            <transmittable>1</transmittable>
+            <description>&lt;!DOCTYPE HTML PUBLIC &quot;-//W3C//DTD HTML 4.0//EN&quot; &quot;http://www.w3.org/TR/REC-html40/strict.dtd&quot;&gt;
+&lt;html&gt;&lt;head&gt;&lt;meta name=&quot;qrichtext&quot; content=&quot;1&quot; /&gt;&lt;style type=&quot;text/css&quot;&gt;
+p, li { white-space: pre-wrap; }
+&lt;/style&gt;&lt;/head&gt;&lt;body style=&quot; font-family:'Roboto'; ; font-weight:400; font-style:normal;&quot;&gt;
+&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;Smoothing alpha for the 3-Stage filter. Higher numbers mean less filtering, recommended between 0.2 (minimal filtering) and 0.02 (heavy filtering).&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</description>
+            <cDefine>CFG_DFLT_SETPOINT_FILTER_SMOOTH_OUT_ALPHA</cDefine>
+            <editorDecimalsDouble>3</editorDecimalsDouble>
+            <editorScale>1</editorScale>
+            <editAsPercentage>0</editAsPercentage>
+            <maxDouble>1</maxDouble>
+            <minDouble>0</minDouble>
+            <showDisplay>0</showDisplay>
+            <stepDouble>0.01</stepDouble>
+            <valDouble>0.1</valDouble>
+            <vTxDoubleScale>10000</vTxDoubleScale>
+            <suffix></suffix>
+            <vTx>7</vTx>
+        </setpoint_filter.smooth_out_alpha>
         <hertz>
             <longName>Loop Hertz</longName>
             <type>2</type>
@@ -3716,6 +3738,7 @@ p, li { white-space: pre-wrap; }
         <ser>setpoint_filter.alpha</ser>
         <ser>setpoint_filter.in_alpha_away</ser>
         <ser>setpoint_filter.in_alpha_back</ser>
+        <ser>setpoint_filter.smooth_out_alpha</ser>
         <ser>hertz</ser>
         <ser>fault_pitch</ser>
         <ser>fault_roll</ser>
@@ -3885,6 +3908,7 @@ p, li { white-space: pre-wrap; }
                     <param>setpoint_filter.alpha</param>
                     <param>setpoint_filter.in_alpha_away</param>
                     <param>setpoint_filter.in_alpha_back</param>
+                    <param>setpoint_filter.smooth_out_alpha</param>
                     <param>::sep::Booster (Acceleration)</param>
                     <param>booster_angle</param>
                     <param>booster_ramp</param>
diff --git a/src/smooth_target.c b/src/smooth_target.c
index 883eb17..3180785 100644
--- a/src/smooth_target.c
+++ b/src/smooth_target.c
@@ -27,25 +27,27 @@ void smooth_target_configure(SmoothTarget *st, CfgSmoothTarget *cfg) {
 
 void smooth_target_reset(SmoothTarget *st, float value) {
     if (st->cfg.type == SFT_EMA2) {
-        st->target = value;
+        st->smooth_in = value;
     } else {
-        st->target = 0;
+        st->smooth_in = 0;
     }
     st->step = 0;
+    st->raw_out = value;
     st->value = value;
 }
 
 void smooth_target_update(SmoothTarget *st, float target) {
     if (st->cfg.type == SFT_EMA2) {
-        st->target += st->cfg.alpha * (target - st->target);
-        st->value += st->cfg.alpha * (st->target - st->value);
+        st->smooth_in += st->cfg.alpha * (target - st->smooth_in);
+        st->value += st->cfg.alpha * (st->smooth_in - st->value);
     } else {
-        st->target += st->cfg.smooth_alpha * (target - st->target);
+        st->smooth_in += st->cfg.smooth_alpha * (target - st->smooth_in);
 
-        float delta = st->cfg.alpha * (st->target - st->value);
+        float delta = st->cfg.alpha * (st->smooth_in - st->raw_out);
 
         if (fabsf(delta) > fabsf(st->step) || sign(delta) != sign(st->step)) {
-            if (fabsf(st->target) > fabsf(st->value)) {
+            // if (fabsf(st->target) > fabsf(st->raw_out)) {
+            if (sign(st->raw_out) == sign(delta)) {
                 st->step += st->cfg.in_alpha_away * (delta - st->step);
             } else {
                 st->step += st->cfg.in_alpha_back * (delta - st->step);
@@ -54,6 +56,8 @@ void smooth_target_update(SmoothTarget *st, float target) {
             st->step = delta;
         }
 
-        st->value += st->step;
+        st->raw_out += st->step;
+
+        st->value += st->cfg.smooth_out_alpha * (st->raw_out - st->value);
     }
 }
diff --git a/src/smooth_target.h b/src/smooth_target.h
index c072d93..8fe07c2 100644
--- a/src/smooth_target.h
+++ b/src/smooth_target.h
@@ -22,8 +22,9 @@
 typedef struct {
     CfgSmoothTarget cfg;
 
-    float target;
+    float smooth_in;
     float step;
+    float raw_out;
     float value;
 } SmoothTarget;