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 c09678a9f4..55dbc1546a 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1375,10 +1375,11 @@ MulticopterPositionControl::task_main()
 					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
 				}
 
-				// do not go slower than the follow target velocity when position tracking is active
+				// do not go slower than the follow target velocity when position tracking is active (set to valid)
 
                 if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
-                   _pos_sp_triplet.current.velocity_valid) {
+                   _pos_sp_triplet.current.velocity_valid &&
+                   _pos_sp_triplet.current.position_valid) {
 
                     math::Vector<3> ft_vel(_pos_sp_triplet.current.vx, _pos_sp_triplet.current.vy, 0);
 
@@ -1397,16 +1398,20 @@ MulticopterPositionControl::task_main()
 
                     _vel_sp(0) = fabs(ft_vel(0)) > fabs(_vel_sp(0)) ? ft_vel(0) : _vel_sp(0);
                     _vel_sp(1) = fabs(ft_vel(1)) > fabs(_vel_sp(1)) ? ft_vel(1) : _vel_sp(1);
-                }
 
-                // Ignore position control calculated vel set points
+                    // track target using velocity only
+
+                } else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET &&
+                           _pos_sp_triplet.current.velocity_valid) {
 
-                if(_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_VELOCITY &&
-                   _pos_sp_triplet.current.velocity_valid) {
                     _vel_sp(0) = _pos_sp_triplet.current.vx;
                     _vel_sp(1) = _pos_sp_triplet.current.vy;
                 }
 
+                if (_run_alt_control) {
+                    _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
+                }
+
                 /* make sure velocity setpoint is saturated in xy*/
 
                 float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +  _vel_sp(1) * _vel_sp(1));
@@ -1417,11 +1422,8 @@ MulticopterPositionControl::task_main()
                     _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
                 }
 
-				if (_run_alt_control) {
-					_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
-				}
-
 				/* make sure velocity setpoint is saturated in z*/
+
 				float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
 
 				if (vel_norm_z > _params.vel_max(2)) {
diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
index 0223774fa9..aeb88a78fc 100644
--- a/src/modules/navigator/follow_target.cpp
+++ b/src/modules/navigator/follow_target.cpp
@@ -135,7 +135,7 @@ void FollowTarget::on_active()
             _follow_target_state = TRACK_VELOCITY;
         } else {
             track_target_position();
-        }
+       }
         break;
     }
     case TRACK_VELOCITY: {
@@ -175,7 +175,7 @@ void FollowTarget::track_target_position()
 
     // keep the current velocity updated
 
-    _current_vel =  _target_vel;
+    _current_vel = _target_vel;
 }
 
 void FollowTarget::track_target_velocity()
@@ -271,16 +271,20 @@ void FollowTarget::update_position_sp(math::Vector<3> & vel)
     struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
     // activate line following in pos control
+
     pos_sp_triplet->previous.valid = true;
     pos_sp_triplet->previous = pos_sp_triplet->current;
     mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+    pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
 
     switch(_follow_target_state) {
     case TRACK_POSITION:
-        pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
+        pos_sp_triplet->current.position_valid = true;
+        pos_sp_triplet->current.velocity_valid = true;
         break;
     case TRACK_VELOCITY:
-        pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_VELOCITY;
+        pos_sp_triplet->current.position_valid = false;
+        pos_sp_triplet->current.velocity_valid = true;
         break;
     default:
         break;
@@ -288,7 +292,6 @@ void FollowTarget::update_position_sp(math::Vector<3> & vel)
 
     pos_sp_triplet->current.vx = vel(0);
     pos_sp_triplet->current.vy = vel(1);
-    pos_sp_triplet->current.velocity_valid = true;
     pos_sp_triplet->next.valid = false;
 
     _navigator->set_position_setpoint_triplet_updated();
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 15d1eb343a..eef6a303d3 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -374,10 +374,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 		if(_navigator->get_vstatus()->is_vtol && _param_vtol_wv_loiter.get()){
 			sp->disable_mc_yaw_control = true;
 		}
-		break;// get rid of this
-	case NAV_CMD_FOLLOW_TARGET:
-	    sp->type = position_setpoint_s::SETPOINT_TYPE_VELOCITY;
-	    break;
+		break;
 
 	default:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;