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@1:2053662b1167, 2016-02-16 (annotated)

Committer:
aktk
Date:
Tue Feb 16 07:52:38 2016 +0000
Revision:
1:2053662b1167
Parent:
0:e85788b14028
Child:
3:b56e933bebc2
data list range [0,255]. message has been little changed.

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 1:2053662b1167 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 1:2053662b1167 120 // For evry set,
aktk 1:2053662b1167 121 // 1, get dst data via serai com,
aktk 1:2053662b1167 122 // 2, get vol data,
aktk 0:e85788b14028 123 // and then do same for next index set.
aktk 0:e85788b14028 124 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 125 //
aktk 0:e85788b14028 126 // Recieve a Distance datus and store it into member
aktk 0:e85788b14028 127 //
aktk 0:e85788b14028 128 if(_Data_Input_Type == KEYBORD) {
aktk 0:e85788b14028 129 g_Serial_Signal.putc('>');
aktk 0:e85788b14028 130 _Sample_Set[i].dst = 0;
aktk 0:e85788b14028 131 do {
aktk 0:e85788b14028 132 sig = g_Serial_Signal.getc();
aktk 0:e85788b14028 133 if('0' <= sig && sig <= '9') {
aktk 0:e85788b14028 134 _Sample_Set[i].dst = 10 * _Sample_Set[i].dst + sig - 48;
aktk 0:e85788b14028 135 g_Serial_Signal.putc(char(sig));
aktk 0:e85788b14028 136 } else if(sig == 0x08) {
aktk 0:e85788b14028 137 _Sample_Set[i].dst = 0;
aktk 0:e85788b14028 138 g_Serial_Signal.printf("[canseled!]");
aktk 0:e85788b14028 139 g_Serial_Signal.putc('\n');
aktk 0:e85788b14028 140 g_Serial_Signal.putc('>');
aktk 0:e85788b14028 141 }
aktk 0:e85788b14028 142 } while (!(sig == 0x0a || sig == 0x0d));
aktk 0:e85788b14028 143 g_Serial_Signal.putc('\n');
aktk 0:e85788b14028 144 } else { //_Data_Input_Type == SYSTEM
aktk 0:e85788b14028 145 _Sample_Set[i].dst = g_Serial_Signal.getc();
aktk 0:e85788b14028 146 }
aktk 0:e85788b14028 147
aktk 0:e85788b14028 148 // if input data is over the bound calibrate it below
aktk 0:e85788b14028 149 if (_Sample_Set[i].dst < 0)
aktk 0:e85788b14028 150 _Sample_Set[i].dst = 0;
aktk 0:e85788b14028 151 else if (_ENUM < _Sample_Set[i].dst)
aktk 0:e85788b14028 152 _Sample_Set[i].dst = _ENUM;
aktk 0:e85788b14028 153 //
aktk 0:e85788b14028 154 // Recieve a Voltage datus and store it into member
aktk 0:e85788b14028 155 //
aktk 0:e85788b14028 156 // LOW PASS FILTERED
aktk 0:e85788b14028 157 // Get 10 data and store mean as a sample.
aktk 0:e85788b14028 158 // After get one original sample, system waits for 0.1 sec,
aktk 0:e85788b14028 159 // thus it takes 1 sec evry sampling.
aktk 0:e85788b14028 160 _Sample_Set[i].vol = 0;
aktk 0:e85788b14028 161 for(int j = 0; j < 10; j++) {
aktk 0:e85788b14028 162 //unsigned short 's range [0 , 65535]
aktk 0:e85788b14028 163 //the Number of significant figures of read voltage is 3 or 4.
aktk 0:e85788b14028 164 tmp_vol = g_Sensor_Voltage.read_u16();
aktk 0:e85788b14028 165 #ifdef DEBUG
aktk 0:e85788b14028 166 g_Serial_Signal.printf("%d,",tmp_vol);
aktk 0:e85788b14028 167 #endif
aktk 0:e85788b14028 168 _Sample_Set[i].vol += (tmp_vol / 10);
aktk 0:e85788b14028 169 wait(0.1);
aktk 0:e85788b14028 170 }
aktk 0:e85788b14028 171 #ifdef DEBUG
aktk 0:e85788b14028 172 g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].vol);
aktk 0:e85788b14028 173 #endif
aktk 0:e85788b14028 174 }
aktk 0:e85788b14028 175 //
aktk 0:e85788b14028 176 // Sort set data array in distanceAscending order
aktk 0:e85788b14028 177 //
aktk 0:e85788b14028 178 // Bucket sort
aktk 0:e85788b14028 179 for(int i = 0; i < _ENUM; i++)
aktk 0:e85788b14028 180 tmp_set[i].dst = 0xaaaa;
aktk 0:e85788b14028 181 tmp = 0;
aktk 0:e85788b14028 182 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 183 // use dst as index for dst range [2,20]
aktk 0:e85788b14028 184 if (tmp_set[_Sample_Set[i].dst].dst == 0xaaaa) {
aktk 0:e85788b14028 185 tmp_set[_Sample_Set[i].dst].dst = _Sample_Set[i].dst;
aktk 0:e85788b14028 186 tmp_set[_Sample_Set[i].dst].vol = _Sample_Set[i].vol;
aktk 0:e85788b14028 187 } else { // if a same dst has been input, calcurate mean.
aktk 0:e85788b14028 188 tmp_set[_Sample_Set[i].dst].vol += _Sample_Set[i].vol;
aktk 0:e85788b14028 189 tmp_set[_Sample_Set[i].dst].vol /= 2;
aktk 0:e85788b14028 190 tmp++;
aktk 0:e85788b14028 191 }
aktk 0:e85788b14028 192 }
aktk 0:e85788b14028 193 #ifdef DEBUG
aktk 0:e85788b14028 194 g_Serial_Signal.printf("%d\n", _Sample_Num );
aktk 0:e85788b14028 195 #endif
aktk 0:e85788b14028 196 // substruct tmp from number of sample.
aktk 0:e85788b14028 197 _Sample_Num -= tmp;
aktk 0:e85788b14028 198
aktk 0:e85788b14028 199 #ifdef DEBUG
aktk 0:e85788b14028 200 g_Serial_Signal.printf("tmp: %d\n", tmp );
aktk 0:e85788b14028 201 #endif
aktk 0:e85788b14028 202 // apply sort on _Sample_Set
aktk 0:e85788b14028 203 tmp = 0;
aktk 0:e85788b14028 204 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 205 if(tmp_set[i].dst != 0xaaaa) {
aktk 0:e85788b14028 206 _Sample_Set[i - tmp].dst = tmp_set[i].dst;
aktk 0:e85788b14028 207 _Sample_Set[i - tmp].vol = tmp_set[i].vol;
aktk 0:e85788b14028 208 } else // if no data, skip it
aktk 0:e85788b14028 209 tmp++;
aktk 0:e85788b14028 210 }
aktk 0:e85788b14028 211 }
aktk 0:e85788b14028 212
aktk 0:e85788b14028 213 //
aktk 0:e85788b14028 214 // Function to define _u_spline, specific constants of spline.
aktk 0:e85788b14028 215 //
aktk 0:e85788b14028 216 void TRP105FS::_makeSpline()
aktk 0:e85788b14028 217 {
aktk 0:e85788b14028 218 // x: dst, distance
aktk 0:e85788b14028 219 // y: vol, voltage
aktk 0:e85788b14028 220 //
aktk 0:e85788b14028 221 // N: max of index <=> (_Sample_Num - 1)
aktk 0:e85788b14028 222 //
aktk 0:e85788b14028 223 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 224 // i:[0,N]
aktk 0:e85788b14028 225 // _u_spline[0] = _u_spline[N] = 0
aktk 0:e85788b14028 226 //
aktk 0:e85788b14028 227 // h[i] = x[i+1] - x[i]
aktk 0:e85788b14028 228 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
aktk 0:e85788b14028 229 double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 0:e85788b14028 230 //unsigned short *h __attribute__((at(0x20080000)));
aktk 0:e85788b14028 231 //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
aktk 0:e85788b14028 232 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
aktk 0:e85788b14028 233 // i:[0,N-2]
aktk 0:e85788b14028 234 double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 235 //unsigned short *v __attribute__((at(0x20080100)));
aktk 0:e85788b14028 236 //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 237 // temporary array whose num of elm equals v array
aktk 0:e85788b14028 238 double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 239 //unsigned short *w __attribute__((at(0x20080200)));
aktk 0:e85788b14028 240 //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 241 //
aktk 0:e85788b14028 242 // [ 2(h[0]+h[1]) , h[1] , O ] [_u[1] ] [v[0] ]
aktk 0:e85788b14028 243 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [_u[2] ] [v[1] ]
aktk 0:e85788b14028 244 // [ ... ] * [... ] = [... ]
aktk 0:e85788b14028 245 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 246 // [ ... ] [ ... ] [ ... ]
aktk 0:e85788b14028 247 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 248 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [_u[N-1]] [v[N-2]]
aktk 0:e85788b14028 249 //
aktk 0:e85788b14028 250 // For LU decomposition
aktk 0:e85788b14028 251 double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 252 //unsigned short *Upper __attribute__((at(0x20080300)));
aktk 0:e85788b14028 253 //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 254 double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 255 //unsigned short *Lower __attribute__((at(0x20080400)));
aktk 0:e85788b14028 256 //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 257 #ifdef DEBUG
aktk 0:e85788b14028 258 _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
aktk 0:e85788b14028 259 #endif
aktk 0:e85788b14028 260 for(int i = 0; i < _Sample_Num - 1; i++)
aktk 0:e85788b14028 261 h[i] = (double)(_Sample_Set[i + 1].dst - _Sample_Set[i].dst);
aktk 0:e85788b14028 262 //(unsigned short)(_Sample_Set[i + 1].dst - _Sample_Set[i].dst);
aktk 0:e85788b14028 263
aktk 0:e85788b14028 264 for(int i = 0; i < _Sample_Num - 2; i++)
aktk 0:e85788b14028 265 v[i] = 6 * (
aktk 0:e85788b14028 266 ((double)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol)) / h[i + 1]
aktk 0:e85788b14028 267 //(unsigned short)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol) / h[i + 1]
aktk 0:e85788b14028 268 -
aktk 0:e85788b14028 269 ((double)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol)) / h[i]
aktk 0:e85788b14028 270 //(unsigned short)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol) / h[i]
aktk 0:e85788b14028 271 );
aktk 0:e85788b14028 272
aktk 0:e85788b14028 273 //
aktk 0:e85788b14028 274 // LU decomposition
aktk 0:e85788b14028 275 //
aktk 0:e85788b14028 276 Upper[0] = 2 * (h[0] + h[1]);
aktk 0:e85788b14028 277 Lower[0] = 0;
aktk 0:e85788b14028 278 for (int i = 1; i < _Sample_Num - 2; i++) {
aktk 0:e85788b14028 279 Lower[i] = h[i] / Upper[i - 1];
aktk 0:e85788b14028 280 Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
aktk 0:e85788b14028 281 }
aktk 0:e85788b14028 282
aktk 0:e85788b14028 283
aktk 0:e85788b14028 284 //
aktk 0:e85788b14028 285 // forward substitution
aktk 0:e85788b14028 286 //
aktk 0:e85788b14028 287 w[0] = v[0];
aktk 0:e85788b14028 288 for (int i = 1; i < _Sample_Num - 2; i ++) {
aktk 0:e85788b14028 289 w[i] = v[i] - Lower[i] * w[i-1];
aktk 0:e85788b14028 290 }
aktk 0:e85788b14028 291
aktk 0:e85788b14028 292
aktk 0:e85788b14028 293 //
aktk 0:e85788b14028 294 // backward substitution
aktk 0:e85788b14028 295 //
aktk 0:e85788b14028 296 _u_spline[_Sample_Num - 2] = w[_Sample_Num - 3] / Upper[_Sample_Num - 3];
aktk 0:e85788b14028 297 for(int i = _Sample_Num - 3; i > 0; i--) {
aktk 0:e85788b14028 298 _u_spline[i] = (w[(i - 1)] - h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
aktk 0:e85788b14028 299 }
aktk 0:e85788b14028 300
aktk 0:e85788b14028 301 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 302 _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;
aktk 0:e85788b14028 303
aktk 0:e85788b14028 304 #ifdef DEBUG
aktk 0:e85788b14028 305 _printOutData(h, _Sample_Num - 1, "h");
aktk 0:e85788b14028 306 _printOutData(v, _Sample_Num - 2, "v");
aktk 0:e85788b14028 307 _printOutData(w, _Sample_Num - 2, "w");
aktk 0:e85788b14028 308 _printOutData(Upper, _Sample_Num - 2, "Upper");
aktk 0:e85788b14028 309 _printOutData(Lower, _Sample_Num - 2, "Lower");
aktk 0:e85788b14028 310 _printOutData(_u_spline, _Sample_Num, "u");
aktk 0:e85788b14028 311 #endif
aktk 0:e85788b14028 312 free(h);
aktk 0:e85788b14028 313 free(v);
aktk 0:e85788b14028 314 free(w);
aktk 0:e85788b14028 315 free(Upper);
aktk 0:e85788b14028 316 free(Lower);
aktk 0:e85788b14028 317 }
aktk 0:e85788b14028 318 //
aktk 0:e85788b14028 319 // Function to return Voltage for distance.
aktk 0:e85788b14028 320 //
aktk 0:e85788b14028 321 unsigned short TRP105FS:: _getSplineYof(
aktk 0:e85788b14028 322 double arg_x // the argument is supposed as distance [mm]
aktk 0:e85788b14028 323 )
aktk 0:e85788b14028 324 {
aktk 0:e85788b14028 325 double y; // voltage calculated by spline polynomial
aktk 0:e85788b14028 326 double a,b,c,d; // which is specific constant of spline, and can be expressed with _u.
aktk 0:e85788b14028 327 int itv = 0; // interval(section) of interpolation
aktk 0:e85788b14028 328 // the number of interval is less 1 than the number of sample sets,
aktk 0:e85788b14028 329 // which means the max number of interval is _Sample_num - 2.
aktk 0:e85788b14028 330 if((double)(_Sample_Set[0].dst) <= arg_x) {
aktk 0:e85788b14028 331 while (!((double)(_Sample_Set[itv].dst) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].dst))) {
aktk 0:e85788b14028 332 itv++;
aktk 0:e85788b14028 333 if(itv > _Sample_Num - 2) {
aktk 0:e85788b14028 334 itv = _Sample_Num - 2;
aktk 0:e85788b14028 335 break;
aktk 0:e85788b14028 336 }
aktk 0:e85788b14028 337 }
aktk 0:e85788b14028 338 }
aktk 0:e85788b14028 339 a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst);
aktk 0:e85788b14028 340 b = (double)(_u_spline[itv]) / 2.0;
aktk 0:e85788b14028 341 c = (double)(_Sample_Set[itv + 1].vol - _Sample_Set[itv].vol) / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst)
aktk 0:e85788b14028 342 -
aktk 0:e85788b14028 343 (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 344 d = (double)(_Sample_Set[itv].vol);
aktk 0:e85788b14028 345 // cubic spline expression
aktk 0:e85788b14028 346 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 347 +
aktk 0:e85788b14028 348 b * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
aktk 0:e85788b14028 349 +
aktk 0:e85788b14028 350 c * (arg_x - (double)(_Sample_Set[itv].dst))
aktk 0:e85788b14028 351 +
aktk 0:e85788b14028 352 d;
aktk 0:e85788b14028 353
aktk 0:e85788b14028 354 #ifdef DEBUG2
aktk 0:e85788b14028 355 g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
aktk 0:e85788b14028 356 g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
aktk 0:e85788b14028 357 g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
aktk 0:e85788b14028 358 #endif
aktk 0:e85788b14028 359
aktk 0:e85788b14028 360 return ((unsigned short)(int)y);
aktk 0:e85788b14028 361 }
aktk 0:e85788b14028 362
aktk 0:e85788b14028 363 void TRP105FS::calibrateSensor()
aktk 0:e85788b14028 364 {
aktk 0:e85788b14028 365 _sampleData();
aktk 0:e85788b14028 366 _makeSpline();
aktk 0:e85788b14028 367
aktk 0:e85788b14028 368 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 369 _Set[i].dst = i;
aktk 0:e85788b14028 370 _Set[i].vol = _getSplineYof((double)(_Set[i].dst));
aktk 0:e85788b14028 371 _Threshold[i] = _getSplineYof((double)(_Set[i].dst) + 0.5);
aktk 0:e85788b14028 372 #ifdef DEBUG2
aktk 0:e85788b14028 373 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 0:e85788b14028 374 #endif
aktk 0:e85788b14028 375 }
aktk 0:e85788b14028 376 }
aktk 0:e85788b14028 377
aktk 0:e85788b14028 378 void TRP105FS::saveSetting()
aktk 0:e85788b14028 379 {
aktk 0:e85788b14028 380 FILE *fp;
aktk 0:e85788b14028 381
aktk 0:e85788b14028 382 fp = fopen("/local/savedata.log", "wb");
aktk 0:e85788b14028 383
aktk 0:e85788b14028 384 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 385 fwrite(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 386 fputc(0x2c, fp);
aktk 0:e85788b14028 387 fwrite(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 388 fputc(0x2c, fp);
aktk 0:e85788b14028 389 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 390 fputc(0x3b, fp);
aktk 0:e85788b14028 391 }
aktk 0:e85788b14028 392 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 0:e85788b14028 393 fputc(0x3b, fp);
aktk 0:e85788b14028 394 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 395 fwrite(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 396 fputc(0x2c, fp);
aktk 0:e85788b14028 397 fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 398 fputc(0x3b, fp);
aktk 0:e85788b14028 399 }
aktk 0:e85788b14028 400 fclose(fp);
aktk 0:e85788b14028 401
aktk 0:e85788b14028 402 }
aktk 0:e85788b14028 403
aktk 0:e85788b14028 404 void TRP105FS::printThresholds()
aktk 0:e85788b14028 405 {
aktk 0:e85788b14028 406 for(int i = 0; i < _ENUM; i++)
aktk 0:e85788b14028 407 g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
aktk 0:e85788b14028 408 }
aktk 0:e85788b14028 409 void TRP105FS::loadSetting()
aktk 0:e85788b14028 410 {
aktk 0:e85788b14028 411 FILE *fp;
aktk 0:e85788b14028 412 char tmp;
aktk 0:e85788b14028 413
aktk 0:e85788b14028 414 //sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 415 //fp = fopen(filepath, "rb");
aktk 0:e85788b14028 416 fp = fopen("/local/savedata.log", "rb");
aktk 0:e85788b14028 417 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 418
aktk 0:e85788b14028 419 fread(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 420 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 421 #ifdef DEBUG2
aktk 0:e85788b14028 422 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp);
aktk 0:e85788b14028 423 #endif
aktk 0:e85788b14028 424
aktk 0:e85788b14028 425 fread(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 426 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 427 #ifdef DEBUG2
aktk 0:e85788b14028 428 g_Serial_Signal.printf("%d%c", _Set[i].vol, tmp);
aktk 0:e85788b14028 429 #endif
aktk 0:e85788b14028 430
aktk 0:e85788b14028 431 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 432 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 433 #ifdef DEBUG2
aktk 0:e85788b14028 434 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 435 #endif
aktk 0:e85788b14028 436 }
aktk 0:e85788b14028 437
aktk 0:e85788b14028 438 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 439 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 440
aktk 0:e85788b14028 441 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 442 fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 443 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 444 fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 445 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 446 }
aktk 0:e85788b14028 447 fclose(fp);
aktk 0:e85788b14028 448 }
aktk 0:e85788b14028 449
aktk 0:e85788b14028 450
aktk 0:e85788b14028 451 void TRP105FS::saveSetting(
aktk 0:e85788b14028 452 const char *filename
aktk 0:e85788b14028 453 )
aktk 0:e85788b14028 454 {
aktk 0:e85788b14028 455 FILE *fp;
aktk 0:e85788b14028 456 char *filepath;
aktk 0:e85788b14028 457 int fnnum = 0;
aktk 0:e85788b14028 458
aktk 0:e85788b14028 459 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 460 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 461
aktk 0:e85788b14028 462 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 463 fp = fopen(filepath, "wb");
aktk 0:e85788b14028 464
aktk 0:e85788b14028 465 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 466 fwrite(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 467 fputc(0x2c, fp);
aktk 0:e85788b14028 468 fwrite(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 469 fputc(0x2c, fp);
aktk 0:e85788b14028 470 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 471 fputc(0x3b, fp);
aktk 0:e85788b14028 472 }
aktk 0:e85788b14028 473 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 0:e85788b14028 474 fputc(0x3b, fp);
aktk 0:e85788b14028 475 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 476 fwrite(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 477 fputc(0x2c, fp);
aktk 0:e85788b14028 478 fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 479 fputc(0x3b, fp);
aktk 0:e85788b14028 480 }
aktk 0:e85788b14028 481 fclose(fp);
aktk 0:e85788b14028 482 free(filepath);
aktk 0:e85788b14028 483 }
aktk 0:e85788b14028 484
aktk 0:e85788b14028 485 void TRP105FS::loadSetting(
aktk 0:e85788b14028 486 const char *filename
aktk 0:e85788b14028 487 )
aktk 0:e85788b14028 488 {
aktk 0:e85788b14028 489 FILE *fp;
aktk 0:e85788b14028 490 char *filepath;
aktk 0:e85788b14028 491 char tmp;
aktk 0:e85788b14028 492 int fnnum = 0;
aktk 0:e85788b14028 493
aktk 0:e85788b14028 494 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 495 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 496
aktk 0:e85788b14028 497 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 498 fp = fopen(filepath, "rb");
aktk 0:e85788b14028 499 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 500
aktk 0:e85788b14028 501 fread(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 502 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 503 #ifdef DEBUG3
aktk 0:e85788b14028 504 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp);
aktk 0:e85788b14028 505 #endif
aktk 0:e85788b14028 506
aktk 0:e85788b14028 507 fread(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 508 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 509 #ifdef DEBUG3
aktk 0:e85788b14028 510 g_Serial_Signal.printf("%d%c", _Set[i].vol, tmp);
aktk 0:e85788b14028 511 #endif
aktk 0:e85788b14028 512
aktk 0:e85788b14028 513 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 514 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 515 #ifdef DEBUG3
aktk 0:e85788b14028 516 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 517 #endif
aktk 0:e85788b14028 518 }
aktk 0:e85788b14028 519
aktk 0:e85788b14028 520 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 521 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 522 #ifdef DEBUG3
aktk 0:e85788b14028 523 g_Serial_Signal.printf("%d%c\n",_Sample_Num, tmp);
aktk 0:e85788b14028 524 #endif
aktk 0:e85788b14028 525
aktk 0:e85788b14028 526 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 527 fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 528 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 529 #ifdef DEBUG3
aktk 0:e85788b14028 530 g_Serial_Signal.printf("%d%c", _Sample_Set[i].dst, tmp);
aktk 0:e85788b14028 531 #endif
aktk 0:e85788b14028 532
aktk 0:e85788b14028 533 fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 534 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 535 #ifdef DEBUG3
aktk 0:e85788b14028 536 g_Serial_Signal.printf("%d%c", _Sample_Set[i].vol, tmp);
aktk 0:e85788b14028 537 #endif
aktk 0:e85788b14028 538 }
aktk 0:e85788b14028 539 fclose(fp);
aktk 0:e85788b14028 540 free(filepath);
aktk 0:e85788b14028 541 }
aktk 0:e85788b14028 542
aktk 0:e85788b14028 543 void TRP105FS::printOutData()
aktk 0:e85788b14028 544 {
aktk 0:e85788b14028 545 FILE *fp;
aktk 0:e85788b14028 546
aktk 0:e85788b14028 547 fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 0:e85788b14028 548 fprintf(fp, "dst, vol,(threshold)\n");
aktk 0:e85788b14028 549 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 550 fprintf(fp, "%d,%d,(%d)\n", _Set[i].dst, _Set[i].vol, _Threshold[i]);
aktk 0:e85788b14028 551 }
aktk 0:e85788b14028 552 fprintf(fp, "\nSample:dst, vol\n");
aktk 0:e85788b14028 553 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 554 fprintf(fp, "%d,%d\n", _Sample_Set[i].dst, _Sample_Set[i].vol);
aktk 0:e85788b14028 555 }
aktk 0:e85788b14028 556 fclose(fp);
aktk 0:e85788b14028 557
aktk 0:e85788b14028 558 }
aktk 0:e85788b14028 559 void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
aktk 0:e85788b14028 560 {
aktk 0:e85788b14028 561 FILE *fp;
aktk 0:e85788b14028 562 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 563 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 564 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 565 fprintf(fp, "%d, ", arg[i]);
aktk 0:e85788b14028 566 }
aktk 0:e85788b14028 567 fprintf(fp, "\n");
aktk 0:e85788b14028 568 fclose(fp);
aktk 0:e85788b14028 569 }
aktk 0:e85788b14028 570 void TRP105FS::_printOutData(double *arg, int num, char* name)
aktk 0:e85788b14028 571 {
aktk 0:e85788b14028 572 FILE *fp;
aktk 0:e85788b14028 573
aktk 0:e85788b14028 574 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 575 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 576 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 577 fprintf(fp, "%.2f, ", arg[i]);
aktk 0:e85788b14028 578 }
aktk 0:e85788b14028 579 fprintf(fp, "\n");
aktk 0:e85788b14028 580 fclose(fp);
aktk 0:e85788b14028 581 }
aktk 0:e85788b14028 582 void TRP105FS::_printOutData(VDset *arg, int num, char* name)
aktk 0:e85788b14028 583 {
aktk 0:e85788b14028 584 FILE *fp;
aktk 0:e85788b14028 585
aktk 0:e85788b14028 586 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 587 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 588 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 589 fprintf(fp, "%d, ", arg[i].vol);
aktk 0:e85788b14028 590 }
aktk 0:e85788b14028 591 fprintf(fp, "\n");
aktk 0:e85788b14028 592 fclose(fp);
aktk 0:e85788b14028 593 }