Added library for OBD-II UART Adapter MK2

2 files changed, 593 insertions, 0 deletions

diff --git a/libraries/OBDUART/OBDUART.cpp b/libraries/OBDUART/OBDUART.cpp
new file mode 100644
index 0000000..f91c7ea
--- /dev/null
+++ b/libraries/OBDUART/OBDUART.cpp
@@ -0,0 +1,422 @@
+/*************************************************************************
+* Arduino Library for Freematics OBD-II UART Adapter
+* Distributed under BSD License
+* Visit http://freematics.com for more information
+* (C)2012-2016 Stanley Huang <stanleyhuangyc@gmail.com>
+*************************************************************************/
+
+#include "OBDUART.h"
+
+//#define DEBUG Serial
+
+uint16_t hex2uint16(const char *p)
+{
+	char c = *p;
+	uint16_t i = 0;
+	for (char n = 0; c && n < 4; c = *(++p)) {
+		if (c >= 'A' && c <= 'F') {
+			c -= 7;
+		} else if (c>='a' && c<='f') {
+			c -= 39;
+        } else if (c == ' ') {
+            continue;
+        } else if (c < '0' || c > '9') {
+			break;
+        }
+		i = (i << 4) | (c & 0xF);
+		n++;
+	}
+	return i;
+}
+
+byte hex2uint8(const char *p)
+{
+	byte c1 = *p;
+	byte c2 = *(p + 1);
+	if (c1 >= 'A' && c1 <= 'F')
+		c1 -= 7;
+	else if (c1 >='a' && c1 <= 'f')
+	    c1 -= 39;
+	else if (c1 < '0' || c1 > '9')
+		return 0;
+
+	if (c2 >= 'A' && c2 <= 'F')
+		c2 -= 7;
+	else if (c2 >= 'a' && c2 <= 'f')
+	    c2 -= 39;
+	else if (c2 < '0' || c2 > '9')
+		return 0;
+
+	return c1 << 4 | (c2 & 0xf);
+}
+
+/*************************************************************************
+* OBD-II UART Adapter
+*************************************************************************/
+
+byte COBDUART::sendCommand(const char* cmd, char* buf, byte bufsize, int timeout)
+{
+	write(cmd);
+	dataIdleLoop();
+	return receive(buf, bufsize, timeout);
+}
+
+void COBDUART::sendQuery(byte pid)
+{
+	char cmd[8];
+	sprintf(cmd, "%02X%02X\r", dataMode, pid);
+#ifdef DEBUG
+	debugOutput(cmd);
+#endif
+	write(cmd);
+}
+
+bool COBDUART::readPID(byte pid, int& result)
+{
+	// send a query command
+	sendQuery(pid);
+	// receive and parse the response
+	return getResult(pid, result);
+}
+
+byte COBDUART::readPID(const byte pid[], byte count, int result[])
+{
+	// send a multiple query command
+	char buffer[128];
+	char *p = buffer;
+	byte results = 0;
+	for (byte n = 0; n < count; n++) {
+		p += sprintf(p, "%02X%02X\r\n", dataMode, pid[n]);		
+	}
+	write(buffer);
+	// receive and parse the response
+	for (byte n = 0; n < count; n++) {
+		byte curpid = pid[n];
+		if (getResult(curpid, result[n]))
+			results++;
+	}
+	return results;
+}
+
+void COBDUART::clearDTC()
+{
+	char buffer[32];
+	write("04\r");
+	receive(buffer, sizeof(buffer));
+}
+
+void COBDUART::write(const char* s)
+{
+	OBDUART.write(s);
+}
+
+int COBDUART::normalizeData(byte pid, char* data)
+{
+	int result;
+	switch (pid) {
+	case PID_RPM:
+	case PID_EVAP_SYS_VAPOR_PRESSURE: // kPa
+		result = getLargeValue(data) >> 2;
+		break;
+	case PID_FUEL_PRESSURE: // kPa
+		result = getSmallValue(data) * 3;
+		break;
+	case PID_COOLANT_TEMP:
+	case PID_INTAKE_TEMP:
+	case PID_AMBIENT_TEMP:
+	case PID_ENGINE_OIL_TEMP:
+		result = getTemperatureValue(data);
+		break;
+	case PID_THROTTLE:
+	case PID_COMMANDED_EGR:
+	case PID_COMMANDED_EVAPORATIVE_PURGE:
+	case PID_FUEL_LEVEL:
+	case PID_RELATIVE_THROTTLE_POS:
+	case PID_ABSOLUTE_THROTTLE_POS_B:
+	case PID_ABSOLUTE_THROTTLE_POS_C:
+	case PID_ACC_PEDAL_POS_D:
+	case PID_ACC_PEDAL_POS_E:
+	case PID_ACC_PEDAL_POS_F:
+	case PID_COMMANDED_THROTTLE_ACTUATOR:
+	case PID_ENGINE_LOAD:
+	case PID_ABSOLUTE_ENGINE_LOAD:
+	case PID_ETHANOL_FUEL:
+	case PID_HYBRID_BATTERY_PERCENTAGE:
+		result = getPercentageValue(data);
+		break;
+	case PID_MAF_FLOW: // grams/sec
+		result = getLargeValue(data) / 100;
+		break;
+	case PID_TIMING_ADVANCE:
+		result = (int)(getSmallValue(data) / 2) - 64;
+		break;
+	case PID_DISTANCE: // km
+	case PID_DISTANCE_WITH_MIL: // km
+	case PID_TIME_WITH_MIL: // minute
+	case PID_TIME_SINCE_CODES_CLEARED: // minute
+	case PID_RUNTIME: // second
+	case PID_FUEL_RAIL_PRESSURE: // kPa
+	case PID_ENGINE_REF_TORQUE: // Nm
+		result = getLargeValue(data);
+		break;
+	case PID_CONTROL_MODULE_VOLTAGE: // V
+		result = getLargeValue(data) / 1000;
+		break;
+	case PID_ENGINE_FUEL_RATE: // L/h
+		result = getLargeValue(data) / 20;
+		break;
+	case PID_ENGINE_TORQUE_DEMANDED: // %
+	case PID_ENGINE_TORQUE_PERCENTAGE: // %
+		result = (int)getSmallValue(data) - 125;
+		break;
+	case PID_SHORT_TERM_FUEL_TRIM_1:
+	case PID_LONG_TERM_FUEL_TRIM_1:
+	case PID_SHORT_TERM_FUEL_TRIM_2:
+	case PID_LONG_TERM_FUEL_TRIM_2:
+	case PID_EGR_ERROR:
+		result = ((int)getSmallValue(data) - 128) * 100 / 128;
+		break;
+	case PID_FUEL_INJECTION_TIMING:
+		result = ((int32_t)getLargeValue(data) - 26880) / 128;
+		break;
+	case PID_CATALYST_TEMP_B1S1:
+	case PID_CATALYST_TEMP_B2S1:
+	case PID_CATALYST_TEMP_B1S2:
+	case PID_CATALYST_TEMP_B2S2:
+		result = getLargeValue(data) / 10 - 40;
+		break;
+	case PID_AIR_FUEL_EQUIV_RATIO: // 0~200
+		result = (long)getLargeValue(data) * 200 / 65536;
+		break;
+	default:
+		result = getSmallValue(data);
+	}
+	return result;
+}
+
+char* COBDUART::getResponse(byte& pid, char* buffer, byte bufsize)
+{
+	while (receive(buffer, bufsize) > 0) {
+		char *p = buffer;
+		while ((p = strstr(p, "41 "))) {
+		    p += 3;
+		    byte curpid = hex2uint8(p);
+		    if (pid == 0) pid = curpid;
+		    if (curpid == pid) {
+		        errors = 0;
+		        p += 2;
+		        if (*p == ' ')
+		            return p + 1;
+		    }
+		}
+	}
+	return 0;
+}
+
+bool COBDUART::getResult(byte& pid, int& result)
+{
+	char buffer[64];
+	char* data = getResponse(pid, buffer, sizeof(buffer));
+	if (!data) {
+		recover();
+		errors++;
+		return false;
+	}
+	result = normalizeData(pid, data);
+	return true;
+}
+
+bool COBDUART::setProtocol(OBD_PROTOCOLS h)
+{
+    char buf[32];
+	if (h == PROTO_AUTO) {
+		write("ATSP00\r");
+	} else {
+		sprintf(buf, "ATSP%d\r", h);
+		write(buf);
+	}
+	if (receive(buf, sizeof(buf), OBD_TIMEOUT_LONG) > 0 && strstr(buf, "OK"))
+        return true;
+    else
+        return false;
+}
+
+void COBDUART::sleep()
+{
+  	char buf[32];
+	sendCommand("ATLP\r", buf, sizeof(buf));
+}
+
+float COBDUART::getVoltage()
+{
+    char buf[32];
+	if (sendCommand("ATRV\r", buf, sizeof(buf)) > 0) {
+        return atof(buf);
+    }
+    return 0;
+}
+
+bool COBDUART::getVIN(char* buffer, byte bufsize)
+{
+	if (sendCommand("0902\r", buffer, bufsize)) {
+        char *p = strstr(buffer, "0: 49 02");
+        if (p) {
+            char *q = buffer;
+            p += 10;
+            do {
+                for (++p; *p == ' '; p += 3) {
+                    if (*q = hex2uint8(p + 1)) q++;
+                }
+                p = strchr(p, ':');
+            } while(p);
+            *q = 0;
+            return true;
+        }
+    }
+    return false;
+}
+
+bool COBDUART::isValidPID(byte pid)
+{
+	if (pid >= 0x7f)
+		return true;
+	pid--;
+	byte i = pid >> 3;
+	byte b = 0x80 >> (pid & 0x7);
+	return pidmap[i] & b;
+}
+
+void COBDUART::begin()
+{
+	OBDUART.begin(OBD_SERIAL_BAUDRATE);
+#ifdef DEBUG
+	DEBUG.begin(115200);
+#endif
+	recover();
+}
+
+byte COBDUART::receive(char* buffer, byte bufsize, int timeout)
+{
+	unsigned char n = 0;
+	unsigned long startTime = millis();
+	for (;;) {
+		if (OBDUART.available()) {
+			char c = OBDUART.read();
+			if (n > 2 && c == '>') {
+				// prompt char received
+				break;
+			} else if (!buffer) {
+			       n++;
+			} else if (n < bufsize - 1) {
+				if (c == '.' && n > 2 && buffer[n - 1] == '.' && buffer[n - 2] == '.') {
+					// waiting siginal
+					n = 0;
+					timeout = OBD_TIMEOUT_LONG;
+				} else {
+					buffer[n++] = c;
+				}
+			}
+		} else {
+			if (millis() - startTime > timeout) {
+			    // timeout
+			    break;
+			}
+			dataIdleLoop();
+		}
+	}
+	if (buffer) buffer[n] = 0;
+	return n;
+}
+
+void COBDUART::recover()
+{
+	char buf[16];
+	sendCommand("AT\r", buf, sizeof(buf));
+}
+
+bool COBDUART::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]);
+#endif
+		write(initcmd[i]);
+		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) {
+			m_state = OBD_DISCONNECTED;
+			return false;
+		}
+		delay(50);
+	}
+
+	if (protocol != PROTO_AUTO) {
+		setProtocol(protocol);
+	}
+
+	// load pid map
+	memset(pidmap, 0, sizeof(pidmap));
+	for (byte i = 0; i < 4; i++) {
+		byte pid = i * 0x20;
+		sendQuery(pid);
+		char* data = getResponse(pid, buffer, sizeof(buffer));
+		if (!data) break;
+		data--;
+		for (byte n = 0; n < 4; n++) {
+			if (data[n * 3] != ' ')
+				break;
+			pidmap[i * 4 + n] = hex2uint8(data + n * 3 + 1);
+		}
+		delay(100);
+	}
+
+	m_state = OBD_CONNECTED;
+	errors = 0;
+	return true;
+}
+
+void COBDUART::end()
+{
+	m_state = OBD_DISCONNECTED;
+	OBDUART.end();
+}
+
+bool COBDUART::setBaudRate(unsigned long baudrate)
+{
+    OBDUART.print("ATBR1 ");
+    OBDUART.print(baudrate);
+    OBDUART.print('\r');
+    delay(50);
+    OBDUART.end();
+    OBDUART.begin(baudrate);
+    recover();
+    return true;
+}
+
+
+#ifdef DEBUG
+void COBDUART::debugOutput(const char *s)
+{
+	DEBUG.print('[');
+	DEBUG.print(millis());
+	DEBUG.print(']');
+	DEBUG.print(s);
+}
+#endif
+
diff --git a/libraries/OBDUART/OBDUART.h b/libraries/OBDUART/OBDUART.h
new file mode 100644
index 0000000..eb9fad5
--- /dev/null
+++ b/libraries/OBDUART/OBDUART.h
@@ -0,0 +1,171 @@
+/*************************************************************************
+* Arduino Library for Freematics OBD-II UART Adapter
+* Distributed under BSD License
+* Visit http://freematics.com for more information
+* (C)2012-2016 Stanley Huang <stanleyhuangyc@gmail.com>
+*************************************************************************/
+
+#include <Arduino.h>
+
+#define OBD_TIMEOUT_SHORT 1000 /* ms */
+#define OBD_TIMEOUT_LONG 15000 /* ms */
+#define OBD_TIMEOUT_GPS 200 /* ms */
+#define OBD_SERIAL_BAUDRATE 38400
+
+#ifndef OBDUART
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168P__)
+#define OBDUART Serial
+#else
+#define OBDUART Serial1
+#endif
+#endif
+
+// Mode 1 PIDs
+#define PID_ENGINE_LOAD 0x04
+#define PID_COOLANT_TEMP 0x05
+#define PID_SHORT_TERM_FUEL_TRIM_1 0x06
+#define PID_LONG_TERM_FUEL_TRIM_1 0x07
+#define PID_SHORT_TERM_FUEL_TRIM_2 0x08
+#define PID_LONG_TERM_FUEL_TRIM_2 0x09
+#define PID_FUEL_PRESSURE 0x0A
+#define PID_INTAKE_MAP 0x0B
+#define PID_RPM 0x0C
+#define PID_SPEED 0x0D
+#define PID_TIMING_ADVANCE 0x0E
+#define PID_INTAKE_TEMP 0x0F
+#define PID_MAF_FLOW 0x10
+#define PID_THROTTLE 0x11
+#define PID_AUX_INPUT 0x1E
+#define PID_RUNTIME 0x1F
+#define PID_DISTANCE_WITH_MIL 0x21
+#define PID_COMMANDED_EGR 0x2C
+#define PID_EGR_ERROR 0x2D
+#define PID_COMMANDED_EVAPORATIVE_PURGE 0x2E
+#define PID_FUEL_LEVEL 0x2F
+#define PID_WARMS_UPS 0x30
+#define PID_DISTANCE 0x31
+#define PID_EVAP_SYS_VAPOR_PRESSURE 0x32
+#define PID_BAROMETRIC 0x33
+#define PID_CATALYST_TEMP_B1S1 0x3C
+#define PID_CATALYST_TEMP_B2S1 0x3D
+#define PID_CATALYST_TEMP_B1S2 0x3E
+#define PID_CATALYST_TEMP_B2S2 0x3F
+#define PID_CONTROL_MODULE_VOLTAGE 0x42
+#define PID_ABSOLUTE_ENGINE_LOAD 0x43
+#define PID_AIR_FUEL_EQUIV_RATIO 0x44
+#define PID_RELATIVE_THROTTLE_POS 0x45
+#define PID_AMBIENT_TEMP 0x46
+#define PID_ABSOLUTE_THROTTLE_POS_B 0x47
+#define PID_ABSOLUTE_THROTTLE_POS_C 0x48
+#define PID_ACC_PEDAL_POS_D 0x49
+#define PID_ACC_PEDAL_POS_E 0x4A
+#define PID_ACC_PEDAL_POS_F 0x4B
+#define PID_COMMANDED_THROTTLE_ACTUATOR 0x4C
+#define PID_TIME_WITH_MIL 0x4D
+#define PID_TIME_SINCE_CODES_CLEARED 0x4E
+#define PID_ETHANOL_FUEL 0x52
+#define PID_FUEL_RAIL_PRESSURE 0x59
+#define PID_HYBRID_BATTERY_PERCENTAGE 0x5B
+#define PID_ENGINE_OIL_TEMP 0x5C
+#define PID_FUEL_INJECTION_TIMING 0x5D
+#define PID_ENGINE_FUEL_RATE 0x5E
+#define PID_ENGINE_TORQUE_DEMANDED 0x61
+#define PID_ENGINE_TORQUE_PERCENTAGE 0x62
+#define PID_ENGINE_REF_TORQUE 0x63
+
+typedef enum {
+    PROTO_AUTO = 0,
+    PROTO_ISO_9141_2 = 3,
+    PROTO_KWP2000_5KBPS = 4,
+    PROTO_KWP2000_FAST = 5,
+    PROTO_CAN_11B_500K = 6,
+    PROTO_CAN_29B_500K = 7,
+    PROTO_CAN_29B_250K = 8,
+    PROTO_CAN_11B_250K = 9,
+} OBD_PROTOCOLS;
+
+// states
+typedef enum {
+    OBD_DISCONNECTED = 0,
+    OBD_CONNECTING = 1,
+    OBD_CONNECTED = 2,
+	OBD_FAILED = 3
+} OBD_STATES;
+
+uint16_t hex2uint16(const char *p);
+uint8_t hex2uint8(const char *p);
+
+class COBDUART
+{
+public:
+	COBDUART():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {}
+	// begin serial UART
+	virtual void begin();
+	// initialize OBD-II connection
+	virtual bool init(OBD_PROTOCOLS protocol = PROTO_AUTO);
+	// un-initialize OBD-II connection
+	virtual void end();
+	// set serial baud rate
+	virtual bool setBaudRate(unsigned long baudrate);
+	// get connection state
+	virtual OBD_STATES getState() { return m_state; }
+	// read specified OBD-II PID value
+	virtual bool readPID(byte pid, int& result);
+	// read multiple (up to 8) OBD-II PID values, return number of values obtained
+	virtual byte readPID(const byte pid[], byte count, int result[]);
+	// set device into
+	virtual void sleep();
+	// set working protocol (default auto)
+	virtual bool setProtocol(OBD_PROTOCOLS h = PROTO_AUTO);
+	// send AT command and receive response
+	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)
+	virtual float getVoltage();
+	// get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE
+	virtual bool getVIN(char* buffer, byte bufsize);
+	// 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
+	// parse GPS data
+	// set current PID mode
+	byte dataMode;
+	// occurrence of errors
+	byte errors;
+	// bit map of supported PIDs
+	byte pidmap[4 * 4];
+	// adapter version
+	byte version;
+protected:
+	virtual char* getResponse(byte& pid, char* buffer, byte bufsize);
+	virtual byte receive(char* buffer, byte bufsize, int timeout = OBD_TIMEOUT_SHORT);
+	virtual void write(const char* s);
+	virtual void dataIdleLoop() {}
+	void recover();
+	void debugOutput(const char* s);
+	int normalizeData(byte pid, char* data);
+	OBD_STATES m_state;
+private:
+	virtual uint8_t getPercentageValue(char* data)
+	{
+		return (uint16_t)hex2uint8(data) * 100 / 255;
+	}
+	virtual uint16_t getLargeValue(char* data)
+	{
+		return hex2uint16(data);
+	}
+	virtual uint8_t getSmallValue(char* data)
+	{
+		return hex2uint8(data);
+	}
+	virtual int16_t getTemperatureValue(char* data)
+	{
+		return (int)hex2uint8(data) - 40;
+	}
+};
+