Kenta Tanabe / TRP105F_Spline_for_child

for child

Fork of TRP105F_Spline by Akifumi Takahashi

TRP105F_Spline.cpp@27:1f38ee841153, 2017-10-24 (annotated)

Committer:
tanaken0605
Date:
Tue Oct 24 00:44:39 2017 +0000
Revision:
27:1f38ee841153
Parent:
23:772a9736ee59 
?????????

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