diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 7473bc8..ed7ec96 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<SerialLink*>(link);
@@ -1719,7 +1708,7 @@ void Vehicle::_updatePriorityLink(void)
if (config) {
SerialConfiguration* pSerialConfig = qobject_cast<SerialConfiguration*>(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 4c8c5d3..7277cc1 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 f292071..2539cf3 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*>("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 001dc0d..be0c626 100644
--- a/src/comm/LinkInterface.h
+++ b/src/comm/LinkInterface.h
@@ -17,6 +17,7 @@
#include <QMetaType>
#include <QSharedPointer>
#include <QDebug>
+#include <QTimer>
#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<LinkInterface> SharedLinkInterfacePointer;