File content as of revision 0:804498f871bc:

#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;
}