From fa3f884b0e70e40d563ed31ed6426ae334a00a66 Mon Sep 17 00:00:00 2001
From: Brent Picasso <brent@autosportlabs.com>
Date: Mon, 16 May 2022 09:18:59 -0700
Subject: [PATCH] R/2.18.6 (#1112)

* fix edge case where firmware may crash if GPS data does not change (#1109)

* fix edge case where firmware may crash if GPS data does not change

* change how we interpolate longitude

* simplify gps position and speed interpolation algorithm

* reformat code

* update changelog
---
 CHANGELOG                 |  3 +++
 src/lap_stats/lap_stats.c | 38 +++++++++++++++-----------------------
 2 files changed, 18 insertions(+), 23 deletions(-)

diff --git a/CHANGELOG b/CHANGELOG
index 63008101..15da140c 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -1,3 +1,6 @@
+= 2.18.6 =
+* Fix edge case where firmware may crash if GPS data does not change while interpolating points
+
 = 2.18.4 =
 * Enable pit-to-car alert message CAN bus broadcast for app-based telemetry
 
diff --git a/src/lap_stats/lap_stats.c b/src/lap_stats/lap_stats.c
index 71b769ad..6ae94719 100644
--- a/src/lap_stats/lap_stats.c
+++ b/src/lap_stats/lap_stats.c
@@ -793,40 +793,33 @@ void lapstats_processUpdate(GpsSnapshot *gps_snapshot)
 
         /**
          * ============================================
-         * Set up interpolation for GPS Point
+         * Set up interpolation for GPS Points
          * ============================================
          */
+
+        /* Evenly split up difference in latitiude into intervals */
         float lat1 = gps_snapshot->previousPoint.latitude;
-        float lon1 = gps_snapshot->previousPoint.longitude;
         float lat2 = gps_snapshot->sample.point.latitude;
-        float lon2 = gps_snapshot->sample.point.longitude;
-
-        /**
-         * Evenly split up difference in latitiude into intervals
-         * longitude is linearly interpolated based on changing latitude
-         */
-        float lat_interval = fabsf(lat2 - lat1) / (float)interval_count;
-        if (lat1 > lat2)
-                /* Account for reverse direction */
-                lat_interval = -lat_interval;
-
+        float lat_interval = (lat2 - lat1) / (float)interval_count;
         /* Set the starting interpolated latitude */
         float interp_lat = lat1;
 
+        /* Evenly split up difference in longitude into intervals */
+        float lon1 = gps_snapshot->previousPoint.longitude;
+        float lon2 = gps_snapshot->sample.point.longitude;
+        float lon_interval = (lon2 - lon1) / (float)interval_count;
+        /* Set the starting interpolated longitude */
+        float interp_lon = lon1;
+
         /**
          * ============================================
          * Set up interpolation for Speed
          * ============================================
          */
+        /* evenly split up changes in speed based on the interval */
         float speed1 = gps_snapshot->previous_speed;
         float speed2 = gps_snapshot->sample.speed;
-
-        /* evenly split up changes in speed based on the interval */
-        float speed_interval = fabsf(speed2 - speed1)/ (float)interval_count;
-        if (speed1 > speed2)
-                /* Account for reverse direction */
-                speed_interval = -speed_interval;
-
+        float speed_interval = (speed2 - speed1)/ (float)interval_count;
         /* Set the starting speed */
         float interp_speed = speed1;
 
@@ -846,13 +839,11 @@ void lapstats_processUpdate(GpsSnapshot *gps_snapshot)
                 pr_debug_float_msg("Interval count: ", interval_count);
                 pr_debug_float_msg("Speed interval: ", speed_interval);
                 pr_debug_float_msg("Lat. interval:  ", lat_interval);
+                pr_debug_float_msg("Lon. interval:  ", lon_interval);
                 pr_debug_int_msg  ("Time interval:  ", time_interval);
         }
 
         for (size_t i = 0; i < interval_count; i++) {
-                /* Linearly interpolate longitude from latitude */
-                float interp_lon = lon1 + (interp_lat - lat1) * ((lon2 - lon1) / (lat2 - lat1));
-
                 /* Update current GPS snapshot with interpolated values */
                 gps_snapshot->sample.point.latitude = interp_lat;
                 gps_snapshot->sample.point.longitude = interp_lon;
@@ -870,6 +861,7 @@ void lapstats_processUpdate(GpsSnapshot *gps_snapshot)
                 gps_snapshot->previousPoint = gps_snapshot->sample.point;
 
                 interp_lat += lat_interval;
+                interp_lon += lon_interval;
                 interp_time += time_interval;
                 interp_delta_ff += time_interval;