Add a function to parse the entire payload as a float.

3 files changed, 24 insertions, 0 deletions

diff --git a/src/uds/uds.c b/src/uds/uds.c
index 443b05a0..b3f2cdad 100644
--- a/src/uds/uds.c
+++ b/src/uds/uds.c
@@ -1,5 +1,6 @@
 #include <uds/uds.h>
 #include <bitfield/bitfield.h>
+#include <canutil/read.h>
 #include <string.h>
 #include <limits.h>
 #include <stddef.h>
@@ -241,3 +242,9 @@ 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);
+}
diff --git a/src/uds/uds.h b/src/uds/uds.h
index 091d3c9f..78f70e15 100644
--- a/src/uds/uds.h
+++ b/src/uds/uds.h
@@ -83,6 +83,8 @@ DiagnosticResponse diagnostic_receive_can_frame(DiagnosticShims* shims,
         const uint16_t arbitration_id, const uint8_t data[],
         const uint8_t size);
 
+float diagnostic_payload_to_float(const DiagnosticResponse* response);
+
 /* Public: Render a DiagnosticResponse as a string into the given buffer.
  *
  * TODO implement this
diff --git a/tests/test_core.c b/tests/test_core.c
index 489e1740..c37656e8 100644
--- a/tests/test_core.c
+++ b/tests/test_core.c
@@ -260,6 +260,20 @@ START_TEST (test_negative_response)
 }
 END_TEST
 
+START_TEST (test_payload_to_float)
+{
+    uint16_t arb_id = OBD2_MODE_POWERTRAIN_DIAGNOSTIC_REQUEST;
+    DiagnosticRequestHandle handle = diagnostic_request_pid(&SHIMS,
+            DIAGNOSTIC_STANDARD_PID, arb_id, 0x2, response_received_handler);
+
+    fail_if(last_response_was_received);
+    const uint8_t can_data[] = {0x4, 0x1 + 0x40, 0x2, 0x45, 0x12};
+    DiagnosticResponse response = diagnostic_receive_can_frame(&SHIMS, &handle, arb_id + 0x8,
+            can_data, sizeof(can_data));
+    ck_assert_int_eq(diagnostic_payload_to_float(&response), 0x4512);
+}
+END_TEST
+
 Suite* testSuite(void) {
     Suite* s = suite_create("uds");
     TCase *tc_core = tcase_create("core");
@@ -275,6 +289,7 @@ Suite* testSuite(void) {
     tcase_add_test(tc_core, test_missing_pid);
     tcase_add_test(tc_core, test_wrong_pid_then_right_completes);
     tcase_add_test(tc_core, test_negative_response);
+    tcase_add_test(tc_core, test_payload_to_float);
 
     // TODO these are future work:
     // TODO test request MIL