GPS: Prepare for new gps sources & threading changes
This commit is contained in:
Holden
+1
-1
@@ -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
|
||||
|
||||
@@ -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;
|
||||
|
||||
+8
-11
@@ -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})
|
||||
|
||||
+13
-126
@@ -7,142 +7,29 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#include "GPSManager.h"
|
||||
#include "GPSProvider.h"
|
||||
#include "QGCApplication.h"
|
||||
#include "SettingsManager.h"
|
||||
#include "RTKSettings.h"
|
||||
#include "GPSRTKFactGroup.h"
|
||||
#include "RTCMMavlink.h"
|
||||
#include "GPSRtk.h"
|
||||
#include "QGCLoggingCategory.h"
|
||||
|
||||
#include <QtCore/qapplicationstatic.h>
|
||||
|
||||
GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox)
|
||||
: QGCTool(app, toolbox)
|
||||
QGC_LOGGING_CATEGORY(GPSManagerLog, "qgc.gps.gpsmanager")
|
||||
|
||||
Q_APPLICATION_STATIC(GPSManager, _gpsManager);
|
||||
|
||||
GPSManager::GPSManager(QObject *parent)
|
||||
: QObject(parent)
|
||||
, _gpsRtk(new GPSRtk(this))
|
||||
{
|
||||
qRegisterMetaType<GPSPositionMessage>();
|
||||
qRegisterMetaType<GPSSatelliteMessage>();
|
||||
// qCDebug(GPSManagerLog) << Q_FUNC_INFO << this;
|
||||
}
|
||||
|
||||
GPSManager::~GPSManager()
|
||||
{
|
||||
disconnectGPS();
|
||||
|
||||
delete _gpsRtkFactGroup;
|
||||
_gpsRtkFactGroup = nullptr;
|
||||
// qCDebug(GPSManagerLog) << Q_FUNC_INFO << this;
|
||||
}
|
||||
|
||||
void GPSManager::setToolbox(QGCToolbox *toolbox)
|
||||
GPSManager *GPSManager::instance()
|
||||
{
|
||||
QGCTool::setToolbox(toolbox);
|
||||
|
||||
_gpsRtkFactGroup = new GPSRTKFactGroup(this);
|
||||
|
||||
connect(this, &GPSManager::onConnect, this, &GPSManager::_onGPSConnect);
|
||||
connect(this, &GPSManager::onDisconnect, this, &GPSManager::_onGPSDisconnect);
|
||||
connect(this, &GPSManager::surveyInStatus, this, &GPSManager::_gpsSurveyInStatus);
|
||||
connect(this, &GPSManager::satelliteUpdate, this, &GPSManager::_gpsNumSatellites);
|
||||
}
|
||||
|
||||
void GPSManager::_onGPSConnect()
|
||||
{
|
||||
_gpsRtkFactGroup->connected()->setRawValue(true);
|
||||
}
|
||||
|
||||
void GPSManager::_onGPSDisconnect()
|
||||
{
|
||||
_gpsRtkFactGroup->connected()->setRawValue(false);
|
||||
}
|
||||
|
||||
void GPSManager::_gpsSurveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active)
|
||||
{
|
||||
_gpsRtkFactGroup->currentDuration()->setRawValue(duration);
|
||||
_gpsRtkFactGroup->currentAccuracy()->setRawValue(static_cast<double>(accuracyMM) / 1000.0);
|
||||
_gpsRtkFactGroup->currentLatitude()->setRawValue(latitude);
|
||||
_gpsRtkFactGroup->currentLongitude()->setRawValue(longitude);
|
||||
_gpsRtkFactGroup->currentAltitude()->setRawValue(altitude);
|
||||
_gpsRtkFactGroup->valid()->setRawValue(valid);
|
||||
_gpsRtkFactGroup->active()->setRawValue(active);
|
||||
}
|
||||
|
||||
void GPSManager::_gpsNumSatellites(int numSatellites)
|
||||
{
|
||||
_gpsRtkFactGroup->numSatellites()->setRawValue(numSatellites);
|
||||
}
|
||||
|
||||
void GPSManager::connectGPS(const QString& device, const QString& gps_type)
|
||||
{
|
||||
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)
|
||||
{
|
||||
qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count);
|
||||
emit satelliteUpdate(msg.satellite_data.count);
|
||||
return _gpsManager();
|
||||
}
|
||||
|
||||
+10
-39
@@ -7,56 +7,27 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "QGCToolbox.h"
|
||||
#include "GPSPositionMessage.h"
|
||||
|
||||
#include <QtCore/QString>
|
||||
#include <QtCore/QLoggingCategory>
|
||||
#include <QtCore/QObject>
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
@@ -1,32 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2024 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* 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 <QtCore/QMetaType>
|
||||
|
||||
/**
|
||||
** 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);
|
||||
+207
-184
@@ -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 <ashtech.h>
|
||||
#include <base_station.h>
|
||||
#include <definitions.h>
|
||||
#include <femtomes.h>
|
||||
#include <sbf.h>
|
||||
#include <ubx.h>
|
||||
|
||||
#ifdef Q_OS_ANDROID
|
||||
#include "qserialport.h"
|
||||
#else
|
||||
#include <QSerialPort>
|
||||
#include <QtSerialPort/QSerialPort>
|
||||
#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
|
||||
|
||||
|
||||
void GPSProvider::run()
|
||||
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))
|
||||
{
|
||||
#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;
|
||||
|
||||
if (_serial) delete _serial;
|
||||
qCDebug(GPSProviderLog) << QString("Survey in accuracy: %1 | duration: %2").arg(_rtkData.surveyInAccMeters).arg(_rtkData.surveyInDurationSecs);
|
||||
|
||||
_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) {
|
||||
|
||||
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;
|
||||
|
||||
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";
|
||||
}
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
GPSProvider::~GPSProvider()
|
||||
{
|
||||
if (_pReportSatInfo) delete(_pReportSatInfo);
|
||||
if (_serial) delete _serial;
|
||||
// qCDebug(GPSProviderLog) << Q_FUNC_INFO << this;
|
||||
}
|
||||
|
||||
void GPSProvider::publishGPSPosition()
|
||||
void GPSProvider::_publishSatelliteInfo()
|
||||
{
|
||||
GPSPositionMessage msg;
|
||||
msg.position_data = _reportGpsPos;
|
||||
emit positionUpdate(msg);
|
||||
emit satelliteInfoUpdate(_satelliteInfo);
|
||||
}
|
||||
|
||||
void GPSProvider::publishGPSSatellite()
|
||||
void GPSProvider::_publishSensorGNSSRelative()
|
||||
{
|
||||
GPSSatelliteMessage msg;
|
||||
msg.satellite_data = *_pReportSatInfo;
|
||||
emit satelliteInfoUpdate(msg);
|
||||
emit sensorGnssRelativeUpdate(_sensorGnssRelative);
|
||||
}
|
||||
|
||||
void GPSProvider::gotRTCMData(uint8_t* data, size_t len)
|
||||
void GPSProvider::_publishSensorGPS()
|
||||
{
|
||||
QByteArray message((char*)data, static_cast<int>(len));
|
||||
emit sensorGpsUpdate(_sensorGps);
|
||||
}
|
||||
|
||||
void GPSProvider::_gotRTCMData(const uint8_t *data, size_t len)
|
||||
{
|
||||
const QByteArray message(reinterpret_cast<const char*>(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<GPSProvider*>(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 (_serial->bytesAvailable() == 0) {
|
||||
const int timeout = *(reinterpret_cast<int*>(data1));
|
||||
if (!_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 _serial->read(reinterpret_cast<char*>(data1), data2);
|
||||
case GPSCallbackType::writeDeviceData:
|
||||
if (_serial->write(reinterpret_cast<char*>(data1), data2) >= 0) {
|
||||
if (_serial->waitForBytesWritten(-1)) {
|
||||
return data2;
|
||||
}
|
||||
return -1;
|
||||
|
||||
case GPSCallbackType::setBaudrate:
|
||||
return _serial->setBaudRate(data2) ? 0 : -1;
|
||||
|
||||
case GPSCallbackType::gotRTCMMessage:
|
||||
gotRTCMData((uint8_t*) data1, data2);
|
||||
break;
|
||||
|
||||
case GPSCallbackType::surveyInStatus:
|
||||
{
|
||||
SurveyInStatus* status = (SurveyInStatus*)data1;
|
||||
qCDebug(RTKGPSLog) << "Position: " << status->latitude << status->longitude << status->altitude;
|
||||
|
||||
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 -1;
|
||||
case GPSCallbackType::setBaudrate:
|
||||
return (_serial->setBaudRate(data2) ? 0 : -1);
|
||||
case GPSCallbackType::gotRTCMMessage:
|
||||
_gotRTCMData(reinterpret_cast<uint8_t*>(data1), data2);
|
||||
break;
|
||||
case GPSCallbackType::surveyInStatus:
|
||||
{
|
||||
const SurveyInStatus* const status = reinterpret_cast<SurveyInStatus*>(data1);
|
||||
qCDebug(GPSProviderLog) << "Position:" << status->latitude << status->longitude << status->altitude;
|
||||
|
||||
case GPSCallbackType::setClock:
|
||||
/* do nothing */
|
||||
break;
|
||||
const bool valid = status->flags & 1;
|
||||
const bool active = (status->flags >> 1) & 1;
|
||||
|
||||
case GPSCallbackType::gotRelativePositionMessage:
|
||||
/* do nothing */
|
||||
break;
|
||||
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;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void GPSProvider::run()
|
||||
{
|
||||
#ifdef SIMULATE_RTCM_OUTPUT
|
||||
_sendRTCMData();
|
||||
#endif
|
||||
|
||||
_connectSerial();
|
||||
|
||||
GPSBaseStationSupport *gpsDriver = nullptr;
|
||||
|
||||
while (!_requestStop) {
|
||||
if (gpsDriver) {
|
||||
delete gpsDriver;
|
||||
gpsDriver = nullptr;
|
||||
}
|
||||
|
||||
gpsDriver = _connectGPS();
|
||||
if (gpsDriver) {
|
||||
(void) memset(&_sensorGps, 0, sizeof(_sensorGps));
|
||||
|
||||
uint8_t numTries = 0;
|
||||
while (!_requestStop && (numTries < 3)) {
|
||||
const int helperRet = gpsDriver->receive(kGPSReceiveTimeout);
|
||||
|
||||
if (helperRet > 0) {
|
||||
numTries = 0;
|
||||
|
||||
if (helperRet & GPSReceiveType::Position) {
|
||||
_publishSensorGPS();
|
||||
numTries = 0;
|
||||
}
|
||||
|
||||
if (helperRet & GPSReceiveType::Satellite) {
|
||||
_publishSatelliteInfo();
|
||||
numTries = 0;
|
||||
}
|
||||
} else {
|
||||
++numTries;
|
||||
}
|
||||
}
|
||||
|
||||
if ((_serial->error() != QSerialPort::NoError) && (_serial->error() != QSerialPort::TimeoutError)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
delete gpsDriver;
|
||||
gpsDriver = nullptr;
|
||||
|
||||
qCDebug(GPSProviderLog) << Q_FUNC_INFO << "Exiting GPS thread";
|
||||
}
|
||||
|
||||
bool GPSProvider::_connectSerial()
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
if (_serial->error() != QSerialPort::NoError) {
|
||||
qCWarning(GPSProviderLog) << "GPS: Failed to open Serial Device" << _device << _serial->errorString();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
(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);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
GPSBaseStationSupport *GPSProvider::_connectGPS()
|
||||
{
|
||||
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<int>(_type);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
gpsDriver->setSurveyInSpecs(_rtkData.surveyInAccMeters * 10000.f, _rtkData.surveyInDurationSecs);
|
||||
|
||||
if (_rtkData.useFixedBaseLoction) {
|
||||
gpsDriver->setBasePosition(_rtkData.fixedBaseLatitude, _rtkData.fixedBaseLongitude, _rtkData.fixedBaseAltitudeMeters, _rtkData.fixedBaseAccuracyMeters * 1000.0f);
|
||||
}
|
||||
|
||||
_gpsConfig.output_mode = GPSHelper::OutputMode::RTCM;
|
||||
|
||||
if (gpsDriver->configure(baudrate, _gpsConfig) != 0) {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return gpsDriver;
|
||||
}
|
||||
|
||||
void GPSProvider::_sendRTCMData()
|
||||
{
|
||||
RTCMMavlink *const rtcm = new RTCMMavlink(this);
|
||||
|
||||
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<const char*>(fakeData), fakeMsgLengths[i]);
|
||||
rtcm->RTCMDataUpdate(message);
|
||||
msleep(4);
|
||||
}
|
||||
msleep(100);
|
||||
}
|
||||
delete[] fakeData;
|
||||
}
|
||||
|
||||
+56
-52
@@ -7,87 +7,91 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#include "GPSPositionMessage.h"
|
||||
#include "Drivers/src/gps_helper.h"
|
||||
|
||||
#include <QtCore/QByteArray>
|
||||
#include <QtCore/QLoggingCategory>
|
||||
#include <QtCore/QMetaType>
|
||||
#include <QtCore/QObject>
|
||||
#include <QtCore/QString>
|
||||
#include <QtCore/QThread>
|
||||
#include <QtCore/QByteArray>
|
||||
|
||||
#include <gps_helper.h>
|
||||
|
||||
#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;
|
||||
QSerialPort *_serial = nullptr;
|
||||
|
||||
enum GPSReceiveType {
|
||||
Position = 1,
|
||||
Satellite = 2
|
||||
};
|
||||
|
||||
static constexpr uint32_t kGPSReceiveTimeout = 1200;
|
||||
static constexpr float kGPSHeadingOffset = 5.f;
|
||||
};
|
||||
|
||||
@@ -0,0 +1,152 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* 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>("satellite_info_s");
|
||||
qRegisterMetaType<sensor_gnss_relative_s>("sensor_gnss_relative_s");
|
||||
qRegisterMetaType<sensor_gps_s>("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<double>(accuracyMM) / 1000.0);
|
||||
_gpsRtkFactGroup->currentLatitude()->setRawValue(latitude);
|
||||
_gpsRtkFactGroup->currentLongitude()->setRawValue(longitude);
|
||||
_gpsRtkFactGroup->currentAltitude()->setRawValue(altitude);
|
||||
_gpsRtkFactGroup->valid()->setRawValue(valid);
|
||||
_gpsRtkFactGroup->active()->setRawValue(active);
|
||||
}
|
||||
|
||||
void 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);
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* QGroundControl is licensed according to the terms in the file
|
||||
* COPYING.md in the root of the source code directory.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <QtCore/QLoggingCategory>
|
||||
#include <QtCore/QObject>
|
||||
#include <QtCore/QString>
|
||||
|
||||
#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;
|
||||
};
|
||||
+70
-48
@@ -7,74 +7,96 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#include "RTCMMavlink.h"
|
||||
#include "MultiVehicleManager.h"
|
||||
#include "Vehicle.h"
|
||||
#include "MAVLinkProtocol.h"
|
||||
#include "MultiVehicleManager.h"
|
||||
#include "QGCApplication.h"
|
||||
#include "QGCLoggingCategory.h"
|
||||
#include "Vehicle.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;
|
||||
}
|
||||
|
||||
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));
|
||||
void RTCMMavlink::RTCMDataUpdate(QByteArrayView data)
|
||||
{
|
||||
#ifdef QT_DEBUG
|
||||
_calculateBandwith(data.size());
|
||||
#endif
|
||||
|
||||
if (message.size() < maxMessageLength) {
|
||||
mavlinkRtcmData.len = message.size();
|
||||
mavlinkRtcmData.flags = (_sequenceId & 0x1F) << 3;
|
||||
memcpy(&mavlinkRtcmData.data, message.data(), message.size());
|
||||
sendMessageToVehicle(mavlinkRtcmData);
|
||||
mavlink_gps_rtcm_data_t gpsRtcmData{};
|
||||
|
||||
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;
|
||||
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);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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<Vehicle*>(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<Vehicle*>(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;
|
||||
}
|
||||
}
|
||||
|
||||
+14
-15
@@ -10,31 +10,30 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "QGCToolbox.h"
|
||||
#include "MAVLinkLib.h"
|
||||
|
||||
#include <QtCore/QObject>
|
||||
#include <QtCore/QElapsedTimer>
|
||||
#include <QtCore/QLoggingCategory>
|
||||
#include <QtCore/QObject>
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
+23
-35
@@ -49,51 +49,29 @@
|
||||
#pragma once
|
||||
|
||||
#include <QtCore/QtGlobal>
|
||||
|
||||
#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 <QtCore/QDateTime>
|
||||
#include <QtCore/QLoggingCategory>
|
||||
#include <QtCore/QThread>
|
||||
|
||||
#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
|
||||
#define GPS_INFO(...) qCInfo(GPSDriversLog, __VA_ARGS__)
|
||||
#define GPS_WARN(...) qCWarning(GPSDriversLog, __VA_ARGS__)
|
||||
#define GPS_ERR(...) qCCritical(GPSDriversLog, __VA_ARGS__)
|
||||
|
||||
#include <QtCore/QThread>
|
||||
#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
|
||||
|
||||
class Sleeper : public QThread
|
||||
{
|
||||
public:
|
||||
static void usleep(unsigned long usecs) { QThread::usleep(usecs); }
|
||||
};
|
||||
#define M_PI_2_F 0.63661977f
|
||||
|
||||
static inline void gps_usleep(unsigned long usecs) {
|
||||
Sleeper::usleep(usecs);
|
||||
}
|
||||
|
||||
typedef uint64_t gps_abstime;
|
||||
|
||||
#include <QtCore/QDateTime>
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
|
||||
//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);
|
||||
}
|
||||
|
||||
@@ -40,16 +40,19 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* https://github.com/PX4/Firmware/blob/master/msg/SatelliteInfo.msg */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/satellite_info.msg
|
||||
* and was manually copied here.
|
||||
*/
|
||||
#include <QtCore/QMetaType>
|
||||
|
||||
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);
|
||||
|
||||
@@ -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 <stdint.h>
|
||||
|
||||
/*
|
||||
* This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/SensorGnssRelative.msg
|
||||
* and was manually copied here.
|
||||
*/
|
||||
#include <QtCore/QMetaType>
|
||||
|
||||
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);
|
||||
|
||||
+41
-31
@@ -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 <stdint.h>
|
||||
|
||||
/*
|
||||
* This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/SensorGps.msg
|
||||
* and was manually copied here.
|
||||
*/
|
||||
#include <QtCore/QMetaType>
|
||||
|
||||
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;
|
||||
int32_t jamming_indicator;
|
||||
float vel_m_s;
|
||||
float vel_n_m_s;
|
||||
float vel_e_m_s;
|
||||
float vel_d_m_s;
|
||||
float cog_rad;
|
||||
int32_t timestamp_time_relative;
|
||||
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
|
||||
|
||||
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;
|
||||
uint8_t selected_rtcm_instance;
|
||||
|
||||
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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -14,7 +14,6 @@
|
||||
#include <QtCore/QObject>
|
||||
|
||||
class FirmwarePluginManager;
|
||||
class GPSManager;
|
||||
class LinkManager;
|
||||
class MAVLinkProtocol;
|
||||
class MissionCommandTree;
|
||||
@@ -49,9 +48,6 @@ public:
|
||||
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 @@ private:
|
||||
void _scanAndLoadPlugins(QGCApplication *app);
|
||||
|
||||
FirmwarePluginManager* _firmwarePluginManager = nullptr;
|
||||
#ifndef NO_SERIAL_LINK
|
||||
GPSManager* _gpsManager = nullptr;
|
||||
#endif
|
||||
LinkManager* _linkManager = nullptr;
|
||||
MAVLinkProtocol* _mavlinkProtocol = nullptr;
|
||||
MissionCommandTree* _missionCommandTree = nullptr;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
@@ -134,6 +137,7 @@ target_link_libraries(qgctest
|
||||
FactSystemTest
|
||||
FollowMeTest
|
||||
GeoTest
|
||||
GpsTest
|
||||
MAVLinkTest
|
||||
MissionManagerTest
|
||||
QmlControlsTest
|
||||
|
||||
@@ -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})
|
||||
@@ -0,0 +1,19 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* 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 <QtTest/QTest>
|
||||
|
||||
void GpsTest::_testGpsRTCM()
|
||||
{
|
||||
|
||||
}
|
||||
@@ -0,0 +1,20 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||
*
|
||||
* 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();
|
||||
};
|
||||
@@ -42,6 +42,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)
|
||||
|
||||
Reference in New Issue
Block a user