From 6966133cfc4c82ca7261c2930885728130892b2e Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Thu, 17 Aug 2017 13:36:45 -0700 Subject: [PATCH] Allow disconnect/reconnect of RTK GPS --- src/GPS/GPSManager.cc | 29 ++++++++++++++++------------- src/GPS/GPSManager.h | 8 ++++---- src/comm/LinkManager.cc | 13 +++++++++++-- src/comm/LinkManager.h | 1 + 4 files changed, 32 insertions(+), 19 deletions(-) diff --git a/src/GPS/GPSManager.cc b/src/GPS/GPSManager.cc index bf155c9bc..bd55d1034 100644 --- a/src/GPS/GPSManager.cc +++ b/src/GPS/GPSManager.cc @@ -23,14 +23,14 @@ GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox) GPSManager::~GPSManager() { - cleanup(); + disconnectGPS(); } void GPSManager::connectGPS(const QString& device) { RTKSettings* rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings(); - cleanup(); + disconnectGPS(); _requestGpsStop = false; _gpsProvider = new GPSProvider(device, true, rtkSettings->surveyInAccuracyLimit()->rawValue().toDouble(), rtkSettings->surveyInMinObservationDuration()->rawValue().toInt(), _requestGpsStop); _gpsProvider->start(); @@ -49,17 +49,7 @@ void GPSManager::connectGPS(const QString& device) emit onConnect(); } -void GPSManager::GPSPositionUpdate(GPSPositionMessage msg) -{ - qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.alt).arg(msg.position_data.lon).arg(msg.position_data.lat); -} -void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg) -{ - qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count); - emit satelliteUpdate(msg.satellite_data.count); -} - -void GPSManager::cleanup() +void GPSManager::disconnectGPS(void) { if (_gpsProvider) { _requestGpsStop = true; @@ -72,4 +62,17 @@ void GPSManager::cleanup() if (_rtcmMavlink) { delete(_rtcmMavlink); } + _gpsProvider = NULL; + _rtcmMavlink = NULL; +} + + +void GPSManager::GPSPositionUpdate(GPSPositionMessage msg) +{ + qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.alt).arg(msg.position_data.lon).arg(msg.position_data.lat); +} +void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg) +{ + qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count); + emit satelliteUpdate(msg.satellite_data.count); } diff --git a/src/GPS/GPSManager.h b/src/GPS/GPSManager.h index 87936730c..e116f383f 100644 --- a/src/GPS/GPSManager.h +++ b/src/GPS/GPSManager.h @@ -28,8 +28,9 @@ public: GPSManager(QGCApplication* app, QGCToolbox* toolbox); ~GPSManager(); - void connectGPS(const QString& device); - bool connected(void) const { return _gpsProvider && _gpsProvider->isRunning(); } + void connectGPS (const QString& device); + void disconnectGPS (void); + bool connected (void) const { return _gpsProvider && _gpsProvider->isRunning(); } signals: void onConnect(); @@ -40,9 +41,8 @@ signals: private slots: void GPSPositionUpdate(GPSPositionMessage msg); void GPSSatelliteUpdate(GPSSatelliteMessage msg); -private: - void cleanup(); +private: GPSProvider* _gpsProvider = nullptr; RTCMMavlink* _rtcmMavlink = nullptr; diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 37f308371..a2f2adb5a 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -507,7 +507,7 @@ void LinkManager::_updateAutoConnectLinks(void) continue; } - if (_autoconnectConfigurationsContainsPort(portInfo.systemLocation())) { + if (_autoconnectConfigurationsContainsPort(portInfo.systemLocation()) || _autoConnectRTKPort == portInfo.systemLocation()) { qCDebug(LinkManagerVerboseLog) << "Skipping existing autoconnect" << portInfo.systemLocation(); } else if (!_autoconnectWaitList.contains(portInfo.systemLocation())) { // We don't connect to the port the first time we see it. The ability to correctly detect whether we @@ -545,7 +545,8 @@ void LinkManager::_updateAutoConnectLinks(void) #ifndef __mobile__ case QGCSerialPortInfo::BoardTypeRTKGPS: if (_autoConnectSettings->autoConnectRTKGPS()->rawValue().toBool() && !_toolbox->gpsManager()->connected()) { - qCDebug(LinkManagerLog) << "RTK GPS auto-connected"; + qCDebug(LinkManagerLog) << "RTK GPS auto-connected" << portInfo.portName().trimmed(); + _autoConnectRTKPort = portInfo.systemLocation(); _toolbox->gpsManager()->connectGPS(portInfo.systemLocation()); } break; @@ -610,6 +611,14 @@ void LinkManager::_updateAutoConnectLinks(void) } } } + + // Check for RTK GPS connection gone + if (!_autoConnectRTKPort.isEmpty() && !currentPorts.contains(_autoConnectRTKPort)) { + qCDebug(LinkManagerLog) << "RTK GPS disconnected" << _autoConnectRTKPort; + _toolbox->gpsManager()->disconnectGPS(); + _autoConnectRTKPort.clear(); + } + #endif #endif // NO_SERIAL_LINK } diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index fc2a9c74e..b694d6dc3 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -217,6 +217,7 @@ private: QList _sharedLinks; QList _sharedConfigurations; QList _sharedAutoconnectConfigurations; + QString _autoConnectRTKPort; QmlObjectListModel _qmlConfigurations; QMap _autoconnectWaitList; ///< key: QGCSerialPortInfo.systemLocation, value: wait count -- 2.22.0