diff options
Diffstat (limited to 'libraries/OBD')
| -rw-r--r-- | libraries/OBD/OBD.cpp | 26 |
1 files changed, 15 insertions, 11 deletions
diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index f23da93..116a68d 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -401,17 +401,17 @@ bool COBD::init(OBD_PROTOCOLS protocol) const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATH0\r"}; char buffer[64]; + m_state = OBD_DISCONNECTED; for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) { write(initcmd[i]); if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) { - m_state = OBD_DISCONNECTED; return false; } } if (protocol != PROTO_AUTO) { - sprintf_P(buffer, PSTR("ATSP%u\r"), protocol); + sprintf_P(buffer, PSTR("ATSP %u\r"), protocol); + write(buffer); if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 && !strstr(buffer, "OK")) { - m_state = OBD_DISCONNECTED; return false; } } @@ -422,15 +422,19 @@ bool COBD::init(OBD_PROTOCOLS protocol) for (byte i = 0; i < 4; i++) { byte pid = i * 0x20; sendQuery(pid); - char* data = getResponse(pid, buffer, sizeof(buffer)); - if (!data) break; - data--; - for (byte n = 0; n < 4; n++) { - if (data[n * 3] != ' ') - break; - pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1); + if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) > 0) { + char *p = buffer; + while ((p = strstr(p, "41 "))) { + p += 3; + if (hex2uint8(p) == pid) { + p += 2; + for (byte n = 0; n < 4 && *(p + n * 3) == ' '; n++) { + pidmap[i * 4 + n] = hex2uint8(p + n * 3 + 1); + } + success = true; + } + } } - success = true; } if (success) { |