From bc5b5a705f6fa44813d387f31e455bda197c2110 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Fri, 24 Oct 2014 11:22:06 +0200 Subject: use moving average in accela over N seconds Also add showing alphas in UI for configuration's sake. Hopefully update wiki soon enough. --- facetracknoir/facetracknoir.ui | 4 +- .../ftnoir_accela_filtercontrols.ui | 651 +++++++++++---------- ftnoir_filter_accela/ftnoir_filter_accela.cpp | 69 ++- ftnoir_filter_accela/ftnoir_filter_accela.h | 38 +- .../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 @@ 0 0 - 640 - 710 + 1000 + 1000 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 @@ 0 0 - 261 - 330 + 277 + 447 - - - 0 - 0 - - - - - 0 - 0 - - - - - 6667 - 6666 - - Accela filter settings @@ -38,186 +20,345 @@ :/images/filter-16.png:/images/filter-16.png - - Qt::LeftToRight - - - false - - - - - - - 5 - - - 7 - - - - - - 0 - 0 - - - - Translation - - - - - - - - 0 - 0 - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - QAbstractSpinBox::CorrectToPreviousValue - - - 4 - - - 0.100000000000000 - - - 65535.000000000000000 - - - 1.000000000000000 - - - - - - - - 0 - 0 - - - - Rotation - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - - - - - Exponent - - - - - - - Order #3 - - - - - - - - 0 - 0 - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - 4 - - - 0.100000000000000 - - - 65535.000000000000000 - - - 1.000000000000000 - - - - - - - Order #2 - - - - - - - - 0 - 0 - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - 3 - - - 0.000000000000000 - - - 100.000000000000000 - - - 0.050000000000000 - - - 0.000000000000000 + + + + + QFrame::NoFrame + + QFrame::Raised + + + + + + + 0 + 0 + + + + Rotation + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + QAbstractSpinBox::CorrectToPreviousValue + + + 4 + + + 0.100000000000000 + + + 65535.000000000000000 + + + 1.000000000000000 + + + + + + + + 0 + 0 + + + + Translation + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + 4 + + + 0.100000000000000 + + + 65535.000000000000000 + + + 1.000000000000000 + + + + + + + + 0 + 0 + + + + Fast alpha + + + + + + + 100 + + + 8 + + + Qt::Horizontal + + + + + + + + 0 + 0 + + + + color:#0; +background:none; + + + Rotation deadband + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + 3 + + + 0.000000000000000 + + + 100.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 + + + + + + + + 0 + 0 + + + + Translation deadband + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + 3 + + + 0.000000000000000 + + + 100.000000000000000 + + + 0.050000000000000 + + + 0.000000000000000 + + + + + + + + 0 + 0 + + + + Exponent + + + + + + + 3 + + + 0.050000000000000 + + + 100.000000000000000 + + + 0.050000000000000 + + + + - - - - - 0 - 0 - - - - - 25 - 0 - - - - - 150 - 16777215 - - - - color:#0; -background:none; - - - Rotation deadband - + + + + Debug information + + + + + + + 0 + 0 + + + + Yaw + + + + + + + + 0 + 0 + + + + Pitch + + + + + + + + 0 + 0 + + + + Roll + + + + + + + + 0 + 0 + + + + true + + + QAbstractSpinBox::PlusMinus + + + 8 + + + 99.000000000000000 + + + 0.000000000000000 + + + + + + + + 0 + 0 + + + + true + + + QAbstractSpinBox::PlusMinus + + + 8 + + + 99.000000000000000 + + + 0.000000000000000 + + + + + + + + 0 + 0 + + + + true + + + QAbstractSpinBox::PlusMinus + + + 8 + + + 99.000000000000000 + + + 0.000000000000000 + + + + - + - + 0 0 @@ -231,7 +372,7 @@ background:none; QFrame::NoFrame - <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> + <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> Qt::RichText @@ -259,29 +400,7 @@ background:none; - - - - - 0 - 0 - - - - 3 - - - 0.050000000000000 - - - 100.000000000000000 - - - 0.050000000000000 - - - - + @@ -294,107 +413,9 @@ background:none; - - - - - 0 - 0 - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - QAbstractSpinBox::CorrectToPreviousValue - - - 4 - - - 0.100000000000000 - - - 65535.000000000000000 - - - 1.000000000000000 - - - - - - - - 0 - 0 - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - QAbstractSpinBox::CorrectToPreviousValue - - - 4 - - - 0.100000000000000 - - - 65535.000000000000000 - - - 1.000000000000000 - - - - - - - Translation deadband - - - - - - - - 0 - 0 - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - 3 - - - 0.000000000000000 - - - 100.000000000000000 - - - 0.050000000000000 - - - 0.000000000000000 - - - - rotation_alpha - translation_alpha - order_2nd - order_3rd - rot_deadzone - trans_deadzone - expt buttonBox 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(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 #include +#include #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 rotation_alpha, translation_alpha, - second_order_alpha, - third_order_alpha, rot_deadzone, trans_deadzone, expt; + value 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(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() -- cgit v1.2.3