Merge branch 'mission_protocol' of github.com:susurrus/qgroundcontrol into thread_test_integration

src/uas/UAS.cc

@@ -1112,58 +1112,100 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
             break;
         case MAVLINK_MSG_ID_MISSION_COUNT:
         {
-            mavlink_mission_count_t wpc;
+            mavlink_mission_count_t mc;
-            mavlink_msg_mission_count_decode(&message, &wpc);
+            mavlink_msg_mission_count_decode(&message, &mc);
-            if(wpc.target_system == mavlink->getSystemId() || wpc.target_system == 0)
+
+            // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
+            if (mc.target_system == 0) {
+                mc.target_system = mavlink->getSystemId();
+            }
+            if (mc.target_component == 0) {
+                mc.target_component = mavlink->getComponentId();
+            }
+
+            // Check that this message applies to the UAS.
+            if(mc.target_system == mavlink->getSystemId() && mc.target_component == mavlink->getComponentId())
             {
-                waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count);
+                waypointManager.handleWaypointCount(message.sysid, message.compid, mc.count);
             }
             else
             {
-                qDebug() << "Got waypoint message, but was wrong system id" << wpc.target_system;
+                qDebug() << tr("Received mission count message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mc.target_system);
             }
         }
             break;
 
         case MAVLINK_MSG_ID_MISSION_ITEM:
         {
-            mavlink_mission_item_t wp;
+            mavlink_mission_item_t mi;
-            mavlink_msg_mission_item_decode(&message, &wp);
+            mavlink_msg_mission_item_decode(&message, &mi);
-            //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_system == 0)
+            // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
+            if (mi.target_system == 0) {
+                mi.target_system = mavlink->getSystemId();
+            }
+            if (mi.target_component == 0) {
+                mi.target_component = mavlink->getComponentId();
+            }
+
+            // Check that the item pertains to this UAS.
+            if(mi.target_system == mavlink->getSystemId() && mi.target_component == mavlink->getComponentId())
             {
-                waypointManager.handleWaypoint(message.sysid, message.compid, &wp);
+                waypointManager.handleWaypoint(message.sysid, message.compid, &mi);
             }
             else
             {
-                qDebug() << "Got waypoint message, but was wrong system id" << wp.target_system;
+                qDebug() << tr("Received mission item message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mi.target_system);
             }
         }
             break;
 
         case MAVLINK_MSG_ID_MISSION_ACK:
         {
-            mavlink_mission_ack_t wpa;
+            mavlink_mission_ack_t ma;
-            mavlink_msg_mission_ack_decode(&message, &wpa);
+            mavlink_msg_mission_ack_decode(&message, &ma);
-            if((wpa.target_system == mavlink->getSystemId() || wpa.target_system == 0) &&
+
-                    (wpa.target_component == mavlink->getComponentId() || wpa.target_component == 0))
+            // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
+            if (ma.target_system == 0) {
+                ma.target_system = mavlink->getSystemId();
+            }
+            if (ma.target_component == 0) {
+                ma.target_component = mavlink->getComponentId();
+            }
+
+            // Check that the ack pertains to this UAS.
+            if(ma.target_system == mavlink->getSystemId() && ma.target_component == mavlink->getComponentId())
+            {
+                waypointManager.handleWaypointAck(message.sysid, message.compid, &ma);
+            }
+            else
             {
-                waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa);
+                qDebug() << tr("Received mission ack message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(ma.target_system);
             }
         }
             break;
 
         case MAVLINK_MSG_ID_MISSION_REQUEST:
         {
-            mavlink_mission_request_t wpr;
+            mavlink_mission_request_t mr;
-            mavlink_msg_mission_request_decode(&message, &wpr);
+            mavlink_msg_mission_request_decode(&message, &mr);
-            if(wpr.target_system == mavlink->getSystemId() || wpr.target_system == 0)
+
+            // Special case a 0 for the target system or component, it means that anyone is the target, so we should process this.
+            if (mr.target_system == 0) {
+                mr.target_system = mavlink->getSystemId();
+            }
+            if (mr.target_component == 0) {
+                mr.target_component = mavlink->getComponentId();
+            }
+
+            // Check that the request pertains to this UAS.
+            if(mr.target_system == mavlink->getSystemId() && mr.target_component == mavlink->getComponentId())
             {
-                waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr);
+                waypointManager.handleWaypointRequest(message.sysid, message.compid, &mr);
             }
             else
             {
-                qDebug() << "Got waypoint message, but was wrong system id" << wpr.target_system;
+                qDebug() << tr("Received mission request message, but was wrong system id. Expected %1, received %2").arg(mavlink->getSystemId()).arg(mr.target_system);
             }
         }
             break;

src/uas/UASWaypointManager.cc

@@ -921,94 +921,91 @@ void UASWaypointManager::writeWaypoints()
     }
     else
     {
-        //we're in another transaction, ignore command
+        // We're in another transaction, ignore command
-        qDebug() << "UASWaypointManager::sendWaypoints() doing something else ignoring command";
+        qDebug() << tr("UASWaypointManager::sendWaypoints() doing something else. Ignoring command");
     }
 }
 
 void UASWaypointManager::sendWaypointClearAll()
 {
     if (!uas) return;
-    mavlink_message_t message;
-    mavlink_mission_clear_all_t wpca;
-
-    wpca.target_system = uasid;
-    wpca.target_component = MAV_COMP_ID_MISSIONPLANNER;
-
-    emit updateStatusString(QString("Clearing waypoint list..."));
 
+    // Send the message.
+    mavlink_message_t message;
+    mavlink_mission_clear_all_t wpca = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
     mavlink_msg_mission_clear_all_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpca);
-
     uas->sendMessage(message);
+
+    // And update the UI.
+    emit updateStatusString(tr("Clearing waypoint list..."));
+
     QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
 }
 
 void UASWaypointManager::sendWaypointSetCurrent(quint16 seq)
 {
     if (!uas) return;
-    mavlink_message_t message;
-    mavlink_mission_set_current_t wpsc;
-
-    wpsc.target_system = uasid;
-    wpsc.target_component = MAV_COMP_ID_MISSIONPLANNER;
-    wpsc.seq = seq;
-
-    emit updateStatusString(QString("Updating target waypoint..."));
 
+    // Send the message.
+    mavlink_message_t message;
+    mavlink_mission_set_current_t wpsc = {seq, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
     mavlink_msg_mission_set_current_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpsc);
     uas->sendMessage(message);
+
+    // And update the UI.
+    emit updateStatusString(tr("Updating target waypoint..."));
+
     QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
 }
 
 void UASWaypointManager::sendWaypointCount()
 {
     if (!uas) return;
-    mavlink_message_t message;
-    mavlink_mission_count_t wpc;
-
-    wpc.target_system = uasid;
-    wpc.target_component = MAV_COMP_ID_MISSIONPLANNER;
-    wpc.count = current_count;
 
-    emit updateStatusString(QString("Starting to transmit waypoints..."));
 
+    // Tell the UAS how many missions we'll sending.
+    mavlink_message_t message;
+    mavlink_mission_count_t wpc = {current_count, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
     mavlink_msg_mission_count_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpc);
     uas->sendMessage(message);
+
+    // And update the UI.
+    emit updateStatusString(tr("Starting to transmit waypoints..."));
+
     QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
 }
 
 void UASWaypointManager::sendWaypointRequestList()
 {
     if (!uas) return;
-    mavlink_message_t message;
-    mavlink_mission_request_list_t wprl;
 
-    wprl.target_system = uasid;
+    // Send a MISSION_REQUEST message to the uas for this mission manager, using the MISSIONPLANNER component.
-    wprl.target_component = MAV_COMP_ID_MISSIONPLANNER;
+    mavlink_message_t message;
+    mavlink_mission_request_list_t wprl = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
+    mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl);
+    uas->sendMessage(message);
 
+    // And update the UI.
     QString statusMsg(tr("Requesting waypoint list..."));
     qDebug() << __FILE__ << __LINE__ << statusMsg;
     emit updateStatusString(statusMsg);
 
-    mavlink_msg_mission_request_list_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wprl);
-    uas->sendMessage(message);
     QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
 }
 
 void UASWaypointManager::sendWaypointRequest(quint16 seq)
 {
     if (!uas) return;
-    mavlink_message_t message;
-    mavlink_mission_request_t wpr;
-
-    wpr.target_system = uasid;
-    wpr.target_component = MAV_COMP_ID_MISSIONPLANNER;
-    wpr.seq = seq;
-
-    emit updateStatusString(QString("Retrieving waypoint ID %1 of %2 total").arg(wpr.seq).arg(current_count));
 
+    // Send a MISSION_REQUEST message to the UAS's MISSIONPLANNER component.
+    mavlink_message_t message;
+    mavlink_mission_request_t wpr = {seq, (quint8)uasid, MAV_COMP_ID_MISSIONPLANNER};
     mavlink_msg_mission_request_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpr);
     uas->sendMessage(message);
+
+    // And update the UI.
+    emit updateStatusString(tr("Retrieving waypoint ID %1 of %2").arg(wpr.seq).arg(current_count));
+
     QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
 }
 
@@ -1019,18 +1016,18 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
 
     if (seq < waypoint_buffer.count()) {
 
-        mavlink_mission_item_t *wp;
+        // Fetch the current mission to send, and set it to apply to the curent UAS.
-
+        mavlink_mission_item_t *wp = waypoint_buffer.at(seq);
-
-        wp = waypoint_buffer.at(seq);
         wp->target_system = uasid;
         wp->target_component = MAV_COMP_ID_MISSIONPLANNER;
 
-        emit updateStatusString(QString("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
+        // Transmit the new mission
-
-
         mavlink_msg_mission_item_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, wp);
         uas->sendMessage(message);
+
+        // And update the UI.
+        emit updateStatusString(tr("Sending waypoint ID %1 of %2 total").arg(wp->seq).arg(current_count));
+
         QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
     }
 }
@@ -1038,15 +1035,13 @@ void UASWaypointManager::sendWaypoint(quint16 seq)
 void UASWaypointManager::sendWaypointAck(quint8 type)
 {
     if (!uas) return;
-    mavlink_message_t message;
-    mavlink_mission_ack_t wpa;
-
-    wpa.target_system = uasid;
-    wpa.target_component = MAV_COMP_ID_MISSIONPLANNER;
-    wpa.type = type;
 
+    // Send the message.
+    mavlink_message_t message;
+    mavlink_mission_ack_t wpa = {(quint8)uasid, MAV_COMP_ID_MISSIONPLANNER, type};
     mavlink_msg_mission_ack_encode(uas->mavlink->getSystemId(), uas->mavlink->getComponentId(), &message, &wpa);
     uas->sendMessage(message);
+
     QGC::SLEEP::msleep(PROTOCOL_DELAY_MS);
 }
 

src/uas/UASWaypointManager.h

@@ -177,7 +177,7 @@ private:
     QList<mavlink_mission_item_t *> waypoint_buffer;  ///< buffer for waypoints during communication
     QTimer protocol_timer;                          ///< Timer to catch timeouts
     bool standalone;                                ///< If standalone is set, do not write to UAS
-    quint16 uasid;
+    int uasid;                                   ///< The ID of the current UAS. Retrieved via `uas->getUASID();`, stored as an `int` to match its return type.
 
     // XXX export to settings
     static const float defaultAltitudeHomeOffset;    ///< Altitude offset in meters from home for new waypoints