Show negative response codes in log output.

[apps/low-level-can-service.git] / src / uds / uds.c

diff --git a/src/uds/uds.c b/src/uds/uds.c
index 9fec9a8..5a45c0c 100644 (file)
--- a/src/uds/uds.c
+++ b/src/uds/uds.c
@@ -94,18 +94,9 @@ DiagnosticRequestHandle diagnostic_request(DiagnosticShims* shims,
             1 + request->payload_length + request->pid_length,
             NULL);
     if(shims->log != NULL) {
-        shims->log("Sending diagnostic request: arb_id: 0x%02x, mode: 0x%x, pid: 0x%x, payload: 0x%02x%02x%02x%02x%02x%02x%02x, size: %d\r\n",
-                request->arbitration_id,
-                request->mode,
-                request->pid,
-                request->payload[0],
-                request->payload[1],
-                request->payload[2],
-                request->payload[3],
-                request->payload[4],
-                request->payload[5],
-                request->payload[6],
-                request->payload_length);
+        char request_string[128] = {0};
+        diagnostic_request_to_string(request, request_string, sizeof(request_string));
+        shims->log("Sending diagnostic request: %s", request_string);
     }
 
     setup_receive_handle(&handle);
@@ -226,25 +217,39 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
                 if(message.size > 0) {
                     response.mode = message.payload[0];
                     if(handle_negative_response(&message, &response, shims)) {
-                        shims->log("Received a negative response to mode 0x%x on arb ID 0x%x",
-                                response.mode, response.arbitration_id);
+                        if(shims->log != NULL) {
+                            char response_string[128] = {0};
+                            diagnostic_response_to_string(&response, response_string, sizeof(response_string));
+                            shims->log("Received a negative response: %s", response_string);
+                        }
+
                         handle->success = true;
                         handle->completed = true;
                     } else if(handle_positive_response(handle, &message,
                                 &response, shims)) {
-                        shims->log("Received a positive mode 0x%x response on arb ID 0x%x",
-                                response.mode, response.arbitration_id);
+                        if(shims->log != NULL) {
+                            char response_string[128] = {0};
+                            diagnostic_response_to_string(&response, response_string, sizeof(response_string));
+                            shims->log("Received a positive response: %s", response_string);
+                        }
+
                         handle->success = true;
                         handle->completed = true;
                     } else {
-                        shims->log("Response was for a mode 0x%x request (pid 0x%x), not our mode 0x%x request (pid 0x%x)",
-                                MAX(0, response.mode - MODE_RESPONSE_OFFSET),
-                                response.pid, handle->request.mode,
-                                handle->request.pid);
+                        if(shims->log != NULL) {
+                            char response_string[128] = {0};
+                            diagnostic_response_to_string(&response, response_string, sizeof(response_string));
+                            shims->log("Expected a mode 0x%x response to pid 0x%x but received: %s",
+                                    MAX(0, response.mode - MODE_RESPONSE_OFFSET),
+                                    response.pid,
+                                    response_string);
+                        }
                     }
                 } else {
-                    shims->log("Received an empty response on arb ID 0x%x",
-                            response.arbitration_id);
+                    if(shims->log != NULL) {
+                        shims->log("Received an empty response on arb ID 0x%x",
+                                response.arbitration_id);
+                    }
                 }
 
                 if(handle->completed && handle->callback != NULL) {
@@ -258,17 +263,13 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
     return response;
 }
 
-float diagnostic_payload_to_float(const DiagnosticResponse* response) {
-    return bitfield_parse_float(response->payload,
-            response->payload_length, 0,
-            response->payload_length * CHAR_BIT, 1.0, 0);
+int diagnostic_payload_to_integer(const DiagnosticResponse* response) {
+    return get_bitfield(response->payload, response->payload_length, 0,
+            response->payload_length * CHAR_BIT);
 }
 
-/* Public:
- *
- * Functions pulled from http://en.wikipedia.org/wiki/OBD-II_PIDs#Mode_01
- */
-float diagnostic_decode_obd2_pid(const DiagnosticResponse* response) {
+float diagnostic_decode_obd2_pid(const DiagnosticResponse* response,
+        int parsed_payload) {
     // handles on the single number values, not the bit encoded ones
     switch(response->pid) {
         case 0xa:
@@ -300,3 +301,64 @@ float diagnostic_decode_obd2_pid(const DiagnosticResponse* response) {
             return 0;
     }
 }
+
+void diagnostic_response_to_string(const DiagnosticResponse* response,
+        char* destination, size_t destination_length) {
+    int bytes_used = snprintf(destination, destination_length,
+            "arb_id: 0x%02x, mode: 0x%x, ",
+            response->arbitration_id,
+            response->mode);
+
+    if(response->has_pid) {
+        bytes_used += snprintf(destination + bytes_used,
+                destination_length - bytes_used,
+                "pid: 0x%x, ",
+                response->pid);
+    }
+
+    if(!response->success) {
+        bytes_used += snprintf(destination + bytes_used,
+                destination_length - bytes_used,
+                "negative response code: 0x%x, ",
+                response->negative_response_code);
+    }
+
+    if(response->payload_length > 0) {
+        snprintf(destination + bytes_used, destination_length - bytes_used,
+                "payload: 0x%02x%02x%02x%02x%02x%02x%02x",
+                response->payload[0],
+                response->payload[1],
+                response->payload[2],
+                response->payload[3],
+                response->payload[4],
+                response->payload[5],
+                response->payload[6]);
+    } else {
+        snprintf(destination + bytes_used, destination_length - bytes_used,
+                "no payload");
+    }
+}
+
+void diagnostic_request_to_string(const DiagnosticRequest* request,
+        char* destination, size_t destination_length) {
+    int bytes_used = snprintf(destination, destination_length,
+            "arb_id: 0x%02x, mode: 0x%x, pid: 0x%x, ",
+            request->arbitration_id,
+            request->mode,
+            request->pid);
+    int remaining_space = destination_length - bytes_used;
+    if(request->payload_length > 0) {
+        snprintf(destination + bytes_used, remaining_space,
+                "payload: 0x%02x%02x%02x%02x%02x%02x%02x",
+                request->payload[0],
+                request->payload[1],
+                request->payload[2],
+                request->payload[3],
+                request->payload[4],
+                request->payload[5],
+                request->payload[6]);
+    } else {
+        snprintf(destination + bytes_used, remaining_space, "no payload");
+    }
+}
+