Update OBD library

2 files changed, 93 insertions, 37 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index f20d459..bf778bf 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -56,6 +56,12 @@ byte hex2uint8(const char *p)
 *************************************************************************/
 #include <Wire.h>
 
+byte COBD::sendCommand(const char* cmd, char* buf)
+{
+    write(cmd);
+    return receive(buf);
+}
+
 void COBD::sendQuery(byte pid)
 {
 	char cmd[8];
@@ -244,27 +250,15 @@ void COBD::wakeup()
 	receive();
 }
 
-int COBD::getVoltage()
+float COBD::getVoltage()
 {
     char buf[OBD_RECV_BUF_SIZE];
     write("ATRV\r");
     byte n = receive(buf, 100);
     if (n > 0) {
-        for (byte i = 0; i < n; i++) {
-            if (buf[i] >= '0' && buf[i] <= '9') {
-                int v1 = atoi(buf);
-                int v2 = 0;
-                char *p = strchr(buf, '.');
-                if (p++) {
-                    if (*p >= '0' && *p <= '9') {
-                        v2 = *p - '0';
-                    }
-                }
-                return v1 * 10 + v2;
-            }
-        }
+        return atof(buf);
     }
-    return -1;
+    return 0;
 }
 
 bool COBD::isValidPID(byte pid)
@@ -381,14 +375,73 @@ void COBD::end()
 	OBDUART.end();
 }
 
-void COBD::setBaudRate(long baudrate)
+bool COBD::setBaudRate(unsigned long baudrate)
 {
-    OBDUART.print("ATBR1 ");
-    OBDUART.print(baudrate);
-    OBDUART.print('\r');
-    OBDUART.end();
-    delay(100);
-    OBDUART.begin(baudrate);
+    char buf[OBD_RECV_BUF_SIZE];
+    sprintf(buf, "ATBR2 %lu\r", baudrate);
+    OBDUART.print(buf);
+    if (receive(buf) && strstr(buf, "OK")) {
+        OBDUART.end();
+        OBDUART.begin(baudrate);
+        return true;
+    } else {
+        return false;
+    }
+}
+
+bool COBD::initGPS(unsigned long baudrate)
+{
+    char buf[OBD_RECV_BUF_SIZE];
+    sprintf(buf, "ATBR2 %lu\r", baudrate);
+    write(buf);
+    return (receive(buf) && strstr(buf, "OK"));
+}
+
+bool COBD::getGPSData(GPS_DATA* gdata)
+{
+    char buf[OBD_RECV_BUF_SIZE];
+    char *p;
+    write("ATGPS\r");
+    if (receive(buf) == 0 || !(p = strstr(buf, "$GPS")))
+        return false;
+
+    byte index = 0;
+    char *s = buf;
+    s = p + 5;
+    for (p = s; *p; p++) {
+        char c = *p;
+        if (c == ',' || c == '>' || c <= 0x0d) {
+            switch (index) {
+            case 0:
+                gdata->date = (uint32_t)atol(s);
+                break;
+            case 1:
+                gdata->time = (uint32_t)atol(s);
+                break;
+            case 2:
+                gdata->lat = atol(s);
+                break;
+            case 3:
+                gdata->lon = atol(s);
+                break;
+            case 4:
+                gdata->alt = atoi(s);
+                break;
+            case 5:
+                gdata->speed = atof(s);
+                break;
+            case 6:
+                gdata->heading = atoi(s);
+                break;
+            case 7:
+                gdata->sat = atoi(s);
+                break;
+            }
+            index++;
+            s = p + 1;
+        }
+    }
+    return index > 7;
 }
 
 #ifdef DEBUG
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index acf64e9..d7e46cb 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -93,6 +93,17 @@ typedef enum {
     OBD_CONNECTED = 2

 } OBD_STATES;

 

+typedef struct {

+    uint32_t date;

+    uint32_t time;

+    int32_t lat;

+    int32_t lon;

+    int alt;

+    float speed;

+    int heading;

+    int sat;

+} GPS_DATA;

+

 uint16_t hex2uint16(const char *p);

 uint8_t hex2uint8(const char *p);

 

@@ -110,7 +121,7 @@ public:
 	// un-initialize OBD-II connection

 	virtual void end();

 	// set serial baud rate

-	virtual void setBaudRate(long baudrate);

+	virtual bool setBaudRate(unsigned long baudrate);

 	// get connection state

 	virtual OBD_STATES getState() { return m_state; }

 	// read specified OBD-II PID value

@@ -121,16 +132,21 @@ public:
 	virtual void wakeup();

 	// set working protocol (default auto)

 	virtual bool setProtocol(OBD_PROTOCOLS h = PROTO_AUTO);

+	virtual byte sendCommand(const char* cmd, char* buf);

 	// clear diagnostic trouble code

 	virtual void clearDTC();

 	// get battery voltage (in 0.1V, e.g. 125 for 12.5V, works without ECU)

-	virtual int getVoltage();

+	virtual float getVoltage();

 	// 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

+	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

@@ -191,19 +207,6 @@ typedef struct {
     uint8_t data;

 } COMMAND_BLOCK;

 

-typedef struct {

-    uint32_t time;

-    uint32_t date;

-    float lat;

-    float lon;

-    float speed;

-    float alt;

-    uint8_t sat;

-    uint8_t state;

-    uint16_t age;

-    uint8_t reserved[4];

-} GPS_DATA;

-

 class COBDI2C : public COBD {

 public:

     void begin();