Merge pull request #5588 from DonLakeFlyer/RTKGPSDisconnect

Allow disconnect/reconnect of RTK GPS

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);
 }

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;
 

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
 }

src/comm/LinkManager.h

@@ -217,6 +217,7 @@ private:
     QList<SharedLinkInterfacePointer>       _sharedLinks;
     QList<SharedLinkConfigurationPointer>   _sharedConfigurations;
     QList<SharedLinkConfigurationPointer>   _sharedAutoconnectConfigurations;
+    QString                                 _autoConnectRTKPort;
     QmlObjectListModel                      _qmlConfigurations;
 
     QMap<QString, int>  _autoconnectWaitList;   ///< key: QGCSerialPortInfo.systemLocation, value: wait count