Add calculations for many OBD-II PID values.

3 files changed, 39 insertions, 2 deletions

diff --git a/src/uds/extras.h b/src/uds/extras.h
index c59c7bad..126e5d42 100644
--- a/src/uds/extras.h
+++ b/src/uds/extras.h
@@ -63,8 +63,6 @@ bool diagnostic_clear_dtc(DiagnosticShims* shims);
 DiagnosticRequestHandle diagnostic_enumerate_pids(DiagnosticShims* shims,
         DiagnosticRequest* request, DiagnosticPidEnumerationReceived callback);
 
-// TODO
-float diagnostic_decode_OBD2_pid(DiagnosticResponse* response);
 
 #ifdef __cplusplus
 }
diff --git a/src/uds/uds.c b/src/uds/uds.c
index 6a4ce451..ff35b24a 100644
--- a/src/uds/uds.c
+++ b/src/uds/uds.c
@@ -262,3 +262,40 @@ float diagnostic_payload_to_float(const DiagnosticResponse* response) {
             response->payload_length, 0,
             response->payload_length * CHAR_BIT, 1.0, 0);
 }
+
+/* Public:
+ *
+ * Functions pulled from http://en.wikipedia.org/wiki/OBD-II_PIDs#Mode_01
+ */
+float diagnostic_decode_obd2_pid(const DiagnosticResponse* response) {
+    // handles on the single number values, not the bit encoded ones
+    switch(response->pid) {
+        case 0xa:
+            return response->payload[0] * 3;
+        case 0xc:
+            return (response->payload[0] * 256 + response->payload[1]) / 4.0;
+        case 0xd:
+        case 0x33:
+        case 0xb:
+            return response->payload[0];
+        case 0x10:
+            return (response->payload[0] * 256 + response->payload[1]) / 100.0;
+        case 0x11:
+        case 0x2f:
+        case 0x45:
+        case 0x4c:
+        case 0x52:
+        case 0x5a:
+        case 0x4:
+            return response->payload[0] * 100.0 / 255.0;
+        case 0x46:
+        case 0x5c:
+        case 0xf:
+        case 0x5:
+            return response->payload[0] - 40;
+        case 0x62:
+            return response->payload[0] - 125;
+        default:
+            return 0;
+    }
+}
diff --git a/src/uds/uds.h b/src/uds/uds.h
index 78f70e15..a4761ca2 100644
--- a/src/uds/uds.h
+++ b/src/uds/uds.h
@@ -97,6 +97,8 @@ float diagnostic_payload_to_float(const DiagnosticResponse* response);
 // void diagnostic_response_to_string(const DiagnosticResponse* response,
         // char* destination, size_t destination_length);
 
+float diagnostic_decode_obd2_pid(const DiagnosticResponse* response);
+
 #ifdef __cplusplus
 }
 #endif