68 lines
1.6 KiB
C++
Raw Normal View History

#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);
2022-02-15 23:27:14 +01:00
Serial.begin(baudrate);
}
uint32_t Process_GPS_WheelSpeed()
{
static uint32_t lastRecTimestamp;
static uint32_t lastValidSpeedTimestamp;
uint16_t RearWheelSpeed_kmh;
2022-02-15 23:27:14 +01:00
while (Serial.available() > 0)
{
2022-02-15 23:27:14 +01:00
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;
}