File content as of revision 7:467e350d1a10:

#include "mbed.h"
#include "ecu_simulator.h"
#include "globals.h"


// Use a timer to see if things take too long
Timer CANTimer;

namespace mbed
{


ecu_sim::ecu_sim(int can_speed)
{
    if ( can2.frequency(can_speed) )
        pc.printf("CAN speed set to %d \n\r", can_speed);
    else
        pc.printf("CAN speed SET failed\n\r");
}

void ecu_sim::canspeed(int can_speed)
{
    if ( can2.frequency(can_speed) )
        pc.printf("CAN speed set to %d \n\r", can_speed);
    else
        pc.printf("CAN speed SET failed\n\r");        
}



//#define TIMEOUT 500
unsigned char ecu_sim::request(void)
{
    char can_msg[8];
    int done = 0;
    int cnt = 0;
    //pc.printf("*");


    //if ((can2.read(can_MsgRx)) && (can_MsgRx.id == PID_REQUEST) )
    if (can2.read(can_MsgRx)) //&& (can_MsgRx.id == PID_REQUEST) )
    { 
        pc.printf(".");
        led2 = 1;
        if (can_MsgRx.id == ESPAR_CMD) 
        {
            pc.printf("0");
            // wake command wake[8] = { 0x02, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
            if((can_MsgRx.data[0] == 0x02) && (can_MsgRx.data[1] == 0x3E))// wake request
            { //expected[3] = {0x02, 0x7e, 0x00};
               can_msg[0] = 0x02;
               can_msg[1] = 0x7E;
               can_msg[2] = 0x00; can_msg[3] = 0x00; 
               can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00;
               can2.write(CANMessage(ESPAR_RES , can_msg, 8));
               pc.printf("1"); 
            }
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x17))
            {   // got get runtime: 7A0  03 22 FD 17 00 00 00 00 
                // send 73C  10 1F 62 FD 17 00 00 00 
                can_msg[0] = 0x10; can_msg[1] = 0x1F; can_msg[2] = 0x62; can_msg[3] = 0xFD;
                can_msg[4] = 0x17; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00;
                can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                pc.printf("2");
                //wait(0.1); 
                done = 0;
                while ( done == 0 )
                { 
                    //wait for continue msg 7A0 30 00 14 00 00 00 00 00
                    can2.read(can_MsgRx);
                    if ((can_MsgRx.id == ESPAR_CMD) && (can_MsgRx.data[0] == 0x30))
                    {
                        done = 1;
                        pc.printf("a"); 
                        //73C 2377343693 0008 21 91 00 00 00 00 00 00 
                        can_msg[0] = 0x21; can_msg[1] = 0x91; can_msg[2] = 0x00; can_msg[3] = 0x00;
                        can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00;
                        can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                        wait(0.2);                       
                        //73C 2377373697 0008 22 00 00 00 00 00 00 00 
                        can_msg[0] = 0x22; can_msg[1] = 0x00; can_msg[2] = 0x00; can_msg[3] = 0x00;
                        can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00;
                        can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                        wait(0.2);                       
                        //73C 2377403705 0008 23 00 00 5F 00 00 00 00
                        can_msg[0] = 0x23; can_msg[1] = 0x00; can_msg[2] = 0x00; can_msg[3] = 0x5F;
                        can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00;
                        can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                        wait(0.2);                        
                        //73C 2377433695 0008 24 00 00 00 00 00 00 00 
                        can_msg[0] = 0x24; can_msg[1] = 0x00; can_msg[2] = 0x00; can_msg[3] = 0x00;
                        can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00;
                        can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 
                        wait(0.2);                                             
                    }
                }
            } // end of get runtime
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x01))
            {   //get temperatures 7A0 03 22 FD 01 00 00 00 00  -- not sure temperatures???
                //73C 10 09 62 FD 01 1D E2 1C  -- send this
                can_msg[0] = 0x10; can_msg[1] = 0x09; can_msg[2] = 0x62; can_msg[3] = 0xFD;
                can_msg[4] = 0x01; can_msg[5] = 0x1D; can_msg[6] = 0xE2; can_msg[7] = 0x1C;
                pc.printf("3");
                can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                
                //wait(0.1);
                done = 0;
                while ( done == 0 )
                { 
                    //wait for continue msg 7A0 30 00 14 00 00 00 00 00
                    //***************************************************************
                    if (can2.read(can_MsgRx))
                    {
                        pc.printf("m");
                        //pc.printf("m %x %x %x %x %x %x %x %x %x\n\r",can_MsgRx.id,
                        //  can_MsgRx.data[0], can_MsgRx.data[1], can_MsgRx.data[2], can_MsgRx.data[3],
                        //  can_MsgRx.data[4], can_MsgRx.data[5], can_MsgRx.data[6], can_MsgRx.data[7]);
                          cnt++;
                          if ( cnt > 10 ) done = 1;                        
                    }
                    //****************************************************************/
                    //can2.read(can_MsgRx);
                    //pc.printf("x");
                    if ((can_MsgRx.id == ESPAR_CMD) && (can_MsgRx.data[0] == 0x30))
                    {
                        done = 1;
                        pc.printf("b");             
                        //73C 2420412954 0008 21 84 1C B6 01 1D E2 1C
                        can_msg[0] = 0x21; can_msg[1] = 0x84; can_msg[2] = 0x1C; can_msg[3] = 0xB6;
                        can_msg[4] = 0x01; can_msg[5] = 0x1D; can_msg[6] = 0xE2; can_msg[7] = 0x1C;
                        can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                        wait(0.2);          
                    }
                }
            } // end of get temperatures
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x0A))
            { //expected[3] = //73C  05 62 FD 0A 00 00 E2 1C 
               can_msg[0] = 0x05; can_msg[1] = 0x62;
               can_msg[2] = 0xFD; can_msg[3] = 0x0A; 
               can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0xE2; can_msg[7] = 0x1C;
               can2.write(CANMessage(ESPAR_RES , can_msg, 8));
               pc.printf("4"); 
            }
            //7A0  03 22 FD 0D 00 00 00 00
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x0D))
            { //expected[3] = //73C  04 62 FD 0D 00 20 E2 1C  
               can_msg[0] = 0x04; can_msg[1] = 0x62;
               can_msg[2] = 0xFD; can_msg[3] = 0x0D; 
               can_msg[4] = 0x00; can_msg[5] = 0x20; can_msg[6] = 0xE2; can_msg[7] = 0x1C;
               can2.write(CANMessage(ESPAR_RES , can_msg, 8));
               pc.printf("5"); 
            }                
            // 
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x0F))
            {   //get ??? 7A0 03 22 FD 0F 00 00 00 00  -- not sure ???
                //73C  10 0D 62 FD 0F 00 01 75   -- send this
                can_msg[0] = 0x10; can_msg[1] = 0x0D; can_msg[2] = 0x62; can_msg[3] = 0xFD;
                can_msg[4] = 0x0F; can_msg[5] = 0x00; can_msg[6] = 0x01; can_msg[7] = 0x75;
                can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                pc.printf("6");
                //wait(0.1);
                done = 0;
                while ( done == 0 )
                { 
                    //wait for continue msg 7A0 30 00 14 00 00 00 00 00
                    can2.read(can_MsgRx);
                    if ((can_MsgRx.id == ESPAR_CMD) && (can_MsgRx.data[0] == 0x30))
                    {
                        done = 1;
                        pc.printf("d");             
                        //73C 21 30 75 44 7F FC 7F FC 
                        can_msg[0] = 0x21; can_msg[1] = 0x30; can_msg[2] = 0x75; can_msg[3] = 0x44;
                        can_msg[4] = 0x7F; can_msg[5] = 0xFC; can_msg[6] = 0x7F; can_msg[7] = 0xFC;
                        can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                        wait(0.1);          
                    }
                }
            } // end of ??
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x22))
            {   // got get 7a0 3 22 fd 22 0 0 0 0 - get water pump setting
                //send 0x05, 0x62, 0xFD, 0x24, 0x00, 0x00, 0x00, 0x00 will keep it happy
                can_msg[0] = 0x05; can_msg[1] = 0x62; can_msg[2] = 0xFD; can_msg[3] = 0x24;
                can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00;
                can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                pc.printf("7");
            }
             else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x24))
            {   // got get 7a0 3 22 fd 24 0 0 0 0 - get heat time limit
                //send 0x05, 0x62, 0xFD, 0x24, 0x00, 0x00, 0x00, 0x00 will keep it happy
                can_msg[0] = 0x05; can_msg[1] = 0x62; can_msg[2] = 0xFD; can_msg[3] = 0x24;
                can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00;
                can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                pc.printf("8");
            } 
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x36))
            {   // got 7a0 3 22 fd 36 0 0 0 0 - get altitude.
                //send Response is 0x05, 0x62, 0xFD, 0x04, [0x27, 0x10], 0xB0, 0x1C
                can_msg[0] = 0x05; can_msg[1] = 0x62; can_msg[2] = 0xFD; can_msg[3] = 0x04;
                can_msg[4] = 0x27; can_msg[5] = 0x10; can_msg[6] = 0xB0; can_msg[7] = 0x1C;
                if ( got_alt == true )
                {
                    can_msg[4] = alt[0];
                    can_msg[5] = alt[1];
                }
                can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                pc.printf("9");
            } 
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x04))
            {   //   battery send: { 0x03, 0x22, 0xFD, 0x04, 0x00, 0x00, 0x00, 0x00 };
                // Response is 0x05, 0x62, 0xFD, 0x04, MSB, LSB, 0x00, 0x00
                can_msg[0] = 0x05; can_msg[1] = 0x62; can_msg[2] = 0xFD; can_msg[3] = 0x04;
                can_msg[4] = 0x00; can_msg[5] = 0x7A; can_msg[6] = 0x00; can_msg[7] = 0x00;
                can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                pc.printf("V");
            } 
            else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x19) && (can_MsgRx.data[2] == 0x02) && (can_MsgRx.data[3] == 0x08))
            {   //   get single fault: { 0x03, 0x19, 0x02, 0x08, 0x00, 0x00, 0x00, 0x00 };
                // Response with 1 fault 073C: 07 59 02 7B 00 02 11 2A
                // Response with No fault 073C:03 59 02 7B 05 48 C3 01
                if ( ecu.active_fault == true )
                {
                    can_msg[0] = 0x07; can_msg[1] = 0x59; can_msg[2] = 0x02; can_msg[3] = 0x7B;
                    can_msg[4] = 0x00; can_msg[5] = 0x02; can_msg[6] = 0x11; can_msg[7] = 0x2B;
                }
                else    // stored fault
                {   
                    // these 2 lines reset to NO Faults.
                    can_msg[0] = 0x03; can_msg[1] = 0x59; can_msg[2] = 0x02; can_msg[3] = 0x7B;
                    can_msg[4] = 0x05; can_msg[5] = 0x48; can_msg[6] = 0xC3; can_msg[7] = 0x01;
                    // these 2 lines reset to a non-active fault
                    //can_msg[0] = 0x07; can_msg[1] = 0x59; can_msg[2] = 0x02; can_msg[3] = 0x7B;
                    //can_msg[4] = 0x00; can_msg[5] = 0x02; can_msg[6] = 0x11; can_msg[7] = 0x2A;                   
                }
                can2.write(CANMessage(ESPAR_RES , can_msg, 8));
                pc.printf("F");
            }                                 
            else
            {
                pc.printf("\n\r%x %x %x %x %x %x %x %x %x\n\r",can_MsgRx.id,
                      can_MsgRx.data[0], can_MsgRx.data[1], can_MsgRx.data[2], can_MsgRx.data[3],
                      can_MsgRx.data[4], can_MsgRx.data[5], can_MsgRx.data[6], can_MsgRx.data[7]);
            }
        } // end of if ESPAR_CMD
        //"Set" altitude command:
        //addresses0x0055 and 0x009D: { 0x4c, 0x1d, 0, 0, 0, 0, 0, 0 };
        //
        //LSB, MSB altitude x 10 in packet.  1D4C = 7500 = 750hpA, 2710 = 10000 = 1000hPa.
        else if ((can_MsgRx.id == 0x0055) || (can_MsgRx.id == 0x009D))
        {
            alt[0] = can_MsgRx.data[1];
            alt[1] = can_MsgRx.data[0];
            got_alt = true;
            pc.printf("A");
        }
        else if (can_MsgRx.id == PID_REQUEST) 
        {
            pc.printf("-");
            if(can_MsgRx.data[1] == MODE3) // Request trouble codes
            { 
                if(ecu.dtc == false) 
                {
                    can_msg[0] = 0x02;
                    can_msg[1] = MODE3_RESPONSE;
                    can_msg[2] = 0x00;
                } 
                else 
                {
                    can_msg[0] = 0x06;
                    can_msg[1] = MODE3_RESPONSE;
                    can_msg[2] = 0x02;
                    can_msg[3] = 0x01;
                    can_msg[4] = 0x00;
                    can_msg[5] = 0x02;
                    can_msg[6] = 0x00;
                }
                can2.write(CANMessage(PID_REPLY, can_msg, 8));
            }

            if(can_MsgRx.data[1] == MODE4) // Clear trouble codes, clear Check engine light
            { 
                ecu.dtc = false;
                led4 = 0;

                can_msg[0] = 0x00;
                can_msg[1] = MODE4_RESPONSE;
                can2.write(CANMessage(PID_REPLY, can_msg, 8));
            }

            if(can_MsgRx.data[1] == MODE1) 
            {
                can_msg[1] = MODE1_RESPONSE;
                switch(can_MsgRx.data[2]) 
                {
                    /* Details from http://en.wikipedia.org/wiki/OBD-II_PIDs */
                    case PID_SUPPORTED:
                        can_msg[0] = 0x06;
                        can_msg[2] = PID_SUPPORTED;
                        can_msg[3] = 0xE8;
                        can_msg[4] = 0x19;
                        can_msg[5] = 0x30;
                        can_msg[6] = 0x12;
                        can_msg[5] = 0x00;
                        can2.write(CANMessage(PID_REPLY, can_msg, 8));

                        break;

                    case MONITOR_STATUS:
                        can_msg[0] = 0x05;
                        can_msg[2] = MONITOR_STATUS;

                        if(ecu.dtc == true) can_msg[3] = 0x82;
                        else can_msg[3] = 0x00;

                        can_msg[4] = 0x07;
                        can_msg[5] = 0xFF;
                        can2.write(CANMessage(PID_REPLY, can_msg, 8));
                        break;

                    case ENGINE_RPM:              //   ((A*256)+B)/4    [RPM]
                        can_msg[0] = 0x04;
                        can_msg[2] = ENGINE_RPM;
                        can_msg[3] = (ecu.engine_rpm & 0xff00) >> 8;
                        can_msg[4] = ecu.engine_rpm & 0x00ff;
                        can2.write(CANMessage(PID_REPLY, can_msg, 8));
                        break;

                    case ENGINE_COOLANT_TEMP:     //     A-40              [degree C]
                        can_msg[0] = 0x03;
                        can_msg[2] = ENGINE_COOLANT_TEMP;
                        can_msg[3] = ecu.coolant_temp;
                        can2.write(CANMessage(PID_REPLY, can_msg, 8));
                        break;

                    case VEHICLE_SPEED:         // A                  [km]
                        can_msg[0] = 0x03;
                        can_msg[2] = VEHICLE_SPEED;
                        can_msg[3] = ecu.vehicle_speed;
                        can2.write(CANMessage(PID_REPLY, can_msg, 8));
                        break;

                    case MAF_SENSOR:               // ((256*A)+B) / 100  [g/s]
                        can_msg[0] = 0x04;
                        can_msg[2] = MAF_SENSOR;
                        can_msg[3] = (ecu.maf_airflow & 0xff00) >> 8;
                        can_msg[4] =  ecu.maf_airflow & 0x00ff;
                        can2.write(CANMessage(PID_REPLY, can_msg, 8));
                        break;

                    case O2_VOLTAGE:            // A * 0.005   (B-128) * 100/128 (if B==0xFF, sensor is not used in trim calc)
                        can_msg[0] = 0x04;
                        can_msg[2] = O2_VOLTAGE;
                        can_msg[3] = ecu.o2_voltage & 0x00ff;
                        can_msg[4] = (ecu.o2_voltage & 0xff00) >> 8;
                        can2.write(CANMessage(PID_REPLY, can_msg, 8));
                        break;;

                    case THROTTLE:            //
                        can_msg[0] = 0x03;
                        can_msg[2] = THROTTLE;
                        can_msg[3] = ecu.throttle_position;
                        can2.write(CANMessage(PID_REPLY, can_msg, 8));
                        break;
                }//switch
            }
        }

        /*****************************************************************
        pc.printf("\n\r%x %x %x %x %x %x %x %x %x",can_MsgRx.id,can_MsgRx.data[0],
                      can_MsgRx.data[1],
                      can_MsgRx.data[2],
                      can_MsgRx.data[3],
                      can_MsgRx.data[4],
                      can_MsgRx.data[5],
                      can_MsgRx.data[6],
                      can_MsgRx.data[7]);
        ****************************************************************/
        led2 = 0;
    }
    else
    {
       //pc.printf("\n\rNo CAN Message\n\r"); 
       int rxerrors = can2.rderror();
       if ( rxerrors != 0 )
       {
           pc.printf("\n\rCAN Rx Errors: %d\n\r", rxerrors); 
           can2.reset();
        }
        /*************************
        else
        {
          pc.printf(".");      
        }
        **************************/
    }

    return 0;

}


} // namespace mbed