From 3d4522d60683ed5a3ddb2873f8623ed13306d265 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Sat, 6 Jun 2015 10:42:26 +0200
Subject: accela: change gains formula, follow last commit change

New gains formula should reduce the need to use smoothing and deadzone
sliders. It's still very brittle code and may require further
adjustments.
---
 .../ftnoir_accela_filtercontrols.ui                |  4 +-
 ftnoir_filter_accela/ftnoir_filter_accela.cpp      | 56 ++++++++++++++++++----
 ftnoir_filter_accela/ftnoir_filter_accela.h        |  6 +--
 .../ftnoir_filter_accela_dialog.cpp                |  2 +-
 4 files changed, 51 insertions(+), 17 deletions(-)

diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
index 66c56b2e..c16fbe0a 100644
--- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
+++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
@@ -33,7 +33,7 @@
       <item row="1" column="3">
        <widget class="QSlider" name="rotation_slider">
         <property name="maximum">
-         <number>100</number>
+         <number>99</number>
         </property>
         <property name="pageStep">
          <number>5</number>
@@ -186,7 +186,7 @@ background:none;</string>
       <item row="5" column="3">
        <widget class="QSlider" name="translation_slider">
         <property name="maximum">
-         <number>100</number>
+         <number>99</number>
         </property>
         <property name="pageStep">
          <number>5</number>
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 25452219..9c44ce3b 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -16,13 +16,16 @@ FTNoIR_Filter::FTNoIR_Filter() : first_run(true)
 {
 }
 
-double FTNoIR_Filter::f(double vec, double thres)
+double FTNoIR_Filter::f(double val, const double gains[][2])
 {
-    if (vec > thres*high_thres_c)
-        return (vec - thres*high_thres_c) * high_thres_out + thres*high_thres_c;
-    if (vec > thres)
-        return (vec - thres) * low_thres_mult + thres;
-    return pow(vec, 2.0) / thres;
+    for (int i = 0; gains[i][0] >= 0; i++)
+    {
+        if (val >= gains[i][0])
+        {
+            return gains[i][1] * val;
+        }
+    }
+    return 0;
 }
 
 void FTNoIR_Filter::filter(const double* input, double *output)
@@ -39,8 +42,27 @@ void FTNoIR_Filter::filter(const double* input, double *output)
         t.start();
         return;
     }
+    
+    static const double rot_gains[][2] = {
+        { 6, 15 },
+        { 5, 6 },
+        { 4, 3 },
+        { 3, 1.3 },
+        { 2, .7 },
+        { 1, .4 },
+        { 0, .2 },
+        { -1, 0 }
+    };
+    static const double trans_gains[][2] = {
+        { 4, 8 },
+        { 3, 4 },
+        { 2, 2 },
+        { 1, .5 },
+        { 0, .1 },
+        { -1, 0 }
+    };
 
-    const double rot_t = 7. * (1+s.rot_threshold) / 100.;
+    const double rot_t = 10. * (1+s.rot_threshold) / 100.;
     const double trans_t = 5. * (1+s.trans_threshold) / 100.;
     
     const double dt = t.elapsed() * 1e-9;
@@ -60,13 +82,29 @@ void FTNoIR_Filter::filter(const double* input, double *output)
         const double vec = in - last_output[i];
         const double dz = i >= 3 ? rot_dz : trans_dz;
         const double vec_ = max(0., fabs(vec) - dz);
-        const double t = i >= 3 ? rot_t : trans_t;
-        const double val = f(vec_, t);
+        const double thres = i >= 3 ? rot_t : trans_t;
+        const double val = f(vec_ / thres, i >= 3 ? rot_gains : trans_gains) * thres;
+        static Timer tr;
+        static double m = 0, n = 0;
+        if (i == 3)
+        {
+            m = max(vec_ / thres, m);
+            n = max(n, val * dt);
+        }
+        if (tr.elapsed_ms() > 1000)
+        {
+            tr.start();
+            qDebug() << "3" << m << n;
+            m = 0;
+            n = 0;
+        }
         const double result = last_output[i] + (vec < 0 ? -1 : 1) * dt * val;
         const bool negp = vec < 0.;
         const bool done = negp
             ? result <= in
             : result >= in;
+        if (i == 3 && val > 0.1 && done)
+            qDebug() << "done";
         const double ret = done ? in : result;
         
         last_output[i] = output[i] = ret;
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h
index c1a19d96..aa10e161 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.h
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.h
@@ -32,12 +32,8 @@ private:
     double last_output[6];
     double smoothed_input[6];
     Timer t;
-    static double f(double vec, double thres);
     
-    static constexpr double high_thres_c = 4;
-    static constexpr double high_thres_out = 500;
-    
-    static constexpr double low_thres_mult = 100;
+    double f(double val, const double gains[][2]);
 };
 
 class FilterControls: public IFilterDialog
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
index 6bcc249b..550c9d24 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
@@ -56,7 +56,7 @@ void FilterControls::save() {
 
 void FilterControls::update_rot_display(int value)
 {
-    ui.rot_gain->setText(QString::number((value + 1) * 7 / 100.) + "°");
+    ui.rot_gain->setText(QString::number((value + 1) * 10 / 100.) + "°");
 }
 
 void FilterControls::update_trans_display(int value)
-- 
cgit v1.2.3