diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc
index 56e3cd170f49ea378a0fc71ad727a8867355fc60..3ea964c1bb0fe60eb8e5af61511dbc69723f7cea 100644
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -379,7 +379,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_waypoint_count_t wpc;
                 mavlink_msg_waypoint_count_decode(&message, &wpc);
-                waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
+                if (wpc.target_system == mavlink->getSystemId() && wpc.target_component == mavlink->getComponentId())
+                {
+                    waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
+                }
             }
             break;
 
@@ -387,7 +390,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_waypoint_t wp;
                 mavlink_msg_waypoint_decode(&message, &wp);
-                waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
+                if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId())
+                {
+                    waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
+                }
             }
             break;
 
@@ -395,7 +401,10 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_waypoint_request_t wpr;
                 mavlink_msg_waypoint_request_decode(&message, &wpr);
-                waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
+                if(wpr.target_system == mavlink->getSystemId() && wpr.target_component == mavlink->getComponentId())
+                {
+                    waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
+                }
             }
             break;
 
diff --git a/src/uas/UASWaypointManager.cc b/src/uas/UASWaypointManager.cc
index 44943966714aa88fe2f7cbb120609f8cec8b4ed2..38f7151d22dcce214f078ec16b24611c70c3f3ca 100644
--- a/src/uas/UASWaypointManager.cc
+++ b/src/uas/UASWaypointManager.cc
@@ -132,7 +132,15 @@ void UASWaypointManager::requestWaypoints()
 
 void UASWaypointManager::sendWaypoints(void)
 {
+    mavlink_message_t message;
+    mavlink_waypoint_count_t wpc;
 
+    wpc.target_system = uas.getUASID();
+    wpc.target_component = MAV_COMP_ID_WAYPOINTPLANNER;
+    wpc.count = 0;  //TODO
+
+    mavlink_msg_waypoint_count_encode(uas.mavlink->getSystemId(), uas.mavlink->getComponentId(), &message, &wpc);
+    uas.sendMessage(message);
 }
 
 void UASWaypointManager::getWaypoint(quint16 seq)