filter/accela: add rotation nonlinearity switch

This only applies to very beginning of gain spline.

4 files changed, 155 insertions, 81 deletions

diff --git a/filter-accela/ftnoir_accela_filtercontrols.ui b/filter-accela/ftnoir_accela_filtercontrols.ui
index be14f842..86ca57b1 100644
--- a/filter-accela/ftnoir_accela_filtercontrols.ui
+++ b/filter-accela/ftnoir_accela_filtercontrols.ui
@@ -9,8 +9,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>657</width>
-    <height>358</height>
+    <width>558</width>
+    <height>411</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -24,6 +24,64 @@
    <property name="topMargin">
     <number>5</number>
    </property>
+   <item row="4" column="0">
+    <widget class="QLabel" name="label_9">
+     <property name="sizePolicy">
+      <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
+       <horstretch>0</horstretch>
+       <verstretch>0</verstretch>
+      </sizepolicy>
+     </property>
+     <property name="font">
+      <font>
+       <pointsize>7</pointsize>
+      </font>
+     </property>
+     <property name="frameShape">
+      <enum>QFrame::NoFrame</enum>
+     </property>
+     <property name="text">
+      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;right&quot;&gt;&lt;br/&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;Accela by &lt;/span&gt;&lt;a href=&quot;https://github.com/sthalik&quot;&gt;&lt;span style=&quot; font-size:10pt; text-decoration: underline; color:#0057ae;&quot;&gt;Stanisław Halik&lt;/span&gt;&lt;/a&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;&lt;br/&gt;Thanks to &lt;/span&gt;&lt;a href=&quot;https://github.com/dbaarda&quot;&gt;&lt;span style=&quot; font-size:10pt; text-decoration: underline; color:#0057ae;&quot;&gt;Donovan Baarda&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p align=&quot;right&quot;&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;2012-2015&lt;/span&gt;&lt;/p&gt;&lt;p align=&quot;right&quot;&gt;&lt;br/&gt;&lt;span style=&quot; font-size:8pt;&quot;&gt;Visit &lt;/span&gt;&lt;a href=&quot;https://github.com/opentrack/opentrack/wiki/Accela-in-opentrack-2.3&quot;&gt;&lt;span style=&quot; font-size:8pt; text-decoration: underline; color:#0000ff;&quot;&gt;our wiki&lt;/span&gt;&lt;/a&gt;&lt;span style=&quot; font-size:8pt;&quot;&gt; for description of the settings.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+     </property>
+     <property name="textFormat">
+      <enum>Qt::RichText</enum>
+     </property>
+     <property name="scaledContents">
+      <bool>false</bool>
+     </property>
+     <property name="alignment">
+      <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+     </property>
+     <property name="wordWrap">
+      <bool>true</bool>
+     </property>
+     <property name="margin">
+      <number>0</number>
+     </property>
+     <property name="indent">
+      <number>0</number>
+     </property>
+     <property name="openExternalLinks">
+      <bool>true</bool>
+     </property>
+     <property name="textInteractionFlags">
+      <set>Qt::LinksAccessibleByKeyboard|Qt::LinksAccessibleByMouse|Qt::TextBrowserInteraction|Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse</set>
+     </property>
+    </widget>
+   </item>
+   <item row="5" column="0">
+    <widget class="QDialogButtonBox" name="buttonBox">
+     <property name="sizePolicy">
+      <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
+       <horstretch>0</horstretch>
+       <verstretch>0</verstretch>
+      </sizepolicy>
+     </property>
+     <property name="standardButtons">
+      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+     </property>
+    </widget>
+   </item>
    <item row="0" column="0">
     <widget class="QGroupBox" name="groupBox66">
      <property name="title">
@@ -280,61 +338,47 @@
     </widget>
    </item>
    <item row="3" column="0">
-    <widget class="QLabel" name="label_9">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Minimum" vsizetype="Maximum">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="font">
-      <font>
-       <pointsize>7</pointsize>
-      </font>
-     </property>
-     <property name="frameShape">
-      <enum>QFrame::NoFrame</enum>
-     </property>
-     <property name="text">
-      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;right&quot;&gt;&lt;br/&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;Accela by &lt;/span&gt;&lt;a href=&quot;https://github.com/sthalik&quot;&gt;&lt;span style=&quot; font-size:10pt; text-decoration: underline; color:#0057ae;&quot;&gt;Stanisław Halik&lt;/span&gt;&lt;/a&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;&lt;br/&gt;Thanks to &lt;/span&gt;&lt;a href=&quot;https://github.com/dbaarda&quot;&gt;&lt;span style=&quot; font-size:10pt; text-decoration: underline; color:#0057ae;&quot;&gt;Donovan Baarda&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p align=&quot;right&quot;&gt;&lt;span style=&quot; font-size:10pt;&quot;&gt;2012-2015&lt;/span&gt;&lt;/p&gt;&lt;p align=&quot;right&quot;&gt;&lt;br/&gt;&lt;span style=&quot; font-size:8pt;&quot;&gt;Visit &lt;/span&gt;&lt;a href=&quot;https://github.com/opentrack/opentrack/wiki/Accela-in-opentrack-2.3&quot;&gt;&lt;span style=&quot; font-size:8pt; text-decoration: underline; color:#0000ff;&quot;&gt;our wiki&lt;/span&gt;&lt;/a&gt;&lt;span style=&quot; font-size:8pt;&quot;&gt; for description of the settings.&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-     </property>
-     <property name="textFormat">
-      <enum>Qt::RichText</enum>
-     </property>
-     <property name="scaledContents">
-      <bool>false</bool>
-     </property>
-     <property name="alignment">
-      <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
-     </property>
-     <property name="wordWrap">
-      <bool>true</bool>
-     </property>
-     <property name="margin">
-      <number>0</number>
-     </property>
-     <property name="indent">
-      <number>0</number>
-     </property>
-     <property name="openExternalLinks">
-      <bool>true</bool>
-     </property>
-     <property name="textInteractionFlags">
-      <set>Qt::LinksAccessibleByKeyboard|Qt::LinksAccessibleByMouse|Qt::TextBrowserInteraction|Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse</set>
-     </property>
-    </widget>
-   </item>
-   <item row="4" column="0">
-    <widget class="QDialogButtonBox" name="buttonBox">
-     <property name="sizePolicy">
-      <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
-       <horstretch>0</horstretch>
-       <verstretch>0</verstretch>
-      </sizepolicy>
-     </property>
-     <property name="standardButtons">
-      <set>QDialogButtonBox::Cancel|QDialogButtonBox::Ok</set>
+    <widget class="QGroupBox" name="groupBox">
+     <property name="title">
+      <string>Rotation nonlinearity</string>
      </property>
+     <layout class="QGridLayout" name="gridLayout_4">
+      <item row="0" column="2">
+       <widget class="QSlider" name="rot_nl_slider">
+        <property name="maximum">
+         <number>134</number>
+        </property>
+        <property name="orientation">
+         <enum>Qt::Horizontal</enum>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QLabel" name="rot_nl">
+        <property name="minimumSize">
+         <size>
+          <width>50</width>
+          <height>0</height>
+         </size>
+        </property>
+        <property name="font">
+         <font>
+          <pointsize>10</pointsize>
+         </font>
+        </property>
+        <property name="text">
+         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;x&lt;span style=&quot; vertical-align:super;&quot;&gt;2&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="0">
+       <widget class="QLabel" name="label_3">
+        <property name="text">
+         <string>Value</string>
+        </property>
+       </widget>
+      </item>
+     </layout>
     </widget>
    </item>
   </layout>
diff --git a/filter-accela/ftnoir_filter_accela.cpp b/filter-accela/ftnoir_filter_accela.cpp
index fe5aad86..11cb1aab 100644
--- a/filter-accela/ftnoir_filter_accela.cpp
+++ b/filter-accela/ftnoir_filter_accela.cpp
@@ -12,27 +12,28 @@
 #include "opentrack/plugin-api.hpp"
 
 static constexpr double rot_gains[][2] = {
-    { 4.0, 350 },
+    { 5, 350 },
     //{ 3.33, 200 },
     { 2.66, 110 },
     //{ 2.33, 85 },
-    //{ 2, 45 },
-    { 1.66, 30 },
-    //{ 1.33, 15 },
+    { 2, 45 },
+    //{ 1.66, 30 },
     { 1, 5 },
-    { .66, 1.4 },
-    { .33, .4 },
+    //{ .66, 1.3 },
+    //{ .5, .53 },
+    //{ .33, .25 },
+    { .22, .16 },
     { 0, 0 },
     { -1, 0 }
 };
 
 static constexpr double trans_gains[][2] = {
     //{ 3, 200 },
-    { 1.66, 40 },
-    { 1.33, 15 },
-    { 1, 5 },
+    { 2.33, 40 },
+    { 1.66, 13 },
+    { 1.33, 5 },
     { .66, 1 },
-    { .33, .6 },
+    { .33, .5 },
     { 0, 0 },
     { -1, 0 }
 };
@@ -86,6 +87,7 @@ void FTNoIR_Filter::filter(const double* input, double *output)
     const double alpha = dt/(dt+RC);
     const double rot_dz = s.rot_deadzone * s.mult_rot_dz;
     const double trans_dz = s.trans_deadzone * s.mult_trans_dz;
+    const double rot_nl = static_cast<const slider_value&>(s.rot_nonlinearity).cur();
 
     for (int i = 0; i < 6; i++)
     {
@@ -99,8 +101,13 @@ void FTNoIR_Filter::filter(const double* input, double *output)
         const double dz = i >= 3 ? rot_dz : trans_dz;
         const double vec_ = std::max(0., fabs(vec) - dz);
         const double thres = i >= 3 ? rot_t : trans_t;
-        const double val = m.getValue(vec_ / thres);
-        const double result = last_output[i] + (vec < 0 ? -1 : 1) * dt * val;
+        const double out_ = vec_ / thres;
+        const double out = i >= 3 && std::fabs(s.max_rot_nl - 1) > 5e-3
+                               ? (out_ * std::min(1., std::pow(out_/s.max_rot_nl, rot_nl)) * s.max_rot_nl)
+                               : out_;
+        const double val = m.getValue(float(out));
+        const double signum = int(vec < 0) * -2 + 1;
+        const double result = last_output[i] + signum * dt * val;
 
         last_output[i] = output[i] = result;
     }
diff --git a/filter-accela/ftnoir_filter_accela.h b/filter-accela/ftnoir_filter_accela.h
index 113e06e6..69db54cd 100644
--- a/filter-accela/ftnoir_filter_accela.h
+++ b/filter-accela/ftnoir_filter_accela.h
@@ -18,18 +18,21 @@ using namespace options;
 
 struct settings_accela : opts {
     value<int> rot_threshold, trans_threshold, ewma, rot_deadzone, trans_deadzone;
+    value<slider_value> rot_nonlinearity;
     static constexpr double mult_rot = 4. / 100.;
     static constexpr double mult_trans = 4. / 100.;
     static constexpr double mult_rot_dz = 2. / 100.;
     static constexpr double mult_trans_dz = 2. / 100.;
     static constexpr double mult_ewma = 1.25;
+    static constexpr double max_rot_nl = 1.33;
     settings_accela() :
         opts("Accela"),
         rot_threshold(b, "rotation-threshold", 45),
         trans_threshold(b, "translation-threshold", 50),
         ewma(b, "ewma", 2),
         rot_deadzone(b, "rotation-deadzone", 0),
-        trans_deadzone(b, "translation-deadzone", 0)
+        trans_deadzone(b, "translation-deadzone", 0),
+        rot_nonlinearity(b, "rotation-nonlinearity", slider_value(1., .66, 2))
     {}
 };
 
@@ -68,6 +71,7 @@ private slots:
     void update_trans_display(int value);
     void update_rot_dz_display(int value);
     void update_trans_dz_display(int value);
+    void update_rot_nl_slider(const slider_value& sl);
 };
 
 class FTNoIR_FilterDll : public Metadata
diff --git a/filter-accela/ftnoir_filter_accela_dialog.cpp b/filter-accela/ftnoir_filter_accela_dialog.cpp
index 451f476e..c046f986 100644
--- a/filter-accela/ftnoir_filter_accela_dialog.cpp
+++ b/filter-accela/ftnoir_filter_accela_dialog.cpp
@@ -16,7 +16,7 @@
 FilterControls::FilterControls() :
     accela_filter(nullptr)
 {
-    ui.setupUi( this );
+    ui.setupUi(this);
 
     connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
     connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
@@ -26,6 +26,7 @@ FilterControls::FilterControls() :
     connect(ui.ewma_slider, SIGNAL(valueChanged(int)), this, SLOT(update_ewma_display(int)));
     connect(ui.rot_dz_slider, SIGNAL(valueChanged(int)), this, SLOT(update_rot_dz_display(int)));
     connect(ui.trans_dz_slider, SIGNAL(valueChanged(int)), this, SLOT(update_trans_dz_display(int)));
+    connect(&s.rot_nonlinearity, SIGNAL(valueChanged(const slider_value&)), this, SLOT(update_rot_nl_slider(const slider_value&)));
 
     tie_setting(s.rot_threshold, ui.rotation_slider);
     tie_setting(s.trans_threshold, ui.translation_slider);
@@ -33,24 +34,32 @@ FilterControls::FilterControls() :
     tie_setting(s.rot_deadzone, ui.rot_dz_slider);
     tie_setting(s.trans_deadzone, ui.trans_dz_slider);
 
+    tie_setting(s.rot_nonlinearity, ui.rot_nl_slider);
+
     update_rot_display(ui.rotation_slider->value());
     update_trans_display(ui.translation_slider->value());
     update_ewma_display(ui.ewma_slider->value());
     update_rot_dz_display(ui.rot_dz_slider->value());
     update_trans_dz_display(ui.trans_dz_slider->value());
+    update_rot_nl_slider(s.rot_nonlinearity);
 }
 
 void FilterControls::register_filter(IFilter* filter)
 {
     accela_filter = static_cast<FTNoIR_Filter*>(filter);
-//#define LEAKING_DEBUG
-#ifdef LEAKING_DEBUG
-    auto d = new QDialog();
-    auto r = new QFunctionConfigurator(d);
-    r->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
-    r->setConfig(&accela_filter->rot, "");
-    r->setFixedSize(800, 300);
-    d->show();
+//#define SPLINE_ROT_DEBUG
+#if defined(SPLINE_ROT_DEBUG) || defined(SPLINE_TRANS_DEBUG)
+    QDialog d;
+    QFunctionConfigurator r(&d);
+    r.setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
+#if defined(SPLINE_TRANS_DEBUG)
+    r.setConfig(&accela_filter->trans, "");
+#else
+    r.setConfig(&accela_filter->rot, "");
+#endif
+    r.setFixedSize(800, 300);
+    d.show();
+    d.exec();
 #endif
 }
 
@@ -59,16 +68,19 @@ void FilterControls::unregister_filter()
     accela_filter = nullptr;
 }
 
-void FilterControls::doOK() {
+void FilterControls::doOK()
+{
     save();
     close();
 }
 
-void FilterControls::doCancel() {
+void FilterControls::doCancel()
+{
     close();
 }
 
-void FilterControls::save() {
+void FilterControls::save()
+{
     s.b->save();
 }
 
@@ -97,3 +109,10 @@ void FilterControls::update_trans_dz_display(int value)
     ui.trans_dz->setText(QString::number(value * s.mult_trans_dz) + "mm");
 }
 
+void FilterControls::update_rot_nl_slider(const slider_value& sl)
+{
+    ui.rot_nl->setText("<html><head/><body><p>x<span style='vertical-align:super;'>" +
+                       QString::number(sl.cur()) +
+                       "</span></p></body></html>");
+}
+