GPSProvider.cc 5.31 KB
Newer Older
1 2 3 4 5 6 7 8 9
/****************************************************************************
 *
 *   (c) 2009-2016 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.
 *
 ****************************************************************************/

10 11 12 13 14 15 16 17 18 19 20

#include "GPSProvider.h"

#define TIMEOUT_5HZ 500

#include <QDebug>

#include "Drivers/src/ubx.h"
#include "Drivers/src/gps_helper.h"
#include "definitions.h"

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

24 25 26

void GPSProvider::run()
{
27 28 29 30 31 32 33 34 35 36 37 38 39
#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 */

40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56
    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;

57
    while (!_requestStop) {
58

59 60 61 62 63 64
        if (gpsHelper) {
            delete gpsHelper;
            gpsHelper = nullptr;
        }

        gpsHelper = new GPSDriverUBX(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo);
65

66
        if (gpsHelper->configure(baudrate, GPSHelper::OutputMode::RTCM) == 0) {
67

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

71 72 73
            //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;
74

75 76
            while (!_requestStop && numTries < 3) {
                int helperRet = gpsHelper->receive(TIMEOUT_5HZ);
77

78 79
                if (helperRet > 0) {
                    numTries = 0;
80

81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99
                    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";
100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135
}

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)
{
136 137
    GPSProvider *gps = (GPSProvider *)user;
    return gps->callback(type, data1, data2);
138 139 140 141
}

int GPSProvider::callback(GPSCallbackType type, void *data1, int data2)
{
142
    switch (type) {
143
        case GPSCallbackType::readDeviceData: {
144 145 146 147 148
            if (_serial->bytesAvailable() == 0) {
                int timeout = *((int *) data1);
                if (!_serial->waitForReadyRead(timeout))
                    return 0; //timeout
            }
149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177
            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;
    }

178
    return 0;
179
}