OBDII.cpp

00001 #include "mbed.h"
00002 #include "OBDII.h"
00003 
00004 OBDII::OBDII(CAN *c, DigitalOut *rs, int can_speed) {
00005     can = c;
00006     canRS = rs;
00007     *canRS = 0;
00008     
00009     can->frequency(can_speed);
00010     data.mafSensor = -1;
00011     data.speed = -1;
00012 
00013     HostSock = new UDPSock(new Host(IpAddr(), 20000, NULL), 64, this);
00014     statusHost = new Host(IpAddr(224,1,2,3), 21000, NULL); //Join multicast group on port 50000
00015 
00016     CANTimer.start();
00017     done = true;
00018     mode = 0;
00019 }
00020 
00021 void OBDII::UpdateData(void) {
00022     readCAN();
00023 
00024     if (CANTimer.read_ms() > 200) {
00025         done = true;
00026     }
00027 
00028     if (done) {
00029         CANTimer.reset();
00030         mode++;
00031 
00032         switch (mode) {
00033             case 0:
00034                 currentRequest_ = VEHICLE_SPEED;
00035                 request(VEHICLE_SPEED);
00036                 break;
00037 
00038             case 1:
00039                 request(MAF_SENSOR);
00040                 break;
00041 
00042             case 2:
00043                 request(FUEL_LEVEL);
00044                 break;
00045 
00046             case 3:
00047                 request(ENGINE_RPM);
00048                 break;
00049 
00050             case 4:
00051                 request(ENGINE_COOLANT_TEMP);
00052                 break;
00053 
00054             case 5:
00055                 request(THROTTLE);
00056                 break;
00057 
00058             case 6:
00059                 request(O2_VOLTAGE);
00060                 mode = 0;
00061                 break;
00062         }
00063     }
00064 }
00065 
00066 void OBDII::request(unsigned char pid) {
00067     char can_msg[8];
00068 
00069     can_msg[0] = 0x02;
00070     can_msg[1] = 0x01;
00071     can_msg[2] = pid;
00072     can_msg[3] = 0;
00073     can_msg[4] = 0;
00074     can_msg[5] = 0;
00075     can_msg[6] = 0;
00076     can_msg[7] = 0;
00077 
00078     if (can->write(CANMessage(PID_REQUEST, can_msg, 8))) {
00079     }
00080     
00081     done = false;
00082 }
00083 
00084 void OBDII::EnhancedRequest(unsigned short pid) {
00085     char can_msg[8];
00086 
00087     can_msg[0] = 0x03;
00088     can_msg[1] = 0x22;
00089     can_msg[2] = (unsigned char)((pid &0xFF00) >> 8);
00090     can_msg[3] = (unsigned char)((pid &0x00FF));
00091     can_msg[4] = 0;
00092     can_msg[5] = 0;
00093     can_msg[6] = 0;
00094     can_msg[7] = 0;
00095 
00096     if (can->write(CANMessage(PID_ENHANCED_REQUEST, can_msg, 8))) {
00097     }
00098     
00099     done = false;
00100 }
00101 
00102 void OBDII::readCAN()
00103 {
00104     CANMessage can_MsgRx;
00105 
00106     while (can->read(can_MsgRx))
00107     {
00108         if (can_MsgRx.id == PID_REPLY)
00109         {
00110             done = true;
00111 
00112             if (can_MsgRx.data[0] == 3)
00113             {
00114                 switch (can_MsgRx.data[2])
00115                 {
00116                     case ENGINE_RPM:
00117                         data.rpm = (int)(((can_MsgRx.data[3]*256) + can_MsgRx.data[4])/4);
00118                         printf("OBD RPM = %d\n\r", data.rpm);
00119                         break;
00120     
00121                     case ENGINE_COOLANT_TEMP:
00122                         data.coolantTemp =  (int)(can_MsgRx.data[3] * 0.621371192);
00123                         printf("OBD Coolant Temp = %d\n\r", data.coolantTemp);
00124                         break;
00125     
00126                     case MAF_SENSOR:
00127                         data.mafSensor =  (int)(((can_MsgRx.data[3]*256) + can_MsgRx.data[4])/100);
00128                         printf("OBD MAF Sensor = %d\n\r", data.mafSensor);
00129                         break;
00130     
00131                     case O2_VOLTAGE:
00132                         data.o2Voltage = (int)(can_MsgRx.data[3]*0.005);
00133                         printf("OBD O2 Voltage = %d\n\r", data.o2Voltage);
00134                         break;
00135     
00136                     case THROTTLE:            //
00137                         data.throttle = (int) ((can_MsgRx.data[3]*100)/255);
00138                         printf("OBD Throttle = %d\n\r", data.throttle);
00139                         break;
00140     
00141                     case VEHICLE_SPEED:
00142                         data.speed = (int)can_MsgRx.data[3];
00143                         printf("OBD Speed = %d\n\r", data.speed);
00144                         break;
00145     
00146                     case FUEL_LEVEL:
00147                         data.fuel = 100 * can_MsgRx.data[3] / 255;
00148                         printf("OBD Fuel = %d\n\r", data.fuel);
00149                         break;
00150                 }
00151             }
00152             else if (can_MsgRx.data[0] == 4)
00153             {
00154                 switch (can_MsgRx.data[2])
00155                 {
00156                     case 0x0040:
00157                     break;
00158                 }
00159             }
00160             
00161             HostSock->SendTo(statusHost, sizeof(ecu_info), (char *)&data);
00162         }
00163     }
00164 
00165     if ((data.mafSensor != -1) && (data.speed != -1)) {
00166         float mph = 0.621371192 * data.speed;
00167         data.mpg = mph / (data.mafSensor * 0.0804988);
00168 
00169         printf("Current MPG = %f\n\r", data.mpg);
00170     }
00171 }
00172 
00173 void OBDII::ReceivedData(int socketStatus, int len, char *msg)
00174 {
00175 
00176 }