Add priorityLink and sendMessageOnLink

src/Vehicle/Vehicle.cc

@@ -116,6 +116,7 @@ Vehicle::Vehicle(LinkInterface*             link,
 
     connect(_mavlink, &MAVLinkProtocol::messageReceived,     this, &Vehicle::_mavlinkMessageReceived);
     connect(this, &Vehicle::_sendMessageOnThread, this, &Vehicle::_sendMessage, Qt::QueuedConnection);
+    connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
 
     _uas = new UAS(_mavlink, this, _firmwarePluginManager);
 
@@ -435,28 +436,63 @@ void Vehicle::sendMessage(mavlink_message_t message)
     emit _sendMessageOnThread(message);
 }
 
-void Vehicle::_sendMessage(mavlink_message_t message)
+bool Vehicle::sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
 {
-    // Emit message on all links that are currently connected
-    foreach (LinkInterface* link, _links) {
-        if (link->isConnected()) {
-            MAVLinkProtocol* mavlink = _mavlink;
+    if (!link || !_links.contains(link) || !link->isConnected()) {
+        return false;
+    }
+
+    emit _sendMessageOnLinkOnThread(link, message);
+
+    return true;
+}
 
+void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
+{
     // Give the plugin a chance to adjust
     _firmwarePlugin->adjustMavlinkMessage(this, &message);
 
     static const uint8_t messageKeys[256] = MAVLINK_MESSAGE_CRCS;
-            mavlink_finalize_message_chan(&message, mavlink->getSystemId(), mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
+    mavlink_finalize_message_chan(&message, _mavlink->getSystemId(), _mavlink->getComponentId(), link->getMavlinkChannel(), message.len, messageKeys[message.msgid]);
 
     // Write message into buffer, prepending start sign
     uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
     int len = mavlink_msg_to_send_buffer(buffer, &message);
 
     link->writeBytes((const char*)buffer, len);
+}
+
+void Vehicle::_sendMessage(mavlink_message_t message)
+{
+    // Emit message on all links that are currently connected
+    foreach (LinkInterface* link, _links) {
+        if (link->isConnected()) {
+            _sendMessageOnLink(link, message);
         }
     }
 }
 
+/// @return Direct usb connection link to board if one, NULL if none
+LinkInterface* Vehicle::priorityLink(void)
+{
+    foreach (LinkInterface* link, _links) {
+        if (link->isConnected()) {
+            SerialLink* pSerialLink = qobject_cast<SerialLink*>(link);
+            if (pSerialLink) {
+                LinkConfiguration* pLinkConfig = pSerialLink->getLinkConfiguration();
+                if (pLinkConfig) {
+                    SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(pLinkConfig);
+                    if (pSerialConfig && pSerialConfig->usbDirect()) {
+                        return link;
+                    }
+                }
+            }
+        }
+    }
+
+    return _links.count() ? _links[0] : NULL;
+}
+
 void Vehicle::setLatitude(double latitude)
 {
     _coordinate.setLatitude(latitude);

src/Vehicle/Vehicle.h

@@ -166,9 +166,16 @@ public:
     MAV_AUTOPILOT firmwareType(void) { return _firmwareType; }
     MAV_TYPE vehicleType(void) { return _vehicleType; }
 
-    /// Sends this specified message to all links accociated with this vehicle
+    /// Returns the highest quality link available to the Vehicle
+    LinkInterface* priorityLink(void);
+
+    /// Sends a message to all links accociated with this vehicle
     void sendMessage(mavlink_message_t message);
 
+    /// Sends a message to the specified link
+    /// @return true: message sent, false: Link no longer connected
+    bool sendMessageOnLink(LinkInterface* link, mavlink_message_t message);
+
     /// Sends the specified messages multiple times to the vehicle in order to attempt to
     /// guarantee that it makes it to the vehicle.
     void sendMessageMultiple(mavlink_message_t message);
@@ -292,6 +299,7 @@ signals:
 
     /// Used internally to move sendMessage call to main thread
     void _sendMessageOnThread(mavlink_message_t message);
+    void _sendMessageOnLinkOnThread(LinkInterface* link, mavlink_message_t message);
 
     void messageTypeChanged     ();
     void newMessageCountChanged ();
@@ -340,6 +348,7 @@ private slots:
     void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
     void _linkInactiveOrDeleted(LinkInterface* link);
     void _sendMessage(mavlink_message_t message);
+    void _sendMessageOnLink(LinkInterface* link, mavlink_message_t message);
     void _sendMessageMultipleNext(void);
     void _addNewMapTrajectoryPoint(void);
     void _parametersReady(bool parametersReady);