From 321fae7f31bf9470d1904a20bfc95dce5c6ff4c5 Mon Sep 17 00:00:00 2001 From: acfloria Date: Mon, 12 Feb 2018 22:53:58 +0100 Subject: [PATCH] Update active property for each link separately Each link updates the active property based on incoming bytes. The vehicle tracks the active property of each link and in case of an inactive priority link it updates the priority link. --- src/Vehicle/Vehicle.cc | 130 +++++++++++++++++++++----------------- src/Vehicle/Vehicle.h | 7 +- src/comm/LinkInterface.cc | 28 ++++++++ src/comm/LinkInterface.h | 12 +++- 4 files changed, 112 insertions(+), 65 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 7473bc818..ed7ec9648 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -222,12 +222,6 @@ Vehicle::Vehicle(LinkInterface* link, _prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs); _prearmErrorTimer.setSingleShot(true); - // Connection Lost timer - _connectionLostTimer.setInterval(_connectionLostTimeoutMSecs); - _connectionLostTimer.setSingleShot(false); - _connectionLostTimer.start(); - connect(&_connectionLostTimer, &QTimer::timeout, this, &Vehicle::_connectionLostTimeout); - // Send MAV_CMD ack timer _mavCommandAckTimer.setSingleShot(true); _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs); @@ -606,13 +600,6 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes } } - - // Mark this vehicle as active - but only if the traffic is coming from - // the actual vehicle - if (message.sysid == _id) { - _connectionActive(); - } - // Give the plugin a change to adjust the message contents if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) { return; @@ -1622,11 +1609,12 @@ void Vehicle::_addLink(LinkInterface* link) if (!_containsLink(link)) { qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16); _links += link; - _updatePriorityLink(); + _updatePriorityLink(true); _updateHighLatencyLink(); connect(_toolbox->linkManager(), &LinkManager::linkInactive, this, &Vehicle::_linkInactiveOrDeleted); connect(_toolbox->linkManager(), &LinkManager::linkDeleted, this, &Vehicle::_linkInactiveOrDeleted); connect(link, &LinkInterface::highLatencyChanged, this, &Vehicle::_updateHighLatencyLink); + connect(link, &LinkInterface::activeChanged, this, &Vehicle::_linkActiveChanged); } } @@ -1635,7 +1623,7 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link) qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count(); _links.removeOne(link); - _updatePriorityLink(); + _updatePriorityLink(true); if (_links.count() == 0 && !_allLinksInactiveSent) { qCDebug(VehicleLog) << "All links inactive"; @@ -1681,7 +1669,7 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) emit messagesSentChanged(); } -void Vehicle::_updatePriorityLink(void) +void Vehicle::_updatePriorityLink(bool updateActive) { emit linkNamesChanged(); LinkInterface* newPriorityLink = NULL; @@ -1696,7 +1684,7 @@ void Vehicle::_updatePriorityLink(void) // Check for the existing priority link to still be valid for (int i=0; i<_links.count(); i++) { if (_priorityLink.data() == _links[i]) { - if (!_priorityLink.data()->highLatency()) { + if (!_priorityLink.data()->highLatency() && _priorityLink->active()) { // Link is still valid. Continue to use it unless it is high latency. In that case we still look for a better // link to use as priority link. return; @@ -1705,12 +1693,13 @@ void Vehicle::_updatePriorityLink(void) } // The previous priority link is no longer valid. We must no find the best link available in this priority order: - // Direct USB connection - // Not a high latency link + // First active direct USB connection + // Any active non high latency link + // An active high latency link // Any link #ifndef NO_SERIAL_LINK - // Search for direct usb connection + // Search for an active direct usb connection for (int i=0; i<_links.count(); i++) { LinkInterface* link = _links[i]; SerialLink* pSerialLink = qobject_cast(link); @@ -1719,7 +1708,7 @@ void Vehicle::_updatePriorityLink(void) if (config) { SerialConfiguration* pSerialConfig = qobject_cast(config); if (pSerialConfig && pSerialConfig->usbDirect()) { - if (_priorityLink.data() != link) { + if (_priorityLink.data() != link && link->active()) { newPriorityLink = link; break; } @@ -1731,10 +1720,21 @@ void Vehicle::_updatePriorityLink(void) #endif if (!newPriorityLink) { - // Search for non-high latency link + // Search for an active non-high latency link for (int i=0; i<_links.count(); i++) { LinkInterface* link = _links[i]; - if (!link->highLatency()) { + if (!link->highLatency() && link->active()) { + newPriorityLink = link; + break; + } + } + } + + if (!newPriorityLink) { + // Search for an active high latency link + for (int i=0; i<_links.count(); i++) { + LinkInterface* link = _links[i]; + if (link->highLatency() && link->active()) { newPriorityLink = link; break; } @@ -1746,9 +1746,14 @@ void Vehicle::_updatePriorityLink(void) newPriorityLink = _links[0]; } - _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); - _updateHighLatencyLink(); - emit priorityLinkNameChanged(_priorityLink->getName()); + if (_priorityLink.data() != newPriorityLink) { + _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); + _updateHighLatencyLink(); + emit priorityLinkNameChanged(_priorityLink->getName()); + if (updateActive) { + _linkActiveChanged(_priorityLink.data(), _priorityLink->active()); + } + } } void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64) @@ -2125,6 +2130,7 @@ void Vehicle::setPriorityLinkByName(const QString& priorityLinkName) _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); _updateHighLatencyLink(); emit priorityLinkNameChanged(_priorityLink->getName()); + _linkActiveChanged(_priorityLink.data(), _priorityLink->active()); } } @@ -2428,43 +2434,51 @@ void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled) } } -void Vehicle::_connectionLostTimeout(void) +void Vehicle::_linkActiveChanged(LinkInterface *link, bool active) { - if (highLatencyLink()) { - // No connection timeout on high latency links - return; - } + qWarning() << "Vehicle active changed called"; + if (link == _priorityLink) { + if (active && _connectionLost) { + // communication to priority link regained + _connectionLost = false; + _say(QString(tr("%1 communication to priority link %2 regained")).arg(_vehicleIdSpeech()).arg(link->getName())); - if (_connectionLostEnabled && !_connectionLost) { - _connectionLost = true; - _heardFrom = false; - _maxProtoVersion = 0; - emit connectionLostChanged(true); - _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech())); - if (_autoDisconnect) { + // 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 - // Reset link state - for (int i = 0; i < _links.length(); i++) { - _mavlink->resetMetadataForLink(_links.at(i)); - } - disconnectInactiveVehicle(); - } - } -} + } else if (!active && !_connectionLost) { + // communication to priority link lost + _say(QString(tr("%1 communication to priority link %2 lost")).arg(_vehicleIdSpeech()).arg(link->getName())); + _updatePriorityLink(false); -void Vehicle::_connectionActive(void) -{ - _connectionLostTimer.start(); - if (_connectionLost) { - _connectionLost = false; - emit connectionLostChanged(false); - _say(QString(tr("%1 communication regained")).arg(_vehicleIdSpeech())); + if (link == _priorityLink) { + _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech())); - // 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 (_connectionLostEnabled) { + _connectionLost = true; + _heardFrom = false; + _maxProtoVersion = 0; + emit connectionLostChanged(true); + + if (_autoDisconnect) { + // Reset link state + for (int i = 0; i < _links.length(); i++) { + _mavlink->resetMetadataForLink(_links.at(i)); + } + disconnectInactiveVehicle(); + } + } + + } else { + _say(QString(tr("%1 use %2 as the priority link")).arg(_vehicleIdSpeech()).arg(link->getName())); + // nothing more to do + } + } + } else { + _say(QString(tr("%1 communication to auxiliary link %2 %3")).arg(_vehicleIdSpeech()).arg(link->getName()).arg(active ? "regained" : "lost")); } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 4c8c5d3b7..7277cc1fe 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -1042,7 +1042,6 @@ private slots: void _updateAttitude (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp); /** @brief A new camera image has arrived */ void _imageReady (UASInterface* uas); - void _connectionLostTimeout(void); void _prearmErrorTimeout(void); void _missionLoadComplete(void); void _geoFenceLoadComplete(void); @@ -1100,14 +1099,14 @@ private: void _rallyPointManagerError(int errorCode, const QString& errorMsg); void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); - void _connectionActive(void); + void _linkActiveChanged(LinkInterface* link, bool active); void _say(const QString& text); QString _vehicleIdSpeech(void); void _handleMavlinkLoggingData(mavlink_message_t& message); void _handleMavlinkLoggingDataAcked(mavlink_message_t& message); void _ackMavlinkLogData(uint16_t sequence); void _sendNextQueuedMavCommand(void); - void _updatePriorityLink(void); + void _updatePriorityLink(bool updateActive); void _commonInit(void); void _startPlanRequest(void); void _setupAutoDisarmSignalling(void); @@ -1199,8 +1198,6 @@ private: // Lost connection handling bool _connectionLost; bool _connectionLostEnabled; - static const int _connectionLostTimeoutMSecs = 3500; // Signal connection lost after 3.5 seconds of missed heartbeat - QTimer _connectionLostTimer; bool _initialPlanRequestComplete; diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index f2920712e..2539cf3b5 100644 --- a/src/comm/LinkInterface.cc +++ b/src/comm/LinkInterface.cc @@ -43,6 +43,14 @@ 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 @@ -159,3 +167,23 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel) _mavlinkChannelSet = true; _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() +{ + if (_active && !_highLatency) { + setActive(false); + } +} diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 001dc0deb..be0c62672 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -17,6 +17,7 @@ #include #include #include +#include #include "QGCMAVLink.h" #include "LinkConfiguration.h" @@ -42,7 +43,7 @@ public: // Property accessors bool active(void) { return _active; } - void setActive(bool active) { _active = active; emit activeChanged(active); } + void setActive(bool active) { _active = active; emit activeChanged(this, active); } LinkConfiguration* getLinkConfiguration(void) { return _config.data(); } @@ -152,7 +153,7 @@ private slots: signals: void autoconnectChanged(bool autoconnect); - void activeChanged(bool active); + void activeChanged(LinkInterface* link, bool active); void _invokeWriteBytes(QByteArray); void highLatencyChanged(bool highLatency); @@ -248,7 +249,11 @@ private: virtual bool _connect(void) = 0; 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); @@ -276,6 +281,9 @@ private: bool _active; ///< true: link is actively receiving mavlink messages bool _enableRateCollection; 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; }; typedef QSharedPointer SharedLinkInterfacePointer; -- 2.22.0