Merge branch 'master' of https://bdgit.educoder.net/pxsi3jbgc/wwcs-1314
commit
2b214ff4ca
@ -0,0 +1,26 @@
|
||||
|
||||
|
||||
add_library(gps
|
||||
Drivers/src/ashtech.cpp
|
||||
Drivers/src/gps_helper.cpp
|
||||
Drivers/src/mtk.cpp
|
||||
Drivers/src/rtcm.cpp
|
||||
Drivers/src/sbf.cpp
|
||||
Drivers/src/ubx.cpp
|
||||
GPSManager.cc
|
||||
GPSProvider.cc
|
||||
RTCM/RTCMMavlink.cc
|
||||
)
|
||||
|
||||
target_link_libraries(gps
|
||||
Qt6::Core
|
||||
Qt6::Location
|
||||
Qt6::SerialPort
|
||||
Qt6::Svg
|
||||
Qt6::TextToSpeech
|
||||
|
||||
qgc
|
||||
)
|
||||
|
||||
target_include_directories(gps INTERFACE ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
@ -0,0 +1,100 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (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 "GPSManager.h"
|
||||
#include "QGCLoggingCategory.h"
|
||||
#include "QGCApplication.h"
|
||||
#include "SettingsManager.h"
|
||||
#include "RTKSettings.h"
|
||||
|
||||
GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox)
|
||||
: QGCTool(app, toolbox)
|
||||
{
|
||||
qRegisterMetaType<GPSPositionMessage>();
|
||||
qRegisterMetaType<GPSSatelliteMessage>();
|
||||
}
|
||||
|
||||
GPSManager::~GPSManager()
|
||||
{
|
||||
disconnectGPS();
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
void GPSManager::GPSPositionUpdate(GPSPositionMessage msg)
|
||||
{
|
||||
qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.alt).arg(msg.position_data.lon).arg(msg.position_data.lat);
|
||||
}
|
||||
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);
|
||||
}
|
||||
@ -0,0 +1,50 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (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 "GPSProvider.h"
|
||||
#include "RTCM/RTCMMavlink.h"
|
||||
#include <QGCToolbox.h>
|
||||
|
||||
#include <QString>
|
||||
#include <QObject>
|
||||
|
||||
/**
|
||||
** class GPSManager
|
||||
* handles a GPS provider and RTK
|
||||
*/
|
||||
class GPSManager : public QGCTool
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
GPSManager(QGCApplication* app, QGCToolbox* toolbox);
|
||||
~GPSManager();
|
||||
|
||||
void connectGPS (const QString& device, const QString& gps_type);
|
||||
void disconnectGPS (void);
|
||||
bool connected (void) const { return _gpsProvider && _gpsProvider->isRunning(); }
|
||||
|
||||
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);
|
||||
|
||||
private:
|
||||
GPSProvider* _gpsProvider = nullptr;
|
||||
RTCMMavlink* _rtcmMavlink = nullptr;
|
||||
|
||||
std::atomic_bool _requestGpsStop; ///< signals the thread to quit
|
||||
};
|
||||
@ -0,0 +1,33 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (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 "vehicle_gps_position.h"
|
||||
#include "satellite_info.h"
|
||||
#include <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);
|
||||
@ -0,0 +1,234 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (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 "GPSProvider.h"
|
||||
#include "QGCLoggingCategory.h"
|
||||
#include "QGCApplication.h"
|
||||
#include "SettingsManager.h"
|
||||
|
||||
#define GPS_RECEIVE_TIMEOUT 1200
|
||||
|
||||
#include <QDebug>
|
||||
|
||||
#include "Drivers/src/ubx.h"
|
||||
#include "Drivers/src/sbf.h"
|
||||
#include "Drivers/src/ashtech.h"
|
||||
#include "Drivers/src/base_station.h"
|
||||
#include "definitions.h"
|
||||
|
||||
//#define SIMULATE_RTCM_OUTPUT //if defined, generate simulated RTCM messages
|
||||
//additionally make sure to call connectGPS(""), eg. from QGCToolbox.cc
|
||||
|
||||
|
||||
void GPSProvider::run()
|
||||
{
|
||||
#ifdef SIMULATE_RTCM_OUTPUT
|
||||
const int fakeMsgLengths[3] = { 30, 170, 240 };
|
||||
uint8_t* fakeData = new uint8_t[fakeMsgLengths[2]];
|
||||
while (!_requestStop) {
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
gotRTCMData((uint8_t*) fakeData, fakeMsgLengths[i]);
|
||||
msleep(4);
|
||||
}
|
||||
msleep(100);
|
||||
}
|
||||
delete[] fakeData;
|
||||
#endif /* SIMULATE_RTCM_OUTPUT */
|
||||
|
||||
if (_serial) delete _serial;
|
||||
|
||||
_serial = new QSerialPort();
|
||||
_serial->setPortName(_device);
|
||||
if (!_serial->open(QIODevice::ReadWrite)) {
|
||||
int retries = 60;
|
||||
// Give the device some time to come up. In some cases the device is not
|
||||
// immediately accessible right after startup for some reason. This can take 10-20s.
|
||||
while (retries-- > 0 && _serial->error() == QSerialPort::PermissionError) {
|
||||
qCDebug(RTKGPSLog) << "Cannot open device... retrying";
|
||||
msleep(500);
|
||||
if (_serial->open(QIODevice::ReadWrite)) {
|
||||
_serial->clearError();
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (_serial->error() != QSerialPort::NoError) {
|
||||
qWarning() << "GPS: Failed to open Serial Device" << _device << _serial->errorString();
|
||||
return;
|
||||
}
|
||||
}
|
||||
_serial->setBaudRate(QSerialPort::Baud9600);
|
||||
_serial->setDataBits(QSerialPort::Data8);
|
||||
_serial->setParity(QSerialPort::NoParity);
|
||||
_serial->setStopBits(QSerialPort::OneStop);
|
||||
_serial->setFlowControl(QSerialPort::NoFlowControl);
|
||||
|
||||
unsigned int baudrate;
|
||||
GPSBaseStationSupport* gpsDriver = nullptr;
|
||||
|
||||
while (!_requestStop) {
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void GPSProvider::publishGPSPosition()
|
||||
{
|
||||
GPSPositionMessage msg;
|
||||
msg.position_data = _reportGpsPos;
|
||||
emit positionUpdate(msg);
|
||||
}
|
||||
|
||||
void GPSProvider::publishGPSSatellite()
|
||||
{
|
||||
GPSSatelliteMessage msg;
|
||||
msg.satellite_data = *_pReportSatInfo;
|
||||
emit satelliteInfoUpdate(msg);
|
||||
}
|
||||
|
||||
void GPSProvider::gotRTCMData(uint8_t* data, size_t len)
|
||||
{
|
||||
QByteArray message((char*)data, static_cast<int>(len));
|
||||
emit RTCMDataUpdate(message);
|
||||
}
|
||||
|
||||
int GPSProvider::callbackEntry(GPSCallbackType type, void *data1, int data2, void *user)
|
||||
{
|
||||
GPSProvider *gps = (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
|
||||
}
|
||||
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;
|
||||
|
||||
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;
|
||||
|
||||
case GPSCallbackType::setClock:
|
||||
/* do nothing */
|
||||
break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -0,0 +1,93 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (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 <QString>
|
||||
#include <QThread>
|
||||
#include <QByteArray>
|
||||
#include <QSerialPort>
|
||||
|
||||
#include <atomic>
|
||||
|
||||
#include "GPSPositionMessage.h"
|
||||
#include "Drivers/src/gps_helper.h"
|
||||
|
||||
|
||||
/**
|
||||
** class GPSProvider
|
||||
* opens a GPS device and handles the protocol
|
||||
*/
|
||||
class GPSProvider : public QThread
|
||||
{
|
||||
Q_OBJECT
|
||||
public:
|
||||
|
||||
enum class GPSType {
|
||||
u_blox,
|
||||
trimble,
|
||||
septentrio
|
||||
};
|
||||
|
||||
GPSProvider(const QString& device,
|
||||
GPSType type,
|
||||
bool enableSatInfo,
|
||||
double surveyInAccMeters,
|
||||
int surveryInDurationSecs,
|
||||
bool useFixedBaseLocation,
|
||||
double fixedBaseLatitude,
|
||||
double fixedBaseLongitude,
|
||||
float fixedBaseAltitudeMeters,
|
||||
float fixedBaseAccuracyMeters,
|
||||
const std::atomic_bool& requestStop);
|
||||
~GPSProvider();
|
||||
|
||||
/**
|
||||
* this is called by the callback method
|
||||
*/
|
||||
void gotRTCMData(uint8_t *data, size_t len);
|
||||
|
||||
signals:
|
||||
void positionUpdate(GPSPositionMessage message);
|
||||
void satelliteInfoUpdate(GPSSatelliteMessage message);
|
||||
void RTCMDataUpdate(QByteArray message);
|
||||
void surveyInStatus(float duration, float accuracyMM, double latitude, double longitude, float altitude, bool valid, bool active);
|
||||
|
||||
protected:
|
||||
void run();
|
||||
|
||||
private:
|
||||
void publishGPSPosition();
|
||||
void publishGPSSatellite();
|
||||
|
||||
/**
|
||||
* callback from the driver for the platform specific stuff
|
||||
*/
|
||||
static int callbackEntry(GPSCallbackType type, void *data1, int data2, void *user);
|
||||
|
||||
int callback(GPSCallbackType type, void *data1, int data2);
|
||||
|
||||
QString _device;
|
||||
GPSType _type;
|
||||
const std::atomic_bool& _requestStop;
|
||||
double _surveyInAccMeters;
|
||||
int _surveryInDurationSecs;
|
||||
bool _useFixedBaseLoction;
|
||||
double _fixedBaseLatitude;
|
||||
double _fixedBaseLongitude;
|
||||
float _fixedBaseAltitudeMeters;
|
||||
float _fixedBaseAccuracyMeters;
|
||||
GPSHelper::GPSConfig _gpsConfig{};
|
||||
|
||||
struct sensor_gps_s _reportGpsPos;
|
||||
struct satellite_info_s *_pReportSatInfo = nullptr;
|
||||
|
||||
QSerialPort *_serial = nullptr;
|
||||
};
|
||||
@ -0,0 +1,83 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (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 "RTCMMavlink.h"
|
||||
|
||||
#include "MultiVehicleManager.h"
|
||||
#include "Vehicle.h"
|
||||
|
||||
#include <cstdio>
|
||||
|
||||
RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox)
|
||||
: _toolbox(toolbox)
|
||||
{
|
||||
_bandwidthTimer.start();
|
||||
}
|
||||
|
||||
void RTCMMavlink::RTCMDataUpdate(QByteArray message)
|
||||
{
|
||||
/* statistics */
|
||||
_bandwidthByteCounter += message.size();
|
||||
qint64 elapsed = _bandwidthTimer.elapsed();
|
||||
if (elapsed > 1000) {
|
||||
printf("RTCM bandwidth: %.2f kB/s\n", (float) _bandwidthByteCounter / elapsed * 1000.f / 1024.f);
|
||||
_bandwidthTimer.restart();
|
||||
_bandwidthByteCounter = 0;
|
||||
}
|
||||
|
||||
const qsizetype maxMessageLength = MAVLINK_MSG_GPS_RTCM_DATA_FIELD_DATA_LEN;
|
||||
mavlink_gps_rtcm_data_t mavlinkRtcmData;
|
||||
memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_rtcm_data_t));
|
||||
|
||||
if (message.size() < maxMessageLength) {
|
||||
mavlinkRtcmData.len = message.size();
|
||||
mavlinkRtcmData.flags = (_sequenceId & 0x1F) << 3;
|
||||
memcpy(&mavlinkRtcmData.data, message.data(), message.size());
|
||||
sendMessageToVehicle(mavlinkRtcmData);
|
||||
} 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);
|
||||
start += length;
|
||||
}
|
||||
}
|
||||
++_sequenceId;
|
||||
}
|
||||
|
||||
void RTCMMavlink::sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg)
|
||||
{
|
||||
QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles();
|
||||
MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol();
|
||||
for (int i = 0; i < vehicles.count(); i++) {
|
||||
Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]);
|
||||
WeakLinkInterfacePtr weakLink = vehicle->vehicleLinkManager()->primaryLink();
|
||||
|
||||
if (!weakLink.expired()) {
|
||||
mavlink_message_t message;
|
||||
SharedLinkInterfacePtr sharedLink = weakLink.lock();
|
||||
|
||||
mavlink_msg_gps_rtcm_data_encode_chan(mavlinkProtocol->getSystemId(),
|
||||
mavlinkProtocol->getComponentId(),
|
||||
sharedLink->mavlinkChannel(),
|
||||
&message,
|
||||
&msg);
|
||||
vehicle->sendMessageOnLinkThreadSafe(sharedLink.get(), message);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,40 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* (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 <QObject>
|
||||
#include <QElapsedTimer>
|
||||
|
||||
#include "QGCToolbox.h"
|
||||
#include "MAVLinkProtocol.h"
|
||||
|
||||
/**
|
||||
** 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)?
|
||||
|
||||
public slots:
|
||||
void RTCMDataUpdate(QByteArray message);
|
||||
|
||||
private:
|
||||
void sendMessageToVehicle(const mavlink_gps_rtcm_data_t& msg);
|
||||
|
||||
QGCToolbox& _toolbox;
|
||||
QElapsedTimer _bandwidthTimer;
|
||||
int _bandwidthByteCounter = 0;
|
||||
uint8_t _sequenceId = 0;
|
||||
};
|
||||
@ -0,0 +1,94 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file definitions.h
|
||||
* common platform-specific definitions & abstractions for gps
|
||||
* @author Beat Küng <beat-kueng@gmx.net>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <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 "vehicle_gps_position.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
|
||||
|
||||
#include <QThread>
|
||||
|
||||
class Sleeper : public QThread
|
||||
{
|
||||
public:
|
||||
static void usleep(unsigned long usecs) { QThread::usleep(usecs); }
|
||||
};
|
||||
|
||||
static inline void gps_usleep(unsigned long usecs) {
|
||||
Sleeper::usleep(usecs);
|
||||
}
|
||||
|
||||
typedef uint64_t gps_abstime;
|
||||
|
||||
#include <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
|
||||
struct timespec
|
||||
{
|
||||
time_t tv_sec;
|
||||
long tv_nsec;
|
||||
};
|
||||
#else
|
||||
#include <time.h>
|
||||
#endif
|
||||
#endif
|
||||
|
||||
@ -0,0 +1,55 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#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.
|
||||
*/
|
||||
|
||||
struct satellite_info_s {
|
||||
uint64_t timestamp;
|
||||
uint8_t count;
|
||||
uint8_t svid[20];
|
||||
uint8_t used[20];
|
||||
uint8_t elevation[20];
|
||||
uint8_t azimuth[20];
|
||||
uint8_t snr[20];
|
||||
uint8_t prn[20];
|
||||
#ifdef __cplusplus
|
||||
static const uint8_t SAT_INFO_MAX_SATELLITES = 20;
|
||||
|
||||
#endif
|
||||
};
|
||||
@ -0,0 +1,72 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* Auto-generated by genmsg_cpp from file /home/beat/px4/src/Firmware/msg/vehicle_gps_position.msg */
|
||||
|
||||
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
|
||||
/*
|
||||
* This file is auto-generated from https://github.com/PX4/Firmware/blob/master/msg/vehicle_gps_position.msg
|
||||
* and was manually copied here.
|
||||
*/
|
||||
|
||||
struct sensor_gps_s {
|
||||
uint64_t timestamp;
|
||||
uint64_t time_utc_usec;
|
||||
int32_t lat;
|
||||
int32_t lon;
|
||||
int32_t alt;
|
||||
int32_t alt_ellipsoid;
|
||||
uint16_t automatic_gain_control;
|
||||
uint8_t jamming_state;
|
||||
float s_variance_m_s;
|
||||
float c_variance_rad;
|
||||
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;
|
||||
uint8_t fix_type;
|
||||
bool vel_ned_valid;
|
||||
uint8_t satellites_used;
|
||||
};
|
||||
Loading…
Reference in new issue