diff options
| -rw-r--r-- | libraries/OBD/OBD.cpp | 33 | ||||
| -rw-r--r-- | libraries/OBD/OBD.h | 4 |
2 files changed, 32 insertions, 5 deletions
diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp index b881f73..f1b0c0c 100644 --- a/libraries/OBD/OBD.cpp +++ b/libraries/OBD/OBD.cpp @@ -218,25 +218,50 @@ bool COBD::getResult(byte& pid, int& result) return true; } -void COBD::setProtocol(OBD_PROTOCOLS h) +bool COBD::setProtocol(OBD_PROTOCOLS h) { + char buf[OBD_RECV_BUF_SIZE]; if (h == PROTO_AUTO) { write("ATSP00\r"); } else { - char cmd[8]; - sprintf(cmd, "ATSP%d\r", h); - write(cmd); + sprintf(buf, "ATSP%d\r", h); + write(buf); } + if (receive(buf, 3000) > 0 && strstr(buf, "OK")) + return true; + else + return false; } void COBD::sleep() { write("ATLP\r"); + receive(); } void COBD::wakeup() { write('\r'); + receive(); +} + +unsigned int 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) v2 = atoi(p + 1); + return (unsigned int)v1 * 1000 + v2; + } + } + } + return 0; } bool COBD::isValidPID(byte pid) diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h index f5d7e3a..8003fb1 100644 --- a/libraries/OBD/OBD.h +++ b/libraries/OBD/OBD.h @@ -118,9 +118,11 @@ public: // wake up device from previous sleep
virtual void wakeup();
// set working protocol (default auto)
- virtual void setProtocol(OBD_PROTOCOLS h = PROTO_AUTO);
+ virtual bool setProtocol(OBD_PROTOCOLS h = PROTO_AUTO);
// clear diagnostic trouble code
virtual void clearDTC();
+ // get battery voltage (in mV, 12500 for 12.5V, works without ECU)
+ virtual unsigned int getVoltage();
// send query for specified PID
virtual void sendQuery(byte pid);
// retrive and parse the response of specifie PID
|