GPSManager.cc 3.89 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

#include "GPSManager.h"
12
#include "QGCLoggingCategory.h"
Don Gagne's avatar
Don Gagne committed
13 14 15
#include "QGCApplication.h"
#include "SettingsManager.h"
#include "RTKSettings.h"
16

17 18
GPSManager::GPSManager(QGCApplication* app, QGCToolbox* toolbox)
    : QGCTool(app, toolbox)
19 20 21 22 23 24 25
{
    qRegisterMetaType<GPSPositionMessage>();
    qRegisterMetaType<GPSSatelliteMessage>();
}

GPSManager::~GPSManager()
{
26
    disconnectGPS();
27 28
}

29
void GPSManager::connectGPS(const QString& device, const QString& gps_type)
30
{
Don Gagne's avatar
Don Gagne committed
31 32
    RTKSettings* rtkSettings = qgcApp()->toolbox()->settingsManager()->rtkSettings();

33 34 35 36
    GPSProvider::GPSType type;
    if (gps_type.contains("trimble",  Qt::CaseInsensitive)) {
        type = GPSProvider::GPSType::trimble;
        qCDebug(RTKGPSLog) << "Connecting Trimble device";
37 38 39
    } else if (gps_type.contains("septentrio",  Qt::CaseInsensitive)) {
        type = GPSProvider::GPSType::septentrio;
        qCDebug(RTKGPSLog) << "Connecting Septentrio device";
40 41 42 43 44
    } else {
        type = GPSProvider::GPSType::u_blox;
        qCDebug(RTKGPSLog) << "Connecting U-blox device";
    }

45
    disconnectGPS();
46
    _requestGpsStop = false;
47 48 49 50 51 52 53 54 55 56 57
    _gpsProvider = new GPSProvider(device,
                                   type,
                                   true,    /* enableSatInfo */
                                   rtkSettings->surveyInAccuracyLimit()->rawValue().toDouble(),
                                   rtkSettings->surveyInMinObservationDuration()->rawValue().toInt(),
                                   rtkSettings->useFixedBasePosition()->rawValue().toBool(),
                                   rtkSettings->fixedBasePositionLatitude()->rawValue().toDouble(),
                                   rtkSettings->fixedBasePositionLongitude()->rawValue().toDouble(),
                                   rtkSettings->fixedBasePositionAltitude()->rawValue().toFloat(),
                                   rtkSettings->fixedBasePositionAccuracy()->rawValue().toFloat(),
                                   _requestGpsStop);
58 59 60 61 62
    _gpsProvider->start();

    //create RTCM device
    _rtcmMavlink = new RTCMMavlink(*_toolbox);

63
    connect(_gpsProvider, &GPSProvider::RTCMDataUpdate, _rtcmMavlink, &RTCMMavlink::RTCMDataUpdate);
64 65

    //test: connect to position update
66 67 68 69
    connect(_gpsProvider, &GPSProvider::positionUpdate,         this, &GPSManager::GPSPositionUpdate);
    connect(_gpsProvider, &GPSProvider::satelliteInfoUpdate,    this, &GPSManager::GPSSatelliteUpdate);
    connect(_gpsProvider, &GPSProvider::finished,               this, &GPSManager::onDisconnect);
    connect(_gpsProvider, &GPSProvider::surveyInStatus,         this, &GPSManager::surveyInStatus);
70

71
    emit onConnect();
72 73
}

74
void GPSManager::disconnectGPS(void)
75 76 77 78 79 80 81 82 83 84 85 86
{
    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);
    }
87 88
    _gpsProvider = nullptr;
    _rtcmMavlink = nullptr;
89 90 91 92 93 94 95 96 97 98 99
}


void GPSManager::GPSPositionUpdate(GPSPositionMessage msg)
{
    qCDebug(RTKGPSLog) << QString("GPS: got position update: alt=%1, long=%2, lat=%3").arg(msg.position_data.alt).arg(msg.position_data.lon).arg(msg.position_data.lat);
}
void GPSManager::GPSSatelliteUpdate(GPSSatelliteMessage msg)
{
    qCDebug(RTKGPSLog) << QString("GPS: got satellite info update, %1 satellites").arg((int)msg.satellite_data.count);
    emit satelliteUpdate(msg.satellite_data.count);
100
}