main.cpp

00001 #include "mbed.h"
00002 #include "common.h"
00003 #include "TextLCD.h"
00004 #include "SDFileSystem.h"
00005 #include "stdarg.h"
00006 #include <map>
00007 
00008 Ticker          ticker;
00009 Timer           timer;
00010 DigitalOut      led1(LED1);
00011 DigitalOut      led2(LED2);
00012 DigitalOut      led3(LED3);
00013 DigitalOut      led4(LED4);
00014 CAN             can(p30, p29);
00015 Serial          pc(USBTX, USBRX);
00016 TextLCD         lcd(p22, p23, p24, p25, p26, p27, p28);
00017 SDFileSystem    sd(p5, p6, p7, p8, "sd");
00018 LocalFileSystem local("local");
00019 
00020 #define SD_ONE    512
00021 #define SD_BLK    512
00022 #define SD_NUM    2
00023 #define PRINT_MAX 512
00024 #define PRBUF_MAX 4096
00025 #define PRLEN_MAX (PRBUF_MAX-PRINT_MAX)
00026 #define MBUF_MAX  16
00027 #define REQ_TOUT  100
00028 
00029 char  sd_rbuf[SD_BLK*SD_NUM+PRINT_MAX];
00030 char *sd_buf = sd_rbuf;
00031 char *sd_prt = sd_buf;
00032 char *sd_out = sd_buf;
00033 char *sd_end = sd_buf + SD_BLK*SD_NUM;
00034 int   sd_ofs = 0;
00035 
00036 char  pc_rbuf[PRBUF_MAX+PRINT_MAX]; // direct use of pc_buf[] cause memory bug !!
00037 char *pc_buf = pc_rbuf;             // I don't know obvious reason, but it fixes bug.
00038 char *pc_prt = pc_buf;
00039 char *pc_out = pc_buf;
00040 char *pc_end = pc_buf + PRBUF_MAX;
00041 
00042 int   len_max = 0;
00043 
00044 char  msg_Request[]  = { 0x02, 0x21, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00 }; // Request Data
00045 char  msg_Continue[] = { 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; // Continue Receiving Additional Data
00046 int   msg_ReqIdx     = 0;
00047 int   msg_ReqTimeout = 0;
00048 int   msg_ReqNumLeft = 0;
00049 bool  msg_Received30 = false;
00050 
00051 //
00052 
00053 struct CAN_Op {
00054     
00055 };
00056 
00057 struct CAN_Value {
00058     unsigned char  data[8];
00059 };
00060 
00061 std::map<int,CAN_Value>  msg_Map;
00062 
00063 struct CAN_Buffer {
00064     unsigned short id_len;  // 11 bit identifier
00065     unsigned short time;    // Timestamp
00066     unsigned char  data[8]; // Data field
00067 };
00068 
00069 CAN_Buffer               msg_Buffer[MBUF_MAX];
00070 int                      msg_Rpos = 0;
00071 int                      msg_Tpos = 0;
00072 
00073 char len7Ex[2][256] = {
00074     { /* 0x7E0
00075      0   1   2   3   4   5   6   7   8   9   A   B   C   D   E   F */
00076      4, 25,  0,  7, 12,  4, 10,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00077      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00078      4,  0,  0,  2,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00079      0,  0,  0,  3,  0,  0,  0, 13,  0,  4,  9,  0,  5,  0,  0,  0,
00080 
00081      4, 13,  0,  0,  6, 11,  0,  1,  0, 12,  0,  0,  0,  0,  0,  0,
00082      0, 30,  8,  0, 10,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00083      4,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00084      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00085 
00086      4,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00087      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00088      4, 22,  0,  0,  3,  0,  0,  6, 25,  0,  0,  0,  2,  7,  0,  4,
00089      0,  8,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00090 
00091      4, 19, 12,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,
00092      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00093      4,  8,  5,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00094      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 },
00095 
00096     { /* 0x7E2
00097      0   1   2   3   4   5   6   7   8   9   A   B   C   D   E   F */
00098      4, 22,  0,  0,  0,  0,  9,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00099      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00100      4,  5,  0,  0,  0,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00101      0,  0, 45, 45, 45, 45, 45,  0,  0,  0,  0,  0,  0,  0, 36, 32,
00102 
00103      4,  8,  0,  0,  2,  0, 32,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00104      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00105      4,  5,  5,  0,  0,  0,  0,  5,  5,  0,  0,  0,  0,  0,  0,  0,
00106      4,  4,  0,  0,  9,  4,  0,  0,  2,  4,  0,  0,  2,  3,  0,  0,
00107 
00108      4, 33,  0,  0,  0,  0,  0,  8,  0,  0,  2,  0,  0,  0,  2,  0,
00109      0,  0, 15,  0,  0, 14,  0,  0,  8,  0,  0,  4,  0,  0,  0,  0,
00110      4, 48, 30,  8,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00111      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00112 
00113      4, 19, 12,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00114      0,  6,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00115      4,  8,  5,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
00116      0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 }
00117 };
00118 
00119 /*
00120 struct reqTbl
00121 {
00122     int id;   //
00123     int code; //
00124     int freq; //
00125     int fcnt; //
00126 };
00127 
00128 reqTbl req_Table[] = {
00129     { 0x7E0, 0x00,  1, 0 },
00130     { 0x7E0, 0x01,  1, 0 },
00131     { 0x7E0, 0x03,  1, 0 },
00132     { 0x7E0, 0x04,  1, 0 },
00133     { 0x7E0, 0x05,  1, 0 },
00134     { 0x7E0, 0x06,  1, 0 },
00135 
00136     { 0x7E0, 0x20,  1, 0 },
00137     { 0x7E0, 0x23,  1, 0 },
00138     { 0x7E0, 0x24,  1, 0 },
00139     { 0x7E0, 0x25,  1, 0 },
00140     { 0x7E0, 0x26,  1, 0 },
00141 
00142     { 0x7E0, 0x33,  1, 0 },
00143     { 0x7E0, 0x37,  1, 0 },
00144     { 0x7E0, 0x39,  1, 0 },
00145     { 0x7E0, 0x3A,  1, 0 },
00146     { 0x7E0, 0x3C,  1, 0 },
00147 
00148     { 0x7E0, 0x40,  1, 0 },
00149     { 0x7E0, 0x41,  1, 0 },
00150     { 0x7E0, 0x44,  1, 0 },
00151     { 0x7E0, 0x45,  1, 0 },
00152     { 0x7E0, 0x47,  1, 0 },
00153     { 0x7E0, 0x49,  1, 0 },
00154 
00155     { 0x7E0, 0x51,  1, 0 },
00156     { 0x7E0, 0x52,  1, 0 },
00157     { 0x7E0, 0x54,  1, 0 },
00158 
00159     { 0x7E0, 0x60,  1, 0 },
00160 
00161     { 0x7E0, 0x80,  1, 0 },
00162 
00163     { 0x7E0, 0xA0,  1, 0 },
00164     { 0x7E0, 0xA1,  1, 0 },
00165     { 0x7E0, 0xA4,  1, 0 },
00166     { 0x7E0, 0xA7,  1, 0 },
00167     { 0x7E0, 0xA8,  1, 0 },
00168     { 0x7E0, 0xAC,  1, 0 },
00169     { 0x7E0, 0xAD,  1, 0 },
00170     { 0x7E0, 0xAF,  1, 0 },
00171 
00172     { 0x7E0, 0xB1,  1, 0 },
00173     { 0x7E0, 0xB2,  1, 0 },
00174 
00175     { 0x7E0, 0xC0,  1, 0 },
00176     { 0x7E0, 0xC1,  1, 0 },
00177     { 0x7E0, 0xC2,  1, 0 },
00178     { 0x7E0, 0xC9,  1, 0 },
00179 
00180     { 0x7E0, 0xE0,  1, 0 },
00181     { 0x7E0, 0xE1,  1, 0 },
00182     { 0x7E0, 0xE2,  1, 0 },
00183     { 0x7E0, 0xE3,  1, 0 },
00184 
00185     { 0x7E2, 0x00,  1, 0 },
00186     { 0x7E2, 0x01,  1, 0 },
00187     { 0x7E2, 0x06,  1, 0 },
00188 
00189     { 0x7E2, 0x20,  1, 0 },
00190     { 0x7E2, 0x21,  1, 0 },
00191     { 0x7E2, 0x25,  1, 0 },
00192     { 0x7E2, 0x26,  1, 0 },
00193 
00194     { 0x7E2, 0x32, 14,  0 },
00195     { 0x7E2, 0x33, 14, 12 },
00196     { 0x7E2, 0x34, 14, 10 },
00197     { 0x7E2, 0x35, 14,  8 },
00198     { 0x7E2, 0x36, 14,  6 },
00199     { 0x7E2, 0x3E, 14,  4 },
00200     { 0x7E2, 0x3F, 14,  2 },
00201 
00202     { 0x7E2, 0x40,  1, 0 },
00203     { 0x7E2, 0x41,  1, 0 },
00204     { 0x7E2, 0x44,  1, 0 },
00205     { 0x7E2, 0x46,  1, 0 },
00206 
00207     { 0x7E2, 0x60,  1, 0 },
00208     { 0x7E2, 0x61,  1, 0 },
00209     { 0x7E2, 0x62,  1, 0 },
00210     { 0x7E2, 0x67,  1, 0 },
00211     { 0x7E2, 0x68,  1, 0 },
00212 
00213     { 0x7E2, 0x70,  1, 0 },
00214     { 0x7E2, 0x71,  1, 0 },
00215     { 0x7E2, 0x74,  1, 0 },
00216     { 0x7E2, 0x75,  1, 0 },
00217     { 0x7E2, 0x78,  1, 0 },
00218     { 0x7E2, 0x79,  1, 0 },
00219     { 0x7E2, 0x7C,  1, 0 },
00220     { 0x7E2, 0x7D,  1, 0 },
00221 
00222     { 0x7E2, 0x80,  1, 0 },
00223     { 0x7E2, 0x81,  1, 0 },
00224     { 0x7E2, 0x87,  1, 0 },
00225     { 0x7E2, 0x8A,  1, 0 },
00226     { 0x7E2, 0x8E,  1, 0 },
00227 
00228     { 0x7E2, 0x92,  1, 0 },
00229     { 0x7E2, 0x95,  1, 0 },
00230     { 0x7E2, 0x98,  1, 0 },
00231     { 0x7E2, 0x9B,  1, 0 },
00232 
00233     { 0x7E2, 0xA0,  1, 0 },
00234     { 0x7E2, 0xA1,  1, 0 },
00235     { 0x7E2, 0xA2,  1, 0 },
00236     { 0x7E2, 0xA3,  1, 0 },
00237 
00238     { 0x7E2, 0xC0,  1, 0 },
00239     { 0x7E2, 0xC1,  1, 0 },
00240     { 0x7E2, 0xC2,  1, 0 },
00241 
00242     { 0x7E2, 0xD1,  1, 0 },
00243 
00244     { 0x7E2, 0xE0,  1, 0 },
00245     { 0x7E2, 0xE1,  1, 0 },
00246     { 0x7E2, 0xE2,  1, 0 },
00247     { 0x7E2, 0xE3,  1, 0 }
00248 };
00249 
00250 int req_NumTbl = sizeof(req_Table) / sizeof(reqTbl);
00251 */
00252 
00253 //
00254 int can_open = -1;
00255 
00256 void sd_buffold()
00257 {
00258     if(sd_prt >= sd_end) {
00259         sd_prt -= PRBUF_MAX;
00260         if(sd_prt > sd_buf) {
00261             memcpy(sd_buf, sd_end, sd_prt-sd_buf);
00262         }
00263     }
00264 }
00265 
00266 void sd_printf(char *form, ...)
00267 {
00268     va_list vl;
00269     va_start(vl, form);
00270     int len = vsnprintf(sd_prt, PRINT_MAX, form, vl);
00271     sd_prt += len;
00272     sd_buffold();
00273     va_end(vl);
00274 }
00275 
00276 void sd_print(char *str, int len)
00277 {
00278     memcpy(sd_prt, str, len);
00279     sd_prt += len;
00280     sd_buffold();
00281 }
00282 
00283 void sd_hexa(int hex, int len)
00284 {
00285     for(int i = 0; i < len; i++) {
00286         char hc = (hex >> ((len-i-1) * 4)) & 15;
00287         if(hc < 10) {
00288             *sd_prt++ = hc + '0';
00289         } else {
00290             *sd_prt++ = hc + 'A' - 10;
00291         }
00292     }
00293     sd_buffold();
00294 }
00295 
00296 //
00297 
00298 int pc_len()
00299 {
00300     int len = pc_prt - pc_out;
00301     if(len < 0) len += PRBUF_MAX;
00302 
00303     if(len > len_max) len_max = len;
00304 
00305     return len;
00306 }
00307 
00308 void pc_buffold()
00309 {
00310     if(pc_prt >= pc_end) {
00311         pc_prt -= PRBUF_MAX;
00312         if(pc_prt > pc_buf) {
00313             memcpy(pc_buf, pc_end, pc_prt-pc_buf);
00314         }
00315     }
00316 }
00317 
00318 void pc_printf(char *form, ...)
00319 {
00320     if(pc_len() >= PRLEN_MAX) return;
00321 
00322     va_list vl;
00323     va_start(vl, form);
00324     int len = vsnprintf(pc_prt, PRINT_MAX, form, vl);
00325     pc_prt += len;
00326     pc_buffold();
00327     va_end(vl);
00328 }
00329 
00330 void pc_print(char *str, int len)
00331 {
00332     if(pc_len() >= PRLEN_MAX) return;
00333 
00334     memcpy(pc_prt, str, len);
00335     pc_prt += len;
00336     pc_buffold();
00337 }
00338 
00339 void pc_hexa(int hex, int len)
00340 {
00341     if(pc_len() >= PRLEN_MAX) return;
00342 
00343     for(int i = 0; i < len; i++) {
00344         char hc = (hex >> ((len-i-1) * 4)) & 15;
00345         if(hc < 10) {
00346             *pc_prt++ = hc + '0';
00347         } else {
00348             *pc_prt++ = hc + 'A' - 10;
00349         }
00350     }
00351     pc_buffold();
00352 }
00353 
00354 unsigned int prev_us  = 0; // work
00355 unsigned int curr_us  = 0; // micro sec looped
00356 unsigned int curr_uH  = 0; // micro sec upper dword
00357 unsigned int diff_us  = 0; // erapsed us
00358 unsigned int curr_ms  = 0; // milli sec looped
00359 unsigned int left_ms  = 0; // work
00360 unsigned int curr_sc  = 0; // sec looped
00361 int          ms_1000  = 0; // 1000 looped ms
00362 int          ms_60000 = 0; // 60000 looped ms
00363 unsigned int max_diff = 0;
00364 
00365 int          send_per_sec =  0;
00366 
00367 void canchk()
00368 {
00369     CANMessage msg;
00370     int        cnt_read;
00371 
00372     // count time
00373     curr_us = timer.read_us(); // 0x00000000-0xFFFFFFFF looped
00374     diff_us = curr_us - prev_us;
00375     if(curr_us < prev_us) {
00376         curr_uH++;
00377     }
00378     left_ms += diff_us;
00379     while(left_ms >= 1000) { // timer.read_ms() not looped. it calculated from read_us() / 1000. so buggy.
00380         curr_ms++;
00381         ms_60000++;
00382         ms_1000++;
00383         if(ms_60000 >= 60000) {
00384             ms_60000 = 0;
00385         }
00386         if(ms_1000 >= 1000) {
00387             ms_1000 = 0;
00388             curr_sc++;
00389             send_per_sec = 1;
00390         }
00391         led1 = (ms_60000 / 250) & 1;
00392         left_ms -= 1000;
00393         //
00394         if(msg_ReqTimeout > 0) {
00395             msg_ReqTimeout--;
00396         }
00397     }
00398     if(diff_us > max_diff) {
00399         max_diff = diff_us;
00400     }
00401     prev_us = curr_us;
00402 
00403     // CAN receive (have only 2 receive buffers)
00404     for(cnt_read = 0; can.read(msg); cnt_read++) {
00405         if(msg.id < 0x200) {
00406             // maybe .. request ok
00407             if(-10 < msg_ReqTimeout && msg_ReqTimeout <= 0) msg_ReqTimeout--;
00408         } else if(msg.id >= 0x7E8 && msg.len == 8) {
00409             if(msg.data[0] == 0x10) {
00410                 // long response
00411                 if(msg.data[2] == 0x61 || msg.data[2] == 0xE1 || msg.data[2] == 0xE2) {
00412                     if(can.write(CANMessage(msg.id & 0x7F7, msg_Continue, sizeof(msg_Continue)))) {
00413                         msg_ReqNumLeft = msg.data[1] / 7; // num of message continue
00414                         msg_ReqTimeout = REQ_TOUT;        // continue receive
00415                         if(msg.data[2] == 0x61 && msg.data[3] == 0x01) {
00416                             led3 = !led3;
00417                         } else if(msg.data[2] == 0xE2 && msg.data[3] == 0x06) {
00418                             led3 = !led3;
00419                         }
00420                     } else {
00421                         msg_ReqNumLeft = 0; // error
00422                         msg_ReqTimeout = 0;
00423                     }
00424                 } else {
00425                     msg_ReqNumLeft = 0; // may be error
00426                     msg_ReqTimeout = 0;
00427                 }
00428             } else if(msg.data[0] > 0x20 && msg.data[0] <= 0x2F) {
00429                 // 
00430                 msg_ReqNumLeft--;
00431                 if(msg_ReqNumLeft <= 0) {
00432                     msg_ReqNumLeft = 0; // next ok
00433                     msg_ReqTimeout = 0;
00434                 } else {
00435                     msg_ReqTimeout = REQ_TOUT;
00436                 }
00437             } else if(msg.data[0] == 0x30) {
00438                 // 
00439                 msg_Received30 = true;
00440                 msg_ReqNumLeft = 0;
00441                 msg_ReqTimeout = 0;
00442             } else {
00443                 // maybe .. request error
00444                 msg_ReqNumLeft = 0;
00445                 msg_ReqTimeout = 0;
00446             }
00447         }
00448         if(msg.format == CANStandard && msg.type == CANData) {
00449             CAN_Buffer &b = msg_Buffer[msg_Rpos];
00450             b.id_len = (msg.id << 4) | msg.len;
00451             memcpy(b.data, msg.data, 8);
00452             b.time = ms_60000;
00453             if(msg_Rpos >= (MBUF_MAX-1)) {
00454                 msg_Rpos = 0;
00455             } else {
00456                 msg_Rpos++;
00457             }
00458         }
00459     }
00460     //
00461     if(msg_ReqTimeout == -10) {
00462         // send req
00463         msg_Request[2] = req_Table[msg_ReqIdx].code;
00464         if(can.write(CANMessage(req_Table[msg_ReqIdx].id, msg_Request, sizeof(msg_Request)))) {
00465             msg_ReqTimeout = REQ_TOUT;
00466             // next req
00467             do {
00468                 req_Table[msg_ReqIdx].fcnt++;
00469                 if(req_Table[msg_ReqIdx].fcnt > req_Table[msg_ReqIdx].freq) {
00470                     req_Table[msg_ReqIdx].fcnt = 0;
00471                 }
00472                 msg_ReqIdx++;
00473                 if(msg_ReqIdx > req_NumTbl) {
00474                     msg_ReqIdx = 0;
00475                 }
00476             } while(req_Table[msg_ReqIdx].fcnt != 0);
00477         }
00478     }
00479     // Error check
00480     int rerr = can.rderror();
00481     int werr = can.tderror();
00482     if(rerr > 0 || werr > 0) {
00483         can.reset();
00484         led2 = !led2;
00485     }
00486 }
00487 
00488 int main() {
00489     char  log_file[32];
00490     char *ini_file = "/local/CANUSB30.ini";
00491     //
00492     lcd.printf("mCANUSB ver 1.00"
00493                "= for PRIUS 30 =");
00494 
00495     FILE *fpINI = fopen(ini_file, "r");
00496     int c;
00497     int mode = 0;
00498     int num  = 0;
00499     int rdx  = 10;
00500     int logn = 0;
00501     if(fpINI) {
00502         while((c = fgetc(fpINI)) != EOF) {
00503             if(c < 0x20) {
00504                 if(mode == 'l') {
00505                     logn = num;
00506                 }
00507                 mode = 0;
00508                 num  = 0;
00509                 rdx  = 10;
00510             } else {
00511                 if(c >= '0' && c <= '9') {
00512                     num = num * rdx + c - '0';
00513                 } else if(c >= 'A' && c <= 'F') {
00514                     num = num * 16 + c - 'A' + 10;
00515                 } else if(c >= 'a' && c <= 'f') {
00516                     num = num * 16 + c - 'a' + 10;
00517                 } else if(c == 'x') {
00518                     rdx = 16;
00519                 } else if(c == 'l') {
00520                     mode = 'l';
00521                 }
00522             }
00523         }
00524         fclose(fpINI);
00525     }
00526     logn++;
00527     sprintf(log_file, "/sd/CAN%05d.log", logn);
00528 
00529     // save INI
00530     fpINI = fopen(ini_file, "w");
00531     if(fpINI) {
00532         fprintf(fpINI, "l%d\r\n", logn);
00533         fclose(fpINI);
00534     }
00535 
00536     // try SDCard open
00537     FILE *fpSD = fopen(log_file, "w");
00538     if(fpSD) {
00539         fclose(fpSD); // begins from empty
00540     } else {
00541         lcd.cls();
00542         lcd.printf("! SD CARD FAIL !%s", log_file);
00543     }
00544 
00545     // Init
00546     can.frequency(500000);
00547     pc.baud(921600);
00548     timer.start();
00549 
00550     // PC command
00551     char cmds[1+3+1+16];
00552     int  cmdp =  0;
00553 
00554     //__disable_irq();
00555     //__enable_irq();
00556 
00557     // CAN
00558     prev_us = timer.read_us();
00559     ticker.attach_us(&canchk, 100);
00560 
00561     while(1) {
00562         if(msg_Rpos != msg_Tpos) {
00563             if(can_open == 3) {
00564                 // Compress mode
00565                 char tbuf[17]; // "b1----2--------\r";
00566                 int  ln,n;
00567                 tbuf[0] = 'b';
00568                 tbuf[1] = 0x80;
00569                 tbuf[2] = (msg_Buffer[msg_Tpos].time   >> 8) & 0xFF;
00570                 tbuf[3] =  msg_Buffer[msg_Tpos].time         & 0xFF;
00571                 tbuf[4] = (msg_Buffer[msg_Tpos].id_len >> 8) & 0xFF;
00572                 tbuf[5] =  msg_Buffer[msg_Tpos].id_len       & 0xFF;
00573                 tbuf[6] = 0x80;
00574                 ln =  msg_Buffer[msg_Tpos].id_len & 0xF;
00575                 for(n = 0; n < ln; n++) {
00576                     tbuf[7+n] = msg_Buffer[msg_Tpos].data[n];
00577                 }
00578                 tbuf[7+ln] = '\r';
00579                 tbuf[8+ln] = 0;
00580                 // encode data
00581                 for(n = 0; n < ln; n++) {
00582                     if((tbuf[7+n] & 0x80) == 0) {
00583                         tbuf[7+n] ^= 0x80;
00584                         tbuf[6] |= (1 << n);
00585                     }
00586                 }
00587                 for(n = 0; n < 5; n++) {
00588                     if((tbuf[2+n] & 0x80) == 0) {
00589                         tbuf[2+n] ^= 0x80;
00590                         tbuf[1] |= (1 << n);
00591                     }
00592                 }
00593                 pc_print(tbuf, 8+ln);
00594             } else if(can_open > 0) {
00595                 // CANUSB compatible
00596                 pc_print("t", 1);
00597                 pc_hexa(msg_Buffer[msg_Tpos].id_len, 4);
00598                 int ln =  msg_Buffer[msg_Tpos].id_len & 0xF;
00599                 for(int n = 0; n < ln; n++) {
00600                     pc_hexa(msg_Buffer[msg_Tpos].data[n], 2);
00601                 }
00602                 if(can_open > 1) {
00603                     pc_hexa(msg_Buffer[msg_Tpos].time, 4);
00604                 }
00605                 pc_print("\r", 1);
00606             }
00607 
00608 #if 0
00609             if(fpSD) {
00610                 // CANUSB compatible for SDCard
00611                 sd_print("t", 1);
00612                 sd_hexa(msg_Buffer[msg_Tpos].id_len, 4);
00613                 int ln =  msg_Buffer[msg_Tpos].id_len & 0xF;
00614                 for(int n = 0; n < ln; n++) {
00615                     sd_hexa(msg_Buffer[msg_Tpos].data[n], 2);
00616                 }
00617                 sd_hexa(msg_Buffer[msg_Tpos].time, 4);
00618                 sd_print("\r\n", 1);
00619             }
00620 #endif
00621 
00622             if(msg_Tpos >= (MBUF_MAX-1)) {
00623                 msg_Tpos = 0;
00624             } else {
00625                 msg_Tpos++;
00626             }
00627 
00628         } else if(pc.readable() > 0) {
00629             // command check
00630             char c = pc.getc();
00631             if(c == 0x0D) {
00632                 if(cmdp > 0) {
00633                     switch(cmds[0]) {
00634                         case 'V':
00635                             pc_print("V0521\r", 6);
00636                             break;
00637                         case 'N':
00638                             pc_print("NMBED\r", 6);
00639                             break;
00640                         case 'F':
00641                             pc_print("F00\r", 4);
00642                             break;
00643                         case 'O':
00644                             if(can_open < 0) can_open = -can_open;
00645                             pc_print("\r", 1);
00646                             led4 = 1;
00647                             break;
00648                         case 'C':
00649                             if(can_open > 0) can_open = -can_open;
00650                             pc_print("\r", 1);
00651                             led4 = 0;
00652                             max_diff = 0;
00653                             break;
00654                         case 's':
00655                         case 'S':
00656                         case 't':
00657                         case 'T':
00658                         case 'm':
00659                         case 'M':
00660                             pc_print("\r", 1);
00661                             break;
00662                         case 'Z':
00663                             if(cmdp > 1) {
00664                                 if(cmds[1] == '2') {
00665                                     can_open = -3;
00666                                 } else if(cmds[1] == '1') {
00667                                     can_open = -2;
00668                                 } else {
00669                                     can_open = -1;
00670                                 }
00671                             }
00672                             pc_print("\r", 1);
00673                             break;
00674                     }
00675                 }
00676                 cmdp = 0;
00677             } else if(cmdp < 21) {
00678                 cmds[cmdp++] = c;
00679             }
00680 
00681         } else if(pc_out != pc_prt && pc.writeable() > 0) {
00682 
00683             // send data to PC
00684             while(pc_out != pc_prt && pc.writeable() > 0) {
00685                 pc.putc(*pc_out++);
00686                 if(pc_out >= pc_end) pc_out = pc_buf;
00687             }
00688 
00689         } else if(sd_prt < sd_out || (sd_prt - sd_out) >= SD_BLK) {
00690 
00691             if(fpSD) {
00692                 fpSD = fopen(log_file, "a");
00693                 if(fpSD) {
00694                     int len = SD_BLK - sd_ofs;
00695                     while(len > 0 && fpSD) {
00696                         int olen = (len > SD_ONE) ? SD_ONE : len;
00697                         if(fwrite(sd_out+sd_ofs, 1, olen, fpSD) == olen) {
00698                             sd_ofs += olen;
00699                             len -= olen;
00700                             led4 = !led4;
00701                         } else {
00702                             fclose(fpSD);
00703                             fpSD = NULL; // if failed, stop logging
00704                         }
00705                     }
00706                     if(fpSD) fclose(fpSD);
00707                 } else {
00708                     fpSD = NULL;
00709                 }
00710             }
00711             sd_out += SD_BLK;
00712             sd_ofs  = 0;
00713             if(sd_out >= sd_end) sd_out = sd_buf;
00714 
00715         } else if(send_per_sec > 0) {
00716 
00717             if(fpSD && sd_prt != (sd_out + sd_ofs)) {
00718                 fpSD = fopen(log_file, "a");
00719                 if(fpSD) {
00720                     int len = sd_prt - (sd_out + sd_ofs);
00721                     while(len > 0 && fpSD) {
00722                         int olen = (len > SD_ONE) ? SD_ONE : len;
00723                         if(fwrite(sd_out+sd_ofs, 1, olen, fpSD) == olen) {
00724                             sd_ofs += olen;
00725                             len -= olen;
00726                             led4 = 1;
00727                         } else {
00728                             fclose(fpSD);
00729                             fpSD = NULL; // if failed, stop logging
00730                         }
00731                     }
00732                     if(fpSD) fclose(fpSD);
00733                 } else {
00734                     fpSD = NULL;
00735                 }
00736             }
00737 
00738             lcd.locate(0,0);
00739             lcd.printf("%08X--------", curr_us);
00740             send_per_sec--;
00741 
00742         }
00743     }
00744 }