diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro index 89e6fae8cc96..8811525e3a5c 100644 --- a/qgroundcontrol.pro +++ b/qgroundcontrol.pro @@ -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 \ @@ -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 } diff --git a/src/Comms/LinkManager.cc b/src/Comms/LinkManager.cc index 063661eb8001..1673114082ca 100644 --- a/src/Comms/LinkManager.cc +++ b/src/Comms/LinkManager.cc @@ -25,6 +25,7 @@ #include "PositionManager.h" #ifndef NO_SERIAL_LINK #include "GPSManager.h" +#include "GPSRtk.h" #endif #ifdef QT_DEBUG @@ -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; @@ -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(); } @@ -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; diff --git a/src/Comms/MAVLinkProtocol.h b/src/Comms/MAVLinkProtocol.h index 3980aadae3bd..af3e1490027b 100644 --- a/src/Comms/MAVLinkProtocol.h +++ b/src/Comms/MAVLinkProtocol.h @@ -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 { diff --git a/src/GPS/CMakeLists.txt b/src/GPS/CMakeLists.txt index 8d1ed0ae961b..4f53dd9c8252 100644 --- a/src/GPS/CMakeLists.txt +++ b/src/GPS/CMakeLists.txt @@ -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} +) diff --git a/src/GPS/GPSManager.cc b/src/GPS/GPSManager.cc index fc6b6d158910..35ed352eb88e 100644 --- a/src/GPS/GPSManager.cc +++ b/src/GPS/GPSManager.cc @@ -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 -GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox) - : QGCTool(app, toolbox) -{ - qRegisterMetaType(); - qRegisterMetaType(); -} - -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(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; } diff --git a/src/GPS/GPSManager.h b/src/GPS/GPSManager.h index d7ba37be408a..0e5c5bc1a0bf 100644 --- a/src/GPS/GPSManager.h +++ b/src/GPS/GPSManager.h @@ -7,56 +7,27 @@ * ****************************************************************************/ - #pragma once -#include "QGCToolbox.h" -#include "GPSPositionMessage.h" - -#include #include +#include -class GPSRTKFactGroup; -class FactGroup; -class RTCMMavlink; -class GPSProvider; +Q_DECLARE_LOGGING_CATEGORY(GPSManagerLog) -/** - ** class GPSManager - * handles a GPS provider and RTK - */ -class GPSManager : public QGCTool +class GPSRtk; + +class GPSManager : public QObject { Q_OBJECT + public: - GPSManager(QGCApplication* app, QGCToolbox* toolbox); + GPSManager(QObject *parent = nullptr); ~GPSManager(); - void setToolbox(QGCToolbox* toolbox) override; + static GPSManager *instance(); - void connectGPS (const QString& device, const QString& gps_type); - void disconnectGPS (void); - bool connected (void) const; - FactGroup* gpsRtkFactGroup(void); - -signals: - void onConnect(); - void onDisconnect(); - void surveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active); - void satelliteUpdate(int numSats); - -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); + GPSRtk *gpsRtk() { return m_gpsRtk; } private: - GPSProvider* _gpsProvider = nullptr; - RTCMMavlink* _rtcmMavlink = nullptr; - GPSRTKFactGroup* _gpsRtkFactGroup = nullptr; - - std::atomic_bool _requestGpsStop; ///< signals the thread to quit + GPSRtk *m_gpsRtk = nullptr; }; diff --git a/src/GPS/GPSPositionMessage.h b/src/GPS/GPSPositionMessage.h deleted file mode 100644 index 272fcdbe47d1..000000000000 --- a/src/GPS/GPSPositionMessage.h +++ /dev/null @@ -1,32 +0,0 @@ -/**************************************************************************** - * - * (c) 2009-2024 QGROUNDCONTROL PROJECT - * - * QGroundControl is licensed according to the terms in the file - * COPYING.md in the root of the source code directory. - * - ****************************************************************************/ - - -#pragma once - -#include "sensor_gps.h" -#include "satellite_info.h" -#include - -/** - ** struct GPSPositionMessage - * wrapper that can be used for Qt signal/slots - */ -struct GPSPositionMessage -{ - sensor_gps_s position_data; -}; -Q_DECLARE_METATYPE(GPSPositionMessage); - - -struct GPSSatelliteMessage -{ - satellite_info_s satellite_data; -}; -Q_DECLARE_METATYPE(GPSSatelliteMessage); diff --git a/src/GPS/GPSProvider.cc b/src/GPS/GPSProvider.cc index dbec2998375b..978a68cdd86a 100644 --- a/src/GPS/GPSProvider.cc +++ b/src/GPS/GPSProvider.cc @@ -7,233 +7,227 @@ * ****************************************************************************/ - #include "GPSProvider.h" -#include "QGCLoggingCategory.h" #include "Drivers/src/ubx.h" #include "Drivers/src/sbf.h" #include "Drivers/src/ashtech.h" #include "Drivers/src/base_station.h" #include "definitions.h" +#include #ifdef Q_OS_ANDROID -#include "qserialport.h" +#include #else -#include +#include #endif +#include #define GPS_RECEIVE_TIMEOUT 1200 - -//#define SIMULATE_RTCM_OUTPUT //if defined, generate simulated RTCM messages - //additionally make sure to call connectGPS(""), eg. from QGCToolbox.cc - - -void GPSProvider::run() +#define GPS_RECEIVE_POSITION 0b01 +#define GPS_RECEIVE_SATELLITE 0b10 +#define GPS_HEADING_OFFSET 5 + +QGC_LOGGING_CATEGORY(GPSProviderLog, "qgc.gps.gpsprovider") +QGC_LOGGING_CATEGORY(GPSDriversLog, "qgc.gps.drivers") + +GPSProvider::GPSProvider( + const QString &device, + GPSType type, + double surveyInAccMeters, + int surveryInDurationSecs, + bool useFixedBaseLocation, + double fixedBaseLatitude, + double fixedBaseLongitude, + float fixedBaseAltitudeMeters, + float fixedBaseAccuracyMeters, + const std::atomic_bool &requestStop, + QObject *parent +) + : QObject(parent) + , m_device(device) + , m_type(type) + , m_requestStop(requestStop) + , m_surveyInAccMeters(surveyInAccMeters) + , m_surveryInDurationSecs(surveryInDurationSecs) + , m_useFixedBaseLoction(useFixedBaseLocation) + , m_fixedBaseLatitude(fixedBaseLatitude) + , m_fixedBaseLongitude(fixedBaseLongitude) + , m_fixedBaseAltitudeMeters(fixedBaseAltitudeMeters) + , m_fixedBaseAccuracyMeters(fixedBaseAccuracyMeters) + , m_serial(new QSerialPort(this)) + , m_thread(new QThread(this)) { -#ifdef SIMULATE_RTCM_OUTPUT - const int fakeMsgLengths[3] = { 30, 170, 240 }; - uint8_t* fakeData = new uint8_t[fakeMsgLengths[2]]; - while (!_requestStop) { - for (int i = 0; i < 3; ++i) { - gotRTCMData((uint8_t*) fakeData, fakeMsgLengths[i]); - msleep(4); - } - msleep(100); - } - delete[] fakeData; -#endif /* SIMULATE_RTCM_OUTPUT */ - - if (_serial) delete _serial; - - _serial = new QSerialPort(); - _serial->setPortName(_device); - if (!_serial->open(QIODevice::ReadWrite)) { - int retries = 60; - // Give the device some time to come up. In some cases the device is not - // immediately accessible right after startup for some reason. This can take 10-20s. - while (retries-- > 0 && _serial->error() == QSerialPort::PermissionError) { - qCDebug(RTKGPSLog) << "Cannot open device... retrying"; - msleep(500); - if (_serial->open(QIODevice::ReadWrite)) { - _serial->clearError(); - break; - } - } - if (_serial->error() != QSerialPort::NoError) { - qWarning() << "GPS: Failed to open Serial Device" << _device << _serial->errorString(); - return; - } - } - _serial->setBaudRate(QSerialPort::Baud9600); - _serial->setDataBits(QSerialPort::Data8); - _serial->setParity(QSerialPort::NoParity); - _serial->setStopBits(QSerialPort::OneStop); - _serial->setFlowControl(QSerialPort::NoFlowControl); - - unsigned int baudrate; - GPSBaseStationSupport* gpsDriver = nullptr; - - while (!_requestStop) { + // qCDebug(GPSProviderLog) << Q_FUNC_INFO << this; - if (gpsDriver) { - delete gpsDriver; - gpsDriver = nullptr; - } - - if (_type == GPSType::trimble) { - gpsDriver = new GPSDriverAshtech(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo); - baudrate = 115200; - } else if (_type == GPSType::septentrio) { - gpsDriver = new GPSDriverSBF(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo, 5); - baudrate = 0; // auto-configure - } else { - gpsDriver = new GPSDriverUBX(GPSDriverUBX::Interface::UART, &callbackEntry, this, &_reportGpsPos, _pReportSatInfo); - baudrate = 0; // auto-configure - } - gpsDriver->setSurveyInSpecs(_surveyInAccMeters * 10000.0f, _surveryInDurationSecs); - - if (_useFixedBaseLoction) { - gpsDriver->setBasePosition(_fixedBaseLatitude, _fixedBaseLongitude, _fixedBaseAltitudeMeters, _fixedBaseAccuracyMeters * 1000.0f); - } + qCDebug(GPSProviderLog) << QString("Survey in accuracy: %1 | duration: %2").arg(surveyInAccMeters).arg(surveryInDurationSecs); - _gpsConfig.output_mode = GPSHelper::OutputMode::RTCM; - if (gpsDriver->configure(baudrate, _gpsConfig) == 0) { - - /* reset report */ - memset(&_reportGpsPos, 0, sizeof(_reportGpsPos)); - - //In rare cases it can happen that we get an error from the driver (eg. checksum failure) due to - //bus errors or buggy firmware. In this case we want to try multiple times before giving up. - int numTries = 0; - - while (!_requestStop && numTries < 3) { - int helperRet = gpsDriver->receive(GPS_RECEIVE_TIMEOUT); - - if (helperRet > 0) { - numTries = 0; - - if (helperRet & 1) { - publishGPSPosition(); - numTries = 0; - } - - if (_pReportSatInfo && (helperRet & 2)) { - publishGPSSatellite(); - numTries = 0; - } - } else { - ++numTries; - } - } - if (_serial->error() != QSerialPort::NoError && _serial->error() != QSerialPort::TimeoutError) { - break; - } - } - } - qCDebug(RTKGPSLog) << "Exiting GPS thread"; + m_serial->setPortName(m_device); } -GPSProvider::GPSProvider(const QString& device, - GPSType type, - bool enableSatInfo, - double surveyInAccMeters, - int surveryInDurationSecs, - bool useFixedBaseLocation, - double fixedBaseLatitude, - double fixedBaseLongitude, - float fixedBaseAltitudeMeters, - float fixedBaseAccuracyMeters, - const std::atomic_bool& requestStop) - : _device (device) - , _type (type) - , _requestStop (requestStop) - , _surveyInAccMeters (surveyInAccMeters) - , _surveryInDurationSecs (surveryInDurationSecs) - , _useFixedBaseLoction (useFixedBaseLocation) - , _fixedBaseLatitude (fixedBaseLatitude) - , _fixedBaseLongitude (fixedBaseLongitude) - , _fixedBaseAltitudeMeters (fixedBaseAltitudeMeters) - , _fixedBaseAccuracyMeters (fixedBaseAccuracyMeters) +GPSProvider::~GPSProvider() { - qCDebug(RTKGPSLog) << "Survey in accuracy:duration" << surveyInAccMeters << surveryInDurationSecs; - if (enableSatInfo) _pReportSatInfo = new satellite_info_s(); + // qCDebug(GPSProviderLog) << Q_FUNC_INFO << this; } -GPSProvider::~GPSProvider() +void GPSProvider::_publishSatelliteInfo() { - if (_pReportSatInfo) delete(_pReportSatInfo); - if (_serial) delete _serial; + emit satelliteInfoUpdate(m_satelliteInfo); } -void GPSProvider::publishGPSPosition() +void GPSProvider::_publishSensorGNSSRelative() { - GPSPositionMessage msg; - msg.position_data = _reportGpsPos; - emit positionUpdate(msg); + emit sensorGnssRelativeUpdate(m_sensorGnssRelative); } -void GPSProvider::publishGPSSatellite() +void GPSProvider::_publishSensorGPS() { - GPSSatelliteMessage msg; - msg.satellite_data = *_pReportSatInfo; - emit satelliteInfoUpdate(msg); + emit sensorGpsUpdate(m_sensorGps); } -void GPSProvider::gotRTCMData(uint8_t* data, size_t len) +void GPSProvider::_gotRTCMData(const uint8_t *data, size_t len) { - QByteArray message((char*)data, static_cast(len)); + const QByteArray message(reinterpret_cast(data), len); emit RTCMDataUpdate(message); } -int GPSProvider::callbackEntry(GPSCallbackType type, void *data1, int data2, void *user) +int GPSProvider::_callbackEntry(GPSCallbackType type, void *data1, int data2, void *user) { - GPSProvider *gps = (GPSProvider *)user; + GPSProvider* const gps = reinterpret_cast(user); return gps->callback(type, data1, data2); } int GPSProvider::callback(GPSCallbackType type, void *data1, int data2) { switch (type) { - case GPSCallbackType::readDeviceData: { - if (_serial->bytesAvailable() == 0) { - int timeout = *((int *) data1); - if (!_serial->waitForReadyRead(timeout)) - return 0; //timeout + case GPSCallbackType::readDeviceData: + if (m_serial->bytesAvailable() == 0) { + const int timeout = *(reinterpret_cast(data1)); + if (!m_serial->waitForReadyRead(timeout)) { + return 0; } - return (int)_serial->read((char*) data1, data2); } - case GPSCallbackType::writeDeviceData: - if (_serial->write((char*) data1, data2) >= 0) { - if (_serial->waitForBytesWritten(-1)) - return data2; + return m_serial->read(reinterpret_cast(data1), data2); + case GPSCallbackType::writeDeviceData: + if (m_serial->write(reinterpret_cast(data1), data2) >= 0) { + if (m_serial->waitForBytesWritten(-1)) { + return data2; } - return -1; - - case GPSCallbackType::setBaudrate: - return _serial->setBaudRate(data2) ? 0 : -1; + } + return -1; + case GPSCallbackType::setBaudrate: + return (m_serial->setBaudRate(data2) ? 0 : -1); + case GPSCallbackType::gotRTCMMessage: + _gotRTCMData(reinterpret_cast(data1), data2); + break; + case GPSCallbackType::surveyInStatus: + { + const SurveyInStatus* const status = reinterpret_cast(data1); + qCDebug(GPSProviderLog) << "Position:" << status->latitude << status->longitude << status->altitude; + + const bool valid = status->flags & 1; + const bool active = (status->flags >> 1) & 1; + + qCDebug(GPSProviderLog) << QString("Survey-in status: %1s cur accuracy: %2mm valid: %3 active: %4").arg(status->duration).arg(status->mean_accuracy).arg(valid).arg(active); + emit surveyInStatus(status->duration, status->mean_accuracy, status->latitude, status->longitude, status->altitude, valid, active); + break; + } + case GPSCallbackType::setClock: + case GPSCallbackType::gotRelativePositionMessage: + // m_sensorGnssRelative + default: + break; + } - case GPSCallbackType::gotRTCMMessage: - gotRTCMData((uint8_t*) data1, data2); - break; + return 0; +} - case GPSCallbackType::surveyInStatus: - { - SurveyInStatus* status = (SurveyInStatus*)data1; - qCDebug(RTKGPSLog) << "Position: " << status->latitude << status->longitude << status->altitude; +void GPSProvider::run() +{ + if (!m_serial->open(QIODevice::ReadWrite)) { + uint32_t retries = 60; + while ((retries-- > 0) && (m_serial->error() == QSerialPort::PermissionError)) { + qCDebug(GPSProviderLog) << "Cannot open device... retrying"; + msleep(500); + if (m_serial->open(QIODevice::ReadWrite)) { + m_serial->clearError(); + break; + } + } - qCDebug(RTKGPSLog) << QString("Survey-in status: %1s cur accuracy: %2mm valid: %3 active: %4").arg(status->duration).arg(status->mean_accuracy).arg((int)(status->flags & 1)).arg((int)((status->flags>>1) & 1)); - emit surveyInStatus(status->duration, status->mean_accuracy, status->latitude, status->longitude, status->altitude, (int)(status->flags & 1), (int)((status->flags>>1) & 1)); + if (m_serial->error() != QSerialPort::NoError) { + qCWarning(GPSProviderLog) << "GPS: Failed to open Serial Device" << m_device << m_serial->errorString(); + return; } - break; + } + + (void) m_serial->setBaudRate(QSerialPort::Baud9600); + (void) m_serial->setDataBits(QSerialPort::Data8); + (void) m_serial->setParity(QSerialPort::NoParity); + (void) m_serial->setStopBits(QSerialPort::OneStop); + (void) m_serial->setFlowControl(QSerialPort::NoFlowControl); + + GPSBaseStationSupport *gpsDriver = nullptr; + uint32_t baudrate = 0; + switch(m_type) { + case GPSType::trimble: + gpsDriver = new GPSDriverAshtech(&_callbackEntry, this, &m_sensorGps, &m_satelliteInfo); + baudrate = 115200; + break; + case GPSType::septentrio: + gpsDriver = new GPSDriverSBF(&_callbackEntry, this, &m_sensorGps, &m_satelliteInfo, GPS_HEADING_OFFSET); + baudrate = 0; + break; + case GPSType::u_blox: + gpsDriver = new GPSDriverUBX(GPSDriverUBX::Interface::UART, &_callbackEntry, this, &m_sensorGps, &m_satelliteInfo); + baudrate = 0; + break; + case GPSType::femto: + gpsDriver = new GPSDriverFemto(&_callbackEntry, this, &m_sensorGps, &m_satelliteInfo); + baudrate = 0; + default: + // GPSDriverEmlidReach, GPSDriverMTK, GPSDriverNMEA + qCWarning(GPSProviderLog) << "Unsupported GPS Type:" << static_cast(m_type); + return; + } - case GPSCallbackType::setClock: - /* do nothing */ - break; + gpsDriver->setSurveyInSpecs(m_surveyInAccMeters * 10000.0f, m_surveryInDurationSecs); - case GPSCallbackType::gotRelativePositionMessage: - /* do nothing */ - break; + if (m_useFixedBaseLoction) { + gpsDriver->setBasePosition(m_fixedBaseLatitude, m_fixedBaseLongitude, m_fixedBaseAltitudeMeters, m_fixedBaseAccuracyMeters * 1000.0f); } - return 0; + m_gpsConfig.output_mode = GPSHelper::OutputMode::RTCM; + + while (!m_requestStop) { + if (gpsDriver->configure(baudrate, m_gpsConfig) == 0) { + (void) memset(&m_sensorGps, 0, sizeof(m_sensorGps)); + + uint8_t numTries = 0; + while (!m_requestStop && (numTries < 3)) { + const int helperRet = gpsDriver->receive(GPS_RECEIVE_TIMEOUT); + + if (helperRet > 0) { + numTries = 0; + + if (helperRet & GPS_RECEIVE_POSITION) { + _publishSensorGPS(); + numTries = 0; + } + + if (helperRet & GPS_RECEIVE_SATELLITE) { + _publishSatelliteInfo(); + numTries = 0; + } + } else { + ++numTries; + } + } + + if ((m_serial->error() != QSerialPort::NoError) && (m_serial->error() != QSerialPort::TimeoutError)) { + break; + } + } + } + + qCDebug(GPSProviderLog) << Q_FUNC_INFO << "Exiting GPS thread"; } diff --git a/src/GPS/GPSProvider.h b/src/GPS/GPSProvider.h index e78550a8033f..f2d7437ac66f 100644 --- a/src/GPS/GPSProvider.h +++ b/src/GPS/GPSProvider.h @@ -10,84 +10,82 @@ #pragma once +#include +#include +#include +#include -#include "GPSPositionMessage.h" #include "Drivers/src/gps_helper.h" +#include "sensor_gps.h" +#include "sensor_gnss_relative.h" +#include "satellite_info.h" -#include -#include -#include +Q_DECLARE_LOGGING_CATEGORY(GPSProviderLog) class QSerialPort; +class QThread; - -/** - ** class GPSProvider - * opens a GPS device and handles the protocol - */ -class GPSProvider : public QThread +class GPSProvider : public QObject { Q_OBJECT -public: +public: enum class GPSType { u_blox, trimble, - septentrio + septentrio, + femto }; - GPSProvider(const QString& device, - GPSType type, - bool enableSatInfo, - double surveyInAccMeters, - int surveryInDurationSecs, - bool useFixedBaseLocation, - double fixedBaseLatitude, - double fixedBaseLongitude, - float fixedBaseAltitudeMeters, - float fixedBaseAccuracyMeters, - const std::atomic_bool& requestStop); + GPSProvider( + const QString &device, + GPSType type, + double surveyInAccMeters, + int surveryInDurationSecs, + bool useFixedBaseLocation, + double fixedBaseLatitude, + double fixedBaseLongitude, + float fixedBaseAltitudeMeters, + float fixedBaseAccuracyMeters, + const std::atomic_bool &requestStop, + QObject *parent = nullptr + ); ~GPSProvider(); - /** - * this is called by the callback method - */ - void gotRTCMData(uint8_t *data, size_t len); + int callback(GPSCallbackType type, void *data1, int data2); signals: - void positionUpdate(GPSPositionMessage message); - void satelliteInfoUpdate(GPSSatelliteMessage message); - void RTCMDataUpdate(QByteArray message); + void satelliteInfoUpdate(const satellite_info_s &message); + void sensorGnssRelativeUpdate(const sensor_gnss_relative_s &message); + void sensorGpsUpdate(const sensor_gps_s &message); + void RTCMDataUpdate(const QByteArray &message); void surveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active); -protected: - void run(); - private: - void publishGPSPosition(); - void publishGPSSatellite(); - - /** - * callback from the driver for the platform specific stuff - */ - static int callbackEntry(GPSCallbackType type, void *data1, int data2, void *user); - - int callback(GPSCallbackType type, void *data1, int data2); - - QString _device; - GPSType _type; - const std::atomic_bool& _requestStop; - double _surveyInAccMeters; - int _surveryInDurationSecs; - bool _useFixedBaseLoction; - double _fixedBaseLatitude; - double _fixedBaseLongitude; - float _fixedBaseAltitudeMeters; - float _fixedBaseAccuracyMeters; - GPSHelper::GPSConfig _gpsConfig{}; - - struct sensor_gps_s _reportGpsPos; - struct satellite_info_s *_pReportSatInfo = nullptr; - - QSerialPort *_serial = nullptr; + void _publishSensorGPS(); + void _publishSatelliteInfo(); + void _publishSensorGNSSRelative(); + + void _gotRTCMData(const uint8_t *data, size_t len); + + static int _callbackEntry(GPSCallbackType type, void *data1, int data2, void *user); + + QString m_device; + GPSType m_type; + const std::atomic_bool &m_requestStop; + double m_surveyInAccMeters = 0.; + int m_surveryInDurationSecs = 0; + bool m_useFixedBaseLoction = false; + double m_fixedBaseLatitude = 0.; + double m_fixedBaseLongitude = 0.; + float m_fixedBaseAltitudeMeters = 0.f; + float m_fixedBaseAccuracyMeters = 0.f; + GPSHelper::GPSConfig m_gpsConfig{}; + + struct satellite_info_s m_satelliteInfo{}; + struct sensor_gnss_relative_s m_sensorGnssRelative{}; + struct sensor_gps_s m_sensorGps{}; + + QSerialPort *m_serial = nullptr; + QThread *m_thread = nullptr }; diff --git a/src/GPS/GPSRtk.cc b/src/GPS/GPSRtk.cc new file mode 100644 index 000000000000..7ff97e4d0610 --- /dev/null +++ b/src/GPS/GPSRtk.cc @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "GPSRtk.h" +#include "GPSProvider.h" +#include "QGCApplication.h" +#include "SettingsManager.h" +#include "RTKSettings.h" +#include "GPSRTKFactGroup.h" +#include "RTCMMavlink.h" +#include "QGCLoggingCategory.h" + +#define GPS_THREAD_DISCONNECT_TIMEOUT 2000 + +QGC_LOGGING_CATEGORY(GPSRtkLog, "qgc.gps.gpsrtk") + +GPSRtk::GPSRtk(QObject *parent) + : QObject(parent) +{ + // qCDebug(GPSRtkLog) << Q_FUNC_INFO << this; + + qRegisterMetaType("satellite_info_s"); + qRegisterMetaType("sensor_gnss_relative_s"); + qRegisterMetaType("sensor_gps_s"); +} + +GPSRtk::~GPSRtk() +{ + disconnectGPS(); + + // qCDebug(GPSRtkLog) << Q_FUNC_INFO << this; +} + +void GPSRtk::_onGPSConnect() +{ + m_gpsRtkFactGroup->connected()->setRawValue(true); +} + +void GPSRtk::_onGPSDisconnect() +{ + m_gpsRtkFactGroup->connected()->setRawValue(false); +} + +void GPSRtk::_onGPSSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active) +{ + m_gpsRtkFactGroup->currentDuration()->setRawValue(duration); + m_gpsRtkFactGroup->currentAccuracy()->setRawValue(static_cast(accuracyMM) / 1000.0); + m_gpsRtkFactGroup->currentLatitude()->setRawValue(latitude); + m_gpsRtkFactGroup->currentLongitude()->setRawValue(longitude); + m_gpsRtkFactGroup->currentAltitude()->setRawValue(altitude); + m_gpsRtkFactGroup->valid()->setRawValue(valid); + m_gpsRtkFactGroup->active()->setRawValue(active); +} + +void GPSRtk::_onSatelliteUpdate(uint8_t numSats) +{ + m_gpsRtkFactGroup->numSatellites()->setRawValue(numSats); +} + +void GPSRtk::connectGPS(const QString &device, QStringView gps_type) +{ + GPSProvider::GPSType type; + if (gps_type.contains(QStringLiteral("trimble"), Qt::CaseInsensitive)) { + type = GPSProvider::GPSType::trimble; + qCDebug(GPSRtkLog) << QStringLiteral("Connecting Trimble device"); + } else if (gps_type.contains(QStringLiteral("septentrio"), Qt::CaseInsensitive)) { + type = GPSProvider::GPSType::septentrio; + qCDebug(GPSRtkLog) << QStringLiteral("Connecting Septentrio device"); + } else if (gps_type.contains(QStringLiteral("femtomes"), Qt::CaseInsensitive)) { + type = GPSProvider::GPSType::femto; + qCDebug(GPSRtkLog) << QStringLiteral("Connecting Femtomes device"); + } else { + type = GPSProvider::GPSType::u_blox; + qCDebug(GPSRtkLog) << QStringLiteral("Connecting U-blox device"); + } + + disconnectGPS(); + + const RTKSettings* const rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings(); + m_requestGpsStop = false; + m_gpsProvider = new GPSProvider( + device, + type, + 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(), + m_requestGpsStop + ); + (void) QMetaObject::invokeMethod(m_gpsProvider, "start", Qt::AutoConnection); + + m_rtcmMavlink = new RTCMMavlink(this); + + (void) connect(m_gpsProvider, &GPSProvider::RTCMDataUpdate, m_rtcmMavlink, &RTCMMavlink::RTCMDataUpdate); + (void) connect(m_gpsProvider, &GPSProvider::sensorGpsUpdate, this, &GPSRtk::_sensorGpsUpdate); + (void) connect(m_gpsProvider, &GPSProvider::satelliteInfoUpdate, this, &GPSRtk::_satelliteInfoUpdate); + (void) connect(m_gpsProvider, &GPSProvider::finished, this, &GPSRtk::_onGPSDisconnect); + (void) connect(m_gpsProvider, &GPSProvider::surveyInStatus, this, &GPSRtk::_onGPSSurveyInStatus); + + (void) QMetaObject::invokeMethod(this, "_onGPSConnect", Qt::AutoConnection, msg.count); +} + +void GPSRtk::disconnectGPS() +{ + if (m_gpsProvider) { + m_requestGpsStop = true; + if (!m_gpsProvider->wait(GPS_THREAD_DISCONNECT_TIMEOUT)) { + qCWarning(GPSRtkLog) << "Failed to wait for GPS thread exit. Consider increasing the timeout"; + } + + delete m_gpsProvider; + m_gpsProvider = nullptr; + } + + if (m_rtcmMavlink) { + delete m_rtcmMavlink; + m_rtcmMavlink = nullptr; + } +} + +bool GPSRtk::connected() const +{ + return (m_gpsProvider ? m_gpsProvider->isRunning() : false); +} + +FactGroup *GPSRtk::gpsRtkFactGroup() +{ + return m_gpsRtkFactGroup; +} + +void GPSRtk::_satelliteInfoUpdate(const satellite_info_s &msg) +{ + qCDebug(GPSRtkLog) << Q_FUNC_INFO << QStringLiteral("%1 satellites").arg(msg.count); + (void) QMetaObject::invokeMethod(this, "_onSatelliteUpdate", Qt::AutoConnection, msg.count); +} + +void GPSRtk::_sensorGnssRelativeUpdate(const sensor_gnss_relative_s &msg) +{ + qCDebug(GPSRtkLog) << Q_FUNC_INFO; +} + +void GPSRtk::_sensorGpsUpdate(const sensor_gps_s &msg) +{ + qCDebug(GPSRtkLog) << Q_FUNC_INFO << QStringLiteral("alt=%1, long=%2, lat=%3").arg(msg.altitude_msl_m).arg(msg.longitude_deg).arg(msg.latitude_deg); +} diff --git a/src/GPS/GPSRtk.h b/src/GPS/GPSRtk.h new file mode 100644 index 000000000000..fb80ee7d6cc8 --- /dev/null +++ b/src/GPS/GPSRtk.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "sensor_gps.h" +#include "sensor_gnss_relative.h" +#include "satellite_info.h" + +Q_DECLARE_LOGGING_CATEGORY(GPSRtkLog) + +class GPSRTKFactGroup; +class FactGroup; +class RTCMMavlink; +class GPSProvider; + +class GPSRtk : public QObject +{ + Q_OBJECT + +public: + GPSRtk(QObject *parent = nullptr); + ~GPSRtk(); + + void connectGPS(const QString &device, QStringView gps_type); + void disconnectGPS(); + bool connected() const; + FactGroup *gpsRtkFactGroup(); + +private slots: + void _satelliteInfoUpdate(const satellite_info_s &msg); + void _sensorGnssRelativeUpdate(const sensor_gnss_relative_s &msg); + void _sensorGpsUpdate(const sensor_gps_s &msg); + void _onGPSConnect(); + void _onGPSDisconnect(); + void _onGPSSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active); + +private: + GPSProvider *m_gpsProvider = nullptr; + RTCMMavlink *m_rtcmMavlink = nullptr; + GPSRTKFactGroup *m_gpsRtkFactGroup = nullptr; + + std::atomic_bool m_requestGpsStop = false; +}; diff --git a/src/GPS/RTCMMavlink.cc b/src/GPS/RTCMMavlink.cc index 36bed917cf45..f0eb993cc034 100644 --- a/src/GPS/RTCMMavlink.cc +++ b/src/GPS/RTCMMavlink.cc @@ -7,75 +7,89 @@ * ****************************************************************************/ - #include "RTCMMavlink.h" +#include "QGCApplication.h" #include "MultiVehicleManager.h" #include "Vehicle.h" #include "MAVLinkProtocol.h" +#include + +QGC_LOGGING_CATEGORY(RTCMMavlinkLog, "qgc.gps.rtcmmavlink") -RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) - : _toolbox(toolbox) +RTCMMavlink::RTCMMavlink(QObject *parent) + : QObject(parent) { - _bandwidthTimer.start(); + // qCDebug(RTCMMavlinkLog) << Q_FUNC_INFO << this; + + m_bandwidthTimer.start(); } -void RTCMMavlink::RTCMDataUpdate(QByteArray message) +RTCMMavlink::~RTCMMavlink() { - /* statistics */ - _bandwidthByteCounter += message.size(); - qint64 elapsed = _bandwidthTimer.elapsed(); - if (elapsed > 1000) { - qDebug() << QStringLiteral("RTCM bandwidth: %1 kB/s\n").arg(_bandwidthByteCounter / elapsed * 1000.f / 1024.f); - _bandwidthTimer.restart(); - _bandwidthByteCounter = 0; + // qCDebug(RTCMMavlinkLog) << Q_FUNC_INFO << this; +} + +void RTCMMavlink::RTCMDataUpdate(QByteArrayView data) +{ + if (m_bandwidthTimer.isValid()) { + m_bandwidthByteCounter += data.size(); + + const qint64 elapsed = m_bandwidthTimer.elapsed(); + if (elapsed > 1000) { + qCDebug(RTCMMavlinkLog) << QStringLiteral("RTCM bandwidth: %1 kB/s").arg(((m_bandwidthByteCounter / elapsed) * 1000.f) / 1024.f); + (void) m_bandwidthTimer.restart(); + m_bandwidthByteCounter = 0; + } } - const qsizetype maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN; - mavlink_gps_rtcm_data_t mavlinkRtcmData; - memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t)); + mavlink_gps_rtcm_data_t gpsRtcmData; + (void) memset(&gpsRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t)); - if (message.size() < maxMessageLength) { - mavlinkRtcmData.len = message.size(); - mavlinkRtcmData.flags = (_sequenceId & 0x1F) << 3; - memcpy(&mavlinkRtcmData.data, message.data(), message.size()); - sendMessageToVehicle(mavlinkRtcmData); + static constexpr qsizetype maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN; + if (data.size() < maxMessageLength) { + gpsRtcmData.len = data.size(); + gpsRtcmData.flags = (m_sequenceId & 0x1FU) << 3; + (void) memcpy(&gpsRtcmData.data, data.data(), data.size()); + _sendMessageToVehicle(gpsRtcmData); } else { - // We need to fragment + uint8_t fragmentId = 0; + qsizetype start = 0; + while (start < data.size()) { + gpsRtcmData.flags = 0x01U; // LSB set indicates message is fragmented + gpsRtcmData.flags |= fragmentId++ << 1; // Next 2 bits are fragment id + gpsRtcmData.flags |= (m_sequenceId & 0x1FU) << 3; // Next 5 bits are sequence id + + const qsizetype length = std::min(data.size() - start, maxMessageLength); + gpsRtcmData.len = length; + + (void) memcpy(gpsRtcmData.data, data.constData() + start, length); + _sendMessageToVehicle(gpsRtcmData); - uint8_t fragmentId = 0; // Fragment id indicates the fragment within a set - int start = 0; - while (start < message.size()) { - int length = std::min(message.size() - start, maxMessageLength); - mavlinkRtcmData.flags = 1; // LSB set indicates message is fragmented - mavlinkRtcmData.flags |= fragmentId++ << 1; // Next 2 bits are fragment id - mavlinkRtcmData.flags |= (_sequenceId & 0x1F) << 3; // Next 5 bits are sequence id - mavlinkRtcmData.len = length; - memcpy(&mavlinkRtcmData.data, message.data() + start, length); - sendMessageToVehicle(mavlinkRtcmData); start += length; } } - ++_sequenceId; + + ++m_sequenceId; } -void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg) +void RTCMMavlink::_sendMessageToVehicle(const mavlink_gps_rtcm_data_t &data) { - QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles(); - MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol(); - for (int i = 0; i < vehicles.count(); i++) { - Vehicle* vehicle = qobject_cast(vehicles[i]); - WeakLinkInterfacePtr weakLink = vehicle->vehicleLinkManager()->primaryLink(); - + QmlObjectListModel* const vehicles = qgcApp()->toolbox()->multiVehicleManager()->vehicles(); + for (qsizetype i = 0; i < vehicles->count(); i++) { + Vehicle* const vehicle = qobject_cast(vehicles->get(i)); + const WeakLinkInterfacePtr weakLink = vehicle->vehicleLinkManager()->primaryLink(); if (!weakLink.expired()) { - mavlink_message_t message; - SharedLinkInterfacePtr sharedLink = weakLink.lock(); - - mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(), - mavlinkProtocol->getComponentId(), - sharedLink->mavlinkChannel(), - &message, - &msg); - vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message); + const MAVLinkProtocol* const mavlinkProtocol = qgcApp()->toolbox()->mavlinkProtocol(); + const SharedLinkInterfacePtr sharedLink = weakLink.lock(); + mavlink_message_t message; + (void) mavlink_msg_gps_rtcm_data_encode_chan( + mavlinkProtocol->getSystemId(), + mavlinkProtocol->getComponentId(), + sharedLink->mavlinkChannel(), + &message, + &data + ); + (void) vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message); } } } diff --git a/src/GPS/RTCMMavlink.h b/src/GPS/RTCMMavlink.h index abf02549a5af..7175342b6108 100644 --- a/src/GPS/RTCMMavlink.h +++ b/src/GPS/RTCMMavlink.h @@ -10,31 +10,29 @@ #pragma once -#include "QGCToolbox.h" -#include "MAVLinkLib.h" - #include #include +#include + +typedef struct __mavlink_gps_rtcm_data_t mavlink_gps_rtcm_data_t; + +Q_DECLARE_LOGGING_CATEGORY(RTCMMavlinkLog) -/** - ** class RTCMMavlink - * Receives RTCM updates and sends them via MAVLINK to the device - */ class RTCMMavlink : public QObject { Q_OBJECT + public: - RTCMMavlink(QGCToolbox& toolbox); - //TODO: API to select device(s)? + RTCMMavlink(QObject *parent = nullptr); + ~RTCMMavlink(); public slots: - void RTCMDataUpdate(QByteArray message); + void RTCMDataUpdate(QByteArrayView data); private: - void sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg); + static void _sendMessageToVehicle(const mavlink_gps_rtcm_data_t &data); - QGCToolbox& _toolbox; - QElapsedTimer _bandwidthTimer; - int _bandwidthByteCounter = 0; - uint8_t _sequenceId = 0; + QElapsedTimer m_bandwidthTimer; + qsizetype m_bandwidthByteCounter = 0; + uint8_t m_sequenceId = 0; }; diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h index fe6c0e3dabe2..e360f1ae0d8b 100644 --- a/src/GPS/definitions.h +++ b/src/GPS/definitions.h @@ -40,49 +40,27 @@ #pragma once #include +#include +#include -#define GPS_READ_BUFFER_SIZE 1024 - -#define GPS_INFO(...) qInfo(__VA_ARGS__) -#define GPS_WARN(...) qWarning(__VA_ARGS__) -#define GPS_ERR(...) qCritical(__VA_ARGS__) - +#include #include "sensor_gps.h" #include "sensor_gnss_relative.h" #include "satellite_info.h" -#define M_DEG_TO_RAD (M_PI / 180.0) -#define M_RAD_TO_DEG (180.0 / M_PI) -#define M_DEG_TO_RAD_F 0.01745329251994f -#define M_RAD_TO_DEG_F 57.2957795130823f +Q_DECLARE_LOGGING_CATEGORY(GPSDriversLog) -#define M_PI_2_F M_PI - -#include +#define GPS_INFO(...) qCInfo(GPSDriversLog, __VA_ARGS__) +#define GPS_WARN(...) qCWarning(GPSDriversLog, __VA_ARGS__) +#define GPS_ERR(...) qCCritical(GPSDriversLog, __VA_ARGS__) -class Sleeper : public QThread -{ -public: - static void usleep(unsigned long usecs) { QThread::usleep(usecs); } -}; - -static inline void gps_usleep(unsigned long usecs) { - Sleeper::usleep(usecs); -} - -typedef uint64_t gps_abstime; +#define M_DEG_TO_RAD (M_PI / 180.0) +#define M_RAD_TO_DEG (180.0 / M_PI) +#define M_DEG_TO_RAD_F 0.01745329251994f +#define M_RAD_TO_DEG_F 57.2957795130823f -#include -/** - * Get the current time in us. Function signature: - * uint64_t hrt_absolute_time() - */ -static inline gps_abstime gps_absolute_time() { - //FIXME: is there something with microsecond accuracy? - return QDateTime::currentMSecsSinceEpoch() * 1000; -} +#define M_PI_2_F M_PI -//timespec is UNIX-specific #ifdef _WIN32 #if _MSC_VER < 1900 struct timespec @@ -95,3 +73,13 @@ struct timespec #endif #endif +static inline void gps_usleep(unsigned long usecs) +{ + QGC::SLEEP::usleep(usecs); +} + +typedef uint64_t gps_abstime; +static inline gps_abstime gps_absolute_time() +{ + return (QDateTime::currentMSecsSinceEpoch() * 1000); +} diff --git a/src/GPS/satellite_info.h b/src/GPS/satellite_info.h index 27313193721e..d49d805eef99 100644 --- a/src/GPS/satellite_info.h +++ b/src/GPS/satellite_info.h @@ -31,16 +31,18 @@ * ****************************************************************************/ +/* https://github.com/PX4/Firmware/blob/master/msg/SatelliteInfo.msg */ + #pragma once -#include -/* - * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/satellite_info.msg - * and was manually copied here. - */ +#include +#include -struct satellite_info_s { +struct satellite_info_s +{ uint64_t timestamp; + static constexpr uint8_t SAT_INFO_MAX_SATELLITES = 20; + uint8_t count; uint8_t svid[20]; uint8_t used[20]; @@ -48,8 +50,5 @@ struct satellite_info_s { uint8_t azimuth[20]; uint8_t snr[20]; uint8_t prn[20]; -#ifdef __cplusplus - static const uint8_t SAT_INFO_MAX_SATELLITES = 20; - -#endif }; +Q_DECLARE_METATYPE(satellite_info_s); diff --git a/src/GPS/sensor_gnss_relative.h b/src/GPS/sensor_gnss_relative.h index 1fab63728006..fbe83f4b7dca 100644 --- a/src/GPS/sensor_gnss_relative.h +++ b/src/GPS/sensor_gnss_relative.h @@ -31,29 +31,33 @@ * ****************************************************************************/ -/* Auto-generated by genmsg_cpp from file /home/julianoes/src/upstream/PX4-Autopilot/msg/SensorGnssRelative.msg */ - +/* https://github.com/PX4/Firmware/blob/master/msg/SensorGnssRelative.msg */ #pragma once -#include -/* - * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/SensorGnssRelative.msg - * and was manually copied here. - */ +#include +#include -struct sensor_gnss_relative_s { +struct sensor_gnss_relative_s +{ uint64_t timestamp; uint64_t timestamp_sample; - uint64_t time_utc_usec; + uint32_t device_id; + + uint64_t time_utc_usec; + + uint16_t reference_station_id; + float position[3]; float position_accuracy[3]; + float heading; float heading_accuracy; + float position_length; float accuracy_length; - uint16_t reference_station_id; + bool gnss_fix_ok; bool differential_solution; bool relative_position_valid; @@ -65,3 +69,4 @@ struct sensor_gnss_relative_s { bool heading_valid; bool relative_position_normalized; }; +Q_DECLARE_METATYPE(sensor_gnss_relative_s); diff --git a/src/GPS/sensor_gps.h b/src/GPS/sensor_gps.h index 7a831cebd91f..e5b081606d38 100644 --- a/src/GPS/sensor_gps.h +++ b/src/GPS/sensor_gps.h @@ -31,65 +31,74 @@ * ****************************************************************************/ -/* Auto-generated by genmsg_cpp from file /home/julianoes/src/upstream/PX4-Autopilot/msg/SensorGps.msg */ - +/* https://github.com/PX4/Firmware/blob/master/msg/SensorGps.msg */ #pragma once -#include -/* - * This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/SensorGps.msg - * and was manually copied here. - */ +#include +#include -struct sensor_gps_s { +struct sensor_gps_s +{ uint64_t timestamp; uint64_t timestamp_sample; + + uint32_t device_id; + double latitude_deg; double longitude_deg; double altitude_msl_m; double altitude_ellipsoid_m; - uint64_t time_utc_usec; - uint32_t device_id; + float s_variance_m_s; float c_variance_rad; + uint8_t fix_type; + float eph; float epv; + float hdop; float vdop; + int32_t noise_per_ms; + uint16_t automatic_gain_control; + + static constexpr uint8_t JAMMING_STATE_UNKNOWN = 0; + static constexpr uint8_t JAMMING_STATE_OK = 1; + static constexpr uint8_t JAMMING_STATE_WARNING = 2; + static constexpr uint8_t JAMMING_STATE_CRITICAL = 3; + uint8_t jamming_state; int32_t jamming_indicator; + + static constexpr uint8_t SPOOFING_STATE_UNKNOWN = 0; + static constexpr uint8_t SPOOFING_STATE_NONE = 1; + static constexpr uint8_t SPOOFING_STATE_INDICATED = 2; + static constexpr uint8_t SPOOFING_STATE_MULTIPLE = 3; + uint8_t spoofing_state; + float vel_m_s; float vel_n_m_s; float vel_e_m_s; float vel_d_m_s; float cog_rad; + bool vel_ned_valid; + int32_t timestamp_time_relative; + uint64_t time_utc_usec; + + uint8_t satellites_used; + float heading; float heading_offset; float heading_accuracy; + float rtcm_injection_rate; - uint16_t automatic_gain_control; - uint8_t fix_type; - uint8_t jamming_state; - uint8_t spoofing_state; - bool vel_ned_valid; - uint8_t satellites_used; uint8_t selected_rtcm_instance; - bool rtcm_crc_failed; - uint8_t rtcm_msg_used; - uint8_t _padding0[2]; // required for logger - - static constexpr uint8_t JAMMING_STATE_UNKNOWN = 0; - static constexpr uint8_t JAMMING_STATE_OK = 1; - static constexpr uint8_t JAMMING_STATE_WARNING = 2; - static constexpr uint8_t JAMMING_STATE_CRITICAL = 3; - static constexpr uint8_t SPOOFING_STATE_UNKNOWN = 0; - static constexpr uint8_t SPOOFING_STATE_NONE = 1; - static constexpr uint8_t SPOOFING_STATE_INDICATED = 2; - static constexpr uint8_t SPOOFING_STATE_MULTIPLE = 3; + bool rtcm_crc_failed; static constexpr uint8_t RTCM_MSG_USED_UNKNOWN = 0; static constexpr uint8_t RTCM_MSG_USED_NOT_USED = 1; static constexpr uint8_t RTCM_MSG_USED_USED = 2; + uint8_t rtcm_msg_used; }; +Q_DECLARE_METATYPE(sensor_gps_s); diff --git a/src/QGCToolbox.cc b/src/QGCToolbox.cc index 8a78cd18090f..933ea06ce2b8 100644 --- a/src/QGCToolbox.cc +++ b/src/QGCToolbox.cc @@ -9,9 +9,6 @@ #include "FirmwarePluginManager.h" -#ifndef NO_SERIAL_LINK -#include "GPSManager.h" -#endif #include "JoystickManager.h" #include "LinkManager.h" #include "MAVLinkProtocol.h" @@ -47,9 +44,6 @@ QGCToolbox::QGCToolbox(QGCApplication* app) //-- Scan and load plugins _scanAndLoadPlugins(app); _firmwarePluginManager = new FirmwarePluginManager (app, this); -#ifndef NO_SERIAL_LINK - _gpsManager = new GPSManager (app, this); -#endif _joystickManager = new JoystickManager (app, this); _linkManager = new LinkManager (app, this); _mavlinkProtocol = new MAVLinkProtocol (app, this); @@ -77,9 +71,6 @@ void QGCToolbox::setChildToolboxes(void) _corePlugin->setToolbox(this); _firmwarePluginManager->setToolbox(this); -#ifndef NO_SERIAL_LINK - _gpsManager->setToolbox(this); -#endif _joystickManager->setToolbox(this); _linkManager->setToolbox(this); _mavlinkProtocol->setToolbox(this); diff --git a/src/QGCToolbox.h b/src/QGCToolbox.h index dd8c01f31652..00e76abd8d82 100644 --- a/src/QGCToolbox.h +++ b/src/QGCToolbox.h @@ -14,7 +14,6 @@ #include class FirmwarePluginManager; -class GPSManager; class JoystickManager; class FollowMe; class LinkManager; @@ -57,9 +56,6 @@ class QGCToolbox : public QObject { QGCCorePlugin* corePlugin () { return _corePlugin; } SettingsManager* settingsManager () { return _settingsManager; } ADSBVehicleManager* adsbVehicleManager () { return _adsbVehicleManager; } -#ifndef NO_SERIAL_LINK - GPSManager* gpsManager () { return _gpsManager; } -#endif #ifndef QGC_AIRLINK_DISABLED AirLinkManager* airlinkManager () { return _airlinkManager; } #endif @@ -72,9 +68,6 @@ class QGCToolbox : public QObject { void _scanAndLoadPlugins(QGCApplication *app); FirmwarePluginManager* _firmwarePluginManager = nullptr; -#ifndef NO_SERIAL_LINK - GPSManager* _gpsManager = nullptr; -#endif JoystickManager* _joystickManager = nullptr; LinkManager* _linkManager = nullptr; MAVLinkProtocol* _mavlinkProtocol = nullptr; diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index 8d7ed4513be7..eb06a2cfce55 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -17,6 +17,7 @@ #include "PositionManager.h" #ifndef NO_SERIAL_LINK #include "GPSManager.h" +#include "GPSRtk.h" #endif #include "QGCPalette.h" #ifdef QT_DEBUG @@ -82,7 +83,7 @@ void QGroundControlQmlGlobal::setToolbox(QGCToolbox* toolbox) _firmwarePluginManager = toolbox->firmwarePluginManager(); _settingsManager = toolbox->settingsManager(); #ifndef NO_SERIAL_LINK - _gpsRtkFactGroup = toolbox->gpsManager()->gpsRtkFactGroup(); + _gpsRtkFactGroup = GPSManager::instance()->gpsRtk()->gpsRtkFactGroup(); #endif _adsbVehicleManager = toolbox->adsbVehicleManager(); _globalPalette = new QGCPalette(this); diff --git a/src/Utilities/QGCLoggingCategory.cc b/src/Utilities/QGCLoggingCategory.cc index f0d1df621c34..d560a97b8723 100644 --- a/src/Utilities/QGCLoggingCategory.cc +++ b/src/Utilities/QGCLoggingCategory.cc @@ -24,7 +24,6 @@ QGC_LOGGING_CATEGORY(MissionCommandsLog, "MissionCommandsLog") QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog") QGC_LOGGING_CATEGORY(ParameterManagerLog, "ParameterManagerLog") QGC_LOGGING_CATEGORY(GeotaggingLog, "GeotaggingLog") -QGC_LOGGING_CATEGORY(RTKGPSLog, "RTKGPSLog") QGC_LOGGING_CATEGORY(GuidedActionsControllerLog, "GuidedActionsControllerLog") QGC_LOGGING_CATEGORY(LocalizationLog, "LocalizationLog") QGC_LOGGING_CATEGORY(VideoAllLog, kVideoAllLogCategory) diff --git a/src/Utilities/QGCLoggingCategory.h b/src/Utilities/QGCLoggingCategory.h index 30b2589a4ffd..39434a2268a2 100644 --- a/src/Utilities/QGCLoggingCategory.h +++ b/src/Utilities/QGCLoggingCategory.h @@ -20,7 +20,6 @@ Q_DECLARE_LOGGING_CATEGORY(MissionCommandsLog) Q_DECLARE_LOGGING_CATEGORY(MissionItemLog) Q_DECLARE_LOGGING_CATEGORY(ParameterManagerLog) Q_DECLARE_LOGGING_CATEGORY(GeotaggingLog) -Q_DECLARE_LOGGING_CATEGORY(RTKGPSLog) Q_DECLARE_LOGGING_CATEGORY(GuidedActionsControllerLog) Q_DECLARE_LOGGING_CATEGORY(LocalizationLog) Q_DECLARE_LOGGING_CATEGORY(VideoAllLog) // turns on all individual QGC video logs diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a94b08078c09..1bf53f861e1b 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -43,6 +43,9 @@ add_qgc_test(ParameterManagerTest) add_subdirectory(Geo) add_qgc_test(GeoTest) +add_subdirectory(GPS) +add_qgc_test(GpsTest) + add_subdirectory(MAVLink) add_qgc_test(StatusTextHandlerTest) add_qgc_test(SigningTest) @@ -119,6 +122,7 @@ target_link_libraries(qgctest CompressionTest FactSystemTest GeoTest + GpsTest MAVLinkTest MissionManagerTest QmlControlsTest diff --git a/test/GPS/CMakeLists.txt b/test/GPS/CMakeLists.txt new file mode 100644 index 000000000000..54c2adb86c62 --- /dev/null +++ b/test/GPS/CMakeLists.txt @@ -0,0 +1,17 @@ +find_package(Qt6 REQUIRED COMPONENTS Core Positioning Test) + +qt_add_library(GpsTest + STATIC + GpsTest.cc + GpsTest.h +) + +target_link_libraries(GpsTest + PRIVATE + Qt6::Test + GPS + PUBLIC + qgcunittest +) + +target_include_directories(GpsTest PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/test/GPS/GpsTest.cc b/test/GPS/GpsTest.cc new file mode 100644 index 000000000000..9dfe652b2ae3 --- /dev/null +++ b/test/GPS/GpsTest.cc @@ -0,0 +1,32 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#include "GpsTest.h" +#include "GPSProvider.h" + +#include + +GpsTest::GpsTest() +{ + +} + +void GpsTest::_testGps() +{ + /*const int fakeMsgLengths[3] = {30, 170, 240}; + uint8_t *fakeData = new uint8_t[fakeMsgLengths[2]]; + while (!m_requestStop) { + for (int i = 0; i < 3; ++i) { + _gotRTCMData(fakeData, fakeMsgLengths[i]); + msleep(4); + } + msleep(100); + } + delete[] fakeData;*/ +} diff --git a/test/GPS/GpsTest.h b/test/GPS/GpsTest.h new file mode 100644 index 000000000000..a6d545b0a861 --- /dev/null +++ b/test/GPS/GpsTest.h @@ -0,0 +1,23 @@ +/**************************************************************************** + * + * (c) 2009-2020 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +#pragma once + +#include "UnitTest.h" + +class GpsTest : public UnitTest +{ + Q_OBJECT + +public: + GpsTest(); + +private slots: + void _testGps(); +}; diff --git a/test/UnitTestList.cc b/test/UnitTestList.cc index 5b5be05f9478..330e0248ade5 100644 --- a/test/UnitTestList.cc +++ b/test/UnitTestList.cc @@ -35,6 +35,9 @@ // Geo #include "GeoTest.h" +// GPS +#include "GpsTest.h" + // MAVLink #include "StatusTextHandlerTest.h" #include "SigningTest.h" @@ -124,6 +127,9 @@ int runTests(bool stress, QStringView unitTestOptions) // Geo // UT_REGISTER_TEST(GeoTest) + // GPS + // UT_REGISTER_TEST(GpsTest) + // MAVLink UT_REGISTER_TEST(StatusTextHandlerTest) UT_REGISTER_TEST(SigningTest)