Kenta Tanabe / TRP105F_Spline_for_child

for child

Fork of TRP105F_Spline by Akifumi Takahashi

TRP105F_Spline.cpp@0:e85788b14028, 2016-02-12 (annotated)

Committer:
aktk
Date:
Fri Feb 12 11:02:15 2016 +0000
Revision:
0:e85788b14028
Child:
1:2053662b1167
Distance data range : [0:1024] -> [0:256];

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aktk 0:e85788b14028 1 #define DEBUG
aktk 0:e85788b14028 2 #include "TRP105F_Spline.h"
aktk 0:e85788b14028 3
aktk 0:e85788b14028 4 // To get voltage of TRP105F
aktk 0:e85788b14028 5 AnalogIn g_Sensor_Voltage(p16);
aktk 0:e85788b14028 6 // To get sample distance via seral com
aktk 0:e85788b14028 7 Serial g_Serial_Signal(USBTX, USBRX);
aktk 0:e85788b14028 8
aktk 0:e85788b14028 9 LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる）
aktk 0:e85788b14028 10 // for debug
aktk 0:e85788b14028 11 #ifdef DEBUG
aktk 0:e85788b14028 12 DigitalOut led1(LED1);
aktk 0:e85788b14028 13 DigitalOut led2(LED2);
aktk 0:e85788b14028 14 DigitalOut led3(LED3);
aktk 0:e85788b14028 15 DigitalOut led4(LED4);
aktk 0:e85788b14028 16 #endif
aktk 0:e85788b14028 17
aktk 0:e85788b14028 18 TRP105FS::TRP105FS()
aktk 0:e85788b14028 19 :_Data_Input_Type(SYSTEM)
aktk 0:e85788b14028 20 {
aktk 0:e85788b14028 21 _Sample_Num = 5;
aktk 0:e85788b14028 22 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 23 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 0:e85788b14028 24
aktk 0:e85788b14028 25 //calibrateSensor();
aktk 0:e85788b14028 26 }
aktk 0:e85788b14028 27
aktk 0:e85788b14028 28 TRP105FS::TRP105FS(
aktk 0:e85788b14028 29 unsigned int arg_num
aktk 0:e85788b14028 30 )
aktk 0:e85788b14028 31 :_Data_Input_Type(SYSTEM)
aktk 0:e85788b14028 32 {
aktk 0:e85788b14028 33 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 0:e85788b14028 34 else _Sample_Num = arg_num;
aktk 0:e85788b14028 35
aktk 0:e85788b14028 36 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 37 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 0:e85788b14028 38
aktk 0:e85788b14028 39 //calibrateSensor();
aktk 0:e85788b14028 40 }
aktk 0:e85788b14028 41
aktk 0:e85788b14028 42 TRP105FS::TRP105FS(
aktk 0:e85788b14028 43 unsigned int arg_num,
aktk 0:e85788b14028 44 DataInType arg_dit
aktk 0:e85788b14028 45 )
aktk 0:e85788b14028 46 :_Data_Input_Type(arg_dit)
aktk 0:e85788b14028 47 {
aktk 0:e85788b14028 48 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 0:e85788b14028 49 else _Sample_Num = arg_num;
aktk 0:e85788b14028 50
aktk 0:e85788b14028 51 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 52 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 0:e85788b14028 53
aktk 0:e85788b14028 54 //calibrateSensor();
aktk 0:e85788b14028 55 }
aktk 0:e85788b14028 56
aktk 0:e85788b14028 57 TRP105FS::~TRP105FS()
aktk 0:e85788b14028 58 {
aktk 0:e85788b14028 59 free(_Sample_Set);
aktk 0:e85788b14028 60 free(_u_spline);
aktk 0:e85788b14028 61 }
aktk 0:e85788b14028 62
aktk 0:e85788b14028 63 unsigned short TRP105FS::getDistance()
aktk 0:e85788b14028 64 {
aktk 0:e85788b14028 65 int idx;
aktk 0:e85788b14028 66 unsigned short pv = 0;
aktk 0:e85788b14028 67
aktk 0:e85788b14028 68 //low pass filter
aktk 0:e85788b14028 69 for(int i = 0; i < 10; i++)
aktk 0:e85788b14028 70 pv += g_Sensor_Voltage.read_u16() / 10;
aktk 0:e85788b14028 71
aktk 0:e85788b14028 72 idx = _getNearest(_LIDX, _RIDX, pv);
aktk 0:e85788b14028 73
aktk 0:e85788b14028 74 if (idx != 0xFFFF) // unless occuring error
aktk 0:e85788b14028 75 return _Set[idx].dst;
aktk 0:e85788b14028 76 else
aktk 0:e85788b14028 77 return 0xFFFF;
aktk 0:e85788b14028 78 }
aktk 0:e85788b14028 79
aktk 0:e85788b14028 80 /*
aktk 0:e85788b14028 81 Function to find a set whose vol member is nearest a voltage from the sensor, recursively.
aktk 0:e85788b14028 82
aktk 0:e85788b14028 83 SHORT LONG
aktk 0:e85788b14028 84 +------------> HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+
aktk 0:e85788b14028 85 |(if voltage form sensor < threshold[cidx]) ||| (if threshold[cidx] < voltage form sensor)|
aktk 0:e85788b14028 86 | HIGH[lidx , ... , cidx]LOW ||| HIGH[cidx+1 , ... , ridx]LOW |
aktk 0:e85788b14028 87 | | | |
aktk 0:e85788b14028 88 +----------------+ +---------------+
aktk 0:e85788b14028 89 */
aktk 0:e85788b14028 90 int TRP105FS::_getNearest(
aktk 0:e85788b14028 91 int arg_lidx,
aktk 0:e85788b14028 92 int arg_ridx,
aktk 0:e85788b14028 93 unsigned short arg_vol
aktk 0:e85788b14028 94 )
aktk 0:e85788b14028 95 {
aktk 0:e85788b14028 96 int cidx = (arg_lidx + arg_ridx) / 2;
aktk 0:e85788b14028 97
aktk 0:e85788b14028 98 // When the number of element to compare is only one, return it as result.
aktk 0:e85788b14028 99 if(arg_lidx == arg_ridx)
aktk 0:e85788b14028 100 return cidx;
aktk 0:e85788b14028 101 // If the voltage from the sensor is lower than the center threshold
aktk 0:e85788b14028 102 // (_set[cidx] > _threshold[cidx] > _set[cidx+1])
aktk 0:e85788b14028 103 else if(arg_vol > _Threshold[cidx])
aktk 0:e85788b14028 104 return _getNearest(arg_lidx, cidx, arg_vol);
aktk 0:e85788b14028 105 // If the voltage from the sensor is higher than the center threshold
aktk 0:e85788b14028 106 else if(arg_vol < _Threshold[cidx])
aktk 0:e85788b14028 107 return _getNearest(cidx + 1, arg_ridx, arg_vol);
aktk 0:e85788b14028 108 // If the voltage from the sensor eauals the center threshold
aktk 0:e85788b14028 109 else //(arg_vol == _Treshold[cidx].vol)
aktk 0:e85788b14028 110 return cidx;
aktk 0:e85788b14028 111 }
aktk 0:e85788b14028 112
aktk 0:e85788b14028 113 void TRP105FS::_sampleData()
aktk 0:e85788b14028 114 {
aktk 0:e85788b14028 115 int tmp;
aktk 0:e85788b14028 116 char sig;
aktk 0:e85788b14028 117 unsigned short tmp_vol;
aktk 0:e85788b14028 118 VDset tmp_set[_ENUM]; // for bucket sort
aktk 0:e85788b14028 119
aktk 0:e85788b14028 120 // Evry set, first get dst data via serai com,
aktk 0:e85788b14028 121 // next, get vol data,
aktk 0:e85788b14028 122 // and then do same for next index set.
aktk 0:e85788b14028 123 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 124 //
aktk 0:e85788b14028 125 // Recieve a Distance datus and store it into member
aktk 0:e85788b14028 126 //
aktk 0:e85788b14028 127 if(_Data_Input_Type == KEYBORD) {
aktk 0:e85788b14028 128 g_Serial_Signal.putc('>');
aktk 0:e85788b14028 129 _Sample_Set[i].dst = 0;
aktk 0:e85788b14028 130 do {
aktk 0:e85788b14028 131 sig = g_Serial_Signal.getc();
aktk 0:e85788b14028 132 if('0' <= sig && sig <= '9') {
aktk 0:e85788b14028 133 _Sample_Set[i].dst = 10 * _Sample_Set[i].dst + sig - 48;
aktk 0:e85788b14028 134 g_Serial_Signal.putc(char(sig));
aktk 0:e85788b14028 135 } else if(sig == 0x08) {
aktk 0:e85788b14028 136 _Sample_Set[i].dst = 0;
aktk 0:e85788b14028 137 g_Serial_Signal.printf("[canseled!]");
aktk 0:e85788b14028 138 g_Serial_Signal.putc('\n');
aktk 0:e85788b14028 139 g_Serial_Signal.putc('>');
aktk 0:e85788b14028 140 }
aktk 0:e85788b14028 141 } while (!(sig == 0x0a || sig == 0x0d));
aktk 0:e85788b14028 142 g_Serial_Signal.putc('\n');
aktk 0:e85788b14028 143 } else { //_Data_Input_Type == SYSTEM
aktk 0:e85788b14028 144 _Sample_Set[i].dst = g_Serial_Signal.getc();
aktk 0:e85788b14028 145 }
aktk 0:e85788b14028 146
aktk 0:e85788b14028 147 // if input data is over the bound calibrate it below
aktk 0:e85788b14028 148 if (_Sample_Set[i].dst < 0)
aktk 0:e85788b14028 149 _Sample_Set[i].dst = 0;
aktk 0:e85788b14028 150 else if (_ENUM < _Sample_Set[i].dst)
aktk 0:e85788b14028 151 _Sample_Set[i].dst = _ENUM;
aktk 0:e85788b14028 152 //
aktk 0:e85788b14028 153 // Recieve a Voltage datus and store it into member
aktk 0:e85788b14028 154 //
aktk 0:e85788b14028 155 // LOW PASS FILTERED
aktk 0:e85788b14028 156 // Get 10 data and store mean as a sample.
aktk 0:e85788b14028 157 // After get one original sample, system waits for 0.1 sec,
aktk 0:e85788b14028 158 // thus it takes 1 sec evry sampling.
aktk 0:e85788b14028 159 _Sample_Set[i].vol = 0;
aktk 0:e85788b14028 160 for(int j = 0; j < 10; j++) {
aktk 0:e85788b14028 161 //unsigned short 's range [0 , 65535]
aktk 0:e85788b14028 162 //the Number of significant figures of read voltage is 3 or 4.
aktk 0:e85788b14028 163 tmp_vol = g_Sensor_Voltage.read_u16();
aktk 0:e85788b14028 164 #ifdef DEBUG
aktk 0:e85788b14028 165 g_Serial_Signal.printf("%d,",tmp_vol);
aktk 0:e85788b14028 166 #endif
aktk 0:e85788b14028 167 _Sample_Set[i].vol += (tmp_vol / 10);
aktk 0:e85788b14028 168 wait(0.1);
aktk 0:e85788b14028 169 }
aktk 0:e85788b14028 170 #ifdef DEBUG
aktk 0:e85788b14028 171 g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].vol);
aktk 0:e85788b14028 172 #endif
aktk 0:e85788b14028 173 }
aktk 0:e85788b14028 174 //
aktk 0:e85788b14028 175 // Sort set data array in distanceAscending order
aktk 0:e85788b14028 176 //
aktk 0:e85788b14028 177 // Bucket sort
aktk 0:e85788b14028 178 for(int i = 0; i < _ENUM; i++)
aktk 0:e85788b14028 179 tmp_set[i].dst = 0xaaaa;
aktk 0:e85788b14028 180 tmp = 0;
aktk 0:e85788b14028 181 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 182 // use dst as index for dst range [2,20]
aktk 0:e85788b14028 183 if (tmp_set[_Sample_Set[i].dst].dst == 0xaaaa) {
aktk 0:e85788b14028 184 tmp_set[_Sample_Set[i].dst].dst = _Sample_Set[i].dst;
aktk 0:e85788b14028 185 tmp_set[_Sample_Set[i].dst].vol = _Sample_Set[i].vol;
aktk 0:e85788b14028 186 } else { // if a same dst has been input, calcurate mean.
aktk 0:e85788b14028 187 tmp_set[_Sample_Set[i].dst].vol += _Sample_Set[i].vol;
aktk 0:e85788b14028 188 tmp_set[_Sample_Set[i].dst].vol /= 2;
aktk 0:e85788b14028 189 tmp++;
aktk 0:e85788b14028 190 }
aktk 0:e85788b14028 191 }
aktk 0:e85788b14028 192 #ifdef DEBUG
aktk 0:e85788b14028 193 g_Serial_Signal.printf("%d\n", _Sample_Num );
aktk 0:e85788b14028 194 #endif
aktk 0:e85788b14028 195 // substruct tmp from number of sample.
aktk 0:e85788b14028 196 _Sample_Num -= tmp;
aktk 0:e85788b14028 197
aktk 0:e85788b14028 198 #ifdef DEBUG
aktk 0:e85788b14028 199 g_Serial_Signal.printf("tmp: %d\n", tmp );
aktk 0:e85788b14028 200 #endif
aktk 0:e85788b14028 201 // apply sort on _Sample_Set
aktk 0:e85788b14028 202 tmp = 0;
aktk 0:e85788b14028 203 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 204 if(tmp_set[i].dst != 0xaaaa) {
aktk 0:e85788b14028 205 _Sample_Set[i - tmp].dst = tmp_set[i].dst;
aktk 0:e85788b14028 206 _Sample_Set[i - tmp].vol = tmp_set[i].vol;
aktk 0:e85788b14028 207 } else // if no data, skip it
aktk 0:e85788b14028 208 tmp++;
aktk 0:e85788b14028 209 }
aktk 0:e85788b14028 210 }
aktk 0:e85788b14028 211
aktk 0:e85788b14028 212 //
aktk 0:e85788b14028 213 // Function to define _u_spline, specific constants of spline.
aktk 0:e85788b14028 214 //
aktk 0:e85788b14028 215 void TRP105FS::_makeSpline()
aktk 0:e85788b14028 216 {
aktk 0:e85788b14028 217 // x: dst, distance
aktk 0:e85788b14028 218 // y: vol, voltage
aktk 0:e85788b14028 219 //
aktk 0:e85788b14028 220 // N: max of index <=> (_Sample_Num - 1)
aktk 0:e85788b14028 221 //
aktk 0:e85788b14028 222 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 223 // i:[0,N]
aktk 0:e85788b14028 224 // _u_spline[0] = _u_spline[N] = 0
aktk 0:e85788b14028 225 //
aktk 0:e85788b14028 226 // h[i] = x[i+1] - x[i]
aktk 0:e85788b14028 227 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
aktk 0:e85788b14028 228 double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 0:e85788b14028 229 //unsigned short *h __attribute__((at(0x20080000)));
aktk 0:e85788b14028 230 //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
aktk 0:e85788b14028 231 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
aktk 0:e85788b14028 232 // i:[0,N-2]
aktk 0:e85788b14028 233 double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 234 //unsigned short *v __attribute__((at(0x20080100)));
aktk 0:e85788b14028 235 //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 236 // temporary array whose num of elm equals v array
aktk 0:e85788b14028 237 double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 238 //unsigned short *w __attribute__((at(0x20080200)));
aktk 0:e85788b14028 239 //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 240 //
aktk 0:e85788b14028 241 // [ 2(h[0]+h[1]) , h[1] , O ] [_u[1] ] [v[0] ]
aktk 0:e85788b14028 242 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [_u[2] ] [v[1] ]
aktk 0:e85788b14028 243 // [ ... ] * [... ] = [... ]
aktk 0:e85788b14028 244 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 245 // [ ... ] [ ... ] [ ... ]
aktk 0:e85788b14028 246 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 247 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [_u[N-1]] [v[N-2]]
aktk 0:e85788b14028 248 //
aktk 0:e85788b14028 249 // For LU decomposition
aktk 0:e85788b14028 250 double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 251 //unsigned short *Upper __attribute__((at(0x20080300)));
aktk 0:e85788b14028 252 //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 253 double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 254 //unsigned short *Lower __attribute__((at(0x20080400)));
aktk 0:e85788b14028 255 //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 256 #ifdef DEBUG
aktk 0:e85788b14028 257 _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
aktk 0:e85788b14028 258 #endif
aktk 0:e85788b14028 259 for(int i = 0; i < _Sample_Num - 1; i++)
aktk 0:e85788b14028 260 h[i] = (double)(_Sample_Set[i + 1].dst - _Sample_Set[i].dst);
aktk 0:e85788b14028 261 //(unsigned short)(_Sample_Set[i + 1].dst - _Sample_Set[i].dst);
aktk 0:e85788b14028 262
aktk 0:e85788b14028 263 for(int i = 0; i < _Sample_Num - 2; i++)
aktk 0:e85788b14028 264 v[i] = 6 * (
aktk 0:e85788b14028 265 ((double)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol)) / h[i + 1]
aktk 0:e85788b14028 266 //(unsigned short)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol) / h[i + 1]
aktk 0:e85788b14028 267 -
aktk 0:e85788b14028 268 ((double)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol)) / h[i]
aktk 0:e85788b14028 269 //(unsigned short)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol) / h[i]
aktk 0:e85788b14028 270 );
aktk 0:e85788b14028 271
aktk 0:e85788b14028 272 //
aktk 0:e85788b14028 273 // LU decomposition
aktk 0:e85788b14028 274 //
aktk 0:e85788b14028 275 Upper[0] = 2 * (h[0] + h[1]);
aktk 0:e85788b14028 276 Lower[0] = 0;
aktk 0:e85788b14028 277 for (int i = 1; i < _Sample_Num - 2; i++) {
aktk 0:e85788b14028 278 Lower[i] = h[i] / Upper[i - 1];
aktk 0:e85788b14028 279 Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
aktk 0:e85788b14028 280 }
aktk 0:e85788b14028 281
aktk 0:e85788b14028 282
aktk 0:e85788b14028 283 //
aktk 0:e85788b14028 284 // forward substitution
aktk 0:e85788b14028 285 //
aktk 0:e85788b14028 286 w[0] = v[0];
aktk 0:e85788b14028 287 for (int i = 1; i < _Sample_Num - 2; i ++) {
aktk 0:e85788b14028 288 w[i] = v[i] - Lower[i] * w[i-1];
aktk 0:e85788b14028 289 }
aktk 0:e85788b14028 290
aktk 0:e85788b14028 291
aktk 0:e85788b14028 292 //
aktk 0:e85788b14028 293 // backward substitution
aktk 0:e85788b14028 294 //
aktk 0:e85788b14028 295 _u_spline[_Sample_Num - 2] = w[_Sample_Num - 3] / Upper[_Sample_Num - 3];
aktk 0:e85788b14028 296 for(int i = _Sample_Num - 3; i > 0; i--) {
aktk 0:e85788b14028 297 _u_spline[i] = (w[(i - 1)] - h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
aktk 0:e85788b14028 298 }
aktk 0:e85788b14028 299
aktk 0:e85788b14028 300 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 301 _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;
aktk 0:e85788b14028 302
aktk 0:e85788b14028 303 #ifdef DEBUG
aktk 0:e85788b14028 304 _printOutData(h, _Sample_Num - 1, "h");
aktk 0:e85788b14028 305 _printOutData(v, _Sample_Num - 2, "v");
aktk 0:e85788b14028 306 _printOutData(w, _Sample_Num - 2, "w");
aktk 0:e85788b14028 307 _printOutData(Upper, _Sample_Num - 2, "Upper");
aktk 0:e85788b14028 308 _printOutData(Lower, _Sample_Num - 2, "Lower");
aktk 0:e85788b14028 309 _printOutData(_u_spline, _Sample_Num, "u");
aktk 0:e85788b14028 310 #endif
aktk 0:e85788b14028 311 free(h);
aktk 0:e85788b14028 312 free(v);
aktk 0:e85788b14028 313 free(w);
aktk 0:e85788b14028 314 free(Upper);
aktk 0:e85788b14028 315 free(Lower);
aktk 0:e85788b14028 316 }
aktk 0:e85788b14028 317 //
aktk 0:e85788b14028 318 // Function to return Voltage for distance.
aktk 0:e85788b14028 319 //
aktk 0:e85788b14028 320 unsigned short TRP105FS:: _getSplineYof(
aktk 0:e85788b14028 321 double arg_x // the argument is supposed as distance [mm]
aktk 0:e85788b14028 322 )
aktk 0:e85788b14028 323 {
aktk 0:e85788b14028 324 double y; // voltage calculated by spline polynomial
aktk 0:e85788b14028 325 double a,b,c,d; // which is specific constant of spline, and can be expressed with _u.
aktk 0:e85788b14028 326 int itv = 0; // interval(section) of interpolation
aktk 0:e85788b14028 327 // the number of interval is less 1 than the number of sample sets,
aktk 0:e85788b14028 328 // which means the max number of interval is _Sample_num - 2.
aktk 0:e85788b14028 329 if((double)(_Sample_Set[0].dst) <= arg_x) {
aktk 0:e85788b14028 330 while (!((double)(_Sample_Set[itv].dst) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].dst))) {
aktk 0:e85788b14028 331 itv++;
aktk 0:e85788b14028 332 if(itv > _Sample_Num - 2) {
aktk 0:e85788b14028 333 itv = _Sample_Num - 2;
aktk 0:e85788b14028 334 break;
aktk 0:e85788b14028 335 }
aktk 0:e85788b14028 336 }
aktk 0:e85788b14028 337 }
aktk 0:e85788b14028 338 a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst);
aktk 0:e85788b14028 339 b = (double)(_u_spline[itv]) / 2.0;
aktk 0:e85788b14028 340 c = (double)(_Sample_Set[itv + 1].vol - _Sample_Set[itv].vol) / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst)
aktk 0:e85788b14028 341 -
aktk 0:e85788b14028 342 (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0;
aktk 0:e85788b14028 343 d = (double)(_Sample_Set[itv].vol);
aktk 0:e85788b14028 344 // cubic spline expression
aktk 0:e85788b14028 345 y = a * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
aktk 0:e85788b14028 346 +
aktk 0:e85788b14028 347 b * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
aktk 0:e85788b14028 348 +
aktk 0:e85788b14028 349 c * (arg_x - (double)(_Sample_Set[itv].dst))
aktk 0:e85788b14028 350 +
aktk 0:e85788b14028 351 d;
aktk 0:e85788b14028 352
aktk 0:e85788b14028 353 #ifdef DEBUG2
aktk 0:e85788b14028 354 g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
aktk 0:e85788b14028 355 g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
aktk 0:e85788b14028 356 g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
aktk 0:e85788b14028 357 #endif
aktk 0:e85788b14028 358
aktk 0:e85788b14028 359 return ((unsigned short)(int)y);
aktk 0:e85788b14028 360 }
aktk 0:e85788b14028 361
aktk 0:e85788b14028 362 void TRP105FS::calibrateSensor()
aktk 0:e85788b14028 363 {
aktk 0:e85788b14028 364 _sampleData();
aktk 0:e85788b14028 365 _makeSpline();
aktk 0:e85788b14028 366
aktk 0:e85788b14028 367 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 368 _Set[i].dst = i;
aktk 0:e85788b14028 369 _Set[i].vol = _getSplineYof((double)(_Set[i].dst));
aktk 0:e85788b14028 370 _Threshold[i] = _getSplineYof((double)(_Set[i].dst) + 0.5);
aktk 0:e85788b14028 371 #ifdef DEBUG2
aktk 0:e85788b14028 372 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 0:e85788b14028 373 #endif
aktk 0:e85788b14028 374 }
aktk 0:e85788b14028 375 }
aktk 0:e85788b14028 376
aktk 0:e85788b14028 377 void TRP105FS::saveSetting()
aktk 0:e85788b14028 378 {
aktk 0:e85788b14028 379 FILE *fp;
aktk 0:e85788b14028 380
aktk 0:e85788b14028 381 fp = fopen("/local/savedata.log", "wb");
aktk 0:e85788b14028 382
aktk 0:e85788b14028 383 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 384 fwrite(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 385 fputc(0x2c, fp);
aktk 0:e85788b14028 386 fwrite(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 387 fputc(0x2c, fp);
aktk 0:e85788b14028 388 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 389 fputc(0x3b, fp);
aktk 0:e85788b14028 390 }
aktk 0:e85788b14028 391 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 0:e85788b14028 392 fputc(0x3b, fp);
aktk 0:e85788b14028 393 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 394 fwrite(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 395 fputc(0x2c, fp);
aktk 0:e85788b14028 396 fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 397 fputc(0x3b, fp);
aktk 0:e85788b14028 398 }
aktk 0:e85788b14028 399 fclose(fp);
aktk 0:e85788b14028 400
aktk 0:e85788b14028 401 }
aktk 0:e85788b14028 402
aktk 0:e85788b14028 403 void TRP105FS::printThresholds()
aktk 0:e85788b14028 404 {
aktk 0:e85788b14028 405 for(int i = 0; i < _ENUM; i++)
aktk 0:e85788b14028 406 g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
aktk 0:e85788b14028 407 }
aktk 0:e85788b14028 408 void TRP105FS::loadSetting()
aktk 0:e85788b14028 409 {
aktk 0:e85788b14028 410 FILE *fp;
aktk 0:e85788b14028 411 char tmp;
aktk 0:e85788b14028 412
aktk 0:e85788b14028 413 //sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 414 //fp = fopen(filepath, "rb");
aktk 0:e85788b14028 415 fp = fopen("/local/savedata.log", "rb");
aktk 0:e85788b14028 416 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 417
aktk 0:e85788b14028 418 fread(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 419 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 420 #ifdef DEBUG2
aktk 0:e85788b14028 421 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp);
aktk 0:e85788b14028 422 #endif
aktk 0:e85788b14028 423
aktk 0:e85788b14028 424 fread(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 425 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 426 #ifdef DEBUG2
aktk 0:e85788b14028 427 g_Serial_Signal.printf("%d%c", _Set[i].vol, tmp);
aktk 0:e85788b14028 428 #endif
aktk 0:e85788b14028 429
aktk 0:e85788b14028 430 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 431 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 432 #ifdef DEBUG2
aktk 0:e85788b14028 433 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 434 #endif
aktk 0:e85788b14028 435 }
aktk 0:e85788b14028 436
aktk 0:e85788b14028 437 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 438 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 439
aktk 0:e85788b14028 440 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 441 fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 442 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 443 fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 444 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 445 }
aktk 0:e85788b14028 446 fclose(fp);
aktk 0:e85788b14028 447 }
aktk 0:e85788b14028 448
aktk 0:e85788b14028 449
aktk 0:e85788b14028 450 void TRP105FS::saveSetting(
aktk 0:e85788b14028 451 const char *filename
aktk 0:e85788b14028 452 )
aktk 0:e85788b14028 453 {
aktk 0:e85788b14028 454 FILE *fp;
aktk 0:e85788b14028 455 char *filepath;
aktk 0:e85788b14028 456 int fnnum = 0;
aktk 0:e85788b14028 457
aktk 0:e85788b14028 458 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 459 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 460
aktk 0:e85788b14028 461 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 462 fp = fopen(filepath, "wb");
aktk 0:e85788b14028 463
aktk 0:e85788b14028 464 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 465 fwrite(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 466 fputc(0x2c, fp);
aktk 0:e85788b14028 467 fwrite(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 468 fputc(0x2c, fp);
aktk 0:e85788b14028 469 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 470 fputc(0x3b, fp);
aktk 0:e85788b14028 471 }
aktk 0:e85788b14028 472 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 0:e85788b14028 473 fputc(0x3b, fp);
aktk 0:e85788b14028 474 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 475 fwrite(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 476 fputc(0x2c, fp);
aktk 0:e85788b14028 477 fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 478 fputc(0x3b, fp);
aktk 0:e85788b14028 479 }
aktk 0:e85788b14028 480 fclose(fp);
aktk 0:e85788b14028 481 free(filepath);
aktk 0:e85788b14028 482 }
aktk 0:e85788b14028 483
aktk 0:e85788b14028 484 void TRP105FS::loadSetting(
aktk 0:e85788b14028 485 const char *filename
aktk 0:e85788b14028 486 )
aktk 0:e85788b14028 487 {
aktk 0:e85788b14028 488 FILE *fp;
aktk 0:e85788b14028 489 char *filepath;
aktk 0:e85788b14028 490 char tmp;
aktk 0:e85788b14028 491 int fnnum = 0;
aktk 0:e85788b14028 492
aktk 0:e85788b14028 493 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 494 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 495
aktk 0:e85788b14028 496 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 497 fp = fopen(filepath, "rb");
aktk 0:e85788b14028 498 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 499
aktk 0:e85788b14028 500 fread(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 501 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 502 #ifdef DEBUG3
aktk 0:e85788b14028 503 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp);
aktk 0:e85788b14028 504 #endif
aktk 0:e85788b14028 505
aktk 0:e85788b14028 506 fread(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 507 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 508 #ifdef DEBUG3
aktk 0:e85788b14028 509 g_Serial_Signal.printf("%d%c", _Set[i].vol, tmp);
aktk 0:e85788b14028 510 #endif
aktk 0:e85788b14028 511
aktk 0:e85788b14028 512 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 513 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 514 #ifdef DEBUG3
aktk 0:e85788b14028 515 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 516 #endif
aktk 0:e85788b14028 517 }
aktk 0:e85788b14028 518
aktk 0:e85788b14028 519 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 520 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 521 #ifdef DEBUG3
aktk 0:e85788b14028 522 g_Serial_Signal.printf("%d%c\n",_Sample_Num, tmp);
aktk 0:e85788b14028 523 #endif
aktk 0:e85788b14028 524
aktk 0:e85788b14028 525 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 526 fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 527 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 528 #ifdef DEBUG3
aktk 0:e85788b14028 529 g_Serial_Signal.printf("%d%c", _Sample_Set[i].dst, tmp);
aktk 0:e85788b14028 530 #endif
aktk 0:e85788b14028 531
aktk 0:e85788b14028 532 fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 533 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 534 #ifdef DEBUG3
aktk 0:e85788b14028 535 g_Serial_Signal.printf("%d%c", _Sample_Set[i].vol, tmp);
aktk 0:e85788b14028 536 #endif
aktk 0:e85788b14028 537 }
aktk 0:e85788b14028 538 fclose(fp);
aktk 0:e85788b14028 539 free(filepath);
aktk 0:e85788b14028 540 }
aktk 0:e85788b14028 541
aktk 0:e85788b14028 542 void TRP105FS::printOutData()
aktk 0:e85788b14028 543 {
aktk 0:e85788b14028 544 FILE *fp;
aktk 0:e85788b14028 545
aktk 0:e85788b14028 546 fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 0:e85788b14028 547 fprintf(fp, "dst, vol,(threshold)\n");
aktk 0:e85788b14028 548 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 549 fprintf(fp, "%d,%d,(%d)\n", _Set[i].dst, _Set[i].vol, _Threshold[i]);
aktk 0:e85788b14028 550 }
aktk 0:e85788b14028 551 fprintf(fp, "\nSample:dst, vol\n");
aktk 0:e85788b14028 552 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 553 fprintf(fp, "%d,%d\n", _Sample_Set[i].dst, _Sample_Set[i].vol);
aktk 0:e85788b14028 554 }
aktk 0:e85788b14028 555 fclose(fp);
aktk 0:e85788b14028 556
aktk 0:e85788b14028 557 }
aktk 0:e85788b14028 558 void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
aktk 0:e85788b14028 559 {
aktk 0:e85788b14028 560 FILE *fp;
aktk 0:e85788b14028 561 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 562 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 563 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 564 fprintf(fp, "%d, ", arg[i]);
aktk 0:e85788b14028 565 }
aktk 0:e85788b14028 566 fprintf(fp, "\n");
aktk 0:e85788b14028 567 fclose(fp);
aktk 0:e85788b14028 568 }
aktk 0:e85788b14028 569 void TRP105FS::_printOutData(double *arg, int num, char* name)
aktk 0:e85788b14028 570 {
aktk 0:e85788b14028 571 FILE *fp;
aktk 0:e85788b14028 572
aktk 0:e85788b14028 573 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 574 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 575 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 576 fprintf(fp, "%.2f, ", arg[i]);
aktk 0:e85788b14028 577 }
aktk 0:e85788b14028 578 fprintf(fp, "\n");
aktk 0:e85788b14028 579 fclose(fp);
aktk 0:e85788b14028 580 }
aktk 0:e85788b14028 581 void TRP105FS::_printOutData(VDset *arg, int num, char* name)
aktk 0:e85788b14028 582 {
aktk 0:e85788b14028 583 FILE *fp;
aktk 0:e85788b14028 584
aktk 0:e85788b14028 585 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 586 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 587 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 588 fprintf(fp, "%d, ", arg[i].vol);
aktk 0:e85788b14028 589 }
aktk 0:e85788b14028 590 fprintf(fp, "\n");
aktk 0:e85788b14028 591 fclose(fp);
aktk 0:e85788b14028 592 }