main.cpp

00001 #include "mbed.h" //5.4.7 (144)
00002 #include "canid.hpp"
00003 
00004 #define BIG_ENDIAN
00005 
00006 const PinName can1rxPins[] = {PA_11};
00007 const PinName can1txPins[] = {PA_12};
00008 const PinName can2rxPins[] = {PB_5};
00009 const PinName can2txPins[] = {PB_6};
00010 
00011 uint64_t counter=0;
00012 
00013 //CAN1
00014 //0,0=OK 0,1=OK 0,2=OK 1,0=OK 1,1=OK 1,2=OK 2,0=OK 2,1=OK 2,2=OK
00015 //CAN"
00016 //0,0=RX_OK 0,1=OK 1,0=RX_OK 1,1=OK
00017 
00018 CAN can1(can1rxPins[0], can1txPins[0]);
00019 CAN can(can2rxPins[0], can2txPins[0]);
00020 
00021   //UART1, Tx, Rx (Debug)
00022 DigitalOut led1(LED1);
00023 
00024 
00025 Thread sendThread(osPriorityAboveNormal, 2048);
00026 Thread canrx;
00027 CANMessage messageIn;
00028 CANMessage messageOut;
00029 
00030 void setID (CANMessage& message, uint32_t device_id, uint32_t content_id) {
00031     uint32_t id = device_id;
00032     id |= (uint32_t)content_id << 8;
00033     id |= 0x80000000;               // Set EFF(Extended Frame Format) flag true
00034 
00035     message.id = id;
00036     
00037 }
00038 
00039 uint64_t cantoi(CANMessage message) {
00040     uint64_t ret=0;
00041     
00042     #ifdef BIG_ENDIAN
00043     for(int i=message.len-1, k=0; i>=0; i--, k++) {
00044         ret += message.data[i] << (8*k); 
00045     }
00046     #else //LITTLE ENDIAN
00047     for(int i=0, k=0; i>=message.len-1; i++, k++) {
00048             ret += message.data[i] << (8*k); 
00049     }
00050     #endif
00051     
00052     
00053     return ret;
00054     //pose = message.data[0] + (message.data[1] << 8) + (message.data[2] << 16) + (message.data[3] << 24);
00055 }
00056 
00057 template <class data_t> void xtocan(data_t _data, unsigned int n_bytes, CANMessage& message) {
00058     data_t data = 0;
00059     message.len = n_bytes;
00060     int mask = 0xff;    // last 8 bits
00061     
00062     #ifdef BIG_ENDIAN
00063     for(int i=message.len-1; i>=0; i--) {
00064         message.data[i] = data & mask;
00065         data = data>>8; 
00066     }
00067     #else //LITTLE ENDIAN
00068      for(int i=0; i<message.len; i++) {
00069         message.data[i] = data & maks;
00070         data = data>>8; 
00071     }
00072     #endif
00073     
00074     return;
00075 }
00076 
00077 /* DEPRECATED
00078 void ui64tocan(uint64_t data, CANMessage& message) {
00079     
00080     for(int i=message.len-1; i>=0; i--) {
00081         //printf("dataconv:\t% 12llu\n\r", data);
00082         message.data[i] = data & 0x00000000000000ff;
00083         data = data>>8; 
00084     }
00085 }*/
00086 
00087 void canRxIsr()
00088 {
00089     while(1) {
00090         if(can.read(messageIn)) {
00091             led1 = !led1;
00092             printf("received from %lu\t%llu\n\r", messageIn.id, cantoi(messageIn));
00093         }
00094     }
00095 }
00096 
00097 //pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
00098 
00099 void sendMessage()
00100 {
00101     int status = can.write(messageOut);
00102     printf("Send status: %d\r\n", status);
00103 }
00104 
00105 void sendMessageLoop()
00106 {
00107     while (true) {
00108         counter += 1;
00109         
00110         messageOut.id = 1000;  // DELETEME
00111         setID(messageOut, 1, BLDC_SET_RPM);
00112         messageOut.format = CANExtended;
00113         messageOut.data[0]=25;
00114         messageOut.len = 8;
00115     
00116         xtocan<uint64_t>(counter, 8, messageOut);
00117         //ui64tocan(counter, messageOut);
00118         printf("sending couter: %llu\t| CAN data: %llu\n\r", counter, cantoi(messageOut));
00119         sendMessage();
00120         osDelay(1000);
00121     }
00122 }
00123 
00124 int main()
00125 {
00126     
00127     printf("CAN receive / transmit test.\r\n");
00128     can.frequency(500000);
00129 
00130     messageOut.id = 1000;
00131     messageOut.format = CANExtended;
00132     messageOut.data[0]=24;
00133     messageOut.len = 8;
00134 
00135     canrx.start(canRxIsr);
00136       printf("CAN receive / .\r\n");
00137     sendThread.start(&sendMessageLoop);
00138       printf("CAN \r\n");
00139 
00140     while (true)
00141     {
00142         
00143         led1 = 0;
00144         osDelay(1000);
00145     }
00146 }