Update

2 files changed, 55 insertions, 40 deletions

diff --git a/libraries/OBD2UART/OBD2UART.cpp b/libraries/OBD2UART/OBD2UART.cpp
index d9fbdec..99f1e7b 100644
--- a/libraries/OBD2UART/OBD2UART.cpp
+++ b/libraries/OBD2UART/OBD2UART.cpp
@@ -81,19 +81,11 @@ bool COBD::readPID(byte pid, int& result)
 
 byte COBD::readPID(const byte pid[], byte count, int result[])
 {
-	// send a multiple query command
-	char buffer[128];
-	char *p = buffer;
-	byte results = 0;
+	byte results = 0; 
 	for (byte n = 0; n < count; n++) {
-		p += sprintf(p, "%02X%02X\r", dataMode, pid[n]);		
-	}
-	write(buffer);
-	// receive and parse the response
-	for (byte n = 0; n < count; n++) {
-		byte curpid = pid[n];
-		if (getResult(curpid, result[n]))
+		if (readPID(pid[n], result[n])) {
 			results++;
+		}
 	}
 	return results;
 }
@@ -110,9 +102,7 @@ byte COBD::readDTC(uint16_t codes[], byte maxCodes)
 		char buffer[128];
 		sprintf_P(buffer, n == 0 ? PSTR("03\r") : PSTR("03%02X\r"), n);
 		write(buffer);
-		Serial.println(buffer);
 		if (receive(buffer, sizeof(buffer)) > 0) {
-			Serial.println(buffer);
 			if (!strstr(buffer, "NO DATA")) {
 				char *p = strstr(buffer, "43");
 				if (p) {
@@ -144,6 +134,10 @@ void COBD::clearDTC()
 
 void COBD::write(const char* s)
 {
+#ifdef DEBUG
+	DEBUG.print("<<<");
+	DEBUG.println(s);
+#endif
 	OBDUART.write(s);
 }
 
@@ -323,39 +317,43 @@ bool COBD::isValidPID(byte pid)
 	return pidmap[i] & b;
 }
 
-void COBD::begin()
+byte COBD::begin()
 {
-	OBDUART.begin(OBD_SERIAL_BAUDRATE);
-#ifdef DEBUG
-	DEBUG.begin(115200);
-#endif
-	recover();
+	long baudrates[] = {38400, 115200};
+	byte version = 0;
+	for (byte n = 0; n < sizeof(baudrates) / sizeof(baudrates[0]) && version == 0; n++) {
+		OBDUART.begin(baudrates[n]);
+		version = getVersion(); 
+	}
+	return version;	
+}
 
-	char buffer[32];
-	version = 0;
+byte COBD::getVersion()
+{
+	byte version = 0;
 	for (byte n = 0; n < 3; n++) {
-	if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) {
-		char *p = strstr(buffer, "OBDUART");
-		if (p) {
-			p += 9;
-			version = (*p - '0') * 10 + (*(p + 2) - '0');
+		char buffer[32];
+		if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) {
+			char *p = strchr(buffer, ' ');
+			if (p) {
+				p += 2;
+				version = (*p - '0') * 10 + (*(p + 2) - '0');
 				break;
 			}
 		}
 	}
+	return version;
 }
 
 byte COBD::receive(char* buffer, byte bufsize, int timeout)
 {
 	unsigned char n = 0;
 	unsigned long startTime = millis();
+	char c = 0;
 	for (;;) {
 		if (OBDUART.available()) {
-			char c = OBDUART.read();
-			if (n > 2 && c == '>') {
-				// prompt char received
-				break;
-			} else if (!buffer) {
+			c = OBDUART.read();
+			if (!buffer) {
 			       n++;
 			} else if (n < bufsize - 1) {
 				if (c == '.' && n > 2 && buffer[n - 1] == '.' && buffer[n - 2] == '.') {
@@ -363,10 +361,17 @@ byte COBD::receive(char* buffer, byte bufsize, int timeout)
 					n = 0;
 					timeout = OBD_TIMEOUT_LONG;
 				} else {
+					if (c == '\r' || c == '\n' || c == ' ') {
+						if (n == 0 || buffer[n - 1] == '\r' || buffer[n - 1] == '\n') continue;
+					}
 					buffer[n++] = c;
 				}
 			}
 		} else {
+			if (c == '>') {
+				// prompt char received
+				break;
+			}
 			if (millis() - startTime > timeout) {
 			    // timeout
 			    break;
@@ -374,19 +379,24 @@ byte COBD::receive(char* buffer, byte bufsize, int timeout)
 			dataIdleLoop();
 		}
 	}
-	if (buffer) buffer[n] = 0;
+	if (buffer) {
+		buffer[n] = 0;
+	}
+#ifdef DEBUG
+	DEBUG.print(">>>");
+	DEBUG.println(buffer);
+#endif
 	return n;
 }
 
 void COBD::recover()
 {
-	char buf[16];
-	sendCommand("AT\r", buf, sizeof(buf));
+	sendCommand("\r", 0, 0);
 }
 
 bool COBD::init(OBD_PROTOCOLS protocol)
 {
-	const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATL1\r"};
+	const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATH0\r"};
 	char buffer[64];
 
 	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
@@ -446,6 +456,12 @@ bool COBD::setBaudRate(unsigned long baudrate)
     return true;
 }
 
+bool COBD::memsInit()
+{
+	char buf[16];
+	return sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0 && !strchr(buf, '?');
+}
+
 bool COBD::memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0)
 {
 	char buf[64];
diff --git a/libraries/OBD2UART/OBD2UART.h b/libraries/OBD2UART/OBD2UART.h
index 2103cc5..706d4b8 100644
--- a/libraries/OBD2UART/OBD2UART.h
+++ b/libraries/OBD2UART/OBD2UART.h
@@ -10,7 +10,6 @@
 #define OBD_TIMEOUT_SHORT 1000 /* ms */
 #define OBD_TIMEOUT_LONG 5000 /* ms */
 #define OBD_TIMEOUT_GPS 200 /* ms */
-#define OBD_SERIAL_BAUDRATE 38400
 
 #ifndef OBDUART
 #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168P__)
@@ -100,7 +99,7 @@ class COBD
 public:
 	COBD():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {}
 	// begin serial UART
-	virtual void begin();
+	virtual byte begin();
 	// initialize OBD-II connection
 	virtual bool init(OBD_PROTOCOLS protocol = PROTO_AUTO);
 	// un-initialize OBD-II connection
@@ -126,7 +125,7 @@ public:
 	// get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE
 	virtual bool getVIN(char* buffer, byte bufsize);
 	// initialize MEMS sensor
-	virtual bool memsInit() { return version > 10; }
+	virtual bool memsInit();
 	// read out MEMS data (acc for accelerometer, gyr for gyroscope, temp in 0.1 celcius degree)
 	virtual bool memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0);
 	// send query for specified PID
@@ -135,14 +134,14 @@ public:
 	virtual bool getResult(byte& pid, int& result);
 	// determine if the PID is supported
 	virtual bool isValidPID(byte pid);
+	// get adapter firmware version
+	virtual byte getVersion();
 	// set current PID mode
 	byte dataMode;
 	// occurrence of errors
 	byte errors;
 	// bit map of supported PIDs
 	byte pidmap[4 * 4];
-	// adapter version
-	byte version;
 protected:
 	virtual char* getResponse(byte& pid, char* buffer, byte bufsize);
 	virtual byte receive(char* buffer, byte bufsize, int timeout = OBD_TIMEOUT_SHORT);