From a07593476806ce116e36c51b1350a8c2a5d8513c Mon Sep 17 00:00:00 2001
From: Stanley Huang <stanleyhuangyc@gmail.com>
Date: Fri, 13 Jan 2017 10:38:33 +1100
Subject: Improved initialization routine and fixed protocol selection issue

---
 libraries/OBD/OBD.cpp | 42 ++++++++++++------------------------------
 libraries/OBD/OBD.h   |  4 +---
 2 files changed, 13 insertions(+), 33 deletions(-)

(limited to 'libraries/OBD')

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index c66db7b..0219f26 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -265,21 +265,6 @@ bool COBD::getResult(byte& pid, int& result)
 	return true;
 }
 
-bool COBD::setProtocol(OBD_PROTOCOLS h)
-{
-    char buf[32];
-	if (h == PROTO_AUTO) {
-		write("ATSP00\r");
-	} else {
-		sprintf(buf, "ATSP%d\r", h);
-		write(buf);
-	}
-	if (receive(buf, sizeof(buf), OBD_TIMEOUT_LONG) > 0 && strstr(buf, "OK"))
-        return true;
-    else
-        return false;
-}
-
 void COBD::sleep()
 {
   	char buf[32];
@@ -400,29 +385,24 @@ void COBD::recover()
 
 bool COBD::init(OBD_PROTOCOLS protocol)
 {
-	const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r","0100\r"};
+	const char PROGMEM *initcmd[] = {PSTR("ATZ\r"),PSTR("ATE0\r"),PSTR("ATL1\r"),PSTR("ATSP%02u\r")};
 	char buffer[64];
 
-	m_state = OBD_CONNECTING;
-
 	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
+		sprintf_P(buffer, initcmd[i], protocol);
 #ifdef DEBUG
-		debugOutput(initcmd[i]);
+		debugOutput(buffer);
 #endif
-		write(initcmd[i]);
-		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) {
+		write(buffer);
+		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 || (i > 0 && !strstr(buffer, "OK"))) {
 			m_state = OBD_DISCONNECTED;
 			return false;
 		}
-		delay(50);
-	}
-
-	if (protocol != PROTO_AUTO) {
-		setProtocol(protocol);
 	}
 
 	// load pid map
 	memset(pidmap, 0, sizeof(pidmap));
+	bool success = false;
 	for (byte i = 0; i < 4; i++) {
 		byte pid = i * 0x20;
 		sendQuery(pid);
@@ -434,12 +414,14 @@ bool COBD::init(OBD_PROTOCOLS protocol)
 				break;
 			pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1);
 		}
-		delay(100);
+		success = true;
 	}
 
-	m_state = OBD_CONNECTED;
-	errors = 0;
-	return true;
+	if (success) {
+		m_state = OBD_CONNECTED;
+		errors = 0;
+	}
+	return success;
 }
 
 void COBD::end()
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index b6793d7..2e8404f 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -11,7 +11,7 @@
 #define OBD_MODEL_I2C 1
 
 #define OBD_TIMEOUT_SHORT 1000 /* ms */
-#define OBD_TIMEOUT_LONG 15000 /* ms */
+#define OBD_TIMEOUT_LONG 5000 /* ms */
 #define OBD_TIMEOUT_GPS 200 /* ms */
 #define OBD_SERIAL_BAUDRATE 38400
 
@@ -118,8 +118,6 @@ public:
 	virtual byte readPID(const byte pid[], byte count, int result[]);
 	// set device into
 	virtual void sleep();
-	// set working protocol (default auto)
-	virtual bool setProtocol(OBD_PROTOCOLS h = PROTO_AUTO);
 	// send AT command and receive response
 	virtual byte sendCommand(const char* cmd, char* buf, byte bufsize, int timeout = OBD_TIMEOUT_LONG);
 	// read diagnostic trouble codes (return number of DTCs read)
-- 
cgit v1.2.3