diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 44efb3d229122fc44dd32c83477d54b6b8fdae35..ed2714441ff499c8f68b7cdd14bafddf1551a2ba 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -393,12 +393,14 @@ HEADERS += \ src/api/QGCOptions.h \ src/api/QGCSettings.h \ src/api/QmlComponentInfo.h \ + src/comm/HeartbeatTimer.h SOURCES += \ src/api/QGCCorePlugin.cc \ src/api/QGCOptions.cc \ src/api/QGCSettings.cc \ src/api/QmlComponentInfo.cc \ + src/comm/HeartbeatTimer.cc # # Unit Test specific configuration goes here (requires full debug build with all plugins) diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc index 7555cd38f129aada6df5dc73a9db9065dcd5de75..fded1e00dee298b48229b0bf51a7198408f1d545 100644 --- a/qgroundcontrol.qrc +++ b/qgroundcontrol.qrc @@ -9,10 +9,11 @@ src/ui/toolbar/GPSRTKIndicator.qml src/ui/toolbar/MessageIndicator.qml src/ui/toolbar/ModeIndicator.qml - src/ui/toolbar/VTOLModeIndicator.qml + src/ui/toolbar/VTOLModeIndicator.qml src/ui/toolbar/RCRSSIIndicator.qml src/ui/toolbar/TelemetryRSSIIndicator.qml src/ui/toolbar/JoystickIndicator.qml + src/ui/toolbar/LinkIndicator.qml src/PlanView/CorridorScanEditor.qml @@ -126,7 +127,7 @@ src/QmlControls/SliderSwitch.qml src/QmlControls/SubMenuButton.qml src/PlanView/SurveyMapVisual.qml - src/PlanView/TransectStyleComplexItemStats.qml + src/PlanView/TransectStyleComplexItemStats.qml src/QmlControls/ToolStrip.qml src/QmlControls/VehicleRotationCal.qml src/QmlControls/VehicleSummaryRow.qml @@ -139,7 +140,7 @@ src/FactSystem/FactControls/FactTextField.qml src/FactSystem/FactControls/FactTextFieldGrid.qml src/FactSystem/FactControls/FactTextFieldRow.qml - src/FactSystem/FactControls/FactValueSlider.qml + src/FactSystem/FactControls/FactValueSlider.qml src/FactSystem/FactControls/qmldir src/FlightDisplay/FlightDisplayView.qml src/FlightDisplay/FlightDisplayViewMap.qml @@ -225,10 +226,10 @@ src/comm/USBBoardInfo.json src/Vehicle/BatteryFact.json src/Vehicle/ClockFact.json - src/Vehicle/DistanceSensorFact.json + src/Vehicle/DistanceSensorFact.json src/Vehicle/GPSFact.json src/Vehicle/GPSRTKFact.json - src/Vehicle/SetpointFact.json + src/Vehicle/SetpointFact.json src/Vehicle/SubmarineFact.json src/Vehicle/TemperatureFact.json src/Vehicle/VehicleFact.json diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 39b5c7689572c3571fdec8d3777d9ae26b2698d0..344748ac96bf8aac2659006ad708564f3f43d3ed 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -344,6 +344,7 @@ const QVariantList &FirmwarePlugin::toolBarIndicators(const Vehicle* vehicle) _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/VTOLModeIndicator.qml"))); _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/ArmedIndicator.qml"))); _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/GPSRTKIndicator.qml"))); + _toolBarIndicatorList.append(QVariant::fromValue(QUrl::fromUserInput("qrc:/toolbar/LinkIndicator.qml"))); } return _toolBarIndicatorList; } diff --git a/src/QGCApplication.cc b/src/QGCApplication.cc index 8bfd850b3d3e896e58d4101bbc6bbf7efedd432c..35dbde0ebd4d61f057de60c6fa182f87612bd8cf 100644 --- a/src/QGCApplication.cc +++ b/src/QGCApplication.cc @@ -361,6 +361,7 @@ void QGCApplication::_initCommon(void) qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "ParameterManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "QGCCameraControl", "Reference only"); + qmlRegisterUncreatableType ("QGroundControl.Vehicle", 1, 0, "LinkInterface", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "JoystickManager", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.JoystickManager", 1, 0, "Joystick", "Reference only"); qmlRegisterUncreatableType ("QGroundControl.QGCPositionManager", 1, 0, "QGCPositionManager", "Reference only"); diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index cbe4bdea6ee1eb4de6842f290ff453a9e73d7b2d..478a8b1a6dba2f23a86a0f919554d857fd7b8ddb 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -138,9 +138,6 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle setActiveVehicle(vehicle); } - // Mark link as active - link->setActive(true); - #if defined (__ios__) || defined(__android__) if(_vehicles.count() == 1) { //-- Once a vehicle is connected, keep screen from going off diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 128ab8b45a16258df0b0eb512e9db030ba1c44df..29e1c9942122317e4eb169f94f5215b5b16b7269 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -168,6 +168,7 @@ Vehicle::Vehicle(LinkInterface* link, , _gitHash(versionNotSetValue) , _uid(0) , _lastAnnouncedLowBatteryPercent(100) + , _priorityLinkCommanded(false) , _rollFact (0, _rollFactName, FactMetaData::valueTypeDouble) , _pitchFact (0, _pitchFactName, FactMetaData::valueTypeDouble) , _headingFact (0, _headingFactName, FactMetaData::valueTypeDouble) @@ -191,8 +192,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 +199,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); @@ -222,12 +223,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 +601,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; @@ -1160,6 +1148,7 @@ void Vehicle::_handleCommandLong(mavlink_message_t& message) if (sl && sl->getSerialConfig()->usbDirect()) { qDebug() << "Disconnecting:" << _links.at(i)->getName(); qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i)); + emit linksChanged(); } } } @@ -1622,11 +1611,16 @@ void Vehicle::_addLink(LinkInterface* link) if (!_containsLink(link)) { qCDebug(VehicleLog) << "_addLink:" << QString("%1").arg((ulong)link, 0, 16); _links += link; - _updatePriorityLink(); - _updateHighLatencyLink(); + if (_links.count() <= 1) { + _updatePriorityLink(true /* updateActive */, false /* sendCommand */); + } else { + _updatePriorityLink(true /* updateActive */, true /* sendCommand */); + } + 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 +1629,12 @@ void Vehicle::_linkInactiveOrDeleted(LinkInterface* link) qCDebug(VehicleLog) << "_linkInactiveOrDeleted linkCount" << _links.count(); _links.removeOne(link); - _updatePriorityLink(); + + if (_priorityLink.data() == link) { + _priorityLink.clear(); + } + + _updatePriorityLink(true /* updateActive */, true /* sendCommand */); if (_links.count() == 0 && !_allLinksInactiveSent) { qCDebug(VehicleLog) << "All links inactive"; @@ -1681,8 +1680,19 @@ void Vehicle::_sendMessageOnLink(LinkInterface* link, mavlink_message_t message) emit messagesSentChanged(); } -void Vehicle::_updatePriorityLink(void) +void Vehicle::_updatePriorityLink(bool updateActive, bool sendCommand) { + emit linksPropertiesChanged(); + + // if the priority link is commanded and still active don't change anything + if (_priorityLinkCommanded) { + if (_priorityLink.data()->link_active(_id)) { + return; + } else { + _priorityLinkCommanded = false; + } + } + LinkInterface* newPriorityLink = NULL; // This routine specifically does not clear _priorityLink when there are no links remaining. @@ -1695,7 +1705,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->link_active(_id)) { // 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; @@ -1704,12 +1714,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); @@ -1718,7 +1729,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->link_active(_id)) { newPriorityLink = link; break; } @@ -1730,10 +1741,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->link_active(_id)) { + 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->link_active(_id)) { newPriorityLink = link; break; } @@ -1745,8 +1767,19 @@ void Vehicle::_updatePriorityLink(void) newPriorityLink = _links[0]; } - _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); - _updateHighLatencyLink(); + if (_priorityLink.data() != newPriorityLink) { + if (_priorityLink) { + qgcApp()->showMessage((tr("switch to %2 as priority link")).arg(newPriorityLink->getName())); + } + _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); + + _updateHighLatencyLink(sendCommand); + + emit priorityLinkNameChanged(_priorityLink->getName()); + if (updateActive) { + _linkActiveChanged(_priorityLink.data(), _priorityLink->link_active(_id), _id); + } + } } void Vehicle::_updateAttitude(UASInterface*, double roll, double pitch, double yaw, quint64) @@ -2088,6 +2121,53 @@ void Vehicle::setFlightMode(const QString& flightMode) } } +QString Vehicle::priorityLinkName(void) const +{ + if (_priorityLink) { + return _priorityLink->getName(); + } + + return "none"; +} + +QVariantList Vehicle::links(void) const { + QVariantList ret; + + foreach( const auto &item, _links ) + ret << QVariant::fromValue(item); + + return ret; +} + +void Vehicle::setPriorityLinkByName(const QString& priorityLinkName) +{ + if (!_priorityLink) { + return; + } + + if (priorityLinkName == _priorityLink->getName()) { + // The link did not change + return; + } + + LinkInterface* newPriorityLink = NULL; + + + for (int i=0; i<_links.count(); i++) { + if (_links[i]->getName() == priorityLinkName) { + newPriorityLink = _links[i]; + } + } + + if (newPriorityLink) { + _priorityLinkCommanded = true; + _priorityLink = _toolbox->linkManager()->sharedLinkInterfacePointerForLink(newPriorityLink); + _updateHighLatencyLink(true); + emit priorityLinkNameChanged(_priorityLink->getName()); + _linkActiveChanged(_priorityLink.data(), _priorityLink->link_active(_id), _id); + } +} + bool Vehicle::hilMode(void) { return _base_mode & MAV_MODE_FLAG_HIL_ENABLED; @@ -2332,7 +2412,6 @@ void Vehicle::disconnectInactiveVehicle(void) { // Vehicle is no longer communicating with us, disconnect all links - LinkManager* linkMgr = _toolbox->linkManager(); for (int i=0; i<_links.count(); i++) { // FIXME: This linkInUse check is a hack fix for multiple vehicles on the same link. @@ -2341,6 +2420,7 @@ void Vehicle::disconnectInactiveVehicle(void) linkMgr->disconnectLink(_links[i]); } } + emit linksChanged(); } void Vehicle::_imageReady(UASInterface*) @@ -2388,43 +2468,62 @@ void Vehicle::setConnectionLostEnabled(bool connectionLostEnabled) } } -void Vehicle::_connectionLostTimeout(void) +void Vehicle::_linkActiveChanged(LinkInterface *link, bool active, int vehicleID) { - if (highLatencyLink()) { - // No connection timeout on high latency links + // only continue if the vehicle id is correct + if (vehicleID != _id) { return; } - if (_connectionLostEnabled && !_connectionLost) { - _connectionLost = true; - _heardFrom = false; - _maxProtoVersion = 0; - emit connectionLostChanged(true); - _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech())); - if (_autoDisconnect) { + emit linksPropertiesChanged(); - // Reset link state - for (int i = 0; i < _links.length(); i++) { - _mavlink->resetMetadataForLink(_links.at(i)); + if (link == _priorityLink) { + if (active && _connectionLost) { + // communication to priority link regained + _connectionLost = false; + emit connectionLostChanged(false); + qgcApp()->showMessage((tr("%1 communication to %2 link %3 regained")).arg(_vehicleIdSpeech()).arg((_links.count() > 1) ? "priority" : "").arg(link->getName())); + + 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 } - disconnectInactiveVehicle(); - } - } -} -void Vehicle::_connectionActive(void) -{ - _connectionLostTimer.start(); - if (_connectionLost) { - _connectionLost = false; - emit connectionLostChanged(false); - _say(QString(tr("%1 communication regained")).arg(_vehicleIdSpeech())); + } else if (!active && !_connectionLost) { + // communication to priority link lost + qgcApp()->showMessage((tr("%1 communication to %2 link %3 lost")).arg(_vehicleIdSpeech()).arg((_links.count() > 1) ? "priority" : "").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 + _updatePriorityLink(false /* updateActive */, true /* sendCommand */); + + if (link == _priorityLink) { + _say(QString(tr("%1 communication lost")).arg(_vehicleIdSpeech())); + qgcApp()->showMessage((tr("%1 communication lost")).arg(_vehicleIdSpeech())); + + 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 { + qgcApp()->showMessage((tr("%1 communication to auxiliary link %2 %3")).arg(_vehicleIdSpeech()).arg(link->getName()).arg(active ? "regained" : "lost")); + _updatePriorityLink(false /* updateActive */, true /* sendCommand */); } } @@ -3300,12 +3399,23 @@ void Vehicle::_vehicleParamLoaded(bool ready) } } -void Vehicle::_updateHighLatencyLink(void) +void Vehicle::_updateHighLatencyLink(bool sendCommand) { + if (!_priorityLink) { + return; + } + if (_priorityLink->highLatency() != _highLatencyLink) { _highLatencyLink = _priorityLink->highLatency(); _mavCommandAckTimer.setInterval(_highLatencyLink ? _mavCommandAckTimeoutMSecsHighLatency : _mavCommandAckTimeoutMSecs); emit highLatencyLinkChanged(_highLatencyLink); + + if (sendCommand) { + sendMavCommand(defaultComponentId(), + MAV_CMD_CONTROL_HIGH_LATENCY, + true, + _highLatencyLink ? 1.0f : 0.0f); // request start/stop transmitting over high latency telemetry + } } } diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 1323f6fb2d687255da10da365ce098a99173b6fa..0f1265976a10c153c1ecaceb9d6f0fb8c0041c01 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -10,6 +10,7 @@ #pragma once #include +#include #include #include "FactGroup.h" @@ -493,6 +494,8 @@ public: Q_PROPERTY(bool vtolInFwdFlight READ vtolInFwdFlight WRITE setVtolInFwdFlight NOTIFY vtolInFwdFlightChanged) Q_PROPERTY(bool highLatencyLink READ highLatencyLink NOTIFY highLatencyLinkChanged) Q_PROPERTY(bool supportsTerrainFrame READ supportsTerrainFrame NOTIFY firmwareTypeChanged) + Q_PROPERTY(QString priorityLinkName READ priorityLinkName WRITE setPriorityLinkByName NOTIFY priorityLinkNameChanged) + Q_PROPERTY(QVariantList links READ links NOTIFY linksChanged) // Vehicle state used for guided control Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) ///< Vehicle is flying @@ -501,7 +504,7 @@ public: Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT) ///< Guided mode commands are supported by this vehicle Q_PROPERTY(bool pauseVehicleSupported READ pauseVehicleSupported CONSTANT) ///< Pause vehicle command is supported Q_PROPERTY(bool orbitModeSupported READ orbitModeSupported CONSTANT) ///< Orbit mode is supported by this vehicle - Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported + Q_PROPERTY(bool takeoffVehicleSupported READ takeoffVehicleSupported CONSTANT) ///< Guided takeoff supported Q_PROPERTY(ParameterManager* parameterManager READ parameterManager CONSTANT) @@ -692,6 +695,10 @@ public: QString flightMode(void) const; void setFlightMode(const QString& flightMode); + QString priorityLinkName(void) const; + QVariantList links(void) const; + void setPriorityLinkByName(const QString& priorityLinkName); + bool hilMode(void); void setHilMode(bool hilMode); @@ -943,6 +950,9 @@ signals: void capabilityBitsChanged(uint64_t capabilityBits); void toolBarIndicatorsChanged(void); void highLatencyLinkChanged(bool highLatencyLink); + void priorityLinkNameChanged(const QString& priorityLinkName); + void linksChanged(void); + void linksPropertiesChanged(void); void messagesReceivedChanged (); void messagesSentChanged (); @@ -1024,7 +1034,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); @@ -1034,7 +1044,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); @@ -1092,14 +1101,14 @@ private: void _rallyPointManagerError(int errorCode, const QString& errorMsg); void _mapTrajectoryStart(void); void _mapTrajectoryStop(void); - void _connectionActive(void); + void _linkActiveChanged(LinkInterface* link, bool active, int vehicleID); 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, bool sendCommand); void _commonInit(void); void _startPlanRequest(void); void _setupAutoDisarmSignalling(void); @@ -1191,8 +1200,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; @@ -1266,6 +1273,7 @@ private: int _lastAnnouncedLowBatteryPercent; SharedLinkInterfacePointer _priorityLink; // We always keep a reference to the priority link to manage shutdown ordering + bool _priorityLinkCommanded; // FactGroup facts diff --git a/src/comm/HeartbeatTimer.cc b/src/comm/HeartbeatTimer.cc new file mode 100644 index 0000000000000000000000000000000000000000..7090b736701685f346f3787bfeaf563ca500a464 --- /dev/null +++ b/src/comm/HeartbeatTimer.cc @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * (c) 2009-2018 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "HeartbeatTimer.h" + +#include + +HeartbeatTimer::HeartbeatTimer(int vehicle_id, bool high_latency) : + _active(true), + _timer(new QTimer), + _vehicleID(vehicle_id), + _high_latency(high_latency) +{ + if (!high_latency) { + _timer->setInterval(_heartbeatReceivedTimeoutMSecs); + _timer->setSingleShot(true); + _timer->start(); + } + emit activeChanged(true, _vehicleID); + QObject::connect(_timer, &QTimer::timeout, this, &HeartbeatTimer::timerTimeout); +} + +HeartbeatTimer::~HeartbeatTimer() { + if (_timer) { + QObject::disconnect(_timer, &QTimer::timeout, this, &HeartbeatTimer::timerTimeout); + _timer->stop(); + delete _timer; + _timer = nullptr; + } + + emit activeChanged(false, _vehicleID); +} + +void HeartbeatTimer::restartTimer() +{ + if (!_active) { + _active = true; + emit activeChanged(true, _vehicleID); + } + + _timer->start(); +} + +void HeartbeatTimer::timerTimeout() +{ + if (!_high_latency) { + if (_active) { + _active = false; + emit activeChanged(false, _vehicleID); + } + emit heartbeatTimeout(_vehicleID); + } +} diff --git a/src/comm/HeartbeatTimer.h b/src/comm/HeartbeatTimer.h new file mode 100644 index 0000000000000000000000000000000000000000..c62a63f695d43e9d370a021e391fd632cefbad10 --- /dev/null +++ b/src/comm/HeartbeatTimer.h @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * (c) 2009-2018 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#ifndef _HEARTBEATTIMER_H_ +#define _HEARTBEATTIMER_H_ + +#include +#include + +/** + * @brief The HeartbeatTimer class + * + * Track the heartbeat for a single vehicle on one link. + * As long as regular heartbeats are received the status is active. On the timer timeout + * status is set to inactive. On any status change the activeChanged signal is emitted. + * If high_latency is true then active is always true. + */ +class HeartbeatTimer : public QObject +{ + Q_OBJECT + +public: + /** + * @brief HeartbeatTimer + * + * Constructor + * + * @param vehicle_id: The vehicle ID for which the heartbeat will be tracked. + * @param high_latency: Indicates if the link is a high latency one. + */ + HeartbeatTimer(int vehicle_id, bool high_latency); + + ~HeartbeatTimer(); + + /** + * @brief getActive + * @return The current value of active + */ + bool getActive() const { return _active; } + + /** + * @brief getVehicleID + * @return The vehicle ID + */ + int getVehicleID() const { return _vehicleID; } + + /** + * @brief restartTimer + * + * Restarts the timer and emits the signal if the timer was previously inactive + */ + void restartTimer(); + +signals: + /** + * @brief heartbeatTimeout + * + * Emitted if no heartbeat is received over the specified time. + * + * @param vehicle_id: The vehicle ID for which the heartbeat timed out. + */ + void heartbeatTimeout(int vehicle_id); + + /** + * @brief activeChanged + * + * Emitted if the active status changes. + * + * @param active: The new value of the active state. + * @param vehicle_id: The vehicle ID for which the active state changed. + */ + void activeChanged(bool active, int vehicle_id); +private slots: + /** + * @brief timerTimeout + * + * Handle the timer timeout. + * + * Emit the signals according to the current state for regular links. + * Do nothing for a high latency link. + */ + void timerTimeout(); + +private: + bool _active = false; // The state of active. Is true if the timer has not timed out. + QTimer* _timer = nullptr; // Single shot timer + int _vehicleID = -1; // Vehicle ID for which the heartbeat is tracked. + bool _high_latency = false; // Indicates if the link is a high latency link or not. + + static const int _heartbeatReceivedTimeoutMSecs = 3500; // Signal connection lost after 3.5 seconds of no messages +}; + +#endif // _HEARTBEATTIMER_H_ diff --git a/src/comm/LinkInterface.cc b/src/comm/LinkInterface.cc index f2920712efce1b076baafeee3637bcf9fca352c5..49de99628c189dc2b511320e6a3b2d2fe792f39f 100644 --- a/src/comm/LinkInterface.cc +++ b/src/comm/LinkInterface.cc @@ -10,6 +10,28 @@ #include "LinkInterface.h" #include "QGCApplication.h" +bool LinkInterface::active() const +{ + QMapIterator iter(_heartbeatTimers); + while (iter.hasNext()) { + iter.next(); + if (iter.value()->getActive()) { + return true; + } + } + + return false; +} + +bool LinkInterface::link_active(int vehicle_id) const +{ + if (_heartbeatTimers.contains(vehicle_id)) { + return _heartbeatTimers.value(vehicle_id)->getActive(); + } else { + return false; + } +} + /// mavlink channel to use for this link, as used by mavlink_parse_char. The mavlink channel is only /// set into the link when it is added to LinkManager uint8_t LinkInterface::mavlinkChannel(void) const @@ -25,10 +47,11 @@ LinkInterface::LinkInterface(SharedLinkConfigurationPointer& config) , _config (config) , _highLatency (config->isHighLatency()) , _mavlinkChannelSet (false) - , _active (false) , _enableRateCollection (false) , _decodedFirstMavlinkPacket(false) { + QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership); + _config->setLink(this); // Initialize everything for the data rate calculation buffers. @@ -159,3 +182,29 @@ void LinkInterface::_setMavlinkChannel(uint8_t channel) _mavlinkChannelSet = true; _mavlinkChannel = channel; } + +void LinkInterface::_activeChanged(bool active, int vehicle_id) +{ + emit activeChanged(this, active, vehicle_id); +} + +void LinkInterface::startHeartbeatTimer(int vehicle_id) { + if (_heartbeatTimers.contains(vehicle_id)) { + _heartbeatTimers.value(vehicle_id)->restartTimer(); + } else { + _heartbeatTimers.insert(vehicle_id, new HeartbeatTimer(vehicle_id, _highLatency)); + QObject::connect(_heartbeatTimers.value(vehicle_id), &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged); + } +} + +void LinkInterface::stopHeartbeatTimer() { + QMapIterator iter(_heartbeatTimers); + while (iter.hasNext()) { + iter.next(); + QObject::disconnect(iter.value(), &HeartbeatTimer::activeChanged, this, &LinkInterface::_activeChanged); + delete _heartbeatTimers[iter.key()]; + _heartbeatTimers[iter.key()] = nullptr; + } + + _heartbeatTimers.clear(); +} diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index 001dc0debb900d276d73280c108bb36e8025a304..3455279fe7bc1c0bca45ed9c4b9c98b2ceeba2f0 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -17,9 +17,11 @@ #include #include #include +#include #include "QGCMAVLink.h" #include "LinkConfiguration.h" +#include "HeartbeatTimer.h" class LinkManager; @@ -36,13 +38,17 @@ class LinkInterface : public QThread friend class LinkManager; public: - ~LinkInterface() { _config->setLink(NULL); } + virtual ~LinkInterface() { + stopHeartbeatTimer(); + _config->setLink(NULL); + } - Q_PROPERTY(bool active READ active WRITE setActive NOTIFY activeChanged) + Q_PROPERTY(bool active READ active NOTIFY activeChanged) // Property accessors - bool active(void) { return _active; } - void setActive(bool active) { _active = active; emit activeChanged(active); } + bool active() const; + Q_INVOKABLE bool link_active(int vehicle_id) const; + Q_INVOKABLE bool getHighLatency(void) const { return _highLatency; } LinkConfiguration* getLinkConfiguration(void) { return _config.data(); } @@ -51,7 +57,7 @@ public: /** * @brief Get the human readable name of this link */ - virtual QString getName() const = 0; + Q_INVOKABLE virtual QString getName() const = 0; virtual void requestReset() = 0; @@ -149,10 +155,12 @@ public slots: private slots: virtual void _writeBytes(const QByteArray) = 0; + + void _activeChanged(bool active, int vehicle_id); signals: void autoconnectChanged(bool autoconnect); - void activeChanged(bool active); + void activeChanged(LinkInterface* link, bool active, int vehicle_id); void _invokeWriteBytes(QByteArray); void highLatencyChanged(bool highLatency); @@ -248,10 +256,25 @@ private: virtual bool _connect(void) = 0; virtual void _disconnect(void) = 0; - + /// Sets the mavlink channel to use for this link void _setMavlinkChannel(uint8_t channel); + /** + * @brief startHeartbeatTimer + * + * Start/restart the heartbeat timer for the specific vehicle. + * If no timer exists an instance is allocated. + */ + void startHeartbeatTimer(int vehicle_id); + + /** + * @brief stopHeartbeatTimer + * + * Stop and deallocate the heartbeat timers for all vehicles if any exists. + */ + void stopHeartbeatTimer(); + bool _mavlinkChannelSet; ///< true: _mavlinkChannel has been set uint8_t _mavlinkChannel; ///< mavlink channel to use for this link, as used by mavlink_parse_char @@ -273,9 +296,10 @@ private: mutable QMutex _dataRateMutex; // Mutex for accessing the data rate member variables - bool _active; ///< true: link is actively receiving mavlink messages bool _enableRateCollection; bool _decodedFirstMavlinkPacket; ///< true: link has correctly decoded it's first mavlink packet + + QMap _heartbeatTimers; }; typedef QSharedPointer SharedLinkInterfacePointer; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index a425e1a81ef45e4f1e32b975dc55868cba74b8e0..aa2204f6ff408cf3c4a98698ff65c3360899deee 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -80,6 +80,8 @@ void LinkManager::setToolbox(QGCToolbox *toolbox) _autoConnectSettings = toolbox->settingsManager()->autoConnectSettings(); _mavlinkProtocol = _toolbox->mavlinkProtocol(); + connect(_mavlinkProtocol, &MAVLinkProtocol::vehicleHeartbeatInfo, this, &LinkManager::_heartbeatReceived); + connect(&_portListTimer, &QTimer::timeout, this, &LinkManager::_updateAutoConnectLinks); _portListTimer.start(_autoconnectUpdateTimerMSecs); // timeout must be long enough to get past bootloader on second pass @@ -172,7 +174,7 @@ LinkInterface* LinkManager::createConnectedLink(const QString& name) void LinkManager::_addLink(LinkInterface* link) { if (thread() != QThread::currentThread()) { - qWarning() << "_deleteLink called from incorrect thread"; + qWarning() << "_addLink called from incorrect thread"; return; } @@ -338,6 +340,7 @@ void LinkManager::saveLinkConfigurationList() settings.setValue(root + "/name", linkConfig->name()); settings.setValue(root + "/type", linkConfig->type()); settings.setValue(root + "/auto", linkConfig->isAutoConnect()); + settings.setValue(root + "/high_latency", linkConfig->isHighLatency()); // Have the instance save its own values linkConfig->saveSettings(settings, root); } @@ -369,6 +372,7 @@ void LinkManager::loadLinkConfigurationList() if(!name.isEmpty()) { LinkConfiguration* pLink = NULL; bool autoConnect = settings.value(root + "/auto").toBool(); + bool highLatency = settings.value(root + "/high_latency").toBool(); switch((LinkConfiguration::LinkType)type) { #ifndef NO_SERIAL_LINK case LinkConfiguration::TypeSerial: @@ -403,6 +407,7 @@ void LinkManager::loadLinkConfigurationList() if(pLink) { //-- Have the instance load its own values pLink->setAutoConnect(autoConnect); + pLink->setHighLatency(highLatency); pLink->loadSettings(settings, root); addConfiguration(pLink); linksChanged = true; @@ -1000,3 +1005,11 @@ void LinkManager::_freeMavlinkChannel(int channel) { _mavlinkChannelsUsedBitMask &= ~(1 << channel); } + +void LinkManager::_heartbeatReceived(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType) { + Q_UNUSED(componentId); + Q_UNUSED(vehicleFirmwareType); + Q_UNUSED(vehicleType); + + link->startHeartbeatTimer(vehicleId); +} diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index bd719e364305cc1dc8048d5b55a75af7ec4521e7..13c0ed85261fd3f2fff06125879fb3148359ef64 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -204,6 +204,8 @@ private: SerialConfiguration* _autoconnectConfigurationsContainsPort(const QString& portName); #endif + void _heartbeatReceived(LinkInterface* link, int vehicleId, int componentId, int vehicleFirmwareType, int vehicleType); + 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 diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index be016aca4b61fd1eec05f4fb7ffed38c2156e18f..ecbf26ccd1a323dc48253a46f44254a6d8ec4f90 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -309,7 +309,7 @@ bool SerialLink::isConnected() const QString SerialLink::getName() const { - return _serialConfig->portName(); + return _serialConfig->name(); } /** diff --git a/src/ui/toolbar/LinkIndicator.qml b/src/ui/toolbar/LinkIndicator.qml new file mode 100644 index 0000000000000000000000000000000000000000..a7238d44d6a85941ed1b92304d119ca03e973c88 --- /dev/null +++ b/src/ui/toolbar/LinkIndicator.qml @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + + +import QtQuick 2.3 +import QtQuick.Controls 1.2 + +import QGroundControl 1.0 +import QGroundControl.Controls 1.0 +import QGroundControl.MultiVehicleManager 1.0 +import QGroundControl.ScreenTools 1.0 +import QGroundControl.Palette 1.0 +import QGroundControl.Vehicle 1.0 + +//------------------------------------------------------------------------- +//-- Link Indicator +Item { + anchors.top: parent.top + anchors.bottom: parent.bottom + width: priorityLinkSelector.width + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property bool _visible: false + + QGCLabel { + id: priorityLinkSelector + text: _activeVehicle ? _activeVehicle.priorityLinkName : qsTr("N/A", "No data to display") + font.pointSize: ScreenTools.mediumFontPointSize + color: qgcPal.buttonText + anchors.verticalCenter: parent.verticalCenter + visible: _visible + Menu { + id: linkSelectionMenu + } + Component { + id: linkSelectionMenuItemComponent + MenuItem { + onTriggered: _activeVehicle.priorityLinkName = text + } + } + property var linkSelectionMenuItems: [] + function updatelinkSelectionMenu() { + if (_activeVehicle) { + // Remove old menu items + for (var i = 0; i < linkSelectionMenuItems.length; i++) { + linkSelectionMenu.removeItem(linkSelectionMenuItems[i]) + } + linkSelectionMenuItems.length = 0 + + // Add new items + var has_hl = false; + var links = _activeVehicle.links + for (var i = 0; i < links.length; i++) { + var menuItem = linkSelectionMenuItemComponent.createObject(null, { "text": links[i].getName(), "enabled": links[i].link_active(_activeVehicle.id)}) + linkSelectionMenuItems.push(menuItem) + linkSelectionMenu.insertItem(i, menuItem) + + if (links[i].getHighLatency()) { + has_hl = true + } + } + + _visible = links.length > 1 && has_hl + } + } + + Component.onCompleted: priorityLinkSelector.updatelinkSelectionMenu() + + Connections { + target: QGroundControl.multiVehicleManager + onActiveVehicleChanged: priorityLinkSelector.updatelinkSelectionMenu() + } + + Connections { + target: _activeVehicle + onLinksChanged: priorityLinkSelector.updatelinkSelectionMenu() + } + + Connections { + target: _activeVehicle + onLinksPropertiesChanged: priorityLinkSelector.updatelinkSelectionMenu() + } + + MouseArea { + visible: _activeVehicle + anchors.fill: parent + onClicked: linkSelectionMenu.popup() + } + } +}