File content as of revision 0:050fdb1efb7a:

#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--;

        }
    }
}