1 files changed, 73 insertions, 65 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index fcb3c92..042e393 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -9,13 +9,15 @@
 #include <avr/pgmspace.h>
 #include "OBD.h"
 
+//#define DEBUG
+#define DEBUG_SERIAL Serial
+
 #define MAX_CMD_LEN 6
 
-const char PROGMEM s_initcmd[][MAX_CMD_LEN] = {"ATZ\r","ATE0\r","ATL1\r"};
-const char PROGMEM s_searching[] = "SEARCHING";
+const char PROGMEM s_initcmd[][MAX_CMD_LEN] = {"ATZ\r","ATE0\r","ATL1\r","0902\r"};
 const char PROGMEM s_cmd_fmt[] = "%02X%02X 1\r";
 const char PROGMEM s_cmd_sleep[] = "atlp\r";
-const char PROGMEM s_cmd_vin[] = "0902\r";
+#define STR_SEARCHING "SEARCHING..."
 
 unsigned int hex2uint16(const char *p)
 {
@@ -62,6 +64,9 @@ void COBD::sendQuery(unsigned char pid)
 {
 	char cmd[8];
 	sprintf_P(cmd, s_cmd_fmt, dataMode, pid);
+#ifdef DEBUG
+	debugOutput(cmd);
+#endif
 	write(cmd);
 }
 
@@ -76,7 +81,6 @@ bool COBD::readSensor(byte pid, int& result, bool passive)
 		dataIdleLoop();
 	} while (!(hasData = available()) && millis() - tick < OBD_TIMEOUT_SHORT);
 	if (!hasData) {
-        write('\r');
 		errors++;
 		return false;
 	}
@@ -92,6 +96,9 @@ bool COBD::available()
 char COBD::read()
 {
 	char c = OBDUART.read();
+#ifdef DEBUG
+    DEBUG_SERIAL.print(c);
+#endif
 	return c;
 }
 
@@ -146,43 +153,10 @@ int COBD::normalizeData(byte pid, char* data)
 
 char* COBD::getResponse(byte& pid, char* buffer)
 {
-	unsigned long startTime = millis();
-	static const char responseSign[3] = {'4', '1', ' '};
-	byte i = 0;
-
-	for (;;) {
-		if (available()) {
-			char c = read();
-			buffer[i] = c;
-			if (++i == OBD_RECV_BUF_SIZE - 1) {
-			    // buffer overflow
-			    break;
-			}
-			if (c == '>' && i > 6) {
-				// prompt char reached
-				break;
-			}
-		} else {
-			buffer[i] = 0;
-			unsigned int timeout;
-			if (dataMode != 1 || strstr_P(buffer, s_searching)) {
-				timeout = OBD_TIMEOUT_LONG;
-			} else {
-				timeout = OBD_TIMEOUT_SHORT;
-			}
-			if (millis() - startTime > timeout) {
-				// timeout
-				errors++;
-				break;
-			}
-			dataIdleLoop();
-		}
-	}
-	buffer[i] = 0;
-
-	if (i > 6) {
+    byte n = receive(buffer);
+	if (n > 6) {
 		char *p = buffer;
-		while ((p = strstr(p, responseSign))) {
+		while ((p = strstr(p, "41 "))) {
 		    p += 3;
 		    byte curpid = hex2uint8(p);
 		    if (pid == 0) pid = curpid;
@@ -236,42 +210,65 @@ void COBD::begin()
 	OBDUART.begin(OBD_SERIAL_BAUDRATE);
 }
 
+byte COBD::receive(char* buffer)
+{
+    unsigned long startTime = millis();
+	unsigned char n = 0;
+    int timeout = OBD_TIMEOUT_SHORT;
+    bool prompted = false;
+
+    buffer[0] = 0;
+    for (;;) {
+        if (available()) {
+            char c = read();
+            if (n > 2 && c == '>') {
+                // prompt char received
+                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);
+                    timeout = OBD_TIMEOUT_LONG;
+                }
+            }
+        } else if (prompted) {
+            break;
+        } else {
+            if (millis() - startTime > timeout) {
+                // timeout
+                return 0;
+            }
+            dataIdleLoop();
+        }
+    }
+
+    return n;
+}
+
 bool COBD::init(bool passive)
 {
 	unsigned long currentMillis;
-	unsigned char n;
-	char prompted;
 	char buffer[OBD_RECV_BUF_SIZE];
 
+    m_state = OBD_CONNECTING;
+
+    write('\r');
+    delay(100);
+    while (available()) read();
+
 	for (unsigned char i = 0; i < sizeof(s_initcmd) / sizeof(s_initcmd[0]); i++) {
 		if (!passive) {
 			char cmd[MAX_CMD_LEN];
 			strcpy_P(cmd, s_initcmd[i]);
+#ifdef DEBUG
+			debugOutput(cmd);
+#endif
 			write(cmd);
 		}
-		n = 0;
-		prompted = 0;
-		currentMillis = millis();
-		for (;;) {
-			if (available()) {
-				char c = read();
-				if (n > 2 && c == '>') {
-				    buffer[n] = 0;
-				    prompted++;
-				} else if (n < OBD_RECV_BUF_SIZE - 1) {
-				    buffer[n++] = c;
-				}
-			} else if (prompted) {
-				break;
-			} else {
-				unsigned long elapsed = millis() - currentMillis;
-				if (elapsed > OBD_TIMEOUT_INIT) {
-					// init timeout
-					//WriteData("\r");
-					return false;
-				}
-				initIdleLoop();
-			}
+		if (receive(buffer) == 0) {
+            return false;
 		}
 	}
 	while (available()) read();
@@ -292,6 +289,17 @@ bool COBD::init(bool passive)
 	}
 	while (available()) read();
 
+    m_state = OBD_CONNECTED;
 	errors = 0;
 	return true;
 }
+
+#ifdef DEBUG
+void COBD::debugOutput(const char *s)
+{
+    DEBUG_SERIAL.print('[');
+    DEBUG_SERIAL.print(millis());
+    DEBUG_SERIAL.print(']');
+    DEBUG_SERIAL.print(s);
+}
+#endif