1 #include <stdio.h>
2 #include <stdlib.h>
3 #include <cassert>
4 #include <math.h>
5 #include <sys/types.h>
6 #include <sys/stat.h>
7 #include <fcntl.h>
8 #include <sys/mman.h>
9 #include <unistd.h>
10 #include <kinsol/kinsol.h>
11 #include <kinsol/kinsol_dense.h>
12 #include <nvector/nvector_serial.h>
13 #include <sundials/sundials_types.h>
14 #include <sundials/sundials_math.h>
15 #include "circuit.hpp"
16
17 #define DOUBLE_UPDATE false
18
19 //#define FTOL RCONST(1.e-7) /* function tolerance */
20 //#define STOL RCONST(1.e-5) /* step tolerance */
21 #define FTOL RCONST(1.e-11) /* function tolerance */
22 #define STOL RCONST(1.e-9) /* step tolerance */
23
24 #define ZERO RCONST(0.0)
25 #define ONE RCONST(1.0)
26
print_vec(const char * t,N_Vector v)27 static void print_vec(const char *t, N_Vector v) {
28 printf("%s =", t);
29 for (int i = 0; i < NV_LENGTH_S(v); i++) {
30 printf(" %g", Ith(v, i));
31 }
32 printf("\n");
33 }
34
35 /*
36 * Check function return value...
37 * opt == 0 means SUNDIALS function allocates memory so check if
38 * returned NULL pointer
39 * opt == 1 means SUNDIALS function returns a flag so check if
40 * flag >= 0
41 * opt == 2 means function allocates memory so check if returned
42 * NULL pointer
43 */
44
check_flag(void * flagvalue,const char * funcname,int opt)45 static int check_flag(void *flagvalue, const char *funcname, int opt)
46 {
47 int *errflag;
48
49 /* Check if SUNDIALS function returned NULL pointer - no memory allocated */
50 if (opt == 0 && flagvalue == NULL) {
51 fprintf(stderr,
52 "\nSUNDIALS_ERROR: %s() failed - returned NULL pointer\n\n",
53 funcname);
54 return(1);
55 }
56
57 /* Check if flag < 0 */
58 else if (opt == 1) {
59 errflag = (int *) flagvalue;
60 if (*errflag < 0) {
61 fprintf(stderr,
62 "\nSUNDIALS_ERROR: %s() failed with flag = %d\n\n",
63 funcname, *errflag);
64 return(1);
65 }
66 }
67
68 /* Check if function returned NULL pointer - no memory allocated */
69 else if (opt == 2 && flagvalue == NULL) {
70 fprintf(stderr,
71 "\nMEMORY_ERROR: %s() failed - returned NULL pointer\n\n",
72 funcname);
73 return(1);
74 }
75
76 return(0);
77 }
78
79 /*
80 * Print final statistics contained in iopt
81 */
82
PrintFinalStats(void * kmem)83 static void PrintFinalStats(void *kmem)
84 {
85 long int nni, nfe, nje, nfeD;
86 int flag;
87
88 flag = KINGetNumNonlinSolvIters(kmem, &nni);
89 check_flag(&flag, "KINGetNumNonlinSolvIters", 1);
90 flag = KINGetNumFuncEvals(kmem, &nfe);
91 check_flag(&flag, "KINGetNumFuncEvals", 1);
92
93 flag = KINDlsGetNumJacEvals(kmem, &nje);
94 check_flag(&flag, "KINDlsGetNumJacEvals", 1);
95 flag = KINDlsGetNumFuncEvals(kmem, &nfeD);
96 check_flag(&flag, "KINDlsGetNumFuncEvals", 1);
97
98 printf("Final Statistics:\n");
99 printf(" nni = %5ld nfe = %5ld \n", nni, nfe);
100 printf(" nje = %5ld nfeD = %5ld \n", nje, nfeD);
101 }
102
103
104 /****************************************************************
105 * Spline
106 *
107 * http://www.thefullwiki.org/Spline_(mathematics)
108 */
109
Spline(double * y,int n,double xstart,double h)110 Spline::Spline(double *y, int n, double xstart, double h):
111 count(n), x0(xstart), step(h), c(new double[n]), f(y)
112 {
113 double *z = new double[n-1];
114 double *mu = new double[n-1];
115 double l = 1;
116 mu[0] = 0.0;
117 z[0] = 0.0;
118 for (int i = 1; i < n-1; i++) {
119 l = 4 - mu[i-1];
120 mu[i] = 1 / l;
121 z[i] = (3 * (y[i+1]-2*y[i]+y[i-1]) - z[i-1]) / l;
122 }
123 c[n-1] = 0.0;
124 for (int i = n-2; i >= 0; i--) {
125 c[i] = z[i] - mu[i] * c[i+1];
126 }
127 delete[] z;
128 delete[] mu;
129 }
130
~Spline()131 Spline::~Spline()
132 {
133 delete c;
134 }
135
eval(double x)136 double Spline::eval(double x)
137 {
138 x = (x - x0) / step;
139 int i = static_cast<int>(x);
140 if (i < 0) {
141 i = 0;
142 x = 0.0;
143 } else if (i >= count-1) {
144 i = count-2;
145 x = 1.0;
146 } else {
147 x = x - i;
148 }
149 double b = (f[i+1]-f[i]) - (c[i+1]+2*c[i])/3;
150 return ((((c[i+1]-c[i])/3 * x + c[i]) * x) + b) * x + f[i];
151 }
152
153 /****************************************************************
154 * Grid
155 */
156
Grid(int d,Dim * v,int n)157 Grid::Grid(int d, Dim *v, int n)
158 {
159 vals = n;
160 ndim = d;
161 dim = v;
162 weight = new double[ndim];
163 idx = new int[ndim];
164 idx2 = new int[ndim];
165 p2dim = 1;
166 for (int i = 0; i < ndim; i++) p2dim *= 2;
167 pts = new double[vals*p2dim];
168 grid = 0;
169 }
170
~Grid()171 Grid::~Grid()
172 {
173 delete[] weight;
174 delete[] idx;
175 delete[] idx2;
176 delete[] pts;
177 }
178
calc_strides()179 int Grid::calc_strides()
180 {
181 int sz = vals;
182 for (Dim *p = dim+ndim; --p >= dim; ) {
183 p->stride = sz;
184 sz *= p->size;
185 }
186 return sz;
187 }
188
cell(int * ix)189 inline float *Grid::cell(int *ix)
190 {
191 float *p = grid;
192 for (int i = 0; i < ndim; i++) {
193 p += ix[i] * dim[i].stride;
194 }
195 return p;
196 }
197
198 /****************************************************************
199 * Cube
200 */
201
Cube(int d,Dim * v,int n,int * ix,int n_in_,ComponentBase & cb_,N_Vector u0,const char * fname)202 Cube::Cube(int d, Dim *v, int n, int *ix, int n_in_, ComponentBase& cb_, N_Vector u0, const char *fname)
203 : Grid(d, v, n),
204 cb(cb_),
205 mmap_size(0),
206 n_in(n_in_) {
207 grid = 0;
208 int sz = calc_strides();
209 if (fname) {
210 int fd = open(fname, O_RDONLY);
211 if (fd >= 0) {
212 struct stat attr;
213 if (fstat(fd, &attr) >= 0) {
214 if (attr.st_size == sz*(int)sizeof(float)) {
215 void *addr = mmap(0, attr.st_size, PROT_READ, MAP_SHARED, fd, 0);
216 if (addr != (void*)-1) {
217 grid = static_cast<float*>(addr);
218 mmap_size = attr.st_size;
219 }
220 }
221 }
222 }
223 }
224 if (!grid) {
225 grid = new float[sz];
226 N_Vector x = N_VNew_Serial(ndim);
227 for (int i = 0; i < ndim; i++) {
228 idx[i] = 0;
229 Ith(x,i) = dim[i].lower;
230 }
231 fill(ndim, ix, x, u0);
232 N_VDestroy_Serial(x);
233 if (fname) {
234 int fd = creat(fname, 0666);
235 if (fd < 0) {
236 fprintf(stderr, "open %s for writing failed\n", fname);
237 } else {
238 if (write(fd, grid, sz*sizeof(float)) != sz*(int)sizeof(float)) {
239 fprintf(stderr, "write to %s failed\n", fname);
240 } else {
241 printf("written mmap file %s\n", fname);
242 }
243 close(fd);
244 }
245 }
246 }
247 }
248
~Cube()249 Cube::~Cube()
250 {
251 if (mmap_size) {
252 munmap(grid, mmap_size);
253 } else {
254 delete[] grid;
255 }
256 }
257
fill(int n,int * ix,N_Vector x,N_Vector u0)258 int Cube::fill(int n, int *ix, N_Vector x, N_Vector u0)
259 {
260 N_Vector u = N_VNew_Serial(vals);
261 N_VScale_Serial(ONE, u0, u);
262 for (int i = 0; i < dim[*ix].size; i++) {
263 idx[*ix] = i;
264 Ith(x,*ix) = dim[*ix].point(i);
265 if (n > 1) {
266 fill(n-1, ix+1, x, u);
267 } else {
268 //printf("-> "); N_VPrint_Serial(u); N_VPrint_Serial(x);
269 if (cb.findzero(NV_DATA_S(x), NV_DATA_S(x)+n_in, u)) {
270 print_vec("fill error at", x);
271 N_VDestroy_Serial(u);
272 return 1;
273 }
274 //printf("<- "); N_VPrint_Serial(u);
275 float *p = cell(idx);
276 for (int j = 0; j < vals; j++) {
277 p[j] = Ith(u,j);
278 }
279 }
280 if (i == 0) {
281 N_VScale_Serial(ONE,u,u0);
282 }
283 }
284 N_VDestroy_Serial(u);
285 return 0;
286 }
287
startvalue(N_Vector x,N_Vector s,N_Vector u)288 void Cube::startvalue(N_Vector x, N_Vector s, N_Vector u)
289 {
290 assert(NV_LENGTH_S(x)+NV_LENGTH_S(s) == ndim);
291 for (int i = 0; i < NV_LENGTH_S(x); i++) {
292 dim[i].index(Ith(x,i),idx[i],weight[i]);
293 }
294 for (int i = NV_LENGTH_S(x); i < ndim; i++) {
295 dim[i].index(Ith(s,i-NV_LENGTH_S(x)),idx[i],weight[i]);
296 }
297 for (int i = 0; i < p2dim; i++) {
298 for (int k = 0; k < ndim; k++) {
299 idx2[k] = idx[k];
300 if (i & (1 << k)) {
301 idx2[k] += 1;
302 }
303 }
304 float *p = cell(idx2);
305 for (int j = 0; j < vals; j++) {
306 pts[i*vals+j] = p[j];
307 }
308 }
309 int n = p2dim;
310 for (int i = 0; i < ndim; i++) {
311 double w = weight[i];
312 n /= 2;
313 for (int j = 0; j < n; j++) {
314 int b = j*vals;
315 for (int k = 0; k < vals; k++) {
316 pts[b+k] = pts[2*b+k] * (1-w) + pts[2*b+vals+k] * w;
317 }
318 }
319 }
320 for (int i = 0; i < vals; i++) {
321 Ith(u,i) = pts[i];
322 }
323 }
324
calc(N_Vector x,N_Vector s,N_Vector u)325 int Cube::calc(N_Vector x, N_Vector s, N_Vector u)
326 {
327 startvalue(x, s, u);
328 return cb.findzero(NV_DATA_S(x), NV_DATA_S(s), u);
329 }
330
331 /****************************************************************
332 ** class ComponentBase
333 */
334
ComponentBase(int neq,int ndim,int nvals,int n_in,int n_out,int n_params_)335 ComponentBase::ComponentBase(int neq, int ndim, int nvals, int n_in, int n_out, int n_params_)
336 : NEQ(neq),
337 NDIM(ndim),
338 NVALS(nvals),
339 N_IN(n_in),
340 N_OUT(n_out),
341 n_params(n_params_),
342 params(new realtype[n_params_]),
343 param_names(new const char*[n_params_]),
344 in_names(new const char*[n_in]),
345 out_names(new const char*[n_out]),
346 state_names(new const char*[ndim-n_out]),
347 var_names(new const char*[neq]),
348 ix(new int[ndim]),
349 verbose(false),
350 ranges(new Dim[ndim]),
351 cube(0),
352 user_data(),
353 constraints(0),
354 Gco(params[0]),
355 Gcf(params[1]),
356 mu(params[2]),
357 Ex(params[3]),
358 Kg1(params[4]),
359 Kg2(params[5]),
360 Kp(params[6]),
361 Kvb(params[7]) {
362 const char *p[] = { "Gco", "Gcf", "mu", "Ex", "Kg1", "Kg2", "Kp", "Kvb", 0 };
363 set_names(param_names, p, param_off);
364 }
365
~ComponentBase()366 ComponentBase::~ComponentBase() {
367 delete[] params;
368 delete[] param_names;
369 delete[] in_names;
370 delete[] out_names;
371 delete[] state_names;
372 delete[] var_names;
373 delete[] ix;
374 delete[] ranges;
375 delete cube;
376 if (constraints) {
377 N_VDestroy_Serial(constraints);
378 }
379 }
380
set_range(int i,double lower,double upper,int size)381 int ComponentBase::set_range(int i, double lower, double upper, int size) {
382 if (i > NDIM) {
383 return 0;
384 }
385 ranges[i].lower = lower;
386 ranges[i].upper = upper;
387 ranges[i].size = size;
388 return 1;
389 }
390
init(N_Vector u0,const char * fname)391 void ComponentBase::init(N_Vector u0, const char* fname) {
392 N_Vector u = N_VNew_Serial(NEQ);
393 for (int i = 0; i < NEQ; i++) {
394 Ith(u,i) = Ith(u0,i);
395 }
396 delete cube;
397 cube = new Cube(NDIM, ranges, NEQ, ix, N_IN, *this, u, fname);
398 N_VDestroy_Serial(u);
399 }
400
401 //static
sfunc(N_Vector x,N_Vector u,void * data)402 int ComponentBase::sfunc(N_Vector x, N_Vector u, void *data) {
403 ComponentBase *self = static_cast<ComponentBase*>(data);
404 return self->func(x, u, &self->user_data);
405 }
406
findzero(realtype * x,realtype * state,N_Vector u)407 int ComponentBase::findzero(realtype *x, realtype *state, N_Vector u) {
408 int flag;
409
410 N_Vector s = N_VNew_Serial(NEQ);
411 if (check_flag((void *)s, "N_VNew_Serial", 0)) return 1;
412 N_VConst_Serial(ONE,s); /* no scaling */
413
414 realtype fnormtol = FTOL;
415 realtype scsteptol = STOL;
416
417 void *kmem = KINCreate();
418 if (check_flag((void *)kmem, "KINCreate", 0)) return 1;
419
420 user_data.inval = x;
421 user_data.state = state;
422 flag = KINSetUserData(kmem, this);
423 if (check_flag(&flag, "KINSetUserData", 1)) return flag;
424
425 if (constraints) {
426 flag = KINSetConstraints(kmem, constraints);
427 if (check_flag(&flag, "KINSetConstraints", 1)) {
428 return flag;
429 }
430 }
431
432 flag = KINSetFuncNormTol(kmem, fnormtol);
433 if (check_flag(&flag, "KINSetFuncNormTol", 1)) return flag;
434 flag = KINSetScaledStepTol(kmem, scsteptol);
435 if (check_flag(&flag, "KINSetScaledStepTol", 1)) return flag;
436
437 flag = KINInit(kmem, sfunc, u);
438 if (check_flag(&flag, "KINInit", 1)) return flag;
439
440 /* Call KINDense to specify the linear solver */
441
442 flag = KINDense(kmem, NEQ);
443 if (check_flag(&flag, "KINDense", 1)) return flag;
444
445 //flag = KINSetPrintLevel(kmem, 3);
446 //if (check_flag(&flag, "KINSetPrintLevel", 1)) return flag;
447
448 int glstr = KIN_NONE;
449 //int glstr = KIN_LINESEARCH;
450 int mset = 1;
451 flag = KINSetMaxSetupCalls(kmem, mset);
452 if (check_flag(&flag, "KINSetMaxSetupCalls", 1)) return flag;
453 flag = KINSol(kmem, u, glstr, s, s);
454 if (flag > 1) {
455 printf("res = %d\n", flag);
456 return flag;
457 }
458 if (check_flag(&flag, "KINSol", 1)) return flag;
459 if (verbose) {
460 PrintFinalStats(kmem);
461 }
462 N_VDestroy_Serial(s);
463 KINFree(&kmem);
464 user_data.inval = 0;
465 user_data.state = 0;
466 return 0;
467 }
468
startvalue(N_Vector x,N_Vector s,N_Vector u)469 void ComponentBase::startvalue(N_Vector x, N_Vector s, N_Vector u) {
470 cube->startvalue(x, s, u);
471 }
472
calc(N_Vector x,N_Vector s,N_Vector u)473 int ComponentBase::calc(N_Vector x, N_Vector s, N_Vector u) {
474 return cube->calc(x, s, u);
475 }
476
Ig(realtype Ugk)477 inline realtype ComponentBase::Ig(realtype Ugk) {
478 if (Ugk < Gco) {
479 return ZERO;
480 }
481 return Gcf*pow(Ugk-Gco,1.5);
482 }
483
Ia(realtype Ugk,realtype Uak)484 inline realtype ComponentBase::Ia(realtype Ugk, realtype Uak) {
485 if (Uak < ZERO) {
486 return ZERO;
487 }
488 realtype E1;
489 realtype t = Kp*(1/mu+Ugk/sqrt(Kvb+Uak*Uak));
490 if (t > 500) {
491 E1 = Uak/Kp*t;
492 } else if (t < -500) {
493 return ZERO;
494 } else {
495 E1 = Uak/Kp*log(1+exp(t));
496 }
497 return pow(E1,Ex) / Kg1 * 2*(E1 > 0.0);
498 }
499
Iap(realtype Ug1k,realtype Ug2k,realtype Uak)500 inline realtype ComponentBase::Iap(realtype Ug1k, realtype Ug2k, realtype Uak) {
501 realtype E1;
502 if (Ug2k <= 0.0) {
503 return ZERO;
504 }
505 realtype t = Kp*(1/mu+Ug1k/Ug2k);
506 if (t > 500) {
507 E1 = Ug2k/Kp*t;
508 } else if (t < -500) {
509 return ZERO;
510 } else {
511 E1 = Ug2k/Kp*log(1+exp(t));
512 }
513 return pow(E1,Ex)/Kg1 * 2*(E1 > 0.0) * atan(Uak/Kvb);
514 }
515
Is(realtype Ug1k,realtype Ug2k)516 inline realtype ComponentBase::Is(realtype Ug1k, realtype Ug2k) {
517 realtype t = Ug2k/mu+Ug1k;
518 if (t < 0) {
519 return ZERO;
520 }
521 return exp(Ex*log(t))/Kg2;
522 }
523
524
525 /****************************************************************
526 ** Components
527 */
528
TriodeCircuit()529 TriodeCircuit::TriodeCircuit()
530 : ComponentBase(3+2, 2, 2, 1, 1, param_off+8),
531 Ck(params[param_off+0]),
532 G1(params[param_off+1]),
533 G2(params[param_off+2]),
534 Gg(params[param_off+3]),
535 Gk(params[param_off+4]),
536 Ga(params[param_off+5]),
537 Gl(params[param_off+6]),
538 Un(params[param_off+7]) {
539 const char *p_names[] = {
540 "Ck", "G1", "G2", "Gg", "Gk", "Ga", "Gl", "Un", 0 };
541 set_names(param_names+param_off, p_names, n_params-param_off);
542 static const char *i_names[] = { "Uin", 0 };
543 set_names(in_names, i_names, N_IN);
544 static const char *o_names[] = { "Ua", 0 };
545 set_names(out_names, o_names, N_OUT);
546 static const char *s_names[] = { "Uc1m", 0 };
547 set_names(state_names, s_names, NDIM-N_IN);
548 static const char *v_names[] = { "Ug", "Uk", "Ua", "l", "L", 0 };
549 set_names(var_names, v_names, NEQ);
550 ix[0] = 1;
551 ix[1] = 0;
552 constraints = N_VNew_Serial(NEQ);
553 Ith(constraints,0) = ZERO; /* no constraint on f1 */
554 Ith(constraints,1) = ZERO; /* no constraint on f2 */
555 Ith(constraints,2) = ZERO; /* no constraint on f3 */
556 Ith(constraints,3) = ONE; /* l = Ua - Uk >= 0 */
557 Ith(constraints,4) = -ONE; /* L = Ua - Un <= 0 */
558 }
559
update(N_Vector y,N_Vector x,N_Vector state)560 void TriodeCircuit::update(N_Vector y, N_Vector x, N_Vector state) {
561 realtype *fdata = NV_DATA_S(y);
562 realtype Uk = fdata[1];
563 realtype Uc1m = Uk;
564 if (DOUBLE_UPDATE) {
565 realtype Ug = fdata[0];
566 realtype Ua = fdata[2];
567 Uc1m = Uc1m - (Uk*Gk - Ia(Ug-Uk,Ua-Uk) - Ig(Ug-Uk)) / (Ck*fs);
568 }
569 Ith(state,0) = Uc1m;
570 }
571
func(N_Vector u,N_Vector f,UserData * data)572 int TriodeCircuit::func(N_Vector u, N_Vector f, UserData *data) {
573 realtype *udata = NV_DATA_S(u);
574 realtype *fdata = NV_DATA_S(f);
575
576 //printf("-> %g,%g,%g,%g,%g\n", Ith(u,0), Ith(u,1), Ith(u,2), Ith(u,3), Ith(u,4));
577 realtype Ug = udata[0];
578 realtype Uk = udata[1];
579 realtype Ua = udata[2];
580 realtype l = udata[3];
581 realtype L = udata[4];
582 realtype Uc = Uk;
583
584 realtype Uin = data->inval[0];
585 realtype Uc1m = data->state[0];
586
587 realtype ia = Ia(Ug-Uk,Ua-Uk);
588 realtype ig = Ig(Ug-Uk);
589 fdata[0] = G2*(Uin*G1+Ug*G2)/(G1+Gg+G2)-Ug*G2-ig;
590 fdata[1] = Uc1m-Uc-(Uk*Gk-ia-ig)/(Ck*fs);
591 fdata[2] = Un*Ga-Ua*Ga-Ua*Gl-ia;
592 fdata[3] = l - (Ua - Uk);
593 fdata[4] = L - (Ua - Un);
594 //printf("<- %g,%g,%g,%g,%g\n", Ith(f,0), Ith(f,1), Ith(f,2), Ith(f,3), Ith(f,4));
595
596 return(0);
597 }
598
599
CoupledTriodeCircuit()600 CoupledTriodeCircuit::CoupledTriodeCircuit()
601 : ComponentBase(7, 3, 3, 1, 1, param_off+13),
602 Ck(params[param_off+0]),
603 Ca(params[param_off+1]),
604 Un(params[param_off+2]),
605 G1(params[param_off+3]),
606 G2(params[param_off+4]),
607 Gg(params[param_off+5]),
608 Gk(params[param_off+6]),
609 Ga(params[param_off+7]),
610 G3(params[param_off+8]),
611 Gg2(params[param_off+9]),
612 Gk2(params[param_off+10]),
613 Ga2(params[param_off+11]),
614 Gl(params[param_off+12]) {
615 const char *p_names[] = {
616 "Ck", "Ca", "Un", "G1", "G2", "Gg", "Gk", "Ga", "G3", "Gg2", "Gk2", "Ga2", "Gl", 0 };
617 set_names(param_names+param_off, p_names, n_params-param_off);
618 static const char *i_names[] = { "Uin", 0 };
619 set_names(in_names, i_names, N_IN);
620 static const char *o_names[] = { "U2", 0 };
621 set_names(out_names, o_names, N_OUT);
622 static const char *s_names[] = { "Uc1m", "Uc2m", 0 };
623 set_names(state_names, s_names, NDIM-N_IN);
624 static const char *v_names[] = { "Ug", "Uk", "Ua", "U2", "Ug2", "Uk2", "Ua2", 0 };
625 set_names(var_names, v_names, NEQ);
626 ix[0] = 2;
627 ix[1] = 1;
628 ix[2] = 0;
629 }
630
update(N_Vector y,N_Vector x,N_Vector state)631 void CoupledTriodeCircuit::update(N_Vector y, N_Vector x, N_Vector state)
632 {
633 realtype *fdata = NV_DATA_S(y);
634 realtype Uk = fdata[1];
635 realtype Ua = fdata[2];
636 realtype U2 = fdata[3];
637
638 realtype Uc1m = Uk;
639 realtype Uc2m = Ua-U2;
640
641 if (DOUBLE_UPDATE) {
642 realtype Ug = fdata[0];
643 realtype Ug2 = fdata[4];
644 realtype ia1 = Ia(Ug-Uk,Ua-Uk);
645 realtype ig1 = Ig(Ug-Uk);
646 Uc1m = Uc1m - (Uk*Gk-ia1-ig1) / (Ck*fs);
647 Uc2m = Uc2m + ((U2-Ug2)*G3) / (Ca*fs);
648 }
649 Ith(state,0) = Uc1m;
650 Ith(state,1) = Uc2m;
651 }
652
func(N_Vector u,N_Vector f,UserData * data)653 int CoupledTriodeCircuit::func(N_Vector u, N_Vector f, UserData *data) {
654 realtype *udata = NV_DATA_S(u);
655 realtype *fdata = NV_DATA_S(f);
656
657 //printf("<- %g,%g,%g,%g,%g,%g,%g\n", Ith(u,0), Ith(u,1), Ith(u,2), Ith(u,3), Ith(u,4), Ith(u,5), Ith(u,6));
658 realtype Ug = udata[0];
659 realtype Uk = udata[1];
660 realtype Ua = udata[2];
661 realtype U2 = udata[3];
662 realtype Ug2 = udata[4];
663 realtype Uk2 = udata[5];
664 realtype Ua2 = udata[6];
665 realtype Uc1 = Uk;
666 realtype Uc2 = Ua-U2;
667
668 realtype Uin = data->inval[0];
669 realtype Uc1m = data->state[0];
670 realtype Uc2m = data->state[1];
671
672 realtype ia1 = Ia(Ug-Uk,Ua-Uk);
673 realtype ig1 = Ig(Ug-Uk);
674 realtype ia2 = Ia(Ug2-Uk2,Ua2-Uk2);
675 realtype ig2 = Ig(Ug2-Uk2);
676 fdata[0] = G2*(Uin*G1+Ug*G2)/(G1+Gg+G2)-Ug*G2-ig1;
677 fdata[1] = Uc1m-Uc1-(Uk*Gk-ia1-ig1)/(Ck*fs);
678 fdata[2] = (Un-Ua)*Ga-ia1-(U2-Ug2)*G3;
679 fdata[3] = Uc2m-Uc2+((U2-Ug2)*G3)/(Ca*fs);
680 fdata[4] = (U2-Ug2)*G3-Ug2*Gg2-ig2;
681 fdata[5] = Uk2*Gk2-ia2-ig2;
682 fdata[6] = (Un-Ua2)*Ga2-Ua2*Gl-ia2;
683 //printf("<- %g,%g,%g,%g,%g,%g,%g\n", Ith(f,0), Ith(f,1), Ith(f,2), Ith(f,3), Ith(f,4), Ith(f,5), Ith(f,6));
684
685 return(0);
686 }
687
688
PowerAmpGate()689 PowerAmpGate::PowerAmpGate()
690 : ComponentBase(3, 2, 2, 1, 1, param_off+5),
691 C1(params[param_off+0]),
692 G1(params[param_off+1]),
693 Gb(params[param_off+2]),
694 Gg(params[param_off+3]),
695 Ub(params[param_off+4]) {
696 const char *p_names[] = { "C1", "G1", "Gb", "Gg", "Ub", 0 };
697 set_names(param_names+param_off, p_names, n_params-param_off);
698 static const char *i_names[] = { "Uin", 0 };
699 set_names(in_names, i_names, N_IN);
700 static const char *o_names[] = { "Ug", 0 };
701 set_names(out_names, o_names, N_OUT);
702 static const char *s_names[] = { "Uc1m", 0 };
703 set_names(state_names, s_names, NDIM-N_IN);
704 static const char *v_names[] = { "U0", "U1", "Ug", 0 };
705 set_names(var_names, v_names, NEQ);
706 ix[0] = 1;
707 ix[1] = 0;
708 }
709
update(N_Vector y,N_Vector x,N_Vector state)710 void PowerAmpGate::update(N_Vector y, N_Vector x, N_Vector state) {
711 realtype *fdata = NV_DATA_S(y);
712 realtype U0 = fdata[0];
713 realtype U1 = fdata[1];
714 realtype Uc1m = U0-U1;
715
716 if (DOUBLE_UPDATE) {
717 realtype Ug = fdata[2];
718 Uc1m = Uc1m + ((U1-Ub)*Gb+(U1-Ug)*Gg)/(C1*fs);
719 }
720 Ith(state,0) = Uc1m;
721 }
722
func(N_Vector u,N_Vector f,UserData * data)723 int PowerAmpGate::func(N_Vector u, N_Vector f, UserData *data) {
724 realtype *udata = NV_DATA_S(u);
725 realtype *fdata = NV_DATA_S(f);
726
727 realtype U0 = udata[0];
728 realtype U1 = udata[1];
729 realtype Ug = udata[2];
730 realtype Uc1 = U0 - U1;
731
732 realtype Uin = data->inval[0];
733 realtype Uc1m = data->state[0];
734
735 fdata[0] = C1*(Uc1-Uc1m)*fs-(Uin-U0)*G1;
736 fdata[1] = (Uin-U0)*G1-(U1-Ub)*Gb-(U1-Ug)*Gg;
737 fdata[2] = (U1-Ug)*Gg-Ig(Ug);
738
739 return(0);
740 }
741
742
PowerAmpPlate()743 PowerAmpPlate::PowerAmpPlate()
744 : ComponentBase(5+2, 3, 2, 2, 2, param_off+5),
745 C2(params[param_off+0]),
746 Gd(params[param_off+1]),
747 Ga(params[param_off+2]),
748 Gs(params[param_off+3]),
749 Un(params[param_off+4]) {
750 const char *p_names[] = { "C2", "Gd", "Ga", "Gs", "Un", 0 };
751 set_names(param_names+param_off, p_names, n_params-param_off);
752 static const char *i_names[] = { "Ug1", "Ug2", 0 };
753 set_names(in_names, i_names, N_IN);
754 static const char *o_names[] = { "Ua1", "Ua2", 0 };
755 set_names(out_names, o_names, N_OUT);
756 static const char *s_names[] = { "Uc2m", 0 };
757 set_names(state_names, s_names, NDIM-N_IN);
758 static const char *v_names[] = { "Us1", "Us2", "Ud", "Ua1", "Ua2", "L1", "L2", 0 };
759 set_names(var_names, v_names, NEQ);
760 ix[0] = 2;
761 ix[1] = 1;
762 ix[2] = 0;
763 constraints = N_VNew_Serial(NEQ);
764 Ith(constraints,0) = ZERO; /* no constraint on f1 */
765 Ith(constraints,1) = ZERO; /* no constraint on f2 */
766 Ith(constraints,2) = ZERO; /* no constraint on f3 */
767 Ith(constraints,3) = ZERO; /* no constraint on f4 */
768 Ith(constraints,4) = ZERO; /* no constraint on f5 */
769 Ith(constraints,5) = ONE; /* L1 = Us1 - Ud/2 >= 0 */
770 Ith(constraints,6) = ONE; /* L2 = Us2 - Ud/2 >= 0 */
771 }
772
update(N_Vector y,N_Vector x,N_Vector state)773 void PowerAmpPlate::update(N_Vector y, N_Vector x, N_Vector state) {
774 realtype *fdata = NV_DATA_S(y);
775 realtype Ud = fdata[2];
776 realtype Uc2m = Ud;
777 if (DOUBLE_UPDATE) {
778 realtype Us1 = fdata[0];
779 realtype Us2 = fdata[1];
780 realtype Ua1 = fdata[3];
781 realtype Ua2 = fdata[4];
782 realtype Ug1 = Ith(x,0);
783 realtype Ug2 = Ith(x,1);
784 realtype ia1 = Iap(Ug1,Us1,Ua1);
785 realtype is1 = Is(Ug1,Us1);
786 realtype ia2 = Iap(Ug2,Us2,Ua2);
787 realtype is2 = Is(Ug2,Us2);
788 Uc2m = Uc2m + ((Un-Uc2m)*Gd-(ia1+ia2+is1+is2))/(C2*fs);
789 }
790 Ith(state,0) = Uc2m;
791 }
792
func(N_Vector u,N_Vector f,UserData * data)793 int PowerAmpPlate::func(N_Vector u, N_Vector f, UserData *data) {
794 realtype *udata = NV_DATA_S(u);
795 realtype *fdata = NV_DATA_S(f);
796
797 realtype Us1 = udata[0];
798 realtype Us2 = udata[1];
799 realtype Ud = udata[2];
800 realtype Ua1 = udata[3];
801 realtype Ua2 = udata[4];
802 realtype L1 = udata[5];
803 realtype L2 = udata[6];
804 realtype Uc2 = Ud;
805
806 realtype Ug1 = data->inval[0];
807 realtype Ug2 = data->inval[1];
808 realtype Uc2m = data->state[0];
809
810 realtype ia1 = Iap(Ug1,Us1,Ua1);
811 realtype is1 = Is(Ug1,Us1);
812 realtype ia2 = Iap(Ug2,Us2,Ua2);
813 realtype is2 = Is(Ug2,Us2);
814 fdata[0] = -ia1+(Ud-Ua1)*Ga;
815 fdata[1] = -ia2+(Ud-Ua2)*Ga;
816 fdata[2] = -is1+(Ud-Us1)*Gs;
817 fdata[3] = -is2+(Ud-Us2)*Gs;
818 fdata[4] = -ia1-ia2-is1-is2+(Un-Ud)*Gd-C2*(Uc2-Uc2m)*fs;
819 fdata[5] = L1 - (Us1 - -Ud);
820 fdata[6] = L2 - (Us2 - -Ud);
821
822 return(0);
823 }
824
825
PhaseSplitter()826 PhaseSplitter::PhaseSplitter()
827 : ComponentBase(9, 4, 5, 1, 2, param_off+16),
828 C1(params[param_off+0]),
829 C2(params[param_off+1]),
830 C3(params[param_off+2]),
831 Un(params[param_off+3]),
832 G1(params[param_off+4]),
833 Gg1(params[param_off+5]),
834 Gg2(params[param_off+6]),
835 Gk(params[param_off+7]),
836 G2(params[param_off+8]),
837 G3(params[param_off+9]),
838 G4(params[param_off+10]),
839 G5(params[param_off+11]),
840 Ga1(params[param_off+12]),
841 Ga2(params[param_off+13]),
842 Gl1(params[param_off+14]),
843 Gl2(params[param_off+15]) {
844 const char *p_names[] = {
845 "C1", "C2", "C3", "Un", "G1", "Gg1", "Gg2", "Gk", "G2", "G3", "G4", "G5", "Ga1", "Ga2", "Gl1", "Gl2", 0 };
846 set_names(param_names+param_off, p_names, n_params-param_off);
847 static const char *i_names[] = { "Uin", 0 };
848 set_names(in_names, i_names, N_IN);
849 static const char *o_names[] = { "Ua1", "Ua2", 0 };
850 set_names(out_names, o_names, N_OUT);
851 static const char *s_names[] = { "Uc1m", "Uc2m", "Uc3m", 0 };
852 set_names(state_names, s_names, NDIM-N_IN);
853 static const char *v_names[] = { "U1", "Ug1", "Uk", "Ua1", "U2", "U3", "U4", "Ug2", "Ua2", 0 };
854 set_names(var_names, v_names, NEQ);
855 ix[0] = 3;
856 ix[1] = 2;
857 ix[2] = 1;
858 ix[3] = 0;
859 }
860
update(N_Vector y,N_Vector x,N_Vector state)861 void PhaseSplitter::update(N_Vector y, N_Vector x, N_Vector state) {
862 realtype *fdata = NV_DATA_S(y);
863 realtype U1 = fdata[0];
864 realtype Ug1 = fdata[1];
865 realtype U3 = fdata[5];
866 realtype U4 = fdata[6];
867 realtype Ug2 = fdata[7];
868 realtype Uc1m = U1-Ug1;
869 realtype Uc2m = Ug2-U3;
870 realtype Uc3m = U3-U4;
871 if (DOUBLE_UPDATE) {
872 realtype Uk = fdata[2];
873 realtype U2 = fdata[4];
874 realtype ig1 = Ig(Ug1-Uk);
875 realtype ig2 = Ig(Ug2-Uk);
876 Uc1m = Uc1m + ((Ug1-U2)*Gg1+ig1) / (C1*fs);
877 Uc2m = Uc2m - ((Ug2-U2)*Gg2+ig2) / (C2*fs);
878 Uc3m = Uc3m + (U4*G4) / (C3*fs);
879 }
880 Ith(state,0) = Uc1m;
881 Ith(state,1) = Uc2m;
882 Ith(state,2) = Uc3m;
883 }
884
func(N_Vector u,N_Vector f,UserData * data)885 int PhaseSplitter::func(N_Vector u, N_Vector f, UserData *data) {
886 realtype *udata = NV_DATA_S(u);
887 realtype *fdata = NV_DATA_S(f);
888
889 realtype U1 = udata[0];
890 realtype Ug1 = udata[1];
891 realtype Uk = udata[2];
892 realtype Ua1 = udata[3];
893 realtype U2 = udata[4];
894 realtype U3 = udata[5];
895 realtype U4 = udata[6];
896 realtype Ug2 = udata[7];
897 realtype Ua2 = udata[8];
898 realtype Uc1 = U1 - Ug1;
899 realtype Uc2 = Ug2 - U3;
900 realtype Uc3 = U3 - U4;
901
902 realtype Uin = data->inval[0];
903 realtype Uc1m = data->state[0];
904 realtype Uc2m = data->state[1];
905 realtype Uc3m = data->state[2];
906
907 realtype ia1 = Ia(Ug1-Uk,Ua1-Uk);
908 realtype ig1 = Ig(Ug1-Uk);
909 realtype ia2 = Ia(Ug2-Uk,Ua2-Uk);
910 realtype ig2 = Ig(Ug2-Uk);
911 fdata[0] = (Uin-U1)*G1-(Ug1-U2)*Gg1-ig1;
912 fdata[1] = C1*(Uc1-Uc1m)*fs-(Uin-U1)*G1;
913 fdata[2] = (Uk-U2)*Gk-ia1-ig1-ia2-ig2;
914 fdata[3] = (Un-Ua1)*Ga1-Ua1*Gl1-ia1;
915 fdata[4] = (Un-Ua2)*Ga2-Ua2*Gl2-ia2;
916 fdata[5] = -ig2+(U2-Ug2)*Gg2-C2*(Uc2-Uc2m)*fs;
917 fdata[6] = (Ug1-U2)*Gg1+(Uk-U2)*Gk-(U2-Ug2)*Gg2-(U2-U3)*G2;
918 fdata[7] = (U2-U3)*Gg2-U3*G3-U3*G5-U4*G4+C2*(Uc2-Uc2m)*fs;
919 fdata[8] = C3*(Uc3-Uc3m)*fs-U4*G4;
920
921 return(0);
922 }
923
924
PowerAmpPlateTrans()925 PowerAmpPlateTrans::PowerAmpPlateTrans()
926 : ComponentBase(5+2, 3, 2, 2, 2, param_off+5),
927 C2(params[param_off+0]),
928 Gd(params[param_off+1]),
929 Ga(params[param_off+2]),
930 Gs(params[param_off+3]),
931 Un(params[param_off+4]) {
932 const char *p_names[] = { "C2", "Gd", "Ga", "Gs", "Un", 0 };
933 set_names(param_names+param_off, p_names, n_params-param_off);
934 static const char *i_names[] = { "Ug1", "Ug2", 0 };
935 set_names(in_names, i_names, N_IN);
936 static const char *o_names[] = { "Ua1", "Ua2", 0 };
937 set_names(out_names, o_names, N_OUT);
938 static const char *s_names[] = { "Uc2m", 0 };
939 set_names(state_names, s_names, NVALS-N_OUT);
940 static const char *v_names[] = { "Us1", "Us2", "Ud", "Ua1", "Ua2", "L1", "L2", 0 };
941 set_names(var_names, v_names, NEQ);
942 ix[0] = 2;
943 ix[1] = 1;
944 ix[2] = 0;
945 constraints = N_VNew_Serial(NEQ);
946 Ith(constraints,0) = ZERO; /* no constraint on f1 */
947 Ith(constraints,1) = ZERO; /* no constraint on f2 */
948 Ith(constraints,2) = ZERO; /* no constraint on f3 */
949 Ith(constraints,3) = ZERO; /* no constraint on f4 */
950 Ith(constraints,4) = ZERO; /* no constraint on f5 */
951 Ith(constraints,5) = ONE; /* L1 = Us1 - Ud/2 >= 0 */
952 Ith(constraints,6) = ONE; /* L2 = Us2 - Ud/2 >= 0 */
953 }
954
update(N_Vector y,N_Vector x,N_Vector state)955 void PowerAmpPlateTrans::update(N_Vector y, N_Vector x, N_Vector state) {
956 realtype *fdata = NV_DATA_S(y);
957 realtype Ud = fdata[2];
958 realtype Uc2m = Ud;
959 if (DOUBLE_UPDATE) {
960 realtype Us1 = fdata[0];
961 realtype Us2 = fdata[1];
962 realtype Ua1 = fdata[3];
963 realtype Ua2 = fdata[4];
964 realtype Ug1 = Ith(x,0);
965 realtype Ug2 = Ith(x,1);
966 realtype ia1 = Iap(Ug1,Us1,Ua1);
967 realtype is1 = Is(Ug1,Us1);
968 realtype ia2 = Iap(Ug2,Us2,Ua2);
969 realtype is2 = Is(Ug2,Us2);
970 Uc2m = Uc2m + ((Un-Uc2m)*Gd-(ia1+ia2+is1+is2))/(C2*fs);
971 }
972 Ith(state,0) = Uc2m;
973 }
974
func(N_Vector u,N_Vector f,UserData * data)975 int PowerAmpPlateTrans::func(N_Vector u, N_Vector f, UserData *data) {
976 realtype *udata = NV_DATA_S(u);
977 realtype *fdata = NV_DATA_S(f);
978
979 realtype Us1 = udata[0];
980 realtype Us2 = udata[1];
981 realtype Ud = udata[2];
982 realtype Ua1 = udata[3];
983 realtype Ua2 = udata[4];
984 realtype L1 = udata[5];
985 realtype L2 = udata[6];
986 realtype Uc2 = Ud;
987
988 realtype Ug1 = data->inval[0];
989 realtype Ug2 = data->inval[1];
990 realtype Uc2m = data->state[0];
991
992 realtype ia1 = Iap(Ug1,Us1,Ua1);
993 realtype is1 = Is(Ug1,Us1);
994 realtype ia2 = Iap(Ug2,Us2,Ua2);
995 realtype is2 = Is(Ug2,Us2);
996 fdata[0] = -ia1+(Ud-Ua1)*Ga;
997 fdata[1] = -ia2+(Ud-Ua2)*Ga;
998 fdata[2] = -is1+(Ud-Us1)*Gs;
999 fdata[3] = -is2+(Ud-Us2)*Gs;
1000 fdata[4] = -ia1-ia2-is1-is2+(Un-Ud)*Gd-C2*(Uc2-Uc2m)*fs;
1001 fdata[5] = L1 - (Us1 - -Ud);
1002 fdata[6] = L2 - (Us2 - -Ud);
1003
1004 return(0);
1005 }
1006
1007
1008 /*
1009 *--------------------------------------------------------------------
1010 * MAIN PROGRAM
1011 *--------------------------------------------------------------------
1012 */
1013
1014 #if 0
1015 int main()
1016 {
1017 N_Vector u = N_VNew_Serial(NEQ);
1018 for (int i = 0; i < 200; i++) {
1019 realtype Uin = RCONST(-40.0) + i * (RCONST(40.0)-RCONST(-40.0))/200;
1020 for (int j = 0; j < 12; j++) {
1021 realtype Uc1m = RCONST(-15.0) + j * (RCONST(15.0)-RCONST(-15.0))/12;
1022 Ith(u,0) = RCONST(0.0);
1023 Ith(u,1) = RCONST(0.0);
1024 Ith(u,2) = RCONST(340.0);
1025 printf("-> %g, %g\n", Uin, Uc1m);
1026 fflush(stdout);
1027 if (tc(Uin, Uc1m, u)) {
1028 return 1;
1029 }
1030 printf("<- %g, %g, %g\n", Ith(u,0), Ith(u,1), Ith(u,2));
1031 }
1032 }
1033 N_VDestroy_Serial(u);
1034 return 0;
1035 }
1036 #endif
1037