summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2017-06-15 10:18:22 +0200
committerStanislaw Halik <sthalik@misaki.pl>2017-06-18 18:48:42 +0200
commit9e58b8d50f4914e79c8165c71ab43eb6ce335173 (patch)
tree681bbcd93b82afec227b5af8e25c806655f282b9
parent07a9abdfcb1880bcb7f2aec41ee96b86612fed52 (diff)
proto/freetrack: workaround falcon bms 90 pitch limit
-rw-r--r--proto-ft/ftnoir_protocol_ft.cpp5
1 files changed, 4 insertions, 1 deletions
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;