Ryan Malloy
8ba53630c0
New protocol handler alongside BMW I/K-Bus: - KLineObd2: 5-baud slow init, fast init (TiniPulse), request/response with half-duplex echo clearing, PID convenience wrapper - Obd2Pids.h: ~20 common PIDs with SAE J1979 decode helpers - obd2_scanner.cpp: polls RPM, speed, coolant, throttle, voltage Build config changes: - config.h: KLINE_* defaults (10400/8N1/MOD256/no idle detect) - platformio.ini: build_src_filter separates sketches, new [env:obd2-scanner] environment with KLINE_TX_INVERT=0
295 lines
9.1 KiB
C++
295 lines
9.1 KiB
C++
// OBD-II K-line protocol handler (ISO 9141 / ISO 14230)
|
|
// 5-baud slow init, fast init, request/response with echo clearing.
|
|
|
|
#include "KLineObd2.h"
|
|
#include "Obd2Pids.h"
|
|
|
|
KLineObd2::KLineObd2(KLineTransport& transport)
|
|
: _transport(transport),
|
|
_initialized(false),
|
|
_kw1(0),
|
|
_kw2(0),
|
|
_testerAddr(obd2::TESTER_ADDR),
|
|
_ecuAddr(0) {}
|
|
|
|
// --- 5-baud slow init (ISO 9141-2) ---
|
|
// Bit-bang the target address at 5 baud on the TX pin.
|
|
// The UART peripheral must be detached from the TX pin during this sequence,
|
|
// then re-attached for normal 10400 baud communication.
|
|
//
|
|
// Sequence:
|
|
// 1. TX pin LOW for 200ms (start bit at 5 baud)
|
|
// 2. 8 data bits of targetAddr, LSB first, 200ms each
|
|
// 3. TX pin HIGH for 200ms (stop bit)
|
|
// 4. Re-attach UART, wait for ECU response
|
|
// 5. ECU sends: 0x55 (sync), keyword1, keyword2
|
|
// 6. Tester sends: inverted keyword2
|
|
// 7. ECU sends: inverted tester address (0xCC for 0x33)
|
|
|
|
bool KLineObd2::slowInit(uint8_t targetAddr) {
|
|
int8_t txp = _transport.txPin();
|
|
uint8_t unum = _transport.uartNum();
|
|
|
|
// Detach UART from TX pin so we can bit-bang
|
|
uart_set_pin((uart_port_t)unum, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE,
|
|
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
|
|
|
// Configure TX pin as GPIO output, start HIGH (idle)
|
|
pinMode(txp, OUTPUT);
|
|
digitalWrite(txp, HIGH);
|
|
delay(300); // W0: idle before init (>300ms per ISO 9141)
|
|
|
|
// Bit-bang target address at 5 baud
|
|
sendByte5Baud(targetAddr);
|
|
|
|
// Re-attach UART to TX pin
|
|
uart_set_pin((uart_port_t)unum, txp, UART_PIN_NO_CHANGE,
|
|
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
|
|
|
// Wait for ECU sync byte (0x55) — W1: 20-300ms per ISO 9141
|
|
int sync = readByteTimeout(300);
|
|
if (sync != obd2::SYNC_BYTE) {
|
|
return false;
|
|
}
|
|
|
|
// Read keyword bytes — W2: 5-20ms between bytes
|
|
int kw1 = readByteTimeout(50);
|
|
if (kw1 < 0) return false;
|
|
_kw1 = (uint8_t)kw1;
|
|
|
|
int kw2 = readByteTimeout(50);
|
|
if (kw2 < 0) return false;
|
|
_kw2 = (uint8_t)kw2;
|
|
|
|
// W3: 25-50ms before tester responds
|
|
delay(30);
|
|
|
|
// Send inverted keyword2 back to ECU
|
|
uint8_t invKw2 = ~_kw2;
|
|
_transport.serial()->write(invKw2);
|
|
_transport.serial()->flush();
|
|
|
|
// Clear our own echo (half-duplex bus)
|
|
clearEcho(1);
|
|
|
|
// W4: ECU confirms by sending inverted target address
|
|
int ack = readByteTimeout(50);
|
|
if (ack < 0) return false;
|
|
|
|
// The ack should be the complement of the target address
|
|
if ((uint8_t)ack != (uint8_t)~targetAddr) {
|
|
return false;
|
|
}
|
|
|
|
_ecuAddr = targetAddr;
|
|
_initialized = true;
|
|
return true;
|
|
}
|
|
|
|
// --- Fast init (ISO 14230-2, KWP2000) ---
|
|
// 25ms LOW + 25ms HIGH "TiniPulse" on TX, then StartComm request.
|
|
|
|
bool KLineObd2::fastInit() {
|
|
int8_t txp = _transport.txPin();
|
|
uint8_t unum = _transport.uartNum();
|
|
|
|
// Detach UART from TX pin for the wake-up pulse
|
|
uart_set_pin((uart_port_t)unum, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE,
|
|
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
|
|
|
pinMode(txp, OUTPUT);
|
|
digitalWrite(txp, HIGH);
|
|
delay(300); // idle before pulse
|
|
|
|
// TiniPulse: 25ms LOW + 25ms HIGH
|
|
digitalWrite(txp, LOW);
|
|
delay(25);
|
|
digitalWrite(txp, HIGH);
|
|
delay(25);
|
|
|
|
// Re-attach UART
|
|
uart_set_pin((uart_port_t)unum, txp, UART_PIN_NO_CHANGE,
|
|
UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
|
|
|
// Send StartCommunication request (KWP2000 service 0x81)
|
|
// Format: [fmt+len, target, source, service_id]
|
|
uint8_t startComm[] = {
|
|
0xC1, // format: functional addressing, 1 data byte
|
|
obd2::FUNC_ADDR, // target: functional address
|
|
obd2::TESTER_ADDR, // source: tester
|
|
0x81 // StartCommunication service
|
|
};
|
|
uint8_t checksum = KLineTransport::checksumMod256(startComm, 4);
|
|
|
|
for (uint8_t i = 0; i < 4; i++) {
|
|
_transport.serial()->write(startComm[i]);
|
|
}
|
|
_transport.serial()->write(checksum);
|
|
_transport.serial()->flush();
|
|
|
|
// Clear echo (5 bytes: 4 data + 1 checksum)
|
|
clearEcho(5);
|
|
|
|
// Read StartComm positive response
|
|
// Expected: [fmt+len, source, target, 0xC1, kw1, kw2, checksum]
|
|
uint8_t resp[7];
|
|
uint8_t respLen = readResponse(resp, sizeof(resp), 300);
|
|
if (respLen < 4) return false;
|
|
|
|
// Positive response service ID is request + 0x40
|
|
// Find the service byte — it's after header bytes
|
|
// For physical addressing response: [fmt, target, source, 0xC1, kw1, kw2, chk]
|
|
bool foundResponse = false;
|
|
for (uint8_t i = 0; i < respLen; i++) {
|
|
if (resp[i] == 0xC1) { // StartComm positive response
|
|
if (i + 2 < respLen) {
|
|
_kw1 = resp[i + 1];
|
|
_kw2 = resp[i + 2];
|
|
foundResponse = true;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!foundResponse) return false;
|
|
|
|
_ecuAddr = obd2::FUNC_ADDR;
|
|
_initialized = true;
|
|
return true;
|
|
}
|
|
|
|
// --- Request/response ---
|
|
|
|
void KLineObd2::sendRequest(const uint8_t* data, uint8_t len) {
|
|
uint8_t checksum = KLineTransport::checksumMod256(data, len);
|
|
|
|
for (uint8_t i = 0; i < len; i++) {
|
|
_transport.serial()->write(data[i]);
|
|
}
|
|
_transport.serial()->write(checksum);
|
|
_transport.serial()->flush();
|
|
|
|
// Clear echo: data bytes + checksum byte
|
|
clearEcho(len + 1);
|
|
}
|
|
|
|
uint8_t KLineObd2::readResponse(uint8_t* buf, uint8_t maxLen, uint16_t timeoutMs) {
|
|
uint8_t count = 0;
|
|
unsigned long start = millis();
|
|
|
|
// Read first byte (format/header) to determine message length
|
|
int first = readByteTimeout(timeoutMs);
|
|
if (first < 0) return 0;
|
|
buf[count++] = (uint8_t)first;
|
|
|
|
// Determine expected length from format byte
|
|
// Bits 6-7: address mode (00=no addr, 10=phys, 11=func)
|
|
// Bits 0-5: data length (0 = length in separate byte)
|
|
uint8_t fmt = (uint8_t)first;
|
|
uint8_t addrBytes = (fmt & 0x80) ? 2 : 0; // target + source if addressing present
|
|
uint8_t dataLen = fmt & 0x3F;
|
|
|
|
// Read address bytes if present
|
|
for (uint8_t i = 0; i < addrBytes && count < maxLen; i++) {
|
|
int b = readByteTimeout(timeoutMs - (millis() - start));
|
|
if (b < 0) return count;
|
|
buf[count++] = (uint8_t)b;
|
|
}
|
|
|
|
// If dataLen == 0, next byte is the actual length
|
|
if (dataLen == 0 && count < maxLen) {
|
|
int lenByte = readByteTimeout(timeoutMs - (millis() - start));
|
|
if (lenByte < 0) return count;
|
|
buf[count++] = (uint8_t)lenByte;
|
|
dataLen = (uint8_t)lenByte;
|
|
}
|
|
|
|
// Read data bytes + checksum
|
|
uint8_t remaining = dataLen + 1; // +1 for checksum
|
|
for (uint8_t i = 0; i < remaining && count < maxLen; i++) {
|
|
uint16_t elapsed = millis() - start;
|
|
if (elapsed >= timeoutMs) return count;
|
|
int b = readByteTimeout(timeoutMs - elapsed);
|
|
if (b < 0) return count;
|
|
buf[count++] = (uint8_t)b;
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
uint8_t KLineObd2::requestPid(uint8_t mode, uint8_t pid,
|
|
uint8_t* response, uint8_t maxLen) {
|
|
if (!_initialized) return 0;
|
|
|
|
// ISO 14230 format: physical addressing, 2 data bytes
|
|
uint8_t req[] = {
|
|
(uint8_t)(0x82), // fmt: physical addressing, 2 data bytes
|
|
_ecuAddr, // target
|
|
_testerAddr, // source
|
|
mode, // service/mode
|
|
pid // PID
|
|
};
|
|
|
|
sendRequest(req, sizeof(req));
|
|
|
|
uint8_t raw[16];
|
|
uint8_t rawLen = readResponse(raw, sizeof(raw));
|
|
if (rawLen < 5) return 0; // minimum: fmt + target + source + response_mode + pid
|
|
|
|
// Find response data — skip header, copy from response_mode onward
|
|
// Response mode = request mode + 0x40
|
|
uint8_t respMode = mode + 0x40;
|
|
for (uint8_t i = 0; i < rawLen; i++) {
|
|
if (raw[i] == respMode && (i + 1) < rawLen && raw[i + 1] == pid) {
|
|
// Copy data bytes after mode+pid, excluding checksum
|
|
uint8_t dataStart = i + 2;
|
|
uint8_t dataEnd = rawLen - 1; // exclude checksum
|
|
uint8_t dataLen = 0;
|
|
for (uint8_t j = dataStart; j < dataEnd && dataLen < maxLen; j++) {
|
|
response[dataLen++] = raw[j];
|
|
}
|
|
return dataLen;
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
// --- Private helpers ---
|
|
|
|
void KLineObd2::sendByte5Baud(uint8_t data) {
|
|
int8_t txp = _transport.txPin();
|
|
|
|
// Start bit (LOW)
|
|
digitalWrite(txp, LOW);
|
|
delay(200);
|
|
|
|
// 8 data bits, LSB first
|
|
for (uint8_t i = 0; i < 8; i++) {
|
|
digitalWrite(txp, (data >> i) & 0x01 ? HIGH : LOW);
|
|
delay(200);
|
|
}
|
|
|
|
// Stop bit (HIGH)
|
|
digitalWrite(txp, HIGH);
|
|
delay(200);
|
|
}
|
|
|
|
void KLineObd2::clearEcho(uint8_t count) {
|
|
for (uint8_t i = 0; i < count; i++) {
|
|
readByteTimeout(KLINE_ECHO_TIMEOUT_MS);
|
|
}
|
|
}
|
|
|
|
int KLineObd2::readByteTimeout(uint16_t timeoutMs) {
|
|
unsigned long start = millis();
|
|
while (millis() - start < timeoutMs) {
|
|
_transport.drainUart();
|
|
if (_transport.rxAvailable() > 0) {
|
|
return _transport.rxRead();
|
|
}
|
|
delayMicroseconds(100); // prevent busy-wait burning CPU
|
|
}
|
|
return -1;
|
|
}
|