From 6538a7e88c3d06f7048349be8ec5f94ee9b42aa9 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Thu, 15 Feb 2018 11:14:33 +0100
Subject: [PATCH] FlightTaskSport: replace rotation by axis angle

---
 src/lib/FlightTasks/tasks/FlightTaskSport.hpp | 11 +++++++----
 1 file changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
index cf2c29d1e7..c16b9cff51 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskSport.hpp
@@ -43,6 +43,8 @@
 #include "mathlib/mathlib.h"
 #include <float.h>
 
+using namespace matrix;
+
 class FlightTaskSport : public FlightTaskManualPosition
 {
 public:
@@ -70,12 +72,13 @@ protected:
 		}
 
 		/* Scale to velocity using max velocity */
-		_vel_sp_xy = stick_xy * _vel_xy_max.get();
+		Vector2f vel_sp_xy = stick_xy * _vel_xy_max.get();
 
 		/* Rotate setpoint into local frame. */
-		matrix::Vector3f vel_sp { _vel_sp_xy(0), _vel_sp_xy(1), 0.0f };
-		vel_sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw)) * vel_sp);
-		_vel_sp_xy = matrix::Vector2f(vel_sp(0), vel_sp(1));
+		matrix::Quatf q_yaw = matrix::AxisAnglef(matrix::Vector3f(0.0f, 0.0f, 1.0f), _yaw);
+		matrix::Vector3f vel_world = q_yaw.conjugate(matrix::Vector3f(vel_sp_xy(0), vel_sp_xy(1), 0.0f));
+		_vel_sp(0) = vel_world(0);
+		_vel_sp(1) = vel_world(1);
 	}
 
 private: