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@13:16fc61b41eff, 2016-06-07 (annotated)

Committer:
aktk
Date:
Tue Jun 07 16:23:19 2016 +0000
Revision:
13:16fc61b41eff
Parent:
12:db5110d9d494
Child:
14:a99bf22b919d
type of _Sample_Num was modified: int -> unsigned short.; in saveSetting(any), size of _Sample_num to write into savedata file was modified: int -> unsigned short.

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 0:e85788b14028 24 #ifdef DEBUG
aktk 0:e85788b14028 25 DigitalOut led1(LED1);
aktk 0:e85788b14028 26 DigitalOut led2(LED2);
aktk 0:e85788b14028 27 DigitalOut led3(LED3);
aktk 0:e85788b14028 28 DigitalOut led4(LED4);
aktk 0:e85788b14028 29 #endif
aktk 0:e85788b14028 30
aktk 3:b56e933bebc2 31 // Constructor
aktk 0:e85788b14028 32 TRP105FS::TRP105FS()
aktk 3:b56e933bebc2 33 :_useType(AsMODULE)
aktk 9:ec1ee4a6b6a4 34 ,_ai(AnalogIn(DEFALT_AI_PIN))
aktk 0:e85788b14028 35 {
aktk 0:e85788b14028 36 _Sample_Num = 5;
aktk 0:e85788b14028 37 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 38 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:701f958d137a 39
aktk 4:701f958d137a 40 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 41 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 42 _u_spline[i] = 0.0;
aktk 4:701f958d137a 43 }
aktk 0:e85788b14028 44 }
aktk 0:e85788b14028 45
aktk 0:e85788b14028 46 TRP105FS::TRP105FS(
aktk 0:e85788b14028 47 unsigned int arg_num
aktk 0:e85788b14028 48 )
aktk 3:b56e933bebc2 49 :_useType(AsMODULE)
aktk 9:ec1ee4a6b6a4 50 ,_ai(AnalogIn(DEFALT_AI_PIN))
aktk 3:b56e933bebc2 51 {
aktk 3:b56e933bebc2 52 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 3:b56e933bebc2 53 else _Sample_Num = arg_num;
aktk 3:b56e933bebc2 54
aktk 3:b56e933bebc2 55 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 3:b56e933bebc2 56 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:701f958d137a 57
aktk 4:701f958d137a 58 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 59 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 60 _u_spline[i] = 0.0;
aktk 4:701f958d137a 61 }
aktk 3:b56e933bebc2 62 }
aktk 3:b56e933bebc2 63
aktk 3:b56e933bebc2 64 TRP105FS::TRP105FS(
aktk 3:b56e933bebc2 65 unsigned int arg_num,
aktk 3:b56e933bebc2 66 UseType arg_type
aktk 3:b56e933bebc2 67 )
aktk 3:b56e933bebc2 68 :_useType(arg_type)
aktk 9:ec1ee4a6b6a4 69 ,_ai(AnalogIn(DEFALT_AI_PIN))
aktk 0:e85788b14028 70 {
aktk 0:e85788b14028 71 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 0:e85788b14028 72 else _Sample_Num = arg_num;
aktk 0:e85788b14028 73
aktk 0:e85788b14028 74 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 75 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:701f958d137a 76
aktk 4:701f958d137a 77 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 78 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 79 _u_spline[i] = 0.0;
aktk 4:701f958d137a 80 }
aktk 0:e85788b14028 81 }
aktk 0:e85788b14028 82
aktk 0:e85788b14028 83 TRP105FS::TRP105FS(
aktk 0:e85788b14028 84 unsigned int arg_num,
aktk 10:b50e4bb40571 85 PinName pin
aktk 10:b50e4bb40571 86 )
aktk 10:b50e4bb40571 87 :_ai(AnalogIn(pin))
aktk 10:b50e4bb40571 88
aktk 10:b50e4bb40571 89 {
aktk 10:b50e4bb40571 90 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 10:b50e4bb40571 91 else _Sample_Num = arg_num;
aktk 10:b50e4bb40571 92
aktk 10:b50e4bb40571 93 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 10:b50e4bb40571 94 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 10:b50e4bb40571 95
aktk 10:b50e4bb40571 96 for(int i = 0; i < _Sample_Num; i++) {
aktk 10:b50e4bb40571 97 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 10:b50e4bb40571 98 _u_spline[i] = 0.0;
aktk 10:b50e4bb40571 99 }
aktk 10:b50e4bb40571 100 }
aktk 10:b50e4bb40571 101
aktk 10:b50e4bb40571 102 TRP105FS::TRP105FS(
aktk 10:b50e4bb40571 103 unsigned int arg_num,
aktk 3:b56e933bebc2 104 UseType arg_type,
aktk 3:b56e933bebc2 105 PinName pin
aktk 0:e85788b14028 106 )
aktk 3:b56e933bebc2 107 :_useType(arg_type)
aktk 9:ec1ee4a6b6a4 108 ,_ai(AnalogIn(pin))
aktk 0:e85788b14028 109 {
aktk 0:e85788b14028 110 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 0:e85788b14028 111 else _Sample_Num = arg_num;
aktk 0:e85788b14028 112
aktk 0:e85788b14028 113 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 114 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:701f958d137a 115
aktk 4:701f958d137a 116 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 117 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 118 _u_spline[i] = 0.0;
aktk 4:701f958d137a 119 }
aktk 0:e85788b14028 120 }
aktk 0:e85788b14028 121
aktk 3:b56e933bebc2 122 // Destructor
aktk 0:e85788b14028 123 TRP105FS::~TRP105FS()
aktk 0:e85788b14028 124 {
aktk 0:e85788b14028 125 free(_Sample_Set);
aktk 0:e85788b14028 126 free(_u_spline);
aktk 9:ec1ee4a6b6a4 127 //delete g_Sensor_Voltage;
aktk 0:e85788b14028 128 }
aktk 0:e85788b14028 129
aktk 3:b56e933bebc2 130
aktk 3:b56e933bebc2 131 unsigned short TRP105FS::getX(unsigned short arg_y)
aktk 0:e85788b14028 132 {
aktk 0:e85788b14028 133 int idx;
aktk 0:e85788b14028 134 unsigned short pv = 0;
aktk 0:e85788b14028 135
aktk 3:b56e933bebc2 136 pv = arg_y;
aktk 0:e85788b14028 137
aktk 0:e85788b14028 138 idx = _getNearest(_LIDX, _RIDX, pv);
aktk 0:e85788b14028 139
aktk 0:e85788b14028 140 if (idx != 0xFFFF) // unless occuring error
aktk 3:b56e933bebc2 141 return _Set[idx].x;
aktk 0:e85788b14028 142 else
aktk 0:e85788b14028 143 return 0xFFFF;
aktk 0:e85788b14028 144 }
aktk 0:e85788b14028 145
aktk 3:b56e933bebc2 146 unsigned short TRP105FS::getDistance(unsigned short arg_y)
aktk 3:b56e933bebc2 147 {
aktk 3:b56e933bebc2 148 return getX(arg_y);
aktk 3:b56e933bebc2 149 }
aktk 3:b56e933bebc2 150
aktk 3:b56e933bebc2 151 unsigned short TRP105FS::getDistance()
aktk 3:b56e933bebc2 152 {
aktk 9:ec1ee4a6b6a4 153 return getX(_ai.read_u16());
aktk 3:b56e933bebc2 154 }
aktk 3:b56e933bebc2 155
aktk 0:e85788b14028 156 /*
aktk 3:b56e933bebc2 157 Function to find a set whose y member is nearest a ytage from the sensor, recursively.
aktk 0:e85788b14028 158
aktk 0:e85788b14028 159 SHORT LONG
aktk 0:e85788b14028 160 +------------> HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+
aktk 3:b56e933bebc2 161 |(if ytage form sensor < threshold[cidx]) ||| (if threshold[cidx] < ytage form sensor)|
aktk 0:e85788b14028 162 | HIGH[lidx , ... , cidx]LOW ||| HIGH[cidx+1 , ... , ridx]LOW |
aktk 0:e85788b14028 163 | | | |
aktk 0:e85788b14028 164 +----------------+ +---------------+
aktk 0:e85788b14028 165 */
aktk 0:e85788b14028 166 int TRP105FS::_getNearest(
aktk 0:e85788b14028 167 int arg_lidx,
aktk 0:e85788b14028 168 int arg_ridx,
aktk 3:b56e933bebc2 169 unsigned short arg_y
aktk 0:e85788b14028 170 )
aktk 0:e85788b14028 171 {
aktk 0:e85788b14028 172 int cidx = (arg_lidx + arg_ridx) / 2;
aktk 0:e85788b14028 173
aktk 0:e85788b14028 174 // When the number of element to compare is only one, return it as result.
aktk 0:e85788b14028 175 if(arg_lidx == arg_ridx)
aktk 0:e85788b14028 176 return cidx;
aktk 3:b56e933bebc2 177 // If the ytage from the sensor is lower than the center threshold
aktk 0:e85788b14028 178 // (_set[cidx] > _threshold[cidx] > _set[cidx+1])
aktk 3:b56e933bebc2 179 else if(arg_y > _Threshold[cidx])
aktk 3:b56e933bebc2 180 return _getNearest(arg_lidx, cidx, arg_y);
aktk 3:b56e933bebc2 181 // If the ytage from the sensor is higher than the center threshold
aktk 3:b56e933bebc2 182 else if(arg_y < _Threshold[cidx])
aktk 3:b56e933bebc2 183 return _getNearest(cidx + 1, arg_ridx, arg_y);
aktk 3:b56e933bebc2 184 // If the ytage from the sensor eauals the center threshold
aktk 3:b56e933bebc2 185 else //(arg_y == _Treshold[cidx].y)
aktk 0:e85788b14028 186 return cidx;
aktk 0:e85788b14028 187 }
aktk 0:e85788b14028 188
aktk 4:701f958d137a 189 void TRP105FS::setSample(unsigned short arg_x, unsigned short arg_y)
aktk 4:701f958d137a 190 {
aktk 4:701f958d137a 191 _setSample(arg_x, arg_y);
aktk 3:b56e933bebc2 192 }
aktk 3:b56e933bebc2 193
aktk 3:b56e933bebc2 194 void TRP105FS::_setSample(unsigned short arg_x, unsigned short arg_y)
aktk 3:b56e933bebc2 195 {
aktk 3:b56e933bebc2 196 static unsigned int snum = 0;
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 3:b56e933bebc2 202 snum++;
aktk 4:701f958d137a 203 #ifdef DEBUG
aktk 4:701f958d137a 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 4:701f958d137a 209 if (snum < _Sample_Num) {
aktk 4:701f958d137a 210 num = snum;
aktk 3:b56e933bebc2 211 } else {
aktk 3:b56e933bebc2 212 // To reclloc memories if snum is bigger than _Sample_Num.
aktk 3:b56e933bebc2 213 // When realloc is failed, snum is back to porevius.
aktk 3:b56e933bebc2 214 VDset* tmp_Set = (VDset *)realloc(_Sample_Set, snum * sizeof(VDset));
aktk 3:b56e933bebc2 215 double* tmp__u_ = (double*)realloc(_u_spline, snum * sizeof(double));
aktk 3:b56e933bebc2 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 3:b56e933bebc2 219 num = _Sample_Num = snum;
aktk 3:b56e933bebc2 220 } else {
aktk 3:b56e933bebc2 221 snum--;
aktk 3:b56e933bebc2 222 num = snum = _Sample_Num ;
aktk 4:701f958d137a 223 #ifdef DEBUG
aktk 4:701f958d137a 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 4:701f958d137a 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 void TRP105FS::saveSetting()
aktk 0:e85788b14028 567 {
aktk 0:e85788b14028 568 FILE *fp;
aktk 0:e85788b14028 569
aktk 0:e85788b14028 570 fp = fopen("/local/savedata.log", "wb");
aktk 0:e85788b14028 571
aktk 0:e85788b14028 572 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 573 fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 574 fputc(0x2c, fp);
aktk 3:b56e933bebc2 575 fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 576 fputc(0x2c, fp);
aktk 0:e85788b14028 577 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 578 fputc(0x3b, fp);
aktk 0:e85788b14028 579 }
aktk 13:16fc61b41eff 580 fwrite(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 581 fputc(0x3b, fp);
aktk 0:e85788b14028 582 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 583 fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 584 fputc(0x2c, fp);
aktk 3:b56e933bebc2 585 fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 586 fputc(0x3b, fp);
aktk 0:e85788b14028 587 }
aktk 0:e85788b14028 588 fclose(fp);
aktk 0:e85788b14028 589
aktk 0:e85788b14028 590 }
aktk 0:e85788b14028 591
aktk 11:d60fb729eacf 592 #ifdef TARGET_LPC1768
aktk 0:e85788b14028 593 void TRP105FS::printThresholds()
aktk 0:e85788b14028 594 {
aktk 0:e85788b14028 595 for(int i = 0; i < _ENUM; i++)
aktk 0:e85788b14028 596 g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
aktk 0:e85788b14028 597 }
aktk 11:d60fb729eacf 598 #endif
aktk 11:d60fb729eacf 599
aktk 0:e85788b14028 600 void TRP105FS::loadSetting()
aktk 0:e85788b14028 601 {
aktk 0:e85788b14028 602 FILE *fp;
aktk 0:e85788b14028 603 char tmp;
aktk 0:e85788b14028 604
aktk 0:e85788b14028 605 //sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 606 //fp = fopen(filepath, "rb");
aktk 0:e85788b14028 607 fp = fopen("/local/savedata.log", "rb");
aktk 0:e85788b14028 608 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 609
aktk 3:b56e933bebc2 610 fread(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 611 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 612 #ifdef DEBUG2
aktk 3:b56e933bebc2 613 g_Serial_Signal.printf("%d%c", _Set[i].x, tmp);
aktk 0:e85788b14028 614 #endif
aktk 0:e85788b14028 615
aktk 3:b56e933bebc2 616 fread(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 617 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 618 #ifdef DEBUG2
aktk 3:b56e933bebc2 619 g_Serial_Signal.printf("%d%c", _Set[i].y, tmp);
aktk 0:e85788b14028 620 #endif
aktk 0:e85788b14028 621
aktk 0:e85788b14028 622 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 623 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 624 #ifdef DEBUG2
aktk 0:e85788b14028 625 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 626 #endif
aktk 0:e85788b14028 627 }
aktk 0:e85788b14028 628
aktk 0:e85788b14028 629 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 630 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 631
aktk 0:e85788b14028 632 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 633 fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 634 fread(&tmp, sizeof(char),1,fp);
aktk 3:b56e933bebc2 635 fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 636 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 637 }
aktk 0:e85788b14028 638 fclose(fp);
aktk 0:e85788b14028 639 }
aktk 0:e85788b14028 640
aktk 0:e85788b14028 641
aktk 0:e85788b14028 642 void TRP105FS::saveSetting(
aktk 0:e85788b14028 643 const char *filename
aktk 0:e85788b14028 644 )
aktk 0:e85788b14028 645 {
aktk 0:e85788b14028 646 FILE *fp;
aktk 0:e85788b14028 647 char *filepath;
aktk 0:e85788b14028 648 int fnnum = 0;
aktk 0:e85788b14028 649
aktk 0:e85788b14028 650 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 651 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 652
aktk 0:e85788b14028 653 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 654 fp = fopen(filepath, "wb");
aktk 0:e85788b14028 655
aktk 0:e85788b14028 656 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 657 fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 658 fputc(0x2c, fp);
aktk 3:b56e933bebc2 659 fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 660 fputc(0x2c, fp);
aktk 0:e85788b14028 661 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 662 fputc(0x3b, fp);
aktk 0:e85788b14028 663 }
aktk 13:16fc61b41eff 664 fwrite(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 665 fputc(0x3b, fp);
aktk 0:e85788b14028 666 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 667 fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 668 fputc(0x2c, fp);
aktk 3:b56e933bebc2 669 fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 670 fputc(0x3b, fp);
aktk 0:e85788b14028 671 }
aktk 0:e85788b14028 672 fclose(fp);
aktk 0:e85788b14028 673 free(filepath);
aktk 0:e85788b14028 674 }
aktk 0:e85788b14028 675
aktk 0:e85788b14028 676 void TRP105FS::loadSetting(
aktk 0:e85788b14028 677 const char *filename
aktk 0:e85788b14028 678 )
aktk 0:e85788b14028 679 {
aktk 0:e85788b14028 680 FILE *fp;
aktk 0:e85788b14028 681 char *filepath;
aktk 0:e85788b14028 682 char tmp;
aktk 0:e85788b14028 683 int fnnum = 0;
aktk 0:e85788b14028 684
aktk 0:e85788b14028 685 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 686 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 687
aktk 0:e85788b14028 688 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 689 fp = fopen(filepath, "rb");
aktk 10:b50e4bb40571 690
aktk 0:e85788b14028 691 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 692
aktk 3:b56e933bebc2 693 fread(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 694 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 695 #ifdef DEBUG3
aktk 3:b56e933bebc2 696 g_Serial_Signal.printf("%d%c", _Set[i].x, tmp);
aktk 0:e85788b14028 697 #endif
aktk 0:e85788b14028 698
aktk 3:b56e933bebc2 699 fread(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 700 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 701 #ifdef DEBUG3
aktk 3:b56e933bebc2 702 g_Serial_Signal.printf("%d%c", _Set[i].y, tmp);
aktk 0:e85788b14028 703 #endif
aktk 0:e85788b14028 704
aktk 0:e85788b14028 705 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 706 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 707 #ifdef DEBUG3
aktk 0:e85788b14028 708 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 709 #endif
aktk 0:e85788b14028 710 }
aktk 0:e85788b14028 711
aktk 0:e85788b14028 712 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 713 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 714 #ifdef DEBUG3
aktk 0:e85788b14028 715 g_Serial_Signal.printf("%d%c\n",_Sample_Num, tmp);
aktk 0:e85788b14028 716 #endif
aktk 0:e85788b14028 717
aktk 0:e85788b14028 718 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 719 fread(&_Sample_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", _Sample_Set[i].x, tmp);
aktk 0:e85788b14028 723 #endif
aktk 0:e85788b14028 724
aktk 3:b56e933bebc2 725 fread(&_Sample_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", _Sample_Set[i].y, tmp);
aktk 0:e85788b14028 729 #endif
aktk 0:e85788b14028 730 }
aktk 0:e85788b14028 731 fclose(fp);
aktk 0:e85788b14028 732 free(filepath);
aktk 0:e85788b14028 733 }
aktk 0:e85788b14028 734
aktk 12:db5110d9d494 735 void TRP105FS::loadSetting_fromSerial(
aktk 12:db5110d9d494 736 Serial* com
aktk 12:db5110d9d494 737 )
aktk 12:db5110d9d494 738 {
aktk 12:db5110d9d494 739 char buffer[3];
aktk 12:db5110d9d494 740
aktk 12:db5110d9d494 741 for(int i = 0; i < _ENUM; i++) {
aktk 13:16fc61b41eff 742 //Set X
aktk 12:db5110d9d494 743 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 744 buffer[j] = com->getc();
aktk 12:db5110d9d494 745 }
aktk 13:16fc61b41eff 746 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 747 _Set[i].x = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 12:db5110d9d494 748
aktk 13:16fc61b41eff 749 //Set Y
aktk 12:db5110d9d494 750 for(int j = 0; j < 3; j++) {
aktk 13:16fc61b41eff 751 buffer[j] = com->getc();
aktk 13:16fc61b41eff 752 }
aktk 13:16fc61b41eff 753 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 754 _Set[i].y = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 13:16fc61b41eff 755
aktk 13:16fc61b41eff 756 //Threshold
aktk 13:16fc61b41eff 757 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 758 buffer[j] = com->getc();
aktk 12:db5110d9d494 759 }
aktk 13:16fc61b41eff 760 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 761 _Threshold[i] = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 12:db5110d9d494 762 }
aktk 12:db5110d9d494 763
aktk 13:16fc61b41eff 764 //Sample Num
aktk 12:db5110d9d494 765 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 766 buffer[j] = com->getc();
aktk 12:db5110d9d494 767 }
aktk 13:16fc61b41eff 768 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 769 _Sample_Num = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 12:db5110d9d494 770
aktk 12:db5110d9d494 771 for(int i = 0; i < _Sample_Num; i++) {
aktk 13:16fc61b41eff 772 //Sample Set X
aktk 12:db5110d9d494 773 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 774 buffer[j] = com->getc();
aktk 12:db5110d9d494 775 }
aktk 13:16fc61b41eff 776 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 777 _Sample_Set[i].x= static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 13:16fc61b41eff 778
aktk 13:16fc61b41eff 779 //Sample Set Y
aktk 12:db5110d9d494 780 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 781 buffer[j] = com->getc();
aktk 12:db5110d9d494 782 }
aktk 13:16fc61b41eff 783 if(i != _Sample_Num - 1)
aktk 13:16fc61b41eff 784 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 785 else
aktk 13:16fc61b41eff 786 com->putc(0x04); //return EOT
aktk 13:16fc61b41eff 787 _Sample_Set[i].y= static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 12:db5110d9d494 788 }
aktk 12:db5110d9d494 789 }
aktk 12:db5110d9d494 790
aktk 4:701f958d137a 791 void TRP105FS::printOutData(
aktk 4:701f958d137a 792 const char *filename)
aktk 0:e85788b14028 793 {
aktk 0:e85788b14028 794 FILE *fp;
aktk 4:701f958d137a 795 char *filepath;
aktk 4:701f958d137a 796 int fnnum = 0;
aktk 4:701f958d137a 797
aktk 4:701f958d137a 798 while (filename[fnnum] != 0) fnnum++;
aktk 4:701f958d137a 799 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 4:701f958d137a 800 sprintf(filepath, "/local/%s", filename);
aktk 6:d2363b50aeaf 801 fp = fopen(filepath, "w");
aktk 6:d2363b50aeaf 802 //fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 10:b50e4bb40571 803
aktk 3:b56e933bebc2 804 fprintf(fp, "x, y,(threshold)\n");
aktk 0:e85788b14028 805 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 806 fprintf(fp, "%d,%d,(%d)\n", _Set[i].x, _Set[i].y, _Threshold[i]);
aktk 0:e85788b14028 807 }
aktk 3:b56e933bebc2 808 fprintf(fp, "\nSample:x, y\n");
aktk 0:e85788b14028 809 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 810 fprintf(fp, "%d,%d\n", _Sample_Set[i].x, _Sample_Set[i].y);
aktk 0:e85788b14028 811 }
aktk 10:b50e4bb40571 812
aktk 4:701f958d137a 813 free(filepath);
aktk 0:e85788b14028 814 fclose(fp);
aktk 0:e85788b14028 815
aktk 0:e85788b14028 816 }
aktk 4:701f958d137a 817
aktk 0:e85788b14028 818 void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
aktk 0:e85788b14028 819 {
aktk 0:e85788b14028 820 FILE *fp;
aktk 0:e85788b14028 821 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 822 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 823 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 824 fprintf(fp, "%d, ", arg[i]);
aktk 0:e85788b14028 825 }
aktk 0:e85788b14028 826 fprintf(fp, "\n");
aktk 0:e85788b14028 827 fclose(fp);
aktk 0:e85788b14028 828 }
aktk 0:e85788b14028 829 void TRP105FS::_printOutData(double *arg, int num, char* name)
aktk 0:e85788b14028 830 {
aktk 0:e85788b14028 831 FILE *fp;
aktk 0:e85788b14028 832
aktk 0:e85788b14028 833 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 834 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 835 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 836 fprintf(fp, "%.2f, ", arg[i]);
aktk 0:e85788b14028 837 }
aktk 0:e85788b14028 838 fprintf(fp, "\n");
aktk 0:e85788b14028 839 fclose(fp);
aktk 0:e85788b14028 840 }
aktk 0:e85788b14028 841 void TRP105FS::_printOutData(VDset *arg, int num, char* name)
aktk 0:e85788b14028 842 {
aktk 0:e85788b14028 843 FILE *fp;
aktk 0:e85788b14028 844
aktk 0:e85788b14028 845 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 846 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 847 for(int i = 0; i < num; i++) {
aktk 3:b56e933bebc2 848 fprintf(fp, "%d, ", arg[i].y);
aktk 0:e85788b14028 849 }
aktk 0:e85788b14028 850 fprintf(fp, "\n");
aktk 0:e85788b14028 851 fclose(fp);
aktk 0:e85788b14028 852 }