diff options
| -rw-r--r-- | ftnoir_filter_accela/ftnoir_accela_filtercontrols.ui | 4 | ||||
| -rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela.cpp | 56 | ||||
| -rw-r--r-- | ftnoir_filter_accela/ftnoir_filter_accela.h | 6 | ||||
| -rw-r--r-- | ftnoir_filter_accela/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) |