main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "IsoTpHandler.h"
00004 #include "MODSERIAL.h"
00005 #include "display.h"
00006 
00007 #include "EngineCoolantTemperature.h"
00008 #include "OilTemperature.h"
00009 #include "VehicleSpeed.h"
00010 #include "Throttle.h"
00011 #include "EngineRpm.h"
00012 
00013 //#define CAN1_TEST
00014 //#define CAN1_OBD_CAR_SIMULATOR
00015 
00016 // Make TX buffer 1024bytes and RX buffer use 512bytes.
00017 MODSERIAL pc(USBTX, USBRX, 2 * 1024, 512); // tx, rx
00018 DigitalOut led1(LED1);
00019 DigitalOut led2(LED2);
00020 DigitalOut led3(LED3);
00021 DigitalOut led4(LED4);
00022 
00023 CAN can1(p9, p10);
00024 DigitalOut can1_disable(p8);
00025 CAN can2(p30, p29);
00026 DigitalOut can2_disable(p28);
00027 
00028 IsoTpHandler tpHandler(&can2);
00029 
00030 Display display;
00031 
00032 //#define ACTIVATE_DEBUG_OUTPUT
00033 #ifdef ACTIVATE_DEBUG_OUTPUT
00034 #define DEBUG_PRINT(format, ...) pc.printf(format, ##__VA_ARGS__) 
00035 #else
00036 #define DEBUG_PRINT(format, ...)
00037 #endif
00038 
00039 void led2_thread(void const *args) {
00040     while (true) {
00041         led2 = !led2;
00042         Thread::wait(1000);
00043     }
00044 }
00045 
00046 Mail<CANMessage, 16> can_rx_queue;
00047 
00048 void can_process_packets(void const *args) {
00049     while (true) {
00050         //pc.printf("Th wait for can packet\r\n");
00051         osEvent evt = can_rx_queue.get(osWaitForever);
00052         //pc.printf("Got evt %d\r\n", evt.status);
00053         if (evt.status == osEventMail) {
00054             //pc.printf("Got can packet\r\n");
00055             CANMessage *msg = (CANMessage*) evt.value.p;
00056             //pc.printf("Process can packet\r\n");
00057             tpHandler.processCanMessage(msg);
00058             //pc.printf("Processed can packet\r\n");
00059             can_rx_queue.free(msg);
00060             //pc.printf("Freed can packet\r\n");
00061         }
00062     }
00063 }
00064 
00065  
00066 void can_rx_int_handler() {
00067     //pc.printf("can_rx_int_handler\r\n");
00068     CANMessage* msg = can_rx_queue.alloc();
00069     if (!can2.read(*msg))
00070     {
00071         //pc.printf("can_rx_int_handler no read\r\n");
00072         //this should not happen, because this function is called from the rx interrupt
00073         can_rx_queue.free(msg);
00074         //pc.printf("can_rx_int_handler ret 1\r\n");
00075         return;
00076     }
00077     if (msg->id != 0x7E8)
00078     {
00079         //no OBD message
00080         can_rx_queue.free(msg);
00081         return;
00082     }
00083     //pc.printf("can_rx_int_handler got packet\r\n");
00084     osStatus error_code = can_rx_queue.put(msg);
00085     //pc.printf("can_rx_int_handler in queue\r\n"); 
00086     if (error_code != osOK) {
00087         //pc.printf("can_rx_int_handler failed\r\n");
00088         //error("Putting can message into mailbox failed with code %d!", error);
00089     }
00090     
00091     //pc.printf("can_rx_int_handler ok\r\n");
00092 }
00093 
00094 Mail<CANMessage, 16> can2_tx_queue;
00095 void can2_send_packets(void const *args) {
00096     DEBUG_PRINT("TX2 start\r\n");
00097     while (true) {
00098         osEvent evt = can2_tx_queue.get(osWaitForever);
00099         if (evt.status == osEventMail) {
00100             CANMessage *msg = (CANMessage*) evt.value.p;
00101             DEBUG_PRINT("TX2 check\r\n");
00102             if (can2.write(*msg))
00103             {
00104                 DEBUG_PRINT("TX2 send\r\n");
00105                 can2_tx_queue.free(msg);
00106                 Thread::wait(150);
00107             }
00108             else
00109             {
00110                 DEBUG_PRINT("TX2 wait \r\n");
00111                 Thread::wait(150);
00112             }
00113         }
00114     }
00115 }
00116 
00117 
00118 struct behaviour_t {
00119     const char *rxData; //[8];
00120     char txData[8];
00121 };
00122 
00123 
00124 const char broken_message[] = {0x02, 0x01, 0x21, 0, 0, 0, 0, 0};
00125 behaviour_t behaviour[] = 
00126 {
00127     {OilTemperature::REQUEST_DATA, {0x10, 0x1F, 0x61, 0x01, 0x51, 0, 0x37, 0x01}}, //first oil temp packet
00128     {OilTemperature::SECOND_MESSAGE, {0x21, 0x1F, 0x61, 0x01, 0x51, 0, 0x37, 0x01}}, //second oil temp packet, TODO more pakets must be sent
00129     {broken_message, {0x04, 0x41, 0x21, 0, 0, 0, 0, 0}},
00130     {VehicleSpeed::REQUEST_DATA, {0x03, 0x41, 0x0D, 0x26, 0, 0, 0, 0}},
00131     {EngineRpm::REQUEST_DATA, {0x04, 0x41, 0x0C, 0x0F, 0xA2, 0, 0, 0}},
00132     {Throttle::REQUEST_DATA, {0x03, 0x41, 0x11, 0x26, 0, 0, 0, 0}},
00133     {EngineCoolantTemp::REQUEST_DATA, {0x03, 0x41, 0x05, 0x4D, 0, 0, 0, 0}},  //engine coolant temp
00134 };
00135 
00136 void can2_send_requests(void const *args) {
00137     while (true) {
00138         Thread::wait(2000);
00139         CANMessage sendMsg(0x7E0, (char*) behaviour[3].rxData, 8);
00140         CANMessage* msg = can2_tx_queue.alloc();
00141         *msg = sendMsg;
00142         can2_tx_queue.put(msg);
00143         Thread::wait(200);
00144         msg = can2_tx_queue.alloc();
00145         sendMsg.data[2] = behaviour[4].rxData[2];
00146         *msg = sendMsg;
00147         can2_tx_queue.put(msg);
00148         Thread::wait(200);
00149         sendMsg.data[2] = behaviour[5].rxData[2];
00150         *msg = sendMsg;
00151         can2_tx_queue.put(msg);
00152         Thread::wait(200);
00153         sendMsg.data[2] = behaviour[6].rxData[2];
00154         *msg = sendMsg;
00155         can2_tx_queue.put(msg);
00156         Thread::wait(200);
00157         CANMessage sendMsg2(0x7E0, (char*) behaviour[0].rxData, 8);
00158         msg = can2_tx_queue.alloc();
00159         *msg = sendMsg2;
00160         can2_tx_queue.put(msg);
00161     }
00162 }
00163 
00164 #ifdef CAN1_TEST
00165 Mail<CANMessage, 16> can1_rx_queue;
00166 CANMessage msg1;
00167 void can1_rx_int_handler() {
00168     led3 = !led3;
00169     CANMessage* msg = can1_rx_queue.alloc();
00170     if (!can1.read(*msg))
00171     {
00172         can1_rx_queue.free(msg);
00173         return;
00174     }
00175     if ((msg->id != 0x7E8) && (msg->id != 0x7E0))
00176     {
00177         can1_rx_queue.free(msg);
00178         return;
00179     }
00180     can1_rx_queue.put(msg);
00181     led4 = !led4;
00182 }
00183 
00184 
00185 #ifdef CAN1_OBD_CAR_SIMULATOR
00186 
00187 Mail<CANMessage, 16> can1_tx_queue;
00188 
00189 void can1_obd_car_simulator_process_packet(CANMessage &msg)
00190 {
00191     if (msg.id != 0x7E0)
00192     {
00193         return;
00194     }
00195     
00196     for (unsigned int i = 0; i < sizeof(behaviour) / sizeof (behaviour[0]); i++)
00197     {
00198         if (memcmp(msg.data, behaviour[i].rxData, 8) == 0)
00199         {
00200             CANMessage sendMsg(0x7E8, (char*) behaviour[i].txData, 8);
00201             CANMessage* msg = can1_tx_queue.alloc();
00202             *msg = sendMsg;
00203             can1_tx_queue.put(msg);
00204             
00205             if (i == 1)
00206             {
00207                 //send additinal packets later
00208                 sendMsg.data[0] = 0x22;
00209                 msg = can1_tx_queue.alloc();
00210                 *msg = sendMsg;
00211                 can1_tx_queue.put(msg);
00212                 sendMsg.data[0] = 0x23;
00213                 msg = can1_tx_queue.alloc();
00214                 *msg = sendMsg;
00215                 can1_tx_queue.put(msg);
00216                 sendMsg.data[0] = 0x24;
00217                 msg = can1_tx_queue.alloc();
00218                 *msg = sendMsg;
00219                 can1_tx_queue.put(msg);
00220             }
00221         }
00222     }
00223 }
00224 
00225 #endif //CAN1_OBD_CAR_SIMULATOR
00226 
00227 void can1_process_packets(void const *args) {
00228     while (true) {
00229         osEvent evt = can1_rx_queue.get(osWaitForever);
00230         if (evt.status == osEventMail) {
00231             CANMessage *msg = (CANMessage*) evt.value.p;
00232             
00233             pc.printf("\r\nRX1: '%d' '%d' '%d' '%x' '", msg->format, msg->type, msg->len, msg->id);
00234             for (unsigned int i = 0; i < msg->len; i++)
00235             {
00236                 pc.printf("%x ", msg->data[i]);
00237             }
00238             pc.printf("'\r\n");
00239 #ifdef CAN1_OBD_CAR_SIMULATOR
00240             can1_obd_car_simulator_process_packet(*msg);
00241 #endif //CAN1_OBD_CAR_SIMULATOR            
00242             can1_rx_queue.free(msg);
00243         }
00244     }
00245 }    
00246 
00247 #ifdef CAN1_OBD_CAR_SIMULATOR
00248 void can1_send_packets(void const *args) {
00249     DEBUG_PRINT("TX1 start\r\n");
00250     while (true) {
00251         osEvent evt = can1_tx_queue.get(osWaitForever);
00252         if (evt.status == osEventMail) {
00253             CANMessage *msg = (CANMessage*) evt.value.p;
00254             DEBUG_PRINT("TX1 check\r\n");
00255             if (can1.write(*msg))
00256             {
00257                 DEBUG_PRINT("TX1 send\r\n");
00258                 can1_tx_queue.free(msg);
00259                 Thread::wait(50);
00260             }
00261             else
00262             {
00263                 DEBUG_PRINT("TX1 wait \r\n");
00264                 Thread::wait(50);
00265             }
00266         }
00267     }
00268 }    
00269 #endif //CAN1_OBD_CAR_SIMULATOR
00270     
00271 #endif //CAN1_TEST
00272 
00273 char can_msg[8] = {0};
00274 CANMessage msg(0x7E0, can_msg, 8);
00275 void serial_int_handler() {
00276     if (!pc.readable()) {
00277         return;
00278     }
00279     uint8_t character = pc.getc();
00280     //pc.printf("Received '%c'\r\n", character);
00281     
00282     msg.data[0] = 0x02;
00283     msg.data[1] = 0x01;
00284     char pid = 0;
00285     switch (character)
00286     {
00287         case '1':
00288             pid = 0x0C; //engine rpm
00289             break;
00290         case '2':
00291             pid = 0x11; //throttle
00292             break;
00293         case '3': //oil 1
00294             msg.data[1] = 0x21; //endian
00295             pid = 1;
00296             break;
00297         case '4': //oil 2
00298             msg.data[1] = 1; //endian
00299             pid = 0x21;
00300             break;
00301         default:
00302             pid = 0x05; //engine coolant temp
00303     }
00304     msg.data[2] = pid;
00305     msg.len = 8;
00306     
00307     //pc.printf("Sending message\r\n");
00308     int result = can2.write(msg);  //or 0x7DF ?
00309     //pc.printf("Can write %d\r\n", result);
00310     //pc.printf("ret 1\r\n");
00311  }
00312  
00313 int main() {
00314     display.clear();
00315     display.sendTo("Starting...\r\n");
00316     display.display();
00317     
00318     pc.baud(921600);
00319     //pc.attach(&serial_int_handler);
00320     can2_disable = 0;
00321     can2.frequency(500000);
00322     //mbed can filter is not working? check it later
00323     //can2.filter
00324     can2.attach(can_rx_int_handler);
00325     Thread thread(led2_thread);
00326     Thread can_thread(can_process_packets);
00327 #ifdef CAN1_TEST
00328     can1_disable = 0;
00329     can1.frequency(500000);
00330     can1.attach(&can1_rx_int_handler);
00331     Thread can1_thread(can1_process_packets);
00332 #ifdef CAN1_OBD_CAR_SIMULATOR
00333     Thread can1_tx_thread(can1_send_packets);
00334 #endif //CAN1_OBD_CAR_SIMULATOR    
00335 #endif //CAN1_TEST
00336     Thread can2_send_request_thread(can2_send_requests);
00337     Thread can2_tx_thread(can2_send_packets);
00338     display.sendTo("Init done.\r\n");
00339     display.display();
00340     
00341     pc.printf("Start\r\n");
00342     while (true) {
00343         led1 = !led1;
00344         Thread::wait(500);
00345         if (pc.readable()) {
00346             serial_int_handler();
00347         }
00348     }
00349 }