added first code for OBD2 via CAN and K-Line

2025-05-04 12:03:19 +02:00
parent b627f52272
commit 1ee326fd57
12 changed files with 277 additions and 53 deletions
--- a/Software/src/obd2_can.cpp
+++ b/Software/src/obd2_can.cpp
@@ -0,0 +1,84 @@
+#include "obd2_can.h"
+#include <mcp_can.h>
+#include <SPI.h>
+#include "common.h"
+#include "globals.h"
+#include "dtc.h"
+#include "debugger.h"
+
+// === Setup: MCP2515 CS-Pin definieren ===
+#define OBD2_CAN_CS_PIN 10
+#define OBD2_OBD_REQUEST_ID 0x7DF
+#define OBD2_OBD_RESPONSE_ID 0x7E8
+
+MCP_CAN OBD_CAN(OBD2_CAN_CS_PIN);
+
+static uint32_t lastQueryTime = 0;
+static uint32_t lastRecvTime = 0;
+static uint32_t lastSpeedMMperSec = 0;
+
+#define OBD2_QUERY_INTERVAL 500 // alle 500ms
+
+void Init_OBD2_CAN()
+{
+    if (OBD_CAN.begin(MCP_STD, CAN_500KBPS, MCP_16MHZ) != CAN_OK)
+    {
+        Serial.println("OBD2 CAN Init FAILED!");
+        return;
+    }
+
+    OBD_CAN.setMode(MCP_NORMAL);
+    delay(100);
+    Serial.println("OBD2 CAN Init OK");
+}
+
+uint32_t Process_OBD2_CAN_Speed()
+{
+    if (millis() - lastQueryTime < OBD2_QUERY_INTERVAL)
+        return 0;
+
+    lastQueryTime = millis();
+
+    // 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;
+    }
+
+    unsigned long rxId;
+    byte len = 0;
+    byte rxBuf[8];
+    uint32_t timeout = millis() + 100;
+
+    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
+
+                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;
+            }
+        }
+    }
+
+    // Keine Antwort erhalten
+    MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true);
+    Debug_pushMessage("OBD2_CAN: no response within timeout\n");
+    return 0;
+}
+