From 28c989144dd6d01b238a65c02d077471910acb6b Mon Sep 17 00:00:00 2001 From: pixhawk Date: Wed, 9 Jun 2010 16:14:45 +0200 Subject: [PATCH] waypoints --- src/uas/UAS.cc | 15 ++++++++++++--- src/uas/UASWaypointManager.cc | 8 ++++++++ 2 files changed, 20 insertions(+), 3 deletions(-) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 56e3cd170..3ea964c1b 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 449439667..38f7151d2 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) -- 2.22.0