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 870f6bec1b..3020179283 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1277,8 +1277,6 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c
 	math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
 	float cd_len = (sphere_c - d).length();
 
-	//PX4_INFO("sphere_r: %.5f",(double)sphere_r);
-	//PX4_INFO("cd len: %.5f",(double)cd_len);
 	if (sphere_r > cd_len) {
 		/* we have triangle CDX with known CD and CX = R, find DX */
 		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
@@ -1296,20 +1294,16 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, c
 
 	} else {
 
-		PX4_INFO("return false");
 		/* have no roots, return D */
 		res = d; /* go directly to line */
 
 		/* previous waypoint is still in front of us */
 		if ((sphere_c - line_a) * ab_norm < 0.0f) {
-			PX4_INFO("previous is in front");
 			res = line_a;
 		}
 
 		/* target waypoint is already behind us */
 		if ((sphere_c - line_b) * ab_norm > 0.0f) {
-			PX4_INFO("target is behind");
-
 			res = line_b;
 		}
 
@@ -1426,15 +1420,15 @@ void MulticopterPositionControl::control_auto(float dt)
 
 				/* unit vector from previous to current */
 				matrix::Vector2f unit_prev_to_current((_curr_pos_sp(0) - prev_sp(0)), (_curr_pos_sp(1) - prev_sp(1)));
-				unit_prev_to_current.normalize();
+				unit_prev_to_current = unit_prev_to_current.normalized();
 
 				/* orthogonal distance from current position to unit_prev_to_current */
 				matrix::Vector2f closest_point = matrix::Vector2f(prev_sp(0), prev_sp(1)) + unit_prev_to_current *
 								 (matrix::Vector2f((_pos(0) - prev_sp(0)), (_pos(1) - prev_sp(1))) * unit_prev_to_current);
 
 				/* check if we need to adjust position setpoint based on cruise velocity */
-				matrix::Vector2f vec_pos_to_current = matrix::Vector2f((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1)));
-				matrix::Vector2f vec_prev_to_pos = matrix::Vector2f((_pos(0) - prev_sp(0)), (_pos(1) - prev_sp(1)));
+				matrix::Vector2f vec_pos_to_current((_curr_pos_sp(0) - _pos(0)), (_curr_pos_sp(1) - _pos(1)));
+				matrix::Vector2f vec_prev_to_pos((_pos(0) - prev_sp(0)), (_pos(1) - prev_sp(1)));
 
 				/* slow down */
 				if (vec_pos_to_current.length() < _target_threshold_xy.get()) {
@@ -1448,10 +1442,12 @@ void MulticopterPositionControl::control_auto(float dt)
 								       unit_current_to_next;
 
 						/* angle goes from 0 to 2 with 0 being  large angle, 2 being tight angle:   0 = PI ; 2 = PI*0 */
-						float angle = unit_current_to_next * (-unit_prev_to_current) + 1.0f;
+						float angle = unit_current_to_next * (unit_prev_to_current * -1.0f) + 1.0f;
 
 						/* velocity close to target adjusted to angle
-						 * TODO: add explanation*/
+						 * vel_close = a *b ^x + c; where at angle = 0 -> vel_close = vel_cruise; angle = 1 -> vel_cruise/4.0 (this means that at 90degrees
+						 * the velocity at target should be 1/4 * cruising speed;
+						 * angle = 2 -> vel_close = min_cruising_speed */
 						float M = get_cruising_speed_xy() / 4.0f;
 						float a = -((M - get_cruising_speed_xy()) * (M - get_cruising_speed_xy())) / (2.0f * M - get_cruising_speed_xy() -
 								_min_cruise_speed.get());
@@ -1469,10 +1465,8 @@ void MulticopterPositionControl::control_auto(float dt)
 						cruising_speed_xy =  slope * vec_pos_to_current.length() + _min_cruise_speed.get();
 					}
 
-
-					/* accelerate */
-
 				} else if (vec_prev_to_pos.length() < _target_threshold_xy.get()) {
+					/* accelerate */
 					float slope = (get_cruising_speed_xy() - _min_cruise_speed.get())  / _target_threshold_xy.get();
 					cruising_speed_xy =  slope * vec_prev_to_pos.length() + _min_cruise_speed.get();
 				}
@@ -1485,12 +1479,31 @@ void MulticopterPositionControl::control_auto(float dt)
 					matrix::Vector2f vec_pos_to_closest = closest_point - matrix::Vector2f(_pos(0), _pos(1));
 					float vel_orthogonal = vec_pos_to_closest.length() * _params.pos_p(0);
 
+					/* we split position error vector into along track and orthogonal such that cruise speed is never exceeded */
 					if (vel_orthogonal <= cruising_speed_xy) {
 						float vel_along_track = sqrtf(cruising_speed_xy * cruising_speed_xy - vel_orthogonal * vel_orthogonal);
 						pos_sp(0) = closest_point(0) + unit_prev_to_current(0) * vel_along_track / _params.pos_p(0);
 						pos_sp(1) = closest_point(1) + unit_prev_to_current(1) * vel_along_track / _params.pos_p(0);
 
 					} else {
+						/* we are more then cruisespeed away from track */
+						/* check on which section we are with default closest point */
+						if ((vec_prev_to_pos * unit_prev_to_current) < 0.0f) {
+							/* previous is in front */
+							vec_pos_to_closest(0) = prev_sp(0);
+							vec_pos_to_closest(1) = prev_sp(1);
+						}
+
+						if (((vec_pos_to_current * -1.0f) * unit_prev_to_current) > 0.0f) {
+							/* we already passed current_sp */
+							vec_pos_to_closest(0) = _curr_pos_sp(0);
+							vec_pos_to_closest(1) = _curr_pos_sp(1);
+
+							/* we dont want to limit cruise velocity */
+							cruising_speed_xy = get_cruising_speed_xy();
+
+						}
+
 						pos_sp(0) = _pos(0) + vec_pos_to_closest(0) / vec_pos_to_closest.length() * cruising_speed_xy / _params.pos_p(0);
 						pos_sp(1) = _pos(1) + vec_pos_to_closest(1) / vec_pos_to_closest.length() * cruising_speed_xy / _params.pos_p(1);