Akifumi Takahashi / CubicSpline

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of TRP105F_Spline by Akifumi Takahashi

CubicSpline.c@4:8db89b731133, 2016-05-20 (annotated)

Committer:
aktk
Date:
Fri May 20 14:25:39 2016 +0000
Revision:
4:8db89b731133
Parent:
3:75f50dbedf1b
Child:
6:c4f36cee3ceb
_makeSpline() might have been refactored.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aktk 3:75f50dbedf1b 1 #define DEBUG
aktk 3:75f50dbedf1b 2 #include "TRP105F_Spline.h"
aktk 4:8db89b731133 3 #include <cmath>
aktk 3:75f50dbedf1b 4
aktk 3:75f50dbedf1b 5 // To get voltage of TRP105F
aktk 3:75f50dbedf1b 6 AnalogIn g_Sensor_Voltage(p16);
aktk 3:75f50dbedf1b 7 // To get sample distance via seral com
aktk 3:75f50dbedf1b 8 Serial g_Serial_Signal(USBTX, USBRX);
aktk 3:75f50dbedf1b 9
aktk 3:75f50dbedf1b 10 LocalFileSystem local("local"); // マウントポイントを定義（ディレクトリパスになる）
aktk 3:75f50dbedf1b 11 // for debug
aktk 3:75f50dbedf1b 12 #ifdef DEBUG
aktk 3:75f50dbedf1b 13 DigitalOut led1(LED1);
aktk 3:75f50dbedf1b 14 DigitalOut led2(LED2);
aktk 3:75f50dbedf1b 15 DigitalOut led3(LED3);
aktk 3:75f50dbedf1b 16 DigitalOut led4(LED4);
aktk 3:75f50dbedf1b 17 #endif
aktk 3:75f50dbedf1b 18
aktk 4:8db89b731133 19 CubicSpline2d::CubicSpline2d()
aktk 3:75f50dbedf1b 20 :_Data_Input_Type(SYSTEM)
aktk 3:75f50dbedf1b 21 {
aktk 3:75f50dbedf1b 22 _Sample_Num = 5;
aktk 4:8db89b731133 23 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
aktk 4:8db89b731133 24 //_u_param = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:8db89b731133 25 for(int i = 0; i < _4; i++) {
aktk 4:8db89b731133 26 _C_x[i] = (double*)malloc((_Sample_Num - 1)* sizeof(double));
aktk 4:8db89b731133 27 _C_y[i] = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 4:8db89b731133 28 }
aktk 3:75f50dbedf1b 29 //calibrateSensor();
aktk 3:75f50dbedf1b 30 }
aktk 3:75f50dbedf1b 31
aktk 4:8db89b731133 32 CubicSpline2d::CubicSpline2d(
aktk 3:75f50dbedf1b 33 unsigned int arg_num
aktk 3:75f50dbedf1b 34 )
aktk 3:75f50dbedf1b 35 :_Data_Input_Type(SYSTEM)
aktk 3:75f50dbedf1b 36 {
aktk 3:75f50dbedf1b 37 _Sample_Num = arg_num;
aktk 4:8db89b731133 38 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
aktk 4:8db89b731133 39 //_u_param = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:8db89b731133 40 for(int i = 0; i < 4; i++) {
aktk 4:8db89b731133 41 _C_x[i] = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 4:8db89b731133 42 _C_y[i] = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 4:8db89b731133 43 }
aktk 3:75f50dbedf1b 44 //calibrateSensor();
aktk 3:75f50dbedf1b 45 }
aktk 3:75f50dbedf1b 46
aktk 4:8db89b731133 47 CubicSpline2d::CubicSpline2d(
aktk 3:75f50dbedf1b 48 unsigned int arg_num,
aktk 3:75f50dbedf1b 49 UseType arg_useType
aktk 3:75f50dbedf1b 50 )
aktk 3:75f50dbedf1b 51 :_useType(arg_useType)
aktk 3:75f50dbedf1b 52 {
aktk 3:75f50dbedf1b 53 _Sample_Num = arg_num;
aktk 4:8db89b731133 54 _Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
aktk 4:8db89b731133 55 //_u_param = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:8db89b731133 56 for(int i = 0; i < 4; i++) {
aktk 4:8db89b731133 57 _C_x[i] = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 4:8db89b731133 58 _C_y[i] = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 4:8db89b731133 59 }
aktk 3:75f50dbedf1b 60 //calibrateSensor();
aktk 3:75f50dbedf1b 61 }
aktk 3:75f50dbedf1b 62
aktk 4:8db89b731133 63 CubicSpline2d::~CubicSpline2d()
aktk 3:75f50dbedf1b 64 {
aktk 3:75f50dbedf1b 65 free(_Sample_Set);
aktk 4:8db89b731133 66 //free(_u_param);
aktk 4:8db89b731133 67 for(int i = 0; i < 4; i++) {
aktk 4:8db89b731133 68 free(_C_x[i]);
aktk 4:8db89b731133 69 free(_C_y[i]);
aktk 4:8db89b731133 70 }
aktk 3:75f50dbedf1b 71 }
aktk 3:75f50dbedf1b 72
aktk 4:8db89b731133 73 unsigned short CubicSpline2d::getDistance()
aktk 3:75f50dbedf1b 74 {
aktk 3:75f50dbedf1b 75 int idx;
aktk 3:75f50dbedf1b 76 unsigned short pv = 0;
aktk 3:75f50dbedf1b 77
aktk 3:75f50dbedf1b 78 // low-pass filter
aktk 3:75f50dbedf1b 79 for(int i = 0; i < 10; i++)
aktk 3:75f50dbedf1b 80 pv += g_Sensor_Voltage.read_u16() / 10;
aktk 3:75f50dbedf1b 81
aktk 3:75f50dbedf1b 82 idx = _getNearest(_LIDX, _RIDX, pv);
aktk 3:75f50dbedf1b 83
aktk 3:75f50dbedf1b 84 if (idx != 0xFFFF) // unless occuring error
aktk 3:75f50dbedf1b 85 return _Set[idx].dst;
aktk 3:75f50dbedf1b 86 else
aktk 3:75f50dbedf1b 87 return 0xFFFF;
aktk 3:75f50dbedf1b 88 }
aktk 3:75f50dbedf1b 89
aktk 3:75f50dbedf1b 90
aktk 4:8db89b731133 91 void CubicSpline2d::_sampleData()
aktk 3:75f50dbedf1b 92 {
aktk 3:75f50dbedf1b 93 int tmp;
aktk 3:75f50dbedf1b 94 char sig;
aktk 4:8db89b731133 95 Vxyt tmp_set;
aktk 3:75f50dbedf1b 96
aktk 3:75f50dbedf1b 97 // For evry set,
aktk 3:75f50dbedf1b 98 // 1, get dst data via serai com,
aktk 3:75f50dbedf1b 99 // 2, get vol data,
aktk 3:75f50dbedf1b 100 // and then do same for next index set.
aktk 3:75f50dbedf1b 101 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:8db89b731133 102 if(_useType == AsDebug) {
aktk 4:8db89b731133 103 //
aktk 4:8db89b731133 104 // Recieve a Distance datus and store it into member
aktk 4:8db89b731133 105 //
aktk 4:8db89b731133 106 g_Serial_Signal.printf("X:");
aktk 4:8db89b731133 107 _Sample_Set[i].x = 0;
aktk 3:75f50dbedf1b 108 do {
aktk 3:75f50dbedf1b 109 sig = g_Serial_Signal.getc();
aktk 3:75f50dbedf1b 110 if('0' <= sig && sig <= '9') {
aktk 4:8db89b731133 111 _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
aktk 3:75f50dbedf1b 112 g_Serial_Signal.putc(char(sig));
aktk 3:75f50dbedf1b 113 } else if(sig == 0x08) {
aktk 4:8db89b731133 114 _Sample_Set[i].x = 0;
aktk 3:75f50dbedf1b 115 g_Serial_Signal.printf("[canseled!]");
aktk 3:75f50dbedf1b 116 g_Serial_Signal.putc('\n');
aktk 3:75f50dbedf1b 117 g_Serial_Signal.putc('>');
aktk 3:75f50dbedf1b 118 }
aktk 3:75f50dbedf1b 119 } while (!(sig == 0x0a || sig == 0x0d));
aktk 3:75f50dbedf1b 120 g_Serial_Signal.putc('\n');
aktk 4:8db89b731133 121 //
aktk 4:8db89b731133 122 // Recieve a Voltage datus and store it into member
aktk 4:8db89b731133 123 //
aktk 4:8db89b731133 124 // LOW PASS FILTERED
aktk 4:8db89b731133 125 // Get 10 data and store mean as a sample.
aktk 4:8db89b731133 126 // After get one original sample, system waits for 0.1 sec,
aktk 4:8db89b731133 127 // thus it takes 1 sec evry sampling.
aktk 4:8db89b731133 128 _Sample_Set[i].y = 0;
aktk 4:8db89b731133 129 for(int j = 0; j < 10; j++) {
aktk 4:8db89b731133 130 tmp_set.y = g_Sensor_Voltage.read();
aktk 4:8db89b731133 131 #ifdef DEBUG
aktk 4:8db89b731133 132 g_Serial_Signal.printf("%d,",tmp_set.y);
aktk 4:8db89b731133 133 #endif
aktk 4:8db89b731133 134 _Sample_Set[i].y += (tmp_set.y / 10);
aktk 4:8db89b731133 135 wait(0.1);
aktk 4:8db89b731133 136 }
aktk 4:8db89b731133 137 #ifdef DEBUG
aktk 4:8db89b731133 138 g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y);
aktk 4:8db89b731133 139 #endif
aktk 3:75f50dbedf1b 140 }
aktk 3:75f50dbedf1b 141
aktk 4:8db89b731133 142 // if the input data is over the bound, it is calibrated
aktk 4:8db89b731133 143 if (_Sample_Set[i].x < 0)
aktk 4:8db89b731133 144 _Sample_Set[i].x = 0;
aktk 3:75f50dbedf1b 145 }
aktk 3:75f50dbedf1b 146 //
aktk 4:8db89b731133 147 // Sort set data array in x-Ascending order
aktk 3:75f50dbedf1b 148 //
aktk 3:75f50dbedf1b 149 tmp = 0;
aktk 4:8db89b731133 150 for( int i = 0 ; i < _Sumple_Num; i++) {
aktk 4:8db89b731133 151 for(int j = _Sample_Num - 1; j < i+1 ; j++) {
aktk 4:8db89b731133 152 // use dst as index for dst range [2,20]
aktk 4:8db89b731133 153 if (_Sample_Set[i].x > _Sample_set[j].x) {
aktk 4:8db89b731133 154 tmp_set.x = _Sample_Set[i].x;
aktk 4:8db89b731133 155 tmp_set.y = _Sample_Set[i].y;
aktk 4:8db89b731133 156 _Sample_Set[i].x = _Sample_Set[j].x;
aktk 4:8db89b731133 157 _Sample_Set[i].y = _Sample_Set[j].y;
aktk 4:8db89b731133 158 _Sample_Set[j].x = tmp_set.x;
aktk 4:8db89b731133 159 _Sample_Set[j].y = tmp_set.y;
aktk 4:8db89b731133 160 }
aktk 4:8db89b731133 161 // if a same dst has been input, calcurate mean.
aktk 4:8db89b731133 162 else if (_Sample_Set[i].x == _Sample_set[j]) {
aktk 4:8db89b731133 163 tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2;
aktk 4:8db89b731133 164 _Sample_Set[i] = _Sample_Set[j] = tmp_set.y;
aktk 4:8db89b731133 165 tmp++;
aktk 4:8db89b731133 166 }
aktk 3:75f50dbedf1b 167 }
aktk 3:75f50dbedf1b 168 }
aktk 3:75f50dbedf1b 169 #ifdef DEBUG
aktk 4:8db89b731133 170 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 4:8db89b731133 171 g_Serial_Signal.printf("-) tmp: %d\n", tmp );
aktk 3:75f50dbedf1b 172 #endif
aktk 3:75f50dbedf1b 173 // substruct tmp from number of sample.
aktk 3:75f50dbedf1b 174 _Sample_Num -= tmp;
aktk 3:75f50dbedf1b 175 #ifdef DEBUG
aktk 4:8db89b731133 176 g_Serial_Signal.printf("-----------------\n");
aktk 4:8db89b731133 177 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 3:75f50dbedf1b 178 #endif
aktk 4:8db89b731133 179
aktk 4:8db89b731133 180 // generate t which is parameter related to x,y
aktk 4:8db89b731133 181 _Sample_Set[i].t = 0;
aktk 4:8db89b731133 182 for(int i = 1; i < _Sample_Num; i++)
aktk 4:8db89b731133 183 _Sample_Set[i].t =
aktk 4:8db89b731133 184 _Sample_Set[i-1]
aktk 4:8db89b731133 185 + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2)
aktk 4:8db89b731133 186 +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2));
aktk 3:75f50dbedf1b 187 }
aktk 3:75f50dbedf1b 188
aktk 3:75f50dbedf1b 189 //
aktk 3:75f50dbedf1b 190 // Function to define _u_spline, specific constants of spline.
aktk 3:75f50dbedf1b 191 //
aktk 4:8db89b731133 192 void CubicSpline2d::_makeSpline(double* arg_t, double* arg_ft, double* arg_C[4], unsigned int arg_num)
aktk 3:75f50dbedf1b 193 {
aktk 4:8db89b731133 194 // arg_t : t; parameter of x or y
aktk 4:8db89b731133 195 // arg_ft: f(t); cubic poliminal. Value:<=> x or y.
aktk 4:8db89b731133 196 // arg_C[i]: Ci; The parameter (set) that defines Spline Model Poliminal. Coefficient of t^i of f(t).
aktk 4:8db89b731133 197 // arg_num: j in [0,_Sample_Num-1]; The number of interval.
aktk 4:8db89b731133 198 // f(t)j = C3j*t^3 + C2j*t^2 + C1j*t + C0j
aktk 3:75f50dbedf1b 199 //
aktk 3:75f50dbedf1b 200 // N: max of index <=> (_Sample_Num - 1)
aktk 3:75f50dbedf1b 201 //
aktk 4:8db89b731133 202 // u[i] === d^2/dx^2(Spline f)[i]
aktk 3:75f50dbedf1b 203 // i:[0,N]
aktk 4:8db89b731133 204 // u[0] = u[N] = 0
aktk 4:8db89b731133 205 double *u = (double*)malloc((arg_num ) * sizeof(double));
aktk 3:75f50dbedf1b 206 //
aktk 3:75f50dbedf1b 207 // h[i] = x[i+1] - x[i]
aktk 3:75f50dbedf1b 208 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
aktk 4:8db89b731133 209 double *h = (double*)malloc((arg_num - 1) * sizeof(double));
aktk 4:8db89b731133 210 //
aktk 3:75f50dbedf1b 211 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
aktk 3:75f50dbedf1b 212 // i:[0,N-2]
aktk 4:8db89b731133 213 double *v = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 4:8db89b731133 214 //
aktk 3:75f50dbedf1b 215 // temporary array whose num of elm equals v array
aktk 4:8db89b731133 216 double *w = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 3:75f50dbedf1b 217 //
aktk 4:8db89b731133 218 // [ 2(h[0]+h[1]) , h[1] , O ] [u[1] ] [v[0] ]
aktk 4:8db89b731133 219 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [u[2] ] [v[1] ]
aktk 4:8db89b731133 220 // [ ... ] * [... ] = [... ]
aktk 4:8db89b731133 221 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [u[j+1]] [v[j] ]
aktk 4:8db89b731133 222 // [ ... ] [ ... ] [ ... ]
aktk 4:8db89b731133 223 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [u[j+1]] [v[j] ]
aktk 4:8db89b731133 224 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [u[N-1]] [v[N-2]]
aktk 3:75f50dbedf1b 225 //
aktk 3:75f50dbedf1b 226 // For LU decomposition
aktk 4:8db89b731133 227 double *Upper = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 4:8db89b731133 228 double *Lower = (double*)malloc((arg_num - 2) * sizeof(double));
aktk 3:75f50dbedf1b 229 #ifdef DEBUG
aktk 4:8db89b731133 230 _printOutData(arg_t, arg_ft, arg_num, "\nargment set\n");
aktk 3:75f50dbedf1b 231 #endif
aktk 4:8db89b731133 232 for(int i = 0; i < arg_num - 1; i++)
aktk 4:8db89b731133 233 h[i] = (double)(arg_t[i + 1] - arg_t[i]);
aktk 3:75f50dbedf1b 234
aktk 4:8db89b731133 235 for(int i = 0; i < arg_num - 2; i++)
aktk 3:75f50dbedf1b 236 v[i] = 6 * (
aktk 4:8db89b731133 237 ((double)(arg_ft[i + 2] - arg_ft[i + 1])) / h[i + 1]
aktk 3:75f50dbedf1b 238 -
aktk 4:8db89b731133 239 ((double)(arg_ft[i + 1] - arg_ft[i])) / h[i]
aktk 3:75f50dbedf1b 240 );
aktk 3:75f50dbedf1b 241
aktk 3:75f50dbedf1b 242 //
aktk 3:75f50dbedf1b 243 // LU decomposition
aktk 3:75f50dbedf1b 244 //
aktk 3:75f50dbedf1b 245 Upper[0] = 2 * (h[0] + h[1]);
aktk 3:75f50dbedf1b 246 Lower[0] = 0;
aktk 4:8db89b731133 247 for (int i = 1; i < arg_num - 2; i++) {
aktk 3:75f50dbedf1b 248 Lower[i] = h[i] / Upper[i - 1];
aktk 3:75f50dbedf1b 249 Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
aktk 3:75f50dbedf1b 250 }
aktk 3:75f50dbedf1b 251
aktk 3:75f50dbedf1b 252
aktk 3:75f50dbedf1b 253 //
aktk 3:75f50dbedf1b 254 // forward substitution
aktk 3:75f50dbedf1b 255 //
aktk 3:75f50dbedf1b 256 w[0] = v[0];
aktk 4:8db89b731133 257 for (int i = 1; i < arg_num - 2; i ++) {
aktk 3:75f50dbedf1b 258 w[i] = v[i] - Lower[i] * w[i-1];
aktk 3:75f50dbedf1b 259 }
aktk 3:75f50dbedf1b 260
aktk 3:75f50dbedf1b 261 //
aktk 3:75f50dbedf1b 262 // backward substitution
aktk 3:75f50dbedf1b 263 //
aktk 4:8db89b731133 264 u[arg_num - 2] = w[arg_num - 3] / Upper[arg_num - 3];
aktk 4:8db89b731133 265 for(int i = arg_num - 3; i > 0; i--) {
aktk 4:8db89b731133 266 u[i] = (w[(i - 1)] - h[(i)] * u[(i) + 1]) / Upper[(i - 1)];
aktk 3:75f50dbedf1b 267 }
aktk 3:75f50dbedf1b 268
aktk 3:75f50dbedf1b 269 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 4:8db89b731133 270 u[0] = u[arg_num - 1] = 0.0;
aktk 3:75f50dbedf1b 271
aktk 3:75f50dbedf1b 272 #ifdef DEBUG
aktk 4:8db89b731133 273 _printOutData(h, arg_num - 1, "h");
aktk 4:8db89b731133 274 _printOutData(v, arg_num - 2, "v");
aktk 4:8db89b731133 275 _printOutData(w, arg_num - 2, "w");
aktk 4:8db89b731133 276 _printOutData(Upper, arg_num - 2, "Upper");
aktk 4:8db89b731133 277 _printOutData(Lower, arg_num - 2, "Lower");
aktk 4:8db89b731133 278 _printOutData(u, arg_num , "u");
aktk 3:75f50dbedf1b 279 #endif
aktk 4:8db89b731133 280
aktk 4:8db89b731133 281 for(int itv = 0; itv < arg_num - 1; itv++) {
aktk 4:8db89b731133 282 C[3][itv] = (u[itv + 1] - u[itv]) / 6.0 / (arg_t[itv + 1] - arg_t[itv]);
aktk 4:8db89b731133 283 C[2][itv] = (u[itv]) / 2.0;
aktk 4:8db89b731133 284 C[1][itv] = (arg_ft[itv + 1] - arg_ft[itv]) / (arg_t[itv + 1] - arg_t[itv])
aktk 4:8db89b731133 285 -
aktk 4:8db89b731133 286 (arg_t[itv + 1] - arg_t[itv]) * (u[itv + 1] + 2.0 * u[itv]) / 6.0;
aktk 4:8db89b731133 287 C[0][itv] = (arg_ft[itv]);
aktk 4:8db89b731133 288 }
aktk 3:75f50dbedf1b 289 free(h);
aktk 4:8db89b731133 290 free(u);
aktk 3:75f50dbedf1b 291 free(v);
aktk 3:75f50dbedf1b 292 free(w);
aktk 3:75f50dbedf1b 293 free(Upper);
aktk 3:75f50dbedf1b 294 free(Lower);
aktk 3:75f50dbedf1b 295 }
aktk 4:8db89b731133 296
aktk 3:75f50dbedf1b 297 //
aktk 3:75f50dbedf1b 298 // Function to return Voltage for distance.
aktk 3:75f50dbedf1b 299 //
aktk 4:8db89b731133 300 unsigned short CubicSpline2d:: _getSplineYof(
aktk 3:75f50dbedf1b 301 double arg_x // the argument is supposed as distance [mm]
aktk 3:75f50dbedf1b 302 )
aktk 3:75f50dbedf1b 303 {
aktk 3:75f50dbedf1b 304 double y; // voltage calculated by spline polynomial
aktk 3:75f50dbedf1b 305 double a,b,c,d; // which is specific constant of spline, and can be expressed with _u.
aktk 3:75f50dbedf1b 306 int itv = 0; // interval(section) of interpolation
aktk 3:75f50dbedf1b 307 // the number of interval is less 1 than the number of sample sets,
aktk 3:75f50dbedf1b 308 // which means the max number of interval is _Sample_num - 2.
aktk 3:75f50dbedf1b 309 if((double)(_Sample_Set[0].dst) <= arg_x) {
aktk 3:75f50dbedf1b 310 while (!((double)(_Sample_Set[itv].dst) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].dst))) {
aktk 3:75f50dbedf1b 311 itv++;
aktk 3:75f50dbedf1b 312 if(itv > _Sample_Num - 2) {
aktk 3:75f50dbedf1b 313 itv = _Sample_Num - 2;
aktk 3:75f50dbedf1b 314 break;
aktk 3:75f50dbedf1b 315 }
aktk 3:75f50dbedf1b 316 }
aktk 3:75f50dbedf1b 317 }
aktk 3:75f50dbedf1b 318 a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst);
aktk 3:75f50dbedf1b 319 b = (double)(_u_spline[itv]) / 2.0;
aktk 3:75f50dbedf1b 320 c = (double)(_Sample_Set[itv + 1].vol - _Sample_Set[itv].vol) / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst)
aktk 3:75f50dbedf1b 321 -
aktk 3:75f50dbedf1b 322 (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0;
aktk 3:75f50dbedf1b 323 d = (double)(_Sample_Set[itv].vol);
aktk 3:75f50dbedf1b 324 // cubic spline expression
aktk 3:75f50dbedf1b 325 y = a * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
aktk 3:75f50dbedf1b 326 +
aktk 3:75f50dbedf1b 327 b * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
aktk 3:75f50dbedf1b 328 +
aktk 3:75f50dbedf1b 329 c * (arg_x - (double)(_Sample_Set[itv].dst))
aktk 3:75f50dbedf1b 330 +
aktk 3:75f50dbedf1b 331 d;
aktk 3:75f50dbedf1b 332
aktk 3:75f50dbedf1b 333 #ifdef DEBUG2
aktk 3:75f50dbedf1b 334 g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
aktk 3:75f50dbedf1b 335 g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
aktk 3:75f50dbedf1b 336 g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
aktk 3:75f50dbedf1b 337 #endif
aktk 3:75f50dbedf1b 338
aktk 3:75f50dbedf1b 339 return ((unsigned short)(int)y);
aktk 3:75f50dbedf1b 340 }
aktk 3:75f50dbedf1b 341
aktk 4:8db89b731133 342 void CubicSpline2d::calibrateSensor()
aktk 3:75f50dbedf1b 343 {
aktk 3:75f50dbedf1b 344 _sampleData();
aktk 3:75f50dbedf1b 345 _makeSpline();
aktk 3:75f50dbedf1b 346
aktk 3:75f50dbedf1b 347 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 348 _Set[i].dst = i;
aktk 3:75f50dbedf1b 349 _Set[i].vol = _getSplineYof((double)(_Set[i].dst));
aktk 3:75f50dbedf1b 350 _Threshold[i] = _getSplineYof((double)(_Set[i].dst) + 0.5);
aktk 3:75f50dbedf1b 351 #ifdef DEBUG2
aktk 3:75f50dbedf1b 352 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 3:75f50dbedf1b 353 #endif
aktk 3:75f50dbedf1b 354 }
aktk 3:75f50dbedf1b 355 }
aktk 3:75f50dbedf1b 356
aktk 4:8db89b731133 357 void CubicSpline2d::saveSetting()
aktk 3:75f50dbedf1b 358 {
aktk 3:75f50dbedf1b 359 FILE *fp;
aktk 3:75f50dbedf1b 360
aktk 3:75f50dbedf1b 361 fp = fopen("/local/savedata.log", "wb");
aktk 3:75f50dbedf1b 362
aktk 3:75f50dbedf1b 363 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 364 fwrite(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 365 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 366 fwrite(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 367 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 368 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 369 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 370 }
aktk 3:75f50dbedf1b 371 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 3:75f50dbedf1b 372 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 373 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 374 fwrite(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 375 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 376 fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 377 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 378 }
aktk 3:75f50dbedf1b 379 fclose(fp);
aktk 3:75f50dbedf1b 380
aktk 3:75f50dbedf1b 381 }
aktk 3:75f50dbedf1b 382
aktk 4:8db89b731133 383 void CubicSpline2d::printThresholds()
aktk 3:75f50dbedf1b 384 {
aktk 3:75f50dbedf1b 385 for(int i = 0; i < _ENUM; i++)
aktk 3:75f50dbedf1b 386 g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
aktk 3:75f50dbedf1b 387 }
aktk 4:8db89b731133 388 void CubicSpline2d::loadSetting()
aktk 3:75f50dbedf1b 389 {
aktk 3:75f50dbedf1b 390 FILE *fp;
aktk 3:75f50dbedf1b 391 char tmp;
aktk 3:75f50dbedf1b 392
aktk 3:75f50dbedf1b 393 //sprintf(filepath, "/local/%s", filename);
aktk 3:75f50dbedf1b 394 //fp = fopen(filepath, "rb");
aktk 3:75f50dbedf1b 395 fp = fopen("/local/savedata.log", "rb");
aktk 3:75f50dbedf1b 396 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 397
aktk 3:75f50dbedf1b 398 fread(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 399 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 400 #ifdef DEBUG2
aktk 3:75f50dbedf1b 401 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp);
aktk 3:75f50dbedf1b 402 #endif
aktk 3:75f50dbedf1b 403
aktk 3:75f50dbedf1b 404 fread(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 405 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 406 #ifdef DEBUG2
aktk 3:75f50dbedf1b 407 g_Serial_Signal.printf("%d%c", _Set[i].vol, tmp);
aktk 3:75f50dbedf1b 408 #endif
aktk 3:75f50dbedf1b 409
aktk 3:75f50dbedf1b 410 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 411 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 412 #ifdef DEBUG2
aktk 3:75f50dbedf1b 413 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 3:75f50dbedf1b 414 #endif
aktk 3:75f50dbedf1b 415 }
aktk 3:75f50dbedf1b 416
aktk 3:75f50dbedf1b 417 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 418 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 419
aktk 3:75f50dbedf1b 420 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 421 fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 422 fread(&tmp, sizeof(char),1,fp);
aktk 3:75f50dbedf1b 423 fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 424 fread(&tmp, sizeof(char),1,fp);
aktk 3:75f50dbedf1b 425 }
aktk 3:75f50dbedf1b 426 fclose(fp);
aktk 3:75f50dbedf1b 427 }
aktk 3:75f50dbedf1b 428
aktk 3:75f50dbedf1b 429
aktk 4:8db89b731133 430 void CubicSpline2d::saveSetting(
aktk 3:75f50dbedf1b 431 const char *filename
aktk 3:75f50dbedf1b 432 )
aktk 3:75f50dbedf1b 433 {
aktk 3:75f50dbedf1b 434 FILE *fp;
aktk 3:75f50dbedf1b 435 char *filepath;
aktk 3:75f50dbedf1b 436 int fnnum = 0;
aktk 3:75f50dbedf1b 437
aktk 3:75f50dbedf1b 438 while (filename[fnnum] != 0) fnnum++;
aktk 3:75f50dbedf1b 439 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 3:75f50dbedf1b 440
aktk 3:75f50dbedf1b 441 sprintf(filepath, "/local/%s", filename);
aktk 3:75f50dbedf1b 442 fp = fopen(filepath, "wb");
aktk 3:75f50dbedf1b 443
aktk 3:75f50dbedf1b 444 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 445 fwrite(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 446 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 447 fwrite(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 448 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 449 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 450 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 451 }
aktk 3:75f50dbedf1b 452 fwrite(&_Sample_Num, sizeof(int), 1, fp);
aktk 3:75f50dbedf1b 453 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 454 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 455 fwrite(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 456 fputc(0x2c, fp);
aktk 3:75f50dbedf1b 457 fwrite(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 458 fputc(0x3b, fp);
aktk 3:75f50dbedf1b 459 }
aktk 3:75f50dbedf1b 460 fclose(fp);
aktk 3:75f50dbedf1b 461 free(filepath);
aktk 3:75f50dbedf1b 462 }
aktk 3:75f50dbedf1b 463
aktk 4:8db89b731133 464 void CubicSpline2d::loadSetting(
aktk 3:75f50dbedf1b 465 const char *filename
aktk 3:75f50dbedf1b 466 )
aktk 3:75f50dbedf1b 467 {
aktk 3:75f50dbedf1b 468 FILE *fp;
aktk 3:75f50dbedf1b 469 char *filepath;
aktk 3:75f50dbedf1b 470 char tmp;
aktk 3:75f50dbedf1b 471 int fnnum = 0;
aktk 3:75f50dbedf1b 472
aktk 3:75f50dbedf1b 473 while (filename[fnnum] != 0) fnnum++;
aktk 3:75f50dbedf1b 474 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 3:75f50dbedf1b 475
aktk 3:75f50dbedf1b 476 sprintf(filepath, "/local/%s", filename);
aktk 3:75f50dbedf1b 477 fp = fopen(filepath, "rb");
aktk 3:75f50dbedf1b 478 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 479
aktk 3:75f50dbedf1b 480 fread(&_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 481 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 482 #ifdef DEBUG3
aktk 3:75f50dbedf1b 483 g_Serial_Signal.printf("%d%c", _Set[i].dst, tmp);
aktk 3:75f50dbedf1b 484 #endif
aktk 3:75f50dbedf1b 485
aktk 3:75f50dbedf1b 486 fread(&_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 487 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 488 #ifdef DEBUG3
aktk 3:75f50dbedf1b 489 g_Serial_Signal.printf("%d%c", _Set[i].vol, tmp);
aktk 3:75f50dbedf1b 490 #endif
aktk 3:75f50dbedf1b 491
aktk 3:75f50dbedf1b 492 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 493 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 494 #ifdef DEBUG3
aktk 3:75f50dbedf1b 495 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 3:75f50dbedf1b 496 #endif
aktk 3:75f50dbedf1b 497 }
aktk 3:75f50dbedf1b 498
aktk 3:75f50dbedf1b 499 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 500 fread(&tmp, sizeof(char), 1, fp);
aktk 3:75f50dbedf1b 501 #ifdef DEBUG3
aktk 3:75f50dbedf1b 502 g_Serial_Signal.printf("%d%c\n",_Sample_Num, tmp);
aktk 3:75f50dbedf1b 503 #endif
aktk 3:75f50dbedf1b 504
aktk 3:75f50dbedf1b 505 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 506 fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 507 fread(&tmp, sizeof(char),1,fp);
aktk 3:75f50dbedf1b 508 #ifdef DEBUG3
aktk 3:75f50dbedf1b 509 g_Serial_Signal.printf("%d%c", _Sample_Set[i].dst, tmp);
aktk 3:75f50dbedf1b 510 #endif
aktk 3:75f50dbedf1b 511
aktk 3:75f50dbedf1b 512 fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
aktk 3:75f50dbedf1b 513 fread(&tmp, sizeof(char),1,fp);
aktk 3:75f50dbedf1b 514 #ifdef DEBUG3
aktk 3:75f50dbedf1b 515 g_Serial_Signal.printf("%d%c", _Sample_Set[i].vol, tmp);
aktk 3:75f50dbedf1b 516 #endif
aktk 3:75f50dbedf1b 517 }
aktk 3:75f50dbedf1b 518 fclose(fp);
aktk 3:75f50dbedf1b 519 free(filepath);
aktk 3:75f50dbedf1b 520 }
aktk 3:75f50dbedf1b 521
aktk 4:8db89b731133 522 void CubicSpline2d::printOutData()
aktk 3:75f50dbedf1b 523 {
aktk 3:75f50dbedf1b 524 FILE *fp;
aktk 3:75f50dbedf1b 525
aktk 3:75f50dbedf1b 526 fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 3:75f50dbedf1b 527 fprintf(fp, "dst, vol,(threshold)\n");
aktk 3:75f50dbedf1b 528 for(int i = 0; i < _ENUM; i++) {
aktk 3:75f50dbedf1b 529 fprintf(fp, "%d,%d,(%d)\n", _Set[i].dst, _Set[i].vol, _Threshold[i]);
aktk 3:75f50dbedf1b 530 }
aktk 3:75f50dbedf1b 531 fprintf(fp, "\nSample:dst, vol\n");
aktk 3:75f50dbedf1b 532 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:75f50dbedf1b 533 fprintf(fp, "%d,%d\n", _Sample_Set[i].dst, _Sample_Set[i].vol);
aktk 3:75f50dbedf1b 534 }
aktk 3:75f50dbedf1b 535 fclose(fp);
aktk 3:75f50dbedf1b 536
aktk 3:75f50dbedf1b 537 }
aktk 4:8db89b731133 538 void CubicSpline2d::_printOutData(unsigned short *arg, int num, char* name)
aktk 3:75f50dbedf1b 539 {
aktk 3:75f50dbedf1b 540 FILE *fp;
aktk 3:75f50dbedf1b 541 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 3:75f50dbedf1b 542 fprintf(fp, "%10s\n", name);
aktk 3:75f50dbedf1b 543 for(int i = 0; i < num; i++) {
aktk 3:75f50dbedf1b 544 fprintf(fp, "%d, ", arg[i]);
aktk 3:75f50dbedf1b 545 }
aktk 3:75f50dbedf1b 546 fprintf(fp, "\n");
aktk 3:75f50dbedf1b 547 fclose(fp);
aktk 3:75f50dbedf1b 548 }
aktk 4:8db89b731133 549 void CubicSpline2d::_printOutData(double *arg, int num, char* name)
aktk 3:75f50dbedf1b 550 {
aktk 3:75f50dbedf1b 551 FILE *fp;
aktk 3:75f50dbedf1b 552
aktk 3:75f50dbedf1b 553 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 3:75f50dbedf1b 554 fprintf(fp, "%10s\n", name);
aktk 3:75f50dbedf1b 555 for(int i = 0; i < num; i++) {
aktk 3:75f50dbedf1b 556 fprintf(fp, "%.2f, ", arg[i]);
aktk 3:75f50dbedf1b 557 }
aktk 3:75f50dbedf1b 558 fprintf(fp, "\n");
aktk 3:75f50dbedf1b 559 fclose(fp);
aktk 3:75f50dbedf1b 560 }
aktk 4:8db89b731133 561 void CubicSpline2d::_printOutDataCouple(double *arg1, double *arg2, int num, char* name)
aktk 4:8db89b731133 562 {
aktk 4:8db89b731133 563 FILE *fp;
aktk 4:8db89b731133 564
aktk 4:8db89b731133 565 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 4:8db89b731133 566 fprintf(fp, "%10s\n", name);
aktk 4:8db89b731133 567 for(int i = 0; i < num; i++) {
aktk 4:8db89b731133 568 fprintf(fp, "(%.2f, %.2f)\n", arg1[i], arg2[i]);
aktk 4:8db89b731133 569 }
aktk 4:8db89b731133 570 fprintf(fp, "\n");
aktk 4:8db89b731133 571 fclose(fp);
aktk 4:8db89b731133 572 }
aktk 4:8db89b731133 573 void CubicSpline2d::_printOutData(Vxyt *arg, int num, char* name)
aktk 3:75f50dbedf1b 574 {
aktk 3:75f50dbedf1b 575 FILE *fp;
aktk 3:75f50dbedf1b 576
aktk 3:75f50dbedf1b 577 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 3:75f50dbedf1b 578 fprintf(fp, "%10s\n", name);
aktk 3:75f50dbedf1b 579 for(int i = 0; i < num; i++) {
aktk 3:75f50dbedf1b 580 fprintf(fp, "%d, ", arg[i].vol);
aktk 3:75f50dbedf1b 581 }
aktk 3:75f50dbedf1b 582 fprintf(fp, "\n");
aktk 3:75f50dbedf1b 583 fclose(fp);
aktk 3:75f50dbedf1b 584 }