OBDII Can Bus

Notice: This code has not been fully tested. It comes AS IS with no guarantees that it will work. This is an adaptation of SK Pang's ECU_Reader project which is used to read OBD-II codes. We've expanded the libraries to include more OBD-II PIDs, which can be found here: http://en.wikipedia.org/wiki/OBD-II_PIDs. We were only able WRITE the standard OBD-II PID_REQUEST (0x7DF) to the CAN bus on a 2010 Toyota Corolla and READ back this data. This code does not output engine diagnostics on the test bed we were working with (2010 Toyota Corolla), as it was not returning a valid PID_REPLY (0x7E8), but others might find success with different cars. Most cars after 2008 have CAN buses, with some cars after 2003 supporting them as well (source: http://www.auterraweb.com/aboutcan.html).

Cars that failed writing of PID_REQUEST:

  • 2002 Toyota Camry
  • 2006 Nissan Sentra
  • 2006 VW Jetta

Cars that passed writing of PID_REQUEST:

  • 2010 Toyota Corolla

Assembled OBDII CAN Bus Scanner:

/media/uploads/rfernandes3/_scaled_circuit_setup.jpg

There are several breakout board options for this library:

These breakout boards utilize a DB9 to OBDII cable available from SKPang:

This design uses the following connections:

Connections for CAN Breakout

This is the workhorse of the project, placing the mbed's signals onto the OBD-II connector.

NameMBEDCAN Breakout
5VVU (p39)+5V
GNDGND (p01)GND
CAN_TXcanrd (p30)CAN_TX
CAN_RXcantd (p29)CAN_RX

Connection for TextLCD (2x16)

Displays output from the mbed module

NameMBED2x16 TextLCD
GNDGND (p01)Pin1 (GND)
5VVU (p39)Pin2 (V+)
VON/CPin3 (to GND w/ 1K Ohm)
RSp18Pin4
RWp19Pin5
Ep20Pin6
D0N/CPin7
D1N/CPin8
D2N/CPin9
D3N/CPin10
D4p17Pin11
D5p16Pin12
D6p15Pin13
D7p14Pin14

Hookup for uSD Breakout board

This can be used for datalogging while driving.

NameMBEDuSD Breakout
CDN/CCD
DOp6DO
GNDGND (p01)GND
SCKp7SCK
VCCVout (p40)VCC
DIp5DI
CSp8CS

main.cpp

main.cpp

/*
mbed Can-Bus demo
This program is to demonstrate the CAN-bus capability of the mbed module.
http://www.skpang.co.uk/catalog/product_info.php?products_id=741
v1.0 July 2010
********************************************************************************
WARNING: Use at your own risk, sadly this software comes with no guarantees.
This software is provided 'free' and in good faith, but the author does not
accept liability for any damage arising from its use.
********************************************************************************
*/

#include "mbed.h"
#include "ecu_reader.h"
#include "globals.h"

/*
Create ecu_reader objects
for different CAN bus speeds
*/
ecu_reader obdii1(CANSPEED_125);
ecu_reader obdii2(CANSPEED_250);
ecu_reader obdii3(CANSPEED_500);

int main() {
    char buffer[20];
    pc.printf("\n\rCAN-bus demo CANSPEED_500...\n\r");

    while (1) {
        if (obdii3.request(ENGINE_RPM, buffer) == 1)  // Get engine rpm and display on USB port
            pc.printf("%s\n\r", buffer);
        else
            pc.printf("Engine Request failed\n\r");
        wait(1);
    }
    /*
    while(1)
    {   // Main CAN loop
        led2 = 1;
        wait(.5);
        led2 = 0;
        wait(.5);

        if(obdii3.request(ENGINE_RPM, buffer) == 1)   // Get engine rpm and display on USB port
            pc.printf("%s\n\r", buffer);
        else
            pc.printf("Engine Request failed\n\r");
        if(obdii3.request(ENGINE_COOLANT_TEMP, buffer) == 1)
            pc.printf("%s\n\r", buffer);
        else
            pc.printf("Engine Coolant Temp failed\n\r");

        if(obdii3.request(VEHICLE_SPEED, buffer) == 1)
            pc.printf("%s\n\r", buffer);
        else
            pc.printf("Vehicle Speed failed\n\r");

        if(obdii3.request(THROTTLE,buffer) == 1)
            pc.printf("%s\n\r", buffer);
        else
            pc.printf("Throttle failed\n\r");

        if(obdii3.request(MAF_SENSOR,buffer) == 1)
            pc.printf("%s\n\r", buffer);
        else
            pc.printf("Maf sensor failed\n\r");

        if(obdii3.request(O2_VOLTAGE,buffer) == 1)
            pc.printf("%s\n\r", buffer);
        else
            pc.printf("O2 Voltage failed\n\r");
    }*/
}

globals.h

The globals section defines the CAN Message class, creates the PID020, PID2140 and PID4160 Integers. It is suggested to query the ECU with PID_0_20, PID_21_40, and PID_41_60 first. These queries will populate the PID020... PID4160 integers with bit values outlining the ECU's support for various PID's. For example, upon querying the ECU with PID_0_20 (Hex code 0x00), the PID020 integer will be populated, with the MSB indicating support for PID 0x01 (STATUS_DTC), while the LSB indicates support for PID 0x20 (PID_21_40)

globals.h

#ifndef GLOBALS_H
#define GLOBALS_H

#include "mbed.h"

extern Serial pc;
extern DigitalOut led1;
extern DigitalOut led2;
extern DigitalOut led3;
extern DigitalOut led4;

// We use can on mbed pins 29(CAN_TXD) and 30(CAN_RXD).
extern CAN can2;
extern CANMessage can_MsgRx;
extern int PID020;
extern int PID2140;
extern int PID4160; //PID Support Masks
#endif

globals.cpp

globals.cpp

#include "globals.h"

Serial pc (USBTX,USBRX);

DigitalOut led1 (LED1);
DigitalOut led2 (LED2);
DigitalOut led3 (LED3);
DigitalOut led4 (LED4);

// We use can on mbed pins 29(CAN_TXD) and 30(CAN_RXD).
CAN can2(p30, p29);
CANMessage can_MsgRx;
int PID020, PID2140, PID4160; //PID Support Masks

ecu_reader.h

This header defines the PID constants and a short description of each constant.

ecu_reader.h

#ifndef ECU_READER_H
#define ECU_READER_H

#define CANSPEED_125      125000        // CAN speed at 125 kbps
#define CANSPEED_250      250000        // CAN speed at 250 kbps
#define CANSPEED_500      500000        // CAN speed at 500 kbps

/* Details from http://en.wikipedia.org/wiki/OBD-II_PIDs */
#define PID_0_20            0x00    //PID 0 - 20 supported
#define PID_0_20_DESC               "PID 0x00 - 0x20 Supported"
#define STATUS_DTC          0x01    ///
#define STATUS_DTC_DESC             "Status since DTC Cleared"
#define FREEZE_DTC          0x02    ///
#define FREEZE_DTC_DESC             "Freeze Diagnostic Trouble Code"
#define FUEL_SYS_STATUS     0x03    ///
#define FUEL_SYS_STATUS_DESC        "Fuel System Status"
#define ENGINE_LOAD         0x04    //
#define ENGINE_LOAD_DESC            "Calculated Engine Load"
#define ENGINE_COOLANT_TEMP 0x05
#define ENGINE_COOLANT_TEMP_DESC    "Engine Coolant Temperature"
#define ST_FUEL_TRIM_1      0x06    ///
#define ST_FUEL_TRIM_1_DESC         "Short Term Fuel % Trim - Bank 1"
#define LT_FUEL_TRIM_1      0x07    ///
#define LT_FUEL_TRIM_1_DESC         "Long Term Fuel % Trim - Bank 1"
#define ST_FUEL_TRIM_2      0x08    ///
#define ST_FUEL_TRIM_2_DESC         "Short Term Fuel % Trim - Bank 2"
#define LT_FUEL_TRIM_2      0x09    ///
#define LT_FUEL_TRIM_2_DESC         "Long Term Fuel % Trim - Bank 2"
#define FUEL_PRESSURE       0x0A    //
#define FUEL_PRESSURE_DESC          "Fuel Pressure"
#define INTAKE_PRESSURE     0x0B    //
#define INTAKE_PRESSURE_DESC        "Intake Manifold Absolute Pressure"
#define ENGINE_RPM          0x0C
#define ENGINE_RPM_DESC             "Engine RPM"
#define VEHICLE_SPEED       0x0D
#define VEHICLE_SPEED_DESC          "Vehicle Speed"
#define TIMING_ADVANCE      0x0E    //
#define TIMING_ADVANCE_DESC         "Timing Advance"
#define INTAKE_TEMP         0x0F    //
#define INTAKE_TEMP_DESC            "Intake Air Temperature"
#define MAF_SENSOR          0x10
#define MAF_SENSOR_DESC             "MAF Sensor Air Flow Rate"
#define THROTTLE            0x11
#define THROTTLE_DESC               "Throttle Position"
#define COMMANDED_SEC_AIR   0x12    ///
#define COMMANDED_SEC_AIR_DESC      "Commanded Secondary Air Status"
#define O2_SENS_PRES        0x13    ///
#define O2_SENS_PRES_DESC           "Detected O2 Sensors"
#define O2_B1S1_VOLTAGE     0x14    ///
#define O2_B1S1_VOLTAGE_DESC        "O2 Sensor Voltage - Bank 1 Sensor 1"
#define O2_B1S2_VOLTAGE     0x15    ///
#define O2_B1S2_VOLTAGE_DESC        "O2 Sensor Voltage - Bank 1 Sensor 2"
#define O2_B1S3_VOLTAGE     0x16    ///
#define O2_B1S3_VOLTAGE_DESC        "O2 Sensor Voltage - Bank 1 Sensor 3"
#define O2_B1S4_VOLTAGE     0x17    ///
#define O2_B1S4_VOLTAGE_DESC        "O2 Sensor Voltage - Bank 1 Sensor 4"
#define O2_B2S1_VOLTAGE     0x18    ///
#define O2_B2S1_VOLTAGE_DESC        "O2 Sensor Voltage - Bank 2 Sensor 1"
#define O2_B2S2_VOLTAGE     0x19    ///
#define O2_B2S2_VOLTAGE_DESC        "O2 Sensor Voltage - Bank 2 Sensor 2"
#define O2_B2S3_VOLTAGE     0x1A    ///
#define O2_B2S3_VOLTAGE_DESC        "O2 Sensor Voltage - Bank 2 Sensor 3"
#define O2_B2S4_VOLTAGE     0x1B    ///
#define O2_B2S4_VOLTAGE_DESC        "O2 Sensor Voltage - Bank 2 Sensor 4"
#define OBDII_STANDARDS     0x1C    //List of OBDII Standars the car conforms to
#define OBDII_STANDARDS_DESC        "Supported OBDII Standards"
#define O2_SENS_PRES_ALT    0x1D    ///
#define O2_SENS_PRES_ALT_DESC       "Detected O2 Sensors - Alternate Grouping"
#define AUX_IN_STATUS       0x1E    ///
#define AUX_IN_STATUS_DESC          "Auxiliary Input Status"
#define ENGINE_RUNTIME      0x1F    //
#define ENGINE_RUNTIME_DESC         "Run Time Since Engine Started"
#define PID_21_40           0x20    //PID 21-40 supported
#define PID_21_40_DESC              "PID 0x21 - 0x40 Supported"
#define DIST_TRAVELED_MIL   0x21    ///
#define DIST_TRAVELED_MIL_DESC      "Distance Traveled with MIL On"
#define FUEL_RAIL_PRESSURE  0x22    //
#define FUEL_RAIL_PRESSURE_DESC     "Fuel Rail Pressure Relative to Manifold"
#define FUEL_RAIL_PRES_ALT  0x23    ///
#define FUEL_RAIL_PRES_ALT_DESC     "MPI/Diesel Fuel Rail Pressure"
#define O2S1_WR_LAMBDA_V    0x24    ///
#define O2S1_WR_LAMBDA_V_DESC       "O2 Sensor 1 Equivalence Ratio Voltage"
#define O2S2_WR_LAMBDA_V    0x25    ///
#define O2S2_WR_LAMBDA_V_DESC       "O2 Sensor 2 Equivalence Ratio Voltage"
#define O2S3_WR_LAMBDA_V    0x26    ///
#define O2S3_WR_LAMBDA_V_DESC       "O2 Sensor 3 Equivalence Ratio Voltage"
#define O2S4_WR_LAMBDA_V    0x27    ///
#define O2S4_WR_LAMBDA_V_DESC       "O2 Sensor 4 Equivalence Ratio Voltage"
#define O2S5_WR_LAMBDA_V    0x28    ///
#define O2S5_WR_LAMBDA_V_DESC       "O2 Sensor 5 Equivalence Ratio Voltage"
#define O2S6_WR_LAMBDA_V    0x29    ///
#define O2S6_WR_LAMBDA_V_DESC       "O2 Sensor 6 Equivalence Ratio Voltage"
#define O2S7_WR_LAMBDA_V    0x2A    ///
#define O2S7_WR_LAMBDA_V_DESC       "O2 Sensor 7 Equivalence Ratio Voltage"
#define O2S8_WR_LAMBDA_V    0x2B    ///
#define O2S8_WR_LAMBDA_V_DESC       "O2 Sensor 8 Equivalence Ratio Voltage"
#define COMMANDED_EGR       0x2C    //
#define COMMANDED_EGR_DESC          "Commanded EGR"
#define EGR_ERROR           0x2D    //
#define EGR_ERROR_DESC              "EGR Error"
#define COMMANDED_EVAP_P    0x2E    ///
#define COMMANDED_EVAP_P_DESC       "Commanded Evaporative Purge"
#define FUEL_LEVEL          0x2F    //
#define FUEL_LEVEL_DESC             "Fuel Level Input"
#define WARMUPS_SINCE_CLR   0x30    ///
#define WARMUPS_SINCE_CLR_DESC      "Number of Warmups since DTC Cleared"
#define DIST_SINCE_CLR      0x31    ///
#define DIST_SINCE_CLR_DESC         "Distance Traveled Since DTC Cleared"
#define EVAP_PRESSURE       0x32    //
#define EVAP_PRESSURE_DESC          "Evap. System Vapor Pressure"
#define BAROMETRIC_PRESSURE 0x33    //
#define BAROMETRIC_PRESSURE_DESC    "Barometric Pressure"
#define O2S1_WR_LAMBDA_I    0x34    ///
#define O2S1_WR_LAMBDA_I_DESC       "O2 Sensor 1 Equivalence Ratio Current"
#define O2S2_WR_LAMBDA_I    0x35    ///
#define O2S2_WR_LAMBDA_I_DESC       "O2 Sensor 2 Equivalence Ratio Current"
#define O2S3_WR_LAMBDA_I    0x36    ///
#define O2S3_WR_LAMBDA_I_DESC       "O2 Sensor 3 Equivalence Ratio Current"
#define O2S4_WR_LAMBDA_I    0x37    ///
#define O2S4_WR_LAMBDA_I_DESC       "O2 Sensor 4 Equivalence Ratio Current"
#define O2S5_WR_LAMBDA_I    0x38    ///
#define O2S5_WR_LAMBDA_I_DESC       "O2 Sensor 5 Equivalence Ratio Current"
#define O2S6_WR_LAMBDA_I    0x39    ///
#define O2S6_WR_LAMBDA_I_DESC       "O2 Sensor 6 Equivalence Ratio Current"
#define O2S7_WR_LAMBDA_I    0x3A    ///
#define O2S7_WR_LAMBDA_I_DESC       "O2 Sensor 7 Equivalence Ratio Current"
#define O2S8_WR_LAMBDA_I    0x3B    ///
#define O2S8_WR_LAMBDA_I_DESC       "O2 Sensor 8 Equivalence Ratio Current"
#define CAT_TEMP_B1S1       0x3C    ///
#define CAT_TEMP_B1S1_DESC          "Catalyst Temperature Bank 1 Sensor 1"
#define CAT_TEMP_B1S2       0x3E    ///
#define CAT_TEMP_B1S2_DESC          "Catalyst Temperature Bank 1 Sensor 2"
#define CAT_TEMP_B2S1       0x3D    ///
#define CAT_TEMP_B2S1_DESC          "Catalyst Temperature Bank 2 Sensor 1"
#define CAT_TEMP_B2S2       0x3F    ///
#define CAT_TEMP_B2S2_DESC          "Catalyst Temperature Bank 2 Sensor 2"
#define PID_41_60           0x40    //PID 41-60 supported
#define PID_41_60_DESC              "PID 0x41 - 0x60 Supported"
#define MONITOR_STATUS      0x41    ///
#define MONITOR_STATUS_DESC         "Monitor Status This Drive Cycle"
#define ECU_VOLTAGE         0x42    //
#define ECU_VOLTAGE_DESC            "Control Module Voltage"
#define ABSOLUTE_LOAD       0x43    //
#define ABSOLUTE_LOAD_DESC          "Absolute Load Value"
#define COMMANDED_EQUIV_R   0x44    ///
#define COMMANDED_EQUIV_R_DESC      "Commanded Equivalence Ratio"
#define REL_THROTTLE_POS    0x45    ///
#define REL_THROTTLE_POS_DESC       "Relative Throttle Position"
#define AMB_AIR_TEMP        0x46    ///
#define AMB_AIR_TEMP_DESC           "Ambient Air Temperature"
#define ABS_THROTTLE_POS_B  0x47    ///
#define ABS_THROTTLE_POS_B_DESC     "Absolute Throttle Position B"
#define ABS_THROTTLE_POS_C  0x48    ///
#define ABS_THROTTLE_POS_C_DESC     "Absolute Throttle Position C"
#define ACCEL_POS_D         0x49    ///
#define ACCEL_POS_D_DESC            "Accelerator Pedal Position D"
#define ACCEL_POS_E         0x4A    ///
#define ACCEL_POS_E_DESC            "Accelerator Pedal Position E"
#define ACCEL_POS_F         0x4B    ///
#define ACCEL_POS_F_DESC            "Accelerator Pedal Position F"
#define COMMANDED_THROTTLE  0x4C    ///
#define COMMANDED_THROTTLE_DESC     "Commanded Throttle Actuator"
#define TIME_RUN_WITH_MIL   0x4D    ///
#define TIME_RUN_WITH_MIL_DESC      "Time Run with MIL on"
#define TIME_SINCE_CLR      0x4E    ///
#define TIME_SINCE_CLR_DESC         "Time Since DTC Cleared"
#define MAX_R_O2_VI_PRES    0x4F    ///
#define MAX_R_O2_VI_PRES_DESC       "Maximum Value - Equivalence ratio, O2 Voltage, O2 Current, Intake Manifold Pressure"
#define MAX_AIRFLOW_MAF     0x50    ///
#define MAX_AIRFLOW_MAF_DESC        "Maximum MAF Airflow Value"
#define FUEL_TYPE           0x51    //
#define FUEL_TYPE_DESC              "Fuel Type"
#define ETHANOL_PERCENT     0x52    //
#define ETHANOL_PERCENT_DESC        "Ethanol fuel %"
#define ABS_EVAP_SYS_PRES   0x53    ///
#define ABS_EVAP_SYS_PRES_DESC      "absolute Evap. System Vapor Pressure"
#define EVAP_SYS_PRES       0x54    ///
#define EVAP_SYS_PRES_DESC          "Evap. System Vapor Pressure"
#define ST_O2_TRIM_B1B3     0x55    ///
#define ST_O2_TRIM_B1B3_DESC        "Short Term Secondary O2 Sensor Trim - Bank 1 and 3"
#define LT_O2_TRIM_B1B3     0x56    ///
#define LT_O2_TRIM_B1B3_DESC        "Long Term Secondary O2 Sensor Trim - Bank 1 and 3"
#define ST_02_TRIM_B2B4     0x57    ///
#define ST_O2_TRIM_B2B4_DESC        "Short Term Secondary O2 Sensor Trim - Bank 2 and 4"
#define LT_O2_TRIM_B2B4     0x58    ///
#define LT_O2_TRIM_B2B4_DESC        "Long Term Secondary O2 Sensor Trim - Bank 2 and 4"
#define ABS_FUEL_RAIL_PRES  0x59    ///
#define ABS_FUEL_RAIL_PRES_DESC     "Absolute Fuel Rail Pressure"
#define REL_ACCEL_POS       0x5A    ///
#define REL_ACCEL_POS_DESC          "Relative Accelerator Pedal Position"
#define HYBRID_BATT_PCT     0x5B    ///
#define HYBRID_BATT_PCT_DESC        "Hybrid Battery Pack Charge Percent"
#define ENGINE_OIL_TEMP     0x5C    ///
#define ENGINE_OIL_TEMP_DESC        "Engine Oil Temperature"
#define FUEL_TIMING         0x5D    //
#define FUEL_TIMING_DESC            "Fuel Injection Timing"
#define FUEL_RATE           0x5E    //
#define FUEL_RATE_DESC              "Engine Fuel Rate"
#define EMISSIONS_STANDARD  0x5F    ///
#define EMISSIONS_STANDARD_DESC     "Emmissions Requirements"
#define DEMANDED_TORQUE     0x61    ///
#define DEMANDED_TORQUE_DESC        "Driver's Demanded Torque - Percent"
#define ACTUAL_TORQUE       0x62    ///
#define ACTUAL_TORQUE_DESC          "Actual Engine Torque - Percent"
#define REFERENCE_TORQUE    0x63    //
#define REFERENCE_TORQUE_DESC       "Engine Reference Torque"
#define ENGINE_PCT_TORQUE   0x64    ///
#define ENGINE_PCT_TORQUE_DESC      "Engine Percent Torque"
#define AUX_IO_SUPPORTED    0x65    ///
#define AUX_IO_SUPPORTED_DESC       "Auxiliary Input/Output Supported"
#define P_MAF_SENSOR        0x66    ///
#define P_ENGINE_COOLANT_T  0x67    ///
#define P_INTAKE_TEMP       0x68    ///
#define P_COMMANDED_EGR     0x69    ///
#define P_COMMANDED_INTAKE  0x6A    ///
#define P_EGR_TEMP          0x6B    ///
#define P_COMMANDED_THROT   0x6C    ///
#define P_FUEL_PRESSURE     0x6D    ///
#define P_FUEL_INJ_PRES     0x6E    ///
#define P_TURBO_PRESSURE    0x6F    ///
#define P_BOOST_PRES_CONT   0x70    ///
#define P_VGT_CONTROL       0x71    ///
#define P_WASTEGATE_CONT    0x72    ///
#define P_EXHAUST_PRESSURE  0x73    ///
#define P_TURBO_RPM         0x74    ///
#define P_TURBO_TEMP1       0x75    ///
#define P_TURBO_TEMP2       0x76    ///
#define P_CACT              0x77    ///
#define P_EGT_B1            0x78    ///
#define P_EGT_B2            0x79    ///
#define P_DPF1              0x7A    ///
#define P_DPF2              0x7B    ///
#define P_DPF_TEMP          0x7C    ///
#define P_NOX_NTE_STATUS    0x7D    ///
#define P_PM_NTE_STATUS     0x7E    ///
#define P_ENGINE_RUNTUME    0x7F    ///
#define P_ENGINE_AECD_1     0x81    ///
#define P_ENGINE_AECD_2     0x82    ///
#define P_NOX_SENSOR        0x83    ///
#define P_MANIFOLD_TEMP     0x84    ///
#define P_NOX_SYSTEM        0x85    ///
#define P_PM_SENSOR         0x86    ///
#define P_IN_MANIF_TEMP     0x87    ///

#define PID_REQUEST         0x7DF
#define PID_REPLY           0x7E8

namespace mbed {
class ecu_reader {
public:
    ecu_reader(int can_speed);
    unsigned char request(unsigned char pid,  char *buffer, char *buffer2 = NULL, char *buffer3 = NULL, char *buffer4 = NULL);
private:
    int i;
};
}
#endif

ecu_reader.cpp

This part of the library is where most of the magic happens. The upper part of the program sends the the CAN Query message to the Engine Control Unit. The query consists of 3 bytes. The first byte is the query length of 2 bytes, the second byte is the mode. For PID requests this is set to 0x01. To get Diagnostic Trouble Codes, mode 0x03 would be desired. The last byte of the query is the PID. These are defined in ecu_reader.h above.

ecu_reader.cpp

#include "mbed.h"
#include "ecu_reader.h"
#include "globals.h"
#include "TextLCD.h"

TextLCD lcd(p18, p19, p20, p17, p16, p15, p14); // rs, rw, e, d0, d1, d2, d3

// Use a timer to see if things take too long
Timer CANTimer;
namespace mbed {
ecu_reader::ecu_reader(int can_speed) {
    can2.frequency(can_speed);
}

#define TIMEOUT 200
unsigned char ecu_reader::request(unsigned char pid,  char *buffer, char *buffer2, char *buffer3, char *buffer4) {
    led1 = 1;
    char can_msg[8];
    float engine_data;
    can_msg[0] = 0x02;
    can_msg[1] = 0x01;
    can_msg[2] = pid;
    can_msg[3] = 0;
    can_msg[4] = 0;
    can_msg[5] = 0;
    can_msg[6] = 0;
    can_msg[7] = 0;

    if (can2.write(CANMessage(PID_REQUEST, can_msg, 8))) {
        pc.printf("*********Request write passed*********\n\r");
    } else {
        pc.printf("*********Request write failed*********\n\r");
    }

    led1 = 0;
    CANTimer.reset();
    CANTimer.start();

    while (CANTimer.read_ms() < TIMEOUT) {
        pc.printf("CANTimer.read_ms(): %dms ", CANTimer.read_ms());

        if (can2.read(can_MsgRx)) {
            lcd.printf("Message read!\n");
            pc.printf("Message read\n\r");
            
            //print message id
            pc.printf("can_MsgRx.id: %x\n\r\n\r", can_MsgRx.id);

            //print length of message
            pc.printf("Hex: can_MsgRx.len: %x\n\r", can_MsgRx.len);

            //print data[2] and PID
            pc.printf("can_MsgRx.data[2]: %x, pid: %x\n\r", can_MsgRx.data[2], pid);

            for (int i = 0; i < (int)can_MsgRx.len; i++) {
                pc.printf("can_MsgRx.data[%d]: %x\n\r", i, can_MsgRx.data[i]);
            }

            if ((can_MsgRx.id == PID_REPLY) && (can_MsgRx.data[2] == pid)) {
                pc.printf("Valid OBD-II PID reply\n\r");
                /* Details from http://en.wikipedia.org/wiki/OBD-II_PIDs */
                switch (can_MsgRx.data[2]) {                /* Details from http://en.wikipedia.org/wiki/OBD-II_PIDs */
                    case PID_0_20:                          // PID 0-20 Supported
                        PID020 = ((can_MsgRx.data[3] << 24) | (can_MsgRx.data[4] << 16) | (can_MsgRx.data[5] << 8) | (can_MsgRx.data[6]));
                        break;
                    case STATUS_DTC: {                      // bit encoded
                        if (can_MsgRx.data[4] & 0x04) {     //Compression Ignition (Diesel)
                            if (can_MsgRx.data[3] & 0x80) { //MIL Light on
                                engine_data = (can_MsgRx.data[3] - 128);
                                sprintf(buffer,"MIL ON, %d DTCs", (int) engine_data);
                            } else {                        //MIL Light off
                                engine_data = (can_MsgRx.data[3]);
                                sprintf(buffer,"MIL OFF, %d DTCs", (int) engine_data);
                            }
                            // Diesel C and D bytes (can_MsgRx.data[5] and can_MsgRx.data[6])
                            //                      Test available  Test incomplete
                            // Catalyst             C0              D0
                            // Heated Catalyst      C1              D1
                            // Evap System          C2              D2
                            // Secondary Air        C3              D3
                            // A/C Refrigerant      C4              D4
                            // O2 Sensor            C5              D5
                            // O2 Sensor Heater     C6              D6
                            // EGR System           C7              D7
                        } else {                            //Spark Ignition (Gasoline)
                            if (can_MsgRx.data[3] & 0x80) { //MIL Light on
                                engine_data = (can_MsgRx.data[3] - 128);
                                sprintf(buffer,"MIL ON, %d DTCs", (int) engine_data);
                            } else {                        //MIL Light off
                                engine_data = (can_MsgRx.data[3]);
                                sprintf(buffer,"MIL OFF, %d DTCs", (int) engine_data);
                            }
                            // Gasoline C and D bytes (can_MsgRx.data[5] and can_MsgRx.data[6])
                            //                      Test available  Test incomplete
                            // NMHC Catalyst        C0              D0
                            // NOx/SCR Monitoring   C1              D1
                            // Boost Pressure       C3              D3
                            // Exhaust Gas Sensor   C5              D5
                            // Particulate Filter   C6              D6
                            // EGR and/or VVT/VTEC  C7              D7
                        }
                        // Common Tests between Gas and Diesel Engines, byte B (can_MsgRx.data[4])
                        //                  Test available  Test incomplete
                        // Misfire          B0              B4
                        // Fuel System      B1              B5
                        // Components       B2              B6
                        break;
                    }
                    case FREEZE_DTC:                        // Locks in Diagnostic trouble Codes
                        break;
                    case FUEL_SYS_STATUS:                   // bit encoded
                        //This tells us the warmup status of the engine. Only 1 bit should be set
                        engine_data = can_MsgRx.data[3];
                        if (((int) engine_data) & 0x01) {   // Open loop - Engine warmup
                            sprintf(buffer,"Open Loop - Warmup");
                        }
                        if (((int) engine_data) & 0x02) {   // Closed Loop - O2 Sensor feedback
                            sprintf(buffer,"Closed Loop - Normal");
                        }
                        if (((int) engine_data) & 0x04) {   // Open loop,
                            sprintf(buffer,"Open Loop-Load/Decel");
                        }
                        if (((int) engine_data) & 0x08) {   // Open loop - system failure
                            sprintf(buffer,"Open Loop - FAILURE");
                        }
                        if (((int) engine_data) & 0x10) {   // Closed Loop - O2 Sensor feedback failure
                            sprintf(buffer,"Closed Loop - O2Fail");
                        }
                        if ((((int) engine_data) & 0x20) | (((int) engine_data) & 0x40) | (((int) engine_data) & 0x80)) { //These shouldnt be on, assume Proprietary status
                            sprintf(buffer,"Unsupported Status");
                        }
                        break;
                    case ENGINE_LOAD:                       // A*100/255
                        engine_data = (can_MsgRx.data[3]*100)/255;
                        sprintf(buffer,"%d %% ",(int) engine_data);
                        break;
                    case ENGINE_COOLANT_TEMP:               //     A-40              [degree C]
                        engine_data = can_MsgRx.data[3] - 40;
                        sprintf(buffer,"%d degC ",(int) engine_data);
                        break;
                    case ST_FUEL_TRIM_1:                    // (A-128)*100/128
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %% ", (int) engine_data);
                        break;
                    case LT_FUEL_TRIM_1:                    // (A-128)*100/128
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %% ", (int) engine_data);
                        break;
                    case ST_FUEL_TRIM_2:                    // (A-128)*100/128
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %% ", (int) engine_data);
                        break;
                    case LT_FUEL_TRIM_2:                    // (A-128)*100/128
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %% ", (int) engine_data);
                        break;
                    case FUEL_PRESSURE:                     // A*3
                        engine_data = (can_MsgRx.data[3]*3);
                        sprintf(buffer,"%d kPa",(int) engine_data);
                        break;
                    case INTAKE_PRESSURE:                   // A
                        engine_data = can_MsgRx.data[3];
                        sprintf(buffer,"%d kPa",(int) engine_data);
                        break;
                    case ENGINE_RPM:                        //   ((A*256)+B)/4    [RPM]
                        engine_data = ((can_MsgRx.data[3]*256) + can_MsgRx.data[4])/4;
                        sprintf(buffer,"%d rpm ",(int) engine_data);
                        break;
                    case VEHICLE_SPEED:                     // A                  [km]
                        engine_data = can_MsgRx.data[3];
                        sprintf(buffer,"%d km ",(int) engine_data);
                        break;
                    case TIMING_ADVANCE:                    // A/2 - 64
                        engine_data = (can_MsgRx.data[3]/2) - 64;
                        sprintf(buffer,"%d Deg",(int) engine_data);
                        break;
                    case INTAKE_TEMP:                       // A - 40
                        engine_data = (can_MsgRx.data[3] - 40);
                        sprintf(buffer,"%d DegC",(int) engine_data);
                        break;
                    case MAF_SENSOR:                        // ((256*A)+B) / 100  [g/s]
                        engine_data = ((can_MsgRx.data[3]*256) + can_MsgRx.data[4])/100;
                        sprintf(buffer,"%d g/s",(int) engine_data);
                        break;
                    case THROTTLE:                          // A*100/255
                        engine_data = (can_MsgRx.data[3]*100)/255;
                        sprintf(buffer,"%d %% ",(int) engine_data);
                        break;
                    case COMMANDED_SEC_AIR:                 // bit encoded
                        engine_data = can_MsgRx.data[3];
                        if (((int) engine_data) & 0x01) {           //Upstream of Catalytic Converter
                            sprintf(buffer,"Upstream of Cat.");
                        }
                        if (((int) engine_data) & 0x02) {           //Downstream of Catalytic Converter
                            sprintf(buffer,"Downstream of Cat.");
                        }
                        if (((int) engine_data) & 0x04) {           //From outside atmosphere or off
                            sprintf(buffer,"Off");
                        }
                        break;
                    case O2_SENS_PRES: {                    // A [A0..A3] == Bank 1, [A4..A7] == Bank 2
                        engine_data = (can_MsgRx.data[3]);  //Check # of O2 sensors present by masking individual bits and counting
                        int o2pres = 0;
                        if (((int) engine_data) & 0x01) {           // Bank 1 Sensor 1
                            o2pres++;
                        }
                        if (((int) engine_data) & 0x02) {           // Bank 1 Sensor 2
                            o2pres++;
                        }
                        if (((int) engine_data) & 0x04) {           // Bank 1 Sensor 3
                            o2pres++;
                        }
                        if (((int) engine_data) & 0x08) {           // Bank 1 Sensor 4
                            o2pres++;
                        }
                        if (((int) engine_data) & 0x10) {           // Bank 2 Sensor 1
                            o2pres++;
                        }
                        if (((int) engine_data) & 0x20) {           // Bank 2 Sensor 2
                            o2pres++;
                        }
                        if (((int) engine_data) & 0x40) {           // Bank 2 Sensor 3
                            o2pres++;
                        }
                        if (((int) engine_data) & 0x80) {           // Bank 2 Sensor 4
                            o2pres++;
                        }
                        sprintf(buffer,"%d Present",(int) o2pres);
                        break;
                    }
                    case O2_B1S1_VOLTAGE:                   // A/200, (B-128) * 100/128
                        engine_data = (can_MsgRx.data[3]/200);
                        sprintf(buffer,"%d V ",(int) engine_data);              //Raw O2 Voltage
                        if (can_MsgRx.data[4] & 0xFF) {
                            sprintf(buffer,"Not Present");
                        } else {
                            engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                            sprintf(buffer2,"%d %% ",(int) engine_data);        //Calculated lean/rich
                        }
                        break;
                    case O2_B1S2_VOLTAGE:                   //
                        engine_data = (can_MsgRx.data[3]/200);
                        sprintf(buffer,"%d V ",(int) engine_data);
                        if (can_MsgRx.data[4] & 0xFF) {
                            sprintf(buffer,"Not Present");
                        } else {
                            engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                            sprintf(buffer2,"%d %% ",(int) engine_data);
                        }
                        break;
                    case O2_B1S3_VOLTAGE:                   //
                        engine_data = (can_MsgRx.data[3]/200);
                        sprintf(buffer,"%d V ",(int) engine_data);
                        if (can_MsgRx.data[4] & 0xFF) {
                            sprintf(buffer,"Not Present");
                        } else {
                            engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                            sprintf(buffer2,"%d %% ",(int) engine_data);
                        }
                        break;
                    case O2_B1S4_VOLTAGE:                   //
                        engine_data = (can_MsgRx.data[3]/200);
                        sprintf(buffer,"%d V ",(int) engine_data);
                        if (can_MsgRx.data[4] & 0xFF) {
                            sprintf(buffer,"Not Present");
                        } else {
                            engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                            sprintf(buffer2,"%d %% ",(int) engine_data);
                        }
                        break;
                    case O2_B2S1_VOLTAGE:                   //
                        engine_data = (can_MsgRx.data[3]/200);
                        sprintf(buffer,"%d V ",(int) engine_data);
                        if (can_MsgRx.data[4] & 0xFF) {
                            sprintf(buffer,"Not Present");
                        } else {
                            engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                            sprintf(buffer2,"%d %% ",(int) engine_data);
                        }
                        break;
                    case O2_B2S2_VOLTAGE:                   //
                        engine_data = (can_MsgRx.data[3]/200);
                        sprintf(buffer,"%d V ",(int) engine_data);
                        if (can_MsgRx.data[4] & 0xFF) {
                            sprintf(buffer,"Not Present");
                        } else {
                            engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                            sprintf(buffer2,"%d %% ",(int) engine_data);
                        }
                        break;
                    case O2_B2S3_VOLTAGE: {                 //
                        engine_data = (can_MsgRx.data[3]/200);
                        sprintf(buffer,"%d V ",(int) engine_data);
                        if (can_MsgRx.data[4] & 0xFF) {
                            sprintf(buffer,"Not Present");
                        } else {
                            engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                            sprintf(buffer2,"%d %% ",(int) engine_data);
                        }
                        break;
                    }
                    case O2_B2S4_VOLTAGE: {                 //
                        engine_data = (can_MsgRx.data[3]/200);
                        sprintf(buffer,"%d V ",(int) engine_data);
                        if (can_MsgRx.data[4] & 0xFF) {
                            sprintf(buffer,"Not Present");
                        } else {
                            engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                            sprintf(buffer2,"%d %% ",(int) engine_data);
                        }
                        break;
                    }
                    case OBDII_STANDARDS: {                 //bit encoded NOT DONE
                        engine_data = can_MsgRx.data[3];
                        if (((int) engine_data) & 0x0D) {           //JOBD, EOBD, and OBD II
                            sprintf(buffer,"JOBD,EOBD,OBDII");
                        }
                        if (((int) engine_data) & 0x0C) {           //JOBD and EOBD
                            sprintf(buffer,"JOBD,EOBD");
                        }
                        if (((int) engine_data) & 0x0B) {           //JOBD and OBDII
                            sprintf(buffer,"JOBD,OBDII");
                        }
                        if (((int) engine_data) & 0x0A) {           //JOBD
                            sprintf(buffer,"JOBD");
                        }
                        if (((int) engine_data) & 0x09) {           //EOBD, OBD, and OBD II
                            sprintf(buffer,"EOBD,OBDI,OBDII");
                        }
                        if (((int) engine_data) & 0x08) {           //EOBD and OBD
                            sprintf(buffer,"EOBD,OBDI");
                        }
                        if (((int) engine_data) & 0x07) {           //EOBD and OBDII
                            sprintf(buffer,"EOBD,OBDII");
                        }
                        if (((int) engine_data) & 0x06) {           //EOBD
                            sprintf(buffer,"EOBD");
                        }
                        if (((int) engine_data) & 0x05) {           //Not meant to comply with any OBD standard
                            sprintf(buffer,"No Compliance");
                        }
                        if (((int) engine_data) & 0x04) {           //OBDI
                            sprintf(buffer,"OBDI");
                        }
                        if (((int) engine_data) & 0x03) {           //OBD and OBDII
                            sprintf(buffer,"OBDI,OBDII");
                        }
                        if (((int) engine_data) & 0x02) {           //OBD and defined by the EPA
                            sprintf(buffer,"OBD");
                        }
                        if (((int) engine_data) & 0x01) {           //OBD-II as defined by CARB
                            sprintf(buffer,"OBDII");
                        }
                        sprintf(buffer,"ERROR");
                        break;
                    }
                    case O2_SENS_PRES_ALT: {                //*******************
                        engine_data = (can_MsgRx.data[3]);  //Check # of O2 sensors present by masking individual bits and counting
                        int o2presalt = 0;
                        if (((int) engine_data) & 0x01) {           // Bank 1 Sensor 1
                            o2presalt++;
                        }
                        if (((int) engine_data) & 0x02) {           // Bank 1 Sensor 2
                            o2presalt++;
                        }
                        if (((int) engine_data) & 0x04) {           // Bank 2 Sensor 1
                            o2presalt++;
                        }
                        if (((int) engine_data) & 0x08) {           // Bank 2 Sensor 2
                            o2presalt++;
                        }
                        if (((int) engine_data) & 0x10) {           // Bank 3 Sensor 1
                            o2presalt++;
                        }
                        if (((int) engine_data) & 0x20) {           // Bank 3 Sensor 2
                            o2presalt++;
                        }
                        if (((int) engine_data) & 0x40) {           // Bank 4 Sensor 1
                            o2presalt++;
                        }
                        if (((int) engine_data) & 0x80) {           // Bank 4 Sensor 2
                            o2presalt++;
                        }
                        sprintf(buffer,"%d Present",(int) o2presalt);
                        break;
                    }
                    case AUX_IN_STATUS: {                   // A (A0 == PTO Active)
                        engine_data = can_MsgRx.data[3];
                        if (((int) engine_data) & 0x01) {
                            sprintf(buffer,"PTO Active");
                        } else {
                            sprintf(buffer,"PTO Inactive");
                        }
                        break;
                    }
                    case ENGINE_RUNTIME:                    // (A*256)+B
                        engine_data = (can_MsgRx.data[3]*256)+(can_MsgRx.data[4]);
                        sprintf(buffer,"%d Sec",(int) engine_data);
                        break;
                    case PID_21_40:                         // bit encoded NOT DONE
                        PID2140 = ((can_MsgRx.data[3] << 24) | (can_MsgRx.data[4] << 16) | (can_MsgRx.data[5] << 8) | (can_MsgRx.data[6]));
                        break;
                    case DIST_TRAVELED_MIL:                 // (A*256) + B
                        engine_data = ((can_MsgRx.data[3] * 256) + can_MsgRx.data[4]);
                        sprintf(buffer,"%d km",(int) engine_data);
                        break;
                    case FUEL_RAIL_PRESSURE:                // ((A*256)+B)*0.079
                        engine_data = ((can_MsgRx.data[3] * 256)+can_MsgRx.data[4])*0.079;
                        sprintf(buffer,"%d kPa",(int) engine_data);
                        break;
                    case FUEL_RAIL_PRES_ALT:                // ((A*256)+B)*0.079
                        engine_data = ((can_MsgRx.data[3] * 256) + can_MsgRx.data[4])*10;
                        sprintf(buffer,"%d kPa",(int) engine_data);
                        break;
                    case O2S1_WR_LAMBDA_V:                  // ((A*256)+B)*2/65535 [ratio], ((C*256)+D)*8/65535 [V]
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*2)/65535);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])*8)/65535);
                        sprintf(buffer2,"%d V",(int) engine_data);
                        break;
                    case O2S2_WR_LAMBDA_V:                  //
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*2)/65535);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])*8)/65535);
                        sprintf(buffer2,"%d V",(int) engine_data);
                        break;
                    case O2S3_WR_LAMBDA_V:                  //
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*2)/65535);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])*8)/65535);
                        sprintf(buffer2,"%d V",(int) engine_data);
                        break;
                    case O2S4_WR_LAMBDA_V:                  //
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*2)/65535);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])*8)/65535);
                        sprintf(buffer2,"%d V",(int) engine_data);
                        break;
                    case O2S5_WR_LAMBDA_V:                  //
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*2)/65535);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])*8)/65535);
                        sprintf(buffer2,"%d V",(int) engine_data);
                        break;
                    case O2S6_WR_LAMBDA_V:                  //
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*2)/65535);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])*8)/65535);
                        sprintf(buffer2,"%d V",(int) engine_data);
                        break;
                    case O2S7_WR_LAMBDA_V:                  //
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*2)/65535);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])*8)/65535);
                        sprintf(buffer2,"%d V",(int) engine_data);
                        break;
                    case O2S8_WR_LAMBDA_V:                  //
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*2)/65535);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])*8)/65535);
                        sprintf(buffer2,"%d V",(int) engine_data);
                        break;
                    case COMMANDED_EGR:                     // 100*A/255
                        engine_data = (can_MsgRx.data[3]*100/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case EGR_ERROR:                         // (A-128)*100/128
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case COMMANDED_EVAP_P:                  // 100*A/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case FUEL_LEVEL:                        //100*A/255
                        engine_data = ((100*can_MsgRx.data[3])/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case WARMUPS_SINCE_CLR:                 //A
                        engine_data = (can_MsgRx.data[3]);
                        sprintf(buffer,"%d Warmups",(int) engine_data);
                        break;
                    case DIST_SINCE_CLR:                    //A*256+B   [km]
                        engine_data = ((can_MsgRx.data[3]*256)+can_MsgRx.data[4]);
                        sprintf(buffer,"%d km",(int) engine_data);
                        break;
                    case EVAP_PRESSURE:                     //((A*256)+B)/4
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/4);
                        sprintf(buffer,"%d Pa",(int) engine_data); //Yes it's in pascals
                        break;
                    case BAROMETRIC_PRESSURE:               //A
                        engine_data = can_MsgRx.data[3];
                        sprintf(buffer,"%d kPa",(int) engine_data);
                        break;
                    case O2S1_WR_LAMBDA_I:                  //((A*256)+B)/32,768 [Ratio], ((C*256)+D)/256 - 128 [mA]
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])/256)-128);
                        sprintf(buffer2,"%d mA",(int) engine_data);
                        break;
                    case O2S2_WR_LAMBDA_I:
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])/256)-128);
                        sprintf(buffer2,"%d mA",(int) engine_data);
                        break;
                    case O2S3_WR_LAMBDA_I:
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])/256)-128);
                        sprintf(buffer2,"%d mA",(int) engine_data);
                        break;
                    case O2S4_WR_LAMBDA_I:
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])/256)-128);
                        sprintf(buffer2,"%d mA",(int) engine_data);
                        break;
                    case O2S5_WR_LAMBDA_I:
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])/256)-128);
                        sprintf(buffer2,"%d mA",(int) engine_data);
                        break;
                    case O2S6_WR_LAMBDA_I:
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])/256)-128);
                        sprintf(buffer2,"%d mA",(int) engine_data);
                        break;
                    case O2S7_WR_LAMBDA_I:
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])/256)-128);
                        sprintf(buffer2,"%d mA",(int) engine_data);
                        break;
                    case O2S8_WR_LAMBDA_I:
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = ((((can_MsgRx.data[5]*256)+can_MsgRx.data[6])/256)-128);
                        sprintf(buffer2,"%d mA",(int) engine_data);
                        break;
                    case CAT_TEMP_B1S1:                     //((A*256)+B)/10 - 40 [DegC]
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/10)-40);
                        sprintf(buffer,"%d DegC",(int) engine_data);
                        break;
                    case CAT_TEMP_B1S2:
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/10)-40);
                        sprintf(buffer,"%d DegC",(int) engine_data);
                        break;
                    case CAT_TEMP_B2S1:
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/10)-40);
                        sprintf(buffer,"%d DegC",(int) engine_data);
                        break;
                    case CAT_TEMP_B2S2:
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/10)-40);
                        sprintf(buffer,"%d DegC",(int) engine_data);
                        break;
                    case PID_41_60:                         //bit encoded NOT DONE
                        PID4160 = ((can_MsgRx.data[3] << 24) | (can_MsgRx.data[4] << 16) | (can_MsgRx.data[5] << 8) | (can_MsgRx.data[6]));
                        break;
                    case MONITOR_STATUS:                    // bit encoded
                        //LUT: (Uses multiple bytes) A7..0 always 0
                        //                  Test enabled    Test Incomplete
                        // Misfire          B0              B4
                        // Fuel System      B1              B5
                        // Components       B2              B6
                        // Reserved         B3              B7
                        // Catalyst         C0              D0
                        // Heated Catalyst  C1              D1
                        // Evap System      C2              D2
                        // Sec. Ait system  C3              D3
                        // A/C Refrigerant  C4              D4
                        // O2 Sensor        C5              D5
                        // O2 Sensor Heater C6              D6
                        // EGR System       C7              D7
                        break;
                    case ECU_VOLTAGE:                       //((A*256)+B)/1000 [V]
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/1000);
                        sprintf(buffer,"%d V",(int) engine_data);
                        break;
                    case ABSOLUTE_LOAD:                     //((A*256)+B)*100/255 [%]
                        engine_data = ((((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case COMMANDED_EQUIV_R:                 //((A*256)+B)/32768 [Ratio]
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])/32768);
                        sprintf(buffer,"Ratio %d",(int) engine_data);
                        break;
                    case REL_THROTTLE_POS:                  // A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case AMB_AIR_TEMP:                      // A-40 [DegC]
                        engine_data = (can_MsgRx.data[3]-40);
                        sprintf(buffer,"%d DegC",(int) engine_data);
                        break;
                    case ABS_THROTTLE_POS_B:                // A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ABS_THROTTLE_POS_C:                // A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ACCEL_POS_D:                       // A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ACCEL_POS_E:                       // A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ACCEL_POS_F:                       // A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case COMMANDED_THROTTLE:                //A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case TIME_RUN_WITH_MIL:                 //(A*256)+B [minutes]
                        engine_data = ((can_MsgRx.data[3]*256)/(can_MsgRx.data[4]));
                        sprintf(buffer,"%d Mins",(int) engine_data);
                        break;
                    case TIME_SINCE_CLR:                    //(A*256)+B [minutes]
                        engine_data = ((can_MsgRx.data[3]*256)/(can_MsgRx.data[4]));
                        sprintf(buffer,"%d Mins",(int) engine_data);
                        break;
                    case MAX_R_O2_VI_PRES:                  //A,B,C,D*10 [Ratio,V,mA,kPa]
                        engine_data = can_MsgRx.data[3];
                        sprintf(buffer,"Ratio: %d",(int) engine_data);
                        engine_data = can_MsgRx.data[4];
                        sprintf(buffer,"%d V",(int) engine_data);
                        engine_data = can_MsgRx.data[5];
                        sprintf(buffer,"%d mA",(int) engine_data);
                        engine_data = (can_MsgRx.data[6]*10);
                        sprintf(buffer,"%d kPa",(int) engine_data);
                        break;
                    case MAX_AIRFLOW_MAF:                   //A*10 [g/s]
                        engine_data = (can_MsgRx.data[3]*10);
                        sprintf(buffer,"%d g/s",(int) engine_data);
                        break;
                    case FUEL_TYPE:                         // USE LUT NOT DONE
                        break;
                    case ETHANOL_PERCENT:                   //A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ABS_EVAP_SYS_PRES:                 //1/200 per bit [kPa] ----NOT DONE----
                        break;
                    case EVAP_SYS_PRES:                     // (A*256)+B - 32768 [Pa]
                        engine_data = ((can_MsgRx.data[3]*256)+can_MsgRx.data[4]-32768);
                        sprintf(buffer,"%d Pa",(int) engine_data);
                        break;
                    case ST_O2_TRIM_B1B3:                   // ((A-128)*100/128 (B-128)*100/128 [%]
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case LT_O2_TRIM_B1B3:
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ST_02_TRIM_B2B4:
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case LT_O2_TRIM_B2B4:
                        engine_data = ((can_MsgRx.data[3]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        engine_data = ((can_MsgRx.data[4]-128)*(100/128));
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ABS_FUEL_RAIL_PRES:                //((A*256)+B)*10 [kPa]
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*10);
                        sprintf(buffer,"%d kPa",(int) engine_data);
                        break;
                    case REL_ACCEL_POS:                     //A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case HYBRID_BATT_PCT:                   //A*100/255 [%]
                        engine_data = ((can_MsgRx.data[3]*100)/255);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ENGINE_OIL_TEMP:                   //A-40 [DegC]
                        engine_data = (can_MsgRx.data[3]-40);
                        sprintf(buffer,"%d DegC",(int) engine_data);
                        break;
                    case FUEL_TIMING:                       //(38655-((A*256)+B))/128
                        engine_data = ((38655 - ((can_MsgRx.data[3]*256)+can_MsgRx.data[4]))/128);
                        sprintf(buffer,"%d Deg",(int) engine_data);
                        break;
                    case FUEL_RATE:                         //((A*256)+B)*0.05
                        engine_data = (((can_MsgRx.data[3]*256)+can_MsgRx.data[4])*0.05);
                        sprintf(buffer,"%d L/m",(int) engine_data);
                        break;
                    case EMISSIONS_STANDARD:                //bit encoded ----NOT DONE----
                        break;
                    case DEMANDED_TORQUE:                   //A-125 [%]
                        engine_data = (can_MsgRx.data[3]-125);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case ACTUAL_TORQUE:                     //A-125 [%]
                        engine_data = (can_MsgRx.data[3]-125);
                        sprintf(buffer,"%d %%",(int) engine_data);
                        break;
                    case REFERENCE_TORQUE:                  //A*256+b [Nm]
                        engine_data = ((can_MsgRx.data[3]*256)+can_MsgRx.data[4]);
                        sprintf(buffer,"%d Nm",(int) engine_data);
                        break;
                    case ENGINE_PCT_TORQUE:                 //A-125 idle, B-125 pt 1, C-125, D-125
                        engine_data = (can_MsgRx.data[3]);
                        sprintf(buffer,"%d %% - Idle",(int) engine_data);
                        engine_data = (can_MsgRx.data[4]);
                        sprintf(buffer2,"%d %% - Point 1",(int) engine_data);
                        engine_data = (can_MsgRx.data[5]);
                        sprintf(buffer3,"%d %% - Point 2",(int) engine_data);
                        engine_data = (can_MsgRx.data[6]);
                        sprintf(buffer4,"%d %% - Point 3",(int) engine_data);
                        break;
                    case AUX_IO_SUPPORTED:                  //Bit encoded ----NOT DONE----
                        break;
                    case P_MAF_SENSOR:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_ENGINE_COOLANT_T:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_INTAKE_TEMP:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_COMMANDED_EGR:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_COMMANDED_INTAKE:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_EGR_TEMP:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_COMMANDED_THROT:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_FUEL_PRESSURE:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_FUEL_INJ_PRES:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_TURBO_PRESSURE:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_BOOST_PRES_CONT:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_VGT_CONTROL:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_WASTEGATE_CONT:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_EXHAUST_PRESSURE:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_TURBO_RPM:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_TURBO_TEMP1:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_TURBO_TEMP2:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_CACT:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_EGT_B1:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_EGT_B2:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_DPF1:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_DPF2:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_DPF_TEMP:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_NOX_NTE_STATUS:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_PM_NTE_STATUS:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_ENGINE_RUNTUME:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_ENGINE_AECD_1:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_ENGINE_AECD_2:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_NOX_SENSOR:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_MANIFOLD_TEMP:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_NOX_SYSTEM:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_PM_SENSOR:
                        sprintf(buffer,"Not supported");
                        break;
                    case P_IN_MANIF_TEMP:
                        sprintf(buffer,"Not supported");
                        break;
                }
                return 1;
            }
        }
    }
    pc.printf("CANBus Timeout -- ");
    return 0;
}
}

Sample Debug Log Output from USB

Request write passed

CANTimer.read_ms(): 0ms Message read

can_MsgRx.id: 620


Hex: can_MsgRx.len: 8

can_MsgRx.data[2]: 0, pid: c

can_MsgRx.data[0]: 10

can_MsgRx.data[1]: 0

can_MsgRx.data[2]: 0

can_MsgRx.data[3]: 0

can_MsgRx.data[4]: 10

can_MsgRx.data[5]: 40

can_MsgRx.data[6]: 0

can_MsgRx.data[7]: 80

CANBus Timeout -- Engine Request failed

Request write passed

CANTimer.read_ms(): 0ms Message read

can_MsgRx.id: 440


Hex: can_MsgRx.len: 8

can_MsgRx.data[2]: 0, pid: c

can_MsgRx.data[0]: 42

can_MsgRx.data[1]: 2

can_MsgRx.data[2]: 0

can_MsgRx.data[3]: 0

can_MsgRx.data[4]: 0

can_MsgRx.data[5]: 0

can_MsgRx.data[6]: 0

can_MsgRx.data[7]: 0

CANBus Timeout -- Engine Request failed