FlightTaskManualAltitude: add slow upwards start

when still close to ground

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);
 	}
 }
 

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:
 	/**