diff options
Diffstat (limited to 'logic/tracker.cpp')
| -rw-r--r-- | logic/tracker.cpp | 16 |
1 files changed, 11 insertions, 5 deletions
diff --git a/logic/tracker.cpp b/logic/tracker.cpp index 5733166a..a5892dd0 100644 --- a/logic/tracker.cpp +++ b/logic/tracker.cpp @@ -300,15 +300,21 @@ 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)), + euler_t(value(Yaw) * d2r * tcomp_c[0], + value(Pitch) * d2r * tcomp_c[1], + value(Roll) * d2r * tcomp_c[2])), value_, value_, - false, - false, + s.tcomp_disable_tx, + s.tcomp_disable_ty, s.tcomp_disable_tz); if (is_nan(value_)) nanp = true; |