From 1966705f7f512d263d32cb9f251e7b153324c392 Mon Sep 17 00:00:00 2001 From: Marcel Peterkau Date: Sun, 24 Aug 2025 16:33:48 +0200 Subject: [PATCH] reworked obd2_can part --- Software/include/obd2_can.h | 6 +- Software/src/obd2_can.cpp | 228 ++++++++++++++++++++++++++++-------- 2 files changed, 180 insertions(+), 54 deletions(-) diff --git a/Software/include/obd2_can.h b/Software/include/obd2_can.h index 9075fbf..4ce497e 100644 --- a/Software/include/obd2_can.h +++ b/Software/include/obd2_can.h @@ -3,8 +3,10 @@ #include -// === 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 diff --git a/Software/src/obd2_can.cpp b/Software/src/obd2_can.cpp index adacd4f..ef74812 100644 --- a/Software/src/obd2_can.cpp +++ b/Software/src/obd2_can.cpp @@ -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 + // Viele Stacks liefern 03 41 0D ... 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; +}