From e14808fc32529033a615416f4cf35f6918750e60 Mon Sep 17 00:00:00 2001
From: Stanley Huang <stanleyhuangyc@gmail.com>
Date: Mon, 19 Dec 2016 17:20:57 +1100
Subject: Added readDTC()

---
 libraries/OBD/OBD.cpp | 37 +++++++++++++++++++++++++++++++++++++
 libraries/OBD/OBD.h   |  2 ++
 2 files changed, 39 insertions(+)

(limited to 'libraries')

diff --git a/libraries/OBD/OBD.cpp b/libraries/OBD/OBD.cpp
index d551f93..c66db7b 100644
--- a/libraries/OBD/OBD.cpp
+++ b/libraries/OBD/OBD.cpp
@@ -100,6 +100,43 @@ byte COBD::readPID(const byte pid[], byte count, int result[])
 	return results;
 }
 
+byte COBD::readDTC(uint16_t codes[], byte count)
+{
+	/*
+	Response example:
+	0: 43 04 01 08 01 09 
+	1: 01 11 01 15 00 00 00
+	*/ 
+	byte codesRead = 0;
+ 	for (byte n = 0; n < 6; n++) {
+		char buffer[128];
+		sprintf(buffer, "03%02X\r", n);
+		write(buffer);
+		Serial.println(buffer);
+		if (receive(buffer, sizeof(buffer)) > 0) {
+			Serial.println(buffer);
+			if (!strstr(buffer, "NO DATA")) {
+				char *p = strstr(buffer, "43");
+				if (p) {
+					while (codesRead < count && *p) {
+						p += 6;
+						if (*p == '\r') {
+							p = strchr(p, ':');
+							if (!p) break;
+							p += 2; 
+						}
+						uint16_t code = hex2uint16(p);
+						if (code == 0) break;
+						codes[codesRead++] = code;
+					}
+				}
+				break;
+			}
+		}
+	}
+	return codesRead;
+}
+
 void COBD::clearDTC()
 {
 	char buffer[32];
diff --git a/libraries/OBD/OBD.h b/libraries/OBD/OBD.h
index 4109533..b6793d7 100644
--- a/libraries/OBD/OBD.h
+++ b/libraries/OBD/OBD.h
@@ -122,6 +122,8 @@ public:
 	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);
+	// read diagnostic trouble codes (return number of DTCs read)
+	virtual byte readDTC(uint16_t codes[], byte count = 1);
 	// clear diagnostic trouble code
 	virtual void clearDTC();
 	// get battery voltage (works without ECU)
-- 
cgit v1.2.3