Fix the jump of Yaw and Roll when crossing the border +/-180 degrees

When crossing the border on Yaw or Roll +/-180 degrees, the Octopus makes a full rotation of 360 degrees. This made it difficult to use an inertial tracker in a wireless VR.

2 files changed, 16 insertions, 2 deletions

diff --git a/filter-accela/ftnoir_filter_accela.cpp b/filter-accela/ftnoir_filter_accela.cpp
index 44a341e0..2059d7f9 100644
--- a/filter-accela/ftnoir_filter_accela.cpp
+++ b/filter-accela/ftnoir_filter_accela.cpp
@@ -64,6 +64,8 @@ static void do_deltas(const double* deltas, double* output, F&& fun)
 
 void accela::filter(const double* input, double *output)
 {
+#define FULL_TURN 360.0
+#define HALF_TURN 180.0
     if (unlikely(first_run))
     {
         first_run = false;
@@ -98,6 +100,7 @@ void accela::filter(const double* input, double *output)
     for (unsigned i = 3; i < 6; i++)
     {
         double d = input[i] - last_output[i];
+		if (fabs(d) > HALF_TURN) d -= copysign(FULL_TURN, d);
 
         if (fabs(d) > rot_dz)
             d -= copysign(rot_dz, d);
@@ -134,6 +137,7 @@ void accela::filter(const double* input, double *output)
     {
         output[k] *= dt;
         output[k] += last_output[k];
+		if (fabs(output[k]) > HALF_TURN) output[k] -= copysign(FULL_TURN, output[k]);
 
         last_output[k] = output[k];
     }
diff --git a/filter-ewma2/ftnoir_filter_ewma2.cpp b/filter-ewma2/ftnoir_filter_ewma2.cpp
index ce6cd040..09e6f5d7 100644
--- a/filter-ewma2/ftnoir_filter_ewma2.cpp
+++ b/filter-ewma2/ftnoir_filter_ewma2.cpp
@@ -26,6 +26,8 @@ ewma::ewma() = default;
 
 void ewma::filter(const double *input, double *output)
 {
+#define FULL_TURN 360.0
+#define HALF_TURN 180.0
     // Start the timer and initialise filter state if it's not running.
     if (first_run)
     {
@@ -57,7 +59,13 @@ void ewma::filter(const double *input, double *output)
         using std::pow;
 
         // Calculate the current and smoothed delta.
-        double delta = input[i] - last_output[i];
+		double input_value = input[i];
+		double delta = input_value - last_output[i];
+		if (fabs(delta) > HALF_TURN)
+		{
+			delta -= copysign(FULL_TURN, delta);
+			input_value -= copysign(FULL_TURN, input_value);
+		}
         last_delta[i] = delta_alpha*delta + (1.0-delta_alpha)*last_delta[i];
         // Calculate the current and smoothed noise variance.
         double noise = last_delta[i]*last_delta[i];
@@ -70,7 +78,9 @@ void ewma::filter(const double *input, double *output)
         // Calculate the dynamic alpha.
         double alpha = dt/(dt + RC);
         // Calculate the new output position.
-        output[i] = last_output[i] = alpha*input[i] + (1.0-alpha)*last_output[i];
+		output[i] = alpha*input_value + (1.0-alpha)*last_output[i];
+		if (fabs(output[i]) > HALF_TURN) output[i] -= copysign(FULL_TURN, output[i]);
+		last_output[i] = output[i];
     }
 }