ROM Comm
RIXEN SIM with BluePill Maple Mini
Dependencies: mbed mbed-STM32F103C8T6
Embed:
(wiki syntax)
Show/hide line numbers
ecu_simulator.cpp
00001 #include "mbed.h" 00002 #include "ecu_simulator.h" 00003 #include "globals.h" 00004 00005 00006 // Use a timer to see if things take too long 00007 Timer CANTimer; 00008 00009 namespace mbed 00010 { 00011 00012 00013 ecu_sim::ecu_sim(int can_speed) 00014 { 00015 if ( can2.frequency(can_speed) ) 00016 pc.printf("CAN speed set to %d \n\r", can_speed); 00017 else 00018 pc.printf("CAN speed SET failed\n\r"); 00019 } 00020 00021 void ecu_sim::canspeed(int can_speed) 00022 { 00023 if ( can2.frequency(can_speed) ) 00024 pc.printf("CAN speed set to %d \n\r", can_speed); 00025 else 00026 pc.printf("CAN speed SET failed\n\r"); 00027 } 00028 00029 00030 00031 //#define TIMEOUT 500 00032 unsigned char ecu_sim::request(void) 00033 { 00034 char can_msg[8]; 00035 int done = 0; 00036 int cnt = 0; 00037 //pc.printf("*"); 00038 00039 00040 //if ((can2.read(can_MsgRx)) && (can_MsgRx.id == PID_REQUEST) ) 00041 if (can2.read(can_MsgRx)) //&& (can_MsgRx.id == PID_REQUEST) ) 00042 { 00043 pc.printf("."); 00044 led2 = 1; 00045 if (can_MsgRx.id == ESPAR_CMD) 00046 { 00047 pc.printf("0"); 00048 // wake command wake[8] = { 0x02, 0x3e, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; 00049 if((can_MsgRx.data[0] == 0x02) && (can_MsgRx.data[1] == 0x3E))// wake request 00050 { //expected[3] = {0x02, 0x7e, 0x00}; 00051 can_msg[0] = 0x02; 00052 can_msg[1] = 0x7E; 00053 can_msg[2] = 0x00; can_msg[3] = 0x00; 00054 can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00; 00055 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00056 pc.printf("1"); 00057 } 00058 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x17)) 00059 { // got get runtime: 7A0 03 22 FD 17 00 00 00 00 00060 // send 73C 10 1F 62 FD 17 00 00 00 00061 can_msg[0] = 0x10; can_msg[1] = 0x1F; can_msg[2] = 0x62; can_msg[3] = 0xFD; 00062 can_msg[4] = 0x17; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00; 00063 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00064 pc.printf("2"); 00065 //wait(0.1); 00066 done = 0; 00067 while ( done == 0 ) 00068 { 00069 //wait for continue msg 7A0 30 00 14 00 00 00 00 00 00070 can2.read(can_MsgRx); 00071 if ((can_MsgRx.id == ESPAR_CMD) && (can_MsgRx.data[0] == 0x30)) 00072 { 00073 done = 1; 00074 pc.printf("a"); 00075 //73C 2377343693 0008 21 91 00 00 00 00 00 00 00076 can_msg[0] = 0x21; can_msg[1] = 0x91; can_msg[2] = 0x00; can_msg[3] = 0x00; 00077 can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00; 00078 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00079 wait(0.2); 00080 //73C 2377373697 0008 22 00 00 00 00 00 00 00 00081 can_msg[0] = 0x22; can_msg[1] = 0x00; can_msg[2] = 0x00; can_msg[3] = 0x00; 00082 can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00; 00083 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00084 wait(0.2); 00085 //73C 2377403705 0008 23 00 00 5F 00 00 00 00 00086 can_msg[0] = 0x23; can_msg[1] = 0x00; can_msg[2] = 0x00; can_msg[3] = 0x5F; 00087 can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00; 00088 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00089 wait(0.2); 00090 //73C 2377433695 0008 24 00 00 00 00 00 00 00 00091 can_msg[0] = 0x24; can_msg[1] = 0x00; can_msg[2] = 0x00; can_msg[3] = 0x00; 00092 can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00; 00093 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00094 wait(0.2); 00095 } 00096 } 00097 } // end of get runtime 00098 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x01)) 00099 { //get temperatures 7A0 03 22 FD 01 00 00 00 00 -- not sure temperatures??? 00100 //73C 10 09 62 FD 01 1D E2 1C -- send this 00101 can_msg[0] = 0x10; can_msg[1] = 0x09; can_msg[2] = 0x62; can_msg[3] = 0xFD; 00102 can_msg[4] = 0x01; can_msg[5] = 0x1D; can_msg[6] = 0xE2; can_msg[7] = 0x1C; 00103 pc.printf("3"); 00104 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00105 00106 //wait(0.1); 00107 done = 0; 00108 while ( done == 0 ) 00109 { 00110 //wait for continue msg 7A0 30 00 14 00 00 00 00 00 00111 //*************************************************************** 00112 if (can2.read(can_MsgRx)) 00113 { 00114 pc.printf("m"); 00115 //pc.printf("m %x %x %x %x %x %x %x %x %x\n\r",can_MsgRx.id, 00116 // can_MsgRx.data[0], can_MsgRx.data[1], can_MsgRx.data[2], can_MsgRx.data[3], 00117 // can_MsgRx.data[4], can_MsgRx.data[5], can_MsgRx.data[6], can_MsgRx.data[7]); 00118 cnt++; 00119 if ( cnt > 10 ) done = 1; 00120 } 00121 //****************************************************************/ 00122 //can2.read(can_MsgRx); 00123 //pc.printf("x"); 00124 if ((can_MsgRx.id == ESPAR_CMD) && (can_MsgRx.data[0] == 0x30)) 00125 { 00126 done = 1; 00127 pc.printf("b"); 00128 //73C 2420412954 0008 21 84 1C B6 01 1D E2 1C 00129 can_msg[0] = 0x21; can_msg[1] = 0x84; can_msg[2] = 0x1C; can_msg[3] = 0xB6; 00130 can_msg[4] = 0x01; can_msg[5] = 0x1D; can_msg[6] = 0xE2; can_msg[7] = 0x1C; 00131 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00132 wait(0.2); 00133 } 00134 } 00135 } // end of get temperatures 00136 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x0A)) 00137 { //expected[3] = //73C 05 62 FD 0A 00 00 E2 1C 00138 can_msg[0] = 0x05; can_msg[1] = 0x62; 00139 can_msg[2] = 0xFD; can_msg[3] = 0x0A; 00140 can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0xE2; can_msg[7] = 0x1C; 00141 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00142 pc.printf("4"); 00143 } 00144 //7A0 03 22 FD 0D 00 00 00 00 00145 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x0D)) 00146 { //expected[3] = //73C 04 62 FD 0D 00 20 E2 1C 00147 can_msg[0] = 0x04; can_msg[1] = 0x62; 00148 can_msg[2] = 0xFD; can_msg[3] = 0x0D; 00149 can_msg[4] = 0x00; can_msg[5] = 0x20; can_msg[6] = 0xE2; can_msg[7] = 0x1C; 00150 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00151 pc.printf("5"); 00152 } 00153 // 00154 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x0F)) 00155 { //get ??? 7A0 03 22 FD 0F 00 00 00 00 -- not sure ??? 00156 //73C 10 0D 62 FD 0F 00 01 75 -- send this 00157 can_msg[0] = 0x10; can_msg[1] = 0x0D; can_msg[2] = 0x62; can_msg[3] = 0xFD; 00158 can_msg[4] = 0x0F; can_msg[5] = 0x00; can_msg[6] = 0x01; can_msg[7] = 0x75; 00159 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00160 pc.printf("6"); 00161 //wait(0.1); 00162 done = 0; 00163 while ( done == 0 ) 00164 { 00165 //wait for continue msg 7A0 30 00 14 00 00 00 00 00 00166 can2.read(can_MsgRx); 00167 if ((can_MsgRx.id == ESPAR_CMD) && (can_MsgRx.data[0] == 0x30)) 00168 { 00169 done = 1; 00170 pc.printf("d"); 00171 //73C 21 30 75 44 7F FC 7F FC 00172 can_msg[0] = 0x21; can_msg[1] = 0x30; can_msg[2] = 0x75; can_msg[3] = 0x44; 00173 can_msg[4] = 0x7F; can_msg[5] = 0xFC; can_msg[6] = 0x7F; can_msg[7] = 0xFC; 00174 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00175 wait(0.1); 00176 } 00177 } 00178 } // end of ?? 00179 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x22)) 00180 { // got get 7a0 3 22 fd 22 0 0 0 0 - get water pump setting 00181 //send 0x05, 0x62, 0xFD, 0x24, 0x00, 0x00, 0x00, 0x00 will keep it happy 00182 can_msg[0] = 0x05; can_msg[1] = 0x62; can_msg[2] = 0xFD; can_msg[3] = 0x24; 00183 can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00; 00184 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00185 pc.printf("7"); 00186 } 00187 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x24)) 00188 { // got get 7a0 3 22 fd 24 0 0 0 0 - get heat time limit 00189 //send 0x05, 0x62, 0xFD, 0x24, 0x00, 0x00, 0x00, 0x00 will keep it happy 00190 can_msg[0] = 0x05; can_msg[1] = 0x62; can_msg[2] = 0xFD; can_msg[3] = 0x24; 00191 can_msg[4] = 0x00; can_msg[5] = 0x00; can_msg[6] = 0x00; can_msg[7] = 0x00; 00192 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00193 pc.printf("8"); 00194 } 00195 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x36)) 00196 { // got 7a0 3 22 fd 36 0 0 0 0 - get altitude. 00197 //send Response is 0x05, 0x62, 0xFD, 0x04, [0x27, 0x10], 0xB0, 0x1C 00198 can_msg[0] = 0x05; can_msg[1] = 0x62; can_msg[2] = 0xFD; can_msg[3] = 0x04; 00199 can_msg[4] = 0x27; can_msg[5] = 0x10; can_msg[6] = 0xB0; can_msg[7] = 0x1C; 00200 if ( got_alt == true ) 00201 { 00202 can_msg[4] = alt[0]; 00203 can_msg[5] = alt[1]; 00204 } 00205 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00206 pc.printf("9"); 00207 } 00208 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x22) && (can_MsgRx.data[2] == 0xFD) && (can_MsgRx.data[3] == 0x04)) 00209 { // battery send: { 0x03, 0x22, 0xFD, 0x04, 0x00, 0x00, 0x00, 0x00 }; 00210 // Response is 0x05, 0x62, 0xFD, 0x04, MSB, LSB, 0x00, 0x00 00211 can_msg[0] = 0x05; can_msg[1] = 0x62; can_msg[2] = 0xFD; can_msg[3] = 0x04; 00212 can_msg[4] = 0x00; can_msg[5] = 0x7A; can_msg[6] = 0x00; can_msg[7] = 0x00; 00213 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00214 pc.printf("V"); 00215 } 00216 else if ((can_MsgRx.data[0] == 0x03) && (can_MsgRx.data[1] == 0x19) && (can_MsgRx.data[2] == 0x02) && (can_MsgRx.data[3] == 0x08)) 00217 { // get single fault: { 0x03, 0x19, 0x02, 0x08, 0x00, 0x00, 0x00, 0x00 }; 00218 // Response with 1 fault 073C: 07 59 02 7B 00 02 11 2A 00219 // Response with No fault 073C:03 59 02 7B 05 48 C3 01 00220 if ( ecu.active_fault == true ) 00221 { 00222 can_msg[0] = 0x07; can_msg[1] = 0x59; can_msg[2] = 0x02; can_msg[3] = 0x7B; 00223 can_msg[4] = 0x00; can_msg[5] = 0x02; can_msg[6] = 0x11; can_msg[7] = 0x2B; 00224 } 00225 else // stored fault 00226 { 00227 // these 2 lines reset to NO Faults. 00228 can_msg[0] = 0x03; can_msg[1] = 0x59; can_msg[2] = 0x02; can_msg[3] = 0x7B; 00229 can_msg[4] = 0x05; can_msg[5] = 0x48; can_msg[6] = 0xC3; can_msg[7] = 0x01; 00230 // these 2 lines reset to a non-active fault 00231 //can_msg[0] = 0x07; can_msg[1] = 0x59; can_msg[2] = 0x02; can_msg[3] = 0x7B; 00232 //can_msg[4] = 0x00; can_msg[5] = 0x02; can_msg[6] = 0x11; can_msg[7] = 0x2A; 00233 } 00234 can2.write(CANMessage(ESPAR_RES , can_msg, 8)); 00235 pc.printf("F"); 00236 } 00237 else 00238 { 00239 pc.printf("\n\r%x %x %x %x %x %x %x %x %x\n\r",can_MsgRx.id, 00240 can_MsgRx.data[0], can_MsgRx.data[1], can_MsgRx.data[2], can_MsgRx.data[3], 00241 can_MsgRx.data[4], can_MsgRx.data[5], can_MsgRx.data[6], can_MsgRx.data[7]); 00242 } 00243 } // end of if ESPAR_CMD 00244 //"Set" altitude command: 00245 //addresses0x0055 and 0x009D: { 0x4c, 0x1d, 0, 0, 0, 0, 0, 0 }; 00246 // 00247 //LSB, MSB altitude x 10 in packet. 1D4C = 7500 = 750hpA, 2710 = 10000 = 1000hPa. 00248 else if ((can_MsgRx.id == 0x0055) || (can_MsgRx.id == 0x009D)) 00249 { 00250 alt[0] = can_MsgRx.data[1]; 00251 alt[1] = can_MsgRx.data[0]; 00252 got_alt = true; 00253 pc.printf("A"); 00254 } 00255 else if (can_MsgRx.id == PID_REQUEST) 00256 { 00257 pc.printf("-"); 00258 if(can_MsgRx.data[1] == MODE3) // Request trouble codes 00259 { 00260 if(ecu.dtc == false) 00261 { 00262 can_msg[0] = 0x02; 00263 can_msg[1] = MODE3_RESPONSE; 00264 can_msg[2] = 0x00; 00265 } 00266 else 00267 { 00268 can_msg[0] = 0x06; 00269 can_msg[1] = MODE3_RESPONSE; 00270 can_msg[2] = 0x02; 00271 can_msg[3] = 0x01; 00272 can_msg[4] = 0x00; 00273 can_msg[5] = 0x02; 00274 can_msg[6] = 0x00; 00275 } 00276 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00277 } 00278 00279 if(can_MsgRx.data[1] == MODE4) // Clear trouble codes, clear Check engine light 00280 { 00281 ecu.dtc = false; 00282 led4 = 0; 00283 00284 can_msg[0] = 0x00; 00285 can_msg[1] = MODE4_RESPONSE; 00286 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00287 } 00288 00289 if(can_MsgRx.data[1] == MODE1) 00290 { 00291 can_msg[1] = MODE1_RESPONSE; 00292 switch(can_MsgRx.data[2]) 00293 { 00294 /* Details from http://en.wikipedia.org/wiki/OBD-II_PIDs */ 00295 case PID_SUPPORTED: 00296 can_msg[0] = 0x06; 00297 can_msg[2] = PID_SUPPORTED; 00298 can_msg[3] = 0xE8; 00299 can_msg[4] = 0x19; 00300 can_msg[5] = 0x30; 00301 can_msg[6] = 0x12; 00302 can_msg[5] = 0x00; 00303 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00304 00305 break; 00306 00307 case MONITOR_STATUS: 00308 can_msg[0] = 0x05; 00309 can_msg[2] = MONITOR_STATUS; 00310 00311 if(ecu.dtc == true) can_msg[3] = 0x82; 00312 else can_msg[3] = 0x00; 00313 00314 can_msg[4] = 0x07; 00315 can_msg[5] = 0xFF; 00316 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00317 break; 00318 00319 case ENGINE_RPM: // ((A*256)+B)/4 [RPM] 00320 can_msg[0] = 0x04; 00321 can_msg[2] = ENGINE_RPM; 00322 can_msg[3] = (ecu.engine_rpm & 0xff00) >> 8; 00323 can_msg[4] = ecu.engine_rpm & 0x00ff; 00324 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00325 break; 00326 00327 case ENGINE_COOLANT_TEMP: // A-40 [degree C] 00328 can_msg[0] = 0x03; 00329 can_msg[2] = ENGINE_COOLANT_TEMP; 00330 can_msg[3] = ecu.coolant_temp; 00331 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00332 break; 00333 00334 case VEHICLE_SPEED: // A [km] 00335 can_msg[0] = 0x03; 00336 can_msg[2] = VEHICLE_SPEED; 00337 can_msg[3] = ecu.vehicle_speed; 00338 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00339 break; 00340 00341 case MAF_SENSOR: // ((256*A)+B) / 100 [g/s] 00342 can_msg[0] = 0x04; 00343 can_msg[2] = MAF_SENSOR; 00344 can_msg[3] = (ecu.maf_airflow & 0xff00) >> 8; 00345 can_msg[4] = ecu.maf_airflow & 0x00ff; 00346 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00347 break; 00348 00349 case O2_VOLTAGE: // A * 0.005 (B-128) * 100/128 (if B==0xFF, sensor is not used in trim calc) 00350 can_msg[0] = 0x04; 00351 can_msg[2] = O2_VOLTAGE; 00352 can_msg[3] = ecu.o2_voltage & 0x00ff; 00353 can_msg[4] = (ecu.o2_voltage & 0xff00) >> 8; 00354 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00355 break;; 00356 00357 case THROTTLE: // 00358 can_msg[0] = 0x03; 00359 can_msg[2] = THROTTLE; 00360 can_msg[3] = ecu.throttle_position; 00361 can2.write(CANMessage(PID_REPLY, can_msg, 8)); 00362 break; 00363 }//switch 00364 } 00365 } 00366 00367 /***************************************************************** 00368 pc.printf("\n\r%x %x %x %x %x %x %x %x %x",can_MsgRx.id,can_MsgRx.data[0], 00369 can_MsgRx.data[1], 00370 can_MsgRx.data[2], 00371 can_MsgRx.data[3], 00372 can_MsgRx.data[4], 00373 can_MsgRx.data[5], 00374 can_MsgRx.data[6], 00375 can_MsgRx.data[7]); 00376 ****************************************************************/ 00377 led2 = 0; 00378 } 00379 else 00380 { 00381 //pc.printf("\n\rNo CAN Message\n\r"); 00382 int rxerrors = can2.rderror(); 00383 if ( rxerrors != 0 ) 00384 { 00385 pc.printf("\n\rCAN Rx Errors: %d\n\r", rxerrors); 00386 can2.reset(); 00387 } 00388 /************************* 00389 else 00390 { 00391 pc.printf("."); 00392 } 00393 **************************/ 00394 } 00395 00396 return 0; 00397 00398 } 00399 00400 00401 } // namespace mbed
Generated on Mon Jul 25 2022 18:33:33 by
1.7.2