Akifumi Takahashi / TRP105F_Spline

This lib is supposed to be used as a sensor's calibration or control program. This makes Cubic Spline Model from some sample plots(sets of (value, voltage)), and then discretize the model (dividing the range of voltage into some steps) in order to use the calibrated model data without getting the INVERSE function.

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 }