Diff: main.cpp

Revision:

0:050fdb1efb7a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 10 12:21:50 2011 +0000
@@ -0,0 +1,744 @@
+#include "mbed.h"
+#include "common.h"
+#include "TextLCD.h"
+#include "SDFileSystem.h"
+#include "stdarg.h"
+#include <map>
+
+Ticker          ticker;
+Timer           timer;
+DigitalOut      led1(LED1);
+DigitalOut      led2(LED2);
+DigitalOut      led3(LED3);
+DigitalOut      led4(LED4);
+CAN             can(p30, p29);
+Serial          pc(USBTX, USBRX);
+TextLCD         lcd(p22, p23, p24, p25, p26, p27, p28);
+SDFileSystem    sd(p5, p6, p7, p8, "sd");
+LocalFileSystem local("local");
+
+#define SD_ONE    512
+#define SD_BLK    512
+#define SD_NUM    2
+#define PRINT_MAX 512
+#define PRBUF_MAX 4096
+#define PRLEN_MAX (PRBUF_MAX-PRINT_MAX)
+#define MBUF_MAX  16
+#define REQ_TOUT  100
+
+char  sd_rbuf[SD_BLK*SD_NUM+PRINT_MAX];
+char *sd_buf = sd_rbuf;
+char *sd_prt = sd_buf;
+char *sd_out = sd_buf;
+char *sd_end = sd_buf + SD_BLK*SD_NUM;
+int   sd_ofs = 0;
+
+char  pc_rbuf[PRBUF_MAX+PRINT_MAX]; // direct use of pc_buf[] cause memory bug !!
+char *pc_buf = pc_rbuf;             // I don't know obvious reason, but it fixes bug.
+char *pc_prt = pc_buf;
+char *pc_out = pc_buf;
+char *pc_end = pc_buf + PRBUF_MAX;
+
+int   len_max = 0;
+
+char  msg_Request[]  = { 0x02, 0x21, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00 }; // Request Data
+char  msg_Continue[] = { 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; // Continue Receiving Additional Data
+int   msg_ReqIdx     = 0;
+int   msg_ReqTimeout = 0;
+int   msg_ReqNumLeft = 0;
+bool  msg_Received30 = false;
+
+//
+
+struct CAN_Op {
+    
+};
+
+struct CAN_Value {
+    unsigned char  data[8];
+};
+
+std::map<int,CAN_Value>  msg_Map;
+
+struct CAN_Buffer {
+    unsigned short id_len;  // 11 bit identifier
+    unsigned short time;    // Timestamp
+    unsigned char  data[8]; // Data field
+};
+
+CAN_Buffer               msg_Buffer[MBUF_MAX];
+int                      msg_Rpos = 0;
+int                      msg_Tpos = 0;
+
+char len7Ex[2][256] = {
+    { /* 0x7E0
+     0   1   2   3   4   5   6   7   8   9   A   B   C   D   E   F */
+     4, 25,  0,  7, 12,  4, 10,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     4,  0,  0,  2,  1,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  3,  0,  0,  0, 13,  0,  4,  9,  0,  5,  0,  0,  0,
+
+     4, 13,  0,  0,  6, 11,  0,  1,  0, 12,  0,  0,  0,  0,  0,  0,
+     0, 30,  8,  0, 10,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     4,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+
+     4,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     4, 22,  0,  0,  3,  0,  0,  6, 25,  0,  0,  0,  2,  7,  0,  4,
+     0,  8,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+
+     4, 19, 12,  0,  0,  0,  0,  0,  0,  1,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     4,  8,  5,  2,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 },
+
+    { /* 0x7E2
+     0   1   2   3   4   5   6   7   8   9   A   B   C   D   E   F */
+     4, 22,  0,  0,  0,  0,  9,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     4,  5,  0,  0,  0,  1,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0, 45, 45, 45, 45, 45,  0,  0,  0,  0,  0,  0,  0, 36, 32,
+
+     4,  8,  0,  0,  2,  0, 32,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     4,  5,  5,  0,  0,  0,  0,  5,  5,  0,  0,  0,  0,  0,  0,  0,
+     4,  4,  0,  0,  9,  4,  0,  0,  2,  4,  0,  0,  2,  3,  0,  0,
+
+     4, 33,  0,  0,  0,  0,  0,  8,  0,  0,  2,  0,  0,  0,  2,  0,
+     0,  0, 15,  0,  0, 14,  0,  0,  8,  0,  0,  4,  0,  0,  0,  0,
+     4, 48, 30,  8,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+
+     4, 19, 12,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  6,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     4,  8,  5,  1,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
+     0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0 }
+};
+
+/*
+struct reqTbl
+{
+    int id;   //
+    int code; //
+    int freq; //
+    int fcnt; //
+};
+
+reqTbl req_Table[] = {
+    { 0x7E0, 0x00,  1, 0 },
+    { 0x7E0, 0x01,  1, 0 },
+    { 0x7E0, 0x03,  1, 0 },
+    { 0x7E0, 0x04,  1, 0 },
+    { 0x7E0, 0x05,  1, 0 },
+    { 0x7E0, 0x06,  1, 0 },
+
+    { 0x7E0, 0x20,  1, 0 },
+    { 0x7E0, 0x23,  1, 0 },
+    { 0x7E0, 0x24,  1, 0 },
+    { 0x7E0, 0x25,  1, 0 },
+    { 0x7E0, 0x26,  1, 0 },
+
+    { 0x7E0, 0x33,  1, 0 },
+    { 0x7E0, 0x37,  1, 0 },
+    { 0x7E0, 0x39,  1, 0 },
+    { 0x7E0, 0x3A,  1, 0 },
+    { 0x7E0, 0x3C,  1, 0 },
+
+    { 0x7E0, 0x40,  1, 0 },
+    { 0x7E0, 0x41,  1, 0 },
+    { 0x7E0, 0x44,  1, 0 },
+    { 0x7E0, 0x45,  1, 0 },
+    { 0x7E0, 0x47,  1, 0 },
+    { 0x7E0, 0x49,  1, 0 },
+
+    { 0x7E0, 0x51,  1, 0 },
+    { 0x7E0, 0x52,  1, 0 },
+    { 0x7E0, 0x54,  1, 0 },
+
+    { 0x7E0, 0x60,  1, 0 },
+
+    { 0x7E0, 0x80,  1, 0 },
+
+    { 0x7E0, 0xA0,  1, 0 },
+    { 0x7E0, 0xA1,  1, 0 },
+    { 0x7E0, 0xA4,  1, 0 },
+    { 0x7E0, 0xA7,  1, 0 },
+    { 0x7E0, 0xA8,  1, 0 },
+    { 0x7E0, 0xAC,  1, 0 },
+    { 0x7E0, 0xAD,  1, 0 },
+    { 0x7E0, 0xAF,  1, 0 },
+
+    { 0x7E0, 0xB1,  1, 0 },
+    { 0x7E0, 0xB2,  1, 0 },
+
+    { 0x7E0, 0xC0,  1, 0 },
+    { 0x7E0, 0xC1,  1, 0 },
+    { 0x7E0, 0xC2,  1, 0 },
+    { 0x7E0, 0xC9,  1, 0 },
+
+    { 0x7E0, 0xE0,  1, 0 },
+    { 0x7E0, 0xE1,  1, 0 },
+    { 0x7E0, 0xE2,  1, 0 },
+    { 0x7E0, 0xE3,  1, 0 },
+
+    { 0x7E2, 0x00,  1, 0 },
+    { 0x7E2, 0x01,  1, 0 },
+    { 0x7E2, 0x06,  1, 0 },
+
+    { 0x7E2, 0x20,  1, 0 },
+    { 0x7E2, 0x21,  1, 0 },
+    { 0x7E2, 0x25,  1, 0 },
+    { 0x7E2, 0x26,  1, 0 },
+
+    { 0x7E2, 0x32, 14,  0 },
+    { 0x7E2, 0x33, 14, 12 },
+    { 0x7E2, 0x34, 14, 10 },
+    { 0x7E2, 0x35, 14,  8 },
+    { 0x7E2, 0x36, 14,  6 },
+    { 0x7E2, 0x3E, 14,  4 },
+    { 0x7E2, 0x3F, 14,  2 },
+
+    { 0x7E2, 0x40,  1, 0 },
+    { 0x7E2, 0x41,  1, 0 },
+    { 0x7E2, 0x44,  1, 0 },
+    { 0x7E2, 0x46,  1, 0 },
+
+    { 0x7E2, 0x60,  1, 0 },
+    { 0x7E2, 0x61,  1, 0 },
+    { 0x7E2, 0x62,  1, 0 },
+    { 0x7E2, 0x67,  1, 0 },
+    { 0x7E2, 0x68,  1, 0 },
+
+    { 0x7E2, 0x70,  1, 0 },
+    { 0x7E2, 0x71,  1, 0 },
+    { 0x7E2, 0x74,  1, 0 },
+    { 0x7E2, 0x75,  1, 0 },
+    { 0x7E2, 0x78,  1, 0 },
+    { 0x7E2, 0x79,  1, 0 },
+    { 0x7E2, 0x7C,  1, 0 },
+    { 0x7E2, 0x7D,  1, 0 },
+
+    { 0x7E2, 0x80,  1, 0 },
+    { 0x7E2, 0x81,  1, 0 },
+    { 0x7E2, 0x87,  1, 0 },
+    { 0x7E2, 0x8A,  1, 0 },
+    { 0x7E2, 0x8E,  1, 0 },
+
+    { 0x7E2, 0x92,  1, 0 },
+    { 0x7E2, 0x95,  1, 0 },
+    { 0x7E2, 0x98,  1, 0 },
+    { 0x7E2, 0x9B,  1, 0 },
+
+    { 0x7E2, 0xA0,  1, 0 },
+    { 0x7E2, 0xA1,  1, 0 },
+    { 0x7E2, 0xA2,  1, 0 },
+    { 0x7E2, 0xA3,  1, 0 },
+
+    { 0x7E2, 0xC0,  1, 0 },
+    { 0x7E2, 0xC1,  1, 0 },
+    { 0x7E2, 0xC2,  1, 0 },
+
+    { 0x7E2, 0xD1,  1, 0 },
+
+    { 0x7E2, 0xE0,  1, 0 },
+    { 0x7E2, 0xE1,  1, 0 },
+    { 0x7E2, 0xE2,  1, 0 },
+    { 0x7E2, 0xE3,  1, 0 }
+};
+
+int req_NumTbl = sizeof(req_Table) / sizeof(reqTbl);
+*/
+
+//
+int can_open = -1;
+
+void sd_buffold()
+{
+    if(sd_prt >= sd_end) {
+        sd_prt -= PRBUF_MAX;
+        if(sd_prt > sd_buf) {
+            memcpy(sd_buf, sd_end, sd_prt-sd_buf);
+        }
+    }
+}
+
+void sd_printf(char *form, ...)
+{
+    va_list vl;
+    va_start(vl, form);
+    int len = vsnprintf(sd_prt, PRINT_MAX, form, vl);
+    sd_prt += len;
+    sd_buffold();
+    va_end(vl);
+}
+
+void sd_print(char *str, int len)
+{
+    memcpy(sd_prt, str, len);
+    sd_prt += len;
+    sd_buffold();
+}
+
+void sd_hexa(int hex, int len)
+{
+    for(int i = 0; i < len; i++) {
+        char hc = (hex >> ((len-i-1) * 4)) & 15;
+        if(hc < 10) {
+            *sd_prt++ = hc + '0';
+        } else {
+            *sd_prt++ = hc + 'A' - 10;
+        }
+    }
+    sd_buffold();
+}
+
+//
+
+int pc_len()
+{
+    int len = pc_prt - pc_out;
+    if(len < 0) len += PRBUF_MAX;
+
+    if(len > len_max) len_max = len;
+
+    return len;
+}
+
+void pc_buffold()
+{
+    if(pc_prt >= pc_end) {
+        pc_prt -= PRBUF_MAX;
+        if(pc_prt > pc_buf) {
+            memcpy(pc_buf, pc_end, pc_prt-pc_buf);
+        }
+    }
+}
+
+void pc_printf(char *form, ...)
+{
+    if(pc_len() >= PRLEN_MAX) return;
+
+    va_list vl;
+    va_start(vl, form);
+    int len = vsnprintf(pc_prt, PRINT_MAX, form, vl);
+    pc_prt += len;
+    pc_buffold();
+    va_end(vl);
+}
+
+void pc_print(char *str, int len)
+{
+    if(pc_len() >= PRLEN_MAX) return;
+
+    memcpy(pc_prt, str, len);
+    pc_prt += len;
+    pc_buffold();
+}
+
+void pc_hexa(int hex, int len)
+{
+    if(pc_len() >= PRLEN_MAX) return;
+
+    for(int i = 0; i < len; i++) {
+        char hc = (hex >> ((len-i-1) * 4)) & 15;
+        if(hc < 10) {
+            *pc_prt++ = hc + '0';
+        } else {
+            *pc_prt++ = hc + 'A' - 10;
+        }
+    }
+    pc_buffold();
+}
+
+unsigned int prev_us  = 0; // work
+unsigned int curr_us  = 0; // micro sec looped
+unsigned int curr_uH  = 0; // micro sec upper dword
+unsigned int diff_us  = 0; // erapsed us
+unsigned int curr_ms  = 0; // milli sec looped
+unsigned int left_ms  = 0; // work
+unsigned int curr_sc  = 0; // sec looped
+int          ms_1000  = 0; // 1000 looped ms
+int          ms_60000 = 0; // 60000 looped ms
+unsigned int max_diff = 0;
+
+int          send_per_sec =  0;
+
+void canchk()
+{
+    CANMessage msg;
+    int        cnt_read;
+
+    // count time
+    curr_us = timer.read_us(); // 0x00000000-0xFFFFFFFF looped
+    diff_us = curr_us - prev_us;
+    if(curr_us < prev_us) {
+        curr_uH++;
+    }
+    left_ms += diff_us;
+    while(left_ms >= 1000) { // timer.read_ms() not looped. it calculated from read_us() / 1000. so buggy.
+        curr_ms++;
+        ms_60000++;
+        ms_1000++;
+        if(ms_60000 >= 60000) {
+            ms_60000 = 0;
+        }
+        if(ms_1000 >= 1000) {
+            ms_1000 = 0;
+            curr_sc++;
+            send_per_sec = 1;
+        }
+        led1 = (ms_60000 / 250) & 1;
+        left_ms -= 1000;
+        //
+        if(msg_ReqTimeout > 0) {
+            msg_ReqTimeout--;
+        }
+    }
+    if(diff_us > max_diff) {
+        max_diff = diff_us;
+    }
+    prev_us = curr_us;
+
+    // CAN receive (have only 2 receive buffers)
+    for(cnt_read = 0; can.read(msg); cnt_read++) {
+        if(msg.id < 0x200) {
+            // maybe .. request ok
+            if(-10 < msg_ReqTimeout && msg_ReqTimeout <= 0) msg_ReqTimeout--;
+        } else if(msg.id >= 0x7E8 && msg.len == 8) {
+            if(msg.data[0] == 0x10) {
+                // long response
+                if(msg.data[2] == 0x61 || msg.data[2] == 0xE1 || msg.data[2] == 0xE2) {
+                    if(can.write(CANMessage(msg.id & 0x7F7, msg_Continue, sizeof(msg_Continue)))) {
+                        msg_ReqNumLeft = msg.data[1] / 7; // num of message continue
+                        msg_ReqTimeout = REQ_TOUT;        // continue receive
+                        if(msg.data[2] == 0x61 && msg.data[3] == 0x01) {
+                            led3 = !led3;
+                        } else if(msg.data[2] == 0xE2 && msg.data[3] == 0x06) {
+                            led3 = !led3;
+                        }
+                    } else {
+                        msg_ReqNumLeft = 0; // error
+                        msg_ReqTimeout = 0;
+                    }
+                } else {
+                    msg_ReqNumLeft = 0; // may be error
+                    msg_ReqTimeout = 0;
+                }
+            } else if(msg.data[0] > 0x20 && msg.data[0] <= 0x2F) {
+                // 
+                msg_ReqNumLeft--;
+                if(msg_ReqNumLeft <= 0) {
+                    msg_ReqNumLeft = 0; // next ok
+                    msg_ReqTimeout = 0;
+                } else {
+                    msg_ReqTimeout = REQ_TOUT;
+                }
+            } else if(msg.data[0] == 0x30) {
+                // 
+                msg_Received30 = true;
+                msg_ReqNumLeft = 0;
+                msg_ReqTimeout = 0;
+            } else {
+                // maybe .. request error
+                msg_ReqNumLeft = 0;
+                msg_ReqTimeout = 0;
+            }
+        }
+        if(msg.format == CANStandard && msg.type == CANData) {
+            CAN_Buffer &b = msg_Buffer[msg_Rpos];
+            b.id_len = (msg.id << 4) | msg.len;
+            memcpy(b.data, msg.data, 8);
+            b.time = ms_60000;
+            if(msg_Rpos >= (MBUF_MAX-1)) {
+                msg_Rpos = 0;
+            } else {
+                msg_Rpos++;
+            }
+        }
+    }
+    //
+    if(msg_ReqTimeout == -10) {
+        // send req
+        msg_Request[2] = req_Table[msg_ReqIdx].code;
+        if(can.write(CANMessage(req_Table[msg_ReqIdx].id, msg_Request, sizeof(msg_Request)))) {
+            msg_ReqTimeout = REQ_TOUT;
+            // next req
+            do {
+                req_Table[msg_ReqIdx].fcnt++;
+                if(req_Table[msg_ReqIdx].fcnt > req_Table[msg_ReqIdx].freq) {
+                    req_Table[msg_ReqIdx].fcnt = 0;
+                }
+                msg_ReqIdx++;
+                if(msg_ReqIdx > req_NumTbl) {
+                    msg_ReqIdx = 0;
+                }
+            } while(req_Table[msg_ReqIdx].fcnt != 0);
+        }
+    }
+    // Error check
+    int rerr = can.rderror();
+    int werr = can.tderror();
+    if(rerr > 0 || werr > 0) {
+        can.reset();
+        led2 = !led2;
+    }
+}
+
+int main() {
+    char  log_file[32];
+    char *ini_file = "/local/CANUSB30.ini";
+    //
+    lcd.printf("mCANUSB ver 1.00"
+               "= for PRIUS 30 =");
+
+    FILE *fpINI = fopen(ini_file, "r");
+    int c;
+    int mode = 0;
+    int num  = 0;
+    int rdx  = 10;
+    int logn = 0;
+    if(fpINI) {
+        while((c = fgetc(fpINI)) != EOF) {
+            if(c < 0x20) {
+                if(mode == 'l') {
+                    logn = num;
+                }
+                mode = 0;
+                num  = 0;
+                rdx  = 10;
+            } else {
+                if(c >= '0' && c <= '9') {
+                    num = num * rdx + c - '0';
+                } else if(c >= 'A' && c <= 'F') {
+                    num = num * 16 + c - 'A' + 10;
+                } else if(c >= 'a' && c <= 'f') {
+                    num = num * 16 + c - 'a' + 10;
+                } else if(c == 'x') {
+                    rdx = 16;
+                } else if(c == 'l') {
+                    mode = 'l';
+                }
+            }
+        }
+        fclose(fpINI);
+    }
+    logn++;
+    sprintf(log_file, "/sd/CAN%05d.log", logn);
+
+    // save INI
+    fpINI = fopen(ini_file, "w");
+    if(fpINI) {
+        fprintf(fpINI, "l%d\r\n", logn);
+        fclose(fpINI);
+    }
+
+    // try SDCard open
+    FILE *fpSD = fopen(log_file, "w");
+    if(fpSD) {
+        fclose(fpSD); // begins from empty
+    } else {
+        lcd.cls();
+        lcd.printf("! SD CARD FAIL !%s", log_file);
+    }
+
+    // Init
+    can.frequency(500000);
+    pc.baud(921600);
+    timer.start();
+
+    // PC command
+    char cmds[1+3+1+16];
+    int  cmdp =  0;
+
+    //__disable_irq();
+    //__enable_irq();
+
+    // CAN
+    prev_us = timer.read_us();
+    ticker.attach_us(&canchk, 100);
+
+    while(1) {
+        if(msg_Rpos != msg_Tpos) {
+            if(can_open == 3) {
+                // Compress mode
+                char tbuf[17]; // "b1----2--------\r";
+                int  ln,n;
+                tbuf[0] = 'b';
+                tbuf[1] = 0x80;
+                tbuf[2] = (msg_Buffer[msg_Tpos].time   >> 8) & 0xFF;
+                tbuf[3] =  msg_Buffer[msg_Tpos].time         & 0xFF;
+                tbuf[4] = (msg_Buffer[msg_Tpos].id_len >> 8) & 0xFF;
+                tbuf[5] =  msg_Buffer[msg_Tpos].id_len       & 0xFF;
+                tbuf[6] = 0x80;
+                ln =  msg_Buffer[msg_Tpos].id_len & 0xF;
+                for(n = 0; n < ln; n++) {
+                    tbuf[7+n] = msg_Buffer[msg_Tpos].data[n];
+                }
+                tbuf[7+ln] = '\r';
+                tbuf[8+ln] = 0;
+                // encode data
+                for(n = 0; n < ln; n++) {
+                    if((tbuf[7+n] & 0x80) == 0) {
+                        tbuf[7+n] ^= 0x80;
+                        tbuf[6] |= (1 << n);
+                    }
+                }
+                for(n = 0; n < 5; n++) {
+                    if((tbuf[2+n] & 0x80) == 0) {
+                        tbuf[2+n] ^= 0x80;
+                        tbuf[1] |= (1 << n);
+                    }
+                }
+                pc_print(tbuf, 8+ln);
+            } else if(can_open > 0) {
+                // CANUSB compatible
+                pc_print("t", 1);
+                pc_hexa(msg_Buffer[msg_Tpos].id_len, 4);
+                int ln =  msg_Buffer[msg_Tpos].id_len & 0xF;
+                for(int n = 0; n < ln; n++) {
+                    pc_hexa(msg_Buffer[msg_Tpos].data[n], 2);
+                }
+                if(can_open > 1) {
+                    pc_hexa(msg_Buffer[msg_Tpos].time, 4);
+                }
+                pc_print("\r", 1);
+            }
+
+#if 0
+            if(fpSD) {
+                // CANUSB compatible for SDCard
+                sd_print("t", 1);
+                sd_hexa(msg_Buffer[msg_Tpos].id_len, 4);
+                int ln =  msg_Buffer[msg_Tpos].id_len & 0xF;
+                for(int n = 0; n < ln; n++) {
+                    sd_hexa(msg_Buffer[msg_Tpos].data[n], 2);
+                }
+                sd_hexa(msg_Buffer[msg_Tpos].time, 4);
+                sd_print("\r\n", 1);
+            }
+#endif
+
+            if(msg_Tpos >= (MBUF_MAX-1)) {
+                msg_Tpos = 0;
+            } else {
+                msg_Tpos++;
+            }
+
+        } else if(pc.readable() > 0) {
+            // command check
+            char c = pc.getc();
+            if(c == 0x0D) {
+                if(cmdp > 0) {
+                    switch(cmds[0]) {
+                        case 'V':
+                            pc_print("V0521\r", 6);
+                            break;
+                        case 'N':
+                            pc_print("NMBED\r", 6);
+                            break;
+                        case 'F':
+                            pc_print("F00\r", 4);
+                            break;
+                        case 'O':
+                            if(can_open < 0) can_open = -can_open;
+                            pc_print("\r", 1);
+                            led4 = 1;
+                            break;
+                        case 'C':
+                            if(can_open > 0) can_open = -can_open;
+                            pc_print("\r", 1);
+                            led4 = 0;
+                            max_diff = 0;
+                            break;
+                        case 's':
+                        case 'S':
+                        case 't':
+                        case 'T':
+                        case 'm':
+                        case 'M':
+                            pc_print("\r", 1);
+                            break;
+                        case 'Z':
+                            if(cmdp > 1) {
+                                if(cmds[1] == '2') {
+                                    can_open = -3;
+                                } else if(cmds[1] == '1') {
+                                    can_open = -2;
+                                } else {
+                                    can_open = -1;
+                                }
+                            }
+                            pc_print("\r", 1);
+                            break;
+                    }
+                }
+                cmdp = 0;
+            } else if(cmdp < 21) {
+                cmds[cmdp++] = c;
+            }
+
+        } else if(pc_out != pc_prt && pc.writeable() > 0) {
+
+            // send data to PC
+            while(pc_out != pc_prt && pc.writeable() > 0) {
+                pc.putc(*pc_out++);
+                if(pc_out >= pc_end) pc_out = pc_buf;
+            }
+
+        } else if(sd_prt < sd_out || (sd_prt - sd_out) >= SD_BLK) {
+
+            if(fpSD) {
+                fpSD = fopen(log_file, "a");
+                if(fpSD) {
+                    int len = SD_BLK - sd_ofs;
+                    while(len > 0 && fpSD) {
+                        int olen = (len > SD_ONE) ? SD_ONE : len;
+                        if(fwrite(sd_out+sd_ofs, 1, olen, fpSD) == olen) {
+                            sd_ofs += olen;
+                            len -= olen;
+                            led4 = !led4;
+                        } else {
+                            fclose(fpSD);
+                            fpSD = NULL; // if failed, stop logging
+                        }
+                    }
+                    if(fpSD) fclose(fpSD);
+                } else {
+                    fpSD = NULL;
+                }
+            }
+            sd_out += SD_BLK;
+            sd_ofs  = 0;
+            if(sd_out >= sd_end) sd_out = sd_buf;
+
+        } else if(send_per_sec > 0) {
+
+            if(fpSD && sd_prt != (sd_out + sd_ofs)) {
+                fpSD = fopen(log_file, "a");
+                if(fpSD) {
+                    int len = sd_prt - (sd_out + sd_ofs);
+                    while(len > 0 && fpSD) {
+                        int olen = (len > SD_ONE) ? SD_ONE : len;
+                        if(fwrite(sd_out+sd_ofs, 1, olen, fpSD) == olen) {
+                            sd_ofs += olen;
+                            len -= olen;
+                            led4 = 1;
+                        } else {
+                            fclose(fpSD);
+                            fpSD = NULL; // if failed, stop logging
+                        }
+                    }
+                    if(fpSD) fclose(fpSD);
+                } else {
+                    fpSD = NULL;
+                }
+            }
+
+            lcd.locate(0,0);
+            lcd.printf("%08X--------", curr_us);
+            send_per_sec--;
+
+        }
+    }
+}