diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index a2b4a8374980f67cbf39bb1da7cb2e326e4aff2a..ebac53ac576bbbc94d6f1a985d0100e571875875 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -2329,7 +2329,9 @@ void UAS::shutdown()
 void UAS::setTargetPosition(float x, float y, float z, float yaw)
 {
     mavlink_message_t msg;
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 2, 3, 0, yaw, x, y, z);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 1, 0, 0, yaw, x, y, z);
+    sendMessage(msg);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_NAV_PATHPLANNING, 1, 0, 1, 0, yaw, x, y, z);
     sendMessage(msg);
 }