File content as of revision 11:d60fb729eacf:

#include "TRP105F_Spline.h"

//
//  For Platform Variation
//
#if defined(TARGET_LPC1114)
const PinName DEFALT_AI_PIN = dp4;
#elif defined (TARGET_LPC1768)
const PinName DEFALT_AI_PIN = p16;
LocalFileSystem local("local");  // define Mount Point(which becomes Direcory Path)
//  To get sample distance via seral com
Serial      g_Serial_Signal(USBTX, USBRX);
//  To get ytage of TRP105F
//AnalogIn*   g_Sensor_Voltage;
#endif



//
//  For debug
//
//#define DEBUG
//#define DEBUG2
#ifdef DEBUG
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
#endif

//  Constructor
TRP105FS::TRP105FS()
    :_useType(AsMODULE)
    ,_ai(AnalogIn(DEFALT_AI_PIN))
{
    _Sample_Num = 5;
    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));

    for(int i = 0; i < _Sample_Num; i++) {
        _Sample_Set[i].x = _Sample_Set[i].y = 0;
        _u_spline[i] = 0.0;
    }
}

TRP105FS::TRP105FS(
    unsigned int arg_num
)
    :_useType(AsMODULE)
    ,_ai(AnalogIn(DEFALT_AI_PIN))
{
    if(arg_num > _ENUM) _Sample_Num = _ENUM;
    else _Sample_Num = arg_num;

    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));

    for(int i = 0; i < _Sample_Num; i++) {
        _Sample_Set[i].x = _Sample_Set[i].y = 0;
        _u_spline[i] = 0.0;
    }
}

TRP105FS::TRP105FS(
    unsigned int arg_num,
    UseType arg_type
)
    :_useType(arg_type)
    ,_ai(AnalogIn(DEFALT_AI_PIN))
{
    if(arg_num > _ENUM) _Sample_Num = _ENUM;
    else _Sample_Num = arg_num;

    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));

    for(int i = 0; i < _Sample_Num; i++) {
        _Sample_Set[i].x = _Sample_Set[i].y = 0;
        _u_spline[i] = 0.0;
    }
}

TRP105FS::TRP105FS(
    unsigned int arg_num,
    PinName pin
)
    :_ai(AnalogIn(pin))

{
    if(arg_num > _ENUM) _Sample_Num = _ENUM;
    else _Sample_Num = arg_num;

    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));

    for(int i = 0; i < _Sample_Num; i++) {
        _Sample_Set[i].x = _Sample_Set[i].y = 0;
        _u_spline[i] = 0.0;
    }
}

TRP105FS::TRP105FS(
    unsigned int arg_num,
    UseType arg_type,
    PinName pin
)
    :_useType(arg_type)
    ,_ai(AnalogIn(pin))
{
    if(arg_num > _ENUM) _Sample_Num = _ENUM;
    else _Sample_Num = arg_num;

    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));

    for(int i = 0; i < _Sample_Num; i++) {
        _Sample_Set[i].x = _Sample_Set[i].y = 0;
        _u_spline[i] = 0.0;
    }
}

//  Destructor
TRP105FS::~TRP105FS()
{
    free(_Sample_Set);
    free(_u_spline);
    //delete g_Sensor_Voltage;
}


unsigned short TRP105FS::getX(unsigned short arg_y)
{
    int idx;
    unsigned short pv = 0;

    pv = arg_y;

    idx = _getNearest(_LIDX, _RIDX, pv);

    if (idx != 0xFFFF)    //  unless occuring error
        return _Set[idx].x;
    else
        return 0xFFFF;
}

unsigned short TRP105FS::getDistance(unsigned short arg_y)
{
    return getX(arg_y);
}

unsigned short TRP105FS::getDistance()
{
    return getX(_ai.read_u16());
}

/*
    Function to find a set whose y member is nearest a ytage from the sensor, recursively.

                   SHORT                                                          LONG
    +------------>  HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+
    |(if ytage form sensor < threshold[cidx])   |||   (if threshold[cidx] < ytage form sensor)|
    |               HIGH[lidx , ... , cidx]LOW    |||    HIGH[cidx+1 , ... , ridx]LOW             |
    |                |                                                            |               |
    +----------------+                                                            +---------------+
*/
int TRP105FS::_getNearest(
    int arg_lidx,
    int arg_ridx,
    unsigned short arg_y
)
{
    int cidx = (arg_lidx + arg_ridx) / 2;

    //  When the number of element to compare is only one, return it as result.
    if(arg_lidx == arg_ridx)
        return cidx;
    //  If the ytage from the sensor is lower than the center threshold
    //  (_set[cidx] > _threshold[cidx] > _set[cidx+1])
    else if(arg_y > _Threshold[cidx])
        return _getNearest(arg_lidx, cidx,     arg_y);
    //  If the ytage from the sensor is higher than the center threshold
    else if(arg_y < _Threshold[cidx])
        return _getNearest(cidx + 1, arg_ridx, arg_y);
    //  If the ytage from the sensor eauals the center threshold
    else //(arg_y == _Treshold[cidx].y)
        return cidx;
}

void TRP105FS::setSample(unsigned short arg_x, unsigned short arg_y)
{
    _setSample(arg_x, arg_y);
}

void TRP105FS::_setSample(unsigned short arg_x, unsigned short arg_y)
{
    static unsigned int snum = 0;
    unsigned int num;
    int     tmp;
    VDset tmp_set[_ENUM];   // for bucket sort

    // Increment it if this function called.
    snum++;
#ifdef DEBUG
    g_Serial_Signal.printf("snum       : %d\n", snum);
    g_Serial_Signal.printf("(%d,%d)\n",arg_x, arg_y);
#endif

    //  fit to smaller
    if (snum < _Sample_Num) {
        num = snum;
    } else {
        //  To reclloc memories if snum is bigger than _Sample_Num.
        //  When realloc is failed, snum is back to porevius.
        VDset*  tmp_Set = (VDset *)realloc(_Sample_Set, snum * sizeof(VDset));
        double* tmp__u_ = (double*)realloc(_u_spline,   snum * sizeof(double));
        if (tmp_set != NULL && tmp__u_ != NULL) {
            _Sample_Set = tmp_Set;
            _u_spline   = tmp__u_;
            num = _Sample_Num = snum;
        } else {
            snum--;
            num = snum = _Sample_Num ;
#ifdef DEBUG
            g_Serial_Signal.printf("failed to realloc\n", snum);
#endif
        }
    }

    _Sample_Set[num - 1].x = arg_x;
    _Sample_Set[num - 1].y = arg_y;
    if((unsigned short)_RIDX < _Sample_Set[num - 1].x)
        _Sample_Set[num - 1].x = (unsigned short)_RIDX;

    //
    //  Sort set data array in distanceAscending order
    //
    //  Bucket sort
    for(int i = 0; i < _ENUM; i++)
        tmp_set[i].x = 0xaaaa;
    tmp = 0;
    for(int i = 0; i < num; i++) {
        //  use x as index for x range [_LIDX,_RIDX]
        if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
            tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
            tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
        } else { // if a same x has been input, calcurate mean.
            tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
            tmp_set[_Sample_Set[i].x].y /= 2;
            tmp++;
        }
    }
#ifdef DEBUG
    g_Serial_Signal.printf("  _Sample_num: %d\n", _Sample_Num );
    g_Serial_Signal.printf("-)        tmp: %d\n", tmp );
#endif
    //  substruct tmp from number of sample.
    _Sample_Num -= tmp;
    snum -= tmp;
#ifdef DEBUG
    g_Serial_Signal.printf("-----------------\n");
    g_Serial_Signal.printf("  _Sample_num: %d\n", _Sample_Num );
#endif
    //  apply sort on _Sample_Set
    tmp = 0;
    for(int i = 0; i < _ENUM; i++) {
        if(tmp_set[i].x != 0xaaaa) {
            _Sample_Set[i - tmp].x = tmp_set[i].x;
            _Sample_Set[i - tmp].y = tmp_set[i].y;
        } else //  if no data, skip it
            tmp++;
    }
}


#ifdef TARGET_LPC1768
void TRP105FS::_sampleData()
{
    int     tmp;
    char    sig;
    unsigned short tmp_y;
    VDset tmp_set[_ENUM];   // for bucket sort

    //  For evry set,
    //  1, get x data via serai com,
    //  2, get y data,
    //  and then do same for next index set.
    for(int i = 0; i < _Sample_Num; i++) {
        //
        //  Recieve a Distance datus and store it into member
        //
        if(_useType == AsDEBUG) {
            g_Serial_Signal.putc('>');
            _Sample_Set[i].x = 0;
            do {
                sig = g_Serial_Signal.getc();
                if('0' <= sig && sig <= '9') {
                    _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
                    g_Serial_Signal.putc(char(sig));
                } else if(sig == 0x08) {
                    _Sample_Set[i].x = 0;
                    g_Serial_Signal.printf("[canseled!]");
                    g_Serial_Signal.putc('\n');
                    g_Serial_Signal.putc('>');
                }
            } while (!(sig == 0x0a || sig == 0x0d));
            g_Serial_Signal.putc('\n');
        } else {
            _Sample_Set[i].x = g_Serial_Signal.getc();
        }

        //  if input data is over the bound calibrate it below
        if (_Sample_Set[i].x < (unsigned short)_LIDX)
            _Sample_Set[i].x = (unsigned short)_LIDX;
        else if ((unsigned short)_RIDX < _Sample_Set[i].x)
            _Sample_Set[i].x = (unsigned short)_RIDX;
        //
        //  Recieve a Voltage datus and store it into member
        //
        //  LOW PASS FILTERED
        //  Get 10 data and store mean as a sample.
        //  After get one original sample, system waits for 0.1 sec,
        //  thus it takes 1 sec evry sampling.
        _Sample_Set[i].y = 0;
        for(int j = 0; j < 10; j++) {
            //unsigned short 's range [0 , 65535]
            //the Number of significant figures of read ytage is 3 or 4.
            tmp_y = _ai.read_u16();

#ifdef DEBUG
            g_Serial_Signal.printf("%d,",tmp_y);
#endif
            _Sample_Set[i].y += (tmp_y / 10);
            wait(0.1);
        }
#ifdef DEBUG
        g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y);
#endif
    }
    //
    //  Sort set data array in distanceAscending order
    //
    //  Bucket sort
    for(int i = 0; i < _ENUM; i++)
        tmp_set[i].x = 0xaaaa;
    tmp = 0;
    for(int i = 0; i < _Sample_Num; i++) {
        //  use x as index for x range [LIDX,RIDX]
        if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
            tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
            tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
        } else { // if a same x has been input, calcurate mean.
            tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
            tmp_set[_Sample_Set[i].x].y /= 2;
            tmp++;
        }
    }
#ifdef DEBUG
    g_Serial_Signal.printf("%d\n", _Sample_Num );
#endif
    //  substruct tmp from number of sample.
    _Sample_Num -= tmp;

#ifdef DEBUG
    g_Serial_Signal.printf("tmp: %d\n", tmp );
#endif
    //  apply sort on _Sample_Set
    tmp = 0;
    for(int i = 0; i < _ENUM; i++) {
        if(tmp_set[i].x != 0xaaaa) {
            _Sample_Set[i - tmp].x = tmp_set[i].x;
            _Sample_Set[i - tmp].y = tmp_set[i].y;
        } else //  if no data, skip it
            tmp++;
    }
}
#endif

//
//  Function to define _u_spline, specific constants of spline.
//
void TRP105FS::_makeSpline()
{
    //  x: x, distance
    //  y: y, ytage
    //
    //  N: max of index <=> (_Sample_Num - 1)
    //
    //  _u_spline[i] === d^2/dx^2(Spline f)[i]
    //  i:[0,N]
    //  _u_spline[0] = _u_spline[N] = 0
    //
    //  h[i] = x[i+1] - x[i]
    //  i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
    double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
    //unsigned short *h __attribute__((at(0x20080000)));
    //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
    //  v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
    //  i:[0,N-2]
    double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
    //unsigned short *v __attribute__((at(0x20080100)));
    //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
    //  temporary array whose num of elm equals v array
    double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
    //unsigned short *w __attribute__((at(0x20080200)));
    //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
    //
    //  [ 2(h[0]+h[1])  , h[1]          ,                                 O                 ]   [_u[1]  ]   [v[0]  ]
    //  [ h[1]          , 2(h[1]+h[2])  , h[2]                                              ]   [_u[2]  ]   [v[1]  ]
    //  [                       ...                                                         ] * [...    ] = [...   ]
    //  [                   h[j]          , 2(h[j]+h[j+1])  , h[j+1]                        ]   [_u[j+1]]   [v[j]  ]
    //  [                                   ...                                             ]   [ ...   ]   [ ...  ]
    //  [                               h[N-3]        , 2(h[N-3]+h[N-2]), h[N-2]            ]   [_u[j+1]]   [v[j]  ]
    //  [       O                                       h[N-2]          , 2(h[N-2]+h[N-1])  ]   [_u[N-1]]   [v[N-2]]
    //
    // For LU decomposition
    double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
    //unsigned short *Upper __attribute__((at(0x20080300)));
    //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
    double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
    //unsigned short *Lower __attribute__((at(0x20080400)));
    //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
#ifdef DEBUG
    _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
#endif
    for(int i = 0; i < _Sample_Num - 1; i++)
        h[i] =  (double)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
    //(unsigned short)(_Sample_Set[i + 1].x - _Sample_Set[i].x);

    for(int i = 0; i < _Sample_Num - 2; i++)
        v[i] = 6 * (
                   ((double)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y)) / h[i + 1]
                   //(unsigned short)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y) / h[i + 1]
                   -
                   ((double)(_Sample_Set[i + 1].y - _Sample_Set[i].y))     / h[i]
                   //(unsigned short)(_Sample_Set[i + 1].y - _Sample_Set[i].y) / h[i]
               );

    //
    //  LU decomposition
    //
    Upper[0] = 2 * (h[0] + h[1]);
    Lower[0] = 0;
    for (int i = 1; i < _Sample_Num - 2; i++) {
        Lower[i] = h[i] / Upper[i - 1];
        Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
    }


    //
    //  forward substitution
    //
    w[0] = v[0];
    for (int i = 1; i < _Sample_Num - 2; i ++) {
        w[i] = v[i] - Lower[i] * w[i-1];
    }


    //
    //  backward substitution
    //
    _u_spline[_Sample_Num - 2] =  w[_Sample_Num - 3]                         / Upper[_Sample_Num - 3];
    for(int i = _Sample_Num - 3; i > 0; i--) {
        _u_spline[i]           = (w[(i - 1)] -  h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
    }

    // _u_spline[i] === d^2/dx^2(Spline f)[i]
    _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;

#ifdef DEBUG
    _printOutData(h, _Sample_Num - 1, "h");
    _printOutData(v, _Sample_Num - 2, "v");
    _printOutData(w, _Sample_Num - 2, "w");
    _printOutData(Upper, _Sample_Num - 2, "Upper");
    _printOutData(Lower, _Sample_Num - 2, "Lower");
    _printOutData(_u_spline, _Sample_Num, "u");
#endif
    free(h);
    free(v);
    free(w);
    free(Upper);
    free(Lower);
}
//
//  Function to return Voltage for distance.
//
unsigned short TRP105FS:: _getSplineYof(
    double arg_x       //  the argument is supposed as distance [mm]
)
{
    double y;       //  ytage calculated by spline polynomial
    double a,b,c,d; //  which is specific constant of spline, and can be expressed with _u.
    int itv = 0;    //  interval(section) of interpolation
    //  the number of interval is less 1 than the number of sample sets,
    //  which means the max number of interval is _Sample_num - 2.
    if((double)(_Sample_Set[0].x) <= arg_x) {
        while (!((double)(_Sample_Set[itv].x) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].x))) {
            itv++;
            if(itv > _Sample_Num - 2) {
                itv = _Sample_Num - 2;
                break;
            }
        }
    }
    a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x);
    b = (double)(_u_spline[itv]) / 2.0;
    c = (double)(_Sample_Set[itv + 1].y - _Sample_Set[itv].y) / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x)
        -
        (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0;
    d = (double)(_Sample_Set[itv].y);
    //  cubic spline expression
    y = a * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
        +
        b * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
        +
        c * (arg_x - (double)(_Sample_Set[itv].x))
        +
        d;

#ifdef DEBUG2
    g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
    g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
    g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
#endif

    return ((unsigned short)(int)y);
}

#if defined(HAS_COM_TO_CONSOLE)
void TRP105FS::calibrateSensor()
{
    _sampleData();
    _makeSpline();

    for(int i = 0; i < _ENUM; i++) {
        _Set[i].x     = i;
        _Set[i].y     = _getSplineYof((double)(_Set[i].x));
        _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
#ifdef DEBUG2
        g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
#endif
    }
}
#endif

void TRP105FS::calibrate()
{
#ifdef DEBUG2
    g_Serial_Signal.printf("Sample Data Set\n");
    for(int i = 0; i < _Sample_Num; i++)
        g_Serial_Signal.printf("(%d,%d)\n", _Sample_Set[i].x, _Sample_Set[i].y);
#endif

    _makeSpline();

    for(int i = 0; i < _ENUM; i++) {
        _Set[i].x     = i;
        _Set[i].y     = _getSplineYof((double)(_Set[i].x));
        _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);

#ifdef DEBUG2
        g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
#endif
    }
}

void TRP105FS::saveSetting()
{
    FILE *fp;

    fp = fopen("/local/savedata.log", "wb");

    for(int i = 0; i < _ENUM; i++) {
        fwrite(&_Set[i].x,    sizeof(unsigned short), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Set[i].y,    sizeof(unsigned short), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Threshold[i],  sizeof(unsigned short), 1, fp);
        fputc(0x3b, fp);
    }
    fwrite(&_Sample_Num, sizeof(int), 1, fp);
    fputc(0x3b, fp);
    for(int i = 0; i < _Sample_Num; i++) {
        fwrite(&_Sample_Set[i].x,    sizeof(unsigned short), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Sample_Set[i].y,    sizeof(unsigned short), 1, fp);
        fputc(0x3b, fp);
    }
    fclose(fp);

}

#ifdef TARGET_LPC1768
void TRP105FS::printThresholds()
{
    for(int i = 0; i < _ENUM; i++)
        g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
}
#endif

void TRP105FS::loadSetting()
{
    FILE *fp;
    char tmp;

    //sprintf(filepath, "/local/%s", filename);
    //fp = fopen(filepath, "rb");
    fp = fopen("/local/savedata.log", "rb");
    for(int i = 0; i < _ENUM; i++) {

        fread(&_Set[i].x,     sizeof(unsigned short), 1, fp);
        fread(&tmp,             sizeof(char),           1, fp);
#ifdef DEBUG2
        g_Serial_Signal.printf("%d%c",  _Set[i].x, tmp);
#endif

        fread(&_Set[i].y,     sizeof(unsigned short), 1, fp);
        fread(&tmp,             sizeof(char), 1, fp);
#ifdef DEBUG2
        g_Serial_Signal.printf("%d%c",  _Set[i].y, tmp);
#endif

        fread(&_Threshold[i],   sizeof(unsigned short), 1, fp);
        fread(&tmp,             sizeof(char), 1, fp);
#ifdef DEBUG2
        g_Serial_Signal.printf("%d%c\n",_Threshold[i],      tmp);
#endif
    }

    fread(&_Sample_Num, sizeof(unsigned short),  1, fp);
    fread(&tmp,         sizeof(char), 1, fp);

    for(int i = 0; i < _Sample_Num; i++) {
        fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
        fread(&tmp, sizeof(char),1,fp);
        fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
        fread(&tmp, sizeof(char),1,fp);
    }
    fclose(fp);
}


void TRP105FS::saveSetting(
    const char *filename
)
{
    FILE *fp;
    char *filepath;
    int fnnum = 0;

    while (filename[fnnum] != 0) fnnum++;
    filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.

    sprintf(filepath, "/local/%s", filename);
    fp = fopen(filepath, "wb");

    for(int i = 0; i < _ENUM; i++) {
        fwrite(&_Set[i].x,    sizeof(unsigned short), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Set[i].y,    sizeof(unsigned short), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Threshold[i],  sizeof(unsigned short), 1, fp);
        fputc(0x3b, fp);
    }
    fwrite(&_Sample_Num, sizeof(int), 1, fp);
    fputc(0x3b, fp);
    for(int i = 0; i < _Sample_Num; i++) {
        fwrite(&_Sample_Set[i].x,    sizeof(unsigned short), 1, fp);
        fputc(0x2c, fp);
        fwrite(&_Sample_Set[i].y,    sizeof(unsigned short), 1, fp);
        fputc(0x3b, fp);
    }
    fclose(fp);
    free(filepath);
}

void TRP105FS::loadSetting(
    const char *filename
)
{
    FILE *fp;
    char *filepath;
    char tmp;
    int fnnum = 0;

    while (filename[fnnum] != 0) fnnum++;
    filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.

    sprintf(filepath, "/local/%s", filename);
    fp = fopen(filepath, "rb");

    for(int i = 0; i < _ENUM; i++) {

        fread(&_Set[i].x,     sizeof(unsigned short), 1, fp);
        fread(&tmp,             sizeof(char), 1, fp);
#ifdef DEBUG3
        g_Serial_Signal.printf("%d%c",  _Set[i].x, tmp);
#endif

        fread(&_Set[i].y,     sizeof(unsigned short), 1, fp);
        fread(&tmp,             sizeof(char), 1, fp);
#ifdef DEBUG3
        g_Serial_Signal.printf("%d%c",  _Set[i].y, tmp);
#endif

        fread(&_Threshold[i],   sizeof(unsigned short), 1, fp);
        fread(&tmp,             sizeof(char), 1, fp);
#ifdef DEBUG3
        g_Serial_Signal.printf("%d%c\n",_Threshold[i],      tmp);
#endif
    }

    fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
    fread(&tmp,         sizeof(char),           1, fp);
#ifdef DEBUG3
    g_Serial_Signal.printf("%d%c\n",_Sample_Num,      tmp);
#endif

    for(int i = 0; i < _Sample_Num; i++) {
        fread(&_Sample_Set[i].x,  sizeof(unsigned short), 1, fp);
        fread(&tmp,                 sizeof(char),1,fp);
#ifdef DEBUG3
        g_Serial_Signal.printf("%d%c",  _Sample_Set[i].x, tmp);
#endif

        fread(&_Sample_Set[i].y,  sizeof(unsigned short), 1, fp);
        fread(&tmp,                 sizeof(char),1,fp);
#ifdef DEBUG3
        g_Serial_Signal.printf("%d%c",  _Sample_Set[i].y, tmp);
#endif
    }
    fclose(fp);
    free(filepath);
}

void TRP105FS::printOutData(
    const char *filename)
{
    FILE *fp;
    char *filepath;
    int fnnum = 0;

    while (filename[fnnum] != 0) fnnum++;
    filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
    sprintf(filepath, "/local/%s", filename);
    fp = fopen(filepath, "w");
    //fp = fopen("/local/log.txt", "w");  // open file in writing mode

    fprintf(fp, "x, y,(threshold)\n");
    for(int i = 0; i < _ENUM; i++) {
        fprintf(fp, "%d,%d,(%d)\n", _Set[i].x, _Set[i].y, _Threshold[i]);
    }
    fprintf(fp, "\nSample:x, y\n");
    for(int i = 0; i < _Sample_Num; i++) {
        fprintf(fp, "%d,%d\n", _Sample_Set[i].x, _Sample_Set[i].y);
    }

    free(filepath);
    fclose(fp);

}

void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
{
    FILE *fp;
    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
    fprintf(fp, "%10s\n", name);
    for(int i = 0; i < num; i++) {
        fprintf(fp, "%d, ", arg[i]);
    }
    fprintf(fp, "\n");
    fclose(fp);
}
void TRP105FS::_printOutData(double *arg, int num, char* name)
{
    FILE *fp;

    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
    fprintf(fp, "%10s\n", name);
    for(int i = 0; i < num; i++) {
        fprintf(fp, "%.2f, ", arg[i]);
    }
    fprintf(fp, "\n");
    fclose(fp);
}
void TRP105FS::_printOutData(VDset *arg, int num, char* name)
{
    FILE *fp;

    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
    fprintf(fp, "%10s\n", name);
    for(int i = 0; i < num; i++) {
        fprintf(fp, "%d, ", arg[i].y);
    }
    fprintf(fp, "\n");
    fclose(fp);
}