From 6865a70dea821c93517d516977ac7f6634831dad Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Mon, 5 Dec 2016 19:01:02 +0100 Subject: [PATCH] reset position setpoints once altitude condition is reached --- 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 8edc98c11e..5b6229cf91 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -925,9 +925,10 @@ MulticopterPositionControl::control_manual(float dt) /* check for pos. hold */ if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { if (!_pos_hold_engaged) { - if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy - && fabsf(_vel(1)) < _params.hold_max_xy)) { + if (_params.hold_max_xy < FLT_EPSILON || (sqrtf(_vel(0)*_vel(0) + _vel(1)*_vel(1)) < _params.hold_max_xy)) { _pos_hold_engaged = true; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); } else { _pos_hold_engaged = false; @@ -955,6 +956,7 @@ MulticopterPositionControl::control_manual(float dt) if (!_alt_hold_engaged) { if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { _alt_hold_engaged = true; + _pos_sp(2) = _pos(2); } else { _alt_hold_engaged = false;