Commit 916651a6 authored by Don Gagne's avatar Don Gagne

Merge pull request #3246 from bkueng/rtcm_gps

Add GPS drivers and RTCM output for RTK GPS
parents 7bc1d256 90741961
[submodule "libs/mavlink/include/mavlink/v1.0"]
path = libs/mavlink/include/mavlink/v1.0
url = https://github.com/mavlink/c_library.git
[submodule "src/GPS/Drivers"]
path = src/GPS/Drivers
url = https://github.com/PX4/GpsDrivers.git
......@@ -200,6 +200,7 @@ INCLUDEPATH += \
src/input \
src/Joystick \
src/FollowMe \
src/GPS \
src/lib/qmapcontrol \
src/MissionEditor \
src/MissionManager \
......@@ -377,6 +378,15 @@ HEADERS += \
src/ui/uas/UASQuickViewItem.h \
src/ui/uas/UASQuickViewItemSelect.h \
src/ui/uas/UASQuickViewTextItem.h \
src/GPS/Drivers/src/gps_helper.h \
src/GPS/Drivers/src/ubx.h \
src/GPS/definitions.h \
src/GPS/vehicle_gps_position.h \
src/GPS/satellite_info.h \
src/GPS/RTCM/RTCMMavlink.h \
src/GPS/GPSManager.h \
src/GPS/GPSPositionMessage.h \
src/GPS/GPSProvider.h \
src/VehicleSetup/JoystickConfigController.h \
src/ViewWidgets/CustomCommandWidget.h \
src/ViewWidgets/CustomCommandWidgetController.h \
......@@ -511,6 +521,11 @@ SOURCES += \
src/ui/uas/UASQuickViewItem.cc \
src/ui/uas/UASQuickViewItemSelect.cc \
src/ui/uas/UASQuickViewTextItem.cc \
src/GPS/Drivers/src/gps_helper.cpp \
src/GPS/Drivers/src/ubx.cpp \
src/GPS/RTCM/RTCMMavlink.cc \
src/GPS/GPSManager.cc \
src/GPS/GPSProvider.cc \
src/VehicleSetup/JoystickConfigController.cc \
src/ViewWidgets/CustomCommandWidget.cc \
src/ViewWidgets/CustomCommandWidgetController.cc \
......
Subproject commit 60739aaace1723c700f23b22212696dc75169559
/*=====================================================================
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);
}
}
/*=====================================================================
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
};
/*=====================================================================
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);
/*=====================================================================
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;
}
/*=====================================================================
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;
};
/*=====================================================================
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);
}
}
/*=====================================================================
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;
};
/****************************************************************************
*
* 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
/****************************************************************************
*
* 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
};