diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc
index 050ef7a4bddbec6f10f94df97088bd444fb24112..53c1b7c4eae071d64bcf938431c88256938bf64f 100644
--- a/src/comm/MAVLinkSimulationLink.cc
+++ b/src/comm/MAVLinkSimulationLink.cc
@@ -380,7 +380,6 @@ void MAVLinkSimulationLink::mainloop()
         // Send back new setpoint
         mavlink_message_t ret;
         mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, MAV_FRAME_LOCAL_NED, spX, spY, spZ, spYaw); // spYaw/180.0*M_PI);
-        mavlink_msg_local_position_setpoint_pack(systemId, componentId, &ret, spX, spY, spZ,
         bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
         //add data into datastream
         memcpy(stream+streampointer,buffer, bufferlength);
diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index bbc31e710826209178fd7569c472259cd0f3fdb3..a587fb108d1018e69fc1cbec53ec3bf476aac444 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -957,7 +957,7 @@ void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw)
 {
 #ifdef MAVLINK_ENABLED_PIXHAWK
     mavlink_message_t msg;
-    mavlink_msg_local_position_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw/M_PI*180.0);
+    mavlink_msg_set_local_position_setpoint_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_FRAME_LOCAL_NED, x, y, z, yaw/M_PI*180.0);
     sendMessage(msg);
 #else
     Q_UNUSED(x);