Skip to content

Commit

Permalink
Move RTK handling to GPSManager
Browse files Browse the repository at this point in the history
  • Loading branch information
HTRamsey authored and bkueng committed Apr 8, 2024
1 parent 91fd7da commit 7ead1de
Show file tree
Hide file tree
Showing 9 changed files with 61 additions and 60 deletions.
4 changes: 2 additions & 2 deletions qgroundcontrol.pro
Original file line number Diff line number Diff line change
Expand Up @@ -660,7 +660,7 @@ HEADERS += \
src/GPS/GPSManager.h \
src/GPS/GPSPositionMessage.h \
src/GPS/GPSProvider.h \
src/GPS/RTCM/RTCMMavlink.h \
src/GPS/RTCMMavlink.h \
src/GPS/definitions.h \
src/GPS/satellite_info.h \
src/GPS/sensor_gps.h \
Expand Down Expand Up @@ -905,7 +905,7 @@ SOURCES += \
src/GPS/Drivers/src/sbf.cpp \
src/GPS/GPSManager.cc \
src/GPS/GPSProvider.cc \
src/GPS/RTCM/RTCMMavlink.cc \
src/GPS/RTCMMavlink.cc \
src/Joystick/JoystickSDL.cc \
src/RunGuard.cc \
}
Expand Down
5 changes: 2 additions & 3 deletions src/GPS/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,8 @@ qt_add_library(gps STATIC
GPSPositionMessage.h
GPSProvider.cc
GPSProvider.h
RTCM/RTCMMavlink.cc
RTCM/RTCMMavlink.h
RTCMMavlink.cc
RTCMMavlink.h
satellite_info.h
sensor_gnss_relative.h
sensor_gps.h
Expand All @@ -37,5 +37,4 @@ target_include_directories(gps
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
Drivers/src
RTCM
)
42 changes: 41 additions & 1 deletion src/GPS/GPSManager.cc
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,47 @@ GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox)
GPSManager::~GPSManager()
{
disconnectGPS();

delete _gpsRtkFactGroup;
_gpsRtkFactGroup = nullptr;
}

void GPSManager::setToolbox(QGCToolbox *toolbox)
{
QGCTool::setToolbox(toolbox);

_gpsRtkFactGroup = new GPSRTKFactGroup(this);

connect(this, &GPSManager::onConnect, this, &GPSManager::_onGPSConnect);
connect(this, &GPSManager::onDisconnect, this, &GPSManager::_onGPSDisconnect);
connect(this, &GPSManager::surveyInStatus, this, &GPSManager::_gpsSurveyInStatus);
connect(this, &GPSManager::satelliteUpdate, this, &GPSManager::_gpsNumSatellites);
}

void GPSManager::_onGPSConnect()
{
_gpsRtkFactGroup->connected()->setRawValue(true);
}

void GPSManager::_onGPSDisconnect()
{
_gpsRtkFactGroup->connected()->setRawValue(false);
}

void GPSManager::_gpsSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active)
{
_gpsRtkFactGroup->currentDuration()->setRawValue(duration);
_gpsRtkFactGroup->currentAccuracy()->setRawValue(static_cast<double>(accuracyMM) / 1000.0);
_gpsRtkFactGroup->currentLatitude()->setRawValue(latitude);
_gpsRtkFactGroup->currentLongitude()->setRawValue(longitude);
_gpsRtkFactGroup->currentAltitude()->setRawValue(altitude);
_gpsRtkFactGroup->valid()->setRawValue(valid);
_gpsRtkFactGroup->active()->setRawValue(active);
}

void GPSManager::_gpsNumSatellites(int numSatellites)
{
_gpsRtkFactGroup->numSatellites()->setRawValue(numSatellites);
}

void GPSManager::connectGPS(const QString& device, const QString& gps_type)
Expand Down Expand Up @@ -88,7 +129,6 @@ void GPSManager::disconnectGPS(void)
_rtcmMavlink = nullptr;
}


void GPSManager::GPSPositionUpdate(GPSPositionMessage msg)
{
qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.altitude_msl_m).arg(msg.position_data.longitude_deg).arg(msg.position_data.latitude_deg);
Expand Down
11 changes: 10 additions & 1 deletion src/GPS/GPSManager.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,8 @@
#pragma once

#include "GPSProvider.h"
#include "RTCM/RTCMMavlink.h"
#include "RTCMMavlink.h"
#include "GPSRTKFactGroup.h"
#include <QGCToolbox.h>

#include <QString>
Expand All @@ -28,9 +29,12 @@ class GPSManager : public QGCTool
GPSManager(QGCApplication* app, QGCToolbox* toolbox);
~GPSManager();

void setToolbox(QGCToolbox* toolbox) override;

void connectGPS (const QString& device, const QString& gps_type);
void disconnectGPS (void);
bool connected (void) const { return _gpsProvider && _gpsProvider->isRunning(); }
FactGroup* gpsRtkFactGroup(void) { return _gpsRtkFactGroup; }

signals:
void onConnect();
Expand All @@ -41,10 +45,15 @@ class GPSManager : public QGCTool
private slots:
void GPSPositionUpdate(GPSPositionMessage msg);
void GPSSatelliteUpdate(GPSSatelliteMessage msg);
void _onGPSConnect(void);
void _onGPSDisconnect(void);
void _gpsSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active);
void _gpsNumSatellites(int numSatellites);

private:
GPSProvider* _gpsProvider = nullptr;
RTCMMavlink* _rtcmMavlink = nullptr;
GPSRTKFactGroup* _gpsRtkFactGroup = nullptr;

std::atomic_bool _requestGpsStop; ///< signals the thread to quit
};
File renamed without changes.
File renamed without changes.
43 changes: 0 additions & 43 deletions src/QGCApplication.cc
Original file line number Diff line number Diff line change
Expand Up @@ -120,10 +120,6 @@
#include "SerialLink.h"
#endif

#ifndef __mobile__
#include "GPS/GPSManager.h"
#endif

#ifdef QGC_RTLAB_ENABLED
#include "OpalLink.h"
#endif
Expand Down Expand Up @@ -337,17 +333,6 @@ QGCApplication::QGCApplication(int &argc, char* argv[], bool unitTesting)
_toolbox = new QGCToolbox(this);
_toolbox->setChildToolboxes();

#ifndef __mobile__
_gpsRtkFactGroup = new GPSRTKFactGroup(this);
GPSManager *gpsManager = _toolbox->gpsManager();
if (gpsManager) {
connect(gpsManager, &GPSManager::onConnect, this, &QGCApplication::_onGPSConnect);
connect(gpsManager, &GPSManager::onDisconnect, this, &QGCApplication::_onGPSDisconnect);
connect(gpsManager, &GPSManager::surveyInStatus, this, &QGCApplication::_gpsSurveyInStatus);
connect(gpsManager, &GPSManager::satelliteUpdate, this, &QGCApplication::_gpsNumSatellites);
}
#endif /* __mobile__ */

_checkForNewVersion();
}

Expand Down Expand Up @@ -413,7 +398,6 @@ void QGCApplication::_shutdown()
// Close out all Qml before we delete toolbox. This way we don't get all sorts of null reference complaints from Qml.
delete _qmlAppEngine;
delete _toolbox;
delete _gpsRtkFactGroup;
}

QGCApplication::~QGCApplication()
Expand Down Expand Up @@ -891,33 +875,6 @@ bool QGCApplication::_parseVersionText(const QString& versionString, int& majorV
return false;
}


void QGCApplication::_onGPSConnect()
{
_gpsRtkFactGroup->connected()->setRawValue(true);
}

void QGCApplication::_onGPSDisconnect()
{
_gpsRtkFactGroup->connected()->setRawValue(false);
}

void QGCApplication::_gpsSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active)
{
_gpsRtkFactGroup->currentDuration()->setRawValue(duration);
_gpsRtkFactGroup->currentAccuracy()->setRawValue(static_cast<double>(accuracyMM) / 1000.0);
_gpsRtkFactGroup->currentLatitude()->setRawValue(latitude);
_gpsRtkFactGroup->currentLongitude()->setRawValue(longitude);
_gpsRtkFactGroup->currentAltitude()->setRawValue(altitude);
_gpsRtkFactGroup->valid()->setRawValue(valid);
_gpsRtkFactGroup->active()->setRawValue(active);
}

void QGCApplication::_gpsNumSatellites(int numSatellites)
{
_gpsRtkFactGroup->numSatellites()->setRawValue(numSatellites);
}

QString QGCApplication::cachedParameterMetaDataFile(void)
{
QSettings settings;
Expand Down
8 changes: 0 additions & 8 deletions src/QGCApplication.h
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@
#include "AudioOutput.h"
#include "UASMessageHandler.h"
#include "FactSystem.h"
#include "GPSRTKFactGroup.h"

#ifdef QGC_RTLAB_ENABLED
#include "OpalLink.h"
Expand Down Expand Up @@ -89,8 +88,6 @@ class QGCApplication : public QApplication
/// Is Internet available?
bool isInternetAvailable();

FactGroup* gpsRtkFactGroup(void) { return _gpsRtkFactGroup; }

QTranslator& qgcJSONTranslator(void) { return _qgcTranslatorJSON; }

void setLanguage();
Expand Down Expand Up @@ -180,10 +177,6 @@ private slots:
void _missingParamsDisplay (void);
void _qgcCurrentStableVersionDownloadComplete (QString remoteFile, QString localFile, QString errorMsg);
bool _parseVersionText (const QString& versionString, int& majorVersion, int& minorVersion, int& buildVersion);
void _onGPSConnect (void);
void _onGPSDisconnect (void);
void _gpsSurveyInStatus (float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active);
void _gpsNumSatellites (int numSatellites);
void _showDelayedAppMessages (void);

private:
Expand All @@ -206,7 +199,6 @@ private slots:
int _majorVersion = 0;
int _minorVersion = 0;
int _buildVersion = 0;
GPSRTKFactGroup* _gpsRtkFactGroup = nullptr;
QGCToolbox* _toolbox = nullptr;
QQuickWindow* _mainRootWindow = nullptr;
bool _bluetoothAvailable = false;
Expand Down
8 changes: 6 additions & 2 deletions src/QmlControls/QGroundControlQmlGlobal.cc
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,9 @@

#include "QGroundControlQmlGlobal.h"
#include "LinkManager.h"

#ifndef __mobile__
#include "GPSManager.h"
#endif
#include <QSettings>
#include <QLineF>
#include <QPointF>
Expand Down Expand Up @@ -79,7 +81,9 @@ void QGroundControlQmlGlobal::setToolbox(QGCToolbox* toolbox)
_corePlugin = toolbox->corePlugin();
_firmwarePluginManager = toolbox->firmwarePluginManager();
_settingsManager = toolbox->settingsManager();
_gpsRtkFactGroup = qgcApp()->gpsRtkFactGroup();
#ifndef __mobile__
_gpsRtkFactGroup = toolbox->gpsManager()->gpsRtkFactGroup();
#endif
_adsbVehicleManager = toolbox->adsbVehicleManager();
_globalPalette = new QGCPalette(this);
#ifndef QGC_AIRLINK_DISABLED
Expand Down

0 comments on commit 7ead1de

Please sign in to comment.