File content as of revision 3:4a4463380739:

#include "mbed.h"
#include "T15Msg.h"
#include "SpeedMsg.h"

Timer timer100;
Timer timer200;

Serial pc(USBTX, USBRX);
DigitalOut led1(PA_5);
CAN can1(PB_8, PB_9);  // rd, td Transmitter

T15Msg t15;
SpeedMsg speed;

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
*/


void sendT15() {
    if (!t15.sendMessage(&can1)) {
        pc.printf("Cannot send T15!\n");
    }    
/*
    char data_130 [] = { 0x45, 0x42, 0x8F, 0xE2, 0xFE };
    char data_130 [] = { 0x45, 0x40, 0x21, 0x8F, 0x00 };
*/
}

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) {

    /*
    // Light
    CAN ID-----DATA
    ===== ----====
    21A-------00 20 F7 OFF
    21A-------04 22 F7 Parking light / angel eyes
    21A-------05 22 F7 Light on
    */
    
    char *data;
    char data_21A_0 [] = { 0x00, 0x20, 0xF7 };
    char data_21A_1 [] = { 0x04, 0x22, 0xF7 };
    char data_21A_2 [] = { 0x05, 0x22, 0xF7 };

    switch (mode) {
        case 1:
            data = data_21A_1;
            break;
        case 2:
            data = data_21A_2;
            break;
        default:
            data = data_21A_0;
            break;
    }
    if (can1.write(CANMessage(0x21A, data, 3))) {
        pc.printf("Light sent %d\n", mode);
    }        
}

    //Ignition status
    /*
    0[7]: engine runinng
    1[7]: ignition || engine running
    2[7]: ignition || engine running
    2[654312]: 111111
    3: 0x50
    4-7: 0xff
    */
    
    /*
    // Ignition
    char data_26E [] = { 0x40, 0x40, 0x4F, 0x50, 0xFF, 0xFF, 0xFF, 0xFF };
    if(can1.write(CANMessage(0x26E, data_26E, 8))) {
        printf("Ignition sent \n");
    }
    */
    
    /*
    // RPM
    char data_0A5 [] = { 0xCD, 0xD5, 0xEC, 0xC7, 0x7E, 0x34, 0x0C, 0xF1 };
    if(can1.write(CANMessage(0x0A5, data_0A5, 8))) {
        printf("RPM sent \n");
    }
    */
    
    /*
    // Speed
    char data_1A6 [] = { 0x13, 0x4D, 0x46, 0x4D, 0x33, 0x4D, 0xD0, 0xFF };
    if(can1.write(CANMessage(0x1A6, data_1A6, 8))) {
        printf("Speed sent \n");
    }
    */
    
    /*
    // Door
    char data_2FC [] = { 0x81, 0x01, 0x00, 0xFF, 0xFF, 0xFF, 0xFF };
    if(can1.write(CANMessage(0x2FC, data_2FC, 7))) {
        printf("Door sent \n");
    }
    */
    
    /*
    // HB
    char data_34F [] = { 0xFE, 0xFF };
    //data_34F[0] = counter;
    if(can1.write(CANMessage(0x34F, data_34F, 2))) {
        printf("HB 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();
        speed.sendMessage(&can1);
        
        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 (%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*********************

void doCommand(char *command) {

    pc.printf("Command: %s\r\n", command);
    for (int j = 0;;j++) {
        pc.printf("%02x ", command[j]);
        if (command[j] == 0) break;
    }
    pc.printf("\r\n");
                
    if (strcmp(command, "START") == 0) {
        pc.printf("Start\r\n");
        t15.start();
        speed.setSpeed(0x13, 0x4d, 0x46, 0x4d, 0x33, 0x4d);
    }
    
    else if (strcmp(command, "STOP") == 0) {
        pc.printf("Stop\n");
        speed.setSpeed(0x0, 0x0, 0x0, 0x0, 0x0, 0x0);
        t15.stop();
    }
    
    else if (command[0] == 'L') {
        int mode = command[1] - '0';
        sendLight(mode);
    }
    
    else if (command[0] == 'M') {
        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));
        
        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);
    }
}

int main() {

    pc.baud(115200);
    can1.frequency(100000);
    
    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
            if (c == '\r') {
                buffer[bufferlen] = 0;
                doCommand(buffer);
                bufferlen = 0;
            } else {
                buffer[bufferlen++] = c;
            }
        }
    }

}