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@15:1d643b831a03, 2016-06-08 (annotated)

Committer:
aktk
Date:
Wed Jun 08 10:45:19 2016 +0000
Revision:
15:1d643b831a03
Parent:
14:a99bf22b919d
Child:
16:c2b0f3ca63dd
getSampleNum has been implemented.

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