reworked CAN Stack
This commit is contained in:
243
Software/src/can_obd2.cpp
Normal file
243
Software/src/can_obd2.cpp
Normal file
@@ -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 <A> ...
|
||||
// - Mit Längen-Byte: 0x03 0x41 0x0D <A> ...
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user