Lock map tracking to corrected IMU output

src/MapWidget.cpp

@@ -91,9 +91,9 @@ void MapWidget::setPoints(const QVector<TelemetryPoint> &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 &focus = m_points[preferredFocusIndex()];
+        m_centerLat = focus.latitude;
+        m_centerLon = focus.longitude;
         if (wasEmpty) {
             m_zoom = MaxZoom;
         }
@@ -232,9 +232,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 &focus = m_points[preferredFocusIndex()];
+        m_centerLat = focus.latitude;
+        m_centerLon = focus.longitude;
         update();
     }
     emit autoFollowChanged(m_autoFollow);
@@ -522,9 +522,10 @@ 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 focusIndex = preferredFocusIndex();
+    for (int i = 0; i < m_points.size(); ++i) {
+        if (!m_showRecentPoints && i != focusIndex) continue;
+        const bool isLast = i == focusIndex;
         const QPointF screen = coordinateToScreen(m_points[i].latitude, m_points[i].longitude);
         const double radius = isLast ? 8.0 : 5.0;
 
@@ -547,7 +548,7 @@ void MapWidget::drawTelemetry(QPainter &painter)
         }
     }
 
-    const TelemetryPoint &last = m_points.last();
+    const TelemetryPoint &last = m_points[focusIndex];
     const QPointF lastScreen = coordinateToScreen(last.latitude, last.longitude);
     QStringList lines;
     const QString latitudeText = last.latitudeText.isEmpty()
@@ -655,9 +656,10 @@ void MapWidget::drawCepMarker(QPainter &painter)
     painter.drawLine(QPointF(screen.x(), screen.y() - 11.0),
                      QPointF(screen.x(), screen.y() + 11.0));
 
-    const bool overlapsLast = !m_points.isEmpty()
-        && QLineF(screen, coordinateToScreen(m_points.last().latitude,
-                                             m_points.last().longitude)).length() < 30.0;
+    const int focusIndex = preferredFocusIndex();
+    const bool overlapsLast = focusIndex >= 0
+        && QLineF(screen, coordinateToScreen(m_points[focusIndex].latitude,
+                                             m_points[focusIndex].longitude)).length() < 30.0;
     if (!overlapsLast) {
         painter.setFont(QFont(painter.font().family(), 9, QFont::Bold));
         painter.setPen(QColor(20, 83, 78));
@@ -691,9 +693,10 @@ void MapWidget::drawAverageOverlay(QPainter &painter)
     painter.drawLine(QPointF(screen.x() - 12.0, screen.y()), QPointF(screen.x() + 12.0, screen.y()));
     painter.drawLine(QPointF(screen.x(), screen.y() - 12.0), QPointF(screen.x(), screen.y() + 12.0));
 
-    const bool overlapsLast = !m_points.isEmpty()
-        && QLineF(screen, coordinateToScreen(m_points.last().latitude,
-                                             m_points.last().longitude)).length() < 30.0;
+    const int focusIndex = preferredFocusIndex();
+    const bool overlapsLast = focusIndex >= 0
+        && QLineF(screen, coordinateToScreen(m_points[focusIndex].latitude,
+                                             m_points[focusIndex].longitude)).length() < 30.0;
     if (!overlapsLast) {
         painter.setFont(QFont(painter.font().family(), 9, QFont::Bold));
         const QRectF labelRect(screen + QPointF(12.0, 8.0), QSizeF(72.0, 20.0));
@@ -773,3 +776,14 @@ void MapWidget::disableAutoFollowFromUserAction()
     m_autoFollow = false;
     emit autoFollowChanged(false);
 }
+
+int MapWidget::preferredFocusIndex() const
+{
+    // Once corrected ESKF output is present, both camera and detail bubble
+    // stay locked to its newest point. Raw GPS and truth records may arrive
+    // later in an interleaved stream and must not pull the camera away.
+    for (int index = m_points.size() - 1; index >= 0; --index) {
+        if (m_points[index].source == LocationSource::ImuCorrected) return index;
+    }
+    return m_points.isEmpty() ? -1 : m_points.size() - 1;
+}