#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(); } } if (millis() > lastRecTimestamp + 10000) { if (globals.systemStatus != sysStat_Shutdown) globals.systemStatus = sysStat_Error; MaintainDTC(DTC_NO_GPS_SERIAL, true); } else { if (globals.systemStatus != sysStat_Shutdown) globals.systemStatus = globals.resumeStatus; MaintainDTC(DTC_NO_GPS_SERIAL, false); } return 0; }