waypoints

src/uas/UAS.cc

@@ -379,24 +379,33 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             {
                 mavlink_waypoint_count_t wpc;
                 mavlink_msg_waypoint_count_decode(&message, &wpc);
+                if (wpc.target_system == mavlink->getSystemId() && wpc.target_component == mavlink->getComponentId())
+                {
                     waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
                 }
+            }
             break;
 
         case MAVLINK_MSG_ID_WAYPOINT:
             {
                 mavlink_waypoint_t wp;
                 mavlink_msg_waypoint_decode(&message, &wp);
+                if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId())
+                {
                     waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
                 }
+            }
             break;
 
         case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
             {
                 mavlink_waypoint_request_t wpr;
                 mavlink_msg_waypoint_request_decode(&message, &wpr);
+                if(wpr.target_system == mavlink->getSystemId() && wpr.target_component == mavlink->getComponentId())
+                {
                     waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
                 }
+            }
             break;
 
         case MAVLINK_MSG_ID_WAYPOINT_REACHED:

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)