From 7252c00161959b3b7ce9d78506ec6cf026f24d5e Mon Sep 17 00:00:00 2001 From: GO63-samara Date: Tue, 14 Apr 2020 16:27:59 +0400 Subject: 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. --- filter-accela/ftnoir_filter_accela.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'filter-accela') 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]; } -- cgit v1.2.3