diff options
Diffstat (limited to 'libraries/OBD/OBD.cpp')
| -rw-r--r-- | libraries/OBD/OBD.cpp | 104 |
1 files changed, 50 insertions, 54 deletions
diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index 5dc22a2..2dbede8 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -83,19 +83,11 @@ bool COBD::readPID(byte pid, int& result) byte COBD::readPID(const byte pid[], byte count, int result[]) { - // send a multiple query command - char buffer[128]; - char *p = buffer; - byte results = 0; - for (byte n = 0; n < count; n++) { - p += sprintf(p, "%02X%02X\r", dataMode, pid[n]); - } - write(buffer); - // receive and parse the response + byte results = 0; for (byte n = 0; n < count; n++) { - byte curpid = pid[n]; - if (getResult(curpid, result[n])) + if (readPID(pid[n], result[n])) { results++; + } } return results; } @@ -112,9 +104,7 @@ byte COBD::readDTC(uint16_t codes[], byte maxCodes) char buffer[128]; sprintf_P(buffer, n == 0 ? PSTR("03\r") : PSTR("03%02X\r"), n); write(buffer); - Serial.println(buffer); if (receive(buffer, sizeof(buffer)) > 0) { - Serial.println(buffer); if (!strstr(buffer, "NO DATA")) { char *p = strstr(buffer, "43"); if (p) { @@ -146,6 +136,10 @@ void COBD::clearDTC() void COBD::write(const char* s) { +#ifdef DEBUG + DEBUG.print("<<<"); + DEBUG.println(s); +#endif OBDUART.write(s); } @@ -325,39 +319,43 @@ bool COBD::isValidPID(byte pid) return pidmap[i] & b; } -void COBD::begin() +byte COBD::begin() { - OBDUART.begin(OBD_SERIAL_BAUDRATE); -#ifdef DEBUG - DEBUG.begin(115200); -#endif - recover(); + long baudrates[] = {38400, 115200}; + byte version = 0; + for (byte n = 0; n < sizeof(baudrates) / sizeof(baudrates[0]) && version == 0; n++) { + OBDUART.begin(baudrates[n]); + version = getVersion(); + } + return version; +} - char buffer[32]; - version = 0; +byte COBD::getVersion() +{ + byte version = 0; for (byte n = 0; n < 3; n++) { + char buffer[32]; if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) { - char *p = strstr(buffer, "OBDUART"); + char *p = strchr(buffer, ' '); if (p) { - p += 9; + p += 2; version = (*p - '0') * 10 + (*(p + 2) - '0'); break; } } } + return version; } byte COBD::receive(char* buffer, byte bufsize, int timeout) { unsigned char n = 0; unsigned long startTime = millis(); + char c = 0; for (;;) { if (OBDUART.available()) { - char c = OBDUART.read(); - if (n > 2 && c == '>') { - // prompt char received - break; - } else if (!buffer) { + c = OBDUART.read(); + if (!buffer) { n++; } else if (n < bufsize - 1) { if (c == '.' && n > 2 && buffer[n - 1] == '.' && buffer[n - 2] == '.') { @@ -365,10 +363,17 @@ byte COBD::receive(char* buffer, byte bufsize, int timeout) n = 0; timeout = OBD_TIMEOUT_LONG; } else { + if (c == '\r' || c == '\n' || c == ' ') { + if (n == 0 || buffer[n - 1] == '\r' || buffer[n - 1] == '\n') continue; + } buffer[n++] = c; } } } else { + if (c == '>') { + // prompt char received + break; + } if (millis() - startTime > timeout) { // timeout break; @@ -376,19 +381,24 @@ byte COBD::receive(char* buffer, byte bufsize, int timeout) dataIdleLoop(); } } - if (buffer) buffer[n] = 0; + if (buffer) { + buffer[n] = 0; + } +#ifdef DEBUG + DEBUG.print(">>>"); + DEBUG.println(buffer); +#endif return n; } void COBD::recover() { - char buf[16]; - sendCommand("AT\r", buf, sizeof(buf)); + sendCommand("\r", 0, 0); } bool COBD::init(OBD_PROTOCOLS protocol) { - const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATL1\r"}; + const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATH0\r"}; char buffer[64]; for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) { @@ -448,6 +458,12 @@ bool COBD::setBaudRate(unsigned long baudrate) return true; } +bool COBD::memsInit() +{ + char buf[16]; + return sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0 && !strchr(buf, '?'); +} + bool COBD::memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0) { char buf[64]; @@ -508,23 +524,14 @@ void COBD::debugOutput(const char *s) * OBD-II I2C Adapter *************************************************************************/ -void COBDI2C::begin() +byte COBDI2C::begin() { Wire.begin(); #ifdef DEBUG DEBUG.begin(115200); #endif recover(); - - char buffer[32]; - version = 0; - if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) { - char *p = strstr(buffer, "OBDUART"); - if (p) { - p += 9; - version = (*p - '0') * 10 + (*(p + 2) - '0'); - } - } + return getVersion(); } void COBDI2C::end() @@ -592,17 +599,6 @@ byte COBDI2C::receive(char* buffer, byte bufsize, int timeout) return 0; } -byte COBDI2C::readPID(const byte pid[], byte count, int result[]) -{ - byte results = 0; - for (byte n = 0; n < count; n++) { - if (readPID(pid[n], result[n])) { - results++; - } - } - return results; -} - void COBDI2C::setQueryPID(byte pid, byte obdPid[]) { byte n = 0; |