From 873ed17b1428b86d6bb8905988305382811ca3b2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 27 Sep 2016 11:58:19 +0200
Subject: [PATCH] VMount: Do not rely on euler angles

---
 src/drivers/vmount/output.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/drivers/vmount/output.cpp b/src/drivers/vmount/output.cpp
index e1f1c2b7fb..f804c631f9 100644
--- a/src/drivers/vmount/output.cpp
+++ b/src/drivers/vmount/output.cpp
@@ -46,6 +46,7 @@
 #include <px4_defines.h>
 #include <geo/geo.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 
 namespace vmount
 {
@@ -186,11 +187,12 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t)
 		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
 	}
 
-	float att[3] = { vehicle_attitude.roll, vehicle_attitude.pitch, vehicle_attitude.yaw };
+	matrix::Quaternion<float> q(&vehicle_attitude.q[0]);
+	matrix::Euler<float> euler(q);
 
 	for (int i = 0; i < 3; ++i) {
 		if (_stabilize[i]) {
-			_angle_outputs[i] = _angle_setpoints[i] - att[i];
+			_angle_outputs[i] = _angle_setpoints[i] - euler(i);
 
 		} else {
 			_angle_outputs[i] = _angle_setpoints[i];