diff --git a/src/Vehicle/MultiVehicleManager.cc b/src/Vehicle/MultiVehicleManager.cc index 019694d295efc4ed93e9b8d153e8ee6d6c0d091d..14523ff40bf1f9d7def42dc22f945196a2846a22 100644 --- a/src/Vehicle/MultiVehicleManager.cc +++ b/src/Vehicle/MultiVehicleManager.cc @@ -123,6 +123,9 @@ void MultiVehicleManager::_vehicleHeartbeatInfo(LinkInterface* link, int vehicle setActiveVehicle(vehicle); + // Mark link as active + link->setActive(true); + #if defined __android__ if(_vehicles.count() == 1) { //-- Once a vehicle is connected, keep Android screen from going off diff --git a/src/comm/LinkInterface.h b/src/comm/LinkInterface.h index cb5c7c3aa03b0e1c2f1bb0edbd1299f03d4a76a3..ff7393454c294d4da900aa66eacf533a56f1e08f 100644 --- a/src/comm/LinkInterface.h +++ b/src/comm/LinkInterface.h @@ -233,16 +233,6 @@ protected: _logDataRateToBuffer(_outDataWriteAmounts, _outDataWriteTimes, &_outDataIndex, byteCount, time); } -protected slots: - - /** - * @brief Read a number of bytes from the interface. - * - * @param bytes The pointer to write the bytes to - * @param maxLength The maximum length which can be written - **/ - virtual void readBytes() = 0; - private: /** * @brief logDataRateToBuffer Stores transmission times/amounts for statistics diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index aad959d575b06834b6cbf54628040c1e4d2909a5..3e95f3fa5c83ec0abf4986269af00899f73758d5 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #ifndef __ios__ #include "QGCSerialPortInfo.h" @@ -121,7 +122,7 @@ LinkInterface* LinkManager::createConnectedLink(LinkConfiguration* config) if (serialConfig) { pLink = new SerialLink(serialConfig); if (serialConfig->usbDirect()) { - _activeLinkCheckList.append(pLink); + _activeLinkCheckList.append((SerialLink*)pLink); if (!_activeLinkCheckTimer.isActive()) { _activeLinkCheckTimer.start(); } @@ -247,6 +248,7 @@ void LinkManager::disconnectLink(LinkInterface* link) } _deleteLink(link); if (_autoconnectConfigurations.contains(config)) { + qCDebug(LinkManagerLog) << "Removing disconnected autoconnect config" << config->name(); _autoconnectConfigurations.removeOne(config); delete config; } @@ -560,13 +562,17 @@ void LinkManager::_updateAutoConnectLinks(void) SerialConfiguration* linkConfig = _autoconnectConfigurations.value(i); if (linkConfig) { if (!currentPorts.contains(linkConfig->portName())) { - // We don't remove links which are still connected even though at this point the cable may - // have been pulled. This is due to the fact that whether a serial port goes away from the - // list when the cable is pulled is OS dependant. By not disconnecting in this case, we keep - // things working the same across all OS. - if (!linkConfig->link() || !linkConfig->link()->isConnected()) { - _confToDelete.append(linkConfig); + if (linkConfig->link()) { + if (linkConfig->link()->isConnected()) { + if (linkConfig->link()->active()) { + // We don't remove links which are still connected which have been active with a vehicle on them + // even though at this point the cable may have been pulled. Instead we wait for the user to + // Disconnect. Once the user disconnects, the link will be removed. + continue; + } + } } + _confToDelete.append(linkConfig); } } else { qWarning() << "Internal error"; @@ -577,6 +583,9 @@ void LinkManager::_updateAutoConnectLinks(void) foreach (LinkConfiguration* pDeleteConfig, _confToDelete) { qCDebug(LinkManagerLog) << "Removing unused autoconnect config" << pDeleteConfig->name(); _autoconnectConfigurations.removeOne(pDeleteConfig); + if (pDeleteConfig->link()) { + disconnectLink(pDeleteConfig->link()); + } delete pDeleteConfig; } #endif // __ios__ @@ -842,10 +851,11 @@ bool LinkManager::isBluetoothAvailable(void) void LinkManager::_activeLinkCheck(void) { + SerialLink* link = NULL; bool found = false; if (_activeLinkCheckList.count() != 0) { - LinkInterface* link = _activeLinkCheckList.takeFirst(); + link = _activeLinkCheckList.takeFirst(); if (_links.contains(link) && link->isConnected()) { // Make sure there is a vehicle on the link QmlObjectListModel* vehicles = _toolbox->multiVehicleManager()->vehicles(); @@ -856,6 +866,8 @@ void LinkManager::_activeLinkCheck(void) break; } } + } else { + link = NULL; } } @@ -863,7 +875,20 @@ void LinkManager::_activeLinkCheck(void) _activeLinkCheckTimer.stop(); } - if (!found) { - qgcApp()->showMessage("Your Vehicle is not responding. If this continues please check that you have an SD Card inserted and try again."); + if (!found && link) { + // See if we can get an NSH prompt on this link + bool foundNSHPrompt = false; + link->writeBytes("\r", 1); + QSignalSpy spy(link, SIGNAL(bytesReceived(LinkInterface*, QByteArray))); + if (spy.wait(100)) { + QList arguments = spy.takeFirst(); + if (arguments[1].value().contains("nsh>")) { + foundNSHPrompt = true; + } + } + + qgcApp()->showMessage(foundNSHPrompt ? + QStringLiteral("Please check to make sure you have an SD Card inserted in your Vehicle and try again.") : + QStringLiteral("Your Vehicle is not responding. If this continues shutdown QGroundControl, restart the Vehicle letting it boot completely, then start QGroundControl.")); } } diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index eda25d6223f5ddc298cbac5f1c24fb821f227942..b01a1a78ed5ecbddf7f1cf7a091046f01079a80c 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -153,14 +153,6 @@ public: /// Disconnect the specified link Q_INVOKABLE void disconnectLink(LinkInterface* link); - /// Called to notify that a heartbeat was received with the specified information. Will transition - /// a link to active as needed. - /// @param link Heartbeat came through on this link - /// @param vehicleId Mavlink system id for vehicle - /// @param heartbeat Mavlink heartbeat message - /// @return true: continue further processing of this message, false: disregard this message - bool notifyHeartbeatInfo(LinkInterface* link, int vehicleId, mavlink_heartbeat_t& heartbeat); - // The following APIs are public but should not be called in normal use. The are mainly exposed // here for unit test code. void _deleteLink(LinkInterface* link); @@ -244,9 +236,9 @@ private: bool _autoconnect3DRRadio; bool _autoconnectPX4Flow; - QTimer _activeLinkCheckTimer; ///< Timer which checks for a vehicle showing up on a usb direct link - QList _activeLinkCheckList; ///< List of links we are waiting for a vehicle to show up on - static const int _activeLinkCheckTimeoutMSecs = 10000; ///< Amount of time to wait for a heatbeat. Keep in mind ArduPilot stack heartbeat is slow to come. + QTimer _activeLinkCheckTimer; ///< Timer which checks for a vehicle showing up on a usb direct link + QList _activeLinkCheckList; ///< List of links we are waiting for a vehicle to show up on + static const int _activeLinkCheckTimeoutMSecs = 15000; ///< Amount of time to wait for a heatbeat. Keep in mind ArduPilot stack heartbeat is slow to come. static const char* _settingsGroup; static const char* _autoconnectUDPKey; diff --git a/src/comm/LogReplayLink.cc b/src/comm/LogReplayLink.cc index 27d4f262016194fc35ae1ac7c4d4a3374c9703a9..f3ba81eacbdd82dc6469b0fbe7b925b26da2fa61 100644 --- a/src/comm/LogReplayLink.cc +++ b/src/comm/LogReplayLink.cc @@ -150,11 +150,6 @@ void LogReplayLink::_replayError(const QString& errorMsg) emit communicationError(_errorTitle, errorMsg); } -void LogReplayLink::readBytes(void) -{ - // FIXME: This is a bad virtual from LinkInterface? -} - /// Since this is log replay, we just drops writes on the floor void LogReplayLink::writeBytes(const char* bytes, qint64 cBytes) { diff --git a/src/comm/LogReplayLink.h b/src/comm/LogReplayLink.h index deb62f3a8026e1f5a5f771dc83a8b0891542d453..034e5ae0fa370fae56b7d1696a95f13e491525b1 100644 --- a/src/comm/LogReplayLink.h +++ b/src/comm/LogReplayLink.h @@ -114,10 +114,6 @@ signals: void _pauseOnThread(void); void _setAccelerationFactorOnThread(int factor); -protected slots: - // FIXME: This should not be part of LinkInterface. It is an internal link implementation detail. - virtual void readBytes(void); - private slots: void _readNextLogEntry(void); void _play(void); diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 1fd725a13f1a63b48360f9475d3d5b9b3881dc01..07d4b290e2860b080ebf28fdfc751450e1da41a8 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -121,11 +121,6 @@ MockLink::~MockLink(void) _disconnect(); } -void MockLink::readBytes(void) -{ - // FIXME: This is a bad virtual from LinkInterface? -} - bool MockLink::_connect(void) { if (!_connected) { diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 8392056c31324ae02126371ea39761438d1eb80f..304b941241661e11543271f52162edc637c8e1d2 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -157,10 +157,6 @@ signals: public slots: virtual void writeBytes(const char *bytes, qint64 cBytes); -protected slots: - // FIXME: This should not be part of LinkInterface. It is an internal link implementation detail. - virtual void readBytes(void); - private slots: void _run1HzTasks(void); void _run10HzTasks(void); diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 524683ff83cb459d7dc687ea83e18e69e25c4d54..e20499ae2b46c0c8ae2642e5b69b1c53c3f9efab 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -93,34 +93,6 @@ void SerialLink::writeBytes(const char* data, qint64 size) } } -/** - * @brief Read a number of bytes from the interface. - * - * @param data Pointer to the data byte array to write the bytes to - * @param maxLength The maximum number of bytes to write - **/ -void SerialLink::readBytes() -{ - if(_port && _port->isOpen()) { - const qint64 maxLength = 2048; - char data[maxLength]; - _dataMutex.lock(); - qint64 numBytes = _port->bytesAvailable(); - - if (numBytes > 0) { - /* Read as much data in buffer as possible without overflow */ - if(maxLength < numBytes) numBytes = maxLength; - - _logInputDataRate(numBytes, QDateTime::currentMSecsSinceEpoch()); - - _port->read(data, numBytes); - QByteArray b(data, numBytes); - emit bytesReceived(this, b); - } - _dataMutex.unlock(); - } -} - /** * @brief Disconnect the connection. * @@ -267,8 +239,7 @@ bool SerialLink::_hardwareConnect(QSerialPort::SerialPortError& error, QString& void SerialLink::_readBytes(void) { qint64 byteCount = _port->bytesAvailable(); - if (byteCount) - { + if (byteCount) { QByteArray buffer; buffer.resize(byteCount); _port->read(buffer.data(), buffer.size()); diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index 225536fc5f713ddadc5d012d1776fca84758344e..011dfb883ef62b8f8dcd11b57e13e1640094e648 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -158,8 +158,6 @@ public: bool disconnect(void); public slots: - - void readBytes(); /** * @brief Write a number of bytes to the interface. *