Fixed manual setting protocol issue

4 files changed, 101 insertions, 84 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index 88ed733..5dc22a2 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -388,16 +388,19 @@ void COBD::recover()
 
 bool COBD::init(OBD_PROTOCOLS protocol)
 {
-	const char PROGMEM *initcmd[] = {PSTR("ATZ\r"),PSTR("ATE0\r"),PSTR("ATL1\r"),PSTR("ATSP%02u\r")};
+	const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATL1\r"};
 	char buffer[64];
 
 	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
-		sprintf_P(buffer, initcmd[i], protocol);
-#ifdef DEBUG
-		debugOutput(buffer);
-#endif
-		write(buffer);
-		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 || (i > 0 && !strstr(buffer, "OK"))) {
+		write(initcmd[i]);
+		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) {
+			m_state = OBD_DISCONNECTED;
+			return false;
+		}
+	}
+	if (protocol != PROTO_AUTO) {
+		sprintf_P(buffer, PSTR("ATSP%u\r"), protocol);
+		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 && !strstr(buffer, "OK")) {
 			m_state = OBD_DISCONNECTED;
 			return false;
 		}
diff --git a/libraries/OBD2UART/OBD2UART.cpp b/libraries/OBD2UART/OBD2UART.cpp
index 7f6fb39..d9fbdec 100644
--- a/libraries/OBD2UART/OBD2UART.cpp
+++ b/libraries/OBD2UART/OBD2UART.cpp
@@ -333,11 +333,14 @@ void COBD::begin()
 
 	char buffer[32];
 	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');
+				break;
+			}
 		}
 	}
 }
@@ -383,16 +386,19 @@ void COBD::recover()
 
 bool COBD::init(OBD_PROTOCOLS protocol)
 {
-	const char PROGMEM *initcmd[] = {PSTR("ATZ\r"),PSTR("ATE0\r"),PSTR("ATL1\r"),PSTR("ATSP%02u\r")};
+	const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATL1\r"};
 	char buffer[64];
 
 	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
-		sprintf_P(buffer, initcmd[i], protocol);
-#ifdef DEBUG
-		debugOutput(buffer);
-#endif
-		write(buffer);
-		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 || (i > 0 && !strstr(buffer, "OK"))) {
+		write(initcmd[i]);
+		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) {
+			m_state = OBD_DISCONNECTED;
+			return false;
+		}
+	}
+	if (protocol != PROTO_AUTO) {
+		sprintf_P(buffer, PSTR("ATSP%u\r"), protocol);
+		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 && !strstr(buffer, "OK")) {
 			m_state = OBD_DISCONNECTED;
 			return false;
 		}
@@ -440,48 +446,50 @@ bool COBD::setBaudRate(unsigned long baudrate)
     return true;
 }
 
-float COBD::getTemperature()
+bool COBD::memsRead(int* acc, int* gyr = 0, int* mag = 0, int* temp = 0)
 {
-	char buf[32];
-	if (sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0) {
-		char* p = getResultValue(buf);
-		if (p) return (float)(atoi(p) + 12412) / 340;
+	char buf[64];
+	bool success;
+	if (acc) {
+		success = false;
+		if (sendCommand("ATACL\r", buf, sizeof(buf)) > 0) do {
+			char* p = getResultValue(buf);
+			if (!p) break;
+			acc[0] = atoi(p++);
+			if (!(p = strchr(p, ','))) break;
+			acc[1] = atoi(++p);
+			if (!(p = strchr(p, ','))) break;
+			acc[2] = atoi(++p);
+			success = true;
+		} while (0);
+		if (!success) return false;
 	}
-	else {
-		return -1000;
+	if (gyr) {
+		success = false;
+		if (sendCommand("ATGYRO\r", buf, sizeof(buf)) > 0) do {
+			char* p = getResultValue(buf);
+			if (!p) break;
+			gyr[0] = atoi(p++);
+			if (!(p = strchr(p, ','))) break;
+			gyr[1] = atoi(++p);
+			if (!(p = strchr(p, ','))) break;
+			gyr[2] = atoi(++p);
+			success = true;
+		} while (0);
+		if (!success) return false;
 	}
-}
-
-bool COBD::readAccel(int& x, int& y, int& z)
-{
-	char buf[32];
-	if (sendCommand("ATACL\r", buf, sizeof(buf)) > 0) do {
-		char* p = getResultValue(buf);
-		if (!p) break;
-		x = atoi(p++);
-		if (!(p = strchr(p, ','))) break;
-		y = atoi(++p);
-		if (!(p = strchr(p, ','))) break;
-		z = atoi(++p);
-		return true;
-	} while (0);
-	return false;
-}
-
-bool COBD::readGyro(int& x, int& y, int& z)
-{
-	char buf[32];
-	if (sendCommand("ATGYRO\r", buf, sizeof(buf)) > 0) do {
-		char* p = getResultValue(buf);
-		if (!p) break;
-		x = atoi(p++);
-		if (!(p = strchr(p, ','))) break;
-		y = atoi(++p);
-		if (!(p = strchr(p, ','))) break;
-		z = atoi(++p);
-		return true;
-	} while (0);
-	return false;
+	if (temp) {
+		success = false;
+		if (sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0) {
+			char* p = getResultValue(buf);
+			if (p) {
+				*temp = (atoi(p) + 12412) / 34;
+				success = true;
+			}
+		}
+		if (!success) return false;
+	}
+	return true;	
 }
 
 #ifdef DEBUG
diff --git a/libraries/OBD2UART/OBD2UART.h b/libraries/OBD2UART/OBD2UART.h
index 149e635..2103cc5 100644
--- a/libraries/OBD2UART/OBD2UART.h
+++ b/libraries/OBD2UART/OBD2UART.h
@@ -125,20 +125,16 @@ public:
 	virtual float getVoltage();
 	// get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE
 	virtual bool getVIN(char* buffer, byte bufsize);
-	// get device temperature (in celsius degree) 
-	virtual float getTemperature();
-	// get accelerometer data
-	virtual bool readAccel(int& x, int& y, int& z);
-	// get gyroscope data
-	virtual bool readGyro(int& x, int& y, int& z);
+	// initialize MEMS sensor
+	virtual bool memsInit() { return version > 10; }
+	// 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
 	virtual void sendQuery(byte pid);
 	// retrive and parse the response of specifie PID
 	virtual bool getResult(byte& pid, int& result);
 	// determine if the PID is supported
 	virtual bool isValidPID(byte pid);
-	// init GPS module
-	// parse GPS data
 	// set current PID mode
 	byte dataMode;
 	// occurrence of errors
diff --git a/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino b/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino
index 2d35485..4307648 100644
--- a/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino
+++ b/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino
@@ -1,9 +1,9 @@
 /*************************************************************************
 * Testing sketch for Freematics OBD-II UART Adapter
-* Reads and prints several OBD-II PIDs value
+* Reads and prints several OBD-II PIDs value and MEMS sensor data
 * Distributed under GPL v2.0
 * Visit http://freematics.com for more information
-* Written by Stanley Huang <stanleyhuangyc@gmail.com>
+* Written by Stanley Huang <support@freematics.com.au>
 *************************************************************************/
 
 #include <SoftwareSerial.h>
@@ -63,7 +63,7 @@ void readPIDSingle()
 
 void readPIDMultiple()
 {
-    static const byte pids[] = {PID_SPEED, PID_ENGINE_LOAD, PID_THROTTLE, PID_COOLANT_TEMP, PID_INTAKE_TEMP};
+    const byte pids[] = {PID_ENGINE_LOAD, PID_COOLANT_TEMP, PID_RPM, PID_SPEED, PID_TIMING_ADVANCE, PID_INTAKE_TEMP, PID_THROTTLE, PID_FUEL_LEVEL};
     int values[sizeof(pids)];
     if (obd.readPID(pids, sizeof(pids), values) == sizeof(pids)) {
       mySerial.print('[');
@@ -91,29 +91,33 @@ void readBatteryVoltage()
 
 void readMEMS()
 {
-  int x, y, z;
+  int acc[3];
+  int gyro[3];
+  int temp;
+
+  if (!obd.memsRead(acc, gyro, 0, &temp)) return;
+
   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();
+
+  mySerial.print("ACC:");
+  mySerial.print(acc[0]);
+  mySerial.print('/');
+  mySerial.print(acc[1]);
+  mySerial.print('/');
+  mySerial.print(acc[2]);
+
+  mySerial.print(" GYRO:");
+  mySerial.print(gyro[0]);
+  mySerial.print('/');
+  mySerial.print(gyro[1]);
+  mySerial.print('/');
+  mySerial.print(gyro[2]);
+
+  mySerial.print(" TEMP:");
+  mySerial.print((float)temp / 10, 1);
+  mySerial.println("C");
 }
 
 void setup()
@@ -131,6 +135,12 @@ void setup()
   // send some commands for testing and show response for debugging purpose
   //testOut();
  
+  Serial.print("MEMS:");
+  if (obd.memsInit()) {
+    Serial.println("OK");
+  } else {
+    Serial.println("NO");
+  }
   // initialize OBD-II adapter
   do {
     mySerial.println("Init...");