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@22:eaaaa42a0ccb, 2016-06-30 (annotated)

Committer:
aktk
Date:
Thu Jun 30 17:41:14 2016 +0000
Revision:
22:eaaaa42a0ccb
Parent:
21:cd20537290f9 
The filename that SPT::TRP105FS::printCalibrationLOG() makes has become ADLOG%2d.TXT.

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