Diff: OBD2.cpp

Revision:

0:804498f871bc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OBD2.cpp	Wed Dec 20 07:20:57 2017 +0000
@@ -0,0 +1,84 @@
+#include "mbed.h"
+#include "OBD2.h"
+
+OBD2::OBD2 (PinName rd, PinName td, int speed) : _can(rd, td) {
+    _can.frequency(speed);
+    _can.attach(this, &OBD2::isrRecv, CAN::RxIrq);
+}
+
+void OBD2::isrRecv () {
+    int i;
+    CANMessage msg;
+
+    if (_can.read(msg)) {
+        printf(" id=%03x, type=%02x, format=%02x, len=%d \r\n", msg.id, msg.type, msg.format, msg.len);
+        for (i = 0; i < 6; i ++) {
+            printf(" %02x", msg.data[i]);
+        }
+        printf("\r\n");
+
+        if (msg.id == PID_REPLY && msg.data[1] == 0x41) {
+            if (msg.data[2] == _pid && !_received) {
+                for (i = 0; i < msg.data[0] - 2 && i < 5; i ++) {
+                    _data[i] = msg.data[i + 3];
+                }
+                _received = 1;
+            }
+        }
+    }
+}
+
+int OBD2::request (int pid) {
+    int i;
+    CANMessage msg;
+
+    msg.id = PID_REQUEST_ALL;
+    msg.len = 8;
+    msg.data[0] = 0x02; // len
+    msg.data[1] = 0x01; // mode: Show current data
+    msg.data[2] = pid;
+    for (i = 3; i < 8; i ++) {
+        msg.data[i] = 0;
+    }
+
+    _pid = pid;
+    _received = 0;
+    if (_can.write(msg)) {
+        return 0;
+    }
+    return -1;
+}
+
+float OBD2::read () {
+    float f = 0;
+    Timer t;
+
+    t.start();
+    while (!_received) {
+        if (t.read_ms() > 500) return 0;
+    }
+    printf("  %dms\r\n", t.read_ms());
+
+    switch (_pid) {
+    case ENGINE_LOAD:
+    case THROTTLE:
+    case FUEL_LEVEL:
+        f = _data[0] / 255.0 * 100; // %
+        break;
+    case ENGINE_COOLANT_TEMP:
+    case INTAKE_TEMP:
+    case AMB_AIR_TEMP:
+        f = _data[0] - 40; // Deg C
+        break;
+    case INTAKE_PRESSURE:
+        f = _data[0]; // kPa
+        break;
+    case ENGINE_RPM:
+        f = ((_data[0] << 8) | _data[1]) / 4.0; // rpm
+        break;
+    case VEHICLE_SPEED:
+        f = _data[0]; // km/h
+        break;
+    }
+    return f;
+}