From b5f3c2d30d85d871222f5cf9b8ea1a67f5d3b282 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Tue, 22 Dec 2015 12:20:11 +0100
Subject: [PATCH] ramp up jump velocity instead doing a huge step

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 8ed95e58e6..2891c15cda 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1365,8 +1365,10 @@ MulticopterPositionControl::task_main()
 				    && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
 					if (!_takeoff_jumped) {
-						/* do a quick jump until we shoot over takeoff speed */
-						_vel_sp(2) = -_params.tko_jmpspd;
+						/* do a quick jump (ramp vel sp up in 1/2sec) until we shoot over takeoff speed */
+						float accel = _vel_sp_prev(2) - _params.tko_jmpspd;
+						float tmp = _vel_sp_prev(2) + accel * dt * 2.0f;
+						_vel_sp(2) = math::max(tmp, -_params.tko_jmpspd);
 
 						if (_vel(2) < -_params.tko_speed) {
 							_takeoff_jumped = true;