Fix QTimer issue and send command on high latency link change

src/Vehicle/Vehicle.cc

@@ -191,8 +191,6 @@ Vehicle::Vehicle(LinkInterface*             link,
     , _clockFactGroup(this)
     , _distanceSensorFactGroup(this)
 {
-    _addLink(link);
-
     connect(_joystickManager, &JoystickManager::activeJoystickChanged, this, &Vehicle::_loadSettings);
     connect(qgcApp()->toolbox()->multiVehicleManager(), &MultiVehicleManager::activeVehicleAvailableChanged, this, &Vehicle::_loadSettings);
 
@@ -200,6 +198,8 @@ Vehicle::Vehicle(LinkInterface*             link,
 
     connect(_mavlink, &MAVLinkProtocol::messageReceived,     this, &Vehicle::_mavlinkMessageReceived);
 
+    _addLink(link);
+
     connect(this, &Vehicle::_sendMessageOnLinkOnThread, this, &Vehicle::_sendMessageOnLink, Qt::QueuedConnection);
     connect(this, &Vehicle::flightModeChanged,          this, &Vehicle::_handleFlightModeChanged);
     connect(this, &Vehicle::armedChanged,               this, &Vehicle::_announceArmedChanged);
@@ -1609,8 +1609,12 @@ void Vehicle::_addLink(LinkInterface* link)
     if (!_containsLink(link)) {
         qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16);
         _links += link;
-        _updatePriorityLink(true);
-        _updateHighLatencyLink();
+        if (_links.count() <= 1) {
+            _updatePriorityLink(true, false);
+        } else {
+            _updatePriorityLink(true, true);
+        }
+
         connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted);
         connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted);
         connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink);
@@ -1623,7 +1627,7 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link)
     qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count();
 
     _links.removeOne(link);
-    _updatePriorityLink(true);
+    _updatePriorityLink(true, true);
 
     if (_links.count() == 0 && !_allLinksInactiveSent) {
         qCDebug(VehicleLog) << "All links inactive";
@@ -1669,7 +1673,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message)
     emit messagesSentChanged();
 }
 
-void Vehicle::_updatePriorityLink(bool updateActive)
+void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand)
 {
     emit linkNamesChanged();
     LinkInterface* newPriorityLink = NULL;
@@ -1751,7 +1755,9 @@ void Vehicle::_updatePriorityLink(bool updateActive)
             qgcApp()->showMessage((tr("switch to %2 as priority link")).arg(newPriorityLink->getName()));
         }
         _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
-        _updateHighLatencyLink();
+
+        _updateHighLatencyLink(sendCommand);
+
         emit priorityLinkNameChanged(_priorityLink->getName());
         if (updateActive) {
             _linkActiveChanged(_priorityLink.data(), _priorityLink->active());
@@ -2131,7 +2137,7 @@ void Vehicle::setPriorityLinkByName(const QString& priorityLinkName)
 
     if (newPriorityLink) {
         _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink);
-        _updateHighLatencyLink();
+        _updateHighLatencyLink(true);
         emit priorityLinkNameChanged(_priorityLink->getName());
         _linkActiveChanged(_priorityLink.data(), _priorityLink->active());
     }
@@ -2443,19 +2449,24 @@ void Vehicle::_linkActiveChanged(LinkInterface *link, bool active)
         if (active && _connectionLost) {
             // communication to priority link regained
             _connectionLost = false;
+            emit connectionLostChanged(false);
             qgcApp()->showMessage((tr("communication to priority link %2 regained")).arg(link->getName()));
 
-            // Re-negotiate protocol version for the link
-            sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
-                           MAV_CMD_REQUEST_PROTOCOL_VERSION,
-                           false,                                   // No error shown if fails
-                           1);                                     // Request protocol version
+            if (_priorityLink->highLatency()) {
+                _setMaxProtoVersion(100);
+            } else {
+                // Re-negotiate protocol version for the link
+                sendMavCommand(MAV_COMP_ID_ALL,                         // Don't know default component id yet.
+                               MAV_CMD_REQUEST_PROTOCOL_VERSION,
+                               false,                                   // No error shown if fails
+                               1);                                     // Request protocol version
+            }
 
         } else if (!active && !_connectionLost) {
             // communication to priority link lost
             qgcApp()->showMessage((tr("communication to priority link %2 lost")).arg(link->getName()));
 
-            _updatePriorityLink(false);
+            _updatePriorityLink(false, true);
 
             if (link == _priorityLink) {
                 _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech()));
@@ -3354,12 +3365,26 @@ void Vehicle::_vehicleParamLoaded(bool ready)
     }
 }
 
-void Vehicle::_updateHighLatencyLink(void)
+void Vehicle::_updateHighLatencyLink(bool sendCommand)
 {
     if (_priorityLink->highLatency() != _highLatencyLink) {
         _highLatencyLink = _priorityLink->highLatency();
         _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs);
         emit highLatencyLinkChanged(_highLatencyLink);
+
+        if (sendCommand) {
+            if (_highLatencyLink) {
+                sendMavCommand(defaultComponentId(),
+                               MAV_CMD_CONTROL_HIGH_LATENCY,
+                               true,
+                               1.0f); // request start transmitting over high latency telemetry
+            } else {
+                sendMavCommand(defaultComponentId(),
+                               MAV_CMD_CONTROL_HIGH_LATENCY,
+                               true,
+                               0.0f); // request stop transmitting over high latency telemetry
+            }
+        }
     }
 }
 

src/Vehicle/Vehicle.h

@@ -1032,7 +1032,7 @@ private slots:
     void _offlineVehicleTypeSettingChanged(QVariant value);
     void _offlineCruiseSpeedSettingChanged(QVariant value);
     void _offlineHoverSpeedSettingChanged(QVariant value);
-    void _updateHighLatencyLink(void);
+    void _updateHighLatencyLink(bool sendCommand = true);
 
     void _handleTextMessage                 (int newCount);
     void _handletextMessageReceived         (UASMessage* message);
@@ -1106,7 +1106,7 @@ private:
     void _handleMavlinkLoggingDataAcked(mavlink_message_t& message);
     void _ackMavlinkLogData(uint16_t sequence);
     void _sendNextQueuedMavCommand(void);
-    void _updatePriorityLink(bool updateActive);
+    void _updatePriorityLink(bool updateActive, bool sendCommand);
     void _commonInit(void);
     void _startPlanRequest(void);
     void _setupAutoDisarmSignalling(void);

src/comm/LinkInterface.cc

@@ -28,6 +28,7 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config)
     , _active                   (false)
     , _enableRateCollection     (false)
     , _decodedFirstMavlinkPacket(false)
+    , _bytesReceivedTimer(NULL)
 {
     _config->setLink(this);
 
@@ -43,14 +44,6 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config)
 
     QObject::connect(this, &LinkInterface::_invokeWriteBytes, this, &LinkInterface::_writeBytes);
     qRegisterMetaType<LinkInterface*>("LinkInterface*");
-
-    // active Lost timer
-    _bytesReceivedTimer.setInterval(_bytesReceivedTimeoutMSecs);
-    _bytesReceivedTimer.setSingleShot(false);
-    _bytesReceivedTimer.start();
-    QObject::connect(&_bytesReceivedTimer, &QTimer::timeout, this, &LinkInterface::_bytesReceivedTimeout);
-
-    QObject::connect(this, &LinkInterface::bytesReceived, this, &LinkInterface::_bytesReceived);
 }
 
 /// This function logs the send times and amounts of datas for input. Data is used for calculating
@@ -168,18 +161,6 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel)
     _mavlinkChannel = channel;
 }
 
-void LinkInterface::_bytesReceived(LinkInterface* link, QByteArray bytes)
-{
-    Q_UNUSED(bytes);
-
-    if (this == link) {
-        _bytesReceivedTimer.start();
-
-        if (!link->active()) {
-            link->setActive(true);
-        }
-    }
-}
 
 void LinkInterface::_bytesReceivedTimeout()
 {
@@ -187,3 +168,23 @@ void LinkInterface::_bytesReceivedTimeout()
         setActive(false);
     }
 }
+
+void LinkInterface::timerStart() {
+    if (_bytesReceivedTimer) {
+        _bytesReceivedTimer->start();
+    } else {
+        _bytesReceivedTimer = new QTimer();
+        _bytesReceivedTimer->setInterval(_bytesReceivedTimeoutMSecs);
+        _bytesReceivedTimer->setSingleShot(true);
+        _bytesReceivedTimer->start();
+        QObject::connect(_bytesReceivedTimer, &QTimer::timeout, this, &LinkInterface::_bytesReceivedTimeout);
+    }
+}
+
+void LinkInterface::timerStop() {
+    if (_bytesReceivedTimer) {
+        _bytesReceivedTimer->stop();
+        QObject::disconnect(_bytesReceivedTimer, &QTimer::timeout, this, &LinkInterface::_bytesReceivedTimeout);
+        delete _bytesReceivedTimer;
+    }
+}

src/comm/LinkInterface.h

@@ -37,7 +37,10 @@ class LinkInterface : public QThread
     friend class LinkManager;
 
 public:    
-    ~LinkInterface() { _config->setLink(NULL); }
+    ~LinkInterface() {
+        _config->setLink(NULL);
+        timerStop();
+    }
 
     Q_PROPERTY(bool active      READ active         WRITE setActive         NOTIFY activeChanged)
 
@@ -150,6 +153,9 @@ public slots:
 
 private slots:
     virtual void _writeBytes(const QByteArray) = 0;
+
+    void _bytesReceivedTimeout(void);
+
     
 signals:
     void autoconnectChanged(bool autoconnect);
@@ -250,13 +256,23 @@ private:
 
     virtual void _disconnect(void) = 0;
 
-    void _bytesReceived(LinkInterface* link, QByteArray bytes);
-    
-    void _bytesReceivedTimeout(void);
-
     /// Sets the mavlink channel to use for this link
     void _setMavlinkChannel(uint8_t channel);
     
+    /**
+     * @brief timerStart
+     *
+     * Allocate the timer if it does not exist yet and start it.
+     */
+    void timerStart();
+
+    /**
+     * @brief timerStop
+     *
+     * Stop and deallocate the timer if it exists.
+     */
+    void timerStop();
+
     bool _mavlinkChannelSet;    ///< true: _mavlinkChannel has been set
     uint8_t _mavlinkChannel;    ///< mavlink channel to use for this link, as used by mavlink_parse_char
     
@@ -283,7 +299,7 @@ private:
     bool _decodedFirstMavlinkPacket;    ///< true: link has correctly decoded it's first mavlink packet
 
     static const int    _bytesReceivedTimeoutMSecs = 3500;  // Signal connection lost after 3.5 seconds of no messages
-    QTimer              _bytesReceivedTimer;
+    QTimer*             _bytesReceivedTimer;
 };
 
 typedef QSharedPointer<LinkInterface> SharedLinkInterfacePointer;

src/comm/LinkManager.cc

@@ -195,6 +195,7 @@ void LinkManager::_addLink(LinkInterface* link)
 
     connect(link, &LinkInterface::communicationError,   _app,               &QGCApplication::criticalMessageBoxOnMainThread);
     connect(link, &LinkInterface::bytesReceived,        _mavlinkProtocol,   &MAVLinkProtocol::receiveBytes);
+    connect(link, &LinkInterface::bytesReceived,        this,               &LinkManager::_bytesReceived);
 
     _mavlinkProtocol->resetMetadataForLink(link);
     _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion());
@@ -1003,3 +1004,13 @@ void LinkManager::_freeMavlinkChannel(int channel)
 {
     _mavlinkChannelsUsedBitMask &= ~(1 << channel);
 }
+
+void LinkManager::_bytesReceived(LinkInterface *link, QByteArray bytes) {
+    Q_UNUSED(bytes);
+
+    link->timerStart();
+
+    if (!link->active()) {
+        link->setActive(true);
+    }
+}

src/comm/LinkManager.h

@@ -204,6 +204,8 @@ private:
     SerialConfiguration* _autoconnectConfigurationsContainsPort(const QString& portName);
 #endif
 
+    void _bytesReceived(LinkInterface* link, QByteArray bytes);
+
     bool    _configUpdateSuspended;                     ///< true: stop updating configuration list
     bool    _configurationsLoaded;                      ///< true: Link configurations have been loaded
     bool    _connectionsSuspended;                      ///< true: all new connections should not be allowed