File content as of revision 2:aa9a344a42a8:

#include "FEP.h"

myFEP::myFEP(PinName tx, PinName rx, uint8_t addr_, int baud=115200) :
    RawSerial(tx, rx)
{
    addr     = addr_;
    bufindex = 0;
    retindex = 0;
}

void myFEP::StartReceive()
{
    attach(callback(this, &myFEP::ReceiveBytes));
    
}

void myFEP::ReceiveBytes()
{
    if (bufindex > 256) bufindex %= 256;
    buffer[bufindex] = getc(); // get data

    if ( (bufindex > 0) && (!strcmp((char*)(buffer + bufindex - 1) , "\r\n")) ) { // <CR><LF>
        CheckData(); // check message
    }
}

void CheckData()
{
    uint8_t temp=0, length=0; // temp:where's array   length:length of array
    for (uint16_t i=0; i<256; i++) {
        temp = (256 + bufindex - i) % 256;
        if ( !str((char*)(buffer + temp) , "RBN") ) { // check header
            length = ctoi(buffer[temp+6])*100 + ctoi(buffer[temp+7])*10 + ctoi(buffer[temp+8]); // calculate length of message
            for (int j = 9; j < length+9; j++) {
                retdata[j-9] = buffer[temp+j]; // 
            }
        }
    }
}

void GetData(uint8_t *data)
{
    strcpy((char*)data, (char*)retdata);
}


uint8_t SendData(uint8_t *data)
{
    return SendData(data, sizeof(data));
}

uint8_t SendData(uint8_t *data, uint8_t length)
{
    if(length > 128) return 1;
    uint8_t sendindex; // index of 'data'
    printf("@TBN%03d%03d", addr, length); // send comand
    for (sendindex=0; sendindex<length; sendindex++) { 
        putc(data[sendindex]); // send message
    }
    printf("\r\n"); // <cr><lf>
    return GetResponse(); // check response
}

uint8_t GetResponse()
{
    char res[4]; // array of storing response
    uint8_t resindex=0; // 
    while (1) {
        res[resindex] = getc();
        if (resindex >= 3) {
            if (!strcmp(temp, "P0\r\n"))  return 0;
            else if (!strcmp(temp, "N0\r\n")) return 2;
            else if (!strcmp(temp, "N1\r\n")) return 3;
            else if (!strcmp(temp, "N3\r\n")) return 4;
        }
        if (++resindex > 3) return -1;
    }
}

uint8_t FEP::ctoi(char c)
{
    if('0' <= c && c <= '9') return (c-'0');
    else return -1;
}