From 93ef70c43d7f56377fe4ed2f3e38a0a2ee27ddd9 Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Fri, 22 Feb 2019 17:41:36 +0100
Subject: [PATCH] vmount: add missing yaw wrap at the end of a ROI update
---
src/modules/vmount/output.cpp | 4 ++++
1 file changed, 4 insertions(+)
diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp
index cddf1b756f..450b7166c9 100644
--- a/src/modules/vmount/output.cpp
+++ b/src/modules/vmount/output.cpp
@@ -51,6 +51,8 @@
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
+using matrix::wrap_pi;
+
namespace vmount
{
@@ -197,6 +199,8 @@ void OutputBase::_handle_position_update(bool force_update)
_angle_setpoints[1] += _cur_control_data->type_data.lonlat.pitch_angle_offset;
_angle_setpoints[2] += _cur_control_data->type_data.lonlat.yaw_angle_offset;
+ // make sure yaw is wrapped correctly for the output
+ _angle_setpoints[2] = wrap_pi(_angle_setpoints[2]);
}
void OutputBase::_calculate_output_angles(const hrt_abstime &t)
--
GitLab