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@10:b50e4bb40571, 2016-06-06 (annotated)

Committer:
aktk
Date:
Mon Jun 06 15:41:44 2016 +0000
Revision:
10:b50e4bb40571
Parent:
9:ec1ee4a6b6a4
Child:
11:d60fb729eacf
New Constructor TRP105FS(unsigned int, PinName) has implemented.

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