filter/accela: add debug knob

2 files changed, 46 insertions, 16 deletions

diff --git a/filter-accela/ftnoir_filter_accela.cpp b/filter-accela/ftnoir_filter_accela.cpp
index a4095774..92b9e197 100644
--- a/filter-accela/ftnoir_filter_accela.cpp
+++ b/filter-accela/ftnoir_filter_accela.cpp
@@ -11,11 +11,9 @@
 #include <QMutexLocker>
 #include "api/plugin-api.hpp"
 
-using std::fabs;
-using std::sqrt;
-using std::pow;
-using std::copysign;
-using std::fmin;
+#include "compat/math-imports.hpp"
+
+using namespace otr_math;
 
 constexpr settings_accela::gains settings_accela::rot_gains[16];
 constexpr settings_accela::gains settings_accela::pos_gains[16];
@@ -25,12 +23,6 @@ accela::accela() : first_run(true)
     s.make_splines(spline_rot, spline_pos);
 }
 
-template <typename T>
-static inline constexpr T signum(T x)
-{
-    return T((T(0) < x) - (x < T(0)));
-}
-
 template<int N = 3, typename F>
 never_inline
 static void do_deltas(const double* deltas, double* output, double alpha, double& smoothed, F&& fun)
@@ -44,7 +36,8 @@ static void do_deltas(const double* deltas, double* output, double alpha, double
         return sqrt(ret);
     );
 
-    const double dist = smoothed = fmin(dist_, alpha*dist_ + (1-alpha)*smoothed);
+    const double dist = fmin(dist_, alpha*dist_ + (1-alpha)*smoothed);
+    smoothed = dist;
     const double value = double(fun(dist));
 
     for (unsigned k = 0; k < N; k++)
@@ -78,18 +71,26 @@ static void do_deltas(const double* deltas, double* output, double alpha, double
 
 void accela::filter(const double* input, double *output)
 {
-    if (first_run)
+    if (unlikely(first_run))
     {
+        first_run = false;
+
         for (int i = 0; i < 6; i++)
         {
             const double f = input[i];
             output[i] = f;
             last_output[i] = f;
         }
-        first_run = false;
+
         smoothed_input[0] = 0;
         smoothed_input[1] = 0;
+
         t.start();
+#if defined DEBUG_ACCELA
+        debug_max = 0;
+        debug_timer.start();
+#endif
+
         return;
     }
 
@@ -132,6 +133,24 @@ void accela::filter(const double* input, double *output)
 
     do_deltas(&deltas[Yaw], &output[Yaw], alpha, smoothed_input[0], [this](double x) { return spline_rot.get_value_no_save(x); });
 
+#if defined DEBUG_ACCELA
+    var.input(smoothed_input[0]);
+    debug_max = fmax(debug_max, smoothed_input[0]);
+
+    using time_units::secs_;
+
+    if (debug_timer.is_elapsed(secs_(1)))
+    {
+        qDebug() << "accela:"
+                 << "max" << debug_max
+                 << "mean" << var.avg()
+                 << "stddev" << var.stddev();
+
+        var.clear();
+        debug_max = 0;
+    }
+#endif
+
     // pos
 
     for (unsigned i = 0; i < 3; i++)
diff --git a/filter-accela/ftnoir_filter_accela.h b/filter-accela/ftnoir_filter_accela.h
index bbe74b08..fa3c611f 100644
--- a/filter-accela/ftnoir_filter_accela.h
+++ b/filter-accela/ftnoir_filter_accela.h
@@ -11,11 +11,17 @@
 #include "accela-settings.hpp"
 #include "api/plugin-api.hpp"
 #include "compat/timer.hpp"
+#include "compat/variance.hpp"
 
 #include <atomic>
 #include <QMutex>
 #include <QTimer>
 
+// ------------------------------------
+// debug knob
+// ------------------------------------
+//#define DEBUG_ACCELA
+
 class accela : public IFilter
 {
 public:
@@ -25,13 +31,18 @@ public:
     spline spline_rot, spline_pos;
 private:
     settings_accela s;
-    bool first_run;
     double last_output[6], deltas[6];
     double smoothed_input[2];
     Timer t;
+#if defined DEBUG_ACCELA
+    Timer debug_timer;
+    double debug_max;
+    variance var;
+#endif
+    bool first_run;
 };
 
-class dialog_accela: public IFilterDialog
+class dialog_accela : public IFilterDialog
 {
     Q_OBJECT
 public: