144 lines
4.4 KiB
C++
144 lines
4.4 KiB
C++
#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;
|
|
}
|