Restructured Folders

Software/src/gps.cpp

@@ -0,0 +1,59 @@
+#ifdef FEATURE_ENABLE_GPS
+#include "gps.h"
+
+TinyGPSPlus gps;
+
+void Init_GPS()
+{
+    uint32_t baudrate;
+    switch (LubeConfig.GPSBaudRate)
+    {
+    case BAUD_9600:
+        baudrate = 9600;
+        break;
+    case BAUD_115200:
+        baudrate = 115200;
+        break;
+    default:
+        baudrate = 4800;
+        break;
+    }
+
+    Serial.printf(PSTR("Init GPS with Baud %d\n"), baudrate);
+    Serial.begin(baudrate);
+}
+
+uint32_t Process_GPS_WheelSpeed()
+{
+    static uint32_t lastRecTimestamp;
+    static uint32_t lastValidSpeedTimestamp;
+    uint16_t RearWheelSpeed_kmh;
+
+    while (Serial.available() > 0)
+    {
+        uint8_t incoming = Serial.read();
+        if (gps.encode(incoming))
+        {
+            RearWheelSpeed_kmh = gps.speed.isValid() ? gps.speed.kmph() : 0;
+
+            if (RearWheelSpeed_kmh > 0)
+            {
+                uint32_t RWP_millimeter_per_second = ((uint32_t)RearWheelSpeed_kmh * 1000000) / 3600;
+
+                uint32_t timesincelast = millis() - lastValidSpeedTimestamp;
+                lastValidSpeedTimestamp = millis();
+
+                uint32_t milimeters_to_add = (RWP_millimeter_per_second * timesincelast) / 1000;
+
+                return milimeters_to_add;
+            }
+            lastRecTimestamp = millis();
+        }
+    }
+
+    MaintainDTC(DTC_NO_GPS_SERIAL, (millis() > lastRecTimestamp + 10000));
+
+    return 0;
+}
+
+#endif