Update OBD-II library

2 files changed, 47 insertions, 31 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index 8df7410..6ebdce4 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -8,8 +8,7 @@
 #include <Arduino.h>
 #include "OBD.h"
 
-//#define DEBUG Serial
-//#define REDIRECT Serial
+#define DEBUG Serial
 
 uint16_t hex2uint16(const char *p)
 {
@@ -89,8 +88,8 @@ bool COBD::available()
 char COBD::read()
 {
 	char c = OBDUART.read();
-#ifdef REDIRECT
-    REDIRECT.write(c);
+#ifdef DEBUG
+    DEBUG.write(c);
 #endif
 	return c;
 }
@@ -281,6 +280,9 @@ bool COBD::isValidPID(byte pid)
 void COBD::begin()
 {
 	OBDUART.begin(OBD_SERIAL_BAUDRATE);
+#ifdef DEBUG
+    DEBUG.begin(115200);
+#endif
 }
 
 byte COBD::receive(char* buffer, int timeout)
@@ -321,7 +323,7 @@ void COBD::recover()
 	while (available()) read();
 }
 
-bool COBD::init(byte protocol)
+bool COBD::init(OBD_PROTOCOLS protocol)
 {
 	const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r"};
 	char buffer[OBD_RECV_BUF_SIZE];
@@ -331,23 +333,26 @@ bool COBD::init(byte protocol)
 
 	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
 #ifdef DEBUG
-	    debugOutput(initcmd[i]);
+		debugOutput(initcmd[i]);
 #endif
-	    write(initcmd[i]);
+		write(initcmd[i]);
 		if (receive(buffer) == 0) {
-	        return false;
+			m_state = OBD_DISCONNECTED;
+			return false;
 		}
 		delay(50);
 	}
-	if (protocol) {
-	        write("ATSP");
-	        write('0' + protocol);
-	        write('\r');
-	        receive(buffer);
-	        delay(50);
-	}
 	while (available()) read();
 
+	if (protocol != PROTO_AUTO) {
+		setProtocol(protocol);
+	}
+    int value;
+	if (!read(PID_RPM, value)) {
+		m_state = OBD_DISCONNECTED;
+		return false;
+	}
+
 	// load pid map
 	memset(pidmap, 0, sizeof(pidmap));
 	for (byte i = 0; i < 4; i++) {
@@ -405,6 +410,9 @@ void COBDI2C::begin()
 	Wire.begin();
 	memset(obdPid, 0, sizeof(obdPid));
 	memset(obdInfo, 0, sizeof(obdInfo));
+#ifdef DEBUG
+    DEBUG.begin(115200);
+#endif
 }
 
 void COBDI2C::end()
@@ -412,8 +420,9 @@ void COBDI2C::end()
 	m_state = OBD_DISCONNECTED;
 }
 
-bool COBDI2C::init(byte protocol)
+bool COBDI2C::init(OBD_PROTOCOLS protocol)
 {
+	bool success = false;
 	m_state = OBD_CONNECTING;
 	sendCommand(CMD_QUERY_STATUS);
 
@@ -426,14 +435,19 @@ bool COBDI2C::init(byte protocol)
 	}
 	if (recvbuf[4] == 'Y') {
 		memcpy(pidmap, recvbuf + 16, sizeof(pidmap));
-		int value;
-		if (read(PID_RPM, value)) {
-			m_state = OBD_CONNECTED;
-			return true;
+		if (protocol != PROTO_AUTO) {
+			setProtocol(protocol);
 		}
+        int value;
+        success = read(PID_RPM, value);
+	}
+	if (success) {
+		return true;
+		m_state = OBD_CONNECTED;
+	} else {
+		m_state = OBD_DISCONNECTED;
+		return false;
 	}
-	m_state = OBD_DISCONNECTED;
-	return false;
 }
 
 bool COBDI2C::read(byte pid, int& result)
@@ -470,8 +484,10 @@ byte COBDI2C::receive(char* buffer, int timeout)
 		Wire.requestFrom((byte)I2C_ADDR, (byte)MAX_PAYLOAD_SIZE, (byte)1);
 
 		bool hasEnd = false;
-		for (byte i = 0; i < MAX_PAYLOAD_SIZE; i++) {
-			if ((buffer[offset + i] = Wire.read()) == 0)
+		for (byte i = 0; i < MAX_PAYLOAD_SIZE && Wire.available(); i++) {
+            char c = Wire.read();
+            buffer[offset + i] = c;
+			if (c == 0)
 				hasEnd = true;
 		}
 
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index 02d735d..b2249dd 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -106,7 +106,7 @@ public:
 	*/

 	virtual void begin();

 	// initialize OBD-II connection

-	virtual bool init(byte protocol = 0);

+	virtual bool init(OBD_PROTOCOLS protocol = PROTO_AUTO);

 	// un-initialize OBD-II connection

 	virtual void end();

 	// set serial baud rate

@@ -139,17 +139,17 @@ public:
 	byte pidmap[4 * 4];

 protected:

 	virtual char* getResponse(byte& pid, char* buffer);

-	virtual byte receive(char* buffer = 0, int timeout = OBD_TIMEOUT_SHORT);

-	virtual bool available();

-	virtual char read();

-	virtual void write(const char* s);

-	virtual void write(char c);

 	virtual void dataIdleLoop() {}

 	void recover();

 	void debugOutput(const char* s);

 	int normalizeData(byte pid, char* data);

 	OBD_STATES m_state;

 private:

+	virtual byte receive(char* buffer = 0, int timeout = OBD_TIMEOUT_SHORT);

+	virtual bool available();

+	virtual char read();

+	virtual void write(const char* s);

+	virtual void write(char c);

 	virtual uint8_t getPercentageValue(char* data)

 	{

 		return (uint16_t)hex2uint8(data) * 100 / 255;

@@ -208,7 +208,7 @@ class COBDI2C : public COBD {
 public:

     void begin();

 	void end();

-    bool init(byte protocol = 0);

+    bool init(OBD_PROTOCOLS protocol = PROTO_AUTO);

     bool read(byte pid, int& result);

     void write(const char* s);

     // Asynchronized access API