File content as of revision 7:a19b63c0a0fa:

#include "mbed.h"
#include "rtos.h"
#include "IsoTpHandler.h"
#include "MODSERIAL.h"
#include "display.h"

#include "EngineCoolantTemperature.h"
#include "OilTemperature.h"
#include "VehicleSpeed.h"
#include "Throttle.h"
#include "EngineRpm.h"

//#define CAN1_TEST
//#define CAN1_OBD_CAR_SIMULATOR

// Make TX buffer 1024bytes and RX buffer use 512bytes.
MODSERIAL pc(USBTX, USBRX, 2 * 1024, 512); // tx, rx
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

CAN can1(p9, p10);
DigitalOut can1_disable(p8);
CAN can2(p30, p29);
DigitalOut can2_disable(p28);

IsoTpHandler tpHandler(&can2);

Display display;

//#define ACTIVATE_DEBUG_OUTPUT
#ifdef ACTIVATE_DEBUG_OUTPUT
#define DEBUG_PRINT(format, ...) pc.printf(format, ##__VA_ARGS__) 
#else
#define DEBUG_PRINT(format, ...)
#endif

void led2_thread(void const *args) {
    while (true) {
        led2 = !led2;
        Thread::wait(1000);
    }
}

Mail<CANMessage, 16> can_rx_queue;

void can_process_packets(void const *args) {
    while (true) {
        //pc.printf("Th wait for can packet\r\n");
        osEvent evt = can_rx_queue.get(osWaitForever);
        //pc.printf("Got evt %d\r\n", evt.status);
        if (evt.status == osEventMail) {
            //pc.printf("Got can packet\r\n");
            CANMessage *msg = (CANMessage*) evt.value.p;
            //pc.printf("Process can packet\r\n");
            tpHandler.processCanMessage(msg);
            //pc.printf("Processed can packet\r\n");
            can_rx_queue.free(msg);
            //pc.printf("Freed can packet\r\n");
        }
    }
}

 
void can_rx_int_handler() {
    //pc.printf("can_rx_int_handler\r\n");
    CANMessage* msg = can_rx_queue.alloc();
    if (!can2.read(*msg))
    {
        //pc.printf("can_rx_int_handler no read\r\n");
        //this should not happen, because this function is called from the rx interrupt
        can_rx_queue.free(msg);
        //pc.printf("can_rx_int_handler ret 1\r\n");
        return;
    }
    if (msg->id != 0x7E8)
    {
        //no OBD message
        can_rx_queue.free(msg);
        return;
    }
    //pc.printf("can_rx_int_handler got packet\r\n");
    osStatus error_code = can_rx_queue.put(msg);
    //pc.printf("can_rx_int_handler in queue\r\n"); 
    if (error_code != osOK) {
        //pc.printf("can_rx_int_handler failed\r\n");
        //error("Putting can message into mailbox failed with code %d!", error);
    }
    
    //pc.printf("can_rx_int_handler ok\r\n");
}

Mail<CANMessage, 16> can2_tx_queue;
void can2_send_packets(void const *args) {
    DEBUG_PRINT("TX2 start\r\n");
    while (true) {
        osEvent evt = can2_tx_queue.get(osWaitForever);
        if (evt.status == osEventMail) {
            CANMessage *msg = (CANMessage*) evt.value.p;
            DEBUG_PRINT("TX2 check\r\n");
            if (can2.write(*msg))
            {
                DEBUG_PRINT("TX2 send\r\n");
                can2_tx_queue.free(msg);
                Thread::wait(150);
            }
            else
            {
                DEBUG_PRINT("TX2 wait \r\n");
                Thread::wait(150);
            }
        }
    }
}


struct behaviour_t {
    const char *rxData; //[8];
    char txData[8];
};


const char broken_message[] = {0x02, 0x01, 0x21, 0, 0, 0, 0, 0};
behaviour_t behaviour[] = 
{
    {OilTemperature::REQUEST_DATA, {0x10, 0x1F, 0x61, 0x01, 0x51, 0, 0x37, 0x01}}, //first oil temp packet
    {OilTemperature::SECOND_MESSAGE, {0x21, 0x1F, 0x61, 0x01, 0x51, 0, 0x37, 0x01}}, //second oil temp packet, TODO more pakets must be sent
    {broken_message, {0x04, 0x41, 0x21, 0, 0, 0, 0, 0}},
    {VehicleSpeed::REQUEST_DATA, {0x03, 0x41, 0x0D, 0x26, 0, 0, 0, 0}},
    {EngineRpm::REQUEST_DATA, {0x04, 0x41, 0x0C, 0x0F, 0xA2, 0, 0, 0}},
    {Throttle::REQUEST_DATA, {0x03, 0x41, 0x11, 0x26, 0, 0, 0, 0}},
    {EngineCoolantTemp::REQUEST_DATA, {0x03, 0x41, 0x05, 0x4D, 0, 0, 0, 0}},  //engine coolant temp
};

void can2_send_requests(void const *args) {
    while (true) {
        Thread::wait(2000);
        CANMessage sendMsg(0x7E0, (char*) behaviour[3].rxData, 8);
        CANMessage* msg = can2_tx_queue.alloc();
        *msg = sendMsg;
        can2_tx_queue.put(msg);
        Thread::wait(200);
        msg = can2_tx_queue.alloc();
        sendMsg.data[2] = behaviour[4].rxData[2];
        *msg = sendMsg;
        can2_tx_queue.put(msg);
        Thread::wait(200);
        sendMsg.data[2] = behaviour[5].rxData[2];
        *msg = sendMsg;
        can2_tx_queue.put(msg);
        Thread::wait(200);
        sendMsg.data[2] = behaviour[6].rxData[2];
        *msg = sendMsg;
        can2_tx_queue.put(msg);
        Thread::wait(200);
        CANMessage sendMsg2(0x7E0, (char*) behaviour[0].rxData, 8);
        msg = can2_tx_queue.alloc();
        *msg = sendMsg2;
        can2_tx_queue.put(msg);
    }
}

#ifdef CAN1_TEST
Mail<CANMessage, 16> can1_rx_queue;
CANMessage msg1;
void can1_rx_int_handler() {
    led3 = !led3;
    CANMessage* msg = can1_rx_queue.alloc();
    if (!can1.read(*msg))
    {
        can1_rx_queue.free(msg);
        return;
    }
    if ((msg->id != 0x7E8) && (msg->id != 0x7E0))
    {
        can1_rx_queue.free(msg);
        return;
    }
    can1_rx_queue.put(msg);
    led4 = !led4;
}


#ifdef CAN1_OBD_CAR_SIMULATOR

Mail<CANMessage, 16> can1_tx_queue;

void can1_obd_car_simulator_process_packet(CANMessage &msg)
{
    if (msg.id != 0x7E0)
    {
        return;
    }
    
    for (unsigned int i = 0; i < sizeof(behaviour) / sizeof (behaviour[0]); i++)
    {
        if (memcmp(msg.data, behaviour[i].rxData, 8) == 0)
        {
            CANMessage sendMsg(0x7E8, (char*) behaviour[i].txData, 8);
            CANMessage* msg = can1_tx_queue.alloc();
            *msg = sendMsg;
            can1_tx_queue.put(msg);
            
            if (i == 1)
            {
                //send additinal packets later
                sendMsg.data[0] = 0x22;
                msg = can1_tx_queue.alloc();
                *msg = sendMsg;
                can1_tx_queue.put(msg);
                sendMsg.data[0] = 0x23;
                msg = can1_tx_queue.alloc();
                *msg = sendMsg;
                can1_tx_queue.put(msg);
                sendMsg.data[0] = 0x24;
                msg = can1_tx_queue.alloc();
                *msg = sendMsg;
                can1_tx_queue.put(msg);
            }
        }
    }
}

#endif //CAN1_OBD_CAR_SIMULATOR

void can1_process_packets(void const *args) {
    while (true) {
        osEvent evt = can1_rx_queue.get(osWaitForever);
        if (evt.status == osEventMail) {
            CANMessage *msg = (CANMessage*) evt.value.p;
            
            pc.printf("\r\nRX1: '%d' '%d' '%d' '%x' '", msg->format, msg->type, msg->len, msg->id);
            for (unsigned int i = 0; i < msg->len; i++)
            {
                pc.printf("%x ", msg->data[i]);
            }
            pc.printf("'\r\n");
#ifdef CAN1_OBD_CAR_SIMULATOR
            can1_obd_car_simulator_process_packet(*msg);
#endif //CAN1_OBD_CAR_SIMULATOR            
            can1_rx_queue.free(msg);
        }
    }
}    

#ifdef CAN1_OBD_CAR_SIMULATOR
void can1_send_packets(void const *args) {
    DEBUG_PRINT("TX1 start\r\n");
    while (true) {
        osEvent evt = can1_tx_queue.get(osWaitForever);
        if (evt.status == osEventMail) {
            CANMessage *msg = (CANMessage*) evt.value.p;
            DEBUG_PRINT("TX1 check\r\n");
            if (can1.write(*msg))
            {
                DEBUG_PRINT("TX1 send\r\n");
                can1_tx_queue.free(msg);
                Thread::wait(50);
            }
            else
            {
                DEBUG_PRINT("TX1 wait \r\n");
                Thread::wait(50);
            }
        }
    }
}    
#endif //CAN1_OBD_CAR_SIMULATOR
    
#endif //CAN1_TEST

char can_msg[8] = {0};
CANMessage msg(0x7E0, can_msg, 8);
void serial_int_handler() {
    if (!pc.readable()) {
        return;
    }
    uint8_t character = pc.getc();
    //pc.printf("Received '%c'\r\n", character);
    
    msg.data[0] = 0x02;
    msg.data[1] = 0x01;
    char pid = 0;
    switch (character)
    {
        case '1':
            pid = 0x0C; //engine rpm
            break;
        case '2':
            pid = 0x11; //throttle
            break;
        case '3': //oil 1
            msg.data[1] = 0x21; //endian
            pid = 1;
            break;
        case '4': //oil 2
            msg.data[1] = 1; //endian
            pid = 0x21;
            break;
        default:
            pid = 0x05; //engine coolant temp
    }
    msg.data[2] = pid;
    msg.len = 8;
    
    //pc.printf("Sending message\r\n");
    int result = can2.write(msg);  //or 0x7DF ?
    //pc.printf("Can write %d\r\n", result);
    //pc.printf("ret 1\r\n");
 }
 
int main() {
    display.clear();
    display.sendTo("Starting...\r\n");
    display.display();
    
    pc.baud(921600);
    //pc.attach(&serial_int_handler);
    can2_disable = 0;
    can2.frequency(500000);
    //mbed can filter is not working? check it later
    //can2.filter
    can2.attach(can_rx_int_handler);
    Thread thread(led2_thread);
    Thread can_thread(can_process_packets);
#ifdef CAN1_TEST
    can1_disable = 0;
    can1.frequency(500000);
    can1.attach(&can1_rx_int_handler);
    Thread can1_thread(can1_process_packets);
#ifdef CAN1_OBD_CAR_SIMULATOR
    Thread can1_tx_thread(can1_send_packets);
#endif //CAN1_OBD_CAR_SIMULATOR    
#endif //CAN1_TEST
    Thread can2_send_request_thread(can2_send_requests);
    Thread can2_tx_thread(can2_send_packets);
    display.sendTo("Init done.\r\n");
    display.display();
    
    pc.printf("Start\r\n");
    while (true) {
        led1 = !led1;
        Thread::wait(500);
        if (pc.readable()) {
            serial_int_handler();
        }
    }
}