From cca9299636ac2f2768256aef2d527d1be22b1616 Mon Sep 17 00:00:00 2001
From: Stanislaw Halik <sthalik@misaki.pl>
Date: Wed, 28 Sep 2016 02:01:42 +0200
Subject: logic/tracker: sanitize tracker's euler angles

Issue: #465
Reported-by: @topiq77
---
 logic/tracker.cpp | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/logic/tracker.cpp b/logic/tracker.cpp
index bfc95eed..325381ee 100644
--- a/logic/tracker.cpp
+++ b/logic/tracker.cpp
@@ -136,6 +136,16 @@ void Tracker::logic()
         raw(i) = newpose(i);
     }
 
+    // hatire, udp, and freepie trackers can mess up here
+    for (unsigned i = 3; i < 6; i++)
+    {
+        using std::fmod;
+        using std::copysign;
+
+        const double x = value(i);
+        value(i) = fmod(x + copysign(180, x), 360) - copysign(180, x);
+    }
+
     logger.write_pose(raw); // raw
 
     if (is_nan(raw))
-- 
cgit v1.2.3