diff options
Diffstat (limited to 'libraries/OBD2UART/OBD2UART.cpp')
| -rw-r--r-- | libraries/OBD2UART/OBD2UART.cpp | 38 |
1 files changed, 13 insertions, 25 deletions
diff --git a/libraries/OBD2UART/OBD2UART.cpp b/libraries/OBD2UART/OBD2UART.cpp index 76968b9..89ca2b1 100644 --- a/libraries/OBD2UART/OBD2UART.cpp +++ b/libraries/OBD2UART/OBD2UART.cpp @@ -1,8 +1,8 @@ /************************************************************************* * Arduino Library for Freematics OBD-II UART Adapter * Distributed under BSD License -* Visit http://freematics.com for more information -* (C)2012-2016 Stanley Huang <stanleyhuangyc@gmail.com> +* Visit https://freematics.com for more information +* (C)2012-2018 Stanley Huang <stanley@freematics.com.au> *************************************************************************/ #include "OBD2UART.h" @@ -417,7 +417,6 @@ bool COBD::init(OBD_PROTOCOLS protocol) byte stage; m_state = OBD_DISCONNECTED; - for (byte n = 0; n < 2; n++) { stage = 0; if (n != 0) reset(); @@ -442,38 +441,27 @@ bool COBD::init(OBD_PROTOCOLS protocol) } stage = 3; // load pid map - memset(pidmap, 0, sizeof(pidmap)); - bool success = false; + memset(pidmap, 0xff, sizeof(pidmap)); for (byte i = 0; i < 4; i++) { byte pid = i * 0x20; sprintf(buffer, "%02X%02X\r", dataMode, pid); delay(10); write(buffer); - if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) > 0) { - if (checkErrorMessage(buffer)) { - break; - } - 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; + delay(10); + if (!receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) || checkErrorMessage(buffer)) break; + for (char *p = buffer; (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); } } - } else { - break; } } - if (success) { - stage = 0xff; - break; - } + break; } - if (stage == 0xff) { + if (stage == 3) { m_state = OBD_CONNECTED; errors = 0; return true; |