main.cpp

00001 #include "mbed.h"
00002 #include "T15Msg.h"
00003 #include "SpeedMsg.h"
00004 
00005 Timer timer100;
00006 Timer timer200;
00007 
00008 Serial pc(USBTX, USBRX);
00009 DigitalOut led1(PA_5);
00010 CAN can1(PB_8, PB_9);  // rd, td Transmitter
00011 
00012 T15Msg t15;
00013 SpeedMsg speed;
00014 
00015 int fuel1 = 0x4F04;
00016 int fuel2 = 0x2312;
00017 
00018 char absCtr = 0;
00019 unsigned char airBagCtr = 0;
00020 char ctr100 = 0;
00021 
00022 /*
00023  0x130 (CAS) T15 signal, 100 ms
00024  0x1B4 (Kombi) OUT? Speed status, 100 ms
00025 */
00026 
00027 
00028 void sendT15() {
00029     if (!t15.sendMessage(&can1)) {
00030         pc.printf("Cannot send T15!\n");
00031     }    
00032 /*
00033     char data_130 [] = { 0x45, 0x42, 0x8F, 0xE2, 0xFE };
00034     char data_130 [] = { 0x45, 0x40, 0x21, 0x8F, 0x00 };
00035 */
00036 }
00037 
00038 void sendABSCtr() {
00039     
00040     char data[2];
00041     absCtr++;
00042     if (absCtr == 0x0f) absCtr = 0;
00043     data[0] = 0xf0 | absCtr;
00044     data[1] = 0xff;
00045     if (!can1.write(CANMessage(0x0C0, data, 2))) {
00046         pc.printf("Cannot send ABS counter!\n");
00047     }        
00048 }
00049 
00050 void sendAirBagCtr() {
00051     
00052     char data[2];
00053     airBagCtr++;
00054     if (airBagCtr == 0xff) airBagCtr = 0;
00055     data[0] = airBagCtr;
00056     data[1] = 0xff;
00057     if (!can1.write(CANMessage(0x0D7, data, 2))) {
00058         pc.printf("Cannot send AirBag counter!\n");
00059     }        
00060 }
00061 
00062 void sendLight(char mode) {
00063 
00064     /*
00065     // Light
00066     CAN ID-----DATA
00067     ===== ----====
00068     21A-------00 20 F7 OFF
00069     21A-------04 22 F7 Parking light / angel eyes
00070     21A-------05 22 F7 Light on
00071     */
00072     
00073     char *data;
00074     char data_21A_0 [] = { 0x00, 0x20, 0xF7 };
00075     char data_21A_1 [] = { 0x04, 0x22, 0xF7 };
00076     char data_21A_2 [] = { 0x05, 0x22, 0xF7 };
00077 
00078     switch (mode) {
00079         case 1:
00080             data = data_21A_1;
00081             break;
00082         case 2:
00083             data = data_21A_2;
00084             break;
00085         default:
00086             data = data_21A_0;
00087             break;
00088     }
00089     if (can1.write(CANMessage(0x21A, data, 3))) {
00090         pc.printf("Light sent %d\n", mode);
00091     }        
00092 }
00093 
00094     //Ignition status
00095     /*
00096     0[7]: engine runinng
00097     1[7]: ignition || engine running
00098     2[7]: ignition || engine running
00099     2[654312]: 111111
00100     3: 0x50
00101     4-7: 0xff
00102     */
00103     
00104     /*
00105     // Ignition
00106     char data_26E [] = { 0x40, 0x40, 0x4F, 0x50, 0xFF, 0xFF, 0xFF, 0xFF };
00107     if(can1.write(CANMessage(0x26E, data_26E, 8))) {
00108         printf("Ignition sent \n");
00109     }
00110     */
00111     
00112     /*
00113     // RPM
00114     char data_0A5 [] = { 0xCD, 0xD5, 0xEC, 0xC7, 0x7E, 0x34, 0x0C, 0xF1 };
00115     if(can1.write(CANMessage(0x0A5, data_0A5, 8))) {
00116         printf("RPM sent \n");
00117     }
00118     */
00119     
00120     /*
00121     // Speed
00122     char data_1A6 [] = { 0x13, 0x4D, 0x46, 0x4D, 0x33, 0x4D, 0xD0, 0xFF };
00123     if(can1.write(CANMessage(0x1A6, data_1A6, 8))) {
00124         printf("Speed sent \n");
00125     }
00126     */
00127     
00128     /*
00129     // Door
00130     char data_2FC [] = { 0x81, 0x01, 0x00, 0xFF, 0xFF, 0xFF, 0xFF };
00131     if(can1.write(CANMessage(0x2FC, data_2FC, 7))) {
00132         printf("Door sent \n");
00133     }
00134     */
00135     
00136     /*
00137     // HB
00138     char data_34F [] = { 0xFE, 0xFF };
00139     //data_34F[0] = counter;
00140     if(can1.write(CANMessage(0x34F, data_34F, 2))) {
00141         printf("HB sent \n");
00142     }
00143     */
00144     
00145 void sendFuel() {
00146     
00147     char data[5];
00148     data[0] = fuel1 / 256;
00149     data[1] = fuel1 & 0xff;
00150     data[2] = fuel2 / 256;
00151     data[3] = fuel2 & 0xff;
00152     data[4] = 0;
00153     
00154     if(!can1.write(CANMessage(0x349, data, 5))) {
00155         printf("Cannot send fuel!\r\n");
00156     }    
00157 }
00158 
00159 void canLoop() {
00160         
00161     if (timer100.read_ms() > 100) {
00162         sendT15();
00163         speed.sendMessage(&can1);
00164         
00165         timer100.reset();
00166         led1 = !led1;
00167     }
00168     
00169     if (timer200.read_ms() > 200) {
00170         sendABSCtr();
00171         sendAirBagCtr();
00172         sendFuel();
00173         
00174         timer200.reset();
00175     }    
00176 }
00177 
00178 
00179 bool sendMessage(int id, char *data, int len) {
00180     
00181     pc.printf("Sending message: %01X%02X (%d), ", id >> 8, id & 0xff, id);
00182     for (int i = 0; i < len; i++) pc.printf("%02X ", data[i]);
00183     pc.printf("\r\n");
00184     
00185     return can1.write(CANMessage(id, data, len));
00186 }
00187 
00188 int readHex(char *hexstr, uint8_t *data, int len) {
00189 
00190     int i = 0, l = 0;
00191     char hexnums[128];
00192     
00193     while (i < len) {
00194         char h = hexstr[i++];
00195         if ((h < '0' || h > 'F') || (h > '9' && h < 'A')) continue;
00196         if (h >= 'A') hexnums[l++] = 10 + (h - 'A');
00197         else hexnums[l++] = h - '0';
00198     }
00199     hexnums[l] = 0; // Extra 0 for uneven pairs
00200     i = 0;
00201     while (i < (l+1) / 2) {
00202         data[i] = (hexnums[i * 2] << 4) | hexnums[i * 2 + 1];
00203         i++;
00204     }
00205     return (l+1) / 2;
00206 
00207 }
00208 
00209 //**********The main program*********************
00210 
00211 void doCommand(char *command) {
00212 
00213     pc.printf("Command: %s\r\n", command);
00214     for (int j = 0;;j++) {
00215         pc.printf("%02x ", command[j]);
00216         if (command[j] == 0) break;
00217     }
00218     pc.printf("\r\n");
00219                 
00220     if (strcmp(command, "START") == 0) {
00221         pc.printf("Start\r\n");
00222         t15.start();
00223         speed.setSpeed(0x13, 0x4d, 0x46, 0x4d, 0x33, 0x4d);
00224     }
00225     
00226     else if (strcmp(command, "STOP") == 0) {
00227         pc.printf("Stop\n");
00228         speed.setSpeed(0x0, 0x0, 0x0, 0x0, 0x0, 0x0);
00229         t15.stop();
00230     }
00231     
00232     else if (command[0] == 'L') {
00233         int mode = command[1] - '0';
00234         sendLight(mode);
00235     }
00236     
00237     else if (command[0] == 'M') {
00238         uint8_t hexid[4];
00239         uint8_t canmsg[8];
00240         int idlen;
00241         
00242         if (strchr(command + 1, ' ')) idlen = strchr(command + 1, ' ') - (command + 1);
00243         else return;
00244         readHex(command + 1, hexid, idlen);
00245         int id = hexid[0] * 256 + hexid[1];
00246         int l = readHex(command + 1 + idlen, canmsg, strlen(command  + 1 + idlen));
00247         
00248         sendMessage(id, (char*) canmsg, l);
00249     }
00250     
00251     else if (command[0] == 'F') {
00252         uint8_t hexfuel[4];
00253         
00254         int l = readHex(command + 1, hexfuel, strlen(command + 1));
00255         fuel1 = hexfuel[0] * 256 + hexfuel[1];
00256         if (l>2) fuel2 = hexfuel[2] * 256 + hexfuel[3];
00257         
00258         pc.printf("Set fuel to %02x%02x : %02x%02x.\r\n", fuel1 / 256, fuel1 & 0xff, fuel2 / 256, fuel2 & 0xff);
00259     }
00260 }
00261 
00262 int main() {
00263 
00264     pc.baud(115200);
00265     can1.frequency(100000);
00266     
00267     timer100.start();
00268     timer200.start();
00269     
00270     pc.printf("CAN hacking started\r\n");
00271     
00272     char buffer[128];
00273     int bufferlen = 0;
00274     
00275     while (1) {
00276         
00277         canLoop();
00278         
00279         if (pc.readable()) {
00280             char c = pc.getc();
00281             pc.putc(c); // Local ECHO
00282             if (c == '\r') {
00283                 buffer[bufferlen] = 0;
00284                 doCommand(buffer);
00285                 bufferlen = 0;
00286             } else {
00287                 buffer[bufferlen++] = c;
00288             }
00289         }
00290     }
00291 
00292 }