for child
Fork of TRP105F_Spline by
TRP105F_Spline.cpp
- Committer:
- tanaken0605
- Date:
- 2017-10-24
- Revision:
- 27:1f38ee841153
- Parent:
- 23:772a9736ee59
File content as of revision 27:1f38ee841153:
#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 //#define DEBUG3 #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)) { _snum = 0; _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 short arg_num ) :_useType(AsMODULE) ,_ai(AnalogIn(DEFALT_AI_PIN)) { _snum = 0; 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 short arg_num, UseType arg_type ) :_useType(arg_type) ,_ai(AnalogIn(DEFALT_AI_PIN)) { _snum = 0; 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 short arg_num, PinName pin ) :_useType(AsMODULE) ,_ai(AnalogIn(pin)) { _snum = 0; 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 short arg_num, UseType arg_type, PinName pin ) :_useType(arg_type) ,_ai(AnalogIn(pin)) { _snum = 0; 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::getSampleNum() { return _Sample_Num; } /* unsigned short getVoltage() // get voltage { return(AnalogIn(pin).read_u16()); } */ 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; } /* 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) { 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 } } #ifdef HAS_COM_TO_CONSOLE void TRP105FS::printThresholds() { for(int i = 0; i < _ENUM; i++) g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]); } #endif #ifdef HAS_LOCAL_FILE_SYSTEM 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::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(unsigned short), 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); } 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(unsigned short), 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); } #endif// HAS_LOCAL_FILE_SYSTEM void TRP105FS::saveSetting_intoSerial( Serial* com ) { char buffer[3]; for(int i = 0; i < _ENUM; i++) { //_Set.X buffer[0] = 0xFF & (_Set[i].x >> 8); buffer[1] = 0xFF & (_Set[i].x); buffer[2] = 0x2c; for(int j = 0; j < 3; j++) com->putc(buffer[j]); while(1) { if(com->getc() == 0x06) break; //wait ACK } //Set Y buffer[0] = 0xFF & (_Set[i].y >> 8); buffer[1] = 0xFF & (_Set[i].y); buffer[2] = 0x2c; for(int j = 0; j < 3; j++) com->putc(buffer[j]); while(1) { if(com->getc() == 0x06) break; //wait ACK } //Threshold buffer[0] = 0xFF & (_Threshold[i] >> 8); buffer[1] = 0xFF & (_Threshold[i]); buffer[2] = 0x3b; for(int j = 0; j < 3; j++) com->putc(buffer[j]); while(1) { if(com->getc() == 0x06) break; //wait ACK } } //Sample Num buffer[0] = 0xFF & (_Sample_Num >> 8); buffer[1] = 0xFF & (_Sample_Num); buffer[2] = 0x3b; for(int j = 0; j < 3; j++) com->putc(buffer[j]); while(1) { if(com->getc() == 0x06) break; //wait ACK } for(int i = 0; i < _Sample_Num; i++) { //Sample Set X buffer[0] = 0xFF & (_Sample_Set[i].x >> 8); buffer[1] = 0xFF & (_Sample_Set[i].x); buffer[2] = 0x2c; for(int j = 0; j < 3; j++) com->putc(buffer[j]); while(1) { if(com->getc() == 0x06) break; //wait ACK } //Sample Set Y buffer[0] = 0xFF & (_Sample_Set[i].x >> 8); buffer[1] = 0xFF & (_Sample_Set[i].x); buffer[2] = 0x2c; for(int j = 0; j < 3; j++) com->putc(buffer[j]); while(1) { if(com->getc() == 0x06) break; //wait ACK } } com->putc(0x04); //send End of Transmission } void TRP105FS::loadSetting_fromSerial( Serial* com ) { char buffer[3]; for(int i = 0; i < _ENUM; i++) { //_Set.X for(int j = 0; j < 3; j++) { buffer[j] = com->getc(); } com->putc(0x06); //return ACK _Set[i].x = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0])); //_Set/Y for(int j = 0; j < 3; j++) { buffer[j] = com->getc(); } com->putc(0x06); //return ACK _Set[i].y = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0])); //_Threshold for(int j = 0; j < 3; j++) { buffer[j] = com->getc(); } com->putc(0x06); //return ACK _Threshold[i] = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0])); } //_Sample_Num for(int j = 0; j < 3; j++) { buffer[j] = com->getc(); } com->putc(0x06); //return ACK _Sample_Num = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0])); for(int i = 0; i < _Sample_Num; i++) { //Sample Set X for(int j = 0; j < 3; j++) { buffer[j] = com->getc(); } com->putc(0x06); //return ACK _Sample_Set[i].x= static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0])); //Sample Set Y for(int j = 0; j < 3; j++) { buffer[j] = com->getc(); } if(i != _Sample_Num - 1) com->putc(0x06); //return ACK else com->putc(0x04); //return EOT _Sample_Set[i].y= static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0])); } } #ifdef HAS_LOCAL_FILE_SYSTEM 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); } #endif// HAS_LOCAL_FILE_SYSTEM