Update OBD library

2 files changed, 44 insertions, 54 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index a3186ea..8bcdb9c 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -12,12 +12,6 @@
 //#define DEBUG Serial

 //#define REDIRECT Serial

 

-#define MAX_CMD_LEN 6

-

-const char PROGMEM s_initcmd[][MAX_CMD_LEN] = {"ATZ\r","ATE0\r","ATL1\r"};

-#define STR_CMD_FMT "%02X%02X 1\r"

-#define STR_SEARCHING "SEARCHING..."

-

 unsigned int hex2uint16(const char *p)

 {

 	char c = *p;

@@ -67,7 +61,7 @@ unsigned char hex2uint8(const char *p)
 void COBD::sendQuery(byte pid)

 {

 	char cmd[8];

-	sprintf(cmd, STR_CMD_FMT, dataMode, pid);

+	sprintf(cmd, "%02X%02X\r", dataMode, pid);

 #ifdef DEBUG

 	debugOutput(cmd);

 #endif

@@ -78,16 +72,6 @@ bool COBD::read(byte pid, int& result)
 {

 	// send a query command

 	sendQuery(pid);

-	// wait for reponse

-	bool hasData;

-	unsigned long tick = millis();

-	do {

-		dataIdleLoop();

-	} while (!(hasData = available()) && millis() - tick < OBD_TIMEOUT_SHORT);

-	if (!hasData) {

-		errors++;

-		return false;

-	}

 	// receive and parse the response

 	return getResult(pid, result);

 }

@@ -106,7 +90,7 @@ char COBD::read()
 	return c;

 }

 

-void COBD::write(char* s)

+void COBD::write(const char* s)

 {

 	OBDUART.write(s);

 }

@@ -169,8 +153,7 @@ int COBD::normalizeData(byte pid, char* data)
 

 char* COBD::getResponse(byte& pid, char* buffer)

 {

-    int timeout = OBD_TIMEOUT_SHORT;

-    while (receive(buffer, timeout) > 0) {

+    while (receive(buffer, OBD_TIMEOUT_SHORT) > 0) {

         char *p = buffer;

         while ((p = strstr(p, "41 "))) {

             p += 3;

@@ -183,7 +166,6 @@ char* COBD::getResponse(byte& pid, char* buffer)
                     return p + 1;

             }

         }

-        timeout = 0;

     }

 	return 0;

 }

@@ -194,6 +176,7 @@ bool COBD::getResult(byte& pid, int& result)
 	char* data = getResponse(pid, buffer);

 	if (!data) {

 		recover();

+		errors++;

 		return false;

 	}

 	result = normalizeData(pid, data);

@@ -224,7 +207,7 @@ void COBD::wakeup()
 bool COBD::isValidPID(byte pid)

 {

 	if (pid >= 0x7f)

-		return false;

+		return true;

 	pid--;

 	byte i = pid >> 3;

 	byte b = 0x80 >> (pid & 0x7);

@@ -251,12 +234,11 @@ byte COBD::receive(char* buffer, int timeout)
 	            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);

+	            if (c == '.') {

+                    n = 0;

 	                timeout = OBD_TIMEOUT_LONG;

 	            }

+	            buffer[n] = 0;

 	        }

 	    } else if (prompted) {

 	        break;

@@ -274,46 +256,54 @@ byte COBD::receive(char* buffer, int timeout)
 void COBD::recover()

 {

 	write('\r');

-	delay(50);

+	delay(100);

 	while (available()) read();

 }

 

-bool COBD::init()

+bool COBD::init(byte protocol)

 {

+    const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r"};

 	char buffer[OBD_RECV_BUF_SIZE];

 

 	m_state = OBD_CONNECTING;

 	recover();

 

-	for (unsigned char i = 0; i < sizeof(s_initcmd) / sizeof(s_initcmd[0]); i++) {

-	    char cmd[MAX_CMD_LEN];

-	    strcpy_P(cmd, s_initcmd[i]);

+	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {

 #ifdef DEBUG

-	    debugOutput(cmd);

+	    debugOutput(initcmd[i]);

 #endif

-	    write(cmd);

+	    write(initcmd[i]);

 		if (receive(buffer) == 0) {

 	        return false;

 		}

+		delay(50);

 	}

-	while (available()) read();

-

-	// load pid map

-	memset(pidmap, 0, sizeof(pidmap));

-	for (byte i = 0; i < 4; i++) {

-	    byte pid = i * 0x20;

-	    sendQuery(pid);

-	    char* data = getResponse(pid, buffer);

-	    if (!data) break;

-	    data--;

-	    for (byte n = 0; n < 4; n++) {

-	        if (data[n * 3] != ' ')

-	            break;

-	        pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1);

-	    }

+	if (protocol) {

+        write("ATSP");

+        write('0' + protocol);

+        write('\r');

+        receive(buffer);

+        delay(50);

 	}

 	while (available()) read();

 

+    // load pid map

+    memset(pidmap, 0, sizeof(pidmap));

+    for (byte i = 0; i < 4; i++) {

+        byte pid = i * 0x20;

+        sendQuery(pid);

+        char* data = getResponse(pid, buffer);

+        if (!data) break;

+        data--;

+        for (byte n = 0; n < 4; n++) {

+            if (data[n * 3] != ' ')

+                break;

+            pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1);

+        }

+        delay(100);

+    }

+    while (available()) read();

+

 	m_state = OBD_CONNECTED;

 	errors = 0;

 	return true;

@@ -341,7 +331,7 @@ void COBDI2C::begin(byte addr)
 	memset(obdInfo, 0, sizeof(obdInfo));

 }

 

-bool COBDI2C::init()

+bool COBDI2C::init(byte protocol)

 {

 	m_state = OBD_CONNECTING;

 	sendCommand(CMD_QUERY_STATUS);

@@ -370,7 +360,7 @@ bool COBDI2C::read(byte pid, int& result)
 	return getResult(pid, result);

 }

 

-void COBDI2C::write(char* s)

+void COBDI2C::write(const char* s)

 {

 	COMMAND_BLOCK cmdblock = {millis(), CMD_SEND_AT_COMMAND};

 	Wire.beginTransmission(m_addr);

diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index 02d677a..1877306 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -59,7 +59,7 @@ class COBD
 public:

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

 	virtual void begin();

-	virtual bool init();

+	virtual bool init(byte protocol = 0);

 	virtual bool read(byte pid, int& result);

 	virtual void sleep();

 	virtual void wakeup();

@@ -78,7 +78,7 @@ protected:
 	virtual byte receive(char* buffer, int timeout = OBD_TIMEOUT_SHORT);

 	virtual bool available();

 	virtual char read();

-	virtual void write(char* s);

+	virtual void write(const char* s);

 	virtual void write(char c);

 	virtual void dataIdleLoop() {}

     void recover();

@@ -143,9 +143,9 @@ typedef struct {
 class COBDI2C : public COBD {

 public:

     void begin(byte addr = I2C_ADDR);

-    bool init();

+    bool init(byte protocol = 0);

     bool read(byte pid, int& result);

-    void write(char* s);

+    void write(const char* s);

     void setProtocol(bool auto, byte h);

     // Asynchronized access API

     void setPID(byte pid);