GPSProvider.cc

/****************************************************************************
 *
 * (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);
        }

        if (gpsDriver->configure(baudrate, GPSDriverUBX::OutputMode::RTCM) == 0) {

            /* reset report */
            memset(&_reportGpsPos, 0, sizeof(_reportGpsPos));