From f35e3335be8092d14136c82f35f48c253e0b250a Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 27 Jan 2016 11:15:22 +0100 Subject: [PATCH] - only use takeoff strategy when in auto mode - after jumped takeoff set previous vel setpoint such that thrust setpoint is continuous --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- 1 file changed, 2 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 f4287903e7..1b8503a790 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1377,7 +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); + _vel_sp_prev(2) = -_params.tko_speed; _takeoff_jumped = true; reset_int_z = false; } @@ -1472,7 +1472,7 @@ MulticopterPositionControl::task_main() math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF - && !_takeoff_jumped) { + && !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) { // for jumped takeoffs use special thrust setpoint calculated above thrust_sp.zero(); thrust_sp(2) = -_takeoff_thrust_sp;