From 601da2818b3e73f5fc12a7615c570ee24a4e52d7 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 1 Nov 2015 10:13:55 +0100 Subject: [PATCH] do not set position setpoint too early --- 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 80847d8e47..2c2010e895 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -714,16 +714,18 @@ MulticopterPositionControl::control_manual(float dt) (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) { _pos_hold_engaged = true; + } else { + _pos_hold_engaged = false; } } else { _pos_hold_engaged = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); } /* set requested velocity setpoint */ if (!_pos_hold_engaged) { + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ _vel_sp(0) = req_vel_sp_scaled(0); _vel_sp(1) = req_vel_sp_scaled(1);