Update

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;