for child
Fork of TRP105F_Spline by
TRP105F_Spline.cpp
- Committer:
- aktk
- Date:
- 2016-02-12
- Revision:
- 0:e85788b14028
- Child:
- 1:2053662b1167
File content as of revision 0:e85788b14028:
#define DEBUG #include "TRP105F_Spline.h" // To get voltage of TRP105F AnalogIn g_Sensor_Voltage(p16); // To get sample distance via seral com Serial g_Serial_Signal(USBTX, USBRX); LocalFileSystem local("local"); // マウントポイントを定義(ディレクトリパスになる) // for debug #ifdef DEBUG DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); #endif TRP105FS::TRP105FS() :_Data_Input_Type(SYSTEM) { _Sample_Num = 5; _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset)); _u_spline = (double*)malloc(_Sample_Num * sizeof(double)); //calibrateSensor(); } TRP105FS::TRP105FS( unsigned int arg_num ) :_Data_Input_Type(SYSTEM) { 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)); //calibrateSensor(); } TRP105FS::TRP105FS( unsigned int arg_num, DataInType arg_dit ) :_Data_Input_Type(arg_dit) { 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)); //calibrateSensor(); } TRP105FS::~TRP105FS() { free(_Sample_Set); free(_u_spline); } unsigned short TRP105FS::getDistance() { int idx; unsigned short pv = 0; //low pass filter for(int i = 0; i < 10; i++) pv += g_Sensor_Voltage.read_u16() / 10; idx = _getNearest(_LIDX, _RIDX, pv); if (idx != 0xFFFF) // unless occuring error return _Set[idx].dst; else return 0xFFFF; } /* Function to find a set whose vol member is nearest a voltage from the sensor, recursively. SHORT LONG +------------> HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+ |(if voltage form sensor < threshold[cidx]) ||| (if threshold[cidx] < voltage form sensor)| | HIGH[lidx , ... , cidx]LOW ||| HIGH[cidx+1 , ... , ridx]LOW | | | | | +----------------+ +---------------+ */ int TRP105FS::_getNearest( int arg_lidx, int arg_ridx, unsigned short arg_vol ) { 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 voltage from the sensor is lower than the center threshold // (_set[cidx] > _threshold[cidx] > _set[cidx+1]) else if(arg_vol > _Threshold[cidx]) return _getNearest(arg_lidx, cidx, arg_vol); // If the voltage from the sensor is higher than the center threshold else if(arg_vol < _Threshold[cidx]) return _getNearest(cidx + 1, arg_ridx, arg_vol); // If the voltage from the sensor eauals the center threshold else //(arg_vol == _Treshold[cidx].vol) return cidx; } void TRP105FS::_sampleData() { int tmp; char sig; unsigned short tmp_vol; VDset tmp_set[_ENUM]; // for bucket sort // Evry set, first get dst data via serai com, // next, get vol 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(_Data_Input_Type == KEYBORD) { g_Serial_Signal.putc('>'); _Sample_Set[i].dst = 0; do { sig = g_Serial_Signal.getc(); if('0' <= sig && sig <= '9') { _Sample_Set[i].dst = 10 * _Sample_Set[i].dst + sig - 48; g_Serial_Signal.putc(char(sig)); } else if(sig == 0x08) { _Sample_Set[i].dst = 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 { //_Data_Input_Type == SYSTEM _Sample_Set[i].dst = g_Serial_Signal.getc(); } // if input data is over the bound calibrate it below if (_Sample_Set[i].dst < 0) _Sample_Set[i].dst = 0; else if (_ENUM < _Sample_Set[i].dst) _Sample_Set[i].dst = _ENUM; // // 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].vol = 0; for(int j = 0; j < 10; j++) { //unsigned short 's range [0 , 65535] //the Number of significant figures of read voltage is 3 or 4. tmp_vol = g_Sensor_Voltage.read_u16(); #ifdef DEBUG g_Serial_Signal.printf("%d,",tmp_vol); #endif _Sample_Set[i].vol += (tmp_vol / 10); wait(0.1); } #ifdef DEBUG g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].vol); #endif } // // Sort set data array in distanceAscending order // // Bucket sort for(int i = 0; i < _ENUM; i++) tmp_set[i].dst = 0xaaaa; tmp = 0; for(int i = 0; i < _Sample_Num; i++) { // use dst as index for dst range [2,20] if (tmp_set[_Sample_Set[i].dst].dst == 0xaaaa) { tmp_set[_Sample_Set[i].dst].dst = _Sample_Set[i].dst; tmp_set[_Sample_Set[i].dst].vol = _Sample_Set[i].vol; } else { // if a same dst has been input, calcurate mean. tmp_set[_Sample_Set[i].dst].vol += _Sample_Set[i].vol; tmp_set[_Sample_Set[i].dst].vol /= 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].dst != 0xaaaa) { _Sample_Set[i - tmp].dst = tmp_set[i].dst; _Sample_Set[i - tmp].vol = tmp_set[i].vol; } else // if no data, skip it tmp++; } } // // Function to define _u_spline, specific constants of spline. // void TRP105FS::_makeSpline() { // x: dst, distance // y: vol, voltage // // 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].dst - _Sample_Set[i].dst); //(unsigned short)(_Sample_Set[i + 1].dst - _Sample_Set[i].dst); for(int i = 0; i < _Sample_Num - 2; i++) v[i] = 6 * ( ((double)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol)) / h[i + 1] //(unsigned short)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol) / h[i + 1] - ((double)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol)) / h[i] //(unsigned short)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol) / 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; // voltage 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].dst) <= arg_x) { while (!((double)(_Sample_Set[itv].dst) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].dst))) { 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].dst - _Sample_Set[itv].dst); b = (double)(_u_spline[itv]) / 2.0; c = (double)(_Sample_Set[itv + 1].vol - _Sample_Set[itv].vol) / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst) - (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0; d = (double)(_Sample_Set[itv].vol); // cubic spline expression y = a * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst)) + b * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst)) + c * (arg_x - (double)(_Sample_Set[itv].dst)) + 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); } void TRP105FS::calibrateSensor() { _sampleData(); _makeSpline(); for(int i = 0; i < _ENUM; i++) { _Set[i].dst = i; _Set[i].vol = _getSplineYof((double)(_Set[i].dst)); _Threshold[i] = _getSplineYof((double)(_Set[i].dst) + 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].dst, sizeof(unsigned short), 1, fp); fputc(0x2c, fp); fwrite(&_Set[i].vol, 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].dst, sizeof(unsigned short), 1, fp); fputc(0x2c, fp); fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp); fputc(0x3b, fp); } fclose(fp); } void TRP105FS::printThresholds() { for(int i = 0; i < _ENUM; i++) g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]); } 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].dst, sizeof(unsigned short), 1, fp); fread(&tmp, sizeof(char), 1, fp); #ifdef DEBUG2 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp); #endif fread(&_Set[i].vol, sizeof(unsigned short), 1, fp); fread(&tmp, sizeof(char), 1, fp); #ifdef DEBUG2 g_Serial_Signal.printf("%d%c", _Set[i].vol, 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].dst, sizeof(unsigned short), 1, fp); fread(&tmp, sizeof(char),1,fp); fread(&_Sample_Set[i].vol, 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].dst, sizeof(unsigned short), 1, fp); fputc(0x2c, fp); fwrite(&_Set[i].vol, 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].dst, sizeof(unsigned short), 1, fp); fputc(0x2c, fp); fwrite(&_Sample_Set[i].vol, 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].dst, sizeof(unsigned short), 1, fp); fread(&tmp, sizeof(char), 1, fp); #ifdef DEBUG3 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp); #endif fread(&_Set[i].vol, sizeof(unsigned short), 1, fp); fread(&tmp, sizeof(char), 1, fp); #ifdef DEBUG3 g_Serial_Signal.printf("%d%c", _Set[i].vol, 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].dst, sizeof(unsigned short), 1, fp); fread(&tmp, sizeof(char),1,fp); #ifdef DEBUG3 g_Serial_Signal.printf("%d%c", _Sample_Set[i].dst, tmp); #endif fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp); fread(&tmp, sizeof(char),1,fp); #ifdef DEBUG3 g_Serial_Signal.printf("%d%c", _Sample_Set[i].vol, tmp); #endif } fclose(fp); free(filepath); } void TRP105FS::printOutData() { FILE *fp; fp = fopen("/local/log.txt", "w"); // open file in writing mode fprintf(fp, "dst, vol,(threshold)\n"); for(int i = 0; i < _ENUM; i++) { fprintf(fp, "%d,%d,(%d)\n", _Set[i].dst, _Set[i].vol, _Threshold[i]); } fprintf(fp, "\nSample:dst, vol\n"); for(int i = 0; i < _Sample_Num; i++) { fprintf(fp, "%d,%d\n", _Sample_Set[i].dst, _Sample_Set[i].vol); } 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].vol); } fprintf(fp, "\n"); fclose(fp); }