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