Improved compatibility

4 files changed, 116 insertions, 105 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index 5dc22a2..2dbede8 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -83,19 +83,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;
-	for (byte n = 0; n < count; n++) {
-		p += sprintf(p, "%02X%02X\r", dataMode, pid[n]);		
-	}
-	write(buffer);
-	// receive and parse the response
+	byte results = 0; 
 	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;
 }
@@ -112,9 +104,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) {
@@ -146,6 +136,10 @@ void COBD::clearDTC()
 
 void COBD::write(const char* s)
 {
+#ifdef DEBUG
+	DEBUG.print("<<<");
+	DEBUG.println(s);
+#endif
 	OBDUART.write(s);
 }
 
@@ -325,39 +319,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++) {
+		char buffer[32];
 		if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) {
-			char *p = strstr(buffer, "OBDUART");
+			char *p = strchr(buffer, ' ');
 			if (p) {
-				p += 9;
+				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] == '.') {
@@ -365,10 +363,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;
@@ -376,19 +381,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++) {
@@ -448,6 +458,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];
@@ -508,23 +524,14 @@ void COBD::debugOutput(const char *s)
 * OBD-II I2C Adapter
 *************************************************************************/
 
-void COBDI2C::begin()
+byte COBDI2C::begin()
 {
 	Wire.begin();
 #ifdef DEBUG
 	DEBUG.begin(115200);
 #endif
 	recover();
-
-	char buffer[32];
-	version = 0;
-	if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) {
-		char *p = strstr(buffer, "OBDUART");
-		if (p) {
-			p += 9;
-			version = (*p - '0') * 10 + (*(p + 2) - '0');
-		}
-	}
+	return getVersion();
 }
 
 void COBDI2C::end()
@@ -592,17 +599,6 @@ byte COBDI2C::receive(char* buffer, byte bufsize, int timeout)
 	return 0;
 }
 
-byte COBDI2C::readPID(const byte pid[], byte count, int result[])
-{
-	byte results = 0; 
-	for (byte n = 0; n < count; n++) {
-		if (readPID(pid[n], result[n])) {
-			results++;
-		}
-	}
-	return results;
-}
-
 void COBDI2C::setQueryPID(byte pid, byte obdPid[])
 {
 	byte n = 0;
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index 711ee8e..f106e41 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -13,7 +13,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__)

@@ -102,8 +101,8 @@ class COBD
 {

 public:

 	COBD():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {}

-	// begin serial UART

-	virtual void begin();

+	// begin serial UART, return the version number on success and 0 on failure

+	virtual byte begin();

 	// initialize OBD-II connection

 	virtual bool init(OBD_PROTOCOLS protocol = PROTO_AUTO);

 	// un-initialize OBD-II connection

@@ -129,7 +128,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

@@ -138,14 +137,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

+	// number of subsequent 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);

@@ -200,10 +199,9 @@ typedef struct {
 

 class COBDI2C : public COBD {

 public:

-	void begin();

+	byte begin();

 	void end();

 	bool readPID(byte pid, int& result);

-	byte readPID(const byte pid[], byte count, int result[]);

 	void write(const char* s);

 	// API not applicable

 	bool setBaudRate(unsigned long baudrate) { return false; }

diff --git a/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino b/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino
index 38da62e..810aacf 100644
--- a/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino
+++ b/libraries/OBD/examples/obd_i2c_test/obd_i2c_test.ino
@@ -12,10 +12,11 @@
 #include <OBD.h>
 
 COBDI2C obd;
+bool hasMEMS;
 
 void testOut()
 {
-    static const char cmds[][6] = {"ATZ\r", "ATL1\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
+    static const char cmds[][6] = {"ATZ\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
     char buf[128];
 
     for (byte i = 0; i < sizeof(cmds) / sizeof(cmds[0]); i++) {
@@ -111,15 +112,12 @@ void setup() {
   delay(500);
   obd.begin();
 
+  hasMEMS = obd.memsInit();
   Serial.print("MEMS:");
-  if (obd.memsInit()) {
-    Serial.println("OK");
-  } else {
-    Serial.println("NO");
-  }
-  
+  Serial.println(hasMEMS ? "Yes" : "No");
+
   // send some commands for testing and show response for debugging purpose
-  //testOut();
+  testOut();
 
   // initialize OBD-II adapter
   do {
@@ -151,6 +149,7 @@ void setup() {
 void loop() {
   readPIDs();
   readBatteryVoltage();
-  readMEMS();
+   if (hasMEMS) {
+    readMEMS();
+  }
 }
- 
diff --git a/libraries/OBD/examples/obd_uart_test/obd_uart_test.ino b/libraries/OBD/examples/obd_uart_test/obd_uart_test.ino
index 477ac61..1f58405 100644
--- a/libraries/OBD/examples/obd_uart_test/obd_uart_test.ino
+++ b/libraries/OBD/examples/obd_uart_test/obd_uart_test.ino
@@ -18,10 +18,11 @@ SoftwareSerial mySerial(A2, A3);
 //#define mySerial Serial
 
 COBD obd;
+bool hasMEMS;
 
 void testOut()
 {
-    static const char cmds[][6] = {"ATZ\r", "ATL1\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
+    static const char cmds[][6] = {"ATZ\r", "ATI\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
     char buf[128];
 
     for (byte i = 0; i < sizeof(cmds) / sizeof(cmds[0]); i++) {
@@ -41,6 +42,7 @@ void testOut()
                     mySerial.write('\n');
                 p++;
             }
+            mySerial.println();
         } else {
             mySerial.println("Timeout");
         }
@@ -64,7 +66,7 @@ void readPIDSingle()
 
 void readPIDMultiple()
 {
-    static const byte pids[] = {PID_SPEED, PID_ENGINE_LOAD, PID_THROTTLE, PID_COOLANT_TEMP, PID_INTAKE_TEMP};
+    static const byte pids[] = {PID_SPEED, PID_ENGINE_LOAD, PID_THROTTLE, PID_COOLANT_TEMP};
     int values[sizeof(pids)];
     if (obd.readPID(pids, sizeof(pids), values) == sizeof(pids)) {
       mySerial.print('[');
@@ -92,29 +94,33 @@ void readBatteryVoltage()
 
 void readMEMS()
 {
-  int x, y, z;
-  mySerial.print('[');
-  mySerial.print(millis());
-  mySerial.print(']');
-  if (obd.readAccel(x, y, z)) {
-    mySerial.print("ACC:");
-    mySerial.print(x);
-    mySerial.print('/');
-    mySerial.print(y);
-    mySerial.print('/');
-    mySerial.print(z);
-    mySerial.print(' ');
-  }
-  if (obd.readGyro(x, y, z)) {
-    mySerial.print("GYRO:");
-    mySerial.print(x);
-    mySerial.print('/');
-    mySerial.print(y);
-    mySerial.print('/');
-    mySerial.print(z);
-    mySerial.print(' ');
-  }
-  mySerial.println();
+    int acc[3];
+    int gyro[3];
+    int temp;
+
+    if (!obd.memsRead(acc, gyro, 0, &temp)) return;
+
+    Serial.print('[');
+    Serial.print(millis());
+    Serial.print(']');
+
+    Serial.print("ACC:");
+    Serial.print(acc[0]);
+    Serial.print('/');
+    Serial.print(acc[1]);
+    Serial.print('/');
+    Serial.print(acc[2]);
+
+    Serial.print(" GYRO:");
+    Serial.print(gyro[0]);
+    Serial.print('/');
+    Serial.print(gyro[1]);
+    Serial.print('/');
+    Serial.print(gyro[2]);
+
+    Serial.print(" TEMP:");
+    Serial.print((float)temp / 10, 1);
+    Serial.println("C");
 }
 
 void setup()
@@ -123,15 +129,27 @@ void setup()
   while (!mySerial);
   
   // this will begin serial
-  obd.begin();
+  byte version = obd.begin();
 
-  mySerial.print("Adapter version: ");
-  mySerial.println(obd.version);
+  mySerial.print("Freematics OBD-II Adapter ");
+  if (version > 0) {
+    mySerial.print("Ver. ");
+    mySerial.print(version / 10);
+    mySerial.print('.');
+    mySerial.println(version % 10);
+  } else {
+    mySerial.println("not detected");
+    for (;;);
+  }
   delay(1000);
 
   // send some commands for testing and show response for debugging purpose
-  //testOut();
- 
+  testOut();
+
+  hasMEMS = obd.memsInit();
+  mySerial.print("MEMS:");
+  mySerial.println(hasMEMS ? "Yes" : "No");
+  
   // initialize OBD-II adapter
   do {
     mySerial.println("Init...");
@@ -156,7 +174,7 @@ void setup()
     }
     mySerial.println();
   }
-  delay(3000);
+  delay(5000);
 }
 
 
@@ -165,7 +183,7 @@ void loop()
   readPIDSingle();
   readPIDMultiple();
   readBatteryVoltage();
-  if (obd.version > 10) {
+  if (hasMEMS) {
     readMEMS();
   }
 }