GPSProvider.cc 8.21 KB
Newer Older
1 2
/****************************************************************************
 *
Gus Grubba's avatar
Gus Grubba committed
3
 * (c) 2009-2020 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
4 5 6 7 8 9
 *
 * QGroundControl is licensed according to the terms in the file
 * COPYING.md in the root of the source code directory.
 *
 ****************************************************************************/

10 11

#include "GPSProvider.h"
12
#include "QGCLoggingCategory.h"
13 14
#include "QGCApplication.h"
#include "SettingsManager.h"
15

16
#define GPS_RECEIVE_TIMEOUT 1200
17 18 19 20

#include <QDebug>

#include "Drivers/src/ubx.h"
21
#include "Drivers/src/sbf.h"
22
#include "Drivers/src/ashtech.h"
23
#include "Drivers/src/base_station.h"
24 25
#include "definitions.h"

26 27 28
//#define SIMULATE_RTCM_OUTPUT //if defined, generate simulated RTCM messages
                               //additionally make sure to call connectGPS(""), eg. from QGCToolbox.cc

29 30 31

void GPSProvider::run()
{
32 33 34 35 36 37 38 39 40 41 42 43 44
#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 */

45 46 47 48 49
    if (_serial) delete _serial;

    _serial = new QSerialPort();
    _serial->setPortName(_device);
    if (!_serial->open(QIODevice::ReadWrite)) {
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
        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;
        }
65 66 67 68 69 70 71 72
    }
    _serial->setBaudRate(QSerialPort::Baud9600);
    _serial->setDataBits(QSerialPort::Data8);
    _serial->setParity(QSerialPort::NoParity);
    _serial->setStopBits(QSerialPort::OneStop);
    _serial->setFlowControl(QSerialPort::NoFlowControl);

    unsigned int baudrate;
73
    GPSBaseStationSupport* gpsDriver = nullptr;
74

75
    while (!_requestStop) {
76

Don Gagne's avatar
Don Gagne committed
77 78 79
        if (gpsDriver) {
            delete gpsDriver;
            gpsDriver = nullptr;
80 81
        }

82 83 84
        if (_type == GPSType::trimble) {
            gpsDriver = new GPSDriverAshtech(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo);
            baudrate = 115200;
85 86 87
        } else if (_type == GPSType::septentrio) {
            gpsDriver = new GPSDriverSBF(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo, 5);
            baudrate = 0; // auto-configure
88 89 90 91
        } else {
            gpsDriver = new GPSDriverUBX(GPSDriverUBX::Interface::UART, &callbackEntry, this, &_reportGpsPos, _pReportSatInfo);
            baudrate = 0; // auto-configure
        }
92 93 94
        gpsDriver->setSurveyInSpecs(_surveyInAccMeters * 10000.0f, _surveryInDurationSecs);

        if (_useFixedBaseLoction) {
Don Gagne's avatar
Don Gagne committed
95
            gpsDriver->setBasePosition(_fixedBaseLatitude, _fixedBaseLongitude, _fixedBaseAltitudeMeters, _fixedBaseAccuracyMeters * 1000.0f);
96
        }
97

Don Gagne's avatar
Don Gagne committed
98
        if (gpsDriver->configure(baudrate, GPSDriverUBX::OutputMode::RTCM) == 0) {
99

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

103 104 105
            //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;
106

107
            while (!_requestStop && numTries < 3) {
Don Gagne's avatar
Don Gagne committed
108
                int helperRet = gpsDriver->receive(GPS_RECEIVE_TIMEOUT);
109

110 111
                if (helperRet > 0) {
                    numTries = 0;
112

113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
                    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;
            }
        }
    }
131
    qCDebug(RTKGPSLog) << "Exiting GPS thread";
132 133
}

134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154
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)
155
{
Don Gagne's avatar
Don Gagne committed
156
    qCDebug(RTKGPSLog) << "Survey in accuracy:duration" << surveyInAccMeters << surveryInDurationSecs;
157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181
    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)
{
DonLakeFlyer's avatar
DonLakeFlyer committed
182
    QByteArray message((char*)data, static_cast<int>(len));
183 184 185 186 187
    emit RTCMDataUpdate(message);
}

int GPSProvider::callbackEntry(GPSCallbackType type, void *data1, int data2, void *user)
{
188 189
    GPSProvider *gps = (GPSProvider *)user;
    return gps->callback(type, data1, data2);
190 191 192 193
}

int GPSProvider::callback(GPSCallbackType type, void *data1, int data2)
{
194
    switch (type) {
195
        case GPSCallbackType::readDeviceData: {
196 197 198 199 200
            if (_serial->bytesAvailable() == 0) {
                int timeout = *((int *) data1);
                if (!_serial->waitForReadyRead(timeout))
                    return 0; //timeout
            }
201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219
            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;
220 221
            qCDebug(RTKGPSLog) << "Position: " << status->latitude << status->longitude << status->altitude;

222
            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));
223
            emit surveyInStatus(status->duration, status->mean_accuracy, status->latitude, status->longitude, status->altitude, (int)(status->flags & 1), (int)((status->flags>>1) & 1));
224 225 226 227 228 229 230 231
        }
            break;

        case GPSCallbackType::setClock:
            /* do nothing */
            break;
    }

232
    return 0;
233
}