diff --git a/.gitmodules b/.gitmodules index 2e3cf6a5200..e37ef969513 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,5 +1,5 @@ [submodule "src/GPS/Drivers"] - path = src/GPS/Drivers + path = libs/PX4-GPSDrivers url = https://github.com/PX4/GpsDrivers.git [submodule "libs/mavlink/include/mavlink/v2.0"] path = libs/mavlink/include/mavlink/v2.0 diff --git a/src/GPS/Drivers b/libs/PX4-GPSDrivers similarity index 100% rename from src/GPS/Drivers rename to libs/PX4-GPSDrivers diff --git a/src/Comms/LinkManager.cc b/src/Comms/LinkManager.cc index 43df54e8d38..790913f68d1 100644 --- a/src/Comms/LinkManager.cc +++ b/src/Comms/LinkManager.cc @@ -28,6 +28,7 @@ #include "GPSManager.h" #include "PositionManager.h" #include "UdpIODevice.h" +#include "GPSRtk.h" #endif #ifdef QT_DEBUG @@ -852,7 +853,7 @@ void LinkManager::_addSerialAutoConnectLink() 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; @@ -876,7 +877,7 @@ void LinkManager::_addSerialAutoConnectLink() // 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(); } } @@ -900,7 +901,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/GPS/CMakeLists.txt b/src/GPS/CMakeLists.txt index eef62e187ec..3f59ed72855 100644 --- a/src/GPS/CMakeLists.txt +++ b/src/GPS/CMakeLists.txt @@ -42,23 +42,20 @@ qt_add_library(GPSDrivers STATIC ${px4-gpsdrivers_SOURCE_DIR}/src/unicore.h ) -target_link_libraries(GPSDrivers - PUBLIC - Qt6::Core - Utilities -) - -target_include_directories(GPSDrivers PUBLIC ${px4-gpsdrivers_SOURCE_DIR}) +target_link_libraries(GPSDrivers PUBLIC Qt6::Core) target_compile_definitions(GPSDrivers PUBLIC GPS_DEFINITIONS_HEADER=<${CMAKE_CURRENT_SOURCE_DIR}/definitions.h>) +target_include_directories(GPSDrivers PUBLIC ${px4-gpsdrivers_SOURCE_DIR}/src) + target_sources(GPS PRIVATE GPSManager.cc GPSManager.h - GPSPositionMessage.h GPSProvider.cc GPSProvider.h + GPSRtk.cc + GPSRtk.h RTCMMavlink.cc RTCMMavlink.h satellite_info.h @@ -69,14 +66,14 @@ target_sources(GPS target_link_libraries(GPS PRIVATE Comms - GPSDrivers + MAVLink + QGC Settings Utilities Vehicle PUBLIC Qt6::Core - MAVLink - QGC + GPSDrivers ) target_include_directories(GPS PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/GPS/GPSManager.cc b/src/GPS/GPSManager.cc index 486a2d9e073..e3e32a98944 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, _gpsManager); -void GPSManager::_gpsSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active) +GPSManager::GPSManager(QObject *parent) + : QObject(parent) + , _gpsRtk(new GPSRtk(this)) { - _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); + // qCDebug(GPSManagerLog) << Q_FUNC_INFO << this; } -void GPSManager::_gpsNumSatellites(int numSatellites) -{ - _gpsRtkFactGroup->numSatellites()->setRawValue(numSatellites); -} - -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) +GPSManager::~GPSManager() { - 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; + // qCDebug(GPSManagerLog) << Q_FUNC_INFO << this; } -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::instance() { - qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count); - emit satelliteUpdate(msg.satellite_data.count); + return _gpsManager(); } diff --git a/src/GPS/GPSManager.h b/src/GPS/GPSManager.h index 00a1232f944..028fe76613d 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 _gpsRtk; } private: - GPSProvider* _gpsProvider = nullptr; - RTCMMavlink* _rtcmMavlink = nullptr; - GPSRTKFactGroup* _gpsRtkFactGroup = nullptr; - - std::atomic_bool _requestGpsStop; ///< signals the thread to quit + GPSRtk *_gpsRtk = nullptr; }; diff --git a/src/GPS/GPSPositionMessage.h b/src/GPS/GPSPositionMessage.h deleted file mode 100644 index 272fcdbe47d..00000000000 --- 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 01c5f9d8b4e..4948455947a 100644 --- a/src/GPS/GPSProvider.cc +++ b/src/GPS/GPSProvider.cc @@ -7,233 +7,256 @@ * ****************************************************************************/ - #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 "RTCMMavlink.h" + +#include +#include +#include +#include +#include +#include #ifdef Q_OS_ANDROID #include "qserialport.h" #else -#include +#include #endif -#define GPS_RECEIVE_TIMEOUT 1200 +QGC_LOGGING_CATEGORY(GPSProviderLog, "qgc.gps.gpsprovider") +QGC_LOGGING_CATEGORY(GPSDriversLog, "qgc.gps.drivers") -//#define SIMULATE_RTCM_OUTPUT //if defined, generate simulated RTCM messages - //additionally make sure to call connectGPS(""), eg. from QGCToolbox.cc +GPSProvider::GPSProvider(const QString &device, GPSType type, const rtk_data_s &rtkData, const std::atomic_bool &requestStop, QObject *parent) + : QThread(parent) + , _device(device) + , _type(type) + , _requestStop(requestStop) + , _rtkData(rtkData) + , _serial(new QSerialPort(this)) +{ + // qCDebug(GPSProviderLog) << Q_FUNC_INFO << this; + qCDebug(GPSProviderLog) << QString("Survey in accuracy: %1 | duration: %2").arg(_rtkData.surveyInAccMeters).arg(_rtkData.surveyInDurationSecs); -void GPSProvider::run() + _serial->setPortName(_device); +} + +GPSProvider::~GPSProvider() { -#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 */ + // qCDebug(GPSProviderLog) << Q_FUNC_INFO << this; +} + +void GPSProvider::_publishSatelliteInfo() +{ + emit satelliteInfoUpdate(_satelliteInfo); +} - if (_serial) delete _serial; +void GPSProvider::_publishSensorGNSSRelative() +{ + emit sensorGnssRelativeUpdate(_sensorGnssRelative); +} - _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; +void GPSProvider::_publishSensorGPS() +{ + emit sensorGpsUpdate(_sensorGps); +} + +void GPSProvider::_gotRTCMData(const uint8_t *data, size_t len) +{ + const QByteArray message(reinterpret_cast(data), len); + emit RTCMDataUpdate(message); +} + +int GPSProvider::_callbackEntry(GPSCallbackType type, void *data1, int data2, void *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) { + const int timeout = *(reinterpret_cast(data1)); + if (!_serial->waitForReadyRead(timeout)) { + return 0; } } - if (_serial->error() != QSerialPort::NoError) { - qWarning() << "GPS: Failed to open Serial Device" << _device << _serial->errorString(); - return; + return _serial->read(reinterpret_cast(data1), data2); + case GPSCallbackType::writeDeviceData: + if (_serial->write(reinterpret_cast(data1), data2) >= 0) { + if (_serial->waitForBytesWritten(-1)) { + return data2; + } } + return -1; + case GPSCallbackType::setBaudrate: + return (_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: + // _sensorGnssRelative + default: + break; } - _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; + return 0; +} - while (!_requestStop) { +void GPSProvider::run() +{ +#ifdef SIMULATE_RTCM_OUTPUT + _sendRTCMData(); +#endif + + _connectSerial(); + + GPSBaseStationSupport *gpsDriver = nullptr; + while (!_requestStop) { 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); - } - - _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; + gpsDriver = _connectGPS(); + if (gpsDriver) { + (void) memset(&_sensorGps, 0, sizeof(_sensorGps)); - while (!_requestStop && numTries < 3) { - int helperRet = gpsDriver->receive(GPS_RECEIVE_TIMEOUT); + uint8_t numTries = 0; + while (!_requestStop && (numTries < 3)) { + const int helperRet = gpsDriver->receive(kGPSReceiveTimeout); if (helperRet > 0) { numTries = 0; - if (helperRet & 1) { - publishGPSPosition(); + if (helperRet & GPSReceiveType::Position) { + _publishSensorGPS(); numTries = 0; } - if (_pReportSatInfo && (helperRet & 2)) { - publishGPSSatellite(); + if (helperRet & GPSReceiveType::Satellite) { + _publishSatelliteInfo(); numTries = 0; } } else { ++numTries; } } - if (_serial->error() != QSerialPort::NoError && _serial->error() != QSerialPort::TimeoutError) { + + if ((_serial->error() != QSerialPort::NoError) && (_serial->error() != QSerialPort::TimeoutError)) { break; } } } - qCDebug(RTKGPSLog) << "Exiting GPS thread"; -} -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) -{ - qCDebug(RTKGPSLog) << "Survey in accuracy:duration" << surveyInAccMeters << surveryInDurationSecs; - if (enableSatInfo) _pReportSatInfo = new satellite_info_s(); -} + delete gpsDriver; + gpsDriver = nullptr; -GPSProvider::~GPSProvider() -{ - if (_pReportSatInfo) delete(_pReportSatInfo); - if (_serial) delete _serial; + qCDebug(GPSProviderLog) << Q_FUNC_INFO << "Exiting GPS thread"; } -void GPSProvider::publishGPSPosition() +bool GPSProvider::_connectSerial() { - GPSPositionMessage msg; - msg.position_data = _reportGpsPos; - emit positionUpdate(msg); -} + if (!_serial->open(QIODevice::ReadWrite)) { + // 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. + uint32_t retries = 60; + while ((retries-- > 0) && (_serial->error() == QSerialPort::PermissionError)) { + qCDebug(GPSProviderLog) << "Cannot open device... retrying"; + msleep(500); + if (_serial->open(QIODevice::ReadWrite)) { + _serial->clearError(); + break; + } + } -void GPSProvider::publishGPSSatellite() -{ - GPSSatelliteMessage msg; - msg.satellite_data = *_pReportSatInfo; - emit satelliteInfoUpdate(msg); -} + if (_serial->error() != QSerialPort::NoError) { + qCWarning(GPSProviderLog) << "GPS: Failed to open Serial Device" << _device << _serial->errorString(); + return false; + } + } -void GPSProvider::gotRTCMData(uint8_t* data, size_t len) -{ - QByteArray message((char*)data, static_cast(len)); - emit RTCMDataUpdate(message); -} + (void) _serial->setBaudRate(QSerialPort::Baud9600); + (void) _serial->setDataBits(QSerialPort::Data8); + (void) _serial->setParity(QSerialPort::NoParity); + (void) _serial->setStopBits(QSerialPort::OneStop); + (void) _serial->setFlowControl(QSerialPort::NoFlowControl); -int GPSProvider::callbackEntry(GPSCallbackType type, void *data1, int data2, void *user) -{ - GPSProvider *gps = (GPSProvider *)user; - return gps->callback(type, data1, data2); + return true; } -int GPSProvider::callback(GPSCallbackType type, void *data1, int data2) +GPSBaseStationSupport *GPSProvider::_connectGPS() { - switch (type) { - case GPSCallbackType::readDeviceData: { - if (_serial->bytesAvailable() == 0) { - int timeout = *((int *) data1); - if (!_serial->waitForReadyRead(timeout)) - return 0; //timeout - } - return (int)_serial->read((char*) data1, data2); - } - case GPSCallbackType::writeDeviceData: - if (_serial->write((char*) data1, data2) >= 0) { - if (_serial->waitForBytesWritten(-1)) - return data2; - } - return -1; + GPSBaseStationSupport *gpsDriver = nullptr; + uint32_t baudrate = 0; + switch(_type) { + case GPSType::trimble: + gpsDriver = new GPSDriverAshtech(&_callbackEntry, this, &_sensorGps, &_satelliteInfo); + baudrate = 115200; + break; + case GPSType::septentrio: + gpsDriver = new GPSDriverSBF(&_callbackEntry, this, &_sensorGps, &_satelliteInfo, kGPSHeadingOffset); + baudrate = 0; + break; + case GPSType::u_blox: + gpsDriver = new GPSDriverUBX(GPSDriverUBX::Interface::UART, &_callbackEntry, this, &_sensorGps, &_satelliteInfo); + baudrate = 0; + break; + case GPSType::femto: + gpsDriver = new GPSDriverFemto(&_callbackEntry, this, &_sensorGps, &_satelliteInfo); + baudrate = 0; + break; + default: + // GPSDriverEmlidReach, GPSDriverMTK, GPSDriverNMEA + qCWarning(GPSProviderLog) << "Unsupported GPS Type:" << static_cast(_type); + return nullptr; + } + + gpsDriver->setSurveyInSpecs(_rtkData.surveyInAccMeters * 10000.f, _rtkData.surveyInDurationSecs); - case GPSCallbackType::setBaudrate: - return _serial->setBaudRate(data2) ? 0 : -1; + if (_rtkData.useFixedBaseLoction) { + gpsDriver->setBasePosition(_rtkData.fixedBaseLatitude, _rtkData.fixedBaseLongitude, _rtkData.fixedBaseAltitudeMeters, _rtkData.fixedBaseAccuracyMeters * 1000.0f); + } - case GPSCallbackType::gotRTCMMessage: - gotRTCMData((uint8_t*) data1, data2); - break; + _gpsConfig.output_mode = GPSHelper::OutputMode::RTCM; - case GPSCallbackType::surveyInStatus: - { - SurveyInStatus* status = (SurveyInStatus*)data1; - qCDebug(RTKGPSLog) << "Position: " << status->latitude << status->longitude << status->altitude; + if (gpsDriver->configure(baudrate, _gpsConfig) != 0) { + return nullptr; + } - 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)); - } - break; + return gpsDriver; +} - case GPSCallbackType::setClock: - /* do nothing */ - break; +void GPSProvider::_sendRTCMData() +{ + RTCMMavlink *const rtcm = new RTCMMavlink(this); - case GPSCallbackType::gotRelativePositionMessage: - /* do nothing */ - break; + const int fakeMsgLengths[3] = { 30, 170, 240 }; + const uint8_t* const fakeData = new uint8_t[fakeMsgLengths[2]]; + while (!_requestStop) { + for (int i = 0; i < 3; ++i) { + const QByteArray message(reinterpret_cast(fakeData), fakeMsgLengths[i]); + rtcm->RTCMDataUpdate(message); + msleep(4); + } + msleep(100); } - - return 0; + delete[] fakeData; } diff --git a/src/GPS/GPSProvider.h b/src/GPS/GPSProvider.h index 1098b12aa27..233c205a50d 100644 --- a/src/GPS/GPSProvider.h +++ b/src/GPS/GPSProvider.h @@ -7,87 +7,91 @@ * ****************************************************************************/ - #pragma once - -#include "GPSPositionMessage.h" -#include "Drivers/src/gps_helper.h" - +#include +#include +#include +#include #include #include -#include -class QSerialPort; +#include + +#include "satellite_info.h" +#include "sensor_gnss_relative.h" +#include "sensor_gps.h" +Q_DECLARE_LOGGING_CATEGORY(GPSProviderLog) + +class QSerialPort; +class GPSBaseStationSupport; -/** - ** class GPSProvider - * opens a GPS device and handles the protocol - */ class GPSProvider : public QThread { 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); + struct rtk_data_s { + double surveyInAccMeters = 0; + int surveyInDurationSecs = 0; + bool useFixedBaseLoction = false; + double fixedBaseLatitude = 0.; + double fixedBaseLongitude = 0.; + float fixedBaseAltitudeMeters = 0.f; + float fixedBaseAccuracyMeters = 0.f; + }; + + GPSProvider(const QString &device, GPSType type, const rtk_data_s &rtkData, 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(); + void run() final; - /** - * callback from the driver for the platform specific stuff - */ - static int callbackEntry(GPSCallbackType type, void *data1, int data2, void *user); + bool _connectSerial(); + GPSBaseStationSupport *_connectGPS(); + void _publishSensorGPS(); + void _publishSatelliteInfo(); + void _publishSensorGNSSRelative(); - int callback(GPSCallbackType type, void *data1, int data2); + void _gotRTCMData(const uint8_t *data, size_t len); + void _sendRTCMData(); + + static int _callbackEntry(GPSCallbackType type, void *data1, int data2, void *user); QString _device; GPSType _type; - const std::atomic_bool& _requestStop; - double _surveyInAccMeters; - int _surveryInDurationSecs; - bool _useFixedBaseLoction; - double _fixedBaseLatitude; - double _fixedBaseLongitude; - float _fixedBaseAltitudeMeters; - float _fixedBaseAccuracyMeters; + const std::atomic_bool &_requestStop; + rtk_data_s _rtkData{}; GPSHelper::GPSConfig _gpsConfig{}; - struct sensor_gps_s _reportGpsPos; - struct satellite_info_s *_pReportSatInfo = nullptr; + struct satellite_info_s _satelliteInfo{}; + struct sensor_gnss_relative_s _sensorGnssRelative{}; + struct sensor_gps_s _sensorGps{}; + + QSerialPort *_serial = nullptr; + + enum GPSReceiveType { + Position = 1, + Satellite = 2 + }; - QSerialPort *_serial = nullptr; + static constexpr uint32_t kGPSReceiveTimeout = 1200; + static constexpr float kGPSHeadingOffset = 5.f; }; diff --git a/src/GPS/GPSRtk.cc b/src/GPS/GPSRtk.cc new file mode 100644 index 00000000000..153c4d8cd65 --- /dev/null +++ b/src/GPS/GPSRtk.cc @@ -0,0 +1,152 @@ +/**************************************************************************** + * + * (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 "GPSRTKFactGroup.h" +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" +#include "RTCMMavlink.h" +#include "RTKSettings.h" +#include "SettingsManager.h" + +QGC_LOGGING_CATEGORY(GPSRtkLog, "qgc.gps.gpsrtk") + +GPSRtk::GPSRtk(QObject *parent) + : QObject(parent) + , _gpsRtkFactGroup(new GPSRTKFactGroup(this)) +{ + // 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() +{ + _gpsRtkFactGroup->connected()->setRawValue(true); +} + +void GPSRtk::_onGPSDisconnect() +{ + _gpsRtkFactGroup->connected()->setRawValue(false); +} + +void GPSRtk::_onGPSSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active) +{ + _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); +} + +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) << "Connecting Trimble device"; + } else if (gps_type.contains(QStringLiteral("septentrio"), Qt::CaseInsensitive)) { + type = GPSProvider::GPSType::septentrio; + qCDebug(GPSRtkLog) << "Connecting Septentrio device"; + } else if (gps_type.contains(QStringLiteral("femtomes"), Qt::CaseInsensitive)) { + type = GPSProvider::GPSType::femto; + qCDebug(GPSRtkLog) << "Connecting Femtomes device"; + } else { + type = GPSProvider::GPSType::u_blox; + qCDebug(GPSRtkLog) << "Connecting U-blox device"; + } + + disconnectGPS(); + + RTKSettings* const rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings(); + _requestGpsStop = false; + const GPSProvider::rtk_data_s rtkData = { + 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() + }; + _gpsProvider = new GPSProvider( + device, + type, + rtkData, + _requestGpsStop, + this + ); + (void) QMetaObject::invokeMethod(_gpsProvider, "start", Qt::AutoConnection); + + _rtcmMavlink = new RTCMMavlink(this); + (void) connect(_gpsProvider, &GPSProvider::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate); + + (void) connect(_gpsProvider, &GPSProvider::satelliteInfoUpdate, this, &GPSRtk::_satelliteInfoUpdate); + (void) connect(_gpsProvider, &GPSProvider::sensorGpsUpdate, this, &GPSRtk::_sensorGpsUpdate); + (void) connect(_gpsProvider, &GPSProvider::surveyInStatus, this, &GPSRtk::_onGPSSurveyInStatus); + (void) connect(_gpsProvider, &GPSProvider::finished, this, &GPSRtk::_onGPSDisconnect); + + (void) QMetaObject::invokeMethod(this, "_onGPSConnect", Qt::AutoConnection); +} + +void GPSRtk::disconnectGPS() +{ + if (_gpsProvider) { + _requestGpsStop = true; + if (!_gpsProvider->wait(kGPSThreadDisconnectTimeout)) { + qCWarning(GPSRtkLog) << "Failed to wait for GPS thread exit. Consider increasing the timeout"; + } + + _gpsProvider->deleteLater(); + _gpsProvider = nullptr; + } + + if (_rtcmMavlink) { + _rtcmMavlink->deleteLater(); + _rtcmMavlink = nullptr; + } +} + +bool GPSRtk::connected() const +{ + return (_gpsProvider ? _gpsProvider->isRunning() : false); +} + +FactGroup *GPSRtk::gpsRtkFactGroup() +{ + return _gpsRtkFactGroup; +} + +void GPSRtk::_satelliteInfoUpdate(const satellite_info_s &msg) +{ + qCDebug(GPSRtkLog) << Q_FUNC_INFO << QStringLiteral("%1 satellites").arg(msg.count); + _gpsRtkFactGroup->numSatellites()->setRawValue(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 00000000000..68d9fae14d2 --- /dev/null +++ b/src/GPS/GPSRtk.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * (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 "satellite_info.h" +#include "sensor_gnss_relative.h" +#include "sensor_gps.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 *_gpsProvider = nullptr; + RTCMMavlink *_rtcmMavlink = nullptr; + GPSRTKFactGroup *_gpsRtkFactGroup = nullptr; + + std::atomic_bool _requestGpsStop = false; + + static constexpr uint32_t kGPSThreadDisconnectTimeout = 2000; +}; diff --git a/src/GPS/RTCMMavlink.cc b/src/GPS/RTCMMavlink.cc index 53272c8b836..b9ae04dd54c 100644 --- a/src/GPS/RTCMMavlink.cc +++ b/src/GPS/RTCMMavlink.cc @@ -7,74 +7,96 @@ * ****************************************************************************/ - #include "RTCMMavlink.h" +#include "MAVLinkProtocol.h" #include "MultiVehicleManager.h" +#include "QGCApplication.h" +#include "QGCLoggingCategory.h" #include "Vehicle.h" -#include "MAVLinkProtocol.h" -RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) - : _toolbox(toolbox) +QGC_LOGGING_CATEGORY(RTCMMavlinkLog, "qgc.gps.rtcmmavlink") + +RTCMMavlink::RTCMMavlink(QObject *parent) + : QObject(parent) { + // qCDebug(RTCMMavlinkLog) << Q_FUNC_INFO << this; + _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) +{ +#ifdef QT_DEBUG + _calculateBandwith(data.size()); +#endif - 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{}; - 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 = (_sequenceId & 0x1FU) << 3; + (void) memcpy(&gpsRtcmData.data, data.data(), data.size()); + _sendMessageToVehicle(gpsRtcmData); } else { - // We need to fragment - - 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); + 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 |= (_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); + start += length; } } + ++_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]); - SharedLinkInterfacePtr sharedLink = vehicle->vehicleLinkManager()->primaryLink().lock(); - + QmlObjectListModel* const vehicles = qgcApp()->toolbox()->multiVehicleManager()->vehicles(); + for (qsizetype i = 0; i < vehicles->count(); i++) { + Vehicle* const vehicle = qobject_cast(vehicles->get(i)); + const SharedLinkInterfacePtr sharedLink = vehicle->vehicleLinkManager()->primaryLink().lock(); if (sharedLink) { - mavlink_message_t message; - - 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(); + 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); } } } + +void RTCMMavlink::_calculateBandwith(qsizetype bytes) +{ + if (!_bandwidthTimer.isValid()) { + return; + } + + _bandwidthByteCounter += bytes; + + const qint64 elapsed = _bandwidthTimer.elapsed(); + if (elapsed > 1000) { + qCDebug(RTCMMavlinkLog) << QStringLiteral("RTCM bandwidth: %1 kB/s").arg(((_bandwidthByteCounter / elapsed) * 1000.f) / 1024.f); + (void) _bandwidthTimer.restart(); + _bandwidthByteCounter = 0; + } +} diff --git a/src/GPS/RTCMMavlink.h b/src/GPS/RTCMMavlink.h index c527b8eaf0d..31498a98c1c 100644 --- a/src/GPS/RTCMMavlink.h +++ b/src/GPS/RTCMMavlink.h @@ -10,31 +10,30 @@ #pragma once -#include "QGCToolbox.h" -#include "MAVLinkLib.h" - -#include #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); + void _calculateBandwith(qsizetype bytes); + static void _sendMessageToVehicle(const mavlink_gps_rtcm_data_t &data); - QGCToolbox& _toolbox; - QElapsedTimer _bandwidthTimer; - int _bandwidthByteCounter = 0; uint8_t _sequenceId = 0; + qsizetype _bandwidthByteCounter = 0; + QElapsedTimer _bandwidthTimer; }; diff --git a/src/GPS/definitions.h b/src/GPS/definitions.h index 6a4c287bff5..366e95f7f96 100644 --- a/src/GPS/definitions.h +++ b/src/GPS/definitions.h @@ -49,51 +49,29 @@ #pragma once #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 +#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 - -#define M_PI_2_F M_PI +Q_DECLARE_LOGGING_CATEGORY(GPSDriversLog) -#include - -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); -} +#define GPS_INFO(...) qCInfo(GPSDriversLog, __VA_ARGS__) +#define GPS_WARN(...) qCWarning(GPSDriversLog, __VA_ARGS__) +#define GPS_ERR(...) qCCritical(GPSDriversLog, __VA_ARGS__) -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.0174532925f +#define M_RAD_TO_DEG_F 57.2957795f -#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 0.63661977f -//timespec is UNIX-specific #ifdef _WIN32 -#if _MSC_VER < 1900 +#if (_MSC_VER < 1900) struct timespec { time_t tv_sec; @@ -104,3 +82,13 @@ struct timespec #endif #endif +static inline void gps_usleep(unsigned long usecs) +{ + QThread::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 33974c31467..8a03709a27c 100644 --- a/src/GPS/satellite_info.h +++ b/src/GPS/satellite_info.h @@ -40,16 +40,19 @@ * ****************************************************************************/ +/* 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 -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]; @@ -57,8 +60,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 e1a39706f35..2de28ba3aa0 100644 --- a/src/GPS/sensor_gnss_relative.h +++ b/src/GPS/sensor_gnss_relative.h @@ -40,29 +40,34 @@ * ****************************************************************************/ -/* 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 -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; @@ -74,3 +79,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 2be1599bb53..6f0a68394c3 100644 --- a/src/GPS/sensor_gps.h +++ b/src/GPS/sensor_gps.h @@ -40,65 +40,75 @@ * ****************************************************************************/ -/* 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 -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 05989093171..1614d88c802 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 "LinkManager.h" #include "MAVLinkProtocol.h" #include "MissionCommandTree.h" @@ -43,9 +40,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 _linkManager = new LinkManager (app, this); _mavlinkProtocol = new MAVLinkProtocol (app, this); _missionCommandTree = new MissionCommandTree (app, this); @@ -69,9 +63,6 @@ void QGCToolbox::setChildToolboxes(void) _corePlugin->setToolbox(this); _firmwarePluginManager->setToolbox(this); -#ifndef NO_SERIAL_LINK - _gpsManager->setToolbox(this); -#endif _linkManager->setToolbox(this); _mavlinkProtocol->setToolbox(this); _missionCommandTree->setToolbox(this); diff --git a/src/QGCToolbox.h b/src/QGCToolbox.h index ba93df9c5d8..acef3379d6a 100644 --- a/src/QGCToolbox.h +++ b/src/QGCToolbox.h @@ -14,7 +14,6 @@ #include class FirmwarePluginManager; -class GPSManager; class LinkManager; class MAVLinkProtocol; class MissionCommandTree; @@ -49,9 +48,6 @@ class QGCToolbox : public QObject { MAVLinkLogManager* mavlinkLogManager () { return _mavlinkLogManager; } QGCCorePlugin* corePlugin () { return _corePlugin; } SettingsManager* settingsManager () { return _settingsManager; } -#ifndef NO_SERIAL_LINK - GPSManager* gpsManager () { return _gpsManager; } -#endif #ifndef QGC_AIRLINK_DISABLED AirLinkManager* airlinkManager () { return _airlinkManager; } #endif @@ -64,9 +60,6 @@ class QGCToolbox : public QObject { void _scanAndLoadPlugins(QGCApplication *app); FirmwarePluginManager* _firmwarePluginManager = nullptr; -#ifndef NO_SERIAL_LINK - GPSManager* _gpsManager = nullptr; -#endif LinkManager* _linkManager = nullptr; MAVLinkProtocol* _mavlinkProtocol = nullptr; MissionCommandTree* _missionCommandTree = nullptr; diff --git a/src/QmlControls/QGroundControlQmlGlobal.cc b/src/QmlControls/QGroundControlQmlGlobal.cc index ffd7923a57b..89d4238f310 100644 --- a/src/QmlControls/QGroundControlQmlGlobal.cc +++ b/src/QmlControls/QGroundControlQmlGlobal.cc @@ -19,6 +19,7 @@ #include "ADSBVehicleManager.h" #ifndef NO_SERIAL_LINK #include "GPSManager.h" +#include "GPSRtk.h" #endif #include "QGCPalette.h" #ifdef QT_DEBUG @@ -85,7 +86,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 _globalPalette = new QGCPalette(this); #ifndef QGC_AIRLINK_DISABLED diff --git a/src/Utilities/QGCLoggingCategory.cc b/src/Utilities/QGCLoggingCategory.cc index fc144b505e2..ec3580dc76e 100644 --- a/src/Utilities/QGCLoggingCategory.cc +++ b/src/Utilities/QGCLoggingCategory.cc @@ -23,7 +23,6 @@ QGC_LOGGING_CATEGORY(FirmwareUpgradeVerboseLog, "FirmwareUpgradeVerboseLog") QGC_LOGGING_CATEGORY(MissionCommandsLog, "MissionCommandsLog") QGC_LOGGING_CATEGORY(MissionItemLog, "MissionItemLog") QGC_LOGGING_CATEGORY(ParameterManagerLog, "ParameterManagerLog") -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 0c74f5d47cf..bd22a3f3426 100644 --- a/src/Utilities/QGCLoggingCategory.h +++ b/src/Utilities/QGCLoggingCategory.h @@ -19,7 +19,6 @@ Q_DECLARE_LOGGING_CATEGORY(FirmwareUpgradeVerboseLog) Q_DECLARE_LOGGING_CATEGORY(MissionCommandsLog) Q_DECLARE_LOGGING_CATEGORY(MissionItemLog) Q_DECLARE_LOGGING_CATEGORY(ParameterManagerLog) -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 ef75ce1f526..92e1eeb5878 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -55,6 +55,9 @@ add_qgc_test(FollowMeTest) 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) @@ -133,6 +136,7 @@ target_link_libraries(qgctest FactSystemTest FollowMeTest GeoTest + GpsTest MAVLinkTest MissionManagerTest QmlControlsTest diff --git a/test/GPS/CMakeLists.txt b/test/GPS/CMakeLists.txt new file mode 100644 index 00000000000..54c2adb86c6 --- /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 00000000000..a6a3fe5ae17 --- /dev/null +++ b/test/GPS/GpsTest.cc @@ -0,0 +1,19 @@ +/**************************************************************************** + * + * (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 "RTCMMavlink.h" + +#include + +void GpsTest::_testGpsRTCM() +{ + +} diff --git a/test/GPS/GpsTest.h b/test/GPS/GpsTest.h new file mode 100644 index 00000000000..3da3e44ebd0 --- /dev/null +++ b/test/GPS/GpsTest.h @@ -0,0 +1,20 @@ +/**************************************************************************** + * + * (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 + +private slots: + void _testGpsRTCM(); +}; diff --git a/test/UnitTestList.cc b/test/UnitTestList.cc index 7a408f67211..94dab492a8f 100644 --- a/test/UnitTestList.cc +++ b/test/UnitTestList.cc @@ -43,6 +43,9 @@ // Geo #include "GeoTest.h" +// GPS +#include "GpsTest.h" + // MAVLink #include "StatusTextHandlerTest.h" #include "SigningTest.h" @@ -140,6 +143,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)