Added new APIs

2 files changed, 84 insertions, 38 deletions

diff --git a/libraries/OBD2UART/OBD2UART.cpp b/libraries/OBD2UART/OBD2UART.cpp
index 78954c4..a079baa 100644
--- a/libraries/OBD2UART/OBD2UART.cpp
+++ b/libraries/OBD2UART/OBD2UART.cpp
@@ -54,14 +54,14 @@ byte hex2uint8(const char *p)
 * OBD-II UART Adapter
 *************************************************************************/
 
-byte COBD2UART::sendCommand(const char* cmd, char* buf, byte bufsize, int timeout)
+byte COBD::sendCommand(const char* cmd, char* buf, byte bufsize, int timeout)
 {
 	write(cmd);
 	dataIdleLoop();
 	return receive(buf, bufsize, timeout);
 }
 
-void COBD2UART::sendQuery(byte pid)
+void COBD::sendQuery(byte pid)
 {
 	char cmd[8];
 	sprintf(cmd, "%02X%02X\r", dataMode, pid);
@@ -71,7 +71,7 @@ void COBD2UART::sendQuery(byte pid)
 	write(cmd);
 }
 
-bool COBD2UART::readPID(byte pid, int& result)
+bool COBD::readPID(byte pid, int& result)
 {
 	// send a query command
 	sendQuery(pid);
@@ -79,7 +79,7 @@ bool COBD2UART::readPID(byte pid, int& result)
 	return getResult(pid, result);
 }
 
-byte COBD2UART::readPID(const byte pid[], byte count, int result[])
+byte COBD::readPID(const byte pid[], byte count, int result[])
 {
 	// send a multiple query command
 	char buffer[128];
@@ -98,19 +98,19 @@ byte COBD2UART::readPID(const byte pid[], byte count, int result[])
 	return results;
 }
 
-void COBD2UART::clearDTC()
+void COBD::clearDTC()
 {
 	char buffer[32];
 	write("04\r");
 	receive(buffer, sizeof(buffer));
 }
 
-void COBD2UART::write(const char* s)
+void COBD::write(const char* s)
 {
 	OBDUART.write(s);
 }
 
-int COBD2UART::normalizeData(byte pid, char* data)
+int COBD::normalizeData(byte pid, char* data)
 {
 	int result;
 	switch (pid) {
@@ -194,7 +194,7 @@ int COBD2UART::normalizeData(byte pid, char* data)
 	return result;
 }
 
-char* COBD2UART::getResponse(byte& pid, char* buffer, byte bufsize)
+char* COBD::getResponse(byte& pid, char* buffer, byte bufsize)
 {
 	while (receive(buffer, bufsize) > 0) {
 		char *p = buffer;
@@ -213,7 +213,7 @@ char* COBD2UART::getResponse(byte& pid, char* buffer, byte bufsize)
 	return 0;
 }
 
-bool COBD2UART::getResult(byte& pid, int& result)
+bool COBD::getResult(byte& pid, int& result)
 {
 	char buffer[64];
 	char* data = getResponse(pid, buffer, sizeof(buffer));
@@ -226,7 +226,7 @@ bool COBD2UART::getResult(byte& pid, int& result)
 	return true;
 }
 
-bool COBD2UART::setProtocol(OBD_PROTOCOLS h)
+bool COBD::setProtocol(OBD_PROTOCOLS h)
 {
     char buf[32];
 	if (h == PROTO_AUTO) {
@@ -241,13 +241,13 @@ bool COBD2UART::setProtocol(OBD_PROTOCOLS h)
         return false;
 }
 
-void COBD2UART::sleep()
+void COBD::sleep()
 {
   	char buf[32];
 	sendCommand("ATLP\r", buf, sizeof(buf));
 }
 
-char* COBD2UART::getResultValue(char* buf)
+char* COBD::getResultValue(char* buf)
 {
 	char* p = buf;
 	for (;;) {
@@ -261,7 +261,7 @@ char* COBD2UART::getResultValue(char* buf)
 	return 0;
 }
 
-float COBD2UART::getVoltage()
+float COBD::getVoltage()
 {
     char buf[32];
 	if (sendCommand("ATRV\r", buf, sizeof(buf)) > 0) {
@@ -271,7 +271,7 @@ float COBD2UART::getVoltage()
     return 0;
 }
 
-bool COBD2UART::getVIN(char* buffer, byte bufsize)
+bool COBD::getVIN(char* buffer, byte bufsize)
 {
 	if (sendCommand("0902\r", buffer, bufsize)) {
         char *p = strstr(buffer, "0: 49 02");
@@ -291,7 +291,7 @@ bool COBD2UART::getVIN(char* buffer, byte bufsize)
     return false;
 }
 
-bool COBD2UART::isValidPID(byte pid)
+bool COBD::isValidPID(byte pid)
 {
 	if (pid >= 0x7f)
 		return true;
@@ -301,16 +301,26 @@ bool COBD2UART::isValidPID(byte pid)
 	return pidmap[i] & b;
 }
 
-void COBD2UART::begin()
+void COBD::begin()
 {
 	OBDUART.begin(OBD_SERIAL_BAUDRATE);
 #ifdef DEBUG
 	DEBUG.begin(115200);
 #endif
 	recover();
+
+	char buffer[32];
+	version = 0;
+	if (sendCommand("ATI\r", buffer, sizeof(buffer), 200)) {
+		char *p = strstr(buffer, "OBDUART");
+		if (p) {
+			p += 9;
+			version = (*p - '0') * 10 + (*(p + 2) - '0');
+		}
+	}
 }
 
-byte COBD2UART::receive(char* buffer, byte bufsize, int timeout)
+byte COBD::receive(char* buffer, byte bufsize, int timeout)
 {
 	unsigned char n = 0;
 	unsigned long startTime = millis();
@@ -343,32 +353,19 @@ byte COBD2UART::receive(char* buffer, byte bufsize, int timeout)
 	return n;
 }
 
-void COBD2UART::recover()
+void COBD::recover()
 {
 	char buf[16];
 	sendCommand("AT\r", buf, sizeof(buf));
 }
 
-bool COBD2UART::init(OBD_PROTOCOLS protocol)
+bool COBD::init(OBD_PROTOCOLS protocol)
 {
 	const char *initcmd[] = {"ATZ\r","ATE0\r","ATL1\r","0100\r"};
 	char buffer[64];
 
 	m_state = OBD_CONNECTING;
 
-	write("ATI\r");
-	if (receive(buffer, sizeof(buffer), 100)) {
-		char *p = strstr(buffer, "OBDUART");
-		if (p) {
-			p += 9;
-			version = (*p - '0') * 10 + (*(p + 2) - '0');
-		}
-	}
-	if (version == 0) {
-		m_state = OBD_FAILED;
-		return false;
-	}
-
 	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
 #ifdef DEBUG
 		debugOutput(initcmd[i]);
@@ -406,13 +403,13 @@ bool COBD2UART::init(OBD_PROTOCOLS protocol)
 	return true;
 }
 
-void COBD2UART::end()
+void COBD::end()
 {
 	m_state = OBD_DISCONNECTED;
 	OBDUART.end();
 }
 
-bool COBD2UART::setBaudRate(unsigned long baudrate)
+bool COBD::setBaudRate(unsigned long baudrate)
 {
     OBDUART.print("ATBR1 ");
     OBDUART.print(baudrate);
@@ -424,9 +421,51 @@ bool COBD2UART::setBaudRate(unsigned long baudrate)
     return true;
 }
 
+float COBD::getTemperature()
+{
+	char buf[32];
+	if (sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0) {
+		char* p = getResultValue(buf);
+		return (float)(atoi(p) + 12412) / 340;
+	}
+	else {
+		return -1000;
+	}
+}
+
+bool COBD::readAccel(int& x, int& y, int& z)
+{
+	char buf[64];
+	if (sendCommand("ATACL\r", buf, sizeof(buf)) > 0) do {
+		char* p = getResultValue(buf);
+		x = atoi(p++);
+		if (!(p = strchr(p, ','))) break;
+		y = atoi(++p);
+		if (!(p = strchr(p, ','))) break;
+		z = atoi(++p);
+		return true;
+	} while (0);
+	return false;
+}
+
+bool COBD::readGyro(int& x, int& y, int& z)
+{
+	char buf[64];
+	if (sendCommand("ATGYRO\r", buf, sizeof(buf)) > 0) do {
+		char* p = getResultValue(buf);
+		x = atoi(p++);
+		if (!(p = strchr(p, ','))) break;
+		y = atoi(++p);
+		if (!(p = strchr(p, ','))) break;
+		z = atoi(++p);
+		return true;
+	} while (0);
+	return false;
+}
+
 
 #ifdef DEBUG
-void COBD2UART::debugOutput(const char *s)
+void COBD::debugOutput(const char *s)
 {
 	DEBUG.print('[');
 	DEBUG.print(millis());
diff --git a/libraries/OBD2UART/OBD2UART.h b/libraries/OBD2UART/OBD2UART.h
index 618b8c6..eaab88d 100644
--- a/libraries/OBD2UART/OBD2UART.h
+++ b/libraries/OBD2UART/OBD2UART.h
@@ -95,10 +95,10 @@ typedef enum {
 uint16_t hex2uint16(const char *p);
 uint8_t hex2uint8(const char *p);
 
-class COBD2UART
+class COBD
 {
 public:
-	COBD2UART():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {}
+	COBD():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {}
 	// begin serial UART
 	virtual void begin();
 	// initialize OBD-II connection
@@ -121,10 +121,16 @@ public:
 	virtual byte sendCommand(const char* cmd, char* buf, byte bufsize, int timeout = OBD_TIMEOUT_LONG);
 	// clear diagnostic trouble code
 	virtual void clearDTC();
-	// get battery voltage (in 0.1V, e.g. 125 for 12.5V, works without ECU)
+	// get battery voltage (works without ECU)
 	virtual float getVoltage();
 	// get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE
 	virtual bool getVIN(char* buffer, byte bufsize);
+	// get device temperature
+	virtual float getTemperature();
+	// get accelerometer data
+	virtual bool readAccel(int& x, int& y, int& z);
+	// get gyroscope data
+	virtual bool readGyro(int& x, int& y, int& z);
 	// send query for specified PID
 	virtual void sendQuery(byte pid);
 	// retrive and parse the response of specifie PID
@@ -167,5 +173,6 @@ private:
 	{
 		return (int)hex2uint8(data) - 40;
 	}
+	char* getResultValue(char* buf);
 };