From 91d2ad17b773446da135f5eca18ec16256448564 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Sun, 16 Apr 2017 16:41:32 -0700 Subject: [PATCH] Fix takeoff overshoot (issue #319). --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 ++- 1 file changed, 2 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 c6aefa5ad6..2b40b9029e 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1230,7 +1230,8 @@ MulticopterPositionControl::control_non_manual(float dt) } else { // copter has reached our takeoff speed. split the thrust setpoint up // into an integral part and into a P part - _thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)); + // remembering to remove _params.thr_hover which is added later as a feed-forward in control_position. + _thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)) - _params.thr_hover; _thrust_int(2) = -math::constrain(_thrust_int(2), _params.thr_min, _params.thr_max); _vel_sp_prev(2) = -_params.tko_speed; _takeoff_jumped = true;