From d4303c21ef09eece26669e956751342a8eaf4316 Mon Sep 17 00:00:00 2001
From: Mariano Lizarraga <malife@gmail.com>
Date: Mon, 29 Nov 2010 17:17:53 -0600
Subject: [PATCH] Trying to get the waypoint protocol to work inside SlugsMav
so not to call UAS::receiveMessage()
---
src/uas/SlugsMAV.cc | 62 ++++++++++++++++++++++++++++++++++++++-------
1 file changed, 53 insertions(+), 9 deletions(-)
diff --git a/src/uas/SlugsMAV.cc b/src/uas/SlugsMAV.cc
index 19beb486ad..c09ef12a55 100644
--- a/src/uas/SlugsMAV.cc
+++ b/src/uas/SlugsMAV.cc
@@ -59,16 +59,14 @@ SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) :
void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
// Let UAS handle the default message set
- //UAS::receiveMessage(link, message);
+ // UAS::receiveMessage(link, message);
// Handle your special messages mavlink_message_t* msg = &message;
switch (message.msgid)
{
-
-
- case MAVLINK_MSG_ID_HEARTBEAT:
+ case MAVLINK_MSG_ID_HEARTBEAT:
emit heartbeat(this);
// Set new type if it has changed
if (this->type != mavlink_msg_heartbeat_get_type(&message))
@@ -80,7 +78,57 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
case MAVLINK_MSG_ID_RAW_IMU:
mavlink_msg_raw_imu_decode(&message, &mlRawImuData);
- break;
+ break;
+
+ case MAVLINK_MSG_ID_WAYPOINT_COUNT:
+ mavlink_waypoint_count_t wpct;
+ mavlink_msg_waypoint_count_decode(&message, &wpct);
+ if (wpct.target_system == mavlink->getSystemId() && wpct.target_component == mavlink->getComponentId())
+ {
+ waypointManager.handleWaypointCount(message.sysid, message.compid, wpct.count);
+ }
+ break;
+
+ case MAVLINK_MSG_ID_WAYPOINT:
+ mavlink_waypoint_t wp;
+ mavlink_msg_waypoint_decode(&message, &wp);
+ //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z;
+ if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId())
+ {
+ waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
+ }
+ break;
+
+ case MAVLINK_MSG_ID_WAYPOINT_ACK:
+ mavlink_waypoint_ack_t wpa;
+ mavlink_msg_waypoint_ack_decode(&message, &wpa);
+ if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId())
+ {
+ waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
+ }
+ break;
+
+ case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
+ mavlink_waypoint_request_t wpr;
+ mavlink_msg_waypoint_request_decode(&message, &wpr);
+ qDebug() << "Waypoint Request" << wpr.seq;
+ 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:
+ mavlink_waypoint_reached_t wpre;
+ mavlink_msg_waypoint_reached_decode(&message, &wpre);
+ waypointManager.handleWaypointReached(message.sysid, message.compid, &wpre);
+ break;
+
+ case MAVLINK_MSG_ID_WAYPOINT_CURRENT:
+ mavlink_waypoint_current_t wpc;
+ mavlink_msg_waypoint_current_decode(&message, &wpc);
+ waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc);
+ break;
#ifdef MAVLINK_ENABLED_SLUGS
@@ -89,8 +137,6 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit slugsBootMsg(uasId, mlBoot);
break;
-
-
case MAVLINK_MSG_ID_ATTITUDE:
mavlink_msg_attitude_decode(&message, &mlAttitude);
break;
@@ -103,8 +149,6 @@ void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_msg_action_ack_decode(&message,&mlActionAck);
break;
-
-
case MAVLINK_MSG_ID_CPU_LOAD: //170
mavlink_msg_cpu_load_decode(&message,&mlCpuLoadData);
break;
--
GitLab