Merge pull request #2590 from DonLakeFlyer/SingleLinkProtocol

Mini-protocol must only send on one link, in some case entire transaction must stay on same link

src/FactSystem/ParameterLoader.cc

@@ -45,15 +45,15 @@ QGC_LOGGING_CATEGORY(ParameterLoaderVerboseLog, "ParameterLoaderVerboseLog")
 
 Fact ParameterLoader::_defaultFact;
 
-ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent) :
-    QObject(parent),
-    _autopilot(autopilot),
-    _vehicle(vehicle),
-    _mavlink(qgcApp()->toolbox()->mavlinkProtocol()),
-    _parametersReady(false),
-    _initialLoadComplete(false),
-    _defaultComponentId(FactSystem::defaultComponentId),
-    _totalParamCount(0)
+ParameterLoader::ParameterLoader(AutoPilotPlugin* autopilot, Vehicle* vehicle, QObject* parent)
+    : QObject(parent)
+    , _autopilot(autopilot)
+    , _vehicle(vehicle)
+    , _mavlink(qgcApp()->toolbox()->mavlinkProtocol())
+    , _parametersReady(false)
+    , _initialLoadComplete(false)
+    , _defaultComponentId(FactSystem::defaultComponentId)
+    , _totalParamCount(0)
 {
     Q_ASSERT(_autopilot);
     Q_ASSERT(_vehicle);
@@ -321,7 +321,7 @@ void ParameterLoader::refreshAllParameters(void)
 
     mavlink_message_t msg;
     mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL);
-    _vehicle->sendMessage(msg);
+    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
 
     qCDebug(ParameterLoaderLog) << "Request to refresh all parameters";
 }
@@ -522,7 +522,7 @@ void ParameterLoader::_tryCacheLookup()
 
     mavlink_message_t msg;
     mavlink_msg_param_request_read_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, _vehicle->id(), MAV_COMP_ID_ALL, "_HASH_CHECK", -1);
-    _vehicle->sendMessage(msg);
+    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
 }
 
 void ParameterLoader::_readParameterRaw(int componentId, const QString& paramName, int paramIndex)
@@ -538,7 +538,7 @@ void ParameterLoader::_readParameterRaw(int componentId, const QString& paramNam
                                         componentId,                // Target component id
                                         fixedParamName,             // Named parameter being requested
                                         paramIndex);                // Parameter index being requested, -1 for named
-    _vehicle->sendMessage(msg);
+    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
 }
 
 void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramName, const QVariant& value)
@@ -591,7 +591,7 @@ void ParameterLoader::_writeParameterRaw(int componentId, const QString& paramNa
 
     mavlink_message_t msg;
     mavlink_msg_param_set_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &p);
-    _vehicle->sendMessage(msg);
+    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
 }
 
 void ParameterLoader::_writeLocalParamCache()
@@ -663,7 +663,7 @@ void ParameterLoader::_saveToEEPROM(void)
     if (_vehicle->firmwarePlugin()->isCapable(FirmwarePlugin::MavCmdPreflightStorageCapability)) {
         mavlink_message_t msg;
         mavlink_msg_command_long_pack(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, _vehicle->id(), 0, MAV_CMD_PREFLIGHT_STORAGE, 1, 1, -1, -1, -1, 0, 0, 0);
-        _vehicle->sendMessage(msg);
+        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
         qCDebug(ParameterLoaderLog) << "_saveToEEPROM";
     } else {
         qCDebug(ParameterLoaderLog) << "_saveToEEPROM skipped due to FirmwarePlugin::isCapable";

src/MissionManager/MissionManager.cc

@@ -34,6 +34,7 @@ QGC_LOGGING_CATEGORY(MissionManagerLog, "MissionManagerLog")
 
 MissionManager::MissionManager(Vehicle* vehicle)
     : _vehicle(vehicle)
+    , _dedicatedLink(NULL)
     , _ackTimeoutTimer(NULL)
     , _retryAck(AckNone)
     , _readTransactionInProgress(false)
@@ -98,7 +99,8 @@ void MissionManager::writeMissionItems(const QmlObjectListModel& missionItems)
 
     mavlink_msg_mission_count_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionCount);
 
-    _vehicle->sendMessage(message);
+    _dedicatedLink = _vehicle->priorityLink();
+    _vehicle->sendMessageOnLink(_dedicatedLink, message);
     _startAckTimeout(AckMissionRequest);
     emit inProgressChanged(true);
 }
@@ -120,7 +122,8 @@ void MissionManager::requestMissionItems(void)
     
     mavlink_msg_mission_request_list_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &request);
     
-    _vehicle->sendMessage(message);
+    _dedicatedLink = _vehicle->priorityLink();
+    _vehicle->sendMessageOnLink(_dedicatedLink, message);
     _startAckTimeout(AckMissionCount);
     emit inProgressChanged(true);
 }
@@ -180,8 +183,8 @@ void MissionManager::_readTransactionComplete(void)
     
     mavlink_msg_mission_ack_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionAck);
     
-    _vehicle->sendMessage(message);
-    
+    _vehicle->sendMessageOnLink(_dedicatedLink, message);
+
     _finishTransaction(true);
     emit newMissionItemsAvailable();
 }
@@ -228,7 +231,7 @@ void MissionManager::_requestNextMissionItem(void)
     
     mavlink_msg_mission_request_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &message, &missionRequest);
     
-    _vehicle->sendMessage(message);
+    _vehicle->sendMessageOnLink(_dedicatedLink, message);
     _startAckTimeout(AckMissionItem);
 }
 
@@ -330,7 +333,7 @@ void MissionManager::_handleMissionRequest(const mavlink_message_t& message)
     
     mavlink_msg_mission_item_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &messageOut, &missionItem);
     
-    _vehicle->sendMessage(messageOut);
+    _vehicle->sendMessageOnLink(_dedicatedLink, messageOut);
     _startAckTimeout(AckMissionRequest);
 }
 

src/MissionManager/MissionManager.h

@@ -33,6 +33,7 @@
 #include "QmlObjectListModel.h"
 #include "QGCMAVLink.h"
 #include "QGCLoggingCategory.h"
+#include "LinkInterface.h"
 
 class Vehicle;
 
@@ -116,6 +117,7 @@ private:
 
 private:
     Vehicle*            _vehicle;
+    LinkInterface*      _dedicatedLink;
     
     QTimer*             _ackTimeoutTimer;
     AckType_t           _retryAck;

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,26 +436,66 @@ void Vehicle::sendMessage(mavlink_message_t message)
     emit _sendMessageOnThread(message);
 }
 
+bool Vehicle::sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
+{
+    if (!link || !_links.contains(link) || !link->isConnected()) {
+        return false;
+    }
+
+    emit _sendMessageOnLinkOnThread(link, message);
+
+    return true;
+}
+
+void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
+{
+    // Make sure this is still a good link
+    if (!link || !_links.contains(link) || !link->isConnected()) {
+        return;
+    }
+
+    // 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]);
+
+    // 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()) {
-            MAVLinkProtocol* mavlink = _mavlink;
-
-            // 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]);
-
-            // Write message into buffer, prepending start sign
-            uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
-            int len = mavlink_msg_to_send_buffer(buffer, &message);
+            _sendMessageOnLink(link, message);
+        }
+    }
+}
 
-            link->writeBytes((const char*)buffer, len);
+/// @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)

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);

src/ViewWidgets/LogDownloadController.cc

@@ -390,7 +390,7 @@ LogDownloadController::_requestLogData(uint8_t id, uint32_t offset, uint32_t cou
             &msg,
             qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), MAV_COMP_ID_ALL,
             id, offset, count);
-        _vehicle->sendMessage(msg);
+        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
     }
 }
 
@@ -419,7 +419,7 @@ LogDownloadController::_requestLogList(uint32_t start, uint32_t end)
             MAV_COMP_ID_ALL,
             start,
             end);
-        _vehicle->sendMessage(msg);
+        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
         //-- Wait 2 seconds before bitching about not getting anything
         _timer.start(2000);
     }
@@ -556,7 +556,7 @@ LogDownloadController::eraseAll(void)
             qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(),
             &msg,
             qgcApp()->toolbox()->multiVehicleManager()->activeVehicle()->id(), MAV_COMP_ID_ALL);
-        _vehicle->sendMessage(msg);
+        _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
         refresh();
     }
 }

src/comm/LinkManager.cc

@@ -173,16 +173,23 @@ void LinkManager::_addLink(LinkInterface* link)
     }
 
     if (!_links.contains(link)) {
+        bool channelSet = false;
+
         // Find a mavlink channel to use for this link
         for (int i=0; i<32; i++) {
             if (!(_mavlinkChannelsUsedBitMask && 1 << i)) {
                 mavlink_reset_channel_status(i);
                 link->_setMavlinkChannel(i);
                 _mavlinkChannelsUsedBitMask |= i << i;
+                channelSet = true;
                 break;
             }
         }
 
+        if (!channelSet) {
+            qWarning() << "Ran out of mavlink channels";
+        }
+
         _links.append(link);
         emit newLink(link);
     }
@@ -508,11 +515,13 @@ void LinkManager::_updateAutoConnectLinks(void)
                 case QGCSerialPortInfo::BoardTypePX4FMUV4:
                     if (_autoconnectPixhawk) {
                         pSerialConfig = new SerialConfiguration(QString("Pixhawk on %1").arg(portInfo.portName().trimmed()));
+                        pSerialConfig->setUsbDirect(true);
                     }
                     break;
                 case QGCSerialPortInfo::BoardTypeAeroCore:
                     if (_autoconnectPixhawk) {
                         pSerialConfig = new SerialConfiguration(QString("AeroCore on %1").arg(portInfo.portName().trimmed()));
+                        pSerialConfig->setUsbDirect(true);
                     }
                     break;
                 case QGCSerialPortInfo::BoardTypePX4Flow:

src/comm/SerialLink.cc

@@ -388,6 +388,7 @@ SerialConfiguration::SerialConfiguration(const QString& name) : LinkConfiguratio
     _parity     = QSerialPort::NoParity;
     _dataBits   = 8;
     _stopBits   = 1;
+    _usbDirect  = false;
 }
 
 SerialConfiguration::SerialConfiguration(SerialConfiguration* copy) : LinkConfiguration(copy)
@@ -399,6 +400,7 @@ SerialConfiguration::SerialConfiguration(SerialConfiguration* copy) : LinkConfig
     _stopBits           = copy->stopBits();
     _portName           = copy->portName();
     _portDisplayName    = copy->portDisplayName();
+    _usbDirect          = copy->_usbDirect;
 }
 
 void SerialConfiguration::copyFrom(LinkConfiguration *source)
@@ -413,6 +415,7 @@ void SerialConfiguration::copyFrom(LinkConfiguration *source)
     _stopBits           = ssource->stopBits();
     _portName           = ssource->portName();
     _portDisplayName    = ssource->portDisplayName();
+    _usbDirect          = ssource->_usbDirect;
 }
 
 void SerialConfiguration::updateSettings()
@@ -554,3 +557,10 @@ void SerialConfiguration::_initBaudRates()
     kSupportedBaudRates << "921600";
 }
 
+void SerialConfiguration::setUsbDirect(bool usbDirect)
+{
+    if (_usbDirect != usbDirect) {
+        _usbDirect = usbDirect;
+        emit usbDirectChanged(_usbDirect);
+    }
+}

src/comm/SerialLink.h

@@ -73,12 +73,14 @@ public:
     Q_PROPERTY(int      parity          READ parity             WRITE setParity             NOTIFY parityChanged)
     Q_PROPERTY(QString  portName        READ portName           WRITE setPortName           NOTIFY portNameChanged)
     Q_PROPERTY(QString  portDisplayName READ portDisplayName                                NOTIFY portDisplayNameChanged)
+    Q_PROPERTY(bool     usbDirect       READ usbDirect          WRITE setUsbDirect          NOTIFY usbDirectChanged)        ///< true: direct usb connection to board
 
     int  baud()         { return _baud; }
     int  dataBits()     { return _dataBits; }
     int  flowControl()  { return _flowControl; }    ///< QSerialPort Enums
     int  stopBits()     { return _stopBits; }
     int  parity()       { return _parity; }         ///< QSerialPort Enums
+    bool usbDirect()    { return _usbDirect; }
 
     const QString portName          () { return _portName; }
     const QString portDisplayName   () { return _portDisplayName; }
@@ -89,6 +91,7 @@ public:
     void setStopBits        (int stopBits);
     void setParity          (int parity);               ///< QSerialPort Enums
     void setPortName        (const QString& portName);
+    void setUsbDirect       (bool usbDirect);
 
     static QStringList supportedBaudRates();
     static QString cleanPortDisplayname(const QString name);
@@ -109,6 +112,7 @@ signals:
     void parityChanged          ();
     void portNameChanged        ();
     void portDisplayNameChanged ();
+    void usbDirectChanged       (bool usbDirect);
 
 private:
     static void _initBaudRates();
@@ -121,6 +125,7 @@ private:
     int _parity;
     QString _portName;
     QString _portDisplayName;
+    bool _usbDirect;
 };
 
 /**

src/uas/FileManager.cc

@@ -34,13 +34,14 @@
 
 QGC_LOGGING_CATEGORY(FileManagerLog, "FileManagerLog")
 
-FileManager::FileManager(QObject* parent, Vehicle* vehicle) :
-    QObject(parent),
-    _currentOperation(kCOIdle),
-    _vehicle(vehicle),
-    _lastOutgoingSeqNumber(0),
-    _activeSession(0),
-    _systemIdQGC(0)
+FileManager::FileManager(QObject* parent, Vehicle* vehicle)
+    : QObject(parent)
+    , _currentOperation(kCOIdle)
+    , _vehicle(vehicle)
+    , _dedicatedLink(NULL)
+    , _lastOutgoingSeqNumber(0)
+    , _activeSession(0)
+    , _systemIdQGC(0)
 {
     connect(&_ackTimer, &QTimer::timeout, this, &FileManager::_ackTimeout);
     
@@ -308,10 +309,8 @@ void FileManager::_writeFileDatablock(void)
     _sendRequest(&request);
 }
 
-void FileManager::receiveMessage(LinkInterface* link, mavlink_message_t message)
+void FileManager::receiveMessage(mavlink_message_t message)
 {
-    Q_UNUSED(link);
-
     // receiveMessage is signalled will all mavlink messages so we need to filter everything else out but ours.
     if (message.msgid != MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) {
         return;
@@ -444,6 +443,12 @@ void FileManager::listDirectory(const QString& dirPath)
         return;
     }
 
+    _dedicatedLink = _vehicle->priorityLink();
+    if (!_dedicatedLink) {
+        _emitErrorMessage(tr("Command not sent. No Vehicle links."));
+        return;
+    }
+
     // initialise the lister
     _listPath = dirPath;
     _listOffset = 0;
@@ -481,6 +486,12 @@ void FileManager::downloadPath(const QString& from, const QDir& downloadDir)
         _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete."));
         return;
     }
+
+    _dedicatedLink = _vehicle->priorityLink();
+    if (!_dedicatedLink) {
+        _emitErrorMessage(tr("Command not sent. No Vehicle links."));
+        return;
+    }
     
 	qCDebug(FileManagerLog) << "downloadPath from:" << from << "to:" << downloadDir;
 	_downloadWorker(from, downloadDir, true /* read file */);
@@ -492,6 +503,12 @@ void FileManager::streamPath(const QString& from, const QDir& downloadDir)
         _emitErrorMessage(tr("Command not sent. Waiting for previous command to complete."));
         return;
     }
+
+    _dedicatedLink = _vehicle->priorityLink();
+    if (!_dedicatedLink) {
+        _emitErrorMessage(tr("Command not sent. No Vehicle links."));
+        return;
+    }
     
 	qCDebug(FileManagerLog) << "streamPath from:" << from << "to:" << downloadDir;
 	_downloadWorker(from, downloadDir, false /* stream file */);
@@ -537,6 +554,12 @@ void FileManager::uploadPath(const QString& toPath, const QFileInfo& uploadFile)
         return;
     }
 
+    _dedicatedLink = _vehicle->priorityLink();
+    if (!_dedicatedLink) {
+        _emitErrorMessage(tr("Command not sent. No Vehicle links."));
+        return;
+    }
+
     if (toPath.isEmpty()) {
         return;
     }
@@ -731,5 +754,5 @@ void FileManager::_sendRequest(Request* request)
                                             0,                  // Target component
                                             (uint8_t*)request); // Payload
     
-    _vehicle->sendMessage(message);
+    _vehicle->sendMessageOnLink(_dedicatedLink, message);
 }

src/uas/FileManager.h

@@ -87,7 +87,7 @@ signals:
     void commandProgress(int value);
 
 public slots:
-    void receiveMessage(LinkInterface* link, mavlink_message_t message);
+    void receiveMessage(mavlink_message_t message);
 	
 private slots:
 	void _ackTimeout(void);
@@ -203,7 +203,8 @@ private:
     OperationState  _currentOperation;              ///< Current operation of state machine
     QTimer          _ackTimer;                      ///< Used to signal a timeout waiting for an ack
     
-    Vehicle* _vehicle;
+    Vehicle*        _vehicle;
+    LinkInterface*  _dedicatedLink; ///< Link to use for communication
     
     uint16_t _lastOutgoingSeqNumber; ///< Sequence number sent in last outgoing packet
 

src/uas/UAS.cc

@@ -185,7 +185,7 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
     }
 
 #ifndef __mobile__
-    connect(mavlink, &MAVLinkProtocol::messageReceived, &fileManager, &FileManager::receiveMessage);
+    connect(_vehicle, &Vehicle::mavlinkMessageReceived, &fileManager, &FileManager::receiveMessage);
 #endif
 
     color = UASInterface::getNextColor();