Don Gagne
9 years ago
15 changed files with 795 additions and 0 deletions
@ -1,3 +1,6 @@ |
|||||||
[submodule "libs/mavlink/include/mavlink/v1.0"] |
[submodule "libs/mavlink/include/mavlink/v1.0"] |
||||||
path = libs/mavlink/include/mavlink/v1.0 |
path = libs/mavlink/include/mavlink/v1.0 |
||||||
url = https://github.com/mavlink/c_library.git |
url = https://github.com/mavlink/c_library.git |
||||||
|
[submodule "src/GPS/Drivers"] |
||||||
|
path = src/GPS/Drivers |
||||||
|
url = https://github.com/PX4/GpsDrivers.git |
||||||
|
|||||||
@ -0,0 +1,86 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#include "GPSManager.h" |
||||||
|
#include <QDebug> |
||||||
|
|
||||||
|
GPSManager::GPSManager(QGCApplication* app) |
||||||
|
: QGCTool(app) |
||||||
|
{ |
||||||
|
qRegisterMetaType<GPSPositionMessage>(); |
||||||
|
qRegisterMetaType<GPSSatelliteMessage>(); |
||||||
|
} |
||||||
|
|
||||||
|
GPSManager::~GPSManager() |
||||||
|
{ |
||||||
|
cleanup(); |
||||||
|
} |
||||||
|
|
||||||
|
void GPSManager::setupGPS(const QString& device) |
||||||
|
{ |
||||||
|
Q_ASSERT(_toolbox); |
||||||
|
|
||||||
|
cleanup(); |
||||||
|
_requestGpsStop = false; |
||||||
|
_gpsProvider = new GPSProvider(device, true, _requestGpsStop); |
||||||
|
_gpsProvider->start(); |
||||||
|
|
||||||
|
//create RTCM device
|
||||||
|
_rtcmMavlink = new RTCMMavlink(*_toolbox); |
||||||
|
|
||||||
|
connect(_gpsProvider, SIGNAL(RTCMDataUpdate(QByteArray)), _rtcmMavlink, |
||||||
|
SLOT(RTCMDataUpdate(QByteArray))); |
||||||
|
|
||||||
|
//test: connect to position update
|
||||||
|
connect(_gpsProvider, SIGNAL(positionUpdate(GPSPositionMessage)), this, |
||||||
|
SLOT(GPSPositionUpdate(GPSPositionMessage))); |
||||||
|
connect(_gpsProvider, SIGNAL(satelliteInfoUpdate(GPSSatelliteMessage)), this, |
||||||
|
SLOT(GPSSatelliteUpdate(GPSSatelliteMessage))); |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
void GPSManager::GPSPositionUpdate(GPSPositionMessage msg) |
||||||
|
{ |
||||||
|
qDebug("GPS: got position update: alt=%i, long=%i, lat=%i", |
||||||
|
msg.position_data.alt, msg.position_data.lon, |
||||||
|
msg.position_data.lat); |
||||||
|
} |
||||||
|
void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg) |
||||||
|
{ |
||||||
|
qDebug("GPS: got satellite info update, %i satellites", (int)msg.satellite_data.count); |
||||||
|
} |
||||||
|
|
||||||
|
void GPSManager::cleanup() |
||||||
|
{ |
||||||
|
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); |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,56 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#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); |
||||||
|
~GPSManager(); |
||||||
|
|
||||||
|
void setupGPS(const QString& device); |
||||||
|
|
||||||
|
private slots: |
||||||
|
void GPSPositionUpdate(GPSPositionMessage msg); |
||||||
|
void GPSSatelliteUpdate(GPSSatelliteMessage msg); |
||||||
|
private: |
||||||
|
void cleanup(); |
||||||
|
|
||||||
|
GPSProvider* _gpsProvider = nullptr; |
||||||
|
RTCMMavlink* _rtcmMavlink = nullptr; |
||||||
|
|
||||||
|
std::atomic_bool _requestGpsStop; ///< signals the thread to quit
|
||||||
|
}; |
||||||
@ -0,0 +1,46 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#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 |
||||||
|
{ |
||||||
|
vehicle_gps_position_s position_data; |
||||||
|
}; |
||||||
|
|
||||||
|
Q_DECLARE_METATYPE(GPSPositionMessage); |
||||||
|
|
||||||
|
|
||||||
|
struct GPSSatelliteMessage |
||||||
|
{ |
||||||
|
satellite_info_s satellite_data; |
||||||
|
}; |
||||||
|
Q_DECLARE_METATYPE(GPSSatelliteMessage); |
||||||
@ -0,0 +1,175 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#include "GPSProvider.h" |
||||||
|
|
||||||
|
#define TIMEOUT_5HZ 500 |
||||||
|
|
||||||
|
#include <QDebug> |
||||||
|
|
||||||
|
#include "Drivers/src/ubx.h" |
||||||
|
#include "Drivers/src/gps_helper.h" |
||||||
|
#include "definitions.h" |
||||||
|
|
||||||
|
|
||||||
|
void GPSProvider::run() |
||||||
|
{ |
||||||
|
if (_serial) delete _serial; |
||||||
|
|
||||||
|
_serial = new QSerialPort(); |
||||||
|
_serial->setPortName(_device); |
||||||
|
if (!_serial->open(QIODevice::ReadWrite)) { |
||||||
|
qWarning() << "GPS: Failed to open Serial Device" << _device; |
||||||
|
return; |
||||||
|
} |
||||||
|
_serial->setBaudRate(QSerialPort::Baud9600); |
||||||
|
_serial->setDataBits(QSerialPort::Data8); |
||||||
|
_serial->setParity(QSerialPort::NoParity); |
||||||
|
_serial->setStopBits(QSerialPort::OneStop); |
||||||
|
_serial->setFlowControl(QSerialPort::NoFlowControl); |
||||||
|
|
||||||
|
unsigned int baudrate; |
||||||
|
GPSHelper* gpsHelper = nullptr; |
||||||
|
|
||||||
|
while (!_requestStop) { |
||||||
|
|
||||||
|
if (gpsHelper) { |
||||||
|
delete gpsHelper; |
||||||
|
gpsHelper = nullptr; |
||||||
|
} |
||||||
|
|
||||||
|
gpsHelper = new GPSDriverUBX(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo); |
||||||
|
|
||||||
|
if (gpsHelper->configure(baudrate, GPSHelper::OutputMode::RTCM) == 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 = gpsHelper->receive(TIMEOUT_5HZ); |
||||||
|
|
||||||
|
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; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
qDebug() << "Exiting GPS thread"; |
||||||
|
} |
||||||
|
|
||||||
|
GPSProvider::GPSProvider(const QString& device, bool enableSatInfo, const std::atomic_bool& requestStop) |
||||||
|
: _device(device), _requestStop(requestStop) |
||||||
|
{ |
||||||
|
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, 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: { |
||||||
|
int timeout = *((int *) data1); |
||||||
|
if (!_serial->waitForReadyRead(timeout)) |
||||||
|
return 0; //timeout
|
||||||
|
msleep(10); //give some more time to buffer data
|
||||||
|
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; |
||||||
|
qInfo("Survey-in status: %is cur accuracy: %imm valid: %i active: %i", |
||||||
|
status->duration, status->mean_accuracy, (int)(status->flags & 1), (int)((status->flags>>1) & 1)); |
||||||
|
} |
||||||
|
break; |
||||||
|
|
||||||
|
case GPSCallbackType::setClock: |
||||||
|
/* do nothing */ |
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
return 0; |
||||||
|
} |
||||||
@ -0,0 +1,78 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#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: |
||||||
|
GPSProvider(const QString& device, bool enableSatInfo, 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); |
||||||
|
|
||||||
|
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; |
||||||
|
const std::atomic_bool& _requestStop; |
||||||
|
|
||||||
|
struct vehicle_gps_position_s _reportGpsPos; |
||||||
|
struct satellite_info_s *_pReportSatInfo = nullptr; |
||||||
|
|
||||||
|
QSerialPort *_serial = nullptr; |
||||||
|
}; |
||||||
@ -0,0 +1,65 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#include "RTCMMavlink.h" |
||||||
|
|
||||||
|
#include "MultiVehicleManager.h" |
||||||
|
#include "MAVLinkProtocol.h" |
||||||
|
#include "Vehicle.h" |
||||||
|
|
||||||
|
#include <cstdio> |
||||||
|
|
||||||
|
RTCMMavlink::RTCMMavlink(QGCToolbox& toolbox) |
||||||
|
: _toolbox(toolbox) |
||||||
|
{ |
||||||
|
_bandwidthTimer.start(); |
||||||
|
} |
||||||
|
|
||||||
|
void RTCMMavlink::RTCMDataUpdate(QByteArray message) |
||||||
|
{ |
||||||
|
Q_ASSERT(message.size() <= 110); //mavlink message uses a fixed-size buffer
|
||||||
|
|
||||||
|
/* 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; |
||||||
|
} |
||||||
|
QmlObjectListModel& vehicles = *_toolbox.multiVehicleManager()->vehicles(); |
||||||
|
MAVLinkProtocol* mavlinkProtocol = _toolbox.mavlinkProtocol(); |
||||||
|
mavlink_gps_inject_data_t mavlinkRtcmData; |
||||||
|
memset(&mavlinkRtcmData, 0, sizeof(mavlink_gps_inject_data_t)); |
||||||
|
|
||||||
|
mavlinkRtcmData.len = message.size(); |
||||||
|
memcpy(&mavlinkRtcmData.data, message.data(), message.size()); |
||||||
|
|
||||||
|
for (int i = 0; i < vehicles.count(); i++) { |
||||||
|
Vehicle* vehicle = qobject_cast<Vehicle*>(vehicles[i]); |
||||||
|
mavlink_message_t message; |
||||||
|
mavlink_msg_gps_inject_data_encode(mavlinkProtocol->getSystemId(), |
||||||
|
mavlinkProtocol->getComponentId(), &message, &mavlinkRtcmData); |
||||||
|
vehicle->sendMessage(message); |
||||||
|
} |
||||||
|
} |
||||||
@ -0,0 +1,49 @@ |
|||||||
|
/*=====================================================================
|
||||||
|
|
||||||
|
QGroundControl Open Source Ground Control Station |
||||||
|
|
||||||
|
(c) 2009 - 2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
|
||||||
|
|
||||||
|
This file is part of the QGROUNDCONTROL project |
||||||
|
|
||||||
|
QGROUNDCONTROL is free software: you can redistribute it and/or modify |
||||||
|
it under the terms of the GNU General Public License as published by |
||||||
|
the Free Software Foundation, either version 3 of the License, or |
||||||
|
(at your option) any later version. |
||||||
|
|
||||||
|
QGROUNDCONTROL is distributed in the hope that it will be useful, |
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||||
|
GNU General Public License for more details. |
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License |
||||||
|
along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
======================================================================*/ |
||||||
|
|
||||||
|
#pragma once |
||||||
|
|
||||||
|
#include <QObject> |
||||||
|
#include <QElapsedTimer> |
||||||
|
|
||||||
|
#include "QGCToolbox.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: |
||||||
|
QGCToolbox& _toolbox; |
||||||
|
QElapsedTimer _bandwidthTimer; |
||||||
|
int _bandwidthByteCounter = 0; |
||||||
|
}; |
||||||
@ -0,0 +1,85 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* 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_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_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); } |
||||||
|
}; |
||||||
|
|
||||||
|
#define usleep Sleeper::usleep |
||||||
|
|
||||||
|
|
||||||
|
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 |
||||||
|
struct timespec |
||||||
|
{ |
||||||
|
time_t tv_sec; |
||||||
|
long tv_nsec; |
||||||
|
}; |
||||||
|
#endif |
||||||
|
|
||||||
@ -0,0 +1,54 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* 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]; |
||||||
|
#ifdef __cplusplus |
||||||
|
static const uint8_t SAT_INFO_MAX_SATELLITES = 20; |
||||||
|
|
||||||
|
#endif |
||||||
|
}; |
||||||
@ -0,0 +1,68 @@ |
|||||||
|
/****************************************************************************
|
||||||
|
* |
||||||
|
* 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/vehicle_gps_position.msg
|
||||||
|
* and was manually copied here. |
||||||
|
*/ |
||||||
|
|
||||||
|
struct vehicle_gps_position_s { |
||||||
|
uint64_t timestamp_position; |
||||||
|
int32_t lat; |
||||||
|
int32_t lon; |
||||||
|
int32_t alt; |
||||||
|
int32_t alt_ellipsoid; |
||||||
|
uint64_t timestamp_variance; |
||||||
|
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; |
||||||
|
uint64_t timestamp_velocity; |
||||||
|
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; |
||||||
|
uint64_t timestamp_time; |
||||||
|
uint64_t time_utc_usec; |
||||||
|
uint8_t satellites_used; |
||||||
|
}; |
||||||
Loading…
Reference in new issue