Kenta Tanabe / TRP105F_Spline_for_child

for child

Fork of TRP105F_Spline by Akifumi Takahashi

TRP105F_Spline.cpp@18:b046bba4ffa7, 2016-06-24 (annotated)

Committer:
aktk
Date:
Fri Jun 24 11:33:01 2016 +0000
Revision:
18:b046bba4ffa7
Parent:
16:c2b0f3ca63dd
Child:
20:0453919a76b2
In constructors, the type of arg_num was changed to unsigned short.; In constructor(ushort, PinName), the defalt useTpe came to be set to AsMODULE.

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