Browse Source

GPSProvider: retry a few times before giving up when we get an error from the driver

This also replaces tabs with spaces
QGC4.4
Beat Küng 9 years ago
parent
commit
f2011b3005
  1. 68
      src/GPS/GPSProvider.cc

68
src/GPS/GPSProvider.cc

@ -51,37 +51,49 @@ void GPSProvider::run() @@ -51,37 +51,49 @@ void GPSProvider::run()
unsigned int baudrate;
GPSHelper* gpsHelper = nullptr;
while (!_requestStop) {
while (!_requestStop) {
if (gpsHelper) {
delete gpsHelper;
gpsHelper = nullptr;
}
if (gpsHelper) {
delete gpsHelper;
gpsHelper = nullptr;
}
gpsHelper = new GPSDriverUBX(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo);
gpsHelper = new GPSDriverUBX(&callbackEntry, this, &_reportGpsPos, _pReportSatInfo);
if (gpsHelper->configure(baudrate, GPSHelper::OutputMode::RTCM) == 0) {
if (gpsHelper->configure(baudrate, GPSHelper::OutputMode::RTCM) == 0) {
/* reset report */
memset(&_reportGpsPos, 0, sizeof(_reportGpsPos));
/* reset report */
memset(&_reportGpsPos, 0, sizeof(_reportGpsPos));
int helperRet;
//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 ((helperRet = gpsHelper->receive(TIMEOUT_5HZ)) > 0 && !_requestStop) {
while (!_requestStop && numTries < 3) {
int helperRet = gpsHelper->receive(TIMEOUT_5HZ);
if (helperRet & 1) {
publishGPSPosition();
}
if (helperRet > 0) {
numTries = 0;
if (_pReportSatInfo && (helperRet & 2)) {
publishGPSSatellite();
}
}
if (_serial->error() != QSerialPort::NoError) {
break;
}
}
}
qDebug() << "Exiting GPS thread";
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)
@ -118,13 +130,13 @@ void GPSProvider::gotRTCMData(uint8_t* data, size_t len) @@ -118,13 +130,13 @@ void GPSProvider::gotRTCMData(uint8_t* data, size_t len)
int GPSProvider::callbackEntry(GPSCallbackType type, void *data1, int data2, void *user)
{
GPSProvider *gps = (GPSProvider *)user;
return gps->callback(type, data1, data2);
GPSProvider *gps = (GPSProvider *)user;
return gps->callback(type, data1, data2);
}
int GPSProvider::callback(GPSCallbackType type, void *data1, int data2)
{
switch (type) {
switch (type) {
case GPSCallbackType::readDeviceData: {
int timeout = *((int *) data1);
if (!_serial->waitForReadyRead(timeout))
@ -159,5 +171,5 @@ int GPSProvider::callback(GPSCallbackType type, void *data1, int data2) @@ -159,5 +171,5 @@ int GPSProvider::callback(GPSCallbackType type, void *data1, int data2)
break;
}
return 0;
return 0;
}

Loading…
Cancel
Save