diff options
author | Stanislaw Halik <sthalik@misaki.pl> | 2014-10-24 11:22:06 +0200 |
---|---|---|
committer | Stanislaw Halik <sthalik@misaki.pl> | 2014-10-24 12:25:35 +0200 |
commit | bc5b5a705f6fa44813d387f31e455bda197c2110 (patch) | |
tree | 2f49864041e52df4bd12c900a6c6b6c0c5f770b5 | |
parent | 5960f42de5cb17c78a3337ee7eed81ab49abd815 (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.ui | 4 | ||||
-rw-r--r-- | ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui | 651 | ||||
-rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela.cpp | 69 | ||||
-rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela.h | 38 | ||||
-rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela_dialog.cpp | 28 |
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><html><head/><body><p align="justify">Accela by <a href="https://github.com/sthalik"><span style=" text-decoration: underline; color:#0057ae;">Stanisław Halik</span></a><br/>Help from <a href="https://github.com/dbaarda"><span style=" text-decoration: underline; color:#0057ae;">Donovan Baarda</span></a></p><p align="justify">2012-2013</p></body></html></string> + <string><html><head/><body><p align="justify"><br/></p><p align="justify"><span style=" font-size:10pt;">Accela by </span><a href="https://github.com/sthalik"><span style=" font-size:10pt; text-decoration: underline; color:#0057ae;">Stanisław Halik</span></a><span style=" font-size:10pt;"><br/>Thanks to </span><a href="https://github.com/dbaarda"><span style=" font-size:10pt; text-decoration: underline; color:#0057ae;">Donovan Baarda</span></a></p><p align="right"><span style=" font-size:10pt;">2012-2014</span></p></body></html></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() |