Marcel Peterkau
98629b744d
Some checks failed
CI-Build/Kettenoeler/pipeline/head There was a failure building this commit
295 lines
7.9 KiB
C++
295 lines
7.9 KiB
C++
#include "can_obd2.h"
|
||
#include "can_hal.h"
|
||
#include "dtc.h"
|
||
#include "debugger.h"
|
||
#include "globals.h"
|
||
#include <stdarg.h>
|
||
|
||
// Trace-Sink aus webui.cpp (o.ä.)
|
||
extern void TRACE_OnObdFrame(uint32_t id, bool rx, const uint8_t *d, uint8_t dlc, const char *note);
|
||
|
||
// =======================
|
||
// 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 120 // etwas großzügiger für reale ECUs
|
||
#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
|
||
|
||
// Max. Delta-Zeit fürs Weg-Integrationsglied (Ausreißer-Klemme)
|
||
#ifndef OBD2_MAX_DT_MS
|
||
#define OBD2_MAX_DT_MS 200
|
||
#endif
|
||
|
||
// Erlaube einmaligen Fallback von funktionaler (0x7DF) auf physische Adresse (0x7E0)
|
||
#ifndef OBD2_ALLOW_PHYSICAL_FALLBACK
|
||
#define OBD2_ALLOW_PHYSICAL_FALLBACK 1
|
||
#endif
|
||
|
||
// =======================
|
||
// OBD-II IDs (11-bit)
|
||
// =======================
|
||
static constexpr uint16_t OBD_REQ_ID_FUNCTIONAL = 0x7DF; // Broadcast-Request
|
||
static constexpr uint16_t OBD_REQ_ID_PHYSICAL = 0x7E0; // Engine ECU (Antwort 0x7E8)
|
||
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); // nimmt va_list
|
||
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[8] = {
|
||
{0x7E8, false},
|
||
{0x7E9, false},
|
||
{0x7EA, false},
|
||
{0x7EB, false},
|
||
{0x7EC, false},
|
||
{0x7ED, false},
|
||
{0x7EE, false},
|
||
{0x7EF, false},
|
||
};
|
||
CAN_HAL_SetFilters(flist, 8);
|
||
|
||
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)
|
||
|
||
// Trace: geplanter Request (functional)
|
||
TRACE_OnObdFrame(OBD_REQ_ID_FUNCTIONAL, /*rx=*/false, req, 8, "req 01 0D (functional)");
|
||
|
||
uint8_t st = CAN_HAL_Send(OBD_REQ_ID_FUNCTIONAL, /*ext=*/false, 8, req);
|
||
s_lastQueryTime = now;
|
||
|
||
if (st == CAN_OK)
|
||
{
|
||
s_state = ObdState::Waiting;
|
||
s_requestDeadline = now + OBD2_RESP_TIMEOUT_MS;
|
||
}
|
||
else
|
||
{
|
||
#if OBD2_ALLOW_PHYSICAL_FALLBACK
|
||
// einmalig physisch versuchen (0x7E0 → Antwort 0x7E8)
|
||
TRACE_OnObdFrame(OBD_REQ_ID_PHYSICAL, /*rx=*/false, req, 8, "req 01 0D (physical)");
|
||
st = CAN_HAL_Send(OBD_REQ_ID_PHYSICAL, /*ext=*/false, 8, req);
|
||
s_lastQueryTime = now;
|
||
if (st == CAN_OK)
|
||
{
|
||
s_state = ObdState::Waiting;
|
||
s_requestDeadline = now + OBD2_RESP_TIMEOUT_MS;
|
||
}
|
||
else
|
||
#endif
|
||
{
|
||
// 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/0x04 0x41 0x0D <A> ...
|
||
uint8_t modeResp = 0, pid = 0, speedKmh = 0;
|
||
if ((rx[0] == 0x03 || rx[0] == 0x04) && 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
|
||
{
|
||
// Nicht das gesuchte PID → optional trotzdem loggen
|
||
TRACE_OnObdFrame(rxId, /*rx=*/true, rx, len, "other OBD resp");
|
||
continue;
|
||
}
|
||
|
||
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);
|
||
|
||
char note[40];
|
||
snprintf(note, sizeof(note), "speed=%ukmh", (unsigned)speedKmh);
|
||
TRACE_OnObdFrame(rxId, /*rx=*/true, rx, len, note);
|
||
|
||
maybeDebug(now, "OBD2 speed: %u km/h (%lu mm/s)\n",
|
||
(unsigned)speedKmh, (unsigned long)s_lastSpeedMMps);
|
||
break; // eine valide Antwort pro Zyklus reicht
|
||
}
|
||
else
|
||
{
|
||
// ist zwar OBD-II Antwort, aber nicht unser PID – optional loggen
|
||
TRACE_OnObdFrame(rxId, /*rx=*/true, rx, len, "other OBD resp");
|
||
}
|
||
}
|
||
|
||
// 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;
|
||
TRACE_OnObdFrame(0x000, /*rx=*/true, nullptr, 0, "timeout 01 0D");
|
||
}
|
||
|
||
// 4) Integration (mm) über dt
|
||
uint32_t add_mm = 0;
|
||
if (s_lastIntegrateMs == 0)
|
||
s_lastIntegrateMs = now;
|
||
|
||
uint32_t raw_dt = now - s_lastIntegrateMs;
|
||
if (raw_dt > OBD2_MAX_DT_MS) raw_dt = OBD2_MAX_DT_MS; // Ausreißer klemmen
|
||
const uint32_t dt_ms = raw_dt;
|
||
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;
|
||
}
|