75 lines
2.0 KiB
C++
75 lines
2.0 KiB
C++
#include "obd2_kline.h"
|
|
|
|
// === Konstante für Anfrageintervalle ===
|
|
#define OBD2_QUERY_INTERVAL 500 // alle 500 ms neue Anfrage
|
|
|
|
// === Private Variablen ===
|
|
static Stream *klineSerial = nullptr;
|
|
static uint32_t lastQueryTime = 0;
|
|
static uint32_t lastRecvTime = 0;
|
|
static uint32_t lastSpeedMMperSec = 0;
|
|
|
|
// === Slow Init nach ISO9141-2 ===
|
|
void OBD2_KLine_SlowInit()
|
|
{
|
|
pinMode(1, OUTPUT); // TXD-Pin (z.B. D1)
|
|
digitalWrite(1, HIGH);
|
|
delay(3000);
|
|
|
|
digitalWrite(1, LOW);
|
|
delay(200); // 200ms
|
|
digitalWrite(1, HIGH);
|
|
delay(400); // 400ms
|
|
digitalWrite(1, LOW);
|
|
delay(400); // 400ms
|
|
digitalWrite(1, HIGH);
|
|
delay(200); // 200ms
|
|
|
|
// zurück auf Serialbetrieb
|
|
pinMode(1, INPUT);
|
|
delay(300);
|
|
}
|
|
|
|
// === Initialisierung ===
|
|
void Init_OBD2_KLine(Stream &serial)
|
|
{
|
|
klineSerial = &serial;
|
|
OBD2_KLine_SlowInit();
|
|
delay(100);
|
|
|
|
// Sende 01 0D (Vehicle Speed)
|
|
byte speedRequest[] = {0x68, 0x6A, 0xF1, 0x01, 0x0D}; // OBD2 PID-Request
|
|
klineSerial->write(speedRequest, sizeof(speedRequest));
|
|
}
|
|
|
|
// === Geschwindigkeit abfragen ===
|
|
uint32_t Process_OBD2_KLine_Speed()
|
|
{
|
|
if (!klineSerial || (millis() - lastQueryTime < OBD2_QUERY_INTERVAL)) return 0;
|
|
|
|
byte req[] = {0x68, 0x6A, 0xF1, 0x01, 0x0D};
|
|
klineSerial->write(req, sizeof(req));
|
|
lastQueryTime = millis();
|
|
|
|
uint8_t buf[16];
|
|
size_t len = klineSerial->readBytes(buf, sizeof(buf));
|
|
for (size_t i = 0; i < len - 2; ++i)
|
|
{
|
|
if (buf[i] == 0x48 && buf[i + 1] == 0x6B && buf[i + 2] == 0x10)
|
|
{
|
|
if (buf[i + 3] == 0x41 && buf[i + 4] == 0x0D)
|
|
{
|
|
uint8_t speed_kmh = buf[i + 5];
|
|
uint32_t speed_mm_per_sec = (uint32_t)speed_kmh * 1000000 / 3600;
|
|
uint32_t dt = millis() - lastRecvTime;
|
|
lastRecvTime = millis();
|
|
lastSpeedMMperSec = speed_mm_per_sec;
|
|
|
|
return (speed_mm_per_sec * dt) / 1000;
|
|
}
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|