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 4d0c3216a5..b73becf3c2 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -685,6 +685,18 @@ MulticopterPositionControl::task_main()
 				_pos_sp(1) = _offboard_control_pos_sp.y;
 				_pos_sp(2) = _offboard_control_pos_sp.z;
 				_att_sp.yaw_body = _offboard_control_pos_sp.yaw;
+
+				/* Make sure position control is selected i.e. not only velocity control */
+				if (_control_mode.flag_control_position_enabled) {
+				_pos_sp(0) = _offboard_control_pos_sp.x;
+				_pos_sp(1) = _offboard_control_pos_sp.y;
+				}
+
+				if (_control_mode.flag_control_altitude_enabled) {
+				_pos_sp(2) = _offboard_control_pos_sp.z;
+				}
+
+				_att_sp.yaw_body = _offboard_control_pos_sp.yaw;
 			
 			} else {
 				/* AUTO */