From 330941c1fc59929ad73a65a344f19e567eabf14e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Fri, 9 Nov 2018 08:25:50 +0100
Subject: [PATCH] FlightTaskManualAltitude: initialze yaw setpoint with NAN

Also update some comments.

In case we activate the task and don't have a locked yaw, we should
initialize the yaw setpoint with NAN to avoid any abrupt changes.
---
 .../tasks/ManualAltitude/FlightTaskManualAltitude.cpp       | 6 ++----
 .../tasks/ManualAltitude/FlightTaskManualAltitude.hpp       | 2 +-
 2 files changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index 2b422b5247..5cbc4636b7 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -51,7 +51,7 @@ bool FlightTaskManualAltitude::updateInitialize()
 bool FlightTaskManualAltitude::activate()
 {
 	bool ret = FlightTaskManual::activate();
-	_yaw_setpoint = _yaw;
+	_yaw_setpoint = NAN;
 	_yawspeed_setpoint = 0.0f;
 	_thrust_setpoint = matrix::Vector3f(0.0f, 0.0f, NAN); // altitude is controlled from position/velocity
 	_position_setpoint(2) = _position(2);
@@ -82,11 +82,9 @@ bool FlightTaskManualAltitude::activate()
 
 void FlightTaskManualAltitude::_scaleSticks()
 {
-	/* Scale sticks to yaw and thrust using
-	 * linear scale for yaw and piecewise linear map for thrust. */
+	// Use sticks input with deadzone and exponential curve for vertical velocity and yawspeed
 	_yawspeed_setpoint = _sticks_expo(3) * math::radians(MPC_MAN_Y_MAX.get());
 
-	// scale horizontal velocity with expo curve stick input
 	const float vel_max_z = (_sticks(2) > 0.0f) ? _constraints.speed_down : _constraints.speed_up;
 	_velocity_setpoint(2) = vel_max_z * _sticks_expo(2);
 }
diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
index 491a17c215..474c0cce6b 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -71,7 +71,7 @@ protected:
 					(ParamFloat<px4::params::MPC_HOLD_MAX_Z>) MPC_HOLD_MAX_Z,
 					(ParamInt<px4::params::MPC_ALT_MODE>) MPC_ALT_MODE,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) MPC_HOLD_MAX_XY,
-					(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P,
+					(ParamFloat<px4::params::MPC_Z_P>) MPC_Z_P, /**< position controller altitude propotional gain */
 					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) MPC_MAN_Y_MAX, /**< scaling factor from stick to yaw rate */
 					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) MPC_MAN_TILT_MAX /**< maximum tilt allowed for manual flight */
 				       )