From 8b7256ed9d4a37ba35bc394c672e4eb6cc4d2dd9 Mon Sep 17 00:00:00 2001 From: onur Date: Thu, 23 Jul 2026 23:55:25 +0300 Subject: [PATCH] Prefer corrected position for map following --- README.md | 10 ++++++--- src/MainWindow.cpp | 15 +++++++++++++- src/MapWidget.cpp | 41 ++++++++++++++++++++++++++++--------- src/MapWidget.h | 1 + tests/main_window_tests.cpp | 21 +++++++++++++++++++ 5 files changed, 74 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index dcbf021..5a01471 100644 --- a/README.md +++ b/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 -only that corrected series; legacy input without a source keeps the previous behavior. +zigzags. A corrected point newer than one second is also preferred for map +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. diff --git a/src/MainWindow.cpp b/src/MainWindow.cpp index 8e861da..c63636f 100644 --- a/src/MainWindow.cpp +++ b/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); - m_globe->setReceiverLocation(point.latitude, point.longitude); + if (point.source == LocationSource::ImuCorrected || !correctedPositionFresh) { + m_globe->setReceiverLocation(point.latitude, point.longitude); + } if (m_locationTimeoutTimer != nullptr && m_serial->isOpen()) { m_locationTimeoutTimer->start(); } diff --git a/src/MapWidget.cpp b/src/MapWidget.cpp index 8df3fb4..d4ac706 100644 --- a/src/MapWidget.cpp +++ b/src/MapWidget.cpp @@ -52,6 +52,20 @@ double metersPerPixel(double latitude, int zoom) return latitudeScale * 2.0 * Pi * WebMercatorRadiusMeters / scale; } +int preferredCurrentPointIndex(const QVector &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 &points) m_points = points; if (!m_points.isEmpty() && m_autoFollow) { - const TelemetryPoint &last = m_points.last(); - m_centerLat = last.latitude; - m_centerLon = last.longitude; + const TelemetryPoint ¤t = m_points[preferredCurrentPointIndex(m_points)]; + m_centerLat = current.latitude; + 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(); - m_centerLat = last.latitude; - m_centerLon = last.longitude; + const TelemetryPoint ¤t = m_points[preferredCurrentPointIndex(m_points)]; + m_centerLat = current.latitude; + 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; - for (int i = firstPoint; i < m_points.size(); ++i) { - const bool isLast = i == m_points.size() - 1; + const int currentPointIndex = preferredCurrentPointIndex(m_points); + const int firstPoint = m_showRecentPoints ? 0 : currentPointIndex; + 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() diff --git a/src/MapWidget.h b/src/MapWidget.h index 213ba1a..5e67a52 100644 --- a/src/MapWidget.h +++ b/src/MapWidget.h @@ -27,6 +27,7 @@ public: void setCepMarker(double latitude, double longitude, const std::optional &radiusMeters); void clearCepMarker(); + [[nodiscard]] QPointF centerCoordinate() const; public slots: void setAutoFollow(bool enabled); diff --git a/tests/main_window_tests.cpp b/tests/main_window_tests.cpp index e96ed3f..c021ff9 100644 --- a/tests/main_window_tests.cpp +++ b/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"));