Merge branch 'master' of https://github.com/stanleyhuangyc/ArduinoOBD

8 files changed, 423 insertions, 163 deletions

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index 8dbdd29..92b9bdf 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -298,21 +298,31 @@ float COBD::getVoltage()
 
 bool COBD::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;
-        }
-    }
+	for (byte n = 0; n < 5; n++) {
+		if (sendCommand("0902\r", buffer, bufsize)) {
+			int len = hex2uint16(buffer);
+			char *p = strstr_P(buffer + 4, PSTR("0: 49 02 01"));
+			if (p) {
+				char *q = buffer;
+				p += 11; // skip the header
+				do {
+					while (*(++p) == ' ');
+					for (;;) {
+						*(q++) = hex2uint8(p);
+						while (*p && *p != ' ') p++;
+						while (*p == ' ') p++;
+						if (!*p || *p == '\r') break;
+					}
+					p = strchr(p, ':');
+				} while(p);
+				*q = 0;
+				if (q - buffer == len - 3) {
+					return true;
+				}
+			}
+		}
+		delay(100);
+	}
     return false;
 }
 
diff --git a/libraries/OBD2UART/OBD2UART.cpp b/libraries/OBD2UART/OBD2UART.cpp
index 0cb72f1..76968b9 100644
--- a/libraries/OBD2UART/OBD2UART.cpp
+++ b/libraries/OBD2UART/OBD2UART.cpp
@@ -9,20 +9,16 @@
 
 //#define DEBUG Serial
 
-#ifdef ESP32
-extern HardwareSerial Serial1;
-#endif
-
 uint16_t hex2uint16(const char *p)
 {
 	char c = *p;
 	uint16_t i = 0;
-	for (char n = 0; c && n < 4; c = *(++p)) {
+	for (uint8_t 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 == ' ') {
+        } else if (c == ' ' && n == 2) {
             continue;
         } else if (c < '0' || c > '9') {
 			break;
@@ -44,7 +40,9 @@ byte hex2uint8(const char *p)
 	else if (c1 < '0' || c1 > '9')
 		return 0;
 
-	if (c2 >= 'A' && c2 <= 'F')
+	if (c2 == 0)
+		return (c1 & 0xf);
+	else if (c2 >= 'A' && c2 <= 'F')
 		c2 -= 7;
 	else if (c2 >= 'a' && c2 <= 'f')
 	    c2 -= 39;
@@ -61,24 +59,15 @@ byte hex2uint8(const char *p)
 byte COBD::sendCommand(const char* cmd, char* buf, byte bufsize, int timeout)
 {
 	write(cmd);
-	dataIdleLoop();
+	idleTasks();
 	return receive(buf, bufsize, timeout);
 }
 
-void COBD::sendQuery(byte pid)
+bool COBD::readPID(byte pid, int& result)
 {
 	char cmd[8];
 	sprintf(cmd, "%02X%02X\r", dataMode, pid);
-#ifdef DEBUG
-	debugOutput(cmd);
-#endif
 	write(cmd);
-}
-
-bool COBD::readPID(byte pid, int& result)
-{
-	// send a query command
-	sendQuery(pid);
 	// receive and parse the response
 	return getResult(pid, result);
 }
@@ -107,7 +96,7 @@ byte COBD::readDTC(uint16_t codes[], byte maxCodes)
 		sprintf_P(buffer, n == 0 ? PSTR("03\r") : PSTR("03%02X\r"), n);
 		write(buffer);
 		if (receive(buffer, sizeof(buffer)) > 0) {
-			if (!strstr(buffer, "NO DATA")) {
+			if (!strstr_P(buffer, PSTR("NO DATA"))) {
 				char *p = strstr(buffer, "43");
 				if (p) {
 					while (codesRead < maxCodes && *p) {
@@ -300,20 +289,30 @@ float COBD::getVoltage()
 
 bool COBD::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;
-        }
+	for (byte n = 0; n < 5; n++) {
+		if (sendCommand("0902\r", buffer, bufsize)) {
+			int len = hex2uint16(buffer);
+			char *p = strstr_P(buffer + 4, PSTR("0: 49 02 01"));
+			if (p) {
+				char *q = buffer;
+				p += 11; // skip the header
+				do {
+					while (*(++p) == ' ');
+					for (;;) {
+						*(q++) = hex2uint8(p);
+						while (*p && *p != ' ') p++;
+						while (*p == ' ') p++;
+						if (!*p || *p == '\r') break;
+					}
+					p = strchr(p, ':');
+				} while(p);
+				*q = 0;
+				if (q - buffer == len - 3) {
+					return true;
+				}
+			}
+		}
+		delay(100);
     }
     return false;
 }
@@ -362,7 +361,7 @@ byte COBD::getVersion()
 	return version;
 }
 
-byte COBD::receive(char* buffer, byte bufsize, int timeout)
+int COBD::receive(char* buffer, int bufsize, unsigned int timeout)
 {
 	unsigned char n = 0;
 	unsigned long startTime = millis();
@@ -393,7 +392,7 @@ byte COBD::receive(char* buffer, byte bufsize, int timeout)
 			    // timeout
 			    break;
 			}
-			dataIdleLoop();
+			idleTasks();
 		}
 	}
 	if (buffer) {
@@ -415,48 +414,77 @@ bool COBD::init(OBD_PROTOCOLS protocol)
 {
 	const char *initcmd[] = {"ATZ\r", "ATE0\r", "ATH0\r"};
 	char buffer[64];
+	byte stage;
 
 	m_state = OBD_DISCONNECTED;
-	for (unsigned char i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
-		write(initcmd[i]);
-		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0) {
-			return false;
+
+	for (byte n = 0; n < 2; n++) {
+		stage = 0;
+		if (n != 0) reset();
+		for (byte i = 0; i < sizeof(initcmd) / sizeof(initcmd[0]); i++) {
+			delay(10);
+			if (!sendCommand(initcmd[i], buffer, sizeof(buffer), OBD_TIMEOUT_SHORT)) {
+				continue;
+			}
 		}
-	}
-	if (protocol != PROTO_AUTO) {
-		sprintf_P(buffer, PSTR("ATSP %u\r"), protocol);
-		write(buffer);
-		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) == 0 && !strstr(buffer, "OK")) {
-			return false;
+		stage = 1;
+		if (protocol != PROTO_AUTO) {
+			sprintf(buffer, "ATSP%u\r", protocol);
+			delay(10);
+			if (!sendCommand(buffer, buffer, sizeof(buffer), OBD_TIMEOUT_SHORT) || !strstr(buffer, "OK")) {
+				continue;
+			}
 		}
-	}
-
-	// load pid map
-	memset(pidmap, 0, sizeof(pidmap));
-	bool success = false;
-	for (byte i = 0; i < 4; i++) {
-		byte pid = i * 0x20;
-		sendQuery(pid);
-		if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) > 0) {
-			char *p = buffer;
-			while ((p = strstr(p, "41 "))) {
-				p += 3;
-				if (hex2uint8(p) == pid) {
-					p += 2;
-					for (byte n = 0; n < 4 && *(p + n * 3) == ' '; n++) {
-						pidmap[i * 4 + n] = hex2uint8(p + n * 3 + 1);
+		stage = 2;
+		delay(10);
+		if (!sendCommand("010D\r", buffer, sizeof(buffer), OBD_TIMEOUT_LONG) || checkErrorMessage(buffer)) {
+			continue;
+		}
+		stage = 3;
+		// load pid map
+		memset(pidmap, 0, sizeof(pidmap));
+		bool success = false;
+		for (byte i = 0; i < 4; i++) {
+			byte pid = i * 0x20;
+			sprintf(buffer, "%02X%02X\r", dataMode, pid);
+			delay(10);
+			write(buffer);
+			if (receive(buffer, sizeof(buffer), OBD_TIMEOUT_LONG) > 0) {
+				if (checkErrorMessage(buffer)) {
+					break;
+				}
+				char *p = buffer;
+				while ((p = strstr(p, "41 "))) {
+					p += 3;
+					if (hex2uint8(p) == pid) {
+						p += 2;
+						for (byte n = 0; n < 4 && *(p + n * 3) == ' '; n++) {
+							pidmap[i * 4 + n] = hex2uint8(p + n * 3 + 1);
+						}
+						success = true;
 					}
-					success = true;
 				}
+			} else {
+				break;
 			}
 		}
+		if (success) {
+			stage = 0xff;
+			break;
+		}
 	}
-
-	if (success) {
+	if (stage == 0xff) {
 		m_state = OBD_CONNECTED;
 		errors = 0;
+		return true;
+	} else {
+#ifdef DEBUG
+		Serial.print("Stage:");
+		Serial.println(stage);
+#endif
+		reset();
+		return false;
 	}
-	return success;
 }
 
 void COBD::end()
@@ -477,10 +505,64 @@ bool COBD::setBaudRate(unsigned long baudrate)
     return true;
 }
 
-bool COBD::memsInit()
+void COBD::reset()
+{
+	char buf[32];
+	sendCommand("ATR\r", buf, sizeof(buf));
+	delay(3000);
+	sendCommand("ATZ\r", buf, sizeof(buf));
+}
+
+void COBD::uninit()
+{
+	char buf[32];
+	sendCommand("ATPC\r", buf, sizeof(buf));
+}
+
+byte COBD::checkErrorMessage(const char* buffer)
+{
+	const char *errmsg[] = {"UNABLE", "ERROR", "TIMEOUT", "NO DATA"};
+	for (byte i = 0; i < sizeof(errmsg) / sizeof(errmsg[0]); i++) {
+		if (strstr(buffer, errmsg[i])) return i + 1;
+	}
+	return 0;
+}
+
+uint8_t COBD::getPercentageValue(char* data)
+{
+  return (uint16_t)hex2uint8(data) * 100 / 255;
+}
+
+uint16_t COBD::getLargeValue(char* data)
+{
+  return hex2uint16(data);
+}
+
+uint8_t COBD::getSmallValue(char* data)
+{
+  return hex2uint8(data);
+}
+
+int16_t COBD::getTemperatureValue(char* data)
+{
+  return (int)hex2uint8(data) - 40;
+}
+
+bool COBD::memsInit(bool fusion)
 {
 	char buf[16];
-	return sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0 && !strchr(buf, '?');
+	if (sendCommand("ATTEMP\r", buf, sizeof(buf)) <= 0 || strchr(buf, '?'))
+		return false;
+	if (fusion) {
+		m_fusion = true;
+		return sendCommand("ATQU1\r", buf, sizeof(buf));
+	} else {
+		if (m_fusion) {
+			m_fusion = false;
+			sendCommand("ATQU0\r", buf, sizeof(buf));
+		}
+		return true;
+	}
 }
 
 bool COBD::memsRead(int16_t* acc, int16_t* gyr, int16_t* mag, int16_t* temp)
@@ -515,6 +597,20 @@ bool COBD::memsRead(int16_t* acc, int16_t* gyr, int16_t* mag, int16_t* temp)
 		} while (0);
 		if (!success) return false;
 	}
+	if (mag) {
+		success = false;
+		if (sendCommand("ATMAG\r", buf, sizeof(buf)) > 0) do {
+			char* p = getResultValue(buf);
+			if (!p) break;
+			mag[0] = atoi(p++);
+			if (!(p = strchr(p, ','))) break;
+			mag[1] = atoi(++p);
+			if (!(p = strchr(p, ','))) break;
+			mag[2] = atoi(++p);
+			success = true;
+		} while (0);
+		if (!success) return false;
+	}
 	if (temp) {
 		success = false;
 		if (sendCommand("ATTEMP\r", buf, sizeof(buf)) > 0) {
@@ -529,6 +625,23 @@ bool COBD::memsRead(int16_t* acc, int16_t* gyr, int16_t* mag, int16_t* temp)
 	return true;	
 }
 
+bool COBD::memsOrientation(float& yaw, float& pitch, float& roll)
+{
+	char buf[64];
+	bool success = false;
+	if (sendCommand("ATORI\r", buf, sizeof(buf)) > 0) do {
+		char* p = getResultValue(buf);
+		if (!p) break;
+		yaw = (float)atof(p++);
+		if (!(p = strchr(p, ','))) break;
+		pitch = (float)atoi(++p);
+		if (!(p = strchr(p, ','))) break;
+		roll = (float) atof(++p);
+		success = true;
+	} while (0);
+	return success;
+}
+
 #ifdef DEBUG
 void COBD::debugOutput(const char *s)
 {
diff --git a/libraries/OBD2UART/OBD2UART.h b/libraries/OBD2UART/OBD2UART.h
index 62ac18b..2674274 100644
--- a/libraries/OBD2UART/OBD2UART.h
+++ b/libraries/OBD2UART/OBD2UART.h
@@ -8,8 +8,7 @@
 #include <Arduino.h>
 
 #define OBD_TIMEOUT_SHORT 1000 /* ms */
-#define OBD_TIMEOUT_LONG 5000 /* ms */
-#define OBD_TIMEOUT_GPS 200 /* ms */
+#define OBD_TIMEOUT_LONG 10000 /* ms */
 
 #ifndef OBDUART
 #if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168P__)
@@ -19,6 +18,10 @@
 #endif
 #endif
 
+#ifdef ESP32
+extern HardwareSerial Serial1;
+#endif
+
 // Mode 1 PIDs
 #define PID_ENGINE_LOAD 0x04
 #define PID_COOLANT_TEMP 0x05
@@ -73,23 +76,12 @@
 #define PID_ENGINE_REF_TORQUE 0x63
 
 // non-OBD/custom PIDs (no mode number)
-#define PID_GPS_LATITUDE 0xA
-#define PID_GPS_LONGITUDE 0xB
-#define PID_GPS_ALTITUDE 0xC
-#define PID_GPS_SPEED 0xD
-#define PID_GPS_HEADING 0xE
-#define PID_GPS_SAT_COUNT 0xF
-#define PID_GPS_TIME 0x10
-#define PID_GPS_DATE 0x11
 #define PID_ACC 0x20
 #define PID_GYRO 0x21
 #define PID_COMPASS 0x22
 #define PID_MEMS_TEMP 0x23
 #define PID_BATTERY_VOLTAGE 0x24
 
-// custom PIDs for calculated data
-#define PID_TRIP_DISTANCE 0x30
-
 typedef enum {
     PROTO_AUTO = 0,
     PROTO_ISO_9141_2 = 3,
@@ -115,13 +107,16 @@ uint8_t hex2uint8(const char *p);
 class COBD
 {
 public:
-	COBD():dataMode(1),errors(0),m_state(OBD_DISCONNECTED) {}
 	// begin serial UART
 	virtual byte begin();
+	// terminate communication channel
+	virtual void end();
 	// initialize OBD-II connection
 	virtual bool init(OBD_PROTOCOLS protocol = PROTO_AUTO);
+	// reset OBD-II connection
+	virtual void reset();
 	// un-initialize OBD-II connection
-	virtual void end();
+	virtual void uninit();
 	// set serial baud rate
 	virtual bool setBaudRate(unsigned long baudrate);
 	// get connection state
@@ -144,12 +139,12 @@ public:
 	virtual float getVoltage();
 	// get VIN as a string, buffer length should be >= OBD_RECV_BUF_SIZE
 	virtual bool getVIN(char* buffer, byte bufsize);
-	// initialize MEMS sensor
-	virtual bool memsInit();
+	// initialize MEMS sensor (enable or disable sensor fusion by quanterion algorithm)
+	virtual bool memsInit(bool fusion = false);
 	// read out MEMS data (acc for accelerometer, gyr for gyroscope, temp in 0.1 celcius degree)
 	virtual bool memsRead(int16_t* acc, int16_t* gyr = 0, int16_t* mag = 0, int16_t* temp = 0);
-	// send query for specified PID
-	virtual void sendQuery(byte pid);
+	// get computed orientation values
+	virtual bool memsOrientation(float& yaw, float& pitch, float& roll);
 	// retrive and parse the response of specifie PID
 	virtual bool getResult(byte& pid, int& result);
 	// determine if the PID is supported
@@ -157,37 +152,26 @@ public:
 	// get adapter firmware version
 	virtual byte getVersion();
 	// set current PID mode
-	byte dataMode;
+	byte dataMode = 1;
 	// occurrence of errors
-	byte errors;
+	byte errors = 0;
 	// bit map of supported PIDs
-	byte pidmap[4 * 4];
+	byte pidmap[4 * 4] = {0};
 protected:
 	virtual char* getResponse(byte& pid, char* buffer, byte bufsize);
-	virtual byte receive(char* buffer, byte bufsize, int timeout = OBD_TIMEOUT_SHORT);
+	virtual int receive(char* buffer, int bufsize, unsigned 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;
+	virtual uint8_t getPercentageValue(char* data);
+	virtual uint16_t getLargeValue(char* data);
+	virtual uint8_t getSmallValue(char* data);
+	virtual int16_t getTemperatureValue(char* data);
+	virtual int normalizeData(byte pid, char* data);
+	virtual byte checkErrorMessage(const char* buffer);
+	virtual char* getResultValue(char* buf);
+	OBD_STATES m_state = OBD_DISCONNECTED;
 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;
-	}
-	char* getResultValue(char* buf);
+	void recover();
+	virtual void idleTasks() {}
+	bool m_fusion = false;
 };
 
diff --git a/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino b/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino
index 9b11eab..4d30b0e 100644
--- a/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino
+++ b/libraries/OBD2UART/examples/obd_uart_test/obd_uart_test.ino
@@ -1,25 +1,32 @@
 /*************************************************************************
-* Testing sketch for Freematics OBD-II UART Adapter
+* Testing sketch for Freematics OBD-II UART Adapter V1/V2/V2.1
+* Performs AT command-set test
 * Reads and prints several OBD-II PIDs value
-* Distributed under GPL v2.0
-* Visit http://freematics.com for more information
-* Written by Stanley Huang <support@freematics.com.au>
+* Reads and prints motion sensor data if available
+* Distributed under BSD
+* Visit https://freematics.com/products for more product information
+* Written by Stanley Huang <stanley@freematics.com.au>
 *************************************************************************/
 
-#include <SoftwareSerial.h>
 #include <OBD2UART.h>
 
-// On Arduino Leonardo, Micro, MEGA or DUE, hardware serial can be used for output
-// as OBD-II UART adapter uses to Serial1
-// On Arduino UNO and those have no Serial1, we use software serial for output
-// as OBD-II UART adapter uses to Serial
+// On Arduino Leonardo, Micro, MEGA or DUE, hardware serial can be used for output as the adapter occupies Serial1
+// On Arduino UNO and those have no Serial1, we use software serial for output as the adapter uses Serial
+#ifdef ARDUINO_AVR_UNO
+#include <SoftwareSerial.h>
 SoftwareSerial mySerial(A2, A3);
-//#define mySerial Serial
+#else
+#define mySerial Serial
+#endif
+
+#if defined(ESP32) && !defined(Serial1)
+HardwareSerial Serial1(1);
+#endif
 
 COBD obd;
 bool hasMEMS;
 
-void testOut()
+void testATcommands()
 {
     static const char cmds[][6] = {"ATZ\r", "ATI\r", "ATH0\r", "ATRV\r", "0100\r", "010C\r", "0902\r"};
     char buf[128];
@@ -93,11 +100,12 @@ void readBatteryVoltage()
 
 void readMEMS()
 {
-  int acc[3];
-  int gyro[3];
-  int temp;
+  int16_t acc[3] = {0};
+  int16_t gyro[3] = {0};
+  int16_t mag[3] = {0};
+  int16_t temp = 0;
 
-  if (!obd.memsRead(acc, gyro, 0, &temp)) return;
+  if (!obd.memsRead(acc, gyro, mag, &temp)) return;
 
   mySerial.print('[');
   mySerial.print(millis());
@@ -117,6 +125,13 @@ void readMEMS()
   mySerial.print('/');
   mySerial.print(gyro[2]);
 
+  mySerial.print(" MAG:");
+  mySerial.print(mag[0]);
+  mySerial.print('/');
+  mySerial.print(mag[1]);
+  mySerial.print('/');
+  mySerial.print(mag[2]);
+
   mySerial.print(" TEMP:");
   mySerial.print((float)temp / 10, 1);
   mySerial.println("C");
@@ -127,23 +142,24 @@ void setup()
   mySerial.begin(115200);
   while (!mySerial);
   
-  // this will begin serial
-  byte version = obd.begin();
-
-  mySerial.print("Freematics OBD-II Adapter ");
-  if (version > 0) {
-    mySerial.print("Ver. ");
-    mySerial.print(version / 10);
-    mySerial.print('.');
-    mySerial.println(version % 10);
-  } else {
-    mySerial.println("not detected");
-    for (;;);
+  for (;;) {
+    delay(1000);
+    byte version = obd.begin();
+    mySerial.print("Freematics OBD-II Adapter ");
+    if (version > 0) {
+      mySerial.println("detected");
+      mySerial.print("OBD firmware version ");
+      mySerial.print(version / 10);
+      mySerial.print('.');
+      mySerial.println(version % 10);
+      break;
+    } else {
+      mySerial.println("not detected");
+    }
   }
-  delay(1000);
 
   // send some commands for testing and show response for debugging purpose
-  testOut();
+  testATcommands();
 
   hasMEMS = obd.memsInit();
   mySerial.print("MEMS:");
@@ -151,8 +167,9 @@ void setup()
   
   // initialize OBD-II adapter
   do {
-    mySerial.println("Init...");
+    mySerial.println("Connecting...");
   } while (!obd.init());
+  mySerial.println("OBD connected!");
 
   char buf[64];
   if (obd.getVIN(buf, sizeof(buf))) {
@@ -160,7 +177,7 @@ void setup()
       mySerial.println(buf);
   }
   
-  unsigned int codes[6];
+  uint16_t codes[6];
   byte dtcCount = obd.readDTC(codes, 6);
   if (dtcCount == 0) {
     mySerial.println("No DTC"); 
@@ -176,7 +193,6 @@ void setup()
   delay(5000);
 }
 
-
 void loop()
 {
   readPIDSingle();
@@ -186,3 +202,4 @@ void loop()
     readMEMS();
   }
 }
+
diff --git a/libraries/OBD2UART/examples/orientation_demo/orientation_demo.ino b/libraries/OBD2UART/examples/orientation_demo/orientation_demo.ino
new file mode 100644
index 0000000..e04949c
--- /dev/null
+++ b/libraries/OBD2UART/examples/orientation_demo/orientation_demo.ino
@@ -0,0 +1,97 @@
+/*************************************************************************
+* Testing sketch for Freematics OBD-II UART Adapter V2.1
+* Reads and prints motion sensor data and computed orientation data
+* Distributed under BSD
+* Visit https://freematics.com/products for more product information
+* Written by Stanley Huang <stanley@freematics.com.au>
+*************************************************************************/
+
+#include <OBD2UART.h>
+
+// On Arduino Leonardo, Micro, MEGA or DUE, hardware serial can be used for output as the adapter occupies Serial1
+// On Arduino UNO and those have no Serial1, we use software serial for output as the adapter uses Serial
+#ifdef ARDUINO_AVR_UNO
+#include <SoftwareSerial.h>
+SoftwareSerial mySerial(A2, A3);
+#else
+#define mySerial Serial
+#endif
+
+#if defined(ESP32) && !defined(Serial1)
+HardwareSerial Serial1(1);
+#endif
+
+COBD obd;
+
+void setup()
+{
+  mySerial.begin(115200);
+  while (!mySerial);
+  
+  // this will begin serial
+
+  for (;;) {
+    delay(1000);
+    byte version = obd.begin();
+    mySerial.print("Freematics OBD-II Adapter ");
+    if (version > 0) {
+      mySerial.println("detected");
+      break;
+    } else {
+      mySerial.println("not detected");
+    }
+  }
+  
+  // initialize MEMS with sensor fusion enabled
+  bool hasMEMS = obd.memsInit(true);
+  mySerial.print("Motion sensor is ");
+  mySerial.println(hasMEMS ? "present" : "not present");
+  if (!hasMEMS) {
+    for (;;) delay(1000);
+  }
+}
+
+
+void loop()
+{
+  int16_t acc[3] = {0};
+  int16_t gyro[3] = {0};
+  int16_t mag[3] = {0};
+
+  if (!obd.memsRead(acc, gyro, mag)) return;
+  
+  mySerial.print("ACC:");
+  mySerial.print(acc[0]);
+  mySerial.print('/');
+  mySerial.print(acc[1]);
+  mySerial.print('/');
+  mySerial.print(acc[2]);
+
+  mySerial.print(" GYRO:");
+  mySerial.print(gyro[0]);
+  mySerial.print('/');
+  mySerial.print(gyro[1]);
+  mySerial.print('/');
+  mySerial.print(gyro[2]);
+
+  mySerial.print(" MAG:");
+  mySerial.print(mag[0]);
+  mySerial.print('/');
+  mySerial.print(mag[1]);
+  mySerial.print('/');
+  mySerial.print(mag[2]);
+
+  mySerial.println();
+
+  float yaw, pitch, roll;
+  if (obd.memsOrientation(yaw, pitch, roll)) {
+    mySerial.print("Orientation: ");
+    mySerial.print(yaw, 2);
+    mySerial.print(' ');
+    mySerial.print(pitch, 2);
+    mySerial.print(' ');
+    mySerial.println(roll, 2);
+  }
+
+  delay(100);
+}
diff --git a/libraries/OBD2UART/examples/rpm_led_uart/rpm_led_uart.ino b/libraries/OBD2UART/examples/rpm_led_uart/rpm_led_uart.ino
index 3e80ae3..5b7a13e 100644
--- a/libraries/OBD2UART/examples/rpm_led_uart/rpm_led_uart.ino
+++ b/libraries/OBD2UART/examples/rpm_led_uart/rpm_led_uart.ino
@@ -1,8 +1,9 @@
 /*************************************************************************
-* Sample sketch based on OBD-II library for Arduino
-* Distributed under GPL v2.0
-* Visit http://freematics.com for more information
-* (C)2012-2014 Stanley Huang <stanleyhuangyc@gmail.com>
+* Testing sketch for Freematics OBD-II UART Adapter
+* Reads engine RPM data from OBD and triggers Arduino onboard LED
+* Distributed under BSD
+* Visit https://freematics.com/products for more product information
+* Written by Stanley Huang <stanley@freematics.com.au>
 *************************************************************************/
 
 #include <OBD2UART.h>
diff --git a/libraries/OBD2UART/keywords.txt b/libraries/OBD2UART/keywords.txt
new file mode 100644
index 0000000..316a4fb
--- /dev/null
+++ b/libraries/OBD2UART/keywords.txt
@@ -0,0 +1,29 @@
+################################################################################
+# Datatypes (KEYWORD1)
+################################################################################
+COBD	KEYWORD1
+
+################################################################################
+# Methods and Functions (KEYWORD2)
+################################################################################
+begin	KEYWORD2
+end	KEYWORD2
+init	KEYWORD2
+uninit	KEYWORD2
+reset	KEYWORD2
+
+readPID	KEYWORD2
+readDTC	KEYWORD2
+clearDTC	KEYWORD2
+getVoltage	KEYWORD2
+getVIN	KEYWORD2
+
+memsInit KEYWORD2
+memsRead KEYWORD2
+memsOrientation KEYWORD2
+
+################################################################################
+# Constants (LITERAL1)
+################################################################################
+OBD_PROTOCOLS	LITERAL1
+OBD_STATES	LITERAL1
\ No newline at end of file
diff --git a/libraries/OBD2UART/library.properties b/libraries/OBD2UART/library.properties
new file mode 100644
index 0000000..cbb921d
--- /dev/null
+++ b/libraries/OBD2UART/library.properties
@@ -0,0 +1,9 @@
+name=OBD2UART
+version=1.2
+author=Stanley Huang <stanley@freematics.com.au>
+maintainer=Stanley Huang <stanley@freematics.com.au>
+sentence=Arduino library for Freematics OBD-II UART Adapter
+paragraph=Arduino library for Freematics OBD-II UART Adapter
+category=Communication
+url=https://freematics.com
+includes=OBD2UART.h
\ No newline at end of file