add support for the new OBD I2C adapter

3 files changed, 190 insertions, 34 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index 042e393..5dcf903 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -9,8 +9,8 @@
 #include <avr/pgmspace.h>
 #include "OBD.h"
 
-//#define DEBUG
-#define DEBUG_SERIAL Serial
+//#define DEBUG Serial
+//#define REDIRECT Serial
 
 #define MAX_CMD_LEN 6
 
@@ -60,6 +60,11 @@ unsigned char hex2uint8(const char *p)
 	return c1 << 4 | (c2 & 0xf);
 }
 
+/*************************************************************************
+* OBD-II UART Adapter
+*************************************************************************/
+#include <Wire.h>
+
 void COBD::sendQuery(unsigned char pid)
 {
 	char cmd[8];
@@ -96,18 +101,18 @@ bool COBD::available()
 char COBD::read()
 {
 	char c = OBDUART.read();
-#ifdef DEBUG
-    DEBUG_SERIAL.print(c);
+#ifdef REDIRECT
+    REDIRECT.write(c);
 #endif
 	return c;
 }
 
-void COBD::write(const char* s)
+void COBD::write(char* s)
 {
 	OBDUART.write(s);
 }
 
-void COBD::write(const char c)
+void COBD::write(char c)
 {
 	OBDUART.write(c);
 }
@@ -177,7 +182,7 @@ bool COBD::getResponseParsed(byte& pid, int& result)
 	char* data = getResponse(pid, buffer);
 	if (!data) {
 		// try recover next time
-		write('\r');
+		//write('\r');
 		return false;
 	}
 	result = normalizeData(pid, data);
@@ -297,9 +302,116 @@ bool COBD::init(bool passive)
 #ifdef DEBUG
 void COBD::debugOutput(const char *s)
 {
-    DEBUG_SERIAL.print('[');
-    DEBUG_SERIAL.print(millis());
-    DEBUG_SERIAL.print(']');
-    DEBUG_SERIAL.print(s);
+    DEBUG.print('[');
+    DEBUG.print(millis());
+    DEBUG.print(']');
+    DEBUG.print(s);
 }
 #endif
+
+/*************************************************************************
+* OBD-II I2C Adapter
+*************************************************************************/
+
+void COBDI2C::begin(byte addr)
+{
+    m_addr = addr;
+    Wire.begin();
+}
+
+bool COBDI2C::init()
+{
+    m_state = OBD_CONNECTING;
+    sendCommand(CMD_QUERY_STATUS);
+
+    char recvbuf[MAX_PAYLOAD_SIZE];
+    for (byte n = 0; n < 3; n++) {
+        memset(recvbuf, 0, sizeof(recvbuf));
+        receive(recvbuf);
+        if (!memcmp(recvbuf, "OBD ", 4))
+            break;
+    }
+    if (recvbuf[4] == 'Y') {
+        memcpy(pidmap, recvbuf + 16, sizeof(pidmap));
+        m_state = OBD_CONNECTED;
+        return true;
+    } else {
+        m_state = OBD_DISCONNECTED;
+        return false;
+    }
+}
+
+bool COBDI2C::readSensor(byte pid, int& result, bool passive)
+{
+    uint32_t t = millis();
+    sendQuery(pid);
+    dataIdleLoop();
+    return getResponseParsed(pid, result);
+}
+
+void COBDI2C::write(char* s)
+{
+    COMMAND_BLOCK cmdblock = {millis(), CMD_SEND_COMMAND};
+    Wire.beginTransmission(m_addr);
+    Wire.write((byte*)&cmdblock, sizeof(cmdblock));
+    Wire.write(s);
+    Wire.endTransmission();
+}
+
+bool COBDI2C::sendCommand(byte cmd, uint8_t data, byte* payload, byte payloadBytes)
+{
+    COMMAND_BLOCK cmdblock = {millis(), cmd, data};
+    Wire.beginTransmission(m_addr);
+    bool success = Wire.write((byte*)&cmdblock, sizeof(COMMAND_BLOCK)) == sizeof(COMMAND_BLOCK);
+    if (payload) Wire.write(payload, payloadBytes);
+    Wire.endTransmission();
+    return success;
+}
+
+byte COBDI2C::receive(char* buffer)
+{
+    uint32_t start = millis();
+    byte offset = 0;
+    do {
+        Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)1);
+
+        bool hasEnd = false;
+        for (byte i = 0; i < MAX_PAYLOAD_SIZE; i++) {
+            if ((buffer[offset + i] = Wire.read()) == 0)
+                hasEnd = true;
+        }
+
+        if (buffer[0] == 0) {
+            // data not ready
+            dataIdleLoop();
+            continue;
+        }
+
+        offset += MAX_PAYLOAD_SIZE;
+        if (!hasEnd) {
+            continue;
+        }
+
+        return offset;
+    } while(millis() - start < OBD_TIMEOUT_LONG);
+    return 0;
+}
+
+bool COBDI2C::btInit(uint16_t baudrate)
+{
+    return sendCommand(CMD_UART_BEGIN, baudrate / 1200);
+}
+
+bool COBDI2C::btSend(byte* data, byte length)
+{
+    return sendCommand(CMD_UART_SEND, 0, data, length);
+}
+
+bool COBDI2C::btReceive(byte* buffer, byte bufsize)
+{
+    if (!sendCommand(CMD_UART_RECV, bufsize)) return false;
+    memset(buffer, 0, MAX_PAYLOAD_SIZE);
+    Wire.requestFrom((byte)m_addr, (byte)MAX_PAYLOAD_SIZE, (byte)1);
+    Wire.readBytes((char*)buffer, MAX_PAYLOAD_SIZE);
+    return true;
+}
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index 1bdaeb1..f64ea35 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -48,45 +48,84 @@ class COBD
 {
 public:
 	COBD():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {}
-	void begin();
-	bool init(bool passive = false);
-	bool readSensor(byte pid, int& result, bool passive = false);
-	bool isValidPID(byte pid);
-	void sleep(int seconds);
+	virtual void begin();
+	virtual bool init(bool passive = false);
+	virtual bool readSensor(byte pid, int& result, bool passive = false);
+	virtual void sleep(int seconds);
 	// Query and GetResponse for advanced usage only
-	void sendQuery(byte pid);
-	char* getResponse(byte& pid, char* buffer);
-	bool getResponseParsed(byte& pid, int& result);
+	virtual void sendQuery(byte pid);
+	bool isValidPID(byte pid);
 	byte getState() { return m_state; }
 	byte dataMode;
 	byte errors;
 	byte pidmap[4 * 4];
 	byte vin[17];
-	//char recvBuf[OBD_RECV_BUF_SIZE];
 protected:
-	byte receive(char* buffer);
+	virtual char* getResponse(byte& pid, char* buffer);
+	virtual bool getResponseParsed(byte& pid, int& result);
+	virtual byte receive(char* buffer);
+	virtual bool available();
+	virtual char read();
+	virtual void write(char* s);
+	virtual void write(char c);
+	virtual void dataIdleLoop() {}
 	void debugOutput(const char* s);
-	static int normalizeData(byte pid, char* data);
-	static int getPercentageValue(char* data)
+	int normalizeData(byte pid, char* data);
+	byte m_state;
+private:
+	virtual uint8_t getPercentageValue(char* data)
 	{
-		return (int)hex2uint8(data) * 100 / 255;
+		return (uint16_t)hex2uint8(data) * 100 / 255;
 	}
-	static unsigned int getLargeValue(char* data)
+	virtual uint16_t getLargeValue(char* data)
 	{
 		return hex2uint16(data);
 	}
-	static int getSmallValue(char* data)
+	virtual uint8_t getSmallValue(char* data)
 	{
 		return hex2uint8(data);
 	}
-	static int getTemperatureValue(char* data)
+	virtual int16_t getTemperatureValue(char* data)
 	{
 		return (int)hex2uint8(data) - 40;
 	}
-	virtual bool available();
-	virtual char read();
-	virtual void write(const char* s);
-	virtual void write(const char c);
-	virtual void dataIdleLoop() {}
-	byte m_state;
+};
+
+#define I2C_ADDR 0x62
+
+#define MAX_PAYLOAD_SIZE 32
+
+#define CMD_QUERY_STATUS 0x10
+#define CMD_SEND_COMMAND 0x11
+#define CMD_QUERY_DATA 0x12
+#define CMD_UART_BEGIN 0x13
+#define CMD_UART_SEND 0x14
+#define CMD_UART_RECV 0x15
+
+typedef struct {
+    uint32_t time;
+    uint16_t pid;
+    float value;
+} PID_INFO;
+
+typedef struct {
+    uint16_t time;
+    uint8_t message;
+    uint8_t data;
+} COMMAND_BLOCK;
+
+class COBDI2C : public COBD {
+public:
+    void begin(byte addr = I2C_ADDR);
+    bool init();
+    bool readSensor(byte pid, int& result, bool passive = false);
+    void write(char* s);
+    // Bluetooth communication API
+    bool btInit(uint16_t baudrate = 9600);
+    bool btSend(byte* data, byte length);
+    bool btReceive(byte* buffer, byte bufsize);
+private:
+    bool sendCommand(byte cmd, uint8_t data = 0, byte* payload = 0, byte payloadBytes = 0);
+    byte receive(char* buffer);
+    byte m_addr;
 };
diff --git a/libraries/OBD/examples/rpm_led/rpm_led.ino b/libraries/OBD/examples/rpm_led/rpm_led.ino
index 669d690..630336c 100644
--- a/libraries/OBD/examples/rpm_led/rpm_led.ino
+++ b/libraries/OBD/examples/rpm_led/rpm_led.ino
@@ -6,10 +6,15 @@
 *************************************************************************/
 
 #include <Arduino.h>
-#include "OBD.h"
+#include <Wire.h>
+#include <OBD.h>
 
+// OBD-II UART Adapter
 COBD obd;
 
+// OBD-II I2C Adapter
+//COBDI2C obd;
+
 void setup()
 {
   // we'll use the debug LED as output