summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2014-10-24 11:22:06 +0200
committerStanislaw Halik <sthalik@misaki.pl>2014-10-24 12:25:35 +0200
commitbc5b5a705f6fa44813d387f31e455bda197c2110 (patch)
tree2f49864041e52df4bd12c900a6c6b6c0c5f770b5
parent5960f42de5cb17c78a3337ee7eed81ab49abd815 (diff)
use moving average in accela over N seconds
Also add showing alphas in UI for configuration's sake. Hopefully update wiki soon enough.
-rw-r--r--facetracknoir/facetracknoir.ui4
-rw-r--r--ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui651
-rw-r--r--ftnoir_filter_accela/ftnoir_filter_accela.cpp69
-rw-r--r--ftnoir_filter_accela/ftnoir_filter_accela.h38
-rw-r--r--ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp28
5 files changed, 428 insertions, 362 deletions
diff --git a/facetracknoir/facetracknoir.ui b/facetracknoir/facetracknoir.ui
index 36fd1e30..c114744b 100644
--- a/facetracknoir/facetracknoir.ui
+++ b/facetracknoir/facetracknoir.ui
@@ -7,8 +7,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>640</width>
- <height>710</height>
+ <width>1000</width>
+ <height>1000</height>
</rect>
</property>
<property name="windowIcon">
diff --git a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
index c544263d..3c7f52ab 100644
--- a/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
+++ b/ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui
@@ -9,28 +9,10 @@
<rect>
<x>0</x>
<y>0</y>
- <width>261</width>
- <height>330</height>
+ <width>277</width>
+ <height>447</height>
</rect>
</property>
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>0</width>
- <height>0</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>6667</width>
- <height>6666</height>
- </size>
- </property>
<property name="windowTitle">
<string>Accela filter settings</string>
</property>
@@ -38,186 +20,345 @@
<iconset resource="../ftnoir_filter_ewma2/ewma-filter.qrc">
<normaloff>:/images/filter-16.png</normaloff>:/images/filter-16.png</iconset>
</property>
- <property name="layoutDirection">
- <enum>Qt::LeftToRight</enum>
- </property>
- <property name="autoFillBackground">
- <bool>false</bool>
- </property>
- <property name="styleSheet">
- <string notr="true"/>
- </property>
- <layout class="QGridLayout" name="gridLayout">
- <property name="bottomMargin">
- <number>5</number>
- </property>
- <property name="horizontalSpacing">
- <number>7</number>
- </property>
- <item row="2" column="0">
- <widget class="QLabel" name="label_3">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Translation</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <widget class="QDoubleSpinBox" name="rotation_alpha">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
- </property>
- <property name="correctionMode">
- <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
- </property>
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>0.100000000000000</double>
- </property>
- <property name="maximum">
- <double>65535.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QLabel" name="label_2">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>Rotation</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
- </property>
- </widget>
- </item>
- <item row="7" column="0">
- <widget class="QLabel" name="label_10">
- <property name="text">
- <string>Exponent</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_12">
- <property name="text">
- <string>Order #3</string>
- </property>
- </widget>
- </item>
- <item row="2" column="1">
- <widget class="QDoubleSpinBox" name="translation_alpha">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
- </property>
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>0.100000000000000</double>
- </property>
- <property name="maximum">
- <double>65535.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_11">
- <property name="text">
- <string>Order #2</string>
- </property>
- </widget>
- </item>
- <item row="5" column="1">
- <widget class="QDoubleSpinBox" name="rot_deadzone">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
- </property>
- <property name="decimals">
- <number>3</number>
- </property>
- <property name="minimum">
- <double>0.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.050000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
+ <layout class="QVBoxLayout" name="verticalLayout">
+ <item>
+ <widget class="QFrame" name="frame">
+ <property name="frameShape">
+ <enum>QFrame::NoFrame</enum>
</property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_3">
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Rotation</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QDoubleSpinBox" name="rotation_alpha">
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+ </property>
+ <property name="correctionMode">
+ <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
+ </property>
+ <property name="decimals">
+ <number>4</number>
+ </property>
+ <property name="minimum">
+ <double>0.100000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>65535.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_3">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Translation</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QDoubleSpinBox" name="translation_alpha">
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+ </property>
+ <property name="decimals">
+ <number>4</number>
+ </property>
+ <property name="minimum">
+ <double>0.100000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>65535.000000000000000</double>
+ </property>
+ <property name="value">
+ <double>1.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_11">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Fast alpha</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QSlider" name="ui_fast_alpha">
+ <property name="maximum">
+ <number>100</number>
+ </property>
+ <property name="pageStep">
+ <number>8</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="0">
+ <widget class="QLabel" name="lblSensYaw_6">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">color:#0;
+background:none;</string>
+ </property>
+ <property name="text">
+ <string>Rotation deadband</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QDoubleSpinBox" name="rot_deadzone">
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+ </property>
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>0.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>100.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.050000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Translation deadband</string>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QDoubleSpinBox" name="trans_deadzone">
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
+ </property>
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>0.000000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>100.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.050000000000000</double>
+ </property>
+ <property name="value">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_10">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Exponent</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QDoubleSpinBox" name="expt">
+ <property name="decimals">
+ <number>3</number>
+ </property>
+ <property name="minimum">
+ <double>0.050000000000000</double>
+ </property>
+ <property name="maximum">
+ <double>100.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.050000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
- <item row="5" column="0">
- <widget class="QLabel" name="lblSensYaw_6">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Expanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="minimumSize">
- <size>
- <width>25</width>
- <height>0</height>
- </size>
- </property>
- <property name="maximumSize">
- <size>
- <width>150</width>
- <height>16777215</height>
- </size>
- </property>
- <property name="styleSheet">
- <string notr="true">color:#0;
-background:none;</string>
- </property>
- <property name="text">
- <string>Rotation deadband</string>
- </property>
+ <item>
+ <widget class="QGroupBox" name="groupBox">
+ <property name="title">
+ <string>Debug information</string>
+ </property>
+ <layout class="QGridLayout" name="gridLayout_2">
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Yaw</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_6">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Pitch</string>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_5">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Minimum" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>Roll</string>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QDoubleSpinBox" name="debug_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ <property name="buttonSymbols">
+ <enum>QAbstractSpinBox::PlusMinus</enum>
+ </property>
+ <property name="decimals">
+ <number>8</number>
+ </property>
+ <property name="maximum">
+ <double>99.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="1">
+ <widget class="QDoubleSpinBox" name="debug_p">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ <property name="buttonSymbols">
+ <enum>QAbstractSpinBox::PlusMinus</enum>
+ </property>
+ <property name="decimals">
+ <number>8</number>
+ </property>
+ <property name="maximum">
+ <double>99.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QDoubleSpinBox" name="debug_r">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ <property name="buttonSymbols">
+ <enum>QAbstractSpinBox::PlusMinus</enum>
+ </property>
+ <property name="decimals">
+ <number>8</number>
+ </property>
+ <property name="maximum">
+ <double>99.000000000000000</double>
+ </property>
+ <property name="singleStep">
+ <double>0.000000000000000</double>
+ </property>
+ </widget>
+ </item>
+ </layout>
</widget>
</item>
- <item row="8" column="0" colspan="2">
+ <item>
<widget class="QLabel" name="label_9">
<property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="Maximum" vsizetype="Maximum">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -231,7 +372,7 @@ background:none;</string>
<enum>QFrame::NoFrame</enum>
</property>
<property name="text">
- <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;justify&quot;&gt;Accela by &lt;a href=&quot;https://github.com/sthalik&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0057ae;&quot;&gt;Stanisław Halik&lt;/span&gt;&lt;/a&gt;&lt;br/&gt;Help from &lt;a href=&quot;https://github.com/dbaarda&quot;&gt;&lt;span style=&quot; text-decoration: underline; color:#0057ae;&quot;&gt;Donovan Baarda&lt;/span&gt;&lt;/a&gt;&lt;/p&gt;&lt;p align=&quot;justify&quot;&gt;2012-2013&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+ <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p align=&quot;justify&quot;&gt;&lt;br/&gt;&lt;/p&gt;&lt;p align=&quot;justify&quot;&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-2014&lt;/span&gt;&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
</property>
<property name="textFormat">
<enum>Qt::RichText</enum>
@@ -259,29 +400,7 @@ background:none;</string>
</property>
</widget>
</item>
- <item row="7" column="1">
- <widget class="QDoubleSpinBox" name="expt">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="decimals">
- <number>3</number>
- </property>
- <property name="minimum">
- <double>0.050000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.050000000000000</double>
- </property>
- </widget>
- </item>
- <item row="9" column="0" colspan="2">
+ <item>
<widget class="QDialogButtonBox" name="buttonBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Maximum" vsizetype="Maximum">
@@ -294,107 +413,9 @@ background:none;</string>
</property>
</widget>
</item>
- <item row="4" column="1">
- <widget class="QDoubleSpinBox" name="order_3rd">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
- </property>
- <property name="correctionMode">
- <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
- </property>
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>0.100000000000000</double>
- </property>
- <property name="maximum">
- <double>65535.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QDoubleSpinBox" name="order_2nd">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
- </property>
- <property name="correctionMode">
- <enum>QAbstractSpinBox::CorrectToPreviousValue</enum>
- </property>
- <property name="decimals">
- <number>4</number>
- </property>
- <property name="minimum">
- <double>0.100000000000000</double>
- </property>
- <property name="maximum">
- <double>65535.000000000000000</double>
- </property>
- <property name="value">
- <double>1.000000000000000</double>
- </property>
- </widget>
- </item>
- <item row="6" column="0">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Translation deadband</string>
- </property>
- </widget>
- </item>
- <item row="6" column="1">
- <widget class="QDoubleSpinBox" name="trans_deadzone">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Expanding" vsizetype="Maximum">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="alignment">
- <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
- </property>
- <property name="decimals">
- <number>3</number>
- </property>
- <property name="minimum">
- <double>0.000000000000000</double>
- </property>
- <property name="maximum">
- <double>100.000000000000000</double>
- </property>
- <property name="singleStep">
- <double>0.050000000000000</double>
- </property>
- <property name="value">
- <double>0.000000000000000</double>
- </property>
- </widget>
- </item>
</layout>
</widget>
<tabstops>
- <tabstop>rotation_alpha</tabstop>
- <tabstop>translation_alpha</tabstop>
- <tabstop>order_2nd</tabstop>
- <tabstop>order_3rd</tabstop>
- <tabstop>rot_deadzone</tabstop>
- <tabstop>trans_deadzone</tabstop>
- <tabstop>expt</tabstop>
<tabstop>buttonBox</tabstop>
</tabstops>
<resources>
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.cpp b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
index 639e2e45..22feafae 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.cpp
@@ -12,7 +12,7 @@
#include "opentrack/plugin-api.hpp"
using namespace std;
-FTNoIR_Filter::FTNoIR_Filter() : first_run(true)
+FTNoIR_Filter::FTNoIR_Filter() : first_run(true), fast_state { 0,0,0, 0,0,0 }
{
}
@@ -20,45 +20,64 @@ static inline double parabola(const double a, const double x, const double dz, c
{
const double sign = x > 0 ? 1 : -1;
const double a1 = 1./a;
- return a1 * pow(std::max<double>(fabs(x) - dz, 0), expt) * sign;
+ return a1 * pow(std::max(0., fabs(x) - dz), expt) * sign;
}
-void FTNoIR_Filter::filter(const double* target_camera_position,
- double *new_camera_position)
+void FTNoIR_Filter::filter(const double* input, double *output)
{
if (first_run)
{
for (int i = 0; i < 6; i++)
{
- new_camera_position[i] = target_camera_position[i];
+ fast_state[i] = 0;
+ output[i] = input[i];
for (int j = 0; j < 3; j++)
- last_output[j][i] = target_camera_position[i];
+ last_output[i] = input[i];
}
-
first_run = false;
- return;
+ return;
}
+ const double fast_c = s.fast_alpha / 100.;
+ const double rot_dz = s.rot_deadzone;
+ const double trans_dz = s.trans_deadzone;
+ const double rot_a = s.rotation_alpha;
+ const double trans_a = s.translation_alpha;
+ const double expt = s.expt;
+
+ static constexpr double fast_alpha = Hz/(Hz + fast_alpha_seconds);
+
for (int i=0;i<6;i++)
{
- const double vec = target_camera_position[i] - last_output[0][i];
- const double vec2 = target_camera_position[i] - last_output[1][i];
- const double vec3 = target_camera_position[i] - last_output[2][i];
- const int sign = vec < 0 ? -1 : 1;
- const double a = i >= 3 ? s.rotation_alpha : s.translation_alpha;
- const double a2 = a * s.second_order_alpha;
- const double a3 = a * s.third_order_alpha;
- const double deadzone = i >= 3 ? s.rot_deadzone : s.trans_deadzone;
- const double velocity =
- parabola(a, vec, deadzone, s.expt) +
- parabola(a2, vec2, deadzone, s.expt) +
- parabola(a3, vec3, deadzone, s.expt);
- const double result = last_output[0][i] + velocity;
- const bool done = sign > 0 ? result >= target_camera_position[i] : result <= target_camera_position[i];
- last_output[2][i] = last_output[1][i];
- last_output[1][i] = last_output[0][i];
- last_output[0][i] = new_camera_position[i] = done ? target_camera_position[i] : result;
+ const double vec = input[i] - last_output[i];
+ const double a = i >= 3 ? rot_a : trans_a;
+ const double deadzone = i >= 3 ? rot_dz : trans_dz;
+
+ double datum;
+
+ if (i >= 3)
+ {
+ const double cur_fast = std::abs(vec) * fast_alpha + fast_state[i]*(1. - fast_alpha);
+ fast_state[i] = cur_fast;
+ const double how_fast = std::max(0., fast_c * (cur_fast - max_slow_delta));
+ datum = parabola(a, vec * (1.-damping + how_fast), deadzone, s.expt);
+ }
+ else
+ datum = parabola(a, vec, deadzone, expt);
+
+ const double result = last_output[i] + datum;
+ const bool negp = vec < 0.;
+ const bool done = negp ? result <= input[i] : result >= input[i];
+
+ last_output[i] = last_output[i];
+ last_output[i] = last_output[i];
+ const double ret = done ? input[i] : result;
+ last_output[i] = output[i] = ret;
}
+
+ state.y = output[Yaw] - input[Yaw];
+ state.p = output[Pitch] - input[Pitch];
+ state.r = output[Roll] - input[Roll];
}
extern "C" OPENTRACK_EXPORT IFilter* GetConstructor()
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela.h b/ftnoir_filter_accela/ftnoir_filter_accela.h
index 5e8ed428..da5155ca 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela.h
+++ b/ftnoir_filter_accela/ftnoir_filter_accela.h
@@ -1,11 +1,13 @@
#pragma once
#include "ui_ftnoir_accela_filtercontrols.h"
#include "opentrack/plugin-api.hpp"
+#include <atomic>
#include <QMutex>
+#include <QTimer>
#define ACCELA_SMOOTHING_ROTATION 60.0
#define ACCELA_SMOOTHING_TRANSLATION 40.0
-#define ACCELA_SECOND_ORDER_ALPHA 100.0
+#define ACCELA_fast_ALPHA 100.0
#define ACCELA_THIRD_ORDER_ALPHA 180.0
#include "opentrack/options.hpp"
@@ -15,38 +17,46 @@ struct settings {
pbundle b;
value<double> rotation_alpha,
translation_alpha,
- second_order_alpha,
- third_order_alpha,
rot_deadzone,
trans_deadzone,
expt;
+ value<int> fast_alpha;
settings() :
b(bundle("Accela")),
rotation_alpha(b, "rotation-alpha", ACCELA_SMOOTHING_ROTATION),
translation_alpha(b, "translation-alpha", ACCELA_SMOOTHING_TRANSLATION),
- second_order_alpha(b, "second-order-alpha", ACCELA_SECOND_ORDER_ALPHA),
- third_order_alpha(b, "third-order-alpha", ACCELA_THIRD_ORDER_ALPHA),
rot_deadzone(b, "rotation-deadband", 0),
trans_deadzone(b, "translation-deadband", 0),
- expt(b, "exponent", 2)
+ expt(b, "exponent", 2),
+ fast_alpha(b, "fast-alpha", 0)
{}
};
+struct state_display
+{
+ double y, p, r;
+ state_display() : y(0), p(0), r(0) {};
+};
+
class FTNoIR_Filter : public IFilter
{
public:
FTNoIR_Filter();
void filter(const double* target_camera_position, double *new_camera_position);
- void reset() {
- first_run = true;
- }
- void receiveSettings() {
- s.b->reload();
- }
+ state_display state;
private:
+ // hardcoded distance between filter() calls
+ static constexpr double Hz = 3./1000;
+ // moving average history
+ static constexpr double fast_alpha_seconds = 10;
+ // max degrees considered "slow" after alpha
+ static constexpr double max_slow_delta = 0.34;
+ // if set to zero, never decreases response
+ static constexpr double damping = 0.3;
settings s;
bool first_run;
- double last_output[3][6];
+ double last_output[6];
+ double fast_state[6];
};
class FilterControls: public IFilterDialog
@@ -62,9 +72,11 @@ private:
void save();
FTNoIR_Filter* accela_filter;
settings s;
+ QTimer t;
private slots:
void doOK();
void doCancel();
+ void timer_fired();
};
class FTNoIR_FilterDll : public Metadata
diff --git a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
index 393a0439..0f21cbcb 100644
--- a/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
+++ b/ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp
@@ -14,26 +14,42 @@ FilterControls::FilterControls() :
tie_setting(s.rotation_alpha, ui.rotation_alpha);
tie_setting(s.translation_alpha, ui.translation_alpha);
- tie_setting(s.second_order_alpha, ui.order_2nd);
- tie_setting(s.third_order_alpha, ui.order_3rd);
+ tie_setting(s.fast_alpha, ui.ui_fast_alpha);
tie_setting(s.rot_deadzone, ui.rot_deadzone);
tie_setting(s.trans_deadzone, ui.trans_deadzone);
tie_setting(s.expt, ui.expt);
+
+ connect(&t, SIGNAL(timeout()), this, SLOT(timer_fired()));
+
+ t.setInterval(250);
+}
+
+void FilterControls::timer_fired()
+{
+ if (accela_filter)
+ {
+ state_display st = accela_filter->state;
+ ui.debug_y->setValue(st.y);
+ ui.debug_p->setValue(st.p);
+ ui.debug_r->setValue(st.r);
+ }
}
void FilterControls::register_filter(IFilter* filter)
{
accela_filter = static_cast<FTNoIR_Filter*>(filter);
+ t.start();
}
void FilterControls::unregister_filter()
{
- accela_filter = NULL;
+ t.stop();
+ accela_filter = nullptr;
}
void FilterControls::doOK() {
- save();
- this->close();
+ save();
+ this->close();
}
void FilterControls::doCancel() {
@@ -48,8 +64,6 @@ void FilterControls::discard()
void FilterControls::save() {
s.b->save();
- if (accela_filter)
- accela_filter->receiveSettings();
}
extern "C" OPENTRACK_EXPORT IFilterDialog* GetDialog()