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@4:701f958d137a, 2016-06-06 (annotated)

Committer:
aktk
Date:
Mon Jun 06 10:27:42 2016 +0000
Revision:
4:701f958d137a
Parent:
3:b56e933bebc2
Child:
6:d2363b50aeaf
debug,; printOut(no arg)-> printOut(filename);

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aktk 0:e85788b14028 1 #define DEBUG
aktk 4:701f958d137a 2 //#define DEBUG2
aktk 0:e85788b14028 3 #include "TRP105F_Spline.h"
aktk 0:e85788b14028 4
aktk 3:b56e933bebc2 5 // To get ytage of TRP105F
aktk 4:701f958d137a 6 AnalogIn* g_Sensor_Voltage;
aktk 0:e85788b14028 7 // To get sample distance via seral com
aktk 4:701f958d137a 8 Serial g_Serial_Signal(USBTX, USBRX);
aktk 0:e85788b14028 9
aktk 4:701f958d137a 10 LocalFileSystem local("local"); // define Mount Point(which becomes Direcory Path)
aktk 0:e85788b14028 11 // for debug
aktk 0:e85788b14028 12 #ifdef DEBUG
aktk 0:e85788b14028 13 DigitalOut led1(LED1);
aktk 0:e85788b14028 14 DigitalOut led2(LED2);
aktk 0:e85788b14028 15 DigitalOut led3(LED3);
aktk 0:e85788b14028 16 DigitalOut led4(LED4);
aktk 0:e85788b14028 17 #endif
aktk 0:e85788b14028 18
aktk 3:b56e933bebc2 19 // Constructor
aktk 0:e85788b14028 20 TRP105FS::TRP105FS()
aktk 3:b56e933bebc2 21 :_useType(AsMODULE)
aktk 0:e85788b14028 22 {
aktk 0:e85788b14028 23 _Sample_Num = 5;
aktk 0:e85788b14028 24 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 25 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 3:b56e933bebc2 26 g_Sensor_Voltage = new AnalogIn(p16);
aktk 4:701f958d137a 27
aktk 4:701f958d137a 28 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 29 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 30 _u_spline[i] = 0.0;
aktk 4:701f958d137a 31 }
aktk 0:e85788b14028 32 }
aktk 0:e85788b14028 33
aktk 0:e85788b14028 34 TRP105FS::TRP105FS(
aktk 0:e85788b14028 35 unsigned int arg_num
aktk 0:e85788b14028 36 )
aktk 3:b56e933bebc2 37 :_useType(AsMODULE)
aktk 3:b56e933bebc2 38 {
aktk 3:b56e933bebc2 39 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 3:b56e933bebc2 40 else _Sample_Num = arg_num;
aktk 3:b56e933bebc2 41
aktk 3:b56e933bebc2 42 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 3:b56e933bebc2 43 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 3:b56e933bebc2 44 g_Sensor_Voltage = new AnalogIn(p16);
aktk 4:701f958d137a 45
aktk 4:701f958d137a 46 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 47 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 48 _u_spline[i] = 0.0;
aktk 4:701f958d137a 49 }
aktk 3:b56e933bebc2 50 }
aktk 3:b56e933bebc2 51
aktk 3:b56e933bebc2 52 TRP105FS::TRP105FS(
aktk 3:b56e933bebc2 53 unsigned int arg_num,
aktk 3:b56e933bebc2 54 UseType arg_type
aktk 3:b56e933bebc2 55 )
aktk 3:b56e933bebc2 56 :_useType(arg_type)
aktk 0:e85788b14028 57 {
aktk 0:e85788b14028 58 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 0:e85788b14028 59 else _Sample_Num = arg_num;
aktk 0:e85788b14028 60
aktk 0:e85788b14028 61 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 62 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 3:b56e933bebc2 63 g_Sensor_Voltage = new AnalogIn(p16);
aktk 4:701f958d137a 64
aktk 4:701f958d137a 65 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 66 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 67 _u_spline[i] = 0.0;
aktk 4:701f958d137a 68 }
aktk 0:e85788b14028 69 }
aktk 0:e85788b14028 70
aktk 0:e85788b14028 71 TRP105FS::TRP105FS(
aktk 0:e85788b14028 72 unsigned int arg_num,
aktk 3:b56e933bebc2 73 UseType arg_type,
aktk 3:b56e933bebc2 74 PinName pin
aktk 0:e85788b14028 75 )
aktk 3:b56e933bebc2 76 :_useType(arg_type)
aktk 0:e85788b14028 77 {
aktk 0:e85788b14028 78 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 0:e85788b14028 79 else _Sample_Num = arg_num;
aktk 0:e85788b14028 80
aktk 0:e85788b14028 81 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 82 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 3:b56e933bebc2 83 g_Sensor_Voltage = new AnalogIn(pin);
aktk 4:701f958d137a 84
aktk 4:701f958d137a 85 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 86 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 87 _u_spline[i] = 0.0;
aktk 4:701f958d137a 88 }
aktk 0:e85788b14028 89 }
aktk 0:e85788b14028 90
aktk 3:b56e933bebc2 91 // Destructor
aktk 0:e85788b14028 92 TRP105FS::~TRP105FS()
aktk 0:e85788b14028 93 {
aktk 0:e85788b14028 94 free(_Sample_Set);
aktk 0:e85788b14028 95 free(_u_spline);
aktk 4:701f958d137a 96 delete g_Sensor_Voltage;
aktk 0:e85788b14028 97 }
aktk 0:e85788b14028 98
aktk 3:b56e933bebc2 99
aktk 3:b56e933bebc2 100 unsigned short TRP105FS::getX(unsigned short arg_y)
aktk 0:e85788b14028 101 {
aktk 0:e85788b14028 102 int idx;
aktk 0:e85788b14028 103 unsigned short pv = 0;
aktk 0:e85788b14028 104
aktk 3:b56e933bebc2 105 pv = arg_y;
aktk 0:e85788b14028 106
aktk 0:e85788b14028 107 idx = _getNearest(_LIDX, _RIDX, pv);
aktk 0:e85788b14028 108
aktk 0:e85788b14028 109 if (idx != 0xFFFF) // unless occuring error
aktk 3:b56e933bebc2 110 return _Set[idx].x;
aktk 0:e85788b14028 111 else
aktk 0:e85788b14028 112 return 0xFFFF;
aktk 0:e85788b14028 113 }
aktk 0:e85788b14028 114
aktk 3:b56e933bebc2 115 unsigned short TRP105FS::getDistance(unsigned short arg_y)
aktk 3:b56e933bebc2 116 {
aktk 3:b56e933bebc2 117 return getX(arg_y);
aktk 3:b56e933bebc2 118 }
aktk 3:b56e933bebc2 119
aktk 3:b56e933bebc2 120 unsigned short TRP105FS::getDistance()
aktk 3:b56e933bebc2 121 {
aktk 3:b56e933bebc2 122 return getX(g_Sensor_Voltage->read_u16());
aktk 3:b56e933bebc2 123 }
aktk 3:b56e933bebc2 124
aktk 0:e85788b14028 125 /*
aktk 3:b56e933bebc2 126 Function to find a set whose y member is nearest a ytage from the sensor, recursively.
aktk 0:e85788b14028 127
aktk 0:e85788b14028 128 SHORT LONG
aktk 0:e85788b14028 129 +------------> HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+
aktk 3:b56e933bebc2 130 |(if ytage form sensor < threshold[cidx]) ||| (if threshold[cidx] < ytage form sensor)|
aktk 0:e85788b14028 131 | HIGH[lidx , ... , cidx]LOW ||| HIGH[cidx+1 , ... , ridx]LOW |
aktk 0:e85788b14028 132 | | | |
aktk 0:e85788b14028 133 +----------------+ +---------------+
aktk 0:e85788b14028 134 */
aktk 0:e85788b14028 135 int TRP105FS::_getNearest(
aktk 0:e85788b14028 136 int arg_lidx,
aktk 0:e85788b14028 137 int arg_ridx,
aktk 3:b56e933bebc2 138 unsigned short arg_y
aktk 0:e85788b14028 139 )
aktk 0:e85788b14028 140 {
aktk 0:e85788b14028 141 int cidx = (arg_lidx + arg_ridx) / 2;
aktk 0:e85788b14028 142
aktk 0:e85788b14028 143 // When the number of element to compare is only one, return it as result.
aktk 0:e85788b14028 144 if(arg_lidx == arg_ridx)
aktk 0:e85788b14028 145 return cidx;
aktk 3:b56e933bebc2 146 // If the ytage from the sensor is lower than the center threshold
aktk 0:e85788b14028 147 // (_set[cidx] > _threshold[cidx] > _set[cidx+1])
aktk 3:b56e933bebc2 148 else if(arg_y > _Threshold[cidx])
aktk 3:b56e933bebc2 149 return _getNearest(arg_lidx, cidx, arg_y);
aktk 3:b56e933bebc2 150 // If the ytage from the sensor is higher than the center threshold
aktk 3:b56e933bebc2 151 else if(arg_y < _Threshold[cidx])
aktk 3:b56e933bebc2 152 return _getNearest(cidx + 1, arg_ridx, arg_y);
aktk 3:b56e933bebc2 153 // If the ytage from the sensor eauals the center threshold
aktk 3:b56e933bebc2 154 else //(arg_y == _Treshold[cidx].y)
aktk 0:e85788b14028 155 return cidx;
aktk 0:e85788b14028 156 }
aktk 0:e85788b14028 157
aktk 4:701f958d137a 158 void TRP105FS::setSample(unsigned short arg_x, unsigned short arg_y)
aktk 4:701f958d137a 159 {
aktk 4:701f958d137a 160 _setSample(arg_x, arg_y);
aktk 3:b56e933bebc2 161 }
aktk 3:b56e933bebc2 162
aktk 3:b56e933bebc2 163 void TRP105FS::_setSample(unsigned short arg_x, unsigned short arg_y)
aktk 3:b56e933bebc2 164 {
aktk 3:b56e933bebc2 165 static unsigned int snum = 0;
aktk 3:b56e933bebc2 166 unsigned int num;
aktk 3:b56e933bebc2 167 int tmp;
aktk 3:b56e933bebc2 168 VDset tmp_set[_ENUM]; // for bucket sort
aktk 3:b56e933bebc2 169
aktk 4:701f958d137a 170 // Increment it if this function called.
aktk 3:b56e933bebc2 171 snum++;
aktk 4:701f958d137a 172 #ifdef DEBUG
aktk 4:701f958d137a 173 g_Serial_Signal.printf("snum : %d\n", snum);
aktk 4:701f958d137a 174 g_Serial_Signal.printf("(%d,%d)\n",arg_x, arg_y);
aktk 4:701f958d137a 175 #endif
aktk 3:b56e933bebc2 176
aktk 3:b56e933bebc2 177 // fit to smaller
aktk 4:701f958d137a 178 if (snum < _Sample_Num) {
aktk 4:701f958d137a 179 num = snum;
aktk 3:b56e933bebc2 180 } else {
aktk 3:b56e933bebc2 181 // To reclloc memories if snum is bigger than _Sample_Num.
aktk 3:b56e933bebc2 182 // When realloc is failed, snum is back to porevius.
aktk 3:b56e933bebc2 183 VDset* tmp_Set = (VDset *)realloc(_Sample_Set, snum * sizeof(VDset));
aktk 3:b56e933bebc2 184 double* tmp__u_ = (double*)realloc(_u_spline, snum * sizeof(double));
aktk 3:b56e933bebc2 185 if (tmp_set != NULL && tmp__u_ != NULL) {
aktk 3:b56e933bebc2 186 _Sample_Set = tmp_Set;
aktk 3:b56e933bebc2 187 _u_spline = tmp__u_;
aktk 3:b56e933bebc2 188 num = _Sample_Num = snum;
aktk 3:b56e933bebc2 189 } else {
aktk 3:b56e933bebc2 190 snum--;
aktk 3:b56e933bebc2 191 num = snum = _Sample_Num ;
aktk 4:701f958d137a 192 #ifdef DEBUG
aktk 4:701f958d137a 193 g_Serial_Signal.printf("failed to realloc\n", snum);
aktk 4:701f958d137a 194 #endif
aktk 3:b56e933bebc2 195 }
aktk 3:b56e933bebc2 196 }
aktk 3:b56e933bebc2 197
aktk 3:b56e933bebc2 198 _Sample_Set[num - 1].x = arg_x;
aktk 3:b56e933bebc2 199 _Sample_Set[num - 1].y = arg_y;
aktk 4:701f958d137a 200 if((unsigned short)_RIDX < _Sample_Set[num - 1].x)
aktk 4:701f958d137a 201 _Sample_Set[num - 1].x = (unsigned short)_RIDX;
aktk 3:b56e933bebc2 202
aktk 3:b56e933bebc2 203 //
aktk 3:b56e933bebc2 204 // Sort set data array in distanceAscending order
aktk 3:b56e933bebc2 205 //
aktk 3:b56e933bebc2 206 // Bucket sort
aktk 3:b56e933bebc2 207 for(int i = 0; i < _ENUM; i++)
aktk 3:b56e933bebc2 208 tmp_set[i].x = 0xaaaa;
aktk 3:b56e933bebc2 209 tmp = 0;
aktk 4:701f958d137a 210 for(int i = 0; i < num; i++) {
aktk 3:b56e933bebc2 211 // use x as index for x range [_LIDX,_RIDX]
aktk 3:b56e933bebc2 212 if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
aktk 3:b56e933bebc2 213 tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
aktk 3:b56e933bebc2 214 tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
aktk 3:b56e933bebc2 215 } else { // if a same x has been input, calcurate mean.
aktk 3:b56e933bebc2 216 tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
aktk 3:b56e933bebc2 217 tmp_set[_Sample_Set[i].x].y /= 2;
aktk 3:b56e933bebc2 218 tmp++;
aktk 3:b56e933bebc2 219 }
aktk 3:b56e933bebc2 220 }
aktk 3:b56e933bebc2 221 #ifdef DEBUG
aktk 3:b56e933bebc2 222 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 3:b56e933bebc2 223 g_Serial_Signal.printf("-) tmp: %d\n", tmp );
aktk 3:b56e933bebc2 224 #endif
aktk 3:b56e933bebc2 225 // substruct tmp from number of sample.
aktk 3:b56e933bebc2 226 _Sample_Num -= tmp;
aktk 4:701f958d137a 227 snum -= tmp;
aktk 3:b56e933bebc2 228 #ifdef DEBUG
aktk 3:b56e933bebc2 229 g_Serial_Signal.printf("-----------------\n");
aktk 3:b56e933bebc2 230 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 3:b56e933bebc2 231 #endif
aktk 3:b56e933bebc2 232 // apply sort on _Sample_Set
aktk 3:b56e933bebc2 233 tmp = 0;
aktk 3:b56e933bebc2 234 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 235 if(tmp_set[i].x != 0xaaaa) {
aktk 3:b56e933bebc2 236 _Sample_Set[i - tmp].x = tmp_set[i].x;
aktk 3:b56e933bebc2 237 _Sample_Set[i - tmp].y = tmp_set[i].y;
aktk 3:b56e933bebc2 238 } else // if no data, skip it
aktk 3:b56e933bebc2 239 tmp++;
aktk 3:b56e933bebc2 240 }
aktk 3:b56e933bebc2 241 }
aktk 0:e85788b14028 242 void TRP105FS::_sampleData()
aktk 0:e85788b14028 243 {
aktk 0:e85788b14028 244 int tmp;
aktk 0:e85788b14028 245 char sig;
aktk 3:b56e933bebc2 246 unsigned short tmp_y;
aktk 0:e85788b14028 247 VDset tmp_set[_ENUM]; // for bucket sort
aktk 0:e85788b14028 248
aktk 3:b56e933bebc2 249 // For evry set,
aktk 3:b56e933bebc2 250 // 1, get x data via serai com,
aktk 3:b56e933bebc2 251 // 2, get y data,
aktk 0:e85788b14028 252 // and then do same for next index set.
aktk 0:e85788b14028 253 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 254 //
aktk 0:e85788b14028 255 // Recieve a Distance datus and store it into member
aktk 0:e85788b14028 256 //
aktk 3:b56e933bebc2 257 if(_useType == AsDEBUG) {
aktk 0:e85788b14028 258 g_Serial_Signal.putc('>');
aktk 3:b56e933bebc2 259 _Sample_Set[i].x = 0;
aktk 0:e85788b14028 260 do {
aktk 0:e85788b14028 261 sig = g_Serial_Signal.getc();
aktk 0:e85788b14028 262 if('0' <= sig && sig <= '9') {
aktk 3:b56e933bebc2 263 _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
aktk 0:e85788b14028 264 g_Serial_Signal.putc(char(sig));
aktk 0:e85788b14028 265 } else if(sig == 0x08) {
aktk 3:b56e933bebc2 266 _Sample_Set[i].x = 0;
aktk 0:e85788b14028 267 g_Serial_Signal.printf("[canseled!]");
aktk 0:e85788b14028 268 g_Serial_Signal.putc('\n');
aktk 0:e85788b14028 269 g_Serial_Signal.putc('>');
aktk 0:e85788b14028 270 }
aktk 0:e85788b14028 271 } while (!(sig == 0x0a || sig == 0x0d));
aktk 0:e85788b14028 272 g_Serial_Signal.putc('\n');
aktk 4:701f958d137a 273 } else {
aktk 3:b56e933bebc2 274 _Sample_Set[i].x = g_Serial_Signal.getc();
aktk 0:e85788b14028 275 }
aktk 0:e85788b14028 276
aktk 0:e85788b14028 277 // if input data is over the bound calibrate it below
aktk 3:b56e933bebc2 278 if (_Sample_Set[i].x < (unsigned short)_LIDX)
aktk 3:b56e933bebc2 279 _Sample_Set[i].x = (unsigned short)_LIDX;
aktk 3:b56e933bebc2 280 else if ((unsigned short)_RIDX < _Sample_Set[i].x)
aktk 3:b56e933bebc2 281 _Sample_Set[i].x = (unsigned short)_RIDX;
aktk 0:e85788b14028 282 //
aktk 0:e85788b14028 283 // Recieve a Voltage datus and store it into member
aktk 0:e85788b14028 284 //
aktk 0:e85788b14028 285 // LOW PASS FILTERED
aktk 0:e85788b14028 286 // Get 10 data and store mean as a sample.
aktk 0:e85788b14028 287 // After get one original sample, system waits for 0.1 sec,
aktk 0:e85788b14028 288 // thus it takes 1 sec evry sampling.
aktk 3:b56e933bebc2 289 _Sample_Set[i].y = 0;
aktk 0:e85788b14028 290 for(int j = 0; j < 10; j++) {
aktk 0:e85788b14028 291 //unsigned short 's range [0 , 65535]
aktk 3:b56e933bebc2 292 //the Number of significant figures of read ytage is 3 or 4.
aktk 3:b56e933bebc2 293 tmp_y = g_Sensor_Voltage->read_u16();
aktk 3:b56e933bebc2 294
aktk 0:e85788b14028 295 #ifdef DEBUG
aktk 3:b56e933bebc2 296 g_Serial_Signal.printf("%d,",tmp_y);
aktk 0:e85788b14028 297 #endif
aktk 3:b56e933bebc2 298 _Sample_Set[i].y += (tmp_y / 10);
aktk 0:e85788b14028 299 wait(0.1);
aktk 0:e85788b14028 300 }
aktk 0:e85788b14028 301 #ifdef DEBUG
aktk 3:b56e933bebc2 302 g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y);
aktk 0:e85788b14028 303 #endif
aktk 0:e85788b14028 304 }
aktk 0:e85788b14028 305 //
aktk 0:e85788b14028 306 // Sort set data array in distanceAscending order
aktk 0:e85788b14028 307 //
aktk 0:e85788b14028 308 // Bucket sort
aktk 0:e85788b14028 309 for(int i = 0; i < _ENUM; i++)
aktk 3:b56e933bebc2 310 tmp_set[i].x = 0xaaaa;
aktk 0:e85788b14028 311 tmp = 0;
aktk 0:e85788b14028 312 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 313 // use x as index for x range [LIDX,RIDX]
aktk 3:b56e933bebc2 314 if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
aktk 3:b56e933bebc2 315 tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
aktk 3:b56e933bebc2 316 tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
aktk 3:b56e933bebc2 317 } else { // if a same x has been input, calcurate mean.
aktk 3:b56e933bebc2 318 tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
aktk 3:b56e933bebc2 319 tmp_set[_Sample_Set[i].x].y /= 2;
aktk 0:e85788b14028 320 tmp++;
aktk 0:e85788b14028 321 }
aktk 0:e85788b14028 322 }
aktk 0:e85788b14028 323 #ifdef DEBUG
aktk 0:e85788b14028 324 g_Serial_Signal.printf("%d\n", _Sample_Num );
aktk 0:e85788b14028 325 #endif
aktk 0:e85788b14028 326 // substruct tmp from number of sample.
aktk 0:e85788b14028 327 _Sample_Num -= tmp;
aktk 0:e85788b14028 328
aktk 0:e85788b14028 329 #ifdef DEBUG
aktk 0:e85788b14028 330 g_Serial_Signal.printf("tmp: %d\n", tmp );
aktk 0:e85788b14028 331 #endif
aktk 0:e85788b14028 332 // apply sort on _Sample_Set
aktk 0:e85788b14028 333 tmp = 0;
aktk 0:e85788b14028 334 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 335 if(tmp_set[i].x != 0xaaaa) {
aktk 3:b56e933bebc2 336 _Sample_Set[i - tmp].x = tmp_set[i].x;
aktk 3:b56e933bebc2 337 _Sample_Set[i - tmp].y = tmp_set[i].y;
aktk 0:e85788b14028 338 } else // if no data, skip it
aktk 0:e85788b14028 339 tmp++;
aktk 0:e85788b14028 340 }
aktk 0:e85788b14028 341 }
aktk 0:e85788b14028 342
aktk 0:e85788b14028 343 //
aktk 0:e85788b14028 344 // Function to define _u_spline, specific constants of spline.
aktk 0:e85788b14028 345 //
aktk 0:e85788b14028 346 void TRP105FS::_makeSpline()
aktk 0:e85788b14028 347 {
aktk 3:b56e933bebc2 348 // x: x, distance
aktk 3:b56e933bebc2 349 // y: y, ytage
aktk 0:e85788b14028 350 //
aktk 0:e85788b14028 351 // N: max of index <=> (_Sample_Num - 1)
aktk 0:e85788b14028 352 //
aktk 0:e85788b14028 353 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 354 // i:[0,N]
aktk 0:e85788b14028 355 // _u_spline[0] = _u_spline[N] = 0
aktk 0:e85788b14028 356 //
aktk 0:e85788b14028 357 // h[i] = x[i+1] - x[i]
aktk 0:e85788b14028 358 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
aktk 0:e85788b14028 359 double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 0:e85788b14028 360 //unsigned short *h __attribute__((at(0x20080000)));
aktk 0:e85788b14028 361 //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
aktk 0:e85788b14028 362 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
aktk 0:e85788b14028 363 // i:[0,N-2]
aktk 0:e85788b14028 364 double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 365 //unsigned short *v __attribute__((at(0x20080100)));
aktk 0:e85788b14028 366 //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 367 // temporary array whose num of elm equals v array
aktk 0:e85788b14028 368 double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 369 //unsigned short *w __attribute__((at(0x20080200)));
aktk 0:e85788b14028 370 //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 371 //
aktk 0:e85788b14028 372 // [ 2(h[0]+h[1]) , h[1] , O ] [_u[1] ] [v[0] ]
aktk 0:e85788b14028 373 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [_u[2] ] [v[1] ]
aktk 0:e85788b14028 374 // [ ... ] * [... ] = [... ]
aktk 0:e85788b14028 375 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 376 // [ ... ] [ ... ] [ ... ]
aktk 0:e85788b14028 377 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 378 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [_u[N-1]] [v[N-2]]
aktk 0:e85788b14028 379 //
aktk 0:e85788b14028 380 // For LU decomposition
aktk 0:e85788b14028 381 double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 382 //unsigned short *Upper __attribute__((at(0x20080300)));
aktk 0:e85788b14028 383 //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 384 double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 385 //unsigned short *Lower __attribute__((at(0x20080400)));
aktk 0:e85788b14028 386 //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 387 #ifdef DEBUG
aktk 0:e85788b14028 388 _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
aktk 0:e85788b14028 389 #endif
aktk 0:e85788b14028 390 for(int i = 0; i < _Sample_Num - 1; i++)
aktk 3:b56e933bebc2 391 h[i] = (double)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
aktk 3:b56e933bebc2 392 //(unsigned short)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
aktk 0:e85788b14028 393
aktk 0:e85788b14028 394 for(int i = 0; i < _Sample_Num - 2; i++)
aktk 0:e85788b14028 395 v[i] = 6 * (
aktk 3:b56e933bebc2 396 ((double)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y)) / h[i + 1]
aktk 3:b56e933bebc2 397 //(unsigned short)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y) / h[i + 1]
aktk 0:e85788b14028 398 -
aktk 3:b56e933bebc2 399 ((double)(_Sample_Set[i + 1].y - _Sample_Set[i].y)) / h[i]
aktk 3:b56e933bebc2 400 //(unsigned short)(_Sample_Set[i + 1].y - _Sample_Set[i].y) / h[i]
aktk 0:e85788b14028 401 );
aktk 0:e85788b14028 402
aktk 0:e85788b14028 403 //
aktk 0:e85788b14028 404 // LU decomposition
aktk 0:e85788b14028 405 //
aktk 0:e85788b14028 406 Upper[0] = 2 * (h[0] + h[1]);
aktk 0:e85788b14028 407 Lower[0] = 0;
aktk 0:e85788b14028 408 for (int i = 1; i < _Sample_Num - 2; i++) {
aktk 0:e85788b14028 409 Lower[i] = h[i] / Upper[i - 1];
aktk 0:e85788b14028 410 Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
aktk 0:e85788b14028 411 }
aktk 0:e85788b14028 412
aktk 0:e85788b14028 413
aktk 0:e85788b14028 414 //
aktk 0:e85788b14028 415 // forward substitution
aktk 0:e85788b14028 416 //
aktk 0:e85788b14028 417 w[0] = v[0];
aktk 0:e85788b14028 418 for (int i = 1; i < _Sample_Num - 2; i ++) {
aktk 0:e85788b14028 419 w[i] = v[i] - Lower[i] * w[i-1];
aktk 0:e85788b14028 420 }
aktk 0:e85788b14028 421
aktk 0:e85788b14028 422
aktk 0:e85788b14028 423 //
aktk 0:e85788b14028 424 // backward substitution
aktk 0:e85788b14028 425 //
aktk 0:e85788b14028 426 _u_spline[_Sample_Num - 2] = w[_Sample_Num - 3] / Upper[_Sample_Num - 3];
aktk 0:e85788b14028 427 for(int i = _Sample_Num - 3; i > 0; i--) {
aktk 0:e85788b14028 428 _u_spline[i] = (w[(i - 1)] - h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
aktk 0:e85788b14028 429 }
aktk 0:e85788b14028 430
aktk 0:e85788b14028 431 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 432 _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;
aktk 0:e85788b14028 433
aktk 0:e85788b14028 434 #ifdef DEBUG
aktk 0:e85788b14028 435 _printOutData(h, _Sample_Num - 1, "h");
aktk 0:e85788b14028 436 _printOutData(v, _Sample_Num - 2, "v");
aktk 0:e85788b14028 437 _printOutData(w, _Sample_Num - 2, "w");
aktk 0:e85788b14028 438 _printOutData(Upper, _Sample_Num - 2, "Upper");
aktk 0:e85788b14028 439 _printOutData(Lower, _Sample_Num - 2, "Lower");
aktk 0:e85788b14028 440 _printOutData(_u_spline, _Sample_Num, "u");
aktk 0:e85788b14028 441 #endif
aktk 0:e85788b14028 442 free(h);
aktk 0:e85788b14028 443 free(v);
aktk 0:e85788b14028 444 free(w);
aktk 0:e85788b14028 445 free(Upper);
aktk 0:e85788b14028 446 free(Lower);
aktk 0:e85788b14028 447 }
aktk 0:e85788b14028 448 //
aktk 0:e85788b14028 449 // Function to return Voltage for distance.
aktk 0:e85788b14028 450 //
aktk 0:e85788b14028 451 unsigned short TRP105FS:: _getSplineYof(
aktk 0:e85788b14028 452 double arg_x // the argument is supposed as distance [mm]
aktk 0:e85788b14028 453 )
aktk 0:e85788b14028 454 {
aktk 3:b56e933bebc2 455 double y; // ytage calculated by spline polynomial
aktk 0:e85788b14028 456 double a,b,c,d; // which is specific constant of spline, and can be expressed with _u.
aktk 0:e85788b14028 457 int itv = 0; // interval(section) of interpolation
aktk 0:e85788b14028 458 // the number of interval is less 1 than the number of sample sets,
aktk 0:e85788b14028 459 // which means the max number of interval is _Sample_num - 2.
aktk 3:b56e933bebc2 460 if((double)(_Sample_Set[0].x) <= arg_x) {
aktk 3:b56e933bebc2 461 while (!((double)(_Sample_Set[itv].x) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].x))) {
aktk 0:e85788b14028 462 itv++;
aktk 0:e85788b14028 463 if(itv > _Sample_Num - 2) {
aktk 0:e85788b14028 464 itv = _Sample_Num - 2;
aktk 0:e85788b14028 465 break;
aktk 0:e85788b14028 466 }
aktk 0:e85788b14028 467 }
aktk 0:e85788b14028 468 }
aktk 3:b56e933bebc2 469 a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x);
aktk 0:e85788b14028 470 b = (double)(_u_spline[itv]) / 2.0;
aktk 3:b56e933bebc2 471 c = (double)(_Sample_Set[itv + 1].y - _Sample_Set[itv].y) / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x)
aktk 0:e85788b14028 472 -
aktk 3:b56e933bebc2 473 (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0;
aktk 3:b56e933bebc2 474 d = (double)(_Sample_Set[itv].y);
aktk 0:e85788b14028 475 // cubic spline expression
aktk 3:b56e933bebc2 476 y = a * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
aktk 0:e85788b14028 477 +
aktk 3:b56e933bebc2 478 b * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
aktk 0:e85788b14028 479 +
aktk 3:b56e933bebc2 480 c * (arg_x - (double)(_Sample_Set[itv].x))
aktk 0:e85788b14028 481 +
aktk 0:e85788b14028 482 d;
aktk 0:e85788b14028 483
aktk 0:e85788b14028 484 #ifdef DEBUG2
aktk 0:e85788b14028 485 g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
aktk 0:e85788b14028 486 g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
aktk 0:e85788b14028 487 g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
aktk 0:e85788b14028 488 #endif
aktk 0:e85788b14028 489
aktk 0:e85788b14028 490 return ((unsigned short)(int)y);
aktk 0:e85788b14028 491 }
aktk 0:e85788b14028 492
aktk 0:e85788b14028 493 void TRP105FS::calibrateSensor()
aktk 0:e85788b14028 494 {
aktk 0:e85788b14028 495 _sampleData();
aktk 0:e85788b14028 496 _makeSpline();
aktk 0:e85788b14028 497
aktk 0:e85788b14028 498 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 499 _Set[i].x = i;
aktk 4:701f958d137a 500 _Set[i].y = _getSplineYof((double)(_Set[i].x));
aktk 4:701f958d137a 501 _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
aktk 3:b56e933bebc2 502 #ifdef DEBUG2
aktk 3:b56e933bebc2 503 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 3:b56e933bebc2 504 #endif
aktk 3:b56e933bebc2 505 }
aktk 3:b56e933bebc2 506 }
aktk 3:b56e933bebc2 507
aktk 3:b56e933bebc2 508 void TRP105FS::calibrate()
aktk 3:b56e933bebc2 509 {
aktk 4:701f958d137a 510 #ifdef DEBUG2
aktk 4:701f958d137a 511 g_Serial_Signal.printf("Sample Data Set\n");
aktk 4:701f958d137a 512 for(int i = 0; i < _Sample_Num; i++)
aktk 4:701f958d137a 513 g_Serial_Signal.printf("(%d,%d)\n", _Sample_Set[i].x, _Sample_Set[i].y);
aktk 4:701f958d137a 514 #endif
aktk 4:701f958d137a 515
aktk 3:b56e933bebc2 516 _makeSpline();
aktk 3:b56e933bebc2 517
aktk 3:b56e933bebc2 518 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 519 _Set[i].x = i;
aktk 4:701f958d137a 520 _Set[i].y = _getSplineYof((double)(_Set[i].x));
aktk 4:701f958d137a 521 _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
aktk 4:701f958d137a 522
aktk 0:e85788b14028 523 #ifdef DEBUG2
aktk 0:e85788b14028 524 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 0:e85788b14028 525 #endif
aktk 0:e85788b14028 526 }
aktk 0:e85788b14028 527 }
aktk 0:e85788b14028 528
aktk 0:e85788b14028 529 void TRP105FS::saveSetting()
aktk 0:e85788b14028 530 {
aktk 0:e85788b14028 531 FILE *fp;
aktk 0:e85788b14028 532
aktk 0:e85788b14028 533 fp = fopen("/local/savedata.log", "wb");
aktk 0:e85788b14028 534
aktk 0:e85788b14028 535 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 536 fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 537 fputc(0x2c, fp);
aktk 3:b56e933bebc2 538 fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 539 fputc(0x2c, fp);
aktk 0:e85788b14028 540 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 541 fputc(0x3b, fp);
aktk 0:e85788b14028 542 }
aktk 0:e85788b14028 543 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 0:e85788b14028 544 fputc(0x3b, fp);
aktk 0:e85788b14028 545 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 546 fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 547 fputc(0x2c, fp);
aktk 3:b56e933bebc2 548 fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 549 fputc(0x3b, fp);
aktk 0:e85788b14028 550 }
aktk 0:e85788b14028 551 fclose(fp);
aktk 0:e85788b14028 552
aktk 0:e85788b14028 553 }
aktk 0:e85788b14028 554
aktk 0:e85788b14028 555 void TRP105FS::printThresholds()
aktk 0:e85788b14028 556 {
aktk 0:e85788b14028 557 for(int i = 0; i < _ENUM; i++)
aktk 0:e85788b14028 558 g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
aktk 0:e85788b14028 559 }
aktk 0:e85788b14028 560 void TRP105FS::loadSetting()
aktk 0:e85788b14028 561 {
aktk 0:e85788b14028 562 FILE *fp;
aktk 0:e85788b14028 563 char tmp;
aktk 0:e85788b14028 564
aktk 0:e85788b14028 565 //sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 566 //fp = fopen(filepath, "rb");
aktk 0:e85788b14028 567 fp = fopen("/local/savedata.log", "rb");
aktk 0:e85788b14028 568 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 569
aktk 3:b56e933bebc2 570 fread(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 571 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 572 #ifdef DEBUG2
aktk 3:b56e933bebc2 573 g_Serial_Signal.printf("%d%c", _Set[i].x, tmp);
aktk 0:e85788b14028 574 #endif
aktk 0:e85788b14028 575
aktk 3:b56e933bebc2 576 fread(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 577 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 578 #ifdef DEBUG2
aktk 3:b56e933bebc2 579 g_Serial_Signal.printf("%d%c", _Set[i].y, tmp);
aktk 0:e85788b14028 580 #endif
aktk 0:e85788b14028 581
aktk 0:e85788b14028 582 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 583 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 584 #ifdef DEBUG2
aktk 0:e85788b14028 585 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 586 #endif
aktk 0:e85788b14028 587 }
aktk 0:e85788b14028 588
aktk 0:e85788b14028 589 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 590 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 591
aktk 0:e85788b14028 592 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 593 fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 594 fread(&tmp, sizeof(char),1,fp);
aktk 3:b56e933bebc2 595 fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 596 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 597 }
aktk 0:e85788b14028 598 fclose(fp);
aktk 0:e85788b14028 599 }
aktk 0:e85788b14028 600
aktk 0:e85788b14028 601
aktk 0:e85788b14028 602 void TRP105FS::saveSetting(
aktk 0:e85788b14028 603 const char *filename
aktk 0:e85788b14028 604 )
aktk 0:e85788b14028 605 {
aktk 0:e85788b14028 606 FILE *fp;
aktk 0:e85788b14028 607 char *filepath;
aktk 0:e85788b14028 608 int fnnum = 0;
aktk 0:e85788b14028 609
aktk 0:e85788b14028 610 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 611 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 612
aktk 0:e85788b14028 613 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 614 fp = fopen(filepath, "wb");
aktk 0:e85788b14028 615
aktk 0:e85788b14028 616 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 617 fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 618 fputc(0x2c, fp);
aktk 3:b56e933bebc2 619 fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 620 fputc(0x2c, fp);
aktk 0:e85788b14028 621 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 622 fputc(0x3b, fp);
aktk 0:e85788b14028 623 }
aktk 0:e85788b14028 624 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 0:e85788b14028 625 fputc(0x3b, fp);
aktk 0:e85788b14028 626 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 627 fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 628 fputc(0x2c, fp);
aktk 3:b56e933bebc2 629 fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 630 fputc(0x3b, fp);
aktk 0:e85788b14028 631 }
aktk 0:e85788b14028 632 fclose(fp);
aktk 0:e85788b14028 633 free(filepath);
aktk 0:e85788b14028 634 }
aktk 0:e85788b14028 635
aktk 0:e85788b14028 636 void TRP105FS::loadSetting(
aktk 0:e85788b14028 637 const char *filename
aktk 0:e85788b14028 638 )
aktk 0:e85788b14028 639 {
aktk 0:e85788b14028 640 FILE *fp;
aktk 0:e85788b14028 641 char *filepath;
aktk 0:e85788b14028 642 char tmp;
aktk 0:e85788b14028 643 int fnnum = 0;
aktk 0:e85788b14028 644
aktk 0:e85788b14028 645 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 646 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 647
aktk 0:e85788b14028 648 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 649 fp = fopen(filepath, "rb");
aktk 4:701f958d137a 650
aktk 0:e85788b14028 651 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 652
aktk 3:b56e933bebc2 653 fread(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 654 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 655 #ifdef DEBUG3
aktk 3:b56e933bebc2 656 g_Serial_Signal.printf("%d%c", _Set[i].x, tmp);
aktk 0:e85788b14028 657 #endif
aktk 0:e85788b14028 658
aktk 3:b56e933bebc2 659 fread(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 660 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 661 #ifdef DEBUG3
aktk 3:b56e933bebc2 662 g_Serial_Signal.printf("%d%c", _Set[i].y, tmp);
aktk 0:e85788b14028 663 #endif
aktk 0:e85788b14028 664
aktk 0:e85788b14028 665 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 666 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 667 #ifdef DEBUG3
aktk 0:e85788b14028 668 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 669 #endif
aktk 0:e85788b14028 670 }
aktk 0:e85788b14028 671
aktk 0:e85788b14028 672 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 673 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 674 #ifdef DEBUG3
aktk 0:e85788b14028 675 g_Serial_Signal.printf("%d%c\n",_Sample_Num, tmp);
aktk 0:e85788b14028 676 #endif
aktk 0:e85788b14028 677
aktk 0:e85788b14028 678 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 679 fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 680 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 681 #ifdef DEBUG3
aktk 3:b56e933bebc2 682 g_Serial_Signal.printf("%d%c", _Sample_Set[i].x, tmp);
aktk 0:e85788b14028 683 #endif
aktk 0:e85788b14028 684
aktk 3:b56e933bebc2 685 fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 686 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 687 #ifdef DEBUG3
aktk 3:b56e933bebc2 688 g_Serial_Signal.printf("%d%c", _Sample_Set[i].y, tmp);
aktk 0:e85788b14028 689 #endif
aktk 0:e85788b14028 690 }
aktk 0:e85788b14028 691 fclose(fp);
aktk 0:e85788b14028 692 free(filepath);
aktk 0:e85788b14028 693 }
aktk 0:e85788b14028 694
aktk 4:701f958d137a 695 void TRP105FS::printOutData(
aktk 4:701f958d137a 696 const char *filename)
aktk 0:e85788b14028 697 {
aktk 0:e85788b14028 698 FILE *fp;
aktk 4:701f958d137a 699 char *filepath;
aktk 4:701f958d137a 700 char tmp;
aktk 4:701f958d137a 701 int fnnum = 0;
aktk 4:701f958d137a 702
aktk 4:701f958d137a 703 while (filename[fnnum] != 0) fnnum++;
aktk 4:701f958d137a 704 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 4:701f958d137a 705 sprintf(filepath, "/local/%s", filename);
aktk 4:701f958d137a 706 fp = fopen(filepath, "rb");
aktk 0:e85788b14028 707
aktk 0:e85788b14028 708 fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 3:b56e933bebc2 709 fprintf(fp, "x, y,(threshold)\n");
aktk 0:e85788b14028 710 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 711 fprintf(fp, "%d,%d,(%d)\n", _Set[i].x, _Set[i].y, _Threshold[i]);
aktk 0:e85788b14028 712 }
aktk 3:b56e933bebc2 713 fprintf(fp, "\nSample:x, y\n");
aktk 0:e85788b14028 714 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 715 fprintf(fp, "%d,%d\n", _Sample_Set[i].x, _Sample_Set[i].y);
aktk 0:e85788b14028 716 }
aktk 4:701f958d137a 717 free(filepath);
aktk 0:e85788b14028 718 fclose(fp);
aktk 0:e85788b14028 719
aktk 0:e85788b14028 720 }
aktk 4:701f958d137a 721
aktk 0:e85788b14028 722 void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
aktk 0:e85788b14028 723 {
aktk 0:e85788b14028 724 FILE *fp;
aktk 0:e85788b14028 725 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 726 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 727 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 728 fprintf(fp, "%d, ", arg[i]);
aktk 0:e85788b14028 729 }
aktk 0:e85788b14028 730 fprintf(fp, "\n");
aktk 0:e85788b14028 731 fclose(fp);
aktk 0:e85788b14028 732 }
aktk 0:e85788b14028 733 void TRP105FS::_printOutData(double *arg, int num, char* name)
aktk 0:e85788b14028 734 {
aktk 0:e85788b14028 735 FILE *fp;
aktk 0:e85788b14028 736
aktk 0:e85788b14028 737 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 738 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 739 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 740 fprintf(fp, "%.2f, ", arg[i]);
aktk 0:e85788b14028 741 }
aktk 0:e85788b14028 742 fprintf(fp, "\n");
aktk 0:e85788b14028 743 fclose(fp);
aktk 0:e85788b14028 744 }
aktk 0:e85788b14028 745 void TRP105FS::_printOutData(VDset *arg, int num, char* name)
aktk 0:e85788b14028 746 {
aktk 0:e85788b14028 747 FILE *fp;
aktk 0:e85788b14028 748
aktk 0:e85788b14028 749 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 750 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 751 for(int i = 0; i < num; i++) {
aktk 3:b56e933bebc2 752 fprintf(fp, "%d, ", arg[i].y);
aktk 0:e85788b14028 753 }
aktk 0:e85788b14028 754 fprintf(fp, "\n");
aktk 0:e85788b14028 755 fclose(fp);
aktk 0:e85788b14028 756 }