From c2e024665017208d84c33b54c599bbc811a13406 Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Fri, 12 Aug 2016 00:19:54 +0200
Subject: [PATCH] MC pos control: realize when we are on the trajectory but
 passed the target waypoint

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 5 +++++
 1 file changed, 5 insertions(+)

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 07a13908a1..ba740b59dd 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1127,6 +1127,11 @@ void MulticopterPositionControl::control_auto(float dt)
 
 					if (near) {
 						/* unit sphere crosses trajectory */
+						/* if copter is in front of curr waypoint, go directly to curr waypoint */
+						if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
+							pos_sp_s = curr_sp_s;
+							pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
+						}
 
 					} else {
 						/* copter is too far from trajectory */