Skip to content

Commit

Permalink
GPS: Cleanup
Browse files Browse the repository at this point in the history
  • Loading branch information
@HTRamsey
HTRamsey committed Jul 30, 2024
1 parent e7b222b commit 93cf25f
Show file tree
Hide file tree
Showing 27 changed files with 787 additions and 654 deletions.
3 changes: 2 additions & 1 deletion qgroundcontrol.pro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -678,8 +678,8 @@ HEADERS += \
src/GPS/Drivers/src/ubx.h \
src/GPS/Drivers/src/sbf.h \
src/GPS/GPSManager.h \
src/GPS/GPSPositionMessage.h \
src/GPS/GPSProvider.h \
src/GPS/GPSRtk.h \
src/GPS/RTCMMavlink.h \
src/GPS/definitions.h \
src/GPS/satellite_info.h \
Expand Down Expand Up @@ -941,6 +941,7 @@ SOURCES += \
src/GPS/Drivers/src/sbf.cpp \
src/GPS/GPSManager.cc \
src/GPS/GPSProvider.cc \
src/GPS/GPSRtk.cc \
src/GPS/RTCMMavlink.cc
}

Expand Down
7 changes: 4 additions & 3 deletions src/Comms/LinkManager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include "PositionManager.h"
#ifndef NO_SERIAL_LINK
#include "GPSManager.h"
#include "GPSRtk.h"
#endif

#ifdef QT_DEBUG
Expand Down Expand Up @@ -615,7 +616,7 @@ void LinkManager::_updateAutoConnectLinks(void)
case QGCSerialPortInfo::BoardTypeRTKGPS:
qCDebug(LinkManagerLog) << "RTK GPS auto-connected" << portInfo.portName().trimmed();
_autoConnectRTKPort = portInfo.systemLocation();
_toolbox->gpsManager()->connectGPS(portInfo.systemLocation(), boardName);
GPSManager::instance()->gpsRtk()->connectGPS(portInfo.systemLocation(), boardName);
break;
default:
qCWarning(LinkManagerLog) << "Internal error: Unknown board type" << boardType;
Expand All @@ -638,7 +639,7 @@ 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();
GPSManager::instance()->gpsRtk()->disconnectGPS();
_autoConnectRTKPort.clear();
}

Expand Down Expand Up @@ -881,7 +882,7 @@ bool LinkManager::_allowAutoConnectToBoard(QGCSerialPortInfo::BoardType_t boardT
}
break;
case QGCSerialPortInfo::BoardTypeRTKGPS:
if (_autoConnectSettings->autoConnectRTKGPS()->rawValue().toBool() && !_toolbox->gpsManager()->connected()) {
if (_autoConnectSettings->autoConnectRTKGPS()->rawValue().toBool() && !GPSManager::instance()->gpsRtk()->connected()) {
return true;
}
break;
Expand Down
2 changes: 1 addition & 1 deletion src/Comms/MAVLinkProtocol.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ class MAVLinkProtocol : public QGCTool
/** @brief Get the system id of this application */
int getSystemId() const;
/** @brief Get the component id of this application */
int getComponentId();
static int getComponentId();

/** @brief Get protocol version check state */
bool versionCheckEnabled() const {
Expand Down
147 changes: 90 additions & 57 deletions src/GPS/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,61 +2,94 @@ find_package(Qt6 REQUIRED COMPONENTS Core)

qt_add_library(GPS STATIC)

if(NOT QGC_NO_SERIAL_LINK)
# include(FetchContent)
# FetchContent_Declare(gps_drivers
# GIT_REPOSITORY https://github.com/PX4/PX4-GPSDrivers.git
# GIT_TAG main
# GIT_SHALLOW TRUE
# )
# FetchContent_GetProperties(gps_drivers)
# if(NOT gps_drivers_POPULATED)
# FetchContent_Populate(gps_drivers)
# add_subdirectory(${gps_drivers_SOURCE_DIR} ${gps_drivers_BINARY_DIR} EXCLUDE_FROM_ALL)
# endif()

target_sources(GPS
PRIVATE
definitions.h
Drivers/src/ashtech.cpp
Drivers/src/ashtech.h
Drivers/src/gps_helper.cpp
Drivers/src/gps_helper.h
Drivers/src/mtk.cpp
Drivers/src/mtk.h
Drivers/src/rtcm.cpp
Drivers/src/rtcm.h
Drivers/src/sbf.cpp
Drivers/src/sbf.h
Drivers/src/ubx.cpp
Drivers/src/ubx.h
GPSManager.cc
GPSManager.h
GPSPositionMessage.h
GPSProvider.cc
GPSProvider.h
RTCMMavlink.cc
RTCMMavlink.h
satellite_info.h
sensor_gnss_relative.h
sensor_gps.h
)

target_link_libraries(GPS
PRIVATE
Settings
Utilities
Vehicle
PUBLIC
Qt6::Core
MAVLink
QGC
)

target_include_directories(GPS
PRIVATE
Drivers/src
INTERFACE
${CMAKE_CURRENT_SOURCE_DIR}
)
if(QGC_NO_SERIAL_LINK)
return()
endif()

find_package(Qt6 REQUIRED COMPONENTS QmlIntegration)

# include(FetchContent)
# FetchContent_Declare(gps_drivers
# GIT_REPOSITORY https://github.com/PX4/PX4-GPSDrivers.git
# GIT_TAG main
# GIT_SHALLOW TRUE
# SOURCE_SUBDIR src
# # SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/PX4-GPSDrivers
# )
# FetchContent_MakeAvailable(gps_drivers)

set(gps_drivers_SOURCE_DIR Drivers/src)

qt_add_library(GPSDrivers STATIC
definitions.h
${gps_drivers_SOURCE_DIR}/ashtech.cpp
${gps_drivers_SOURCE_DIR}/ashtech.h
${gps_drivers_SOURCE_DIR}/base_station.h
${gps_drivers_SOURCE_DIR}/crc.cpp
${gps_drivers_SOURCE_DIR}/crc.h
${gps_drivers_SOURCE_DIR}/emlid_reach.cpp
${gps_drivers_SOURCE_DIR}/emlid_reach.h
${gps_drivers_SOURCE_DIR}/femtomes.cpp
${gps_drivers_SOURCE_DIR}/femtomes.h
${gps_drivers_SOURCE_DIR}/gps_helper.cpp
${gps_drivers_SOURCE_DIR}/gps_helper.h
${gps_drivers_SOURCE_DIR}/mtk.cpp
${gps_drivers_SOURCE_DIR}/mtk.h
${gps_drivers_SOURCE_DIR}/nmea.cpp
${gps_drivers_SOURCE_DIR}/nmea.h
${gps_drivers_SOURCE_DIR}/rtcm.cpp
${gps_drivers_SOURCE_DIR}/rtcm.h
${gps_drivers_SOURCE_DIR}/sbf.cpp
${gps_drivers_SOURCE_DIR}/sbf.h
${gps_drivers_SOURCE_DIR}/ubx.cpp
${gps_drivers_SOURCE_DIR}/ubx.h
${gps_drivers_SOURCE_DIR}/unicore.cpp
${gps_drivers_SOURCE_DIR}/unicore.h
)

target_link_libraries(GPSDrivers
PUBLIC
Qt6::Core
Utilities
)

target_include_directories(GPSDrivers
PRIVATE
# ${CMAKE_CURRENT_SOURCE_DIR}/PX4-GPSDrivers/include
PUBLIC
${gps_drivers_SOURCE_DIR}
)

target_sources(GPS
PRIVATE
GPSManager.cc
GPSManager.h
GPSProvider.cc
GPSProvider.h
GPSRtk.cc
GPSRtk.h
RTCMMavlink.cc
RTCMMavlink.h
satellite_info.h
sensor_gnss_relative.h
sensor_gps.h
)

target_link_libraries(GPS
PRIVATE
Comms
FactSystem
GPSDrivers
MAVLink
Settings
Utilities
Vehicle
PUBLIC
Qt6::Core
Qt6::QmlIntegration
)

target_include_directories(GPS
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)
137 changes: 12 additions & 125 deletions src/GPS/GPSManager.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,142 +7,29 @@
*
****************************************************************************/


#include "GPSManager.h"
#include "GPSProvider.h"
#include "QGCApplication.h"
#include "SettingsManager.h"
#include "RTKSettings.h"
#include "GPSRTKFactGroup.h"
#include "RTCMMavlink.h"
#include "GPSRtk.h"
#include "QGCLoggingCategory.h"

#include <QtCore/qapplicationstatic.h>

GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox)
: QGCTool(app, toolbox)
{
qRegisterMetaType<GPSPositionMessage>();
qRegisterMetaType<GPSSatelliteMessage>();
}

GPSManager::~GPSManager()
{
disconnectGPS();

delete _gpsRtkFactGroup;
_gpsRtkFactGroup = nullptr;
}

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

_gpsRtkFactGroup = new GPSRTKFactGroup(this);
QGC_LOGGING_CATEGORY(GPSManagerLog, "qgc.gps.gpsmanager")

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);
}
Q_APPLICATION_STATIC(GPSManager, s_gpsManager);

void GPSManager::_gpsSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active)
GPSManager *GPSManager::instance()
{
_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);
return s_gpsManager();
}

void GPSManager::_gpsNumSatellites(int numSatellites)
GPSManager::GPSManager(QObject *parent)
: QObject(parent)
, m_gpsRtk(new GPSRtk(this))
{
_gpsRtkFactGroup->numSatellites()->setRawValue(numSatellites);
// qCDebug(GPSManagerLog) << Q_FUNC_INFO << this;
}

void GPSManager::connectGPS(const QString& device, const QString& gps_type)
{
RTKSettings* rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings();

GPSProvider::GPSType type;
if (gps_type.contains("trimble", Qt::CaseInsensitive)) {
type = GPSProvider::GPSType::trimble;
qCDebug(RTKGPSLog) << "Connecting Trimble device";
} else if (gps_type.contains("septentrio", Qt::CaseInsensitive)) {
type = GPSProvider::GPSType::septentrio;
qCDebug(RTKGPSLog) << "Connecting Septentrio device";
} else {
type = GPSProvider::GPSType::u_blox;
qCDebug(RTKGPSLog) << "Connecting U-blox device";
}

disconnectGPS();
_requestGpsStop = false;
_gpsProvider = new GPSProvider(device,
type,
true, /* enableSatInfo */
rtkSettings->surveyInAccuracyLimit()->rawValue().toDouble(),
rtkSettings->surveyInMinObservationDuration()->rawValue().toInt(),
rtkSettings->useFixedBasePosition()->rawValue().toBool(),
rtkSettings->fixedBasePositionLatitude()->rawValue().toDouble(),
rtkSettings->fixedBasePositionLongitude()->rawValue().toDouble(),
rtkSettings->fixedBasePositionAltitude()->rawValue().toFloat(),
rtkSettings->fixedBasePositionAccuracy()->rawValue().toFloat(),
_requestGpsStop);
_gpsProvider->start();

//create RTCM device
_rtcmMavlink = new RTCMMavlink(*_toolbox);

connect(_gpsProvider, &GPSProvider::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate);

//test: connect to position update
connect(_gpsProvider, &GPSProvider::positionUpdate, this, &GPSManager::GPSPositionUpdate);
connect(_gpsProvider, &GPSProvider::satelliteInfoUpdate, this, &GPSManager::GPSSatelliteUpdate);
connect(_gpsProvider, &GPSProvider::finished, this, &GPSManager::onDisconnect);
connect(_gpsProvider, &GPSProvider::surveyInStatus, this, &GPSManager::surveyInStatus);

emit onConnect();
}

void GPSManager::disconnectGPS(void)
{
if (_gpsProvider) {
_requestGpsStop = true;
//Note that we need a relatively high timeout to be sure the GPS thread finished.
if (!_gpsProvider->wait(2000)) {
qWarning() << "Failed to wait for GPS thread exit. Consider increasing the timeout";
}
delete(_gpsProvider);
}
if (_rtcmMavlink) {
delete(_rtcmMavlink);
}
_gpsProvider = nullptr;
_rtcmMavlink = nullptr;
}

bool GPSManager::connected() const { return _gpsProvider && _gpsProvider->isRunning(); }

FactGroup* GPSManager::gpsRtkFactGroup(void) { return _gpsRtkFactGroup; }

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);
}
void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg)
GPSManager::~GPSManager()
{
qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count);
emit satelliteUpdate(msg.satellite_data.count);
// qCDebug(GPSManagerLog) << Q_FUNC_INFO << this;
}
Loading

0 comments on commit 93cf25f

Please sign in to comment.