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@16:c2b0f3ca63dd, 2016-06-08 (annotated)

Committer:
aktk
Date:
Wed Jun 08 11:32:29 2016 +0000
Revision:
16:c2b0f3ca63dd
Parent:
15:1d643b831a03
Child:
18:b046bba4ffa7
getDistance(any), getVoltage() have come to be inline function.

Who changed what in which revision?

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