From cd8673beca734a1452c0581e66d616dac4ca8789 Mon Sep 17 00:00:00 2001 From: Stanley Huang Date: Thu, 30 Apr 2015 21:45:14 +1000 Subject: Update OBD library --- libraries/OBD/OBD.cpp | 97 +++++++++++++++++++++++++++++++++++++++------------ libraries/OBD/OBD.h | 33 ++++++++++-------- 2 files changed, 93 insertions(+), 37 deletions(-) (limited to 'libraries/OBD') 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 +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(); -- cgit v1.2.3