Prefer corrected position for map following

README.md

@@ -55,10 +55,14 @@ into a single plotted point.
 
 The GPS firmware's level-1 paired protocol is source-aware. Every received
 `source=gps` location is added to the blue GPS series and every
-`source=imu_corrected` location is immediately added to the red corrected series.
+`source=imu_corrected` location is added to the red corrected series at the
+firmware-configured UART cadence.
 Paths are built independently, so alternating UART lines do not create GPS-to-IMU
-zigzags. When corrected points exist, CEP50, average and scatter calculations use
+zigzags. A corrected point newer than one second is also preferred for map
-only that corrected series; legacy input without a source keeps the previous behavior.
+auto-follow, the current-position label and the globe receiver marker. Raw GPS
+outliers remain visible in blue for diagnosis without pulling the whole view away.
+When corrected points exist, CEP50, average and scatter calculations use only that
+corrected series; legacy input without a source keeps the previous behavior.
 Synthetic `source=trajectory` points form a third independent green ground-truth
 path. The map legend identifies all three series.
 

src/MainWindow.cpp

@@ -1166,8 +1166,21 @@ void MainWindow::addTelemetryPoint(const ParsedTelemetry &telemetry)
     point.rawLine = telemetry.rawLine;
     point.receivedAt = QDateTime::currentDateTimeUtc();
 
+    constexpr qint64 CorrectedPreferenceWindowMs = 1000;
+    bool correctedPositionFresh = false;
+    for (auto existing = m_points.crbegin(); existing != m_points.crend(); ++existing) {
+        if (existing->source == LocationSource::ImuCorrected) {
+            correctedPositionFresh =
+                existing->receivedAt.msecsTo(point.receivedAt)
+                <= CorrectedPreferenceWindowMs;
+            break;
+        }
+    }
+
     m_points.push_back(point);
+    if (point.source == LocationSource::ImuCorrected || !correctedPositionFresh) {
         m_globe->setReceiverLocation(point.latitude, point.longitude);
+    }
     if (m_locationTimeoutTimer != nullptr && m_serial->isOpen()) {
         m_locationTimeoutTimer->start();
     }

src/MapWidget.cpp

@@ -52,6 +52,20 @@ double metersPerPixel(double latitude, int zoom)
     return latitudeScale * 2.0 * Pi * WebMercatorRadiusMeters / scale;
 }
 
+int preferredCurrentPointIndex(const QVector<TelemetryPoint> &points)
+{
+    constexpr qint64 CorrectedPreferenceWindowMs = 1000;
+    const int newestIndex = points.size() - 1;
+    for (int index = points.size() - 1; index >= 0; --index) {
+        if (points[index].source == LocationSource::ImuCorrected) {
+            const qint64 ageMs = points[index].receivedAt.msecsTo(
+                points[newestIndex].receivedAt);
+            return ageMs <= CorrectedPreferenceWindowMs ? index : newestIndex;
+        }
+    }
+    return newestIndex;
+}
+
 } // namespace
 
 MapWidget::MapWidget(QWidget *parent)
@@ -90,9 +104,9 @@ void MapWidget::setPoints(const QVector<TelemetryPoint> &points)
     m_points = points;
 
     if (!m_points.isEmpty() && m_autoFollow) {
-        const TelemetryPoint &last = m_points.last();
+        const TelemetryPoint &current = m_points[preferredCurrentPointIndex(m_points)];
-        m_centerLat = last.latitude;
+        m_centerLat = current.latitude;
-        m_centerLon = last.longitude;
+        m_centerLon = current.longitude;
         if (wasEmpty) {
             m_zoom = MaxZoom;
         }
@@ -191,6 +205,11 @@ void MapWidget::clearCepMarker()
     update();
 }
 
+QPointF MapWidget::centerCoordinate() const
+{
+    return QPointF(m_centerLat, m_centerLon);
+}
+
 void MapWidget::setAutoFollow(bool enabled)
 {
     if (m_autoFollow == enabled) {
@@ -199,9 +218,9 @@ void MapWidget::setAutoFollow(bool enabled)
 
     m_autoFollow = enabled;
     if (m_autoFollow && !m_points.isEmpty()) {
-        const TelemetryPoint &last = m_points.last();
+        const TelemetryPoint &current = m_points[preferredCurrentPointIndex(m_points)];
-        m_centerLat = last.latitude;
+        m_centerLat = current.latitude;
-        m_centerLon = last.longitude;
+        m_centerLon = current.longitude;
         update();
     }
     emit autoFollowChanged(m_autoFollow);
@@ -453,9 +472,11 @@ void MapWidget::drawTelemetry(QPainter &painter)
     drawCepMarker(painter);
 
     const bool drawNumbers = m_points.size() <= 150;
-    const int firstPoint = m_showRecentPoints ? 0 : m_points.size() - 1;
+    const int currentPointIndex = preferredCurrentPointIndex(m_points);
-    for (int i = firstPoint; i < m_points.size(); ++i) {
+    const int firstPoint = m_showRecentPoints ? 0 : currentPointIndex;
-        const bool isLast = i == m_points.size() - 1;
+    const int endPoint = m_showRecentPoints ? m_points.size() : currentPointIndex + 1;
+    for (int i = firstPoint; i < endPoint; ++i) {
+        const bool isLast = i == currentPointIndex;
         const QPointF screen = coordinateToScreen(m_points[i].latitude, m_points[i].longitude);
         const double radius = isLast ? 8.0 : 5.0;
 
@@ -478,7 +499,7 @@ void MapWidget::drawTelemetry(QPainter &painter)
         }
     }
 
-    const TelemetryPoint &last = m_points.last();
+    const TelemetryPoint &last = m_points[currentPointIndex];
     const QPointF lastScreen = coordinateToScreen(last.latitude, last.longitude);
     QStringList lines;
     const QString latitudeText = last.latitudeText.isEmpty()

src/MapWidget.h

@@ -27,6 +27,7 @@ public:
     void setCepMarker(double latitude, double longitude,
                       const std::optional<double> &radiusMeters);
     void clearCepMarker();
+    [[nodiscard]] QPointF centerCoordinate() const;
 
 public slots:
     void setAutoFollow(bool enabled);

tests/main_window_tests.cpp

@@ -201,6 +201,27 @@ int main(int argc, char **argv)
     assert(QMetaObject::invokeMethod(&window, "clearPoints", Qt::DirectConnection));
     maxPointsInput->setValue(10);
 
+    {
+        MapWidget followMap;
+        TelemetryPoint corrected;
+        corrected.source = LocationSource::ImuCorrected;
+        corrected.latitude = 40.1;
+        corrected.longitude = 29.1;
+        corrected.receivedAt = QDateTime::currentDateTimeUtc();
+
+        TelemetryPoint rawGps;
+        rawGps.source = LocationSource::Gps;
+        rawGps.latitude = 41.0;
+        rawGps.longitude = 30.0;
+        rawGps.receivedAt = corrected.receivedAt.addMSecs(100);
+        followMap.setPoints({corrected, rawGps});
+        assert(followMap.centerCoordinate() == QPointF(40.1, 29.1));
+
+        rawGps.receivedAt = corrected.receivedAt.addMSecs(1100);
+        followMap.setPoints({corrected, rawGps});
+        assert(followMap.centerCoordinate() == QPointF(41.0, 30.0));
+    }
+
     const QByteArray screenshotPath = qgetenv("LOCATION_PLOTTER_TEST_SCREENSHOT");
     if (!screenshotPath.isEmpty()) {
         sendLine(window, QStringLiteral("NAV solve start sats:5"));