souko/Kettenoeler
1
0
Fork 0
Code Issues Pull Requests Releases 4 Wiki Activity

reworked obd2_can part

Browse Source
This commit is contained in:
Marcel Peterkau
2025-08-24 16:33:48 +02:00
parent 9cb3a61184
commit 1966705f7f
2 changed files with 180 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Software/include/obd2_can.h
View File

@@ -3,8 +3,10 @@
#include <Arduino.h>
// === Funktionen ===
// Init MCP2515 und OBD2-CAN-Poller (non-blocking)
void Init_OBD2_CAN();
uint32_t Process_OBD2_CAN_Speed();
// Verarbeitet OBD2-CAN nicht-blockierend, integriert Strecke (mm) seit letztem Aufruf.
uint32_t Process_OBD2_CAN_Speed();
#endif

228
Software/src/obd2_can.cpp
View File

@@ -6,79 +6,203 @@
#include "dtc.h"
#include "debugger.h"
// === Setup: MCP2515 CS-Pin definieren ===
// =======================
// Konfiguration
// =======================
#ifndef OBD2_CAN_CS_PIN
#define OBD2_CAN_CS_PIN 10
#define OBD2_OBD_REQUEST_ID 0x7DF
#define OBD2_OBD_RESPONSE_ID 0x7E8
#endif
MCP_CAN OBD_CAN(OBD2_CAN_CS_PIN);
// 11-bit OBD-II IDs (ISO 15765-4, üblich 0x7DF/0x7E8..0x7EF)
#define OBD2_OBD_REQUEST_ID 0x7DF
#define OBD2_OBD_RESP_BASE 0x7E8
#define OBD2_OBD_RESP_LAST 0x7EF
static uint32_t lastQueryTime = 0;
static uint32_t lastRecvTime = 0;
static uint32_t lastSpeedMMperSec = 0;
// Tuning: Poll schneller als 500ms für WheelSpeed
#ifndef OBD2_QUERY_INTERVAL_MS
#define OBD2_QUERY_INTERVAL_MS 100 // 10 Hz Pollrate
#endif
#define OBD2_QUERY_INTERVAL 500 // alle 500ms
#ifndef OBD2_RESP_TIMEOUT_MS
#define OBD2_RESP_TIMEOUT_MS 60 // max. Wartezeit auf Antwort (non-blocking überwacht)
#endif
// Wenn wir X ms keine gültige Antwort haben, wird die Geschwindigkeit als stale behandelt
#ifndef OBD2_STALE_MS
#define OBD2_STALE_MS 600 // danach Speed -> 0
#endif
// Wie viele RX-Frames pro Aufruf maximal ziehen (Begrenzung gegen Busy-Loops)
#ifndef OBD2_MAX_READS_PER_CALL
#define OBD2_MAX_READS_PER_CALL 4
#endif
// Debug-Rate-Limit
#ifndef OBD2_DEBUG_INTERVAL_MS
#define OBD2_DEBUG_INTERVAL_MS 1000
#endif
// =======================
// Internals
// =======================
static MCP_CAN OBD_CAN(OBD2_CAN_CS_PIN);
static uint32_t lastQueryTime = 0;
static uint32_t lastRespTime = 0;
static uint32_t lastIntegrateTime = 0;
static uint32_t requestDeadline = 0;
static uint32_t lastDebugTime = 0;
static uint32_t lastSpeedMMperSec = 0;
enum class ObdState : uint8_t { IDLE = 0, WAITING = 1 };
static ObdState state = ObdState::IDLE;
// =======================
// Hilfsfunktionen
// =======================
static inline bool isObdResponseId(unsigned long id) {
return (id >= OBD2_OBD_RESP_BASE) && (id <= OBD2_OBD_RESP_LAST);
}
static void setupObdFilters() {
// Für STD-IDs: Filter auf 0x7E8..0x7EF, Maske 0x7F0
// Hinweis: Signaturen des MCP_CAN libs:
// init_Mask(num, ext, mask);
// init_Filt(num, ext, filt);
// ext=0 -> Standard (11-bit)
OBD_CAN.init_Mask(0, 0, 0x7F0);
OBD_CAN.init_Filt(0, 0, 0x7E8);
OBD_CAN.init_Filt(1, 0, 0x7E9);
OBD_CAN.init_Mask(1, 0, 0x7F0);
OBD_CAN.init_Filt(2, 0, 0x7EA);
OBD_CAN.init_Filt(3, 0, 0x7EB);
OBD_CAN.init_Filt(4, 0, 0x7EC);
OBD_CAN.init_Filt(5, 0, 0x7ED);
// (0x7EE, 0x7EF fallen auch unter Maske; wenn du willst, kannst du die letzten zwei Filt-Slots umbiegen)
}
static void maybeDebug(uint32_t now, const char* fmt, ...) {
if (now - lastDebugTime < OBD2_DEBUG_INTERVAL_MS) return;
lastDebugTime = now;
va_list ap;
va_start(ap, fmt);
Debug_pushMessage(fmt, ap);
va_end(ap);
}
// =======================
// Öffentliche API
// =======================
void Init_OBD2_CAN()
{
if (OBD_CAN.begin(MCP_STD, CAN_500KBPS, MCP_16MHZ) != CAN_OK)
{
Serial.println("OBD2 CAN Init FAILED!");
return;
}
// Kein delay() hier — watchdog-freundlich.
// Standard-OBD: 500 kbit/s, 11-bit
if (OBD_CAN.begin(MCP_STD, CAN_500KBPS, MCP_16MHZ) != CAN_OK) {
Debug_pushMessage("OBD2 CAN init FAILED\n");
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
return;
}
OBD_CAN.setMode(MCP_NORMAL);
delay(100);
Serial.println("OBD2 CAN Init OK");
setupObdFilters();
OBD_CAN.setMode(MCP_NORMAL);
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, false);
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true); // bis erste Antwort kommt
Debug_pushMessage("OBD2 CAN init OK\n");
// Timestamps zurücksetzen
uint32_t now = millis();
lastQueryTime = now;
lastRespTime = 0;
lastIntegrateTime = now;
requestDeadline = 0;
state = ObdState::IDLE;
}
uint32_t Process_OBD2_CAN_Speed()
{
if (millis() - lastQueryTime < OBD2_QUERY_INTERVAL)
return 0;
const uint32_t now = millis();
lastQueryTime = millis();
// 1) Nicht-blockierende Query: nur senden, wenn es Zeit ist und keine offene Anfrage wartet
if (state == ObdState::IDLE && (now - lastQueryTime) >= OBD2_QUERY_INTERVAL_MS) {
byte req[8] = {0x02, 0x01, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00}; // Mode 01, PID 0x0D (Speed)
byte stat = OBD_CAN.sendMsgBuf(OBD2_OBD_REQUEST_ID, 0, 8, req);
lastQueryTime = now;
// Anfrage: 01 0D → Geschwindigkeit
byte obdRequest[8] = {0x02, 0x01, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00};
byte sendStat = OBD_CAN.sendMsgBuf(OBD2_OBD_REQUEST_ID, 0, 8, obdRequest);
if (sendStat != CAN_OK)
{
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
Debug_pushMessage("OBD2_CAN: send failed (%d)\n", sendStat);
return 0;
if (stat == CAN_OK) {
state = ObdState::WAITING;
requestDeadline = now + OBD2_RESP_TIMEOUT_MS;
// kein delay(), sofort zurück zu Loop
} else {
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
maybeDebug(now, "OBD2_CAN send failed (%d)\n", stat);
// kein busy-wait, wir versuchen es einfach im nächsten Zyklus wieder
}
}
// 2) Non-blocking Receive: ziehe nur wenige Frames pro Aufruf
for (uint8_t i = 0; i < OBD2_MAX_READS_PER_CALL; ++i) {
if (OBD_CAN.checkReceive() != CAN_MSGAVAIL) break;
unsigned long rxId;
byte len = 0;
byte rxBuf[8];
uint32_t timeout = millis() + 100;
OBD_CAN.readMsgBuf(&rxId, &len, rxBuf);
while (millis() < timeout)
{
if (OBD_CAN.checkReceive() == CAN_MSGAVAIL)
{
OBD_CAN.readMsgBuf(&rxId, &len, rxBuf);
if ((rxId & 0xFFF8) == OBD2_OBD_RESPONSE_ID && rxBuf[1] == 0x0D)
{
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, false); // alles ok
if (!isObdResponseId(rxId)) continue; // hart gefiltert
if (len < 4) continue; // zu kurz für 01 0D A (SPEED)
uint8_t speed_kmh = rxBuf[3];
uint32_t speed_mm_per_sec = (uint32_t)speed_kmh * 1000000 / 3600;
uint32_t dt = millis() - lastRecvTime;
lastRecvTime = millis();
lastSpeedMMperSec = speed_mm_per_sec;
Debug_pushMessage("OBD2_CAN: %d km/h (%lu mm/s)\n", speed_kmh, speed_mm_per_sec);
return (speed_mm_per_sec * dt) / 1000;
}
}
// Erwartetes Echo: 0x41 0x0D <A>
// Viele Stacks liefern 03 41 0D <A> ... wir prüfen tolerant:
uint8_t modeResp = 0, pid = 0, speedKmh = 0;
if (rxBuf[0] == 0x03 && rxBuf[1] == 0x41 && rxBuf[2] == 0x0D) {
modeResp = rxBuf[1];
pid = rxBuf[2];
speedKmh = rxBuf[3];
} else if (rxBuf[0] == 0x41 && rxBuf[1] == 0x0D) {
modeResp = rxBuf[0];
pid = rxBuf[1];
speedKmh = rxBuf[2];
} else {
continue; // nicht die erwartete Antwort
}
// Keine Antwort erhalten
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true);
Debug_pushMessage("OBD2_CAN: no response within timeout\n");
return 0;
}
if (modeResp == 0x41 && pid == 0x0D) {
// gültige Antwort
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, false);
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, false);
// mm/s = km/h * (1e6 / 3600)
const uint32_t speed_mmps = (uint32_t)speedKmh * 1000000UL / 3600UL;
lastSpeedMMperSec = speed_mmps;
lastRespTime = now;
state = ObdState::IDLE; // Anfrage bedient
maybeDebug(now, "OBD2_CAN: %u km/h (%lu mm/s)\n", speedKmh, (unsigned long)speed_mmps);
break; // eine valide Antwort reicht
}
}
// 3) Timeout von offenen Anfragen prüfen (non-blocking)
if (state == ObdState::WAITING && (int32_t)(now - requestDeadline) >= 0) {
// keine Antwort in der Zeit
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true);
state = ObdState::IDLE; // freigeben für nächsten Poll
}
// 4) Distanz-Integration (sanft, watchdog-freundlich)
if (lastIntegrateTime == 0) lastIntegrateTime = now;
uint32_t dt_ms = now - lastIntegrateTime;
lastIntegrateTime = now;
// Wenn zu lange keine Antwort, setze Speed -> 0 (kein ausuferndes dt auf Antwortbasis)
uint32_t effectiveSpeed = lastSpeedMMperSec;
if (lastRespTime == 0 || (now - lastRespTime) > OBD2_STALE_MS) {
effectiveSpeed = 0;
}
// mm = (mm/s * ms) / 1000
uint32_t add_mm = (effectiveSpeed * (uint64_t)dt_ms) / 1000ULL;
return add_mm;
}