Akifumi Takahashi
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@3:b56e933bebc2, 2016-06-01 (annotated)
- Committer:
- aktk
- Date:
- Wed Jun 01 05:38:46 2016 +0000
- Revision:
- 3:b56e933bebc2
- Parent:
- 1:2053662b1167
- Child:
- 4:701f958d137a
??to SPI
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| aktk | 0:e85788b14028 | 1 | #define DEBUG |
| aktk | 0:e85788b14028 | 2 | #include "TRP105F_Spline.h" |
| aktk | 0:e85788b14028 | 3 | |
| aktk | 3:b56e933bebc2 | 4 | // To get ytage of TRP105F |
| aktk | 3:b56e933bebc2 | 5 | AnalogIn* g_Sensor_Voltage; |
| aktk | 3:b56e933bebc2 | 6 | // To get ytage of TRP105F with spi |
| aktk | 3:b56e933bebc2 | 7 | //SPI spi(p5, p6, p7); // mosi(out), miso(in), sclk(clock) |
| aktk | 3:b56e933bebc2 | 8 | //DigitalOut cs(p8); // cs (the chip select signal) |
| aktk | 0:e85788b14028 | 9 | // To get sample distance via seral com |
| aktk | 0:e85788b14028 | 10 | Serial g_Serial_Signal(USBTX, USBRX); |
| aktk | 0:e85788b14028 | 11 | |
| aktk | 0:e85788b14028 | 12 | LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる） |
| aktk | 0:e85788b14028 | 13 | // for debug |
| aktk | 0:e85788b14028 | 14 | #ifdef DEBUG |
| aktk | 0:e85788b14028 | 15 | DigitalOut led1(LED1); |
| aktk | 0:e85788b14028 | 16 | DigitalOut led2(LED2); |
| aktk | 0:e85788b14028 | 17 | DigitalOut led3(LED3); |
| aktk | 0:e85788b14028 | 18 | DigitalOut led4(LED4); |
| aktk | 0:e85788b14028 | 19 | #endif |
| aktk | 0:e85788b14028 | 20 | |
| aktk | 3:b56e933bebc2 | 21 | // Constructor |
| aktk | 0:e85788b14028 | 22 | TRP105FS::TRP105FS() |
| aktk | 3:b56e933bebc2 | 23 | :_useType(AsMODULE) |
| aktk | 0:e85788b14028 | 24 | { |
| aktk | 0:e85788b14028 | 25 | _Sample_Num = 5; |
| aktk | 0:e85788b14028 | 26 | _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset)); |
| aktk | 0:e85788b14028 | 27 | _u_spline = (double*)malloc(_Sample_Num * sizeof(double)); |
| aktk | 3:b56e933bebc2 | 28 | g_Sensor_Voltage = new AnalogIn(p16); |
| aktk | 0:e85788b14028 | 29 | } |
| aktk | 0:e85788b14028 | 30 | |
| aktk | 0:e85788b14028 | 31 | TRP105FS::TRP105FS( |
| aktk | 0:e85788b14028 | 32 | unsigned int arg_num |
| aktk | 0:e85788b14028 | 33 | ) |
| aktk | 3:b56e933bebc2 | 34 | :_useType(AsMODULE) |
| aktk | 3:b56e933bebc2 | 35 | { |
| aktk | 3:b56e933bebc2 | 36 | if(arg_num > _ENUM) _Sample_Num = _ENUM; |
| aktk | 3:b56e933bebc2 | 37 | else _Sample_Num = arg_num; |
| aktk | 3:b56e933bebc2 | 38 | |
| aktk | 3:b56e933bebc2 | 39 | _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset)); |
| aktk | 3:b56e933bebc2 | 40 | _u_spline = (double*)malloc(_Sample_Num * sizeof(double)); |
| aktk | 3:b56e933bebc2 | 41 | g_Sensor_Voltage = new AnalogIn(p16); |
| aktk | 3:b56e933bebc2 | 42 | } |
| aktk | 3:b56e933bebc2 | 43 | |
| aktk | 3:b56e933bebc2 | 44 | TRP105FS::TRP105FS( |
| aktk | 3:b56e933bebc2 | 45 | unsigned int arg_num, |
| aktk | 3:b56e933bebc2 | 46 | UseType arg_type |
| aktk | 3:b56e933bebc2 | 47 | ) |
| aktk | 3:b56e933bebc2 | 48 | :_useType(arg_type) |
| aktk | 0:e85788b14028 | 49 | { |
| aktk | 0:e85788b14028 | 50 | if(arg_num > _ENUM) _Sample_Num = _ENUM; |
| aktk | 0:e85788b14028 | 51 | else _Sample_Num = arg_num; |
| aktk | 0:e85788b14028 | 52 | |
| aktk | 0:e85788b14028 | 53 | _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset)); |
| aktk | 0:e85788b14028 | 54 | _u_spline = (double*)malloc(_Sample_Num * sizeof(double)); |
| aktk | 3:b56e933bebc2 | 55 | g_Sensor_Voltage = new AnalogIn(p16); |
| aktk | 0:e85788b14028 | 56 | } |
| aktk | 0:e85788b14028 | 57 | |
| aktk | 0:e85788b14028 | 58 | TRP105FS::TRP105FS( |
| aktk | 0:e85788b14028 | 59 | unsigned int arg_num, |
| aktk | 3:b56e933bebc2 | 60 | UseType arg_type, |
| aktk | 3:b56e933bebc2 | 61 | PinName pin |
| aktk | 0:e85788b14028 | 62 | ) |
| aktk | 3:b56e933bebc2 | 63 | :_useType(arg_type) |
| aktk | 0:e85788b14028 | 64 | { |
| aktk | 0:e85788b14028 | 65 | if(arg_num > _ENUM) _Sample_Num = _ENUM; |
| aktk | 0:e85788b14028 | 66 | else _Sample_Num = arg_num; |
| aktk | 0:e85788b14028 | 67 | |
| aktk | 0:e85788b14028 | 68 | _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset)); |
| aktk | 0:e85788b14028 | 69 | _u_spline = (double*)malloc(_Sample_Num * sizeof(double)); |
| aktk | 3:b56e933bebc2 | 70 | g_Sensor_Voltage = new AnalogIn(pin); |
| aktk | 0:e85788b14028 | 71 | } |
| aktk | 0:e85788b14028 | 72 | |
| aktk | 3:b56e933bebc2 | 73 | // Destructor |
| aktk | 0:e85788b14028 | 74 | TRP105FS::~TRP105FS() |
| aktk | 0:e85788b14028 | 75 | { |
| aktk | 0:e85788b14028 | 76 | free(_Sample_Set); |
| aktk | 0:e85788b14028 | 77 | free(_u_spline); |
| aktk | 0:e85788b14028 | 78 | } |
| aktk | 0:e85788b14028 | 79 | |
| aktk | 3:b56e933bebc2 | 80 | |
| aktk | 3:b56e933bebc2 | 81 | unsigned short TRP105FS::getX(unsigned short arg_y) |
| aktk | 0:e85788b14028 | 82 | { |
| aktk | 0:e85788b14028 | 83 | int idx; |
| aktk | 0:e85788b14028 | 84 | unsigned short pv = 0; |
| aktk | 0:e85788b14028 | 85 | |
| aktk | 3:b56e933bebc2 | 86 | pv = arg_y; |
| aktk | 0:e85788b14028 | 87 | |
| aktk | 0:e85788b14028 | 88 | idx = _getNearest(_LIDX, _RIDX, pv); |
| aktk | 0:e85788b14028 | 89 | |
| aktk | 0:e85788b14028 | 90 | if (idx != 0xFFFF) // unless occuring error |
| aktk | 3:b56e933bebc2 | 91 | return _Set[idx].x; |
| aktk | 0:e85788b14028 | 92 | else |
| aktk | 0:e85788b14028 | 93 | return 0xFFFF; |
| aktk | 0:e85788b14028 | 94 | } |
| aktk | 0:e85788b14028 | 95 | |
| aktk | 3:b56e933bebc2 | 96 | unsigned short TRP105FS::getDistance(unsigned short arg_y) |
| aktk | 3:b56e933bebc2 | 97 | { |
| aktk | 3:b56e933bebc2 | 98 | return getX(arg_y); |
| aktk | 3:b56e933bebc2 | 99 | } |
| aktk | 3:b56e933bebc2 | 100 | |
| aktk | 3:b56e933bebc2 | 101 | unsigned short TRP105FS::getDistance() |
| aktk | 3:b56e933bebc2 | 102 | { |
| aktk | 3:b56e933bebc2 | 103 | return getX(g_Sensor_Voltage->read_u16()); |
| aktk | 3:b56e933bebc2 | 104 | } |
| aktk | 3:b56e933bebc2 | 105 | |
| aktk | 0:e85788b14028 | 106 | /* |
| aktk | 3:b56e933bebc2 | 107 | Function to find a set whose y member is nearest a ytage from the sensor, recursively. |
| aktk | 0:e85788b14028 | 108 | |
| aktk | 0:e85788b14028 | 109 | SHORT LONG |
| aktk | 0:e85788b14028 | 110 | +------------> HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+ |
| aktk | 3:b56e933bebc2 | 111 | |(if ytage form sensor < threshold[cidx]) ||| (if threshold[cidx] < ytage form sensor)| |
| aktk | 0:e85788b14028 | 112 | | HIGH[lidx , ... , cidx]LOW ||| HIGH[cidx+1 , ... , ridx]LOW | |
| aktk | 0:e85788b14028 | 113 | | | | | |
| aktk | 0:e85788b14028 | 114 | +----------------+ +---------------+ |
| aktk | 0:e85788b14028 | 115 | */ |
| aktk | 0:e85788b14028 | 116 | int TRP105FS::_getNearest( |
| aktk | 0:e85788b14028 | 117 | int arg_lidx, |
| aktk | 0:e85788b14028 | 118 | int arg_ridx, |
| aktk | 3:b56e933bebc2 | 119 | unsigned short arg_y |
| aktk | 0:e85788b14028 | 120 | ) |
| aktk | 0:e85788b14028 | 121 | { |
| aktk | 0:e85788b14028 | 122 | int cidx = (arg_lidx + arg_ridx) / 2; |
| aktk | 0:e85788b14028 | 123 | |
| aktk | 0:e85788b14028 | 124 | // When the number of element to compare is only one, return it as result. |
| aktk | 0:e85788b14028 | 125 | if(arg_lidx == arg_ridx) |
| aktk | 0:e85788b14028 | 126 | return cidx; |
| aktk | 3:b56e933bebc2 | 127 | // If the ytage from the sensor is lower than the center threshold |
| aktk | 0:e85788b14028 | 128 | // (_set[cidx] > _threshold[cidx] > _set[cidx+1]) |
| aktk | 3:b56e933bebc2 | 129 | else if(arg_y > _Threshold[cidx]) |
| aktk | 3:b56e933bebc2 | 130 | return _getNearest(arg_lidx, cidx, arg_y); |
| aktk | 3:b56e933bebc2 | 131 | // If the ytage from the sensor is higher than the center threshold |
| aktk | 3:b56e933bebc2 | 132 | else if(arg_y < _Threshold[cidx]) |
| aktk | 3:b56e933bebc2 | 133 | return _getNearest(cidx + 1, arg_ridx, arg_y); |
| aktk | 3:b56e933bebc2 | 134 | // If the ytage from the sensor eauals the center threshold |
| aktk | 3:b56e933bebc2 | 135 | else //(arg_y == _Treshold[cidx].y) |
| aktk | 0:e85788b14028 | 136 | return cidx; |
| aktk | 0:e85788b14028 | 137 | } |
| aktk | 0:e85788b14028 | 138 | |
| aktk | 3:b56e933bebc2 | 139 | void TRP105FS::setSample(unsigned short arg_x, unsigned short arg_y){ |
| aktk | 3:b56e933bebc2 | 140 | _setSample(unsigned short arg_x, unsigned short arg_y); |
| aktk | 3:b56e933bebc2 | 141 | } |
| aktk | 3:b56e933bebc2 | 142 | |
| aktk | 3:b56e933bebc2 | 143 | void TRP105FS::_setSample(unsigned short arg_x, unsigned short arg_y) |
| aktk | 3:b56e933bebc2 | 144 | { |
| aktk | 3:b56e933bebc2 | 145 | static unsigned int snum = 0; |
| aktk | 3:b56e933bebc2 | 146 | unsigned int num; |
| aktk | 3:b56e933bebc2 | 147 | int tmp; |
| aktk | 3:b56e933bebc2 | 148 | VDset tmp_set[_ENUM]; // for bucket sort |
| aktk | 3:b56e933bebc2 | 149 | |
| aktk | 3:b56e933bebc2 | 150 | // Increment it if call this function. |
| aktk | 3:b56e933bebc2 | 151 | snum++; |
| aktk | 3:b56e933bebc2 | 152 | |
| aktk | 3:b56e933bebc2 | 153 | // fit to smaller |
| aktk | 3:b56e933bebc2 | 154 | if ( _Sample_Num < snum) { |
| aktk | 3:b56e933bebc2 | 155 | num = _Sample_Num; |
| aktk | 3:b56e933bebc2 | 156 | } else { |
| aktk | 3:b56e933bebc2 | 157 | // To reclloc memories if snum is bigger than _Sample_Num. |
| aktk | 3:b56e933bebc2 | 158 | // When realloc is failed, snum is back to porevius. |
| aktk | 3:b56e933bebc2 | 159 | VDset* tmp_Set = (VDset *)realloc(_Sample_Set, snum * sizeof(VDset)); |
| aktk | 3:b56e933bebc2 | 160 | double* tmp__u_ = (double*)realloc(_u_spline, snum * sizeof(double)); |
| aktk | 3:b56e933bebc2 | 161 | if (tmp_set != NULL && tmp__u_ != NULL) { |
| aktk | 3:b56e933bebc2 | 162 | _Sample_Set = tmp_Set; |
| aktk | 3:b56e933bebc2 | 163 | _u_spline = tmp__u_; |
| aktk | 3:b56e933bebc2 | 164 | num = _Sample_Num = snum; |
| aktk | 3:b56e933bebc2 | 165 | } else { |
| aktk | 3:b56e933bebc2 | 166 | snum--; |
| aktk | 3:b56e933bebc2 | 167 | num = snum = _Sample_Num ; |
| aktk | 3:b56e933bebc2 | 168 | } |
| aktk | 3:b56e933bebc2 | 169 | } |
| aktk | 3:b56e933bebc2 | 170 | |
| aktk | 3:b56e933bebc2 | 171 | _Sample_Set[num - 1].x = arg_x; |
| aktk | 3:b56e933bebc2 | 172 | _Sample_Set[num - 1].y = arg_y; |
| aktk | 3:b56e933bebc2 | 173 | if((unsigned short)_RIDX < _Sample_Set[num - 1].x)_Sample_Set[num - 1].x = (unsigned short)_RIDX; |
| aktk | 3:b56e933bebc2 | 174 | |
| aktk | 3:b56e933bebc2 | 175 | // |
| aktk | 3:b56e933bebc2 | 176 | // Sort set data array in distanceAscending order |
| aktk | 3:b56e933bebc2 | 177 | // |
| aktk | 3:b56e933bebc2 | 178 | // Bucket sort |
| aktk | 3:b56e933bebc2 | 179 | for(int i = 0; i < _ENUM; i++) |
| aktk | 3:b56e933bebc2 | 180 | tmp_set[i].x = 0xaaaa; |
| aktk | 3:b56e933bebc2 | 181 | tmp = 0; |
| aktk | 3:b56e933bebc2 | 182 | for(int i = 0; i < _Sample_Num; i++) { |
| aktk | 3:b56e933bebc2 | 183 | // use x as index for x range [_LIDX,_RIDX] |
| aktk | 3:b56e933bebc2 | 184 | if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) { |
| aktk | 3:b56e933bebc2 | 185 | tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x; |
| aktk | 3:b56e933bebc2 | 186 | tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y; |
| aktk | 3:b56e933bebc2 | 187 | } else { // if a same x has been input, calcurate mean. |
| aktk | 3:b56e933bebc2 | 188 | tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y; |
| aktk | 3:b56e933bebc2 | 189 | tmp_set[_Sample_Set[i].x].y /= 2; |
| aktk | 3:b56e933bebc2 | 190 | tmp++; |
| aktk | 3:b56e933bebc2 | 191 | } |
| aktk | 3:b56e933bebc2 | 192 | } |
| aktk | 3:b56e933bebc2 | 193 | #ifdef DEBUG |
| aktk | 3:b56e933bebc2 | 194 | g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num ); |
| aktk | 3:b56e933bebc2 | 195 | g_Serial_Signal.printf("-) tmp: %d\n", tmp ); |
| aktk | 3:b56e933bebc2 | 196 | #endif |
| aktk | 3:b56e933bebc2 | 197 | // substruct tmp from number of sample. |
| aktk | 3:b56e933bebc2 | 198 | _Sample_Num -= tmp; |
| aktk | 3:b56e933bebc2 | 199 | #ifdef DEBUG |
| aktk | 3:b56e933bebc2 | 200 | g_Serial_Signal.printf("-----------------\n"); |
| aktk | 3:b56e933bebc2 | 201 | g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num ); |
| aktk | 3:b56e933bebc2 | 202 | #endif |
| aktk | 3:b56e933bebc2 | 203 | // apply sort on _Sample_Set |
| aktk | 3:b56e933bebc2 | 204 | tmp = 0; |
| aktk | 3:b56e933bebc2 | 205 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 3:b56e933bebc2 | 206 | if(tmp_set[i].x != 0xaaaa) { |
| aktk | 3:b56e933bebc2 | 207 | _Sample_Set[i - tmp].x = tmp_set[i].x; |
| aktk | 3:b56e933bebc2 | 208 | _Sample_Set[i - tmp].y = tmp_set[i].y; |
| aktk | 3:b56e933bebc2 | 209 | } else // if no data, skip it |
| aktk | 3:b56e933bebc2 | 210 | tmp++; |
| aktk | 3:b56e933bebc2 | 211 | } |
| aktk | 3:b56e933bebc2 | 212 | } |
| aktk | 0:e85788b14028 | 213 | void TRP105FS::_sampleData() |
| aktk | 0:e85788b14028 | 214 | { |
| aktk | 0:e85788b14028 | 215 | int tmp; |
| aktk | 0:e85788b14028 | 216 | char sig; |
| aktk | 3:b56e933bebc2 | 217 | unsigned short tmp_y; |
| aktk | 0:e85788b14028 | 218 | VDset tmp_set[_ENUM]; // for bucket sort |
| aktk | 0:e85788b14028 | 219 | |
| aktk | 3:b56e933bebc2 | 220 | // For evry set, |
| aktk | 3:b56e933bebc2 | 221 | // 1, get x data via serai com, |
| aktk | 3:b56e933bebc2 | 222 | // 2, get y data, |
| aktk | 0:e85788b14028 | 223 | // and then do same for next index set. |
| aktk | 0:e85788b14028 | 224 | for(int i = 0; i < _Sample_Num; i++) { |
| aktk | 0:e85788b14028 | 225 | // |
| aktk | 0:e85788b14028 | 226 | // Recieve a Distance datus and store it into member |
| aktk | 0:e85788b14028 | 227 | // |
| aktk | 3:b56e933bebc2 | 228 | if(_useType == AsDEBUG) { |
| aktk | 0:e85788b14028 | 229 | g_Serial_Signal.putc('>'); |
| aktk | 3:b56e933bebc2 | 230 | _Sample_Set[i].x = 0; |
| aktk | 0:e85788b14028 | 231 | do { |
| aktk | 0:e85788b14028 | 232 | sig = g_Serial_Signal.getc(); |
| aktk | 0:e85788b14028 | 233 | if('0' <= sig && sig <= '9') { |
| aktk | 3:b56e933bebc2 | 234 | _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48; |
| aktk | 0:e85788b14028 | 235 | g_Serial_Signal.putc(char(sig)); |
| aktk | 0:e85788b14028 | 236 | } else if(sig == 0x08) { |
| aktk | 3:b56e933bebc2 | 237 | _Sample_Set[i].x = 0; |
| aktk | 0:e85788b14028 | 238 | g_Serial_Signal.printf("[canseled!]"); |
| aktk | 0:e85788b14028 | 239 | g_Serial_Signal.putc('\n'); |
| aktk | 0:e85788b14028 | 240 | g_Serial_Signal.putc('>'); |
| aktk | 0:e85788b14028 | 241 | } |
| aktk | 0:e85788b14028 | 242 | } while (!(sig == 0x0a || sig == 0x0d)); |
| aktk | 0:e85788b14028 | 243 | g_Serial_Signal.putc('\n'); |
| aktk | 0:e85788b14028 | 244 | } else { //_Data_Input_Type == SYSTEM |
| aktk | 3:b56e933bebc2 | 245 | _Sample_Set[i].x = g_Serial_Signal.getc(); |
| aktk | 0:e85788b14028 | 246 | } |
| aktk | 0:e85788b14028 | 247 | |
| aktk | 0:e85788b14028 | 248 | // if input data is over the bound calibrate it below |
| aktk | 3:b56e933bebc2 | 249 | if (_Sample_Set[i].x < (unsigned short)_LIDX) |
| aktk | 3:b56e933bebc2 | 250 | _Sample_Set[i].x = (unsigned short)_LIDX; |
| aktk | 3:b56e933bebc2 | 251 | else if ((unsigned short)_RIDX < _Sample_Set[i].x) |
| aktk | 3:b56e933bebc2 | 252 | _Sample_Set[i].x = (unsigned short)_RIDX; |
| aktk | 0:e85788b14028 | 253 | // |
| aktk | 0:e85788b14028 | 254 | // Recieve a Voltage datus and store it into member |
| aktk | 0:e85788b14028 | 255 | // |
| aktk | 0:e85788b14028 | 256 | // LOW PASS FILTERED |
| aktk | 0:e85788b14028 | 257 | // Get 10 data and store mean as a sample. |
| aktk | 0:e85788b14028 | 258 | // After get one original sample, system waits for 0.1 sec, |
| aktk | 0:e85788b14028 | 259 | // thus it takes 1 sec evry sampling. |
| aktk | 3:b56e933bebc2 | 260 | _Sample_Set[i].y = 0; |
| aktk | 0:e85788b14028 | 261 | for(int j = 0; j < 10; j++) { |
| aktk | 0:e85788b14028 | 262 | //unsigned short 's range [0 , 65535] |
| aktk | 3:b56e933bebc2 | 263 | //the Number of significant figures of read ytage is 3 or 4. |
| aktk | 3:b56e933bebc2 | 264 | tmp_y = g_Sensor_Voltage->read_u16(); |
| aktk | 3:b56e933bebc2 | 265 | |
| aktk | 0:e85788b14028 | 266 | #ifdef DEBUG |
| aktk | 3:b56e933bebc2 | 267 | g_Serial_Signal.printf("%d,",tmp_y); |
| aktk | 0:e85788b14028 | 268 | #endif |
| aktk | 3:b56e933bebc2 | 269 | _Sample_Set[i].y += (tmp_y / 10); |
| aktk | 0:e85788b14028 | 270 | wait(0.1); |
| aktk | 0:e85788b14028 | 271 | } |
| aktk | 0:e85788b14028 | 272 | #ifdef DEBUG |
| aktk | 3:b56e933bebc2 | 273 | g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y); |
| aktk | 0:e85788b14028 | 274 | #endif |
| aktk | 0:e85788b14028 | 275 | } |
| aktk | 0:e85788b14028 | 276 | // |
| aktk | 0:e85788b14028 | 277 | // Sort set data array in distanceAscending order |
| aktk | 0:e85788b14028 | 278 | // |
| aktk | 0:e85788b14028 | 279 | // Bucket sort |
| aktk | 0:e85788b14028 | 280 | for(int i = 0; i < _ENUM; i++) |
| aktk | 3:b56e933bebc2 | 281 | tmp_set[i].x = 0xaaaa; |
| aktk | 0:e85788b14028 | 282 | tmp = 0; |
| aktk | 0:e85788b14028 | 283 | for(int i = 0; i < _Sample_Num; i++) { |
| aktk | 3:b56e933bebc2 | 284 | // use x as index for x range [LIDX,RIDX] |
| aktk | 3:b56e933bebc2 | 285 | if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) { |
| aktk | 3:b56e933bebc2 | 286 | tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x; |
| aktk | 3:b56e933bebc2 | 287 | tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y; |
| aktk | 3:b56e933bebc2 | 288 | } else { // if a same x has been input, calcurate mean. |
| aktk | 3:b56e933bebc2 | 289 | tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y; |
| aktk | 3:b56e933bebc2 | 290 | tmp_set[_Sample_Set[i].x].y /= 2; |
| aktk | 0:e85788b14028 | 291 | tmp++; |
| aktk | 0:e85788b14028 | 292 | } |
| aktk | 0:e85788b14028 | 293 | } |
| aktk | 0:e85788b14028 | 294 | #ifdef DEBUG |
| aktk | 0:e85788b14028 | 295 | g_Serial_Signal.printf("%d\n", _Sample_Num ); |
| aktk | 0:e85788b14028 | 296 | #endif |
| aktk | 0:e85788b14028 | 297 | // substruct tmp from number of sample. |
| aktk | 0:e85788b14028 | 298 | _Sample_Num -= tmp; |
| aktk | 0:e85788b14028 | 299 | |
| aktk | 0:e85788b14028 | 300 | #ifdef DEBUG |
| aktk | 0:e85788b14028 | 301 | g_Serial_Signal.printf("tmp: %d\n", tmp ); |
| aktk | 0:e85788b14028 | 302 | #endif |
| aktk | 0:e85788b14028 | 303 | // apply sort on _Sample_Set |
| aktk | 0:e85788b14028 | 304 | tmp = 0; |
| aktk | 0:e85788b14028 | 305 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 3:b56e933bebc2 | 306 | if(tmp_set[i].x != 0xaaaa) { |
| aktk | 3:b56e933bebc2 | 307 | _Sample_Set[i - tmp].x = tmp_set[i].x; |
| aktk | 3:b56e933bebc2 | 308 | _Sample_Set[i - tmp].y = tmp_set[i].y; |
| aktk | 0:e85788b14028 | 309 | } else // if no data, skip it |
| aktk | 0:e85788b14028 | 310 | tmp++; |
| aktk | 0:e85788b14028 | 311 | } |
| aktk | 0:e85788b14028 | 312 | } |
| aktk | 0:e85788b14028 | 313 | |
| aktk | 0:e85788b14028 | 314 | // |
| aktk | 0:e85788b14028 | 315 | // Function to define _u_spline, specific constants of spline. |
| aktk | 0:e85788b14028 | 316 | // |
| aktk | 0:e85788b14028 | 317 | void TRP105FS::_makeSpline() |
| aktk | 0:e85788b14028 | 318 | { |
| aktk | 3:b56e933bebc2 | 319 | // x: x, distance |
| aktk | 3:b56e933bebc2 | 320 | // y: y, ytage |
| aktk | 0:e85788b14028 | 321 | // |
| aktk | 0:e85788b14028 | 322 | // N: max of index <=> (_Sample_Num - 1) |
| aktk | 0:e85788b14028 | 323 | // |
| aktk | 0:e85788b14028 | 324 | // _u_spline[i] === d^2/dx^2(Spline f)[i] |
| aktk | 0:e85788b14028 | 325 | // i:[0,N] |
| aktk | 0:e85788b14028 | 326 | // _u_spline[0] = _u_spline[N] = 0 |
| aktk | 0:e85788b14028 | 327 | // |
| aktk | 0:e85788b14028 | 328 | // h[i] = x[i+1] - x[i] |
| aktk | 0:e85788b14028 | 329 | // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1 |
| aktk | 0:e85788b14028 | 330 | double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double)); |
| aktk | 0:e85788b14028 | 331 | //unsigned short *h __attribute__((at(0x20080000))); |
| aktk | 0:e85788b14028 | 332 | //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short)); |
| aktk | 0:e85788b14028 | 333 | // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i]) |
| aktk | 0:e85788b14028 | 334 | // i:[0,N-2] |
| aktk | 0:e85788b14028 | 335 | double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double)); |
| aktk | 0:e85788b14028 | 336 | //unsigned short *v __attribute__((at(0x20080100))); |
| aktk | 0:e85788b14028 | 337 | //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short)); |
| aktk | 0:e85788b14028 | 338 | // temporary array whose num of elm equals v array |
| aktk | 0:e85788b14028 | 339 | double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double)); |
| aktk | 0:e85788b14028 | 340 | //unsigned short *w __attribute__((at(0x20080200))); |
| aktk | 0:e85788b14028 | 341 | //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short)); |
| aktk | 0:e85788b14028 | 342 | // |
| aktk | 0:e85788b14028 | 343 | // [ 2(h[0]+h[1]) , h[1] , O ] [_u[1] ] [v[0] ] |
| aktk | 0:e85788b14028 | 344 | // [ h[1] , 2(h[1]+h[2]) , h[2] ] [_u[2] ] [v[1] ] |
| aktk | 0:e85788b14028 | 345 | // [ ... ] * [... ] = [... ] |
| aktk | 0:e85788b14028 | 346 | // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [_u[j+1]] [v[j] ] |
| aktk | 0:e85788b14028 | 347 | // [ ... ] [ ... ] [ ... ] |
| aktk | 0:e85788b14028 | 348 | // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [_u[j+1]] [v[j] ] |
| aktk | 0:e85788b14028 | 349 | // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [_u[N-1]] [v[N-2]] |
| aktk | 0:e85788b14028 | 350 | // |
| aktk | 0:e85788b14028 | 351 | // For LU decomposition |
| aktk | 0:e85788b14028 | 352 | double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double)); |
| aktk | 0:e85788b14028 | 353 | //unsigned short *Upper __attribute__((at(0x20080300))); |
| aktk | 0:e85788b14028 | 354 | //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short)); |
| aktk | 0:e85788b14028 | 355 | double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double)); |
| aktk | 0:e85788b14028 | 356 | //unsigned short *Lower __attribute__((at(0x20080400))); |
| aktk | 0:e85788b14028 | 357 | //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short)); |
| aktk | 0:e85788b14028 | 358 | #ifdef DEBUG |
| aktk | 0:e85788b14028 | 359 | _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet"); |
| aktk | 0:e85788b14028 | 360 | #endif |
| aktk | 0:e85788b14028 | 361 | for(int i = 0; i < _Sample_Num - 1; i++) |
| aktk | 3:b56e933bebc2 | 362 | h[i] = (double)(_Sample_Set[i + 1].x - _Sample_Set[i].x); |
| aktk | 3:b56e933bebc2 | 363 | //(unsigned short)(_Sample_Set[i + 1].x - _Sample_Set[i].x); |
| aktk | 0:e85788b14028 | 364 | |
| aktk | 0:e85788b14028 | 365 | for(int i = 0; i < _Sample_Num - 2; i++) |
| aktk | 0:e85788b14028 | 366 | v[i] = 6 * ( |
| aktk | 3:b56e933bebc2 | 367 | ((double)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y)) / h[i + 1] |
| aktk | 3:b56e933bebc2 | 368 | //(unsigned short)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y) / h[i + 1] |
| aktk | 0:e85788b14028 | 369 | - |
| aktk | 3:b56e933bebc2 | 370 | ((double)(_Sample_Set[i + 1].y - _Sample_Set[i].y)) / h[i] |
| aktk | 3:b56e933bebc2 | 371 | //(unsigned short)(_Sample_Set[i + 1].y - _Sample_Set[i].y) / h[i] |
| aktk | 0:e85788b14028 | 372 | ); |
| aktk | 0:e85788b14028 | 373 | |
| aktk | 0:e85788b14028 | 374 | // |
| aktk | 0:e85788b14028 | 375 | // LU decomposition |
| aktk | 0:e85788b14028 | 376 | // |
| aktk | 0:e85788b14028 | 377 | Upper[0] = 2 * (h[0] + h[1]); |
| aktk | 0:e85788b14028 | 378 | Lower[0] = 0; |
| aktk | 0:e85788b14028 | 379 | for (int i = 1; i < _Sample_Num - 2; i++) { |
| aktk | 0:e85788b14028 | 380 | Lower[i] = h[i] / Upper[i - 1]; |
| aktk | 0:e85788b14028 | 381 | Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i]; |
| aktk | 0:e85788b14028 | 382 | } |
| aktk | 0:e85788b14028 | 383 | |
| aktk | 0:e85788b14028 | 384 | |
| aktk | 0:e85788b14028 | 385 | // |
| aktk | 0:e85788b14028 | 386 | // forward substitution |
| aktk | 0:e85788b14028 | 387 | // |
| aktk | 0:e85788b14028 | 388 | w[0] = v[0]; |
| aktk | 0:e85788b14028 | 389 | for (int i = 1; i < _Sample_Num - 2; i ++) { |
| aktk | 0:e85788b14028 | 390 | w[i] = v[i] - Lower[i] * w[i-1]; |
| aktk | 0:e85788b14028 | 391 | } |
| aktk | 0:e85788b14028 | 392 | |
| aktk | 0:e85788b14028 | 393 | |
| aktk | 0:e85788b14028 | 394 | // |
| aktk | 0:e85788b14028 | 395 | // backward substitution |
| aktk | 0:e85788b14028 | 396 | // |
| aktk | 0:e85788b14028 | 397 | _u_spline[_Sample_Num - 2] = w[_Sample_Num - 3] / Upper[_Sample_Num - 3]; |
| aktk | 0:e85788b14028 | 398 | for(int i = _Sample_Num - 3; i > 0; i--) { |
| aktk | 0:e85788b14028 | 399 | _u_spline[i] = (w[(i - 1)] - h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)]; |
| aktk | 0:e85788b14028 | 400 | } |
| aktk | 0:e85788b14028 | 401 | |
| aktk | 0:e85788b14028 | 402 | // _u_spline[i] === d^2/dx^2(Spline f)[i] |
| aktk | 0:e85788b14028 | 403 | _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0; |
| aktk | 0:e85788b14028 | 404 | |
| aktk | 0:e85788b14028 | 405 | #ifdef DEBUG |
| aktk | 0:e85788b14028 | 406 | _printOutData(h, _Sample_Num - 1, "h"); |
| aktk | 0:e85788b14028 | 407 | _printOutData(v, _Sample_Num - 2, "v"); |
| aktk | 0:e85788b14028 | 408 | _printOutData(w, _Sample_Num - 2, "w"); |
| aktk | 0:e85788b14028 | 409 | _printOutData(Upper, _Sample_Num - 2, "Upper"); |
| aktk | 0:e85788b14028 | 410 | _printOutData(Lower, _Sample_Num - 2, "Lower"); |
| aktk | 0:e85788b14028 | 411 | _printOutData(_u_spline, _Sample_Num, "u"); |
| aktk | 0:e85788b14028 | 412 | #endif |
| aktk | 0:e85788b14028 | 413 | free(h); |
| aktk | 0:e85788b14028 | 414 | free(v); |
| aktk | 0:e85788b14028 | 415 | free(w); |
| aktk | 0:e85788b14028 | 416 | free(Upper); |
| aktk | 0:e85788b14028 | 417 | free(Lower); |
| aktk | 0:e85788b14028 | 418 | } |
| aktk | 0:e85788b14028 | 419 | // |
| aktk | 0:e85788b14028 | 420 | // Function to return Voltage for distance. |
| aktk | 0:e85788b14028 | 421 | // |
| aktk | 0:e85788b14028 | 422 | unsigned short TRP105FS:: _getSplineYof( |
| aktk | 0:e85788b14028 | 423 | double arg_x // the argument is supposed as distance [mm] |
| aktk | 0:e85788b14028 | 424 | ) |
| aktk | 0:e85788b14028 | 425 | { |
| aktk | 3:b56e933bebc2 | 426 | double y; // ytage calculated by spline polynomial |
| aktk | 0:e85788b14028 | 427 | double a,b,c,d; // which is specific constant of spline, and can be expressed with _u. |
| aktk | 0:e85788b14028 | 428 | int itv = 0; // interval(section) of interpolation |
| aktk | 0:e85788b14028 | 429 | // the number of interval is less 1 than the number of sample sets, |
| aktk | 0:e85788b14028 | 430 | // which means the max number of interval is _Sample_num - 2. |
| aktk | 3:b56e933bebc2 | 431 | if((double)(_Sample_Set[0].x) <= arg_x) { |
| aktk | 3:b56e933bebc2 | 432 | while (!((double)(_Sample_Set[itv].x) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].x))) { |
| aktk | 0:e85788b14028 | 433 | itv++; |
| aktk | 0:e85788b14028 | 434 | if(itv > _Sample_Num - 2) { |
| aktk | 0:e85788b14028 | 435 | itv = _Sample_Num - 2; |
| aktk | 0:e85788b14028 | 436 | break; |
| aktk | 0:e85788b14028 | 437 | } |
| aktk | 0:e85788b14028 | 438 | } |
| aktk | 0:e85788b14028 | 439 | } |
| aktk | 3:b56e933bebc2 | 440 | a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x); |
| aktk | 0:e85788b14028 | 441 | b = (double)(_u_spline[itv]) / 2.0; |
| aktk | 3:b56e933bebc2 | 442 | c = (double)(_Sample_Set[itv + 1].y - _Sample_Set[itv].y) / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x) |
| aktk | 0:e85788b14028 | 443 | - |
| aktk | 3:b56e933bebc2 | 444 | (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 | 445 | d = (double)(_Sample_Set[itv].y); |
| aktk | 0:e85788b14028 | 446 | // cubic spline expression |
| aktk | 3:b56e933bebc2 | 447 | 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 | 448 | + |
| aktk | 3:b56e933bebc2 | 449 | b * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x)) |
| aktk | 0:e85788b14028 | 450 | + |
| aktk | 3:b56e933bebc2 | 451 | c * (arg_x - (double)(_Sample_Set[itv].x)) |
| aktk | 0:e85788b14028 | 452 | + |
| aktk | 0:e85788b14028 | 453 | d; |
| aktk | 0:e85788b14028 | 454 | |
| aktk | 0:e85788b14028 | 455 | #ifdef DEBUG2 |
| aktk | 0:e85788b14028 | 456 | g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv); |
| aktk | 0:e85788b14028 | 457 | g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d); |
| aktk | 0:e85788b14028 | 458 | g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y); |
| aktk | 0:e85788b14028 | 459 | #endif |
| aktk | 0:e85788b14028 | 460 | |
| aktk | 0:e85788b14028 | 461 | return ((unsigned short)(int)y); |
| aktk | 0:e85788b14028 | 462 | } |
| aktk | 0:e85788b14028 | 463 | |
| aktk | 0:e85788b14028 | 464 | void TRP105FS::calibrateSensor() |
| aktk | 0:e85788b14028 | 465 | { |
| aktk | 0:e85788b14028 | 466 | _sampleData(); |
| aktk | 0:e85788b14028 | 467 | _makeSpline(); |
| aktk | 0:e85788b14028 | 468 | |
| aktk | 0:e85788b14028 | 469 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 3:b56e933bebc2 | 470 | _Set[i].x = i; |
| aktk | 3:b56e933bebc2 | 471 | _Set[i].x = _getSplineYof((double)(_Set[i].x)); |
| aktk | 3:b56e933bebc2 | 472 | _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5); |
| aktk | 3:b56e933bebc2 | 473 | #ifdef DEBUG2 |
| aktk | 3:b56e933bebc2 | 474 | g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]); |
| aktk | 3:b56e933bebc2 | 475 | #endif |
| aktk | 3:b56e933bebc2 | 476 | } |
| aktk | 3:b56e933bebc2 | 477 | } |
| aktk | 3:b56e933bebc2 | 478 | |
| aktk | 3:b56e933bebc2 | 479 | void TRP105FS::calibrate() |
| aktk | 3:b56e933bebc2 | 480 | { |
| aktk | 3:b56e933bebc2 | 481 | _makeSpline(); |
| aktk | 3:b56e933bebc2 | 482 | |
| aktk | 3:b56e933bebc2 | 483 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 3:b56e933bebc2 | 484 | _Set[i].x = i; |
| aktk | 3:b56e933bebc2 | 485 | _Set[i].x = _getSplineYof((double)(_Set[i].x)); |
| aktk | 3:b56e933bebc2 | 486 | _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5); |
| aktk | 0:e85788b14028 | 487 | #ifdef DEBUG2 |
| aktk | 0:e85788b14028 | 488 | g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]); |
| aktk | 0:e85788b14028 | 489 | #endif |
| aktk | 0:e85788b14028 | 490 | } |
| aktk | 0:e85788b14028 | 491 | } |
| aktk | 0:e85788b14028 | 492 | |
| aktk | 0:e85788b14028 | 493 | void TRP105FS::saveSetting() |
| aktk | 0:e85788b14028 | 494 | { |
| aktk | 0:e85788b14028 | 495 | FILE *fp; |
| aktk | 0:e85788b14028 | 496 | |
| aktk | 0:e85788b14028 | 497 | fp = fopen("/local/savedata.log", "wb"); |
| aktk | 0:e85788b14028 | 498 | |
| aktk | 0:e85788b14028 | 499 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 3:b56e933bebc2 | 500 | fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 501 | fputc(0x2c, fp); |
| aktk | 3:b56e933bebc2 | 502 | fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 503 | fputc(0x2c, fp); |
| aktk | 0:e85788b14028 | 504 | fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 505 | fputc(0x3b, fp); |
| aktk | 0:e85788b14028 | 506 | } |
| aktk | 0:e85788b14028 | 507 | fwrite(&_Sample_Num, sizeof(int), 1, fp); |
| aktk | 0:e85788b14028 | 508 | fputc(0x3b, fp); |
| aktk | 0:e85788b14028 | 509 | for(int i = 0; i < _Sample_Num; i++) { |
| aktk | 3:b56e933bebc2 | 510 | fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 511 | fputc(0x2c, fp); |
| aktk | 3:b56e933bebc2 | 512 | fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 513 | fputc(0x3b, fp); |
| aktk | 0:e85788b14028 | 514 | } |
| aktk | 0:e85788b14028 | 515 | fclose(fp); |
| aktk | 0:e85788b14028 | 516 | |
| aktk | 0:e85788b14028 | 517 | } |
| aktk | 0:e85788b14028 | 518 | |
| aktk | 0:e85788b14028 | 519 | void TRP105FS::printThresholds() |
| aktk | 0:e85788b14028 | 520 | { |
| aktk | 0:e85788b14028 | 521 | for(int i = 0; i < _ENUM; i++) |
| aktk | 0:e85788b14028 | 522 | g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]); |
| aktk | 0:e85788b14028 | 523 | } |
| aktk | 0:e85788b14028 | 524 | void TRP105FS::loadSetting() |
| aktk | 0:e85788b14028 | 525 | { |
| aktk | 0:e85788b14028 | 526 | FILE *fp; |
| aktk | 0:e85788b14028 | 527 | char tmp; |
| aktk | 0:e85788b14028 | 528 | |
| aktk | 0:e85788b14028 | 529 | //sprintf(filepath, "/local/%s", filename); |
| aktk | 0:e85788b14028 | 530 | //fp = fopen(filepath, "rb"); |
| aktk | 0:e85788b14028 | 531 | fp = fopen("/local/savedata.log", "rb"); |
| aktk | 0:e85788b14028 | 532 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 0:e85788b14028 | 533 | |
| aktk | 3:b56e933bebc2 | 534 | fread(&_Set[i].x, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 535 | fread(&tmp, sizeof(char), 1, fp); |
| aktk | 0:e85788b14028 | 536 | #ifdef DEBUG2 |
| aktk | 3:b56e933bebc2 | 537 | g_Serial_Signal.printf("%d%c", _Set[i].x, tmp); |
| aktk | 0:e85788b14028 | 538 | #endif |
| aktk | 0:e85788b14028 | 539 | |
| aktk | 3:b56e933bebc2 | 540 | fread(&_Set[i].y, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 541 | fread(&tmp, sizeof(char), 1, fp); |
| aktk | 0:e85788b14028 | 542 | #ifdef DEBUG2 |
| aktk | 3:b56e933bebc2 | 543 | g_Serial_Signal.printf("%d%c", _Set[i].y, tmp); |
| aktk | 0:e85788b14028 | 544 | #endif |
| aktk | 0:e85788b14028 | 545 | |
| aktk | 0:e85788b14028 | 546 | fread(&_Threshold[i], sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 547 | fread(&tmp, sizeof(char), 1, fp); |
| aktk | 0:e85788b14028 | 548 | #ifdef DEBUG2 |
| aktk | 0:e85788b14028 | 549 | g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp); |
| aktk | 0:e85788b14028 | 550 | #endif |
| aktk | 0:e85788b14028 | 551 | } |
| aktk | 0:e85788b14028 | 552 | |
| aktk | 0:e85788b14028 | 553 | fread(&_Sample_Num, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 554 | fread(&tmp, sizeof(char), 1, fp); |
| aktk | 0:e85788b14028 | 555 | |
| aktk | 0:e85788b14028 | 556 | for(int i = 0; i < _Sample_Num; i++) { |
| aktk | 3:b56e933bebc2 | 557 | fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 558 | fread(&tmp, sizeof(char),1,fp); |
| aktk | 3:b56e933bebc2 | 559 | fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 560 | fread(&tmp, sizeof(char),1,fp); |
| aktk | 0:e85788b14028 | 561 | } |
| aktk | 0:e85788b14028 | 562 | fclose(fp); |
| aktk | 0:e85788b14028 | 563 | } |
| aktk | 0:e85788b14028 | 564 | |
| aktk | 0:e85788b14028 | 565 | |
| aktk | 0:e85788b14028 | 566 | void TRP105FS::saveSetting( |
| aktk | 0:e85788b14028 | 567 | const char *filename |
| aktk | 0:e85788b14028 | 568 | ) |
| aktk | 0:e85788b14028 | 569 | { |
| aktk | 0:e85788b14028 | 570 | FILE *fp; |
| aktk | 0:e85788b14028 | 571 | char *filepath; |
| aktk | 0:e85788b14028 | 572 | int fnnum = 0; |
| aktk | 0:e85788b14028 | 573 | |
| aktk | 0:e85788b14028 | 574 | while (filename[fnnum] != 0) fnnum++; |
| aktk | 0:e85788b14028 | 575 | filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char. |
| aktk | 0:e85788b14028 | 576 | |
| aktk | 0:e85788b14028 | 577 | sprintf(filepath, "/local/%s", filename); |
| aktk | 0:e85788b14028 | 578 | fp = fopen(filepath, "wb"); |
| aktk | 0:e85788b14028 | 579 | |
| aktk | 0:e85788b14028 | 580 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 3:b56e933bebc2 | 581 | fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 582 | fputc(0x2c, fp); |
| aktk | 3:b56e933bebc2 | 583 | fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 584 | fputc(0x2c, fp); |
| aktk | 0:e85788b14028 | 585 | fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 586 | fputc(0x3b, fp); |
| aktk | 0:e85788b14028 | 587 | } |
| aktk | 0:e85788b14028 | 588 | fwrite(&_Sample_Num, sizeof(int), 1, fp); |
| aktk | 0:e85788b14028 | 589 | fputc(0x3b, fp); |
| aktk | 0:e85788b14028 | 590 | for(int i = 0; i < _Sample_Num; i++) { |
| aktk | 3:b56e933bebc2 | 591 | fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 592 | fputc(0x2c, fp); |
| aktk | 3:b56e933bebc2 | 593 | fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 594 | fputc(0x3b, fp); |
| aktk | 0:e85788b14028 | 595 | } |
| aktk | 0:e85788b14028 | 596 | fclose(fp); |
| aktk | 0:e85788b14028 | 597 | free(filepath); |
| aktk | 0:e85788b14028 | 598 | } |
| aktk | 0:e85788b14028 | 599 | |
| aktk | 0:e85788b14028 | 600 | void TRP105FS::loadSetting( |
| aktk | 0:e85788b14028 | 601 | const char *filename |
| aktk | 0:e85788b14028 | 602 | ) |
| aktk | 0:e85788b14028 | 603 | { |
| aktk | 0:e85788b14028 | 604 | FILE *fp; |
| aktk | 0:e85788b14028 | 605 | char *filepath; |
| aktk | 0:e85788b14028 | 606 | char tmp; |
| aktk | 0:e85788b14028 | 607 | int fnnum = 0; |
| aktk | 0:e85788b14028 | 608 | |
| aktk | 0:e85788b14028 | 609 | while (filename[fnnum] != 0) fnnum++; |
| aktk | 0:e85788b14028 | 610 | filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char. |
| aktk | 0:e85788b14028 | 611 | |
| aktk | 0:e85788b14028 | 612 | sprintf(filepath, "/local/%s", filename); |
| aktk | 0:e85788b14028 | 613 | fp = fopen(filepath, "rb"); |
| aktk | 0:e85788b14028 | 614 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 0:e85788b14028 | 615 | |
| aktk | 3:b56e933bebc2 | 616 | fread(&_Set[i].x, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 617 | fread(&tmp, sizeof(char), 1, fp); |
| aktk | 0:e85788b14028 | 618 | #ifdef DEBUG3 |
| aktk | 3:b56e933bebc2 | 619 | g_Serial_Signal.printf("%d%c", _Set[i].x, tmp); |
| aktk | 0:e85788b14028 | 620 | #endif |
| aktk | 0:e85788b14028 | 621 | |
| aktk | 3:b56e933bebc2 | 622 | fread(&_Set[i].y, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 623 | fread(&tmp, sizeof(char), 1, fp); |
| aktk | 0:e85788b14028 | 624 | #ifdef DEBUG3 |
| aktk | 3:b56e933bebc2 | 625 | g_Serial_Signal.printf("%d%c", _Set[i].y, tmp); |
| aktk | 0:e85788b14028 | 626 | #endif |
| aktk | 0:e85788b14028 | 627 | |
| aktk | 0:e85788b14028 | 628 | fread(&_Threshold[i], sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 629 | fread(&tmp, sizeof(char), 1, fp); |
| aktk | 0:e85788b14028 | 630 | #ifdef DEBUG3 |
| aktk | 0:e85788b14028 | 631 | g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp); |
| aktk | 0:e85788b14028 | 632 | #endif |
| aktk | 0:e85788b14028 | 633 | } |
| aktk | 0:e85788b14028 | 634 | |
| aktk | 0:e85788b14028 | 635 | fread(&_Sample_Num, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 636 | fread(&tmp, sizeof(char), 1, fp); |
| aktk | 0:e85788b14028 | 637 | #ifdef DEBUG3 |
| aktk | 0:e85788b14028 | 638 | g_Serial_Signal.printf("%d%c\n",_Sample_Num, tmp); |
| aktk | 0:e85788b14028 | 639 | #endif |
| aktk | 0:e85788b14028 | 640 | |
| aktk | 0:e85788b14028 | 641 | for(int i = 0; i < _Sample_Num; i++) { |
| aktk | 3:b56e933bebc2 | 642 | fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 643 | fread(&tmp, sizeof(char),1,fp); |
| aktk | 0:e85788b14028 | 644 | #ifdef DEBUG3 |
| aktk | 3:b56e933bebc2 | 645 | g_Serial_Signal.printf("%d%c", _Sample_Set[i].x, tmp); |
| aktk | 0:e85788b14028 | 646 | #endif |
| aktk | 0:e85788b14028 | 647 | |
| aktk | 3:b56e933bebc2 | 648 | fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp); |
| aktk | 0:e85788b14028 | 649 | fread(&tmp, sizeof(char),1,fp); |
| aktk | 0:e85788b14028 | 650 | #ifdef DEBUG3 |
| aktk | 3:b56e933bebc2 | 651 | g_Serial_Signal.printf("%d%c", _Sample_Set[i].y, tmp); |
| aktk | 0:e85788b14028 | 652 | #endif |
| aktk | 0:e85788b14028 | 653 | } |
| aktk | 0:e85788b14028 | 654 | fclose(fp); |
| aktk | 0:e85788b14028 | 655 | free(filepath); |
| aktk | 0:e85788b14028 | 656 | } |
| aktk | 0:e85788b14028 | 657 | |
| aktk | 0:e85788b14028 | 658 | void TRP105FS::printOutData() |
| aktk | 0:e85788b14028 | 659 | { |
| aktk | 0:e85788b14028 | 660 | FILE *fp; |
| aktk | 0:e85788b14028 | 661 | |
| aktk | 0:e85788b14028 | 662 | fp = fopen("/local/log.txt", "w"); // open file in writing mode |
| aktk | 3:b56e933bebc2 | 663 | fprintf(fp, "x, y,(threshold)\n"); |
| aktk | 0:e85788b14028 | 664 | for(int i = 0; i < _ENUM; i++) { |
| aktk | 3:b56e933bebc2 | 665 | fprintf(fp, "%d,%d,(%d)\n", _Set[i].x, _Set[i].y, _Threshold[i]); |
| aktk | 0:e85788b14028 | 666 | } |
| aktk | 3:b56e933bebc2 | 667 | fprintf(fp, "\nSample:x, y\n"); |
| aktk | 0:e85788b14028 | 668 | for(int i = 0; i < _Sample_Num; i++) { |
| aktk | 3:b56e933bebc2 | 669 | fprintf(fp, "%d,%d\n", _Sample_Set[i].x, _Sample_Set[i].y); |
| aktk | 0:e85788b14028 | 670 | } |
| aktk | 0:e85788b14028 | 671 | fclose(fp); |
| aktk | 0:e85788b14028 | 672 | |
| aktk | 0:e85788b14028 | 673 | } |
| aktk | 0:e85788b14028 | 674 | void TRP105FS::_printOutData(unsigned short *arg, int num, char* name) |
| aktk | 0:e85788b14028 | 675 | { |
| aktk | 0:e85788b14028 | 676 | FILE *fp; |
| aktk | 0:e85788b14028 | 677 | fp = fopen("/local/varlog.txt", "a"); // open file in add mode |
| aktk | 0:e85788b14028 | 678 | fprintf(fp, "%10s\n", name); |
| aktk | 0:e85788b14028 | 679 | for(int i = 0; i < num; i++) { |
| aktk | 0:e85788b14028 | 680 | fprintf(fp, "%d, ", arg[i]); |
| aktk | 0:e85788b14028 | 681 | } |
| aktk | 0:e85788b14028 | 682 | fprintf(fp, "\n"); |
| aktk | 0:e85788b14028 | 683 | fclose(fp); |
| aktk | 0:e85788b14028 | 684 | } |
| aktk | 0:e85788b14028 | 685 | void TRP105FS::_printOutData(double *arg, int num, char* name) |
| aktk | 0:e85788b14028 | 686 | { |
| aktk | 0:e85788b14028 | 687 | FILE *fp; |
| aktk | 0:e85788b14028 | 688 | |
| aktk | 0:e85788b14028 | 689 | fp = fopen("/local/varlog.txt", "a"); // open file in add mode |
| aktk | 0:e85788b14028 | 690 | fprintf(fp, "%10s\n", name); |
| aktk | 0:e85788b14028 | 691 | for(int i = 0; i < num; i++) { |
| aktk | 0:e85788b14028 | 692 | fprintf(fp, "%.2f, ", arg[i]); |
| aktk | 0:e85788b14028 | 693 | } |
| aktk | 0:e85788b14028 | 694 | fprintf(fp, "\n"); |
| aktk | 0:e85788b14028 | 695 | fclose(fp); |
| aktk | 0:e85788b14028 | 696 | } |
| aktk | 0:e85788b14028 | 697 | void TRP105FS::_printOutData(VDset *arg, int num, char* name) |
| aktk | 0:e85788b14028 | 698 | { |
| aktk | 0:e85788b14028 | 699 | FILE *fp; |
| aktk | 0:e85788b14028 | 700 | |
| aktk | 0:e85788b14028 | 701 | fp = fopen("/local/varlog.txt", "a"); // open file in add mode |
| aktk | 0:e85788b14028 | 702 | fprintf(fp, "%10s\n", name); |
| aktk | 0:e85788b14028 | 703 | for(int i = 0; i < num; i++) { |
| aktk | 3:b56e933bebc2 | 704 | fprintf(fp, "%d, ", arg[i].y); |
| aktk | 0:e85788b14028 | 705 | } |
| aktk | 0:e85788b14028 | 706 | fprintf(fp, "\n"); |
| aktk | 0:e85788b14028 | 707 | fclose(fp); |
| aktk | 0:e85788b14028 | 708 | } |