Improve satellite view and fused location telemetry

tests/parser_tests.cpp

@@ -9,9 +9,39 @@
 
 namespace {
 
-bool closeTo(double left, double right)
+bool closeTo(double left, double right, double tolerance = 0.000001)
 {
-    return std::fabs(left - right) < 0.000001;
+    return std::fabs(left - right) < tolerance;
+}
+
+QString rinexField(double value)
+{
+    return QStringLiteral("%1").arg(value, 19, 'E', 12);
+}
+
+QString rinexContinuation(std::initializer_list<double> values)
+{
+    QString line(4, QLatin1Char(' '));
+    for (double value : values) line += rinexField(value);
+    return line + QLatin1Char('\n');
+}
+
+QByteArray syntheticGpsNavigation(const QDateTime &gpsEpochLabel, int gpsWeek, double toe)
+{
+    QString rinex = QStringLiteral(
+        "     3.05           N: GNSS NAV DATA    M: MIXED            RINEX VERSION / TYPE\n"
+        "                                                            END OF HEADER\n");
+    rinex += QStringLiteral("G01 %1").arg(
+        gpsEpochLabel.toString(QStringLiteral("yyyy MM dd HH mm ss")));
+    rinex += rinexField(0.0) + rinexField(0.0) + rinexField(0.0) + QLatin1Char('\n');
+    rinex += rinexContinuation({1.0, 0.0, 0.0, 0.0});
+    rinex += rinexContinuation({0.0, 0.0, 0.0, 5153.7954775});
+    rinex += rinexContinuation({toe, 0.0, 0.0, 0.0});
+    rinex += rinexContinuation({0.0, 0.0, 0.0, 0.0});
+    rinex += rinexContinuation({0.0, 0.0, static_cast<double>(gpsWeek), 0.0});
+    rinex += rinexContinuation({2.0, 0.0, 0.0, 1.0});
+    rinex += rinexContinuation({0.0, 0.0, 0.0, 0.0});
+    return rinex.toLatin1();
 }
 
 } // namespace
@@ -181,11 +211,14 @@ int main(int argc, char **argv)
 
     {
         const auto parsed = LocationParser::parseLine(QStringLiteral(
-            "[LOCATION] source=imu_corrected lat=40.7356 lon=31.5985 height=812.7 "
+            "[LOCATION] source=imu_corrected mode=dead_reckoning "
+            "lat=40.7356 lon=31.5985 height=812.7 gps_age_ms=3180 "
             "gdop=4.7 pdop=4.1 hdop=2.7 vdop=3.0 tdop=2.3 tracked=6 used=5"));
         assert(parsed.has_value());
         assert(parsed->hasLocation());
         assert(parsed->source == LocationSource::ImuCorrected);
+        assert(parsed->fusionMode == FusionMode::DeadReckoning);
+        assert(parsed->gpsAgeMs == 3180);
         assert(parsed->satellites == 6);
         assert(closeTo(*parsed->dop.hdop, 2.7));
     }
@@ -207,6 +240,25 @@ int main(int argc, char **argv)
         assert(parsed->satellites == 4);
     }
 
+    {
+        constexpr int gpsWeek = 2428;
+        constexpr double toe = 403200.0;
+        const QDateTime gpsEpoch(QDate(1980, 1, 6), QTime(0, 0), Qt::UTC);
+        const QDateTime gpsLabel = gpsEpoch.addSecs(
+            static_cast<qint64>(gpsWeek * 604800.0 + toe));
+        const QDateTime matchingUtc = gpsLabel.addSecs(-18);
+        QString error;
+        const auto satellites = RinexNavigation::parseGpsNavigation(
+            syntheticGpsNavigation(gpsLabel, gpsWeek, toe), matchingUtc, &error);
+        assert(error.isEmpty());
+        assert(satellites.size() == 1);
+        const double semiMajorAxis = 5153.7954775 * 5153.7954775;
+        const double node = -7.2921151467e-5 * toe;
+        assert(closeTo(satellites[0].x, semiMajorAxis * std::cos(node), 0.1));
+        assert(closeTo(satellites[0].y, semiMajorAxis * std::sin(node), 0.1));
+        assert(closeTo(satellites[0].z, 0.0, 0.1));
+    }
+
     const QString rinexPath = qEnvironmentVariable("LOCATION_PLOTTER_TEST_RINEX");
     if (!rinexPath.isEmpty()) {
         QFile file(rinexPath);