OBD2.cpp

00001 #include "mbed.h"
00002 #include "OBD2.h"
00003 
00004 OBD2::OBD2 (PinName rd, PinName td, int speed) : _can(rd, td) {
00005     _can.frequency(speed);
00006     _can.attach(this, &OBD2::isrRecv, CAN::RxIrq);
00007 }
00008 
00009 void OBD2::isrRecv () {
00010     int i;
00011     CANMessage msg;
00012 
00013     if (_can.read(msg)) {
00014         printf(" id=%03x, type=%02x, format=%02x, len=%d \r\n", msg.id, msg.type, msg.format, msg.len);
00015         for (i = 0; i < 6; i ++) {
00016             printf(" %02x", msg.data[i]);
00017         }
00018         printf("\r\n");
00019 
00020         if (msg.id == PID_REPLY && msg.data[1] == 0x41) {
00021             if (msg.data[2] == _pid && !_received) {
00022                 for (i = 0; i < msg.data[0] - 2 && i < 5; i ++) {
00023                     _data[i] = msg.data[i + 3];
00024                 }
00025                 _received = 1;
00026             }
00027         }
00028     }
00029 }
00030 
00031 int OBD2::request (int pid) {
00032     int i;
00033     CANMessage msg;
00034 
00035     msg.id = PID_REQUEST_ALL;
00036     msg.len = 8;
00037     msg.data[0] = 0x02; // len
00038     msg.data[1] = 0x01; // mode: Show current data
00039     msg.data[2] = pid;
00040     for (i = 3; i < 8; i ++) {
00041         msg.data[i] = 0;
00042     }
00043 
00044     _pid = pid;
00045     _received = 0;
00046     if (_can.write(msg)) {
00047         return 0;
00048     }
00049     return -1;
00050 }
00051 
00052 float OBD2::read () {
00053     float f = 0;
00054     Timer t;
00055 
00056     t.start();
00057     while (!_received) {
00058         if (t.read_ms() > 500) return 0;
00059     }
00060     printf("  %dms\r\n", t.read_ms());
00061 
00062     switch (_pid) {
00063     case ENGINE_LOAD:
00064     case THROTTLE:
00065     case FUEL_LEVEL:
00066         f = _data[0] / 255.0 * 100; // %
00067         break;
00068     case ENGINE_COOLANT_TEMP:
00069     case INTAKE_TEMP:
00070     case AMB_AIR_TEMP:
00071         f = _data[0] - 40; // Deg C
00072         break;
00073     case INTAKE_PRESSURE:
00074         f = _data[0]; // kPa
00075         break;
00076     case ENGINE_RPM:
00077         f = ((_data[0] << 8) | _data[1]) / 4.0; // rpm
00078         break;
00079     case VEHICLE_SPEED:
00080         f = _data[0]; // km/h
00081         break;
00082     }
00083     return f;
00084 }