From d67cabd13b74d5f6c7720eaedc57f12bcbaf5184 Mon Sep 17 00:00:00 2001
From: Ivor Wanders <ivor@iwanders.net>
Date: Mon, 3 Oct 2022 19:36:29 -0400
Subject: [PATCH] Add KWP slow init procedure.

---
 README.md                                |  2 +-
 examples/readerKWPSlow/readerKWPSlow.ino | 71 ++++++++++++++++++++++++
 src/OBD9141.cpp                          | 23 +++++++-
 src/OBD9141.h                            |  3 +
 4 files changed, 95 insertions(+), 4 deletions(-)
 create mode 100644 examples/readerKWPSlow/readerKWPSlow.ino

diff --git a/README.md b/README.md
index d81bb14..c87f092 100644
--- a/README.md
+++ b/README.md
@@ -47,7 +47,7 @@ The following trouble-code related modes are supported: Reading stored trouble c
 
 ISO 14230-2 (KWP)
 -----------------
-The [ISO 14230-2][KWP] protocol uses the same physical layer as 9141-2, the support for this protocol was developed under [issue #11](https://github.com/iwanders/OBD9141/issues/11). If `initKWP()` is called instead of `init()` the KWP protocol is used for all requests onward. The provided functionality should be the same regardless of the protocol used. A sketch that just tests the KWP functionality is available [`readerKWP`](examples/readerKWP/readerKWP.ino).
+The [ISO 14230-2][KWP] protocol uses the same physical layer as 9141-2, the support for this protocol was developed under [issue #11](https://github.com/iwanders/OBD9141/issues/11). If `initKWP()` is called instead of `init()` the KWP protocol is used for all requests onward. The provided functionality should be the same regardless of the protocol used. A sketch that just tests the KWP functionality is available [`readerKWP`](examples/readerKWP/readerKWP.ino). A KWP slow init method was added based on [issue #34](https://github.com/iwanders/OBD9141/issues/34) and [issue #41](https://github.com/iwanders/OBD9141/issues/41), this is like the standard 9141-2 init, but `v1` and `v2` are not equal.
 
 License
 ------
diff --git a/examples/readerKWPSlow/readerKWPSlow.ino b/examples/readerKWPSlow/readerKWPSlow.ino
new file mode 100644
index 0000000..ff465b0
--- /dev/null
+++ b/examples/readerKWPSlow/readerKWPSlow.ino
@@ -0,0 +1,71 @@
+#include "Arduino.h"
+#include "OBD9141.h"
+
+#define RX_PIN 0
+#define TX_PIN 1
+#define EN_PIN 2
+
+
+OBD9141 obd;
+
+
+void setup(){
+    Serial.begin(9600);
+    delay(2000);
+
+    pinMode(EN_PIN, OUTPUT);
+    digitalWrite(EN_PIN, HIGH);
+
+    obd.begin(Serial1, RX_PIN, TX_PIN);
+
+}
+    
+void loop(){
+    Serial.println("Looping");
+
+    // only change from reader is the init method here.
+    bool init_success =  obd.initKWPSlow();
+    Serial.print("init_success:");
+    Serial.println(init_success);
+    delay(50);
+
+    //init_success = true;
+    // Uncomment this line if you use the simulator to force the init to be
+    // interpreted as successful. With an actual ECU; be sure that the init is 
+    // succesful before trying to request PID's.
+
+    if (init_success){
+        bool res;
+        while(1){
+            res = obd.getCurrentPID(0x11, 1);
+            if (res){
+                Serial.print("Result 0x11 (throttle): ");
+                Serial.println(obd.readUint8());
+            }
+            delay(50);
+            
+            res = obd.getCurrentPID(0x0C, 2);
+            if (res){
+                Serial.print("Result 0x0C (RPM): ");
+                Serial.println(obd.readUint16()/4);
+            }
+            delay(50);
+
+
+            res = obd.getCurrentPID(0x0D, 1);
+            if (res){
+                Serial.print("Result 0x0D (speed): ");
+                Serial.println(obd.readUint8());
+            }
+            Serial.println();
+
+            delay(200);
+        }
+        delay(200);
+    }
+    delay(3000);
+}
+
+
+
+
diff --git a/src/OBD9141.cpp b/src/OBD9141.cpp
index e05090a..d8a2443 100644
--- a/src/OBD9141.cpp
+++ b/src/OBD9141.cpp
@@ -352,7 +352,7 @@ uint16_t OBD9141::getTroubleCode(uint8_t index)
   return *reinterpret_cast<uint16_t*>(&(this->buffer[index*2 + 4]));
 }
 
-bool OBD9141::init(){
+bool OBD9141::initImpl(bool check_v1_v2){
     use_kwp_ = false;
     // this function performs the ISO9141 5-baud 'slow' init.
     this->set_port(false); // disable the port.
@@ -418,8 +418,10 @@ bool OBD9141::init(){
     OBD9141print("v2: "); OBD9141println(v2);
 
     // these two should be identical according to the spec.
-    if (v1 != v2){
-        return false;
+    if (check_v1_v2) {
+      if (v1 != v2){
+          return false;
+      }
     }
 
     // we obtained w1 and w2, now invert and send it back.
@@ -446,6 +448,21 @@ bool OBD9141::init(){
     }
 }
 
+
+bool OBD9141::init(){
+  // Normal 9141-2 slow init, check v1 == v2.
+  return initImpl(true);
+}
+
+bool OBD9141::initKWPSlow(){
+  // KWP slow init, v1 == 233, v2 = 143, don't check v1 == v2.
+  bool res = initImpl(false);
+  // After the init, switch to kwp.
+  use_kwp_ = true;
+  return res;
+}
+
+
 bool OBD9141::initKWP(){
     use_kwp_ = true;
     // this function performs the KWP2000 fast init.
diff --git a/src/OBD9141.h b/src/OBD9141.h
index d505f0e..f3451ba 100644
--- a/src/OBD9141.h
+++ b/src/OBD9141.h
@@ -116,6 +116,8 @@ class OBD9141{
         uint8_t buffer[OBD9141_BUFFER_SIZE]; // internal buffer.
 
         bool use_kwp_;
+
+        bool initImpl(bool check_v1_v2);
     public:
         OBD9141();
 
@@ -196,6 +198,7 @@ class OBD9141{
 
         bool init(); // attempts 'slow' ISO9141 5 baud init.
         bool initKWP();  // attempts kwp2000 fast init.
+        bool initKWPSlow(); // attempts 'slow' 5 baud kwp init, v1 = 233, v2 = 143.
         // returns whether the procedure was finished correctly.
         // The class keeps no track of whether this was successful or not.
         // It is up to the user to ensure that the initialisation is called.