From 9e58b8d50f4914e79c8165c71ab43eb6ce335173 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Thu, 15 Jun 2017 10:18:22 +0200 Subject: proto/freetrack: workaround falcon bms 90 pitch limit --- proto-ft/ftnoir_protocol_ft.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'proto-ft') diff --git a/proto-ft/ftnoir_protocol_ft.cpp b/proto-ft/ftnoir_protocol_ft.cpp index 429ff72b..2c4a0bd1 100644 --- a/proto-ft/ftnoir_protocol_ft.cpp +++ b/proto-ft/ftnoir_protocol_ft.cpp @@ -41,12 +41,15 @@ freetrack::~freetrack() void freetrack::pose(const double* headpose) { const float yaw = -degrees_to_rads(headpose[Yaw]); - const float pitch = -degrees_to_rads(headpose[Pitch]); const float roll = degrees_to_rads(headpose[Roll]); const float tx = float(headpose[TX] * 10); const float ty = float(headpose[TY] * 10); const float tz = float(headpose[TZ] * 10); + // HACK: Falcon BMS makes a "bump" if pitch is over the value -sh 20170615 + const bool is_crossing_90 = std::fabs(headpose[Pitch] - 90) < 1e-4; + const float pitch = -degrees_to_rads(is_crossing_90 ? 89.86 : headpose[Pitch]); + FTHeap* ft = pMemData; FTData* data = &ft->data; -- cgit v1.2.3