diff options
| -rw-r--r-- | libraries/OBD/OBD.cpp | 97 | ||||
| -rw-r--r-- | libraries/OBD/OBD.h | 33 |
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();
|