Added readDTC()

2 files changed, 39 insertions, 0 deletions

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)