From 50fbb567379af8e2b849fdb5405572f38de124f4 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 5 Aug 2019 13:28:14 +0200 Subject: [PATCH] Yaw mode 3 - Add check for minimum distance to target in MPC_YAW_MODE 3 (along trajectory) to be consistent with the other yaw modes --- .../tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp index f538149f35..9c2fc56efa 100644 --- a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp @@ -121,9 +121,12 @@ bool FlightTaskAutoLineSmoothVel::_generateHeadingAlongTraj() { bool res = false; Vector2f vel_sp_xy(_velocity_setpoint); + Vector2f traj_to_target = Vector2f(_target) - Vector2f(_position); - if (vel_sp_xy.length() > .1f) { + if ((vel_sp_xy.length() > .1f) && + (traj_to_target.length() > _target_acceptance_radius)) { // Generate heading from velocity vector, only if it is long enough + // and if the drone is far enough from the target _compute_heading_from_2D_vector(_yaw_setpoint, vel_sp_xy); res = true; }