From c231d6a2ba35281f57024a195e50db6640a6413d Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 29 Apr 2017 16:18:01 +0200
Subject: filter/accela: make ewma working in 3-axis mode

---
 filter-accela/accela-settings.hpp             |  2 +-
 filter-accela/ftnoir_accela_filtercontrols.ui |  4 ++--
 filter-accela/ftnoir_filter_accela.cpp        | 21 +++++++++++----------
 filter-accela/ftnoir_filter_accela.h          |  2 +-
 filter-accela/ftnoir_filter_accela_dialog.cpp |  4 ++--
 5 files changed, 17 insertions(+), 16 deletions(-)

diff --git a/filter-accela/accela-settings.hpp b/filter-accela/accela-settings.hpp
index 6b264f2e..ce116d64 100644
--- a/filter-accela/accela-settings.hpp
+++ b/filter-accela/accela-settings.hpp
@@ -51,7 +51,7 @@ struct settings_accela : opts
         pos_sensitivity(b, "translation-sensitivity", slider_value(1., .05, 1.5)),
         rot_deadzone(b, "rotation-deadzone", slider_value(.03, 0, .1)),
         pos_deadzone(b, "translation-deadzone", slider_value(.1, 0, 1)),
-        ewma(b, "ewma", slider_value(0, 0, 15)),
+        ewma(b, "ewma", slider_value(0, 0, 50)),
         rot_nonlinearity(b, "rotation-nonlinearity", slider_value(1.2, 1, 1.3))
     {}
 };
diff --git a/filter-accela/ftnoir_accela_filtercontrols.ui b/filter-accela/ftnoir_accela_filtercontrols.ui
index 621d32f7..a07df9cc 100644
--- a/filter-accela/ftnoir_accela_filtercontrols.ui
+++ b/filter-accela/ftnoir_accela_filtercontrols.ui
@@ -80,10 +80,10 @@
          <number>0</number>
         </property>
         <property name="maximum">
-         <number>15</number>
+         <number>10</number>
         </property>
         <property name="pageStep">
-         <number>3</number>
+         <number>5</number>
         </property>
         <property name="orientation">
          <enum>Qt::Horizontal</enum>
diff --git a/filter-accela/ftnoir_filter_accela.cpp b/filter-accela/ftnoir_filter_accela.cpp
index 1761dc45..0f4d9dfe 100644
--- a/filter-accela/ftnoir_filter_accela.cpp
+++ b/filter-accela/ftnoir_filter_accela.cpp
@@ -31,17 +31,20 @@ static inline constexpr T signum(T x)
 }
 
 template<int N = 3, typename F>
-static void do_deltas(const double* deltas, double* output, F&& fun)
+static void do_deltas(const double* deltas, double* output, double alpha, double& smoothed, F&& fun)
 {
     double norm[N];
 
-    const double dist = progn(
+    const double dist_ = progn(
         double ret = 0;
         for (unsigned k = 0; k < N; k++)
             ret += deltas[k]*deltas[k];
         return sqrt(ret);
     );
 
+    const double dist = alpha*dist_ + (1-alpha)*smoothed;
+    smoothed = dist;
+
     const double value = double(fun(dist));
 
     for (unsigned k = 0; k < N; k++)
@@ -82,9 +85,10 @@ void accela::filter(const double* input, double *output)
             const double f = input[i];
             output[i] = f;
             last_output[i] = f;
-            smoothed_input[i] = f;
         }
         first_run = false;
+        smoothed_input[0] = 0;
+        smoothed_input[1] = 0;
         t.start();
         return;
     }
@@ -101,14 +105,11 @@ void accela::filter(const double* input, double *output)
     const double pos_dz = s.pos_deadzone.to<double>();
     const double nl = s.rot_nonlinearity.to<double>();
 
-    for (unsigned i = 0; i < 6; i++)
-        smoothed_input[i] = smoothed_input[i] * (1-alpha) + input[i] * alpha;
-
     // rot
 
     for (unsigned i = 3; i < 6; i++)
     {
-        double d = smoothed_input[i] - last_output[i];
+        double d = input[i] - last_output[i];
 
         if (fabs(d) > rot_dz)
             d -= copysign(rot_dz, d);
@@ -129,13 +130,13 @@ void accela::filter(const double* input, double *output)
         }
     }
 
-    do_deltas(&deltas[Yaw], &output[Yaw], [this](double x) { return spline_rot.get_value_no_save(x); });
+    do_deltas(&deltas[Yaw], &output[Yaw], alpha, smoothed_input[0], [this](double x) { return spline_rot.get_value_no_save(x); });
 
     // pos
 
     for (unsigned i = 0; i < 3; i++)
     {
-        double d = smoothed_input[i] - last_output[i];
+        double d = input[i] - last_output[i];
         if (fabs(d) > pos_dz)
             d -= copysign(pos_dz, d);
         else
@@ -144,7 +145,7 @@ void accela::filter(const double* input, double *output)
         deltas[i] = d / pos_thres;
     }
 
-    do_deltas(&deltas[TX], &output[TX], [this](double x) { return spline_pos.get_value_no_save(x); });
+    do_deltas(&deltas[TX], &output[TX], alpha, smoothed_input[1], [this](double x) { return spline_pos.get_value_no_save(x); });
 
     // end
 
diff --git a/filter-accela/ftnoir_filter_accela.h b/filter-accela/ftnoir_filter_accela.h
index 1c27cf40..bbe74b08 100644
--- a/filter-accela/ftnoir_filter_accela.h
+++ b/filter-accela/ftnoir_filter_accela.h
@@ -27,7 +27,7 @@ private:
     settings_accela s;
     bool first_run;
     double last_output[6], deltas[6];
-    double smoothed_input[6];
+    double smoothed_input[2];
     Timer t;
 };
 
diff --git a/filter-accela/ftnoir_filter_accela_dialog.cpp b/filter-accela/ftnoir_filter_accela_dialog.cpp
index cd836aca..3efc81d0 100644
--- a/filter-accela/ftnoir_filter_accela_dialog.cpp
+++ b/filter-accela/ftnoir_filter_accela_dialog.cpp
@@ -27,10 +27,10 @@ dialog_accela::dialog_accela()
     tie_setting(s.pos_deadzone, ui.trans_dz_slider);
     tie_setting(s.rot_nonlinearity, ui.rot_nl_slider);
 
-    tie_setting(s.rot_sensitivity, ui.rot_gain, tr("%1°"), 0, 'g', 4);
+    tie_setting(s.rot_sensitivity, ui.rot_gain, tr(u8"%1°"), 0, 'g', 4);
     tie_setting(s.pos_sensitivity, ui.trans_gain, tr("%1mm"));
     tie_setting(s.ewma, ui.ewma_label, tr("%1ms"));
-    tie_setting(s.rot_deadzone, ui.rot_dz, tr("%1°"), 0, 'g', 4);
+    tie_setting(s.rot_deadzone, ui.rot_dz, tr(u8"%1°"), 0, 'g', 4);
     tie_setting(s.pos_deadzone, ui.trans_dz, tr("%1mm"));
     tie_setting(s.rot_nonlinearity, ui.rot_nl,
         tr("<html><head/><body>"
-- 
cgit v1.2.3