Prep request handler to receive again if we got wrong mode or PID.

1 files changed, 9 insertions, 3 deletions

diff --git a/src/obd2/obd2.c b/src/obd2/obd2.c
index 88ce74cf..1c3d7bc3 100644
--- a/src/obd2/obd2.c
+++ b/src/obd2/obd2.c
@@ -29,6 +29,13 @@ DiagnosticShims diagnostic_init_shims(LogShim log,
     return shims;
 }
 
+static void setup_receive_handle(DiagnosticRequestHandle* handle) {
+    handle->isotp_receive_handle = isotp_receive(&handle->isotp_shims,
+            handle->request.arbitration_id + ARBITRATION_ID_OFFSET,
+            NULL);
+}
+
+
 DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
         DiagnosticRequest* request, DiagnosticResponseReceived callback) {
     DiagnosticRequestHandle handle = {
@@ -72,9 +79,7 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
                 request->payload_length);
     }
 
-    handle.isotp_receive_handle = isotp_receive(&handle.isotp_shims,
-            request->arbitration_id + ARBITRATION_ID_OFFSET,
-            NULL);
+    setup_receive_handle(&handle);
 
     // TODO notes on multi frame:
     // TODO what are the timers for exactly?
@@ -201,6 +206,7 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
                     shims->log("Response was for a mode 0x%x request (pid 0x%x), not our mode 0x%x request (pid 0x%x)",
                             response.mode - MODE_RESPONSE_OFFSET, response.pid,
                             handle->request.mode, handle->request.pid);
+                    setup_receive_handle(handle);
                 }
             } else {
                 shims->log("Received an empty response on arb ID 0x%x",