reworked CAN Stack

Software/src/can_native.cpp

@@ -0,0 +1,143 @@
+#include "can_native.h"
+#include "globals.h" // für LubeConfig, etc.
+#include "dtc.h"
+#include "debugger.h"
+
+// ===== Bike-spezifische Konstanten =====
+// Faktor zur Umrechnung der Rohdaten -> km/h (aus deinem bisherigen Code)
+static constexpr uint16_t FACTOR_RWP_KMH_890ADV = 18;
+static constexpr uint16_t FACTOR_RWP_KMH_1290SD = 18;
+
+// Erwartete CAN-ID(s) für die genutzten Bikes (11-bit)
+static constexpr uint16_t ID_KTM_REAR_WHEEL = 0x12D; // aus deinem Filter-Setup
+
+// ===== Interner Status =====
+static uint32_t s_lastIntegrateMs = 0;
+static uint32_t s_lastRxMs = 0;       // für DTC_NO_CAN_SIGNAL
+static uint32_t s_lastSpeed_mmps = 0; // mm pro Sekunde (Rear Wheel)
+
+// Hilfsfunktion: aus km/h -> mm/s
+static inline uint32_t kmh_to_mmps(uint16_t kmh)
+{
+    // 1 km/h = 1'000'000 mm / 3600 s
+    return (uint32_t)kmh * 1000000UL / 3600UL;
+}
+
+// Hilfsfunktion: aus Rohdaten -> mm/s je nach Bike-Konfiguration
+static uint32_t parse_speed_mmps_from_frame(uint8_t dlc, const uint8_t data[8])
+{
+    if (dlc < 7)
+        return 0; // wir brauchen data[5] & data[6]
+    uint16_t raw = (uint16_t)data[5] << 8 | data[6];
+
+    switch (LubeConfig.CANSource)
+    {
+    case KTM_890_ADV_R_2021:
+        // (raw / FACTOR) km/h -> mm/s
+        // Deine Kommentare: raw * 500 -> cm/s — hier sauber über kmh_to_mmps
+        return (((uint32_t)raw * 1000000UL) / FACTOR_RWP_KMH_890ADV) / 3600UL;
+
+    case KTM_1290_SD_R_2023:
+        return (((uint32_t)raw * 1000000UL) / FACTOR_RWP_KMH_1290SD) / 3600UL;
+
+    default:
+        return 0;
+    }
+}
+
+bool Init_CAN_Native()
+{
+    // 1) HAL bereitstellen (Selftest inklusive). Nur initialisieren, wenn noch nicht ready.
+    if (!CAN_HAL_IsReady())
+    {
+        CanHalConfig cfg;
+        cfg.baud = CAN_500KBPS;
+        cfg.clock = MCP_16MHZ;
+        cfg.listenOnlyProbeMs = 50; // kurzer, unkritischer „Bus lebt?“-Blick
+
+        if (!CAN_HAL_Init(cfg))
+        {
+            // Hardware/Selftest failed → native Pfad nicht nutzbar
+            MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true);
+            Debug_pushMessage("CAN(Native): HAL init failed\n");
+            return false;
+        }
+    }
+
+    // 2) Masken/Filter setzen
+    CAN_HAL_SetStdMask11(0, 0x7FF);
+    CAN_HAL_SetStdMask11(1, 0x7FF);
+
+    CanFilter flist[1] = {{ID_KTM_REAR_WHEEL, false}};
+    CAN_HAL_SetFilters(flist, 1);
+
+    CAN_HAL_SetMode(MCP_NORMAL);
+
+    // 3) Startzustand/Flags
+    MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, false);
+    // DTC_NO_CAN_SIGNAL wird in Process_* verwaltet
+
+    // 4) Status resetten
+    s_lastIntegrateMs = millis();
+    s_lastRxMs = 0;
+    s_lastSpeed_mmps = 0;
+
+    Debug_pushMessage("CAN(Native): Filters set (ID=0x%03X), NORMAL mode\n", ID_KTM_REAR_WHEEL);
+    return true;
+}
+
+uint32_t Process_CAN_Native_WheelSpeed()
+{
+    const uint32_t now = millis();
+    uint32_t add_mm = 0;
+
+    // 1) Frames non-blocking ziehen und relevante verarbeiten
+    for (uint8_t i = 0; i < 6; ++i)
+    { // kleine Obergrenze gegen Busy-Loops
+        unsigned long id;
+        uint8_t dlc;
+        uint8_t buf[8];
+        if (!CAN_HAL_Read(id, dlc, buf))
+            break;
+
+        // Wir erwarten 11-bit 0x12D (Filter sind gesetzt, aber doppelter Boden schadet nicht)
+        if (id == ID_KTM_REAR_WHEEL)
+        {
+            s_lastSpeed_mmps = parse_speed_mmps_from_frame(dlc, buf);
+            s_lastRxMs = now;
+            // Kein "break": falls mehrere Frames in der Queue sind, nehmen wir das letzte als aktuellsten
+        }
+    }
+
+    // 2) CAN-Heartbeat -> DTC_NO_CAN_SIGNAL (Warnung, wenn >10s nix mehr kam)
+    if (s_lastRxMs != 0)
+    {
+        const bool stale = (now - s_lastRxMs) > 10000UL;
+        MaintainDTC(DTC_NO_CAN_SIGNAL, stale);
+    }
+    else
+    {
+        // Seit Start noch kein Frame gesehen -> noch nicht entscheiden, DTC-Logik darf warten
+        // Optional: nach 1s ohne Frames Warnung setzen
+        static uint32_t t0 = now;
+        if (now - t0 > 1000UL)
+        {
+            MaintainDTC(DTC_NO_CAN_SIGNAL, true);
+        }
+    }
+
+    // 3) Integration der Distanz (mm) über dt
+    if (s_lastIntegrateMs == 0)
+        s_lastIntegrateMs = now;
+    const uint32_t dt_ms = now - s_lastIntegrateMs;
+    s_lastIntegrateMs = now;
+
+    // Wenn seit 600 ms keine neue Geschwindigkeit kam, setze v -> 0 (Stale-Schutz)
+    const bool speedStale = (s_lastRxMs == 0) || ((now - s_lastRxMs) > 600UL);
+    const uint32_t v_mmps = speedStale ? 0 : s_lastSpeed_mmps;
+
+    // mm = (mm/s * ms) / 1000
+    add_mm = (uint64_t)v_mmps * dt_ms / 1000ULL;
+
+    return add_mm;
+}