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@18:b046bba4ffa7, 2016-06-24 (annotated)

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

Who changed what in which revision?

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