From ba169ce0b519af67a6f759e550e56c79780cd356 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 26 Jan 2016 18:18:52 +0100 Subject: [PATCH] do not do jumped takeoff if already in air --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 f322924e34..f4287903e7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1360,6 +1360,12 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + // check if we are not already in air. + // if yes then we don't need a jumped takeoff anymore + if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON ) { + _takeoff_jumped = true; + } + if (!_takeoff_jumped) { // ramp thrust setpoint up if (_vel(2) > -(_params.tko_speed / 2.0f)) { @@ -1371,6 +1377,7 @@ MulticopterPositionControl::task_main() // into an integral part and into a P part thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)); thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max); + _vel_sp_prev(2) = _vel(2); _takeoff_jumped = true; reset_int_z = false; } @@ -1378,7 +1385,6 @@ MulticopterPositionControl::task_main() if (_takeoff_jumped) { _vel_sp(2) = -_params.tko_speed; - _vel_sp_prev(2) = -_params.tko_speed; } } else {