Prefer corrected position for map following

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();
-        m_centerLat = last.latitude;
-        m_centerLon = last.longitude;
+        const TelemetryPoint &current = 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 &current = 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()