From 8bafdba4f9f7b8881ac7150e38e49f38c312e0ef Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Thu, 4 Jan 2018 10:46:43 +0100
Subject: [PATCH] FlightTaskManualStabilized: update comments

---
 .../tasks/FlightTaskManualStabilized.cpp         | 16 +++++++++++-----
 .../tasks/FlightTaskManualStabilized.hpp         |  7 +++----
 2 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
index fba3139a55..8f0f8912f2 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
@@ -61,8 +61,7 @@ bool FlightTaskManualStabilized::activate()
 void FlightTaskManualStabilized::_scaleSticks()
 {
 	/* Scale sticks to yaw and thrust using
-	 * linear scale for yaw and quadratic for thrust.
-	 * TODO: add thrust */
+	 * linear scale for yaw and piecewise linear map for thrust. */
 	_yaw_rate_sp = _sticks(3) * math::radians(_yaw_rate_scaling.get());
 	_throttle = _throttleCurve();;
 }
@@ -84,13 +83,18 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
 
 void FlightTaskManualStabilized::_updateThrustSetpoints()
 {
-	/* Body frame */
 	/* 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);
 	const float x = sp(0) * math::radians(_tilt_max_man.get());
 	const float y = sp(1) * math::radians(_tilt_max_man.get());
 
+	/* The norm of the xy stick input provides the pointing
+	 * direction of the horizontal desired thrust setpoint. The magnitude of the
+	 * xy stick inputs represents the desired tilt. Both tilt and magnitude can
+	 * be captured through Axis-Angle.
+	 */
+	/* The Axis-Angle is the perpendicular vector to xy-stick input */
 	matrix::Vector2f v = matrix::Vector2f(y, -x);
 	float v_norm = v.norm(); // the norm of v defines the tilt angle
 
@@ -98,6 +102,9 @@ void FlightTaskManualStabilized::_updateThrustSetpoints()
 		v *= _tilt_max_man.get() / v_norm;
 	}
 
+	/* 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;
 }
@@ -108,10 +115,9 @@ void FlightTaskManualStabilized::_updateSetpoints()
 	_updateThrustSetpoints();
 }
 
-/* TODO: add quadratic funciton */
 float FlightTaskManualStabilized::_throttleCurve()
 {
-	/* Scale stick z from [-1,1] to [min thr, max thr]
+	/* Scale stick z from [-1,1] to [min thrust, max thrust]
 	 * with hover throttle at 0.5 stick */
 	float throttle = -((_sticks_expo(2) - 1.0f) * 0.5f);
 
diff --git a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
index 72774d734d..ff1c3003b3 100644
--- a/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
+++ b/src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
@@ -36,7 +36,6 @@
  *
  * Flight task for manual controlled attitude.
  * It generates thrust and yaw setpoints.
- * TODO: add thrust
  */
 
 #pragma once
@@ -64,15 +63,15 @@ protected:
 
 private:
 
-	float _throttle{};
+	float _throttle{}; /** Mapped from stick z. */
 
 	void _updateHeadingSetpoints(); /**< Sets yaw or yaw speed. */
 	void _updateThrustSetpoints(); /**< Sets thrust setpoint */
-	float _throttleCurve(); /**< piecewise linear mapping for throttle */
+	float _throttleCurve(); /**< Piecewise linear mapping from stick to throttle. */
 
 	control::BlockParamFloat _yaw_rate_scaling; /**< Scaling factor from stick to yaw rate. */
 	control::BlockParamFloat _tilt_max_man; /**< Maximum tilt allowed for manual flight */
 	control::BlockParamFloat _throttle_min; /**< Minimum throttle that always has to be satisfied in flight*/
 	control::BlockParamFloat _throttle_max; /**< Maximum throttle that always has to be satisfied in flight*/
-	control::BlockParamFloat _throttle_hover; /**< Throttel value at which vehicle is at hover equilibrium */
+	control::BlockParamFloat _throttle_hover; /**< Throttle value at which vehicle is at hover equilibrium */
 };