85 lines
2.2 KiB
C++
85 lines
2.2 KiB
C++
#include "obd2_can.h"
|
|
#include <mcp_can.h>
|
|
#include <SPI.h>
|
|
#include "common.h"
|
|
#include "globals.h"
|
|
#include "dtc.h"
|
|
#include "debugger.h"
|
|
|
|
// === Setup: MCP2515 CS-Pin definieren ===
|
|
#define OBD2_CAN_CS_PIN 10
|
|
#define OBD2_OBD_REQUEST_ID 0x7DF
|
|
#define OBD2_OBD_RESPONSE_ID 0x7E8
|
|
|
|
MCP_CAN OBD_CAN(OBD2_CAN_CS_PIN);
|
|
|
|
static uint32_t lastQueryTime = 0;
|
|
static uint32_t lastRecvTime = 0;
|
|
static uint32_t lastSpeedMMperSec = 0;
|
|
|
|
#define OBD2_QUERY_INTERVAL 500 // alle 500ms
|
|
|
|
void Init_OBD2_CAN()
|
|
{
|
|
if (OBD_CAN.begin(MCP_STD, CAN_500KBPS, MCP_16MHZ) != CAN_OK)
|
|
{
|
|
Serial.println("OBD2 CAN Init FAILED!");
|
|
return;
|
|
}
|
|
|
|
OBD_CAN.setMode(MCP_NORMAL);
|
|
delay(100);
|
|
Serial.println("OBD2 CAN Init OK");
|
|
}
|
|
|
|
uint32_t Process_OBD2_CAN_Speed()
|
|
{
|
|
if (millis() - lastQueryTime < OBD2_QUERY_INTERVAL)
|
|
return 0;
|
|
|
|
lastQueryTime = millis();
|
|
|
|
// Anfrage: 01 0D → Geschwindigkeit
|
|
byte obdRequest[8] = {0x02, 0x01, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00};
|
|
byte sendStat = OBD_CAN.sendMsgBuf(OBD2_OBD_REQUEST_ID, 0, 8, obdRequest);
|
|
|
|
if (sendStat != CAN_OK)
|
|
{
|
|
MaintainDTC(DTC_OBD2_CAN_TIMEOUT, true);
|
|
Debug_pushMessage("OBD2_CAN: send failed (%d)\n", sendStat);
|
|
return 0;
|
|
}
|
|
|
|
unsigned long rxId;
|
|
byte len = 0;
|
|
byte rxBuf[8];
|
|
uint32_t timeout = millis() + 100;
|
|
|
|
while (millis() < timeout)
|
|
{
|
|
if (OBD_CAN.checkReceive() == CAN_MSGAVAIL)
|
|
{
|
|
OBD_CAN.readMsgBuf(&rxId, &len, rxBuf);
|
|
if ((rxId & 0xFFF8) == OBD2_OBD_RESPONSE_ID && rxBuf[1] == 0x0D)
|
|
{
|
|
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, false); // alles ok
|
|
|
|
uint8_t speed_kmh = rxBuf[3];
|
|
uint32_t speed_mm_per_sec = (uint32_t)speed_kmh * 1000000 / 3600;
|
|
uint32_t dt = millis() - lastRecvTime;
|
|
lastRecvTime = millis();
|
|
lastSpeedMMperSec = speed_mm_per_sec;
|
|
|
|
Debug_pushMessage("OBD2_CAN: %d km/h (%lu mm/s)\n", speed_kmh, speed_mm_per_sec);
|
|
return (speed_mm_per_sec * dt) / 1000;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Keine Antwort erhalten
|
|
MaintainDTC(DTC_OBD2_CAN_NO_RESPONSE, true);
|
|
Debug_pushMessage("OBD2_CAN: no response within timeout\n");
|
|
return 0;
|
|
}
|
|
|