Support position accuracy in QNmeaPositionInfoSource.

Extract the HDOP and VDOP from the NMEA stream and calculate the twice
the distance root mean square (2DRMS) error based on the user provided
User equivalent range error value.

[ChangeLog][QtPositioning][QNmeaPositionInfoSource] Added support for
reporting position accuracy.

Change-Id: I59e5f8c32070fa96ae4d0bd02a18f38663920e05
Reviewed-by: Alex Blasche <alexander.blasche@digia.com>

9 files changed, 186 insertions, 12 deletions

diff --git a/src/positioning/qlocationutils.cpp b/src/positioning/qlocationutils.cpp
index 5d6cfbb2..830ac64a 100644
--- a/src/positioning/qlocationutils.cpp
+++ b/src/positioning/qlocationutils.cpp
@@ -1,5 +1,7 @@
 /****************************************************************************
 **
+** Copyright (C) 2013 Jolla Ltd.
+** Contact: Aaron McCarthy <aaron.mccarthy@jollamobile.com>
 ** Copyright (C) 2013 Digia Plc and/or its subsidiary(-ies).
 ** Contact: http://www.qt-project.org/legal
 **
@@ -58,7 +60,8 @@ static double qlocationutils_nmeaDegreesToDecimal(double nmeaDegrees)
     return deg + (min / 60.0);
 }
 
-static void qlocationutils_readGga(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
+static void qlocationutils_readGga(const char *data, int size, QGeoPositionInfo *info, double uere,
+                                   bool *hasFix)
 {
     QByteArray sentence(data, size);
     QList<QByteArray> parts = sentence.split(',');
@@ -82,6 +85,13 @@ static void qlocationutils_readGga(const char *data, int size, QGeoPositionInfo
         }
     }
 
+    if (parts.count() > 8 && !parts[8].isEmpty()) {
+        bool hasHdop = false;
+        double hdop = parts[8].toDouble(&hasHdop);
+        if (hasHdop)
+            info->setAttribute(QGeoPositionInfo::HorizontalAccuracy, 2 * hdop * uere);
+    }
+
     if (parts.count() > 9 && parts[9].count() > 0) {
         bool hasAlt = false;
         double alt = parts[9].toDouble(&hasAlt);
@@ -93,6 +103,29 @@ static void qlocationutils_readGga(const char *data, int size, QGeoPositionInfo
         info->setCoordinate(coord);
 }
 
+static void qlocationutils_readGsa(const char *data, int size, QGeoPositionInfo *info, double uere,
+                                   bool *hasFix)
+{
+    QList<QByteArray> parts = QByteArray::fromRawData(data, size).split(',');
+
+    if (hasFix && parts.count() > 2 && !parts[2].isEmpty())
+        *hasFix = parts[2].toInt() > 0;
+
+    if (parts.count() > 16 && !parts[16].isEmpty()) {
+        bool hasHdop = false;
+        double hdop = parts[16].toDouble(&hasHdop);
+        if (hasHdop)
+            info->setAttribute(QGeoPositionInfo::HorizontalAccuracy, 2 * hdop * uere);
+    }
+
+    if (parts.count() > 17 && !parts[17].isEmpty()) {
+        bool hasVdop = false;
+        double vdop = parts[17].toDouble(&hasVdop);
+        if (hasVdop)
+            info->setAttribute(QGeoPositionInfo::VerticalAccuracy, 2 * vdop * uere);
+    }
+}
+
 static void qlocationutils_readGll(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
 {
     QByteArray sentence(data, size);
@@ -227,7 +260,8 @@ static void qlocationutils_readZda(const char *data, int size, QGeoPositionInfo
     info->setTimestamp(QDateTime(date, time, Qt::UTC));
 }
 
-bool QLocationUtils::getPosInfoFromNmea(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
+bool QLocationUtils::getPosInfoFromNmea(const char *data, int size, QGeoPositionInfo *info,
+                                        double uere, bool *hasFix)
 {
     if (!info)
         return false;
@@ -237,9 +271,23 @@ bool QLocationUtils::getPosInfoFromNmea(const char *data, int size, QGeoPosition
     if (size < 6 || data[0] != '$' || !hasValidNmeaChecksum(data, size))
         return false;
 
+    // Adjust size so that * and following characters are not parsed by the following functions.
+    for (int i = 0; i < size; ++i) {
+        if (data[i] == '*') {
+            size = i;
+            break;
+        }
+    }
+
     if (data[3] == 'G' && data[4] == 'G' && data[5] == 'A') {
         // "$--GGA" sentence.
-        qlocationutils_readGga(data, size, info, hasFix);
+        qlocationutils_readGga(data, size, info, uere, hasFix);
+        return true;
+    }
+
+    if (data[3] == 'G' && data[4] == 'S' && data[5] == 'A') {
+        // "$--GSA" sentence.
+        qlocationutils_readGsa(data, size, info, uere, hasFix);
         return true;
     }
 
diff --git a/src/positioning/qlocationutils_p.h b/src/positioning/qlocationutils_p.h
index 4b42695c..10dd61d4 100644
--- a/src/positioning/qlocationutils_p.h
+++ b/src/positioning/qlocationutils_p.h
@@ -94,7 +94,9 @@ public:
         - RMC reports date with a two-digit year so in this case the year
           is assumed to be after the year 2000.
     */
-    Q_AUTOTEST_EXPORT static bool getPosInfoFromNmea(const char *data, int size, QGeoPositionInfo *info, bool *hasFix = 0);
+    Q_AUTOTEST_EXPORT static bool getPosInfoFromNmea(const char *data, int size,
+                                                     QGeoPositionInfo *info, double uere,
+                                                     bool *hasFix = 0);
 
     /*
         Returns true if the given NMEA sentence has a valid checksum.
diff --git a/src/positioning/qnmeapositioninfosource.cpp b/src/positioning/qnmeapositioninfosource.cpp
index 9ac928b2..23706a4f 100644
--- a/src/positioning/qnmeapositioninfosource.cpp
+++ b/src/positioning/qnmeapositioninfosource.cpp
@@ -1,5 +1,7 @@
 /****************************************************************************
 **
+** Copyright (C) 2013 Jolla Ltd.
+** Contact: Aaron McCarthy <aaron.mccarthy@jollamobile.com>
 ** Copyright (C) 2013 Digia Plc and/or its subsidiary(-ies).
 ** Contact: http://www.qt-project.org/legal
 **
@@ -45,6 +47,7 @@
 #include <QBasicTimer>
 #include <QTimerEvent>
 #include <QTimer>
+#include <QtCore/QtNumeric>
 
 
 QT_BEGIN_NAMESPACE
@@ -133,8 +136,7 @@ void QNmeaSimulatedReader::simulatePendingUpdate()
     if (m_pendingUpdates.size() > 0) {
         // will be dequeued in processNextSentence()
         QPendingGeoPositionInfo &pending = m_pendingUpdates.head();
-        if (pending.info.coordinate().type() != QGeoCoordinate::InvalidCoordinate)
-            m_proxy->notifyNewUpdate(&pending.info, pending.hasFix);
+        m_proxy->notifyNewUpdate(&pending.info, pending.hasFix);
     }
 
     processNextSentence();
@@ -173,6 +175,9 @@ void QNmeaSimulatedReader::processNextSentence()
                 timeToNextUpdate = prevTime.msecsTo(time);
                 if (timeToNextUpdate >= 0)
                     break;
+            } else {
+                timeToNextUpdate = 0;
+                break;
             }
         }
     }
@@ -199,10 +204,13 @@ QNmeaPositionInfoSourcePrivate::QNmeaPositionInfoSourcePrivate(QNmeaPositionInfo
         m_device(0),
         m_invokedStart(false),
         m_positionError(QGeoPositionInfoSource::UnknownSourceError),
+        m_userEquivalentRangeError(qQNaN()),
         m_source(parent),
         m_nmeaReader(0),
         m_updateTimer(0),
         m_requestTimer(0),
+        m_horizontalAccuracy(qQNaN()),
+        m_verticalAccuracy(qQNaN()),
         m_noUpdateLastInterval(false),
         m_updateTimeoutSent(false),
         m_connectedReadyRead(false)
@@ -375,6 +383,17 @@ void QNmeaPositionInfoSourcePrivate::notifyNewUpdate(QGeoPositionInfo *update, b
             update->setTimestamp(QDateTime(m_currentDate, time, Qt::UTC));
     }
 
+    // Some attributes are sent in separate NMEA sentences. Save and restore the accuracy
+    // measurements.
+    if (update->hasAttribute(QGeoPositionInfo::HorizontalAccuracy))
+        m_horizontalAccuracy = update->attribute(QGeoPositionInfo::HorizontalAccuracy);
+    else if (!qIsNaN(m_horizontalAccuracy))
+        update->setAttribute(QGeoPositionInfo::HorizontalAccuracy, m_horizontalAccuracy);
+    if (update->hasAttribute(QGeoPositionInfo::VerticalAccuracy))
+        m_verticalAccuracy = update->attribute(QGeoPositionInfo::VerticalAccuracy);
+    else if (!qIsNaN(m_verticalAccuracy))
+        update->setAttribute(QGeoPositionInfo::VerticalAccuracy, m_verticalAccuracy);
+
     if (hasFix && update->isValid()) {
         if (m_requestTimer && m_requestTimer->isActive()) {
             m_requestTimer->stop();
@@ -451,6 +470,10 @@ void QNmeaPositionInfoSourcePrivate::emitUpdated(const QGeoPositionInfo &update)
 
     In both cases the position information is received via the positionUpdated() signal and the
     last known position can be accessed with lastKnownPosition().
+
+    QNmeaPositionInfoSource supports reporting the accuracy of the horizontal and vertical position.
+    To enable position accuracy reporting an estimate of the User Equivalent Range Error associated
+    with the NMEA source must be set with setUserEquivalentRangeError().
 */
 
 
@@ -482,6 +505,41 @@ QNmeaPositionInfoSource::~QNmeaPositionInfoSource()
 }
 
 /*!
+    Sets the User Equivalent Range Error (UERE) to \a uere. The UERE is used in calculating an
+    estimate of the accuracy of the position information reported by the position info source. The
+    UERE should be set to a value appropriate for the GPS device which generated the NMEA stream.
+
+    The true UERE value is calculated from multiple error sources including errors introduced by
+    the satellites and signal propogation delays through the atmosphere as well as errors
+    introduced by the receiving GPS equipment. For details on GPS accuracy see
+    \l {http://edu-observatory.org/gps/gps_accuracy.html}.
+
+    A typical value for UERE is approximately 5.1.
+
+    \since 5.3
+
+    \sa userEquivalentRangeError()
+*/
+void QNmeaPositionInfoSource::setUserEquivalentRangeError(double uere)
+{
+    d->m_userEquivalentRangeError = uere;
+}
+
+/*!
+    Returns the current User Equivalent Range Error (UERE). The UERE is used in calculating an
+    estimate of the accuracy of the position information reported by the position info source. The
+    default value is NaN which means no accuracy information will be provided.
+
+    \since 5.3
+
+    \sa setUserEquivalentRangeError()
+*/
+double QNmeaPositionInfoSource::userEquivalentRangeError() const
+{
+    return d->m_userEquivalentRangeError;
+}
+
+/*!
     Parses an NMEA sentence string into a QGeoPositionInfo.
 
     The default implementation will parse standard NMEA sentences.
@@ -498,7 +556,8 @@ QNmeaPositionInfoSource::~QNmeaPositionInfoSource()
 bool QNmeaPositionInfoSource::parsePosInfoFromNmeaData(const char *data, int size,
         QGeoPositionInfo *posInfo, bool *hasFix)
 {
-    return QLocationUtils::getPosInfoFromNmea(data, size, posInfo, hasFix);
+    return QLocationUtils::getPosInfoFromNmea(data, size, posInfo, d->m_userEquivalentRangeError,
+                                              hasFix);
 }
 
 /*!
diff --git a/src/positioning/qnmeapositioninfosource.h b/src/positioning/qnmeapositioninfosource.h
index 605a5696..ae19309f 100644
--- a/src/positioning/qnmeapositioninfosource.h
+++ b/src/positioning/qnmeapositioninfosource.h
@@ -60,6 +60,9 @@ public:
     explicit QNmeaPositionInfoSource(UpdateMode updateMode, QObject *parent = 0);
     ~QNmeaPositionInfoSource();
 
+    void setUserEquivalentRangeError(double uere);
+    double userEquivalentRangeError() const;
+
     UpdateMode updateMode() const;
 
     void setDevice(QIODevice *source);
diff --git a/src/positioning/qnmeapositioninfosource_p.h b/src/positioning/qnmeapositioninfosource_p.h
index ec9ebeaf..afe0b3aa 100644
--- a/src/positioning/qnmeapositioninfosource_p.h
+++ b/src/positioning/qnmeapositioninfosource_p.h
@@ -96,6 +96,7 @@ public:
     QGeoPositionInfo m_lastUpdate;
     bool m_invokedStart;
     QGeoPositionInfoSource::Error m_positionError;
+    double m_userEquivalentRangeError;
 
 public Q_SLOTS:
     void readyRead();
@@ -120,6 +121,8 @@ private:
     QGeoPositionInfo m_pendingUpdate;
     QDate m_currentDate;
     QTimer *m_requestTimer;
+    qreal m_horizontalAccuracy;
+    qreal m_verticalAccuracy;
     bool m_noUpdateLastInterval;
     bool m_updateTimeoutSent;
     bool m_connectedReadyRead;
diff --git a/tests/auto/qnmeapositioninfosource/tst_qnmeapositioninfosource.cpp b/tests/auto/qnmeapositioninfosource/tst_qnmeapositioninfosource.cpp
index 4b5f4ff6..c0a57535 100644
--- a/tests/auto/qnmeapositioninfosource/tst_qnmeapositioninfosource.cpp
+++ b/tests/auto/qnmeapositioninfosource/tst_qnmeapositioninfosource.cpp
@@ -1,5 +1,7 @@
 /****************************************************************************
 **
+** Copyright (C) 2013 Jolla Ltd.
+** Contact: Aaron McCarthy <aaron.mccarthy@jollamobile.com>
 ** Copyright (C) 2013 Digia Plc and/or its subsidiary(-ies).
 ** Contact: http://www.qt-project.org/legal
 **
@@ -43,6 +45,8 @@
 
 #include "tst_qnmeapositioninfosource.h"
 
+#include <QtCore/QtNumeric>
+
 #ifdef Q_OS_WIN
 
 // Windows seems to require longer timeouts and step length
@@ -103,6 +107,14 @@ void tst_QNmeaPositionInfoSource::minimumUpdateInterval()
     QCOMPARE(source.minimumUpdateInterval(), 100);
 }
 
+void tst_QNmeaPositionInfoSource::userEquivalentRangeError()
+{
+    QNmeaPositionInfoSource source(m_mode);
+    QVERIFY(qIsNaN(source.userEquivalentRangeError()));
+    source.setUserEquivalentRangeError(5.1);
+    QVERIFY(qFuzzyCompare(source.userEquivalentRangeError(), 5.1));
+}
+
 void tst_QNmeaPositionInfoSource::setUpdateInterval_delayedUpdate()
 {
     // If an update interval is set, and an update is not available at a
@@ -348,8 +360,11 @@ void tst_QNmeaPositionInfoSource::startUpdates_waitForValidDateTime()
 
     QFETCH(QByteArray, bytes);
     QFETCH(QList<QDateTime>, dateTimes);
+    QFETCH(QList<bool>, expectHorizontalAccuracy);
+    QFETCH(QList<bool>, expectVerticalAccuracy);
 
     QNmeaPositionInfoSource source(m_mode);
+    source.setUserEquivalentRangeError(5.1);
     QNmeaPositionInfoSourceProxyFactory factory;
     QNmeaPositionInfoSourceProxy *proxy = static_cast<QNmeaPositionInfoSourceProxy*>(factory.createProxy(&source));
 
@@ -359,14 +374,33 @@ void tst_QNmeaPositionInfoSource::startUpdates_waitForValidDateTime()
     proxy->feedBytes(bytes);
     QTRY_COMPARE(spy.count(), dateTimes.count());
 
-    for (int i=0; i<spy.count(); i++)
-        QCOMPARE(spy[i][0].value<QGeoPositionInfo>().timestamp(), dateTimes[i]);
+    for (int i=0; i<spy.count(); i++) {
+        QGeoPositionInfo pInfo = spy[i][0].value<QGeoPositionInfo>();
+
+        QCOMPARE(pInfo.timestamp(), dateTimes[i]);
+
+        // Generated GGA/GSA sentences have hard coded HDOP of 3.5, which corrisponds to a
+        // horizontal accuracy of 35.7, for the user equivalent range error of 5.1 set above.
+        QCOMPARE(pInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy),
+                 expectHorizontalAccuracy[i]);
+        if (pInfo.hasAttribute(QGeoPositionInfo::HorizontalAccuracy))
+            QVERIFY(qFuzzyCompare(pInfo.attribute(QGeoPositionInfo::HorizontalAccuracy), 35.7));
+
+        // Generate GSA sentences have hard coded VDOP of 4.0, which corrisponds to a vertical
+        // accuracy of 40.8, for the user equivalent range error of 5.1 set above.
+        QCOMPARE(pInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy),
+                 expectVerticalAccuracy[i]);
+        if (pInfo.hasAttribute(QGeoPositionInfo::VerticalAccuracy))
+            QVERIFY(qFuzzyCompare(pInfo.attribute(QGeoPositionInfo::VerticalAccuracy), 40.8));
+    }
 }
 
 void tst_QNmeaPositionInfoSource::startUpdates_waitForValidDateTime_data()
 {
     QTest::addColumn<QByteArray>("bytes");
     QTest::addColumn<QList<QDateTime> >("dateTimes");
+    QTest::addColumn<QList<bool> >("expectHorizontalAccuracy");
+    QTest::addColumn<QList<bool> >("expectVerticalAccuracy");
 
     QDateTime dt = QDateTime::currentDateTime().toUTC();
     QByteArray bytes;
@@ -376,7 +410,9 @@ void tst_QNmeaPositionInfoSource::startUpdates_waitForValidDateTime_data()
     bytes += QLocationTestUtils::createRmcSentence(dt.addSecs(2)).toLatin1();
     bytes += QLocationTestUtils::createGgaSentence(dt.addSecs(3).time()).toLatin1();
     QTest::newRow("Feed GGA,RMC,GGA; expect RMC, second GGA only")
-            << bytes << (QList<QDateTime>() << dt.addSecs(2) << dt.addSecs(3));
+            << bytes << (QList<QDateTime>() << dt.addSecs(2) << dt.addSecs(3))
+            << (QList<bool>() << true << true)
+            << (QList<bool>() << false << false);
 
     // should not receive ZDA (has no coordinates) but should get the GGA
     // sentence after it since it got the date/time from ZDA
@@ -385,7 +421,20 @@ void tst_QNmeaPositionInfoSource::startUpdates_waitForValidDateTime_data()
     bytes += QLocationTestUtils::createZdaSentence(dt.addSecs(2)).toLatin1();
     bytes += QLocationTestUtils::createGgaSentence(dt.addSecs(3).time()).toLatin1();
     QTest::newRow("Feed GGA,ZDA,GGA; expect second GGA only")
-            << bytes << (QList<QDateTime>() << dt.addSecs(3));
+            << bytes << (QList<QDateTime>() << dt.addSecs(3))
+            << (QList<bool>() << true)
+            << (QList<bool>() << false);
+
+    // Feed ZDA,GGA,GSA,GGA; expect vertical accuracy from second GGA.
+    bytes.clear();
+    bytes += QLocationTestUtils::createZdaSentence(dt.addSecs(1)).toLatin1();
+    bytes += QLocationTestUtils::createGgaSentence(dt.addSecs(2).time()).toLatin1();
+    bytes += QLocationTestUtils::createGsaSentence().toLatin1();
+    bytes += QLocationTestUtils::createGgaSentence(dt.addSecs(3).time()).toLatin1();
+    QTest::newRow("Feed ZDA,GGA,GSA,GGA; expect vertical accuracy from second GGA")
+            << bytes << (QList<QDateTime>() << dt.addSecs(2) << dt.addSecs(3))
+            << (QList<bool>() << true << true)
+            << (QList<bool>() << false << true);
 
     if (m_mode == QNmeaPositionInfoSource::SimulationMode) {
         // In sim m_mode, should ignore sentence with a date/time before the known date/time
@@ -395,7 +444,9 @@ void tst_QNmeaPositionInfoSource::startUpdates_waitForValidDateTime_data()
         bytes += QLocationTestUtils::createRmcSentence(dt.addSecs(-2)).toLatin1();
         bytes += QLocationTestUtils::createRmcSentence(dt.addSecs(2)).toLatin1();
         QTest::newRow("Feed good RMC, RMC with bad date/time, good RMC; expect first and third RMC only")
-                << bytes << (QList<QDateTime>() << dt.addSecs(1) << dt.addSecs(2));
+                << bytes << (QList<QDateTime>() << dt.addSecs(1) << dt.addSecs(2))
+                << (QList<bool>() << false << false)
+                << (QList<bool>() << false << false);
     }
 }
 
diff --git a/tests/auto/qnmeapositioninfosource/tst_qnmeapositioninfosource.h b/tests/auto/qnmeapositioninfosource/tst_qnmeapositioninfosource.h
index d541465d..9d837933 100644
--- a/tests/auto/qnmeapositioninfosource/tst_qnmeapositioninfosource.h
+++ b/tests/auto/qnmeapositioninfosource/tst_qnmeapositioninfosource.h
@@ -96,6 +96,8 @@ private slots:
 
     void minimumUpdateInterval();
 
+    void userEquivalentRangeError();
+
     void setUpdateInterval_delayedUpdate();
 
     void lastKnownPosition();
diff --git a/tests/auto/utils/qlocationtestutils.cpp b/tests/auto/utils/qlocationtestutils.cpp
index 97e4f8d9..d5eed344 100644
--- a/tests/auto/utils/qlocationtestutils.cpp
+++ b/tests/auto/utils/qlocationtestutils.cpp
@@ -93,3 +93,8 @@ QString QLocationTestUtils::createZdaSentence(const QDateTime &dt)
         .arg(time).arg(dt.toString("dd")).arg(dt.toString("MM")).arg(dt.toString("yyyy"));
     return addNmeaChecksumAndBreaks(nmea);
 }
+
+QString QLocationTestUtils::createGsaSentence()
+{
+    return addNmeaChecksumAndBreaks(QStringLiteral("$GPGSA,A,3,,,,,,,,,,,,,3.0,3.5,4.0*"));
+}
diff --git a/tests/auto/utils/qlocationtestutils_p.h b/tests/auto/utils/qlocationtestutils_p.h
index 2c827d41..6ed5de38 100644
--- a/tests/auto/utils/qlocationtestutils_p.h
+++ b/tests/auto/utils/qlocationtestutils_p.h
@@ -57,6 +57,7 @@ namespace QLocationTestUtils
     QString createGgaSentence(const QTime &time);
     QString createGgaSentence(int lat, int lng, const QTime &time);
     QString createZdaSentence(const QDateTime &dt);
+    QString createGsaSentence();
 
     //The purpose of compareEquality() is to test equality
     //operators where it is expected that A == B.