From 20a47a1a72a974130c386a286dcddba6628d590d Mon Sep 17 00:00:00 2001 From: Dennis Mannhart Date: Wed, 22 Mar 2017 11:45:32 +0100 Subject: [PATCH] mc_pos_control: limit if just xy distance close to target --- 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 63fc7eddf8..2ddbf97f74 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1500,8 +1500,9 @@ void MulticopterPositionControl::control_auto(float dt) } /* set velocity limit if close to current setpoint and no next setpoint available */ + math::Vector<3> dist = _curr_pos_sp - _pos; _limit_vel_xy = (!next_setpoint_valid || (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) - && ((_curr_pos_sp - _pos).length() <= _target_threshold_xy.get()); + && (sqrtf(dist(0) * dist(0) + dist(1) * dist(1)) <= _target_threshold_xy.get()); if (current_setpoint_valid && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_IDLE)) {