diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 55fc8d4d8a8e847475fcf477d83abeefa1ebcda1..5c4340944af0d692d90de06edefe25e33838cefe 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -714,6 +714,7 @@ void UAS::setWaypoint(Waypoint* wp)
     mavlink_message_t message;
     // FIXME
     //messagePackSetWaypoint(MG::SYSTEM::ID, &message, wp->id, wp->x, wp->y, wp->z, wp->yaw, (wp->autocontinue ? 1 : 0));
+   // mavlink_msg_waypoint_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, wp->name, wp->id,wp->x, wp->y, wp->z, wp->yaw, (wp->autocontinue ? 1 : 0));
     sendMessage(message);
     qDebug() << "UAS SENT Waypoint " << wp->id;
 }