From f794ee0c8a1ed033549e436ddd10031dc97839eb Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Fri, 1 Mar 2019 22:51:46 +0100
Subject: [PATCH] FlightTaskManualAltitude: add slow upwards start

when still close to ground
---
 .../ManualAltitude/FlightTaskManualAltitude.cpp     | 13 ++++++++-----
 .../ManualAltitude/FlightTaskManualAltitude.hpp     |  7 ++++---
 2 files changed, 12 insertions(+), 8 deletions(-)

diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index cb2f28deb6..e8eb46b5e7 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -277,12 +277,15 @@ void FlightTaskManualAltitude::_respectGroundSlowdown()
 		dist_to_ground = -(_position(2) - _sub_home_position->get().z);
 	}
 
-	// limit downwards speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
+	// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
 	if (PX4_ISFINITE(dist_to_ground)) {
-		const float slowdown_limit = math::gradual(dist_to_ground,
-					     MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
-					     MPC_LAND_SPEED.get(), _constraints.speed_down);
-		_velocity_setpoint(2) = math::min(_velocity_setpoint(2), slowdown_limit);
+		const float limit_down = math::gradual(dist_to_ground,
+						       MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
+						       MPC_LAND_SPEED.get(), _constraints.speed_down);
+		const float limit_up = math::gradual(dist_to_ground,
+						     MPC_LAND_ALT2.get(), MPC_LAND_ALT1.get(),
+						     MPC_TKO_SPEED.get(), _constraints.speed_up);
+		_velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down);
 	}
 }
 
diff --git a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
index e3c4bed999..8cd75f493d 100644
--- a/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
+++ b/src/lib/FlightTasks/tasks/ManualAltitude/FlightTaskManualAltitude.hpp
@@ -76,9 +76,10 @@ protected:
 					(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 */
-					(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, // altitude at which to start downwards slowdown
-					(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, // altitude below wich to land with land speed
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED
+					(ParamFloat<px4::params::MPC_LAND_ALT1>) MPC_LAND_ALT1, /**< altitude at which to start downwards slowdown */
+					(ParamFloat<px4::params::MPC_LAND_ALT2>) MPC_LAND_ALT2, /**< altitude below wich to land with land speed */
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) MPC_LAND_SPEED, /**< desired downwards speed when approaching the ground */
+					(ParamFloat<px4::params::MPC_TKO_SPEED>) MPC_TKO_SPEED /**< desired upwards speed when still close to the ground */
 				       )
 private:
 	/**