File content as of revision 1:5605f12d5b16:

#include "mbed.h"
#include "OBDII.h"

OBDII::OBDII(CAN *c, DigitalOut *rs, int can_speed) {
    can = c;
    canRS = rs;
    *canRS = 0;
    
    can->frequency(can_speed);
    data.mafSensor = -1;
    data.speed = -1;

    HostSock = new UDPSock(new Host(IpAddr(), 20000, NULL), 64, this);
    statusHost = new Host(IpAddr(224,1,2,3), 21000, NULL); //Join multicast group on port 50000

    CANTimer.start();
    done = true;
    mode = 0;
}

void OBDII::UpdateData(void) {
    readCAN();

    if (CANTimer.read_ms() > 200) {
        done = true;
    }

    if (done) {
        CANTimer.reset();
        mode++;

        switch (mode) {
            case 0:
                currentRequest_ = VEHICLE_SPEED;
                request(VEHICLE_SPEED);
                break;

            case 1:
                request(MAF_SENSOR);
                break;

            case 2:
                request(FUEL_LEVEL);
                break;

            case 3:
                request(ENGINE_RPM);
                break;

            case 4:
                request(ENGINE_COOLANT_TEMP);
                break;

            case 5:
                request(THROTTLE);
                break;

            case 6:
                request(O2_VOLTAGE);
                mode = 0;
                break;
        }
    }
}

void OBDII::request(unsigned char pid) {
    char can_msg[8];

    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 (can->write(CANMessage(PID_REQUEST, can_msg, 8))) {
    }
    
    done = false;
}

void OBDII::EnhancedRequest(unsigned short pid) {
    char can_msg[8];

    can_msg[0] = 0x03;
    can_msg[1] = 0x22;
    can_msg[2] = (unsigned char)((pid &0xFF00) >> 8);
    can_msg[3] = (unsigned char)((pid &0x00FF));
    can_msg[4] = 0;
    can_msg[5] = 0;
    can_msg[6] = 0;
    can_msg[7] = 0;

    if (can->write(CANMessage(PID_ENHANCED_REQUEST, can_msg, 8))) {
    }
    
    done = false;
}

void OBDII::readCAN()
{
    CANMessage can_MsgRx;

    while (can->read(can_MsgRx))
    {
        if (can_MsgRx.id == PID_REPLY)
        {
            done = true;

            if (can_MsgRx.data[0] == 3)
            {
                switch (can_MsgRx.data[2])
                {
                    case ENGINE_RPM:
                        data.rpm = (int)(((can_MsgRx.data[3]*256) + can_MsgRx.data[4])/4);
                        printf("OBD RPM = %d\n\r", data.rpm);
                        break;
    
                    case ENGINE_COOLANT_TEMP:
                        data.coolantTemp =  (int)(can_MsgRx.data[3] * 0.621371192);
                        printf("OBD Coolant Temp = %d\n\r", data.coolantTemp);
                        break;
    
                    case MAF_SENSOR:
                        data.mafSensor =  (int)(((can_MsgRx.data[3]*256) + can_MsgRx.data[4])/100);
                        printf("OBD MAF Sensor = %d\n\r", data.mafSensor);
                        break;
    
                    case O2_VOLTAGE:
                        data.o2Voltage = (int)(can_MsgRx.data[3]*0.005);
                        printf("OBD O2 Voltage = %d\n\r", data.o2Voltage);
                        break;
    
                    case THROTTLE:            //
                        data.throttle = (int) ((can_MsgRx.data[3]*100)/255);
                        printf("OBD Throttle = %d\n\r", data.throttle);
                        break;
    
                    case VEHICLE_SPEED:
                        data.speed = (int)can_MsgRx.data[3];
                        printf("OBD Speed = %d\n\r", data.speed);
                        break;
    
                    case FUEL_LEVEL:
                        data.fuel = 100 * can_MsgRx.data[3] / 255;
                        printf("OBD Fuel = %d\n\r", data.fuel);
                        break;
                }
            }
            else if (can_MsgRx.data[0] == 4)
            {
                switch (can_MsgRx.data[2])
                {
                    case 0x0040:
                    break;
                }
            }
            
            HostSock->SendTo(statusHost, sizeof(ecu_info), (char *)&data);
        }
    }

    if ((data.mafSensor != -1) && (data.speed != -1)) {
        float mph = 0.621371192 * data.speed;
        data.mpg = mph / (data.mafSensor * 0.0804988);

        printf("Current MPG = %f\n\r", data.mpg);
    }
}

void OBDII::ReceivedData(int socketStatus, int len, char *msg)
{

}