diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 7473bc8189242db0776f5cb1f52a9a15902373fb..ed7ec9648974c1414fbf48f9732e253f8fe5cc6d 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 4c8c5d3b78adcf7bf3b75110cf644e5991580bf0..7277cc1fe2cb2de1b25310f9b474cedf73518856 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 f2920712efce1b076baafeee3637bcf9fca352c5..2539cf3b538684ffdf9ace26a720a850f080e280 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 001dc0debb900d276d73280c108bb36e8025a304..be0c626720b3456d68f8b8bd8021f438a76e7c66 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;