Improve VIN reading

1 files changed, 25 insertions, 15 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;
 }