diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 4ab07aeff771b2103c893170bb43ef22c653a01f..71446dfe24c32262f92c5f6135f9b2c759b4d67b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/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 + } + } } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7277cc1fe2cb2de1b25310f9b474cedf73518856..c0802808a2e4fdead917f40b7aa50a22b8af198d 100644 --- a/src/Vehicle/Vehicle.h +++ b/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); diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index 2539cf3b538684ffdf9ace26a720a850f080e280..f5eafe1afc53c43b2e05497e7900e178d45570d4 100644 --- a/src/comm/LinkInterface.cc +++ b/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*"); - - // 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; + } +} diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index be0c626720b3456d68f8b8bd8021f438a76e7c66..9e879ad4dbabaf27a3315475bc2c93354801268d 100644 --- a/src/comm/LinkInterface.h +++ b/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 SharedLinkInterfacePointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 573b9932d7771a34a82facf470bfaa7e007defe5..87ff78f4b487183a0200fb4f90ae81980aec474e 100644 --- a/src/comm/LinkManager.cc +++ b/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); + } +} diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index bd719e364305cc1dc8048d5b55a75af7ec4521e7..46d62b05f66f1e23275c4379d420b84e8390d5f8 100644 --- a/src/comm/LinkManager.h +++ b/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