diff options
| -rw-r--r-- | libraries/OBD/OBD.cpp | 138 |
1 files changed, 73 insertions, 65 deletions
diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index fcb3c92..042e393 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -9,13 +9,15 @@ #include <avr/pgmspace.h> #include "OBD.h" +//#define DEBUG +#define DEBUG_SERIAL Serial + #define MAX_CMD_LEN 6 -const char PROGMEM s_initcmd[][MAX_CMD_LEN] = {"ATZ\r","ATE0\r","ATL1\r"}; -const char PROGMEM s_searching[] = "SEARCHING"; +const char PROGMEM s_initcmd[][MAX_CMD_LEN] = {"ATZ\r","ATE0\r","ATL1\r","0902\r"}; const char PROGMEM s_cmd_fmt[] = "%02X%02X 1\r"; const char PROGMEM s_cmd_sleep[] = "atlp\r"; -const char PROGMEM s_cmd_vin[] = "0902\r"; +#define STR_SEARCHING "SEARCHING..." unsigned int hex2uint16(const char *p) { @@ -62,6 +64,9 @@ void COBD::sendQuery(unsigned char pid) { char cmd[8]; sprintf_P(cmd, s_cmd_fmt, dataMode, pid); +#ifdef DEBUG + debugOutput(cmd); +#endif write(cmd); } @@ -76,7 +81,6 @@ bool COBD::readSensor(byte pid, int& result, bool passive) dataIdleLoop(); } while (!(hasData = available()) && millis() - tick < OBD_TIMEOUT_SHORT); if (!hasData) { - write('\r'); errors++; return false; } @@ -92,6 +96,9 @@ bool COBD::available() char COBD::read() { char c = OBDUART.read(); +#ifdef DEBUG + DEBUG_SERIAL.print(c); +#endif return c; } @@ -146,43 +153,10 @@ int COBD::normalizeData(byte pid, char* data) char* COBD::getResponse(byte& pid, char* buffer) { - unsigned long startTime = millis(); - static const char responseSign[3] = {'4', '1', ' '}; - byte i = 0; - - for (;;) { - if (available()) { - char c = read(); - buffer[i] = c; - if (++i == OBD_RECV_BUF_SIZE - 1) { - // buffer overflow - break; - } - if (c == '>' && i > 6) { - // prompt char reached - break; - } - } else { - buffer[i] = 0; - unsigned int timeout; - if (dataMode != 1 || strstr_P(buffer, s_searching)) { - timeout = OBD_TIMEOUT_LONG; - } else { - timeout = OBD_TIMEOUT_SHORT; - } - if (millis() - startTime > timeout) { - // timeout - errors++; - break; - } - dataIdleLoop(); - } - } - buffer[i] = 0; - - if (i > 6) { + byte n = receive(buffer); + if (n > 6) { char *p = buffer; - while ((p = strstr(p, responseSign))) { + while ((p = strstr(p, "41 "))) { p += 3; byte curpid = hex2uint8(p); if (pid == 0) pid = curpid; @@ -236,42 +210,65 @@ void COBD::begin() OBDUART.begin(OBD_SERIAL_BAUDRATE); } +byte COBD::receive(char* buffer) +{ + unsigned long startTime = millis(); + unsigned char n = 0; + int timeout = OBD_TIMEOUT_SHORT; + bool prompted = false; + + buffer[0] = 0; + for (;;) { + if (available()) { + char c = read(); + if (n > 2 && c == '>') { + // prompt char received + prompted = true; + } else if (n < OBD_RECV_BUF_SIZE - 1) { + buffer[n++] = c; + buffer[n] = 0; + if (strstr(buffer, STR_SEARCHING)) { + strcpy(buffer, buffer + sizeof(STR_SEARCHING)); + n -= sizeof(STR_SEARCHING); + timeout = OBD_TIMEOUT_LONG; + } + } + } else if (prompted) { + break; + } else { + if (millis() - startTime > timeout) { + // timeout + return 0; + } + dataIdleLoop(); + } + } + + return n; +} + bool COBD::init(bool passive) { unsigned long currentMillis; - unsigned char n; - char prompted; char buffer[OBD_RECV_BUF_SIZE]; + m_state = OBD_CONNECTING; + + write('\r'); + delay(100); + while (available()) read(); + for (unsigned char i = 0; i < sizeof(s_initcmd) / sizeof(s_initcmd[0]); i++) { if (!passive) { char cmd[MAX_CMD_LEN]; strcpy_P(cmd, s_initcmd[i]); +#ifdef DEBUG + debugOutput(cmd); +#endif write(cmd); } - n = 0; - prompted = 0; - currentMillis = millis(); - for (;;) { - if (available()) { - char c = read(); - if (n > 2 && c == '>') { - buffer[n] = 0; - prompted++; - } else if (n < OBD_RECV_BUF_SIZE - 1) { - buffer[n++] = c; - } - } else if (prompted) { - break; - } else { - unsigned long elapsed = millis() - currentMillis; - if (elapsed > OBD_TIMEOUT_INIT) { - // init timeout - //WriteData("\r"); - return false; - } - initIdleLoop(); - } + if (receive(buffer) == 0) { + return false; } } while (available()) read(); @@ -292,6 +289,17 @@ bool COBD::init(bool passive) } while (available()) read(); + m_state = OBD_CONNECTED; errors = 0; return true; } + +#ifdef DEBUG +void COBD::debugOutput(const char *s) +{ + DEBUG_SERIAL.print('['); + DEBUG_SERIAL.print(millis()); + DEBUG_SERIAL.print(']'); + DEBUG_SERIAL.print(s); +} +#endif |