diff options
Diffstat (limited to 'logic/tracker.cpp')
| -rw-r--r-- | logic/tracker.cpp | 11 |
1 files changed, 10 insertions, 1 deletions
diff --git a/logic/tracker.cpp b/logic/tracker.cpp index f0826a5f..84d3afdb 100644 --- a/logic/tracker.cpp +++ b/logic/tracker.cpp @@ -288,8 +288,17 @@ void Tracker::logic() if (s.tcomp_p && !get(f_tcomp_disabled)) { + const double tcomp_c[] = + { + double(!s.tcomp_disable_src_yaw), + double(!s.tcomp_disable_src_pitch), + double(!s.tcomp_disable_src_roll), + }; euler_t value_(value(TX), value(TY), value(TZ)); - t_compensate(euler_to_rmat(euler_t(value(Yaw) * d2r, value(Pitch) * d2r, value(Roll) * d2r)), + t_compensate(euler_to_rmat( + euler_t(value(Yaw) * d2r * tcomp_c[0], + value(Pitch) * d2r * tcomp_c[1], + value(Roll) * d2r * tcomp_c[2])), value_, value_, s.tcomp_disable_tx, |