From 852b10e7589aca4444adbdc0bd4b5eb1b9bb18ca Mon Sep 17 00:00:00 2001
From: Stanley Huang <stanleyhuangyc@gmail.com>
Date: Wed, 6 Jan 2016 23:42:25 +1100
Subject: Fix issue with slower protocols

---
 libraries/OBD/OBD.cpp | 64 +++------------------------------------------------
 libraries/OBD/OBD.h   | 15 +-----------
 2 files changed, 4 insertions(+), 75 deletions(-)

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index 277eaf1..9015051 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -56,11 +56,11 @@ byte hex2uint8(const char *p)
 * OBD-II UART Adapter
 *************************************************************************/
 
-byte COBD::sendCommand(const char* cmd, char* buf, byte bufsize)
+byte COBD::sendCommand(const char* cmd, char* buf, byte bufsize, int timeout)
 {
 	write(cmd);
 	dataIdleLoop();
-	return receive(buf, bufsize, OBD_TIMEOUT_LONG);
+	return receive(buf, bufsize, timeout);
 }
 
 void COBD::sendQuery(byte pid)
@@ -316,11 +316,10 @@ void COBD::recover()
 
 bool COBD::init(OBD_PROTOCOLS protocol)
 {
-	const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r"};
+	const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r","0100\r"};
 	char buffer[64];
 
 	m_state = OBD_CONNECTING;
-	//recover();
 
 	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
 #ifdef DEBUG
@@ -343,11 +342,6 @@ bool COBD::init(OBD_PROTOCOLS protocol)
 	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));
@@ -389,58 +383,6 @@ bool COBD::setBaudRate(unsigned long baudrate)
     return true;
 }
 
-bool COBD::initGPS(unsigned long baudrate)
-{
-    char buf[32];
-	return (sendCommand(buf, buf, sizeof(buf)) && strstr(buf, "OK"));
-}
-
-bool COBD::getGPSData(GPS_DATA* gdata)
-{
-    char buf[128];
-    char *p;
-	if (sendCommand("ATGPS\r", buf, sizeof(buf)) == 0 || !(p = strstr(buf, "$GPS")))
-        return false;
-
-    byte index = 0;
-    char *s = buf;
-	s = *(p + 4) == '$' ? p + 9 : p + 5;
-    for (p = s; *p; p++) {
-        char c = *p;
-        if (c == ',' || c == '>' || c <= 0x0d) {
-            long value = atol(s);
-            switch (index) {
-            case 0:
-                gdata->date = (uint32_t)value;
-                break;
-            case 1:
-                gdata->time = (uint32_t)value;
-                break;
-            case 2:
-                gdata->lat = value;
-                break;
-            case 3:
-                gdata->lon = value;
-                break;
-            case 4:
-                gdata->alt = value;
-                break;
-            case 5:
-                gdata->speed = value;
-                break;
-            case 6:
-                gdata->heading = value;
-                break;
-            case 7:
-                gdata->sat = value;
-                break;
-            }
-            index++;
-            s = p + 1;
-        }
-    }
-    return index >= 4;
-}
 
 #ifdef DEBUG
 void COBD::debugOutput(const char *s)
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index 81b442e..dd1cbf6 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -93,17 +93,6 @@ typedef enum {
     OBD_CONNECTED = 2
 } OBD_STATES;
 
-typedef struct {
-    uint32_t date;
-    uint32_t time;
-    int32_t lat;
-    int32_t lon;
-    int16_t alt;
-    uint8_t speed;
-    uint8_t sat;
-    int16_t heading;
-} GPS_DATA;
-
 uint16_t hex2uint16(const char *p);
 uint8_t hex2uint8(const char *p);
 
@@ -131,7 +120,7 @@ public:
 	// set working protocol (default auto)
 	virtual bool setProtocol(OBD_PROTOCOLS h = PROTO_AUTO);
 	// send AT command and receive response
-	virtual byte sendCommand(const char* cmd, char* buf, byte bufsize);
+	virtual byte sendCommand(const char* cmd, char* buf, byte bufsize, int timeout = OBD_TIMEOUT_LONG);
 	// clear diagnostic trouble code
 	virtual void clearDTC();
 	// get battery voltage (in 0.1V, e.g. 125 for 12.5V, works without ECU)
@@ -145,9 +134,7 @@ public:
 	// determine if the PID is supported
 	virtual bool isValidPID(byte pid);
 	// init GPS module
-	virtual bool initGPS(unsigned long baudrate = 38400);
 	// parse GPS data
-	virtual bool getGPSData(GPS_DATA* gdata);
 	// set current PID mode
 	byte dataMode;
 	// occurrence of errors
-- 
cgit v1.2.3