Diff: main.cpp

Revision:

2:ef9a8a114395

Parent:

1:a582aeb10c32

Child:

3:4a4463380739

--- a/main.cpp	Thu Mar 09 12:20:25 2017 +0000
+++ b/main.cpp	Fri Mar 10 09:03:10 2017 +0000
@@ -1,13 +1,21 @@
 #include "mbed.h"
 #include "T15Msg.h"
 
-Ticker ticker;
+Timer timer100;
+Timer timer200;
 
 Serial pc(USBTX, USBRX);
 DigitalOut led1(PA_5);
 CAN can1(PB_8, PB_9);  // rd, td Transmitter
 T15Msg t15;
 
+int fuel1 = 0x4F04;
+int fuel2 = 0x2312;
+
+char absCtr = 0;
+unsigned char airBagCtr = 0;
+char ctr100 = 0;
+
 /*
  0x130 (CAS) T15 signal, 100 ms
  0x1B4 (Kombi) OUT? Speed status, 100 ms
@@ -24,9 +32,28 @@
 */
 }
 
-void tick100() {
-    sendT15();
-    led1 = !led1;
+void sendABSCtr() {
+    
+    char data[2];
+    absCtr++;
+    if (absCtr == 0x0f) absCtr = 0;
+    data[0] = 0xf0 | absCtr;
+    data[1] = 0xff;
+    if (!can1.write(CANMessage(0x0C0, data, 2))) {
+        pc.printf("Cannot send ABS counter!\n");
+    }        
+}
+
+void sendAirBagCtr() {
+    
+    char data[2];
+    airBagCtr++;
+    if (airBagCtr == 0xff) airBagCtr = 0;
+    data[0] = airBagCtr;
+    data[1] = 0xff;
+    if (!can1.write(CANMessage(0x0D7, data, 2))) {
+        pc.printf("Cannot send AirBag counter!\n");
+    }        
 }
 
 void sendLight(char mode) {
@@ -112,26 +139,68 @@
     }
     */
     
-    /*
-    //char data_349 [] = { 0x76, 0x0F, 0xBE, 0x1A, 0x00 };
-    char data_349 [] = { 0x00, 0x00, 0xBE, 0x1A, 0x00 };
-    //data_349[0] = timer14_v;
-    //data_349[1] = timer14_v;
-    // data_349[0] = counter;
-    if(can1.write(CANMessage(0x349, data_349, 5))) {
-        printf("Fuel sent \n");
+void sendFuel() {
+    
+    char data[5];
+    data[0] = fuel1 / 256;
+    data[1] = fuel1 & 0xff;
+    data[2] = fuel2 / 256;
+    data[3] = fuel2 & 0xff;
+    data[4] = 0;
+    
+    if(!can1.write(CANMessage(0x349, data, 5))) {
+        printf("Cannot send fuel!\r\n");
     }    
-    */
+}
+
+void canLoop() {
+        
+    if (timer100.read_ms() > 100) {
+        sendT15();
+        
+        timer100.reset();
+        led1 = !led1;
+    }
+    
+    if (timer200.read_ms() > 200) {
+        sendABSCtr();
+        sendAirBagCtr();
+        sendFuel();
+        
+        timer200.reset();
+    }    
+}
+
 
 bool sendMessage(int id, char *data, int len) {
     
-    pc.printf("Sending message: %01X:%02X, ", id >> 8, id & 0xff);
+    pc.printf("Sending message: %01X%02X (%d), ", id >> 8, id & 0xff, id);
     for (int i = 0; i < len; i++) pc.printf("%02X ", data[i]);
     pc.printf("\r\n");
     
     return can1.write(CANMessage(id, data, len));
 }
 
+int readHex(char *hexstr, uint8_t *data, int len) {
+
+    int i = 0, l = 0;
+    char hexnums[128];
+    
+    while (i < len) {
+        char h = hexstr[i++];
+        if ((h < '0' || h > 'F') || (h > '9' && h < 'A')) continue;
+        if (h >= 'A') hexnums[l++] = 10 + (h - 'A');
+        else hexnums[l++] = h - '0';
+    }
+    hexnums[l] = 0; // Extra 0 for uneven pairs
+    i = 0;
+    while (i < (l+1) / 2) {
+        data[i] = (hexnums[i * 2] << 4) | hexnums[i * 2 + 1];
+        i++;
+    }
+    return (l+1) / 2;
+
+}
 
 //**********The main program*********************
 
@@ -160,24 +229,27 @@
     }
     
     else if (command[0] == 'M') {
-        int i = 1, l = 0;
-        char hexmsg[20];
-        char canmsg[8];
-        int id;
+        uint8_t hexid[4];
+        uint8_t canmsg[8];
+        int idlen;
+        
+        if (strchr(command + 1, ' ')) idlen = strchr(command + 1, ' ') - (command + 1);
+        else return;
+        readHex(command + 1, hexid, idlen);
+        int id = hexid[0] * 256 + hexid[1];
+        int l = readHex(command + 1 + idlen, canmsg, strlen(command  + 1 + idlen));
         
-        while (i < strlen(command)) {
-            char h = command[i++];
-            if (h < '0' || h > 'F') continue;
-            if (h >= 'A') hexmsg[l++] = 10 + (h - 'A');
-            else hexmsg[l++] = h - '0';
-        }
-        i = 0;
-        id = (int) (hexmsg[0] & 0x0f) << 8 + (int) (hexmsg[1] & 0x0f) << 4 + (hexmsg[2] & 0x0f);
-        while (i < (l-3) / 2) {
-            canmsg[i] = ((hexmsg[3 + i * 2] & 0x0f) << 4) | (hexmsg[3 + i * 2 + 1] & 0x0f);
-            i++;
-        }
-        sendMessage(id, canmsg, (l-3) / 2);
+        sendMessage(id, (char*) canmsg, l);
+    }
+    
+    else if (command[0] == 'F') {
+        uint8_t hexfuel[4];
+        
+        int l = readHex(command + 1, hexfuel, strlen(command + 1));
+        fuel1 = hexfuel[0] * 256 + hexfuel[1];
+        if (l>2) fuel2 = hexfuel[2] * 256 + hexfuel[3];
+        
+        pc.printf("Set fuel to %02x%02x : %02x%02x.\r\n", fuel1 / 256, fuel1 & 0xff, fuel2 / 256, fuel2 & 0xff);
     }
 }
 
@@ -185,13 +257,19 @@
 
     pc.baud(115200);
     can1.frequency(100000);
-    ticker.attach(&tick100, 0.1); //Send messages every 100 msec
-    pc.printf("T15 started\r\n");
+    
+    timer100.start();
+    timer200.start();
+    
+    pc.printf("CAN hacking started\r\n");
     
     char buffer[128];
     int bufferlen = 0;
     
     while (1) {
+        
+        canLoop();
+        
         if (pc.readable()) {
             char c = pc.getc();
             pc.putc(c); // Local ECHO