From 421aeb69d85a4962a5f92d96aae3f93cbf2840d1 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis@yuneecresearch.com>
Date: Thu, 22 Feb 2018 08:33:48 +0100
Subject: [PATCH] FlightTaskManual: transformation into heading frame use
 vehicle yaw or yaw_sp based on vehicle rotation in yaw

---
 .../tasks/FlightTaskManualAltitude.cpp        |  4 ++--
 .../tasks/FlightTaskManualPosition.cpp        |  6 +++---
 .../tasks/FlightTaskManualStabilized.cpp      | 19 +++++++++++++------
 .../tasks/FlightTaskManualStabilized.hpp      |  1 +
 4 files changed, 19 insertions(+), 11 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
index 65bdb234bf..fa231d9476 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualAltitude.cpp
@@ -94,8 +94,8 @@ void FlightTaskManualAltitude::_updateSetpoints()
 	 * thrust along xy is required. The maximum thrust along xy depends on the thrust
 	 * setpoint along z-direction, which is computed in PositionControl.cpp.
 	 */
-	Vector3f sp{_sticks(0), _sticks(1), 0.0f};
-	sp = (Dcmf(Eulerf(0.0f, 0.0f, _yaw_sp)) * sp);
+	Vector2f sp{_sticks(0), _sticks(1)};
+	_rotateIntoHeadingFrame(sp);
 
 	if (sp.length() > 1.0f) {
 		sp.normalize();
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
index 0251cad327..2067b5c04c 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp
@@ -73,9 +73,9 @@ void FlightTaskManualPosition::_scaleSticks()
 	Vector2f vel_sp_xy = stick_xy * _vel_xy_manual_max.get();
 
 	/* Rotate setpoint into local frame. */
-	Vector3f vel_local = (Dcmf(Eulerf(0.0f, 0.0f, _yaw_sp)) * Vector3f(vel_sp_xy(0), vel_sp_xy(1), 0.0f));
-	_vel_sp(0) = vel_local(0);
-	_vel_sp(1) = vel_local(1);
+	_rotateIntoHeadingFrame(vel_sp_xy);
+	_vel_sp(0) = vel_sp_xy(0);
+	_vel_sp(1) = vel_sp_xy(1);
 }
 
 void FlightTaskManualPosition::_updateXYlock()
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
index 57c63ba692..4b3882bae8 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
@@ -85,9 +85,8 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
 void FlightTaskManualStabilized::_updateThrustSetpoints()
 {
 	/* Rotate setpoint into local frame. */
-	matrix::Vector3f sp{_sticks(0), _sticks(1), 0.0f};
-
-	sp = (matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, _yaw_sp)) * sp);
+	Vector2f sp{_sticks(0), _sticks(1)};
+	_rotateIntoHeadingFrame(sp);
 
 	/* Ensure that maximum tilt is in [0.001, Pi] */
 	float tilt_max = math::constrain(math::radians(_tilt_max_man.get()), 0.001f, M_PI_F);
@@ -101,7 +100,7 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
 	 * be captured through Axis-Angle.
 	 */
 	/* The Axis-Angle is the perpendicular vector to xy-stick input */
-	matrix::Vector2f v = matrix::Vector2f(y, -x);
+	Vector2f v = Vector2f(y, -x);
 	float v_norm = v.norm(); // the norm of v defines the tilt angle
 
 	if (v_norm > tilt_max) { // limit to the configured maximum tilt angle
@@ -111,8 +110,16 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
 	/* The final thrust setpoint is found by rotating the scaled unit vector pointing
 	 * upward by the Axis-Angle.
 	 */
-	matrix::Quatf q_sp = matrix::AxisAnglef(v(0), v(1), 0.0f);
-	_thr_sp = q_sp.conjugate(matrix::Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
+	Quatf q_sp = AxisAnglef(v(0), v(1), 0.0f);
+	_thr_sp = q_sp.conjugate(Vector3f(0.0f, 0.0f, -1.0f)) * _throttle;
+}
+
+void FlightTaskManualStabilized::_rotateIntoHeadingFrame(Vector2f &v)
+{
+	float yaw_rotate = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw;
+	Vector3f v_r = Vector3f(Dcmf(Eulerf(0.0f, 0.0f, yaw_rotate)) * Vector3f(v(0), v(1), 0.0f));
+	v(0) = v_r(0);
+	v(1) = v_r(1);
 }
 
 void FlightTaskManualStabilized::_updateSetpoints()
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
index e31c0303f4..921597a61a 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
@@ -56,6 +56,7 @@ public:
 protected:
 	virtual void _updateSetpoints(); /**< updates all setpoints*/
 	virtual void _scaleSticks(); /**< scales sticks to yaw and thrust */
+	void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */
 
 private: