added Triumph to native CAN

2025-08-29 23:08:27 +02:00
parent a9053997a1
commit 1126111edb
4 changed files with 155 additions and 62 deletions
--- a/Software/include/common.h
+++ b/Software/include/common.h
@@ -112,6 +112,7 @@ typedef enum CANSource_e
 {
  KTM_890_ADV_R_2021,
  KTM_1290_SD_R_2023,
+  TRIUMPH_SPEED_TWIN_1200_RS_2025,
  CANSOURCE_COUNT            // <- sentinel (must be last)
 } CANSource_t;

--- a/Software/platformio.ini
+++ b/Software/platformio.ini
@@ -21,7 +21,14 @@ upload_speed = 921600

 custom_firmware_version = 1.06

+; --- C++17 erzwingen (für if constexpr etc.) ---
+; Entferne evtl. voreingestelltes -std=gnu++11/14 aus dem Core:
+build_unflags =
+  -std=gnu++11
+  -std=gnu++14
+; Setze C++17 für alle Envs:
 build_flags =
+  -std=gnu++17
  -DWIFI_SSID_CLIENT=${wifi_cred.wifi_ssid_client}
  -DWIFI_PASSWORD_CLIENT=${wifi_cred.wifi_password_client}
  -DADMIN_PASSWORD=${wifi_cred.admin_password}
@@ -96,7 +103,6 @@ build_flags =
  -DPCB_REV=${this.custom_pcb_revision}
 board_build.ldscript = eagle.flash.4m1m.ld

-
 [env:pcb_rev_1-2_serial]
 extends = env
 custom_pcb_revision = 2
--- a/Software/src/can_native.cpp
+++ b/Software/src/can_native.cpp
@@ -1,115 +1,209 @@
+// can_native.cpp – Mehrmodell-Setup (Integer-only), Triumph nutzt NUR Kanal B (W23)
+
 #include "can_native.h"
-#include "globals.h" // für LubeConfig, etc.
+#include "globals.h"   // enthält LubeConfig.CANSource
 #include "dtc.h"
 #include "debugger.h"

-// ===== Bike-spezifische Konstanten =====
-// Faktor zur Umrechnung der Rohdaten -> km/h (aus deinem bisherigen Code)
+// ====================== Gemeinsame Konstanten / Helpers ======================
+
+// KTM-Faktoren: raw/FACTOR -> km/h
 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
+// Triumph 0x208: Fit ≈ 0.0073 km/h/LSB -> exakt 73/10000 km/h/LSB
+// mm/s = km/h * 1_000_000 / 3600 -> 73/36 mm/s pro LSB (bei EINEM 16-Bit-Wert)
+static constexpr uint16_t TRI_MMPS_NUM        = 73;
+static constexpr uint16_t TRI_MMPS_DEN_SINGLE = 36;  // EIN Kanal (W23)

-// ===== Interner Status =====
+// Gemeinsamer Integrations-/Alive-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)
+static uint32_t s_lastRxMs        = 0;       // für DTC_NO_CAN_SIGNAL
+static uint32_t s_lastSpeed_mmps  = 0;       // aktuelle Geschwindigkeit [mm/s]

-// Hilfsfunktion: aus km/h -> mm/s
-static inline uint32_t kmh_to_mmps(uint16_t kmh)
+// mm = (mm/s * ms) / 1000
+static inline uint32_t integrate_mm(uint32_t v_mmps, uint32_t dt_ms)
 {
-    // 1 km/h = 1'000'000 mm / 3600 s
-    return (uint32_t)kmh * 1000000UL / 3600UL;
+    return (uint64_t)v_mmps * dt_ms / 1000ULL;
 }

-// 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];
+// ========================== Modell-Decoder (Integer) =========================

+// --- KTM: 11-bit ID 0x12D, Speed in data[5..6] (BE), raw/FACTOR -> km/h -> mm/s
+static uint32_t dec_ktm_rearwheel_mmps(uint8_t dlc, const uint8_t data[8], uint8_t bikeVariant /*0=890,1=1290*/)
+{
+    if (dlc < 7) return 0; // benötigt data[5], data[6]
+    const uint16_t raw = (uint16_t(data[5]) << 8) | data[6];
+
+    uint16_t factor = FACTOR_RWP_KMH_890ADV;
+    if (bikeVariant == 1) factor = FACTOR_RWP_KMH_1290SD;
+
+    // mm/s = (raw/factor) * 1_000_000 / 3600  -> reine Integer-Mathe:
+    const uint32_t num = (uint32_t)raw * 1000000UL;
+    const uint32_t kmh_times1e6 = num / factor;
+    return kmh_times1e6 / 3600UL;
+}
+
+// --- Triumph: 11-bit ID 0x208, NUR Kanal B = W23 (B2..B3, Little-Endian)
+static uint32_t dec_triumph_0x208_w23_mmps(uint8_t dlc, const uint8_t data[8], uint8_t /*unused*/)
+{
+    if (dlc < 4) return 0;
+
+    // W23 = (B2) + 256*(B3), LE
+    const uint16_t W23 = (uint16_t)data[2] | ((uint16_t)data[3] << 8);
+
+    if (W23 == 0) return 0;
+
+    // mm/s = (W23 * 73) / 36 — rundendes Integer-Divide
+    return ( (uint32_t)W23 * TRI_MMPS_NUM + (TRI_MMPS_DEN_SINGLE/2) ) / TRI_MMPS_DEN_SINGLE;
+}
+
+// ============================ Modell-Registry ================================
+struct ModelSpec
+{
+    // Erwartete 11-bit CAN-ID, min DLC, ob Extended (false=Standard)
+    uint16_t can_id;
+    uint8_t  min_dlc;
+    bool     ext;
+
+    // Decoder-Funktion → mm/s (Integer). bikeVariant: optionale Untervariante.
+    uint32_t (*decode_mmps)(uint8_t dlc, const uint8_t data[8], uint8_t bikeVariant);
+
+    // Optionaler Untervarianten-Index (z.B. 0=890ADV, 1=1290SD)
+    uint8_t bikeVariant;
+};
+
+// Konkrete Modelle (einfach erweiterbar)
+static constexpr uint16_t ID_KTM_REAR_WHEEL = 0x12D;
+static constexpr uint16_t ID_TRIUMPH_SPEED  = 0x208;
+
+static uint32_t trampoline_ktm_890(uint8_t dlc, const uint8_t data[8], uint8_t) {
+    return dec_ktm_rearwheel_mmps(dlc, data, 0);
+}
+static uint32_t trampoline_ktm_1290(uint8_t dlc, const uint8_t data[8], uint8_t) {
+    return dec_ktm_rearwheel_mmps(dlc, data, 1);
+}
+static uint32_t trampoline_triumph_w23(uint8_t dlc, const uint8_t data[8], uint8_t) {
+    return dec_triumph_0x208_w23_mmps(dlc, data, 0);
+}
+
+// getSpec(): mappt LubeConfig.CANSource → ModelSpec
+static bool getSpec(ModelSpec &out)
+{
    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;
+        out = { ID_KTM_REAR_WHEEL, 7, false, trampoline_ktm_890, 0 };
+        return true;

    case KTM_1290_SD_R_2023:
-        return (((uint32_t)raw * 1000000UL) / FACTOR_RWP_KMH_1290SD) / 3600UL;
+        out = { ID_KTM_REAR_WHEEL, 7, false, trampoline_ktm_1290, 1 };
+        return true;
+
+    case TRIUMPH_SPEED_TWIN_1200_RS_2025:
+        // Triumph nutzt NUR W23 (Hinterrad-Kanal B)
+        out = { ID_TRIUMPH_SPEED, 4, false, trampoline_triumph_w23, 0 };
+        return true;

    default:
-        return 0;
+        return false; // unbekannt → optional generisch behandeln
    }
 }

+// ============================== Initialisierung ==============================
 bool Init_CAN_Native()
 {
-    // 1) HAL bereitstellen (Selftest inklusive). Nur initialisieren, wenn noch nicht ready.
+    // HAL bereitstellen
    if (!CAN_HAL_IsReady())
    {
        CanHalConfig cfg;
        cfg.baud = CAN_500KBPS;
        cfg.clock = MCP_16MHZ;
-        cfg.listenOnlyProbeMs = 50; // kurzer, unkritischer „Bus lebt?“-Blick
+        cfg.listenOnlyProbeMs = 50;

        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
+    // Spec laden
+    ModelSpec spec;
+    const bool haveSpec = getSpec(spec);
+
+    // Masken/Filter
    CAN_HAL_SetStdMask11(0, 0x7FF);
    CAN_HAL_SetStdMask11(1, 0x7FF);

-    CanFilter flist[1] = {{ID_KTM_REAR_WHEEL, false}};
-    CAN_HAL_SetFilters(flist, 1);
+    if (haveSpec)
+    {
+        CanFilter flist[1] = { { spec.can_id, spec.ext } };
+        CAN_HAL_SetFilters(flist, 1);
+        Debug_pushMessage("CAN(Native): Filter set (ID=0x%03X, minDLC=%u)\n", spec.can_id, spec.min_dlc);
+    }
+    else
+    {
+        // Fallback: beide IDs aktivieren (KTM+Triumph), falls Quelle unbekannt
+        CanFilter flist[2] = { { ID_KTM_REAR_WHEEL, false }, { ID_TRIUMPH_SPEED, false } };
+        CAN_HAL_SetFilters(flist, 2);
+        Debug_pushMessage("CAN(Native): Fallback filters (KTM=0x%03X, TRI=0x%03X)\n", ID_KTM_REAR_WHEEL, ID_TRIUMPH_SPEED);
+    }

    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;
+    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;
 }

+// ============================== Verarbeitung ================================
 uint32_t Process_CAN_Native_WheelSpeed()
 {
    const uint32_t now = millis();
-    uint32_t add_mm = 0;
+    ModelSpec spec;
+    const bool haveSpec = getSpec(spec);

-    // 1) Frames non-blocking ziehen und relevante verarbeiten
-    for (uint8_t i = 0; i < 6; ++i)
-    { // kleine Obergrenze gegen Busy-Loops
+    // Frames non-blocking 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)
+        if (haveSpec)
        {
-            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
+            if (id == spec.can_id && dlc >= spec.min_dlc)
+            {
+                s_lastSpeed_mmps = spec.decode_mmps(dlc, buf, spec.bikeVariant);
+                s_lastRxMs = now;
+            }
+        }
+        else
+        {
+            // Fallback: KTM prüfen
+            if (id == ID_KTM_REAR_WHEEL && dlc >= 7)
+            {
+                s_lastSpeed_mmps = dec_ktm_rearwheel_mmps(dlc, buf, 0);
+                s_lastRxMs = now;
+            }
+            // Fallback: Triumph prüfen (nur W23)
+            else if (id == ID_TRIUMPH_SPEED && dlc >= 4)
+            {
+                s_lastSpeed_mmps = dec_triumph_0x208_w23_mmps(dlc, buf, 0);
+                s_lastRxMs = now;
+            }
        }
    }

-    // 2) CAN-Heartbeat -> DTC_NO_CAN_SIGNAL (Warnung, wenn >10s nix mehr kam)
+    // CAN-Heartbeat / DTC
    if (s_lastRxMs != 0)
    {
        const bool stale = (now - s_lastRxMs) > 10000UL;
@@ -117,27 +211,18 @@ uint32_t Process_CAN_Native_WheelSpeed()
    }
    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;
+    // Integration Strecke (mm)
+    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;
+    const uint32_t v_mmps = speedStale ? 0u : s_lastSpeed_mmps;

-    // mm = (mm/s * ms) / 1000
-    add_mm = (uint64_t)v_mmps * dt_ms / 1000ULL;
-
-    return add_mm;
+    return integrate_mm(v_mmps, dt_ms);
 }
--- a/Software/src/common.cpp
+++ b/Software/src/common.cpp
@@ -46,6 +46,7 @@ const char *const GPSBaudRateString[GPSBAUDRATE_COUNT] = {
 const char *const CANSourceString[CANSOURCE_COUNT] = {
    "KTM 890 Adventure R (2021)",
    "KTM 1290 Superduke R (2023)",
+    "Triumph Speed Twin 1200 RS (2025)",
 };

 // ---- Centralized, safe getters ----