From 55251d59c94608f1304fed8f61f1940aaad14e4c Mon Sep 17 00:00:00 2001
From: Stanley Huang <stanleyhuangyc@gmail.com>
Date: Tue, 11 Mar 2014 13:20:02 +0800
Subject: Update OBD library

---
 libraries/OBD/OBD.cpp | 90 +++++++++++++++++++++++----------------------------
 1 file changed, 40 insertions(+), 50 deletions(-)

(limited to 'libraries/OBD/OBD.cpp')

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index a3186ea..8bcdb9c 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -12,12 +12,6 @@
 //#define DEBUG Serial
 //#define REDIRECT Serial
 
-#define MAX_CMD_LEN 6
-
-const char PROGMEM s_initcmd[][MAX_CMD_LEN] = {"ATZ\r","ATE0\r","ATL1\r"};
-#define STR_CMD_FMT "%02X%02X 1\r"
-#define STR_SEARCHING "SEARCHING..."
-
 unsigned int hex2uint16(const char *p)
 {
 	char c = *p;
@@ -67,7 +61,7 @@ unsigned char hex2uint8(const char *p)
 void COBD::sendQuery(byte pid)
 {
 	char cmd[8];
-	sprintf(cmd, STR_CMD_FMT, dataMode, pid);
+	sprintf(cmd, "%02X%02X\r", dataMode, pid);
 #ifdef DEBUG
 	debugOutput(cmd);
 #endif
@@ -78,16 +72,6 @@ bool COBD::read(byte pid, int& result)
 {
 	// send a query command
 	sendQuery(pid);
-	// wait for reponse
-	bool hasData;
-	unsigned long tick = millis();
-	do {
-		dataIdleLoop();
-	} while (!(hasData = available()) && millis() - tick < OBD_TIMEOUT_SHORT);
-	if (!hasData) {
-		errors++;
-		return false;
-	}
 	// receive and parse the response
 	return getResult(pid, result);
 }
@@ -106,7 +90,7 @@ char COBD::read()
 	return c;
 }
 
-void COBD::write(char* s)
+void COBD::write(const char* s)
 {
 	OBDUART.write(s);
 }
@@ -169,8 +153,7 @@ int COBD::normalizeData(byte pid, char* data)
 
 char* COBD::getResponse(byte& pid, char* buffer)
 {
-    int timeout = OBD_TIMEOUT_SHORT;
-    while (receive(buffer, timeout) > 0) {
+    while (receive(buffer, OBD_TIMEOUT_SHORT) > 0) {
         char *p = buffer;
         while ((p = strstr(p, "41 "))) {
             p += 3;
@@ -183,7 +166,6 @@ char* COBD::getResponse(byte& pid, char* buffer)
                     return p + 1;
             }
         }
-        timeout = 0;
     }
 	return 0;
 }
@@ -194,6 +176,7 @@ bool COBD::getResult(byte& pid, int& result)
 	char* data = getResponse(pid, buffer);
 	if (!data) {
 		recover();
+		errors++;
 		return false;
 	}
 	result = normalizeData(pid, data);
@@ -224,7 +207,7 @@ void COBD::wakeup()
 bool COBD::isValidPID(byte pid)
 {
 	if (pid >= 0x7f)
-		return false;
+		return true;
 	pid--;
 	byte i = pid >> 3;
 	byte b = 0x80 >> (pid & 0x7);
@@ -251,12 +234,11 @@ byte COBD::receive(char* buffer, int timeout)
 	            prompted = true;
 	        } else if (n < OBD_RECV_BUF_SIZE - 1) {
 	            buffer[n++] = c;
-	            buffer[n] = 0;
-	            if (strstr(buffer, STR_SEARCHING)) {
-	                strcpy(buffer, buffer + sizeof(STR_SEARCHING));
-	                n -= sizeof(STR_SEARCHING);
+	            if (c == '.') {
+                    n = 0;
 	                timeout = OBD_TIMEOUT_LONG;
 	            }
+	            buffer[n] = 0;
 	        }
 	    } else if (prompted) {
 	        break;
@@ -274,46 +256,54 @@ byte COBD::receive(char* buffer, int timeout)
 void COBD::recover()
 {
 	write('\r');
-	delay(50);
+	delay(100);
 	while (available()) read();
 }
 
-bool COBD::init()
+bool COBD::init(byte protocol)
 {
+    const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r"};
 	char buffer[OBD_RECV_BUF_SIZE];
 
 	m_state = OBD_CONNECTING;
 	recover();
 
-	for (unsigned char i = 0; i < sizeof(s_initcmd) / sizeof(s_initcmd[0]); i++) {
-	    char cmd[MAX_CMD_LEN];
-	    strcpy_P(cmd, s_initcmd[i]);
+	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
 #ifdef DEBUG
-	    debugOutput(cmd);
+	    debugOutput(initcmd[i]);
 #endif
-	    write(cmd);
+	    write(initcmd[i]);
 		if (receive(buffer) == 0) {
 	        return false;
 		}
+		delay(50);
 	}
-	while (available()) read();
-
-	// load pid map
-	memset(pidmap, 0, sizeof(pidmap));
-	for (byte i = 0; i < 4; i++) {
-	    byte pid = i * 0x20;
-	    sendQuery(pid);
-	    char* data = getResponse(pid, 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 (protocol) {
+        write("ATSP");
+        write('0' + protocol);
+        write('\r');
+        receive(buffer);
+        delay(50);
 	}
 	while (available()) read();
 
+    // load pid map
+    memset(pidmap, 0, sizeof(pidmap));
+    for (byte i = 0; i < 4; i++) {
+        byte pid = i * 0x20;
+        sendQuery(pid);
+        char* data = getResponse(pid, 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);
+        }
+        delay(100);
+    }
+    while (available()) read();
+
 	m_state = OBD_CONNECTED;
 	errors = 0;
 	return true;
@@ -341,7 +331,7 @@ void COBDI2C::begin(byte addr)
 	memset(obdInfo, 0, sizeof(obdInfo));
 }
 
-bool COBDI2C::init()
+bool COBDI2C::init(byte protocol)
 {
 	m_state = OBD_CONNECTING;
 	sendCommand(CMD_QUERY_STATUS);
@@ -370,7 +360,7 @@ bool COBDI2C::read(byte pid, int& result)
 	return getResult(pid, result);
 }
 
-void COBDI2C::write(char* s)
+void COBDI2C::write(const char* s)
 {
 	COMMAND_BLOCK cmdblock = {millis(), CMD_SEND_AT_COMMAND};
 	Wire.beginTransmission(m_addr);
-- 
cgit v1.2.3