From c6d65f50bfed3ba1eaae9f1f2252ee2fba82c4d6 Mon Sep 17 00:00:00 2001 From: Marcel Peterkau Date: Sun, 24 Aug 2025 21:49:09 +0200 Subject: [PATCH] reworked CAN Stack --- Software/include/can.h | 37 ------ Software/include/can_hal.h | 59 +++++++++ Software/include/can_native.h | 11 ++ Software/include/can_obd2.h | 16 +++ Software/include/obd2_can.h | 12 -- Software/src/can.cpp | 184 ------------------------- Software/src/can_hal.cpp | 240 +++++++++++++++++++++++++++++++++ Software/src/can_native.cpp | 143 ++++++++++++++++++++ Software/src/can_obd2.cpp | 243 ++++++++++++++++++++++++++++++++++ Software/src/main.cpp | 15 ++- Software/src/obd2_can.cpp | 208 ----------------------------- 11 files changed, 721 insertions(+), 447 deletions(-) delete mode 100644 Software/include/can.h create mode 100644 Software/include/can_hal.h create mode 100644 Software/include/can_native.h create mode 100644 Software/include/can_obd2.h delete mode 100644 Software/include/obd2_can.h delete mode 100644 Software/src/can.cpp create mode 100644 Software/src/can_hal.cpp create mode 100644 Software/src/can_native.cpp create mode 100644 Software/src/can_obd2.cpp delete mode 100644 Software/src/obd2_can.cpp diff --git a/Software/include/can.h b/Software/include/can.h deleted file mode 100644 index 1c19d8f..0000000 --- a/Software/include/can.h +++ /dev/null @@ -1,37 +0,0 @@ -/** - * @file can.h - * - * @brief Header file for Controller Area Network (CAN) functionality in the ChainLube application. - * - * This file provides functions and structures related to Controller Area Network (CAN) - * communication for the ChainLube project. It includes functions for initializing CAN, - * processing CAN messages, and retrieving wheel speed from CAN data. - * - * @author Marcel Peterkau - * @date 09.01.2024 - */ - -#ifndef _CAN_H_ -#define _CAN_H_ - -#include -#include -#include -#include "common.h" -#include "globals.h" -#include "dtc.h" -#include "debugger.h" - -// CAN frame structure definition -struct can_frame -{ - unsigned long can_id; - uint8_t can_dlc; - uint8_t data[8] __attribute__((aligned(8))); -}; - -// Function prototypes -void Init_CAN(); -uint32_t Process_CAN_WheelSpeed(); - -#endif diff --git a/Software/include/can_hal.h b/Software/include/can_hal.h new file mode 100644 index 0000000..f74b3f3 --- /dev/null +++ b/Software/include/can_hal.h @@ -0,0 +1,59 @@ +#pragma once +#include +#include +#include "common.h" + +// ==== Board-Pin ==== +// Falls nicht bereits global definiert: +#ifndef GPIO_CS_CAN +#define GPIO_CS_CAN 5 +#endif + +// ==== Öffentliche, einzige Instanz ==== +extern MCP_CAN CAN0; + +// ==== Init-Config ==== +struct CanHalConfig { + uint8_t baud = CAN_500KBPS; // laut Lib + uint8_t clock = MCP_16MHZ; // 8/16 MHz je nach Quarz + uint16_t listenOnlyProbeMs = 0; // optionaler kurzer Hörtest + uint16_t modeSettleMs = 10; // Wartezeit für Mode-Set (Retry-Fenster) +}; + +// ==== Universeller Filter-Descriptor ==== +struct CanFilter { + uint32_t id; // 11-bit oder 29-bit Roh-ID + bool ext; // false=STD(11-bit), true=EXT(29-bit) +}; + +// ==== API ==== + +// 1) Einmalige Hardware-Initialisierung + integrierter Loopback-Selftest. +// - begin() +// - LOOPBACK senden/echo prüfen (ohne Bus) +// - optional: ListenOnly-Probe (nur Heuristik) +// - Default: Filter/Masks weit offen, NORMAL-Mode +// Rückgabe: true = bereit; false = Fehler (kein CAN verwenden) +bool CAN_HAL_Init(const CanHalConfig& cfg); + +// Ist die HAL bereit (nach Init)? +bool CAN_HAL_IsReady(); + +// Bestätigter Moduswechsel (CONFIG/NORMAL/LISTENONLY/LOOPBACK) +// true = erfolgreich; setzt bei Misserfolg DTC_CAN_TRANSCEIVER_FAILED +bool CAN_HAL_SetMode(uint8_t mode); + +// Masken/Filter +bool CAN_HAL_SetMask(uint8_t bank, bool ext, uint32_t rawMask); +bool CAN_HAL_SetStdMask11(uint8_t bank, uint16_t mask11); +void CAN_HAL_ClearFilters(); +bool CAN_HAL_AddFilter(const CanFilter& f); +bool CAN_HAL_SetFilters(const CanFilter* list, size_t count); + +// Non-blocking IO +bool CAN_HAL_Read(unsigned long& id, uint8_t& len, uint8_t data[8]); // true=Frame gelesen +uint8_t CAN_HAL_Send(unsigned long id, bool ext, uint8_t len, const uint8_t* data); // CAN_OK bei Erfolg + +// Diagnose/Utilities (nutzen MCP-APIs) +uint8_t CAN_HAL_GetErrorFlags(); // Intern: getError() +void CAN_HAL_GetErrorCounters(uint8_t& tec, uint8_t& rec); // TX/RX Error Counter diff --git a/Software/include/can_native.h b/Software/include/can_native.h new file mode 100644 index 0000000..81c3959 --- /dev/null +++ b/Software/include/can_native.h @@ -0,0 +1,11 @@ +#pragma once +#include +#include "can_hal.h" + +// Initialisiert den Native-CAN-Profilpfad (setzt Masken/Filter und NORMAL-Mode). +// Voraussetzung: CAN_HAL_Init(...) hat zuvor true geliefert. +bool Init_CAN_Native(); + +// Liest Frames non-blocking, extrahiert Hinterradgeschwindigkeit je nach Bike, +// integriert mm über dt und liefert die seit letztem Aufruf addierten Millimeter. +uint32_t Process_CAN_Native_WheelSpeed(); diff --git a/Software/include/can_obd2.h b/Software/include/can_obd2.h new file mode 100644 index 0000000..de8fd82 --- /dev/null +++ b/Software/include/can_obd2.h @@ -0,0 +1,16 @@ +#pragma once +#include +#include "can_hal.h" + +// Initialisiert das OBD2-CAN-Profil: +// - setzt Masken/Filter für 0x7E8..0x7EF (ECU-Antworten) +// - Normal-Mode sicherstellen +// Voraussetzung: CAN_HAL_Init(...) hat zuvor true geliefert. +bool Init_CAN_OBD2(); + +// Polling-Prozess für OBD2 über CAN (non-blocking): +// - sendet zyklisch Requests (0x7DF) auf PID 0x0D (Fahrzeuggeschwindigkeit) +// - verarbeitet Antworten 0x7E8..0x7EF +// - integriert Millimeter über dt +// Rückgabe: seit letztem Aufruf addierte Millimeter (uint32_t) +uint32_t Process_CAN_OBD2_Speed(); diff --git a/Software/include/obd2_can.h b/Software/include/obd2_can.h deleted file mode 100644 index 4ce497e..0000000 --- a/Software/include/obd2_can.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef _OBD2_CAN_H_ -#define _OBD2_CAN_H_ - -#include - -// Init MCP2515 und OBD2-CAN-Poller (non-blocking) -void Init_OBD2_CAN(); - -// Verarbeitet OBD2-CAN nicht-blockierend, integriert Strecke (mm) seit letztem Aufruf. -uint32_t Process_OBD2_CAN_Speed(); - -#endif diff --git a/Software/src/can.cpp b/Software/src/can.cpp deleted file mode 100644 index b075de5..0000000 --- a/Software/src/can.cpp +++ /dev/null @@ -1,184 +0,0 @@ -/** - * @file can.cpp - * - * @brief Implementation file for CAN-related functions in the ChainLube application. - * - * This file contains the implementation of functions related to CAN (Controller Area Network) - * communication within the ChainLube application. It includes the initialization of the CAN module, - * setup of masks and filters, and processing of CAN messages. Additionally, a debug message function - * is included if CAN debugging is enabled. - * - * @author Your Name - * @date Date - */ - -#include "can.h" - -MCP_CAN CAN0(GPIO_CS_CAN); -#ifdef CAN_DEBUG_MESSAGE -#define MAX_DEBUG_RETRIES 100 -void sendCANDebugMessage(); -#endif - -/** - * @brief Initializes the CAN module, sets masks, and filters based on the configured CAN source. - * - * This function initializes the CAN module, sets masks and filters based on the configured CAN source - * in the application settings, and sets the CAN module in normal mode for communication. - */ -void Init_CAN() -{ - if (CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_16MHZ) != CAN_OK) - MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); - - CAN0.init_Mask(0, 0, 0x07FF0000); // Init first mask... - CAN0.init_Mask(1, 0, 0x07FF0000); // Init second mask... - - switch (LubeConfig.CANSource) - { - case KTM_890_ADV_R_2021: - CAN0.init_Filt(0, 0, 0x012D0000); // Init first filter... - break; - case KTM_1290_SD_R_2023: - CAN0.init_Filt(0, 0, 0x012D0000); // Init first filter... - break; - default: - break; - } - - CAN0.setMode(MCP_NORMAL); -} - -/** - * @brief Processes CAN messages to determine the wheel speed based on the configured CAN source. - * - * This function reads incoming CAN messages and extracts the rear wheel speed information. - * The wheel speed is then converted to millimeters per second based on the configured CAN source. - * The function also monitors the CAN signal for potential issues and triggers diagnostic trouble codes (DTCs). - * - * @return The calculated distance traveled in millimeters since the last call. - */ -uint32_t Process_CAN_WheelSpeed() -{ -#define FACTOR_RWP_KMH_890ADV 18 // Divider to convert Raw Data to km/h -#define FACTOR_RWP_KMH_1290SD 18 // Divider to convert Raw Data to km/h - can_frame canMsg; - static uint32_t lastRecTimestamp = 0; - uint16_t RearWheelSpeed_raw; - uint32_t milimeters_to_add = 0; - uint32_t RWP_millimeter_per_second = 0; - - if (CAN0.readMsgBuf(&canMsg.can_id, &canMsg.can_dlc, canMsg.data) == CAN_OK) - { - - switch (LubeConfig.CANSource) - { - case KTM_890_ADV_R_2021: - // raw / FACTOR_RWP_KMH_890ADV -> km/h * 100000 -> cm/h / 3600 -> cm/s - // raw * 500 -> cm/s - RearWheelSpeed_raw = (uint16_t)canMsg.data[5] << 8 | canMsg.data[6]; - RWP_millimeter_per_second = (((uint32_t)RearWheelSpeed_raw * 1000000) / FACTOR_RWP_KMH_890ADV) / 3600; - break; - case KTM_1290_SD_R_2023: - // raw / FACTOR_RWP_KMH_1290SD -> km/h * 100000 -> cm/h / 3600 -> cm/s - // raw * 500 -> cm/s - RearWheelSpeed_raw = (uint16_t)canMsg.data[5] << 8 | canMsg.data[6]; - RWP_millimeter_per_second = (((uint32_t)RearWheelSpeed_raw * 1000000) / FACTOR_RWP_KMH_1290SD) / 3600; - break; - default: - break; - } - - uint32_t timesincelast = millis() - lastRecTimestamp; - lastRecTimestamp = millis(); - - milimeters_to_add = (RWP_millimeter_per_second * timesincelast) / 1000; - } - - if (lastRecTimestamp > 1000) - { - MaintainDTC(DTC_NO_CAN_SIGNAL, (millis() > lastRecTimestamp + 10000 ? true : false)); - } - -#ifdef CAN_DEBUG_MESSAGE - static uint32_t previousMillis = 0; - - if (millis() - previousMillis >= 1000) - { - sendCANDebugMessage(); - previousMillis = millis(); - } -#endif - - return milimeters_to_add; -} - -#ifdef CAN_DEBUG_MESSAGE -/** - * @brief Sends periodic CAN debug messages for monitoring and diagnostics. - * - * This function sends periodic CAN debug messages containing various system information for monitoring and diagnostics. - * The information includes system status, timestamps, tank percentage, DTC flags, and other relevant data. - * The debug messages are sent with a configurable multiplexer to broadcast different types of information in each cycle. - */ -void sendCANDebugMessage() -{ -#define MAX_DEBUG_MULTIPLEXER 6 - static uint16_t DebugSendFailTimeout = 0; - static uint8_t debugMultiplexer = 0; - byte data[8] = {debugMultiplexer, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - uint32_t millisValue = millis(); - - switch (debugMultiplexer) - { - case 0: - memcpy(&data[1], &millisValue, sizeof(millisValue)); - memcpy(&data[5], &globals.purgePulses, sizeof(globals.purgePulses)); - break; - case 1: - data[1] = (uint8_t)globals.systemStatus; - data[2] = (uint8_t)globals.resumeStatus; - data[3] = (uint8_t)globals.requestEEAction; - data[4] = globals.TankPercentage; - data[5] = (0x01 & globals.hasDTC) | ((0x01 & globals.measurementActive) << 1); - break; - case 2: - memcpy(&data[1], &globals.eePersistenceAddress, sizeof(globals.eePersistenceAddress)); - memcpy(&data[3], &PersistenceData.tankRemain_microL, sizeof(PersistenceData.tankRemain_microL)); - break; - case 3: - memcpy(&data[1], &PersistenceData.writeCycleCounter, sizeof(PersistenceData.writeCycleCounter)); - memcpy(&data[3], &PersistenceData.TravelDistance_highRes_mm, sizeof(PersistenceData.TravelDistance_highRes_mm)); - break; - case 4: - memcpy(&data[1], &PersistenceData.odometer_mm, sizeof(PersistenceData.odometer_mm)); - break; - case 5: - memcpy(&data[1], &PersistenceData.odometer, sizeof(PersistenceData.odometer)); - break; - case 6: - memcpy(&data[1], &PersistenceData.checksum, sizeof(PersistenceData.checksum)); - break; - default: - break; - } - - debugMultiplexer++; - debugMultiplexer = debugMultiplexer > MAX_DEBUG_MULTIPLEXER ? 0 : debugMultiplexer; - - if (DebugSendFailTimeout < MAX_DEBUG_RETRIES) - { - byte sndStat = CAN0.sendMsgBuf(0x7FF, 0, 8, data); - if (sndStat != CAN_OK) - { - Debug_pushMessage("failed sending CAN-DbgMsg: %d, %d\n", sndStat, DebugSendFailTimeout); - DebugSendFailTimeout++; - } - } - else if (DebugSendFailTimeout == MAX_DEBUG_RETRIES) - { - Debug_pushMessage("disable CAN-DbgMsg due to timeout\n"); - DebugSendFailTimeout++; - } -} -#endif \ No newline at end of file diff --git a/Software/src/can_hal.cpp b/Software/src/can_hal.cpp new file mode 100644 index 0000000..f9349a7 --- /dev/null +++ b/Software/src/can_hal.cpp @@ -0,0 +1,240 @@ +#include "can_hal.h" +#include "dtc.h" +// #include "debugger.h" // optional für Logs + +// ==== Interner Zustand/Helper ==== +MCP_CAN CAN0(GPIO_CS_CAN); + +static bool s_ready = false; +static uint8_t s_nextFiltSlot = 0; // 0..5 (MCP2515 hat 6 Filter-Slots) +static uint16_t s_modeSettleMs = 10; // Default aus Config + +// 11-bit: Lib erwartet (value << 16) +static inline uint32_t _std_to_hw(uint16_t v11) { return ((uint32_t)v11) << 16; } + +// „Bestätigter“ Mode-Wechsel mithilfe der Lib-Funktion setMode(newMode) +// Viele Forks nutzen intern mcp2515_requestNewMode(); wir retryen kurz. +static bool _trySetMode(uint8_t mode, uint16_t settleMs) +{ + const uint32_t t0 = millis(); + do { + if (CAN0.setMode(mode) == CAN_OK) return true; + delay(1); + } while ((millis() - t0) < settleMs); + return false; +} + +// LOOPBACK-Selftest (ohne Bus) +static bool _selftest_loopback(uint16_t windowMs) +{ + if (!_trySetMode(MCP_LOOPBACK, s_modeSettleMs)) return false; + + const unsigned long tid = 0x123; + uint8_t tx[8] = {0xA5, 0x5A, 0x11, 0x22, 0x33, 0x44, 0x77, 0x88}; + if (CAN0.sendMsgBuf(tid, 0, 8, tx) != CAN_OK) { + (void)_trySetMode(MCP_NORMAL, s_modeSettleMs); + return false; + } + + bool got = false; + const uint32_t t0 = millis(); + while ((millis() - t0) < windowMs) { + if (CAN0.checkReceive() == CAN_MSGAVAIL) { + unsigned long rid; uint8_t len, rx[8]; + if (CAN0.readMsgBuf(&rid, &len, rx) == CAN_OK) { + if (rid == tid && len == 8 && memcmp(tx, rx, 8) == 0) { got = true; break; } + } + } + delay(1); + } + + (void)_trySetMode(MCP_NORMAL, s_modeSettleMs); + return got; +} + +// Optional: kurzer ListenOnly-Hörtest (nur Heuristik, keine DTC-Änderung) +static void _probe_listen_only(uint16_t ms) +{ + if (ms == 0) return; + if (!_trySetMode(MCP_LISTENONLY, s_modeSettleMs)) return; + const uint32_t t0 = millis(); + while ((millis() - t0) < ms) { + if (CAN0.checkReceive() == CAN_MSGAVAIL) break; + delay(1); + } + (void)_trySetMode(MCP_NORMAL, s_modeSettleMs); +} + +// ==== Öffentliche API ==== + +bool CAN_HAL_Init(const CanHalConfig& cfg) +{ + s_ready = false; + s_modeSettleMs = cfg.modeSettleMs ? cfg.modeSettleMs : 10; + + // 1) SPI/MCP starten + // HIER: MCP_STDEXT statt MCP_STD, damit die Lib nicht ins default/Failure läuft + if (CAN0.begin(MCP_STDEXT, cfg.baud, cfg.clock) != CAN_OK) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + + // 2) Loopback‑Selftest (ohne Bus) + if (!_selftest_loopback(20)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + + // 3) Optional Listen‑Only‑Probe (nur Info) + _probe_listen_only(cfg.listenOnlyProbeMs); + + // 4) Default: Filter/Masks neutral, Mode NORMAL + // -> Für Masken/Filter müssen wir in CONFIG sein (hier: MODE_CONFIG laut deiner Lib) + if (!_trySetMode(MODE_CONFIG, s_modeSettleMs)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + + // weit offen (STD) + CAN0.init_Mask(0, 0, _std_to_hw(0x000)); + CAN0.init_Mask(1, 0, _std_to_hw(0x000)); + for (uint8_t i = 0; i < 6; ++i) { + CAN0.init_Filt(i, 0, _std_to_hw(0x000)); + } + s_nextFiltSlot = 0; + + if (!_trySetMode(MCP_NORMAL, s_modeSettleMs)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + + // Erfolgreich + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, false); + s_ready = true; + return true; +} + +bool CAN_HAL_IsReady() { return s_ready; } + +bool CAN_HAL_SetMode(uint8_t mode) +{ + const bool ok = _trySetMode(mode, s_modeSettleMs); + if (!ok) MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return ok; +} + +bool CAN_HAL_SetMask(uint8_t bank, bool ext, uint32_t rawMask) +{ + if (bank > 1) return false; + if (!CAN_HAL_SetMode(MODE_CONFIG)) return false; + + const bool ok = (CAN0.init_Mask(bank, ext ? 1 : 0, rawMask) == CAN_OK); + + if (!CAN_HAL_SetMode(MCP_NORMAL)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + return ok; +} + +bool CAN_HAL_SetStdMask11(uint8_t bank, uint16_t mask11) +{ + return CAN_HAL_SetMask(bank, false, _std_to_hw(mask11)); +} + +void CAN_HAL_ClearFilters() +{ + if (!CAN_HAL_SetMode(MODE_CONFIG)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return; + } + + CAN0.init_Mask(0, 0, _std_to_hw(0x000)); + CAN0.init_Mask(1, 0, _std_to_hw(0x000)); + for (uint8_t i = 0; i < 6; ++i) { + CAN0.init_Filt(i, 0, _std_to_hw(0x000)); + } + s_nextFiltSlot = 0; + + if (!CAN_HAL_SetMode(MCP_NORMAL)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + } +} + +bool CAN_HAL_AddFilter(const CanFilter& f) +{ + if (s_nextFiltSlot >= 6) return false; + if (!CAN_HAL_SetMode(MODE_CONFIG)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + + const uint32_t hwId = f.ext ? f.id : _std_to_hw((uint16_t)f.id); + const uint8_t slot = s_nextFiltSlot++; + const bool ok = (CAN0.init_Filt(slot, f.ext ? 1 : 0, hwId) == CAN_OK); + + if (!CAN_HAL_SetMode(MCP_NORMAL)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + return ok; +} + +bool CAN_HAL_SetFilters(const CanFilter* list, size_t count) +{ + if (!CAN_HAL_SetMode(MODE_CONFIG)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + + // Slots zurücksetzen + s_nextFiltSlot = 0; + for (uint8_t i = 0; i < 6; ++i) { + CAN0.init_Filt(i, 0, _std_to_hw(0x000)); + } + + // Setzen + for (size_t i = 0; i < count && s_nextFiltSlot < 6; ++i) { + const auto& f = list[i]; + const uint32_t hwId = f.ext ? f.id : _std_to_hw((uint16_t)f.id); + CAN0.init_Filt(s_nextFiltSlot++, f.ext ? 1 : 0, hwId); + } + + if (!CAN_HAL_SetMode(MCP_NORMAL)) { + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + return true; +} + +bool CAN_HAL_Read(unsigned long& id, uint8_t& len, uint8_t data[8]) +{ + if (CAN0.checkReceive() != CAN_MSGAVAIL) return false; + if (CAN0.readMsgBuf(&id, &len, data) != CAN_OK) { + // Echte Lese-Fehler -> vermutlich SPI/Controller-Problem + MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, true); + return false; + } + return true; +} + +uint8_t CAN_HAL_Send(unsigned long id, bool ext, uint8_t len, const uint8_t* data) +{ + // Sende-Fehler (CAN_FAILTX) müssen nicht zwingend Transceiver-Defekte sein (z. B. Bus-Off). + // Höhere Ebene kann bei Bedarf DTCs setzen. Hier nur durchreichen. + return CAN0.sendMsgBuf(id, ext ? 1 : 0, len, const_cast(data)); +} + +// ==== Diagnose/Utilities ==== + +uint8_t CAN_HAL_GetErrorFlags() +{ + // getError() liefert MCP_EFLG Snapshot (Lib-abhängig) + return CAN0.getError(); +} + +void CAN_HAL_GetErrorCounters(uint8_t& tec, uint8_t& rec) +{ + tec = CAN0.errorCountTX(); + rec = CAN0.errorCountRX(); +} diff --git a/Software/src/can_native.cpp b/Software/src/can_native.cpp new file mode 100644 index 0000000..fc6b418 --- /dev/null +++ b/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; +} diff --git a/Software/src/can_obd2.cpp b/Software/src/can_obd2.cpp new file mode 100644 index 0000000..fbe0386 --- /dev/null +++ b/Software/src/can_obd2.cpp @@ -0,0 +1,243 @@ +#include "can_obd2.h" +#include "dtc.h" +#include "debugger.h" +#include "globals.h" // falls du später Einstellungen brauchst + +// ======================= +// Konfiguration (anpassbar) +// ======================= + +// Abfrageintervall für Speed (PID 0x0D) +#ifndef OBD2_QUERY_INTERVAL_MS +#define OBD2_QUERY_INTERVAL_MS 100 // 10 Hz +#endif + +// Antwort-Timeout auf eine einzelne Anfrage +#ifndef OBD2_RESP_TIMEOUT_MS +#define OBD2_RESP_TIMEOUT_MS 60 // ~60 ms +#endif + +// Wenn so lange keine valide Antwort kam, gilt die Geschwindigkeit als stale -> v=0 +#ifndef OBD2_STALE_MS +#define OBD2_STALE_MS 600 // 0,6 s +#endif + +// Begrenzung, wie viele RX-Frames pro Aufruf maximal gezogen werden +#ifndef OBD2_MAX_READS_PER_CALL +#define OBD2_MAX_READS_PER_CALL 4 +#endif + +// Optionales Debug-Rate-Limit +#ifndef OBD2_DEBUG_INTERVAL_MS +#define OBD2_DEBUG_INTERVAL_MS 1000 +#endif + +// ======================= +// OBD-II IDs (11-bit) +// ======================= +static constexpr uint16_t OBD_REQ_ID = 0x7DF; // Broadcast-Request +static constexpr uint16_t OBD_RESP_MIN = 0x7E8; // ECUs antworten 0x7E8..0x7EF +static constexpr uint16_t OBD_RESP_MAX = 0x7EF; + +// ======================= +// Interner Status +// ======================= +enum class ObdState : uint8_t +{ + Idle = 0, + Waiting = 1 +}; + +static ObdState s_state = ObdState::Idle; +static uint32_t s_lastQueryTime = 0; +static uint32_t s_requestDeadline = 0; + +static uint32_t s_lastRespTime = 0; +static uint32_t s_lastIntegrateMs = 0; +static uint32_t s_lastSpeedMMps = 0; + +static uint32_t s_lastDbgMs = 0; + +// ======================= +// Hilfsfunktionen +// ======================= +static inline bool isResponseId(unsigned long id) +{ + return (id >= OBD_RESP_MIN) && (id <= OBD_RESP_MAX); +} + +static inline uint32_t kmh_to_mmps(uint16_t kmh) +{ + return (uint32_t)kmh * 1000000UL / 3600UL; +} + +static inline void maybeDebug(uint32_t now, const char *fmt, ...) +{ +#if 1 + if (now - s_lastDbgMs < OBD2_DEBUG_INTERVAL_MS) + return; + s_lastDbgMs = now; + va_list ap; + va_start(ap, fmt); + Debug_pushMessage(fmt, ap); + va_end(ap); +#else + (void)now; + (void)fmt; +#endif +} + +// ======================= +// Öffentliche API +// ======================= +bool Init_CAN_OBD2() +{ + // 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; + + if (!CAN_HAL_Init(cfg)) + { + // Hardware/Selftest failed → OBD2-CAN nicht nutzbar + MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true); + Debug_pushMessage("CAN(OBD2): HAL init failed\n"); + return false; + } + } + + // 2) Filter/Masken für 0x7E8..0x7EF + CAN_HAL_SetStdMask11(0, 0x7F0); + CAN_HAL_SetStdMask11(1, 0x7F0); + + CanFilter flist[6] = { + {0x7E8, false}, + {0x7E9, false}, + {0x7EA, false}, + {0x7EB, false}, + {0x7EC, false}, + {0x7ED, false}, + }; + CAN_HAL_SetFilters(flist, 6); + + CAN_HAL_SetMode(MCP_NORMAL); + + // 3) DTC-Startzustände + MaintainDTC(DTC_OBD2_CAN_TIMEOUT, false); + MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true); // bis erste Antwort kommt + + // 4) Zeitbasen resetten + const uint32_t now = millis(); + s_state = ObdState::Idle; + s_lastQueryTime = now; + s_requestDeadline = 0; + s_lastRespTime = 0; + s_lastIntegrateMs = now; + s_lastSpeedMMps = 0; + s_lastDbgMs = 0; + + Debug_pushMessage("CAN(OBD2): Filters set (7E8..7EF), NORMAL mode\n"); + return true; +} + +uint32_t Process_CAN_OBD2_Speed() +{ + const uint32_t now = millis(); + + // 1) Anfrage senden (nur wenn Idle und Intervall um) + if (s_state == ObdState::Idle && (now - s_lastQueryTime) >= OBD2_QUERY_INTERVAL_MS) + { + uint8_t req[8] = {0x02, 0x01, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00}; // Mode 01, PID 0x0D (Speed) + const uint8_t st = CAN_HAL_Send(OBD_REQ_ID, /*ext=*/false, 8, req); + s_lastQueryTime = now; + + if (st == CAN_OK) + { + s_state = ObdState::Waiting; + s_requestDeadline = now + OBD2_RESP_TIMEOUT_MS; + } + else + { + // Senden fehlgeschlagen -> harter Timeout-DTC + MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true); + maybeDebug(now, "OBD2-CAN send failed (%u)\n", st); + } + } + + // 2) Non-blocking Receive: wenige Frames pro Tick ziehen + for (uint8_t i = 0; i < OBD2_MAX_READS_PER_CALL; ++i) + { + unsigned long rxId; + uint8_t len; + uint8_t rx[8]; + if (!CAN_HAL_Read(rxId, len, rx)) + break; + if (!isResponseId(rxId)) + continue; + if (len < 3) + continue; + + // Erwartete Formate: + // - Einfache Antwort: 0x41 0x0D ... + // - Mit Längen-Byte: 0x03 0x41 0x0D ... + uint8_t modeResp = 0, pid = 0, speedKmh = 0; + if (rx[0] == 0x03 && len >= 4 && rx[1] == 0x41 && rx[2] == 0x0D) + { + modeResp = rx[1]; + pid = rx[2]; + speedKmh = rx[3]; + } + else if (rx[0] == 0x41 && rx[1] == 0x0D && len >= 3) + { + modeResp = rx[0]; + pid = rx[1]; + speedKmh = rx[2]; + } + else + { + continue; // anderes PID/Format ignorieren + } + + if (modeResp == 0x41 && pid == 0x0D) + { + // Valide Antwort + s_lastSpeedMMps = kmh_to_mmps(speedKmh); + s_lastRespTime = now; + s_state = ObdState::Idle; + + MaintainDTC(DTC_OBD2_CAN_TIMEOUT, false); + MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, false); + + maybeDebug(now, "OBD2 speed: %u km/h (%lu mm/s)\n", + (unsigned)speedKmh, (unsigned long)s_lastSpeedMMps); + break; // eine valide Antwort pro Zyklus reicht + } + } + + // 3) Offene Anfrage: Timeout prüfen + if (s_state == ObdState::Waiting && (int32_t)(now - s_requestDeadline) >= 0) + { + // Keine passende Antwort erhalten + MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true); + s_state = ObdState::Idle; + } + + // 4) Integration (mm) über dt + uint32_t add_mm = 0; + if (s_lastIntegrateMs == 0) + s_lastIntegrateMs = now; + const uint32_t dt_ms = now - s_lastIntegrateMs; + s_lastIntegrateMs = now; + + // Stale-Schutz: wenn lange keine Antwort -> v=0 + const bool stale = (s_lastRespTime == 0) || ((now - s_lastRespTime) > OBD2_STALE_MS); + const uint32_t v_mmps = stale ? 0 : s_lastSpeedMMps; + + // mm = (mm/s * ms) / 1000 + add_mm = (uint64_t)v_mmps * dt_ms / 1000ULL; + + return add_mm; +} diff --git a/Software/src/main.cpp b/Software/src/main.cpp index 7a14562..643ae39 100644 --- a/Software/src/main.cpp +++ b/Software/src/main.cpp @@ -33,12 +33,12 @@ #include "config.h" #include "globals.h" #include "debugger.h" -#include "can.h" #include "gps.h" #include "dtc.h" #include "led_colors.h" #include "obd2_kline.h" -#include "obd2_can.h" +#include "can_obd2.h" +#include "can_native.h" #include "buttoncontrol.h" #include "button_actions.h" #include "ledcontrol.h" @@ -102,6 +102,9 @@ void setup() // Initialize and clear Diagnostic Trouble Code (DTC) storage ClearAllDTC(); + Wire.begin(); + + #ifdef FEATURE_ENABLE_WIFI_CLIENT // Configure WiFi settings for client mode if enabled WiFi.mode(WIFI_STA); @@ -142,8 +145,8 @@ void setup() switch (LubeConfig.SpeedSource) { case SOURCE_CAN: - Init_CAN(); - wheelSpeedcapture = &Process_CAN_WheelSpeed; + Init_CAN_Native(); + wheelSpeedcapture = &Process_CAN_Native_WheelSpeed; Serial.print("\nCAN-Init done"); break; case SOURCE_GPS: @@ -163,8 +166,8 @@ void setup() Serial.print("\nOBD2-KLine-Init done"); break; case SOURCE_OBD2_CAN: - Init_OBD2_CAN(); - wheelSpeedcapture = &Process_OBD2_CAN_Speed; + Init_CAN_OBD2(); + wheelSpeedcapture = &Process_CAN_OBD2_Speed; Serial.print("\nOBD2-CAN-Init done"); break; default: diff --git a/Software/src/obd2_can.cpp b/Software/src/obd2_can.cpp deleted file mode 100644 index ef74812..0000000 --- a/Software/src/obd2_can.cpp +++ /dev/null @@ -1,208 +0,0 @@ -#include "obd2_can.h" -#include -#include -#include "common.h" -#include "globals.h" -#include "dtc.h" -#include "debugger.h" - -// ======================= -// Konfiguration -// ======================= -#ifndef OBD2_CAN_CS_PIN -#define OBD2_CAN_CS_PIN 10 -#endif - -// 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 - -// Tuning: Poll schneller als 500ms für WheelSpeed -#ifndef OBD2_QUERY_INTERVAL_MS -#define OBD2_QUERY_INTERVAL_MS 100 // 10 Hz Pollrate -#endif - -#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() -{ - // 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; - } - - 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() -{ - const uint32_t now = 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; - - 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]; - OBD_CAN.readMsgBuf(&rxId, &len, rxBuf); - - if (!isObdResponseId(rxId)) continue; // hart gefiltert - if (len < 4) continue; // zu kurz für 01 0D A (SPEED) - - // 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 - } - - 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; -}