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

Committer:
aktk
Date:
Wed Jun 08 10:43:53 2016 +0000
Revision:
14:a99bf22b919d
Parent:
13:16fc61b41eff
Child:
15:1d643b831a03
saveSetting_intoSerial(Serial) has briefly 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 3:b56e933bebc2 131
aktk 3:b56e933bebc2 132 unsigned short TRP105FS::getX(unsigned short arg_y)
aktk 0:e85788b14028 133 {
aktk 0:e85788b14028 134 int idx;
aktk 0:e85788b14028 135 unsigned short pv = 0;
aktk 0:e85788b14028 136
aktk 3:b56e933bebc2 137 pv = arg_y;
aktk 0:e85788b14028 138
aktk 0:e85788b14028 139 idx = _getNearest(_LIDX, _RIDX, pv);
aktk 0:e85788b14028 140
aktk 0:e85788b14028 141 if (idx != 0xFFFF) // unless occuring error
aktk 3:b56e933bebc2 142 return _Set[idx].x;
aktk 0:e85788b14028 143 else
aktk 0:e85788b14028 144 return 0xFFFF;
aktk 0:e85788b14028 145 }
aktk 0:e85788b14028 146
aktk 3:b56e933bebc2 147 unsigned short TRP105FS::getDistance(unsigned short arg_y)
aktk 3:b56e933bebc2 148 {
aktk 3:b56e933bebc2 149 return getX(arg_y);
aktk 3:b56e933bebc2 150 }
aktk 3:b56e933bebc2 151
aktk 3:b56e933bebc2 152 unsigned short TRP105FS::getDistance()
aktk 3:b56e933bebc2 153 {
aktk 9:ec1ee4a6b6a4 154 return getX(_ai.read_u16());
aktk 3:b56e933bebc2 155 }
aktk 3:b56e933bebc2 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 static unsigned int snum = 0;
aktk 3:b56e933bebc2 198 unsigned int num;
aktk 3:b56e933bebc2 199 int tmp;
aktk 3:b56e933bebc2 200 VDset tmp_set[_ENUM]; // for bucket sort
aktk 3:b56e933bebc2 201
aktk 4:701f958d137a 202 // Increment it if this function called.
aktk 3:b56e933bebc2 203 snum++;
aktk 4:701f958d137a 204 #ifdef DEBUG
aktk 4:701f958d137a 205 g_Serial_Signal.printf("snum : %d\n", snum);
aktk 4:701f958d137a 206 g_Serial_Signal.printf("(%d,%d)\n",arg_x, arg_y);
aktk 4:701f958d137a 207 #endif
aktk 3:b56e933bebc2 208
aktk 3:b56e933bebc2 209 // fit to smaller
aktk 4:701f958d137a 210 if (snum < _Sample_Num) {
aktk 4:701f958d137a 211 num = snum;
aktk 3:b56e933bebc2 212 } else {
aktk 3:b56e933bebc2 213 // To reclloc memories if snum is bigger than _Sample_Num.
aktk 3:b56e933bebc2 214 // When realloc is failed, snum is back to porevius.
aktk 3:b56e933bebc2 215 VDset* tmp_Set = (VDset *)realloc(_Sample_Set, snum * sizeof(VDset));
aktk 3:b56e933bebc2 216 double* tmp__u_ = (double*)realloc(_u_spline, snum * sizeof(double));
aktk 3:b56e933bebc2 217 if (tmp_set != NULL && tmp__u_ != NULL) {
aktk 3:b56e933bebc2 218 _Sample_Set = tmp_Set;
aktk 3:b56e933bebc2 219 _u_spline = tmp__u_;
aktk 3:b56e933bebc2 220 num = _Sample_Num = snum;
aktk 3:b56e933bebc2 221 } else {
aktk 3:b56e933bebc2 222 snum--;
aktk 3:b56e933bebc2 223 num = snum = _Sample_Num ;
aktk 4:701f958d137a 224 #ifdef DEBUG
aktk 4:701f958d137a 225 g_Serial_Signal.printf("failed to realloc\n", snum);
aktk 4:701f958d137a 226 #endif
aktk 3:b56e933bebc2 227 }
aktk 3:b56e933bebc2 228 }
aktk 3:b56e933bebc2 229
aktk 3:b56e933bebc2 230 _Sample_Set[num - 1].x = arg_x;
aktk 3:b56e933bebc2 231 _Sample_Set[num - 1].y = arg_y;
aktk 4:701f958d137a 232 if((unsigned short)_RIDX < _Sample_Set[num - 1].x)
aktk 4:701f958d137a 233 _Sample_Set[num - 1].x = (unsigned short)_RIDX;
aktk 3:b56e933bebc2 234
aktk 3:b56e933bebc2 235 //
aktk 3:b56e933bebc2 236 // Sort set data array in distanceAscending order
aktk 3:b56e933bebc2 237 //
aktk 3:b56e933bebc2 238 // Bucket sort
aktk 3:b56e933bebc2 239 for(int i = 0; i < _ENUM; i++)
aktk 3:b56e933bebc2 240 tmp_set[i].x = 0xaaaa;
aktk 3:b56e933bebc2 241 tmp = 0;
aktk 4:701f958d137a 242 for(int i = 0; i < num; i++) {
aktk 3:b56e933bebc2 243 // use x as index for x range [_LIDX,_RIDX]
aktk 3:b56e933bebc2 244 if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
aktk 3:b56e933bebc2 245 tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
aktk 3:b56e933bebc2 246 tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
aktk 3:b56e933bebc2 247 } else { // if a same x has been input, calcurate mean.
aktk 3:b56e933bebc2 248 tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
aktk 3:b56e933bebc2 249 tmp_set[_Sample_Set[i].x].y /= 2;
aktk 3:b56e933bebc2 250 tmp++;
aktk 3:b56e933bebc2 251 }
aktk 3:b56e933bebc2 252 }
aktk 3:b56e933bebc2 253 #ifdef DEBUG
aktk 3:b56e933bebc2 254 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 3:b56e933bebc2 255 g_Serial_Signal.printf("-) tmp: %d\n", tmp );
aktk 3:b56e933bebc2 256 #endif
aktk 3:b56e933bebc2 257 // substruct tmp from number of sample.
aktk 3:b56e933bebc2 258 _Sample_Num -= tmp;
aktk 4:701f958d137a 259 snum -= tmp;
aktk 3:b56e933bebc2 260 #ifdef DEBUG
aktk 3:b56e933bebc2 261 g_Serial_Signal.printf("-----------------\n");
aktk 3:b56e933bebc2 262 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 3:b56e933bebc2 263 #endif
aktk 3:b56e933bebc2 264 // apply sort on _Sample_Set
aktk 3:b56e933bebc2 265 tmp = 0;
aktk 3:b56e933bebc2 266 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 267 if(tmp_set[i].x != 0xaaaa) {
aktk 3:b56e933bebc2 268 _Sample_Set[i - tmp].x = tmp_set[i].x;
aktk 3:b56e933bebc2 269 _Sample_Set[i - tmp].y = tmp_set[i].y;
aktk 3:b56e933bebc2 270 } else // if no data, skip it
aktk 3:b56e933bebc2 271 tmp++;
aktk 3:b56e933bebc2 272 }
aktk 3:b56e933bebc2 273 }
aktk 11:d60fb729eacf 274
aktk 11:d60fb729eacf 275
aktk 11:d60fb729eacf 276 #ifdef TARGET_LPC1768
aktk 0:e85788b14028 277 void TRP105FS::_sampleData()
aktk 0:e85788b14028 278 {
aktk 0:e85788b14028 279 int tmp;
aktk 0:e85788b14028 280 char sig;
aktk 3:b56e933bebc2 281 unsigned short tmp_y;
aktk 0:e85788b14028 282 VDset tmp_set[_ENUM]; // for bucket sort
aktk 0:e85788b14028 283
aktk 3:b56e933bebc2 284 // For evry set,
aktk 3:b56e933bebc2 285 // 1, get x data via serai com,
aktk 3:b56e933bebc2 286 // 2, get y data,
aktk 0:e85788b14028 287 // and then do same for next index set.
aktk 0:e85788b14028 288 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 289 //
aktk 0:e85788b14028 290 // Recieve a Distance datus and store it into member
aktk 0:e85788b14028 291 //
aktk 3:b56e933bebc2 292 if(_useType == AsDEBUG) {
aktk 0:e85788b14028 293 g_Serial_Signal.putc('>');
aktk 3:b56e933bebc2 294 _Sample_Set[i].x = 0;
aktk 0:e85788b14028 295 do {
aktk 0:e85788b14028 296 sig = g_Serial_Signal.getc();
aktk 0:e85788b14028 297 if('0' <= sig && sig <= '9') {
aktk 3:b56e933bebc2 298 _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
aktk 0:e85788b14028 299 g_Serial_Signal.putc(char(sig));
aktk 0:e85788b14028 300 } else if(sig == 0x08) {
aktk 3:b56e933bebc2 301 _Sample_Set[i].x = 0;
aktk 0:e85788b14028 302 g_Serial_Signal.printf("[canseled!]");
aktk 0:e85788b14028 303 g_Serial_Signal.putc('\n');
aktk 0:e85788b14028 304 g_Serial_Signal.putc('>');
aktk 0:e85788b14028 305 }
aktk 0:e85788b14028 306 } while (!(sig == 0x0a || sig == 0x0d));
aktk 0:e85788b14028 307 g_Serial_Signal.putc('\n');
aktk 4:701f958d137a 308 } else {
aktk 3:b56e933bebc2 309 _Sample_Set[i].x = g_Serial_Signal.getc();
aktk 0:e85788b14028 310 }
aktk 0:e85788b14028 311
aktk 0:e85788b14028 312 // if input data is over the bound calibrate it below
aktk 3:b56e933bebc2 313 if (_Sample_Set[i].x < (unsigned short)_LIDX)
aktk 3:b56e933bebc2 314 _Sample_Set[i].x = (unsigned short)_LIDX;
aktk 3:b56e933bebc2 315 else if ((unsigned short)_RIDX < _Sample_Set[i].x)
aktk 3:b56e933bebc2 316 _Sample_Set[i].x = (unsigned short)_RIDX;
aktk 0:e85788b14028 317 //
aktk 0:e85788b14028 318 // Recieve a Voltage datus and store it into member
aktk 0:e85788b14028 319 //
aktk 0:e85788b14028 320 // LOW PASS FILTERED
aktk 0:e85788b14028 321 // Get 10 data and store mean as a sample.
aktk 0:e85788b14028 322 // After get one original sample, system waits for 0.1 sec,
aktk 0:e85788b14028 323 // thus it takes 1 sec evry sampling.
aktk 3:b56e933bebc2 324 _Sample_Set[i].y = 0;
aktk 0:e85788b14028 325 for(int j = 0; j < 10; j++) {
aktk 0:e85788b14028 326 //unsigned short 's range [0 , 65535]
aktk 3:b56e933bebc2 327 //the Number of significant figures of read ytage is 3 or 4.
aktk 9:ec1ee4a6b6a4 328 tmp_y = _ai.read_u16();
aktk 3:b56e933bebc2 329
aktk 0:e85788b14028 330 #ifdef DEBUG
aktk 3:b56e933bebc2 331 g_Serial_Signal.printf("%d,",tmp_y);
aktk 0:e85788b14028 332 #endif
aktk 3:b56e933bebc2 333 _Sample_Set[i].y += (tmp_y / 10);
aktk 0:e85788b14028 334 wait(0.1);
aktk 0:e85788b14028 335 }
aktk 0:e85788b14028 336 #ifdef DEBUG
aktk 3:b56e933bebc2 337 g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y);
aktk 0:e85788b14028 338 #endif
aktk 0:e85788b14028 339 }
aktk 0:e85788b14028 340 //
aktk 0:e85788b14028 341 // Sort set data array in distanceAscending order
aktk 0:e85788b14028 342 //
aktk 0:e85788b14028 343 // Bucket sort
aktk 0:e85788b14028 344 for(int i = 0; i < _ENUM; i++)
aktk 3:b56e933bebc2 345 tmp_set[i].x = 0xaaaa;
aktk 0:e85788b14028 346 tmp = 0;
aktk 0:e85788b14028 347 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 348 // use x as index for x range [LIDX,RIDX]
aktk 3:b56e933bebc2 349 if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
aktk 3:b56e933bebc2 350 tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
aktk 3:b56e933bebc2 351 tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
aktk 3:b56e933bebc2 352 } else { // if a same x has been input, calcurate mean.
aktk 3:b56e933bebc2 353 tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
aktk 3:b56e933bebc2 354 tmp_set[_Sample_Set[i].x].y /= 2;
aktk 0:e85788b14028 355 tmp++;
aktk 0:e85788b14028 356 }
aktk 0:e85788b14028 357 }
aktk 0:e85788b14028 358 #ifdef DEBUG
aktk 0:e85788b14028 359 g_Serial_Signal.printf("%d\n", _Sample_Num );
aktk 0:e85788b14028 360 #endif
aktk 0:e85788b14028 361 // substruct tmp from number of sample.
aktk 0:e85788b14028 362 _Sample_Num -= tmp;
aktk 0:e85788b14028 363
aktk 0:e85788b14028 364 #ifdef DEBUG
aktk 0:e85788b14028 365 g_Serial_Signal.printf("tmp: %d\n", tmp );
aktk 0:e85788b14028 366 #endif
aktk 0:e85788b14028 367 // apply sort on _Sample_Set
aktk 0:e85788b14028 368 tmp = 0;
aktk 0:e85788b14028 369 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 370 if(tmp_set[i].x != 0xaaaa) {
aktk 3:b56e933bebc2 371 _Sample_Set[i - tmp].x = tmp_set[i].x;
aktk 3:b56e933bebc2 372 _Sample_Set[i - tmp].y = tmp_set[i].y;
aktk 0:e85788b14028 373 } else // if no data, skip it
aktk 0:e85788b14028 374 tmp++;
aktk 0:e85788b14028 375 }
aktk 0:e85788b14028 376 }
aktk 11:d60fb729eacf 377 #endif
aktk 0:e85788b14028 378
aktk 0:e85788b14028 379 //
aktk 0:e85788b14028 380 // Function to define _u_spline, specific constants of spline.
aktk 0:e85788b14028 381 //
aktk 0:e85788b14028 382 void TRP105FS::_makeSpline()
aktk 0:e85788b14028 383 {
aktk 3:b56e933bebc2 384 // x: x, distance
aktk 3:b56e933bebc2 385 // y: y, ytage
aktk 0:e85788b14028 386 //
aktk 0:e85788b14028 387 // N: max of index <=> (_Sample_Num - 1)
aktk 0:e85788b14028 388 //
aktk 0:e85788b14028 389 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 390 // i:[0,N]
aktk 0:e85788b14028 391 // _u_spline[0] = _u_spline[N] = 0
aktk 0:e85788b14028 392 //
aktk 0:e85788b14028 393 // h[i] = x[i+1] - x[i]
aktk 0:e85788b14028 394 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
aktk 0:e85788b14028 395 double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 0:e85788b14028 396 //unsigned short *h __attribute__((at(0x20080000)));
aktk 0:e85788b14028 397 //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
aktk 0:e85788b14028 398 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
aktk 0:e85788b14028 399 // i:[0,N-2]
aktk 0:e85788b14028 400 double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 401 //unsigned short *v __attribute__((at(0x20080100)));
aktk 0:e85788b14028 402 //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 403 // temporary array whose num of elm equals v array
aktk 0:e85788b14028 404 double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 405 //unsigned short *w __attribute__((at(0x20080200)));
aktk 0:e85788b14028 406 //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 407 //
aktk 0:e85788b14028 408 // [ 2(h[0]+h[1]) , h[1] , O ] [_u[1] ] [v[0] ]
aktk 0:e85788b14028 409 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [_u[2] ] [v[1] ]
aktk 0:e85788b14028 410 // [ ... ] * [... ] = [... ]
aktk 0:e85788b14028 411 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 412 // [ ... ] [ ... ] [ ... ]
aktk 0:e85788b14028 413 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 414 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [_u[N-1]] [v[N-2]]
aktk 0:e85788b14028 415 //
aktk 0:e85788b14028 416 // For LU decomposition
aktk 0:e85788b14028 417 double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 418 //unsigned short *Upper __attribute__((at(0x20080300)));
aktk 0:e85788b14028 419 //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 420 double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 421 //unsigned short *Lower __attribute__((at(0x20080400)));
aktk 0:e85788b14028 422 //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 423 #ifdef DEBUG
aktk 0:e85788b14028 424 _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
aktk 0:e85788b14028 425 #endif
aktk 0:e85788b14028 426 for(int i = 0; i < _Sample_Num - 1; i++)
aktk 3:b56e933bebc2 427 h[i] = (double)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
aktk 3:b56e933bebc2 428 //(unsigned short)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
aktk 0:e85788b14028 429
aktk 0:e85788b14028 430 for(int i = 0; i < _Sample_Num - 2; i++)
aktk 0:e85788b14028 431 v[i] = 6 * (
aktk 3:b56e933bebc2 432 ((double)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y)) / h[i + 1]
aktk 3:b56e933bebc2 433 //(unsigned short)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y) / h[i + 1]
aktk 0:e85788b14028 434 -
aktk 3:b56e933bebc2 435 ((double)(_Sample_Set[i + 1].y - _Sample_Set[i].y)) / h[i]
aktk 3:b56e933bebc2 436 //(unsigned short)(_Sample_Set[i + 1].y - _Sample_Set[i].y) / h[i]
aktk 0:e85788b14028 437 );
aktk 0:e85788b14028 438
aktk 0:e85788b14028 439 //
aktk 0:e85788b14028 440 // LU decomposition
aktk 0:e85788b14028 441 //
aktk 0:e85788b14028 442 Upper[0] = 2 * (h[0] + h[1]);
aktk 0:e85788b14028 443 Lower[0] = 0;
aktk 0:e85788b14028 444 for (int i = 1; i < _Sample_Num - 2; i++) {
aktk 0:e85788b14028 445 Lower[i] = h[i] / Upper[i - 1];
aktk 0:e85788b14028 446 Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
aktk 0:e85788b14028 447 }
aktk 0:e85788b14028 448
aktk 0:e85788b14028 449
aktk 0:e85788b14028 450 //
aktk 0:e85788b14028 451 // forward substitution
aktk 0:e85788b14028 452 //
aktk 0:e85788b14028 453 w[0] = v[0];
aktk 0:e85788b14028 454 for (int i = 1; i < _Sample_Num - 2; i ++) {
aktk 0:e85788b14028 455 w[i] = v[i] - Lower[i] * w[i-1];
aktk 0:e85788b14028 456 }
aktk 0:e85788b14028 457
aktk 0:e85788b14028 458
aktk 0:e85788b14028 459 //
aktk 0:e85788b14028 460 // backward substitution
aktk 0:e85788b14028 461 //
aktk 0:e85788b14028 462 _u_spline[_Sample_Num - 2] = w[_Sample_Num - 3] / Upper[_Sample_Num - 3];
aktk 0:e85788b14028 463 for(int i = _Sample_Num - 3; i > 0; i--) {
aktk 0:e85788b14028 464 _u_spline[i] = (w[(i - 1)] - h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
aktk 0:e85788b14028 465 }
aktk 0:e85788b14028 466
aktk 0:e85788b14028 467 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 468 _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;
aktk 0:e85788b14028 469
aktk 0:e85788b14028 470 #ifdef DEBUG
aktk 0:e85788b14028 471 _printOutData(h, _Sample_Num - 1, "h");
aktk 0:e85788b14028 472 _printOutData(v, _Sample_Num - 2, "v");
aktk 0:e85788b14028 473 _printOutData(w, _Sample_Num - 2, "w");
aktk 0:e85788b14028 474 _printOutData(Upper, _Sample_Num - 2, "Upper");
aktk 0:e85788b14028 475 _printOutData(Lower, _Sample_Num - 2, "Lower");
aktk 0:e85788b14028 476 _printOutData(_u_spline, _Sample_Num, "u");
aktk 0:e85788b14028 477 #endif
aktk 0:e85788b14028 478 free(h);
aktk 0:e85788b14028 479 free(v);
aktk 0:e85788b14028 480 free(w);
aktk 0:e85788b14028 481 free(Upper);
aktk 0:e85788b14028 482 free(Lower);
aktk 0:e85788b14028 483 }
aktk 0:e85788b14028 484 //
aktk 0:e85788b14028 485 // Function to return Voltage for distance.
aktk 0:e85788b14028 486 //
aktk 0:e85788b14028 487 unsigned short TRP105FS:: _getSplineYof(
aktk 0:e85788b14028 488 double arg_x // the argument is supposed as distance [mm]
aktk 0:e85788b14028 489 )
aktk 0:e85788b14028 490 {
aktk 3:b56e933bebc2 491 double y; // ytage calculated by spline polynomial
aktk 0:e85788b14028 492 double a,b,c,d; // which is specific constant of spline, and can be expressed with _u.
aktk 0:e85788b14028 493 int itv = 0; // interval(section) of interpolation
aktk 0:e85788b14028 494 // the number of interval is less 1 than the number of sample sets,
aktk 0:e85788b14028 495 // which means the max number of interval is _Sample_num - 2.
aktk 3:b56e933bebc2 496 if((double)(_Sample_Set[0].x) <= arg_x) {
aktk 3:b56e933bebc2 497 while (!((double)(_Sample_Set[itv].x) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].x))) {
aktk 0:e85788b14028 498 itv++;
aktk 0:e85788b14028 499 if(itv > _Sample_Num - 2) {
aktk 0:e85788b14028 500 itv = _Sample_Num - 2;
aktk 0:e85788b14028 501 break;
aktk 0:e85788b14028 502 }
aktk 0:e85788b14028 503 }
aktk 0:e85788b14028 504 }
aktk 3:b56e933bebc2 505 a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x);
aktk 0:e85788b14028 506 b = (double)(_u_spline[itv]) / 2.0;
aktk 3:b56e933bebc2 507 c = (double)(_Sample_Set[itv + 1].y - _Sample_Set[itv].y) / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x)
aktk 0:e85788b14028 508 -
aktk 3:b56e933bebc2 509 (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 510 d = (double)(_Sample_Set[itv].y);
aktk 0:e85788b14028 511 // cubic spline expression
aktk 3:b56e933bebc2 512 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 513 +
aktk 3:b56e933bebc2 514 b * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
aktk 0:e85788b14028 515 +
aktk 3:b56e933bebc2 516 c * (arg_x - (double)(_Sample_Set[itv].x))
aktk 0:e85788b14028 517 +
aktk 0:e85788b14028 518 d;
aktk 0:e85788b14028 519
aktk 0:e85788b14028 520 #ifdef DEBUG2
aktk 0:e85788b14028 521 g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
aktk 0:e85788b14028 522 g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
aktk 0:e85788b14028 523 g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
aktk 0:e85788b14028 524 #endif
aktk 0:e85788b14028 525
aktk 0:e85788b14028 526 return ((unsigned short)(int)y);
aktk 0:e85788b14028 527 }
aktk 0:e85788b14028 528
aktk 11:d60fb729eacf 529 #if defined(HAS_COM_TO_CONSOLE)
aktk 0:e85788b14028 530 void TRP105FS::calibrateSensor()
aktk 0:e85788b14028 531 {
aktk 0:e85788b14028 532 _sampleData();
aktk 0:e85788b14028 533 _makeSpline();
aktk 0:e85788b14028 534
aktk 0:e85788b14028 535 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 536 _Set[i].x = i;
aktk 4:701f958d137a 537 _Set[i].y = _getSplineYof((double)(_Set[i].x));
aktk 4:701f958d137a 538 _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
aktk 3:b56e933bebc2 539 #ifdef DEBUG2
aktk 3:b56e933bebc2 540 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 3:b56e933bebc2 541 #endif
aktk 3:b56e933bebc2 542 }
aktk 3:b56e933bebc2 543 }
aktk 11:d60fb729eacf 544 #endif
aktk 3:b56e933bebc2 545
aktk 3:b56e933bebc2 546 void TRP105FS::calibrate()
aktk 3:b56e933bebc2 547 {
aktk 4:701f958d137a 548 #ifdef DEBUG2
aktk 4:701f958d137a 549 g_Serial_Signal.printf("Sample Data Set\n");
aktk 4:701f958d137a 550 for(int i = 0; i < _Sample_Num; i++)
aktk 4:701f958d137a 551 g_Serial_Signal.printf("(%d,%d)\n", _Sample_Set[i].x, _Sample_Set[i].y);
aktk 4:701f958d137a 552 #endif
aktk 4:701f958d137a 553
aktk 3:b56e933bebc2 554 _makeSpline();
aktk 3:b56e933bebc2 555
aktk 3:b56e933bebc2 556 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 557 _Set[i].x = i;
aktk 4:701f958d137a 558 _Set[i].y = _getSplineYof((double)(_Set[i].x));
aktk 4:701f958d137a 559 _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
aktk 4:701f958d137a 560
aktk 0:e85788b14028 561 #ifdef DEBUG2
aktk 0:e85788b14028 562 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 0:e85788b14028 563 #endif
aktk 0:e85788b14028 564 }
aktk 0:e85788b14028 565 }
aktk 0:e85788b14028 566
aktk 0:e85788b14028 567 void TRP105FS::saveSetting()
aktk 0:e85788b14028 568 {
aktk 0:e85788b14028 569 FILE *fp;
aktk 0:e85788b14028 570
aktk 0:e85788b14028 571 fp = fopen("/local/savedata.log", "wb");
aktk 0:e85788b14028 572
aktk 0:e85788b14028 573 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 574 fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 575 fputc(0x2c, fp);
aktk 3:b56e933bebc2 576 fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 577 fputc(0x2c, fp);
aktk 0:e85788b14028 578 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 579 fputc(0x3b, fp);
aktk 0:e85788b14028 580 }
aktk 13:16fc61b41eff 581 fwrite(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 582 fputc(0x3b, fp);
aktk 0:e85788b14028 583 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 584 fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 585 fputc(0x2c, fp);
aktk 3:b56e933bebc2 586 fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 587 fputc(0x3b, fp);
aktk 0:e85788b14028 588 }
aktk 0:e85788b14028 589 fclose(fp);
aktk 0:e85788b14028 590
aktk 0:e85788b14028 591 }
aktk 0:e85788b14028 592
aktk 14:a99bf22b919d 593 #ifdef HAS_COM_TO_CONSOLE
aktk 0:e85788b14028 594 void TRP105FS::printThresholds()
aktk 0:e85788b14028 595 {
aktk 0:e85788b14028 596 for(int i = 0; i < _ENUM; i++)
aktk 0:e85788b14028 597 g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
aktk 0:e85788b14028 598 }
aktk 11:d60fb729eacf 599 #endif
aktk 11:d60fb729eacf 600
aktk 14:a99bf22b919d 601
aktk 14:a99bf22b919d 602 #ifdef HAS_LOCAL_FILE_SYSTEM
aktk 14:a99bf22b919d 603 void TRP105FS::printOutData(
aktk 14:a99bf22b919d 604 const char *filename)
aktk 14:a99bf22b919d 605 {
aktk 14:a99bf22b919d 606 FILE *fp;
aktk 14:a99bf22b919d 607 char *filepath;
aktk 14:a99bf22b919d 608 int fnnum = 0;
aktk 14:a99bf22b919d 609
aktk 14:a99bf22b919d 610 while (filename[fnnum] != 0) fnnum++;
aktk 14:a99bf22b919d 611 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 14:a99bf22b919d 612 sprintf(filepath, "/local/%s", filename);
aktk 14:a99bf22b919d 613 fp = fopen(filepath, "w");
aktk 14:a99bf22b919d 614 //fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 14:a99bf22b919d 615
aktk 14:a99bf22b919d 616 fprintf(fp, "x, y,(threshold)\n");
aktk 14:a99bf22b919d 617 for(int i = 0; i < _ENUM; i++) {
aktk 14:a99bf22b919d 618 fprintf(fp, "%d,%d,(%d)\n", _Set[i].x, _Set[i].y, _Threshold[i]);
aktk 14:a99bf22b919d 619 }
aktk 14:a99bf22b919d 620 fprintf(fp, "\nSample:x, y\n");
aktk 14:a99bf22b919d 621 for(int i = 0; i < _Sample_Num; i++) {
aktk 14:a99bf22b919d 622 fprintf(fp, "%d,%d\n", _Sample_Set[i].x, _Sample_Set[i].y);
aktk 14:a99bf22b919d 623 }
aktk 14:a99bf22b919d 624
aktk 14:a99bf22b919d 625 free(filepath);
aktk 14:a99bf22b919d 626 fclose(fp);
aktk 14:a99bf22b919d 627
aktk 14:a99bf22b919d 628 }
aktk 14:a99bf22b919d 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