1/*.......1.........2.........3.........4.........5.........6.........7.........8 2================================================================================ 3 4FILE s_xfer/cfunc.mod 5 6Public Domain 7 8Georgia Tech Research Corporation 9Atlanta, Georgia 30332 10PROJECT A-8503-405 11 12 13AUTHORS 14 15 17 Mar 1991 Jeffrey P. Murray 16 17 18MODIFICATIONS 19 20 18 Apr 1991 Harry Li 21 27 Sept 1991 Jeffrey P. Murray 22 23 24SUMMARY 25 26 This file contains the functional description of the s-domain 27 transfer function (s_xfer) code model. 28 29 30INTERFACES 31 32 FILE ROUTINE CALLED 33 34 CMmacros.h cm_message_send(); 35 36 CM.c void *cm_analog_alloc() 37 void *cm_analog_get_ptr() 38 int cm_analog_integrate() 39 40 41 42 43REFERENCED FILES 44 45 Inputs from and outputs to ARGS structure. 46 47 48NON-STANDARD FEATURES 49 50 NONE 51 52===============================================================================*/ 53 54/*=== INCLUDE FILES ====================*/ 55 56#include <math.h> 57 58 59 60/*=== CONSTANTS ========================*/ 61 62 63 64 65 66/*=== MACROS ===========================*/ 67 68 69 70 71/*=== LOCAL VARIABLES & TYPEDEFS =======*/ 72 73 74 75 76 77/*=== FUNCTION PROTOTYPE DEFINITIONS ===*/ 78 79 80 81 82 83/*============================================================================= 84 85FUNCTION cm_complex_div 86 87AUTHORS 88 89 27 Sept 1991 Jeffrey P. Murray 90 91MODIFICATIONS 92 93 NONE 94 95SUMMARY 96 97 Performs a complex division. 98 99INTERFACES 100 101 FILE ROUTINE CALLED 102 103 N/A N/A 104 105 106RETURNED VALUE 107 108 A Mif_Complex_t value representing the result of the complex division. 109 110GLOBAL VARIABLES 111 112 NONE 113 114NON-STANDARD FEATURES 115 116 NONE 117 118==============================================================================*/ 119#include <stdlib.h> 120 121/*=== Static CM_COMPLEX_DIV ROUTINE ===*/ 122 123/**** Cm_complex_div Function - FAKE ***********/ 124/* */ 125/* Function will not be used in finished */ 126/* system...provides a stub for performing */ 127/* a simple complex division. */ 128/* 12/3/90 JPM */ 129/* */ 130/***********************************************/ 131 132static Mif_Complex_t cm_complex_div(Mif_Complex_t x, Mif_Complex_t y) 133{ 134double mag_x, phase_x, mag_y, phase_y; 135 136Mif_Complex_t out; 137 138mag_x = hypot(x.real, x.imag); 139phase_x = atan2(x.imag, x.real); 140 141mag_y = hypot(y.real, y.imag); 142phase_y = atan2(y.imag, y.real); 143 144mag_x = mag_x/mag_y; 145phase_x = phase_x - phase_y; 146 147out.real = mag_x * cos(phase_x); 148out.imag = mag_x * sin(phase_x); 149 150return out; 151} 152 153 154 155/*============================================================================== 156 157FUNCTION cm_s_xfer() 158 159AUTHORS 160 161 17 Mar 1991 Jeffrey P. Murray 162 163MODIFICATIONS 164 165 18 Apr 1991 Harry Li 166 27 Sept 1991 Jeffrey P. Murray 167 168SUMMARY 169 170 This function implements the s_xfer code model. 171 172INTERFACES 173 174 FILE ROUTINE CALLED 175 176 CMmacros.h cm_message_send(); 177 178 CM.c void *cm_analog_alloc() 179 void *cm_analog_get_ptr() 180 int cm_analog_integrate() 181 182RETURNED VALUE 183 184 Returns inputs and outputs via ARGS structure. 185 186GLOBAL VARIABLES 187 188 NONE 189 190NON-STANDARD FEATURES 191 192 NONE 193 194==============================================================================*/ 195 196/*=== CM_S_XFER ROUTINE ===*/ 197 198/**************************************** 199* S-Domain Transfer Function - * 200* Code Body * 201* * 202* Last Modified - 9/27/91 JPM * 203****************************************/ 204 205void cm_s_xfer(ARGS) /* structure holding parms, inputs, outputs, etc. */ 206{ 207 double *out; /* pointer to the output */ 208 double *in; /* pointer to the input */ 209 double in_offset; /* input offset */ 210 double *gain; /* pointer to the gain */ 211 double **den_coefficient; /* dynamic array that holds the denominator 212 coefficients */ 213 double **old_den_coefficient;/* dynamic array that holds the old 214 denonminator coefficients */ 215 double **num_coefficient; /* dynamic array that holds the numerator 216 coefficients */ 217 double **old_num_coefficient;/* dynamic array that holds the old numerator 218 coefficients */ 219 double factor; /* gain factor in case the highest 220 denominator coefficient is not 1 */ 221 double **integrator; /* outputs of the integrators */ 222 double **old_integrator; /* previous integrator outputs */ 223 double null; /* dummy pointer for use with the 224 integrate function */ 225 double pout_pin; /* partial out wrt in */ 226 /*double total_gain;*/ /* not used, currently-used with ITP stuff */ 227 double temp; /* temporary variable used with the 228 correct type of AC value */ 229 double frac; /* holds fractional part of a divide */ 230 double divide_integer; /* integer part of a modf used in AC */ 231 double denormalized_freq; /* denormalization constant...the nominal 232 corner or center frequencies specified 233 by the model coefficients will be 234 denormalized by this amount. Thus, if 235 coefficients were obtained which specified 236 a 1 rad/sec cornere frequency, specifying 237 a value of 1000.0 for denormalized_freq 238 will cause the model to shift the corner 239 freq. to 2.0 * pi * 1000.0 */ 240 double *old_gain; /* pointer to the gain if the highest order 241 denominator coefficient is not factored out */ 242 243 Mif_Complex_t ac_gain, acc_num, acc_den; 244 245 int i; /* generic loop counter index */ 246 int den_size; /* size of the denominator coefficient array */ 247 int num_size; /* size of the numerator coefficient array */ 248 249 char *num_size_error="\n***ERROR***\nS_XFER: Numerator coefficient array size greater than\ndenominator coefficiant array size.\n"; 250 251 252 253 /** Retrieve frequently used parameters (used by all analyses)... **/ 254 255 in_offset = PARAM(in_offset); 256 num_size = PARAM_SIZE(num_coeff); 257 den_size = PARAM_SIZE(den_coeff); 258 if ( PARAM_NULL(denormalized_freq) ) { 259 denormalized_freq = 1.0; 260 } 261 else { 262 denormalized_freq = PARAM(denormalized_freq); 263 } 264 265 if ( num_size > den_size ) { 266 cm_message_send(num_size_error); 267 return; 268 } 269 270 /** Test for INIT; if so, allocate storage, otherwise, retrieve previous **/ 271 /** timepoint input values as necessary in subsequent analysis sections... **/ 272 273 if (INIT==1) { /* First pass...allocate storage for previous values... */ 274 275 /* Allocate rotational storage for integrator outputs, in & out */ 276 277 278/***** The following two lines may be unnecessary in the final version *****/ 279 280/* We have to allocate memory and use cm_analog_alloc, because the ITP variables 281 are not functional */ 282 283 integrator = (double **) calloc((size_t) den_size, sizeof(double *)); 284 old_integrator = (double **) calloc((size_t) den_size, sizeof(double *)); 285 286 /* Allocate storage for coefficient values */ 287 288 den_coefficient = (double **) calloc((size_t) den_size, sizeof(double *)); 289 old_den_coefficient = (double **) calloc((size_t) den_size, sizeof(double *)); 290 291 num_coefficient = (double **) calloc((size_t) num_size, sizeof(double *)); 292 old_num_coefficient = (double **) calloc((size_t) num_size, sizeof(double *)); 293 294 for (i=0; i < (2*den_size + num_size + 3); i++) 295 cm_analog_alloc(i,sizeof(double)); 296 297 /* ITP_VAR_SIZE(den) = den_size; */ 298 299 /* gain = (double *) calloc(1,sizeof(double)); 300 ITP_VAR(total_gain) = gain; 301 ITP_VAR_SIZE(total_gain) = 1.0; */ 302 303 // Retrieve pointers 304 305 for (i=0; i<den_size; i++) { 306 integrator[i] = (double *) cm_analog_get_ptr(i,0); 307 old_integrator[i] = (double *) cm_analog_get_ptr(i,0); 308 } 309 310 for(i=den_size;i<2*den_size;i++) { 311 den_coefficient[i-den_size] = (double *) cm_analog_get_ptr(i,0); 312 old_den_coefficient[i-den_size] = (double *) cm_analog_get_ptr(i,0); 313 } 314 315 for(i=2*den_size;i<2*den_size+num_size;i++) { 316 num_coefficient[i-2*den_size] = (double *) cm_analog_get_ptr(i,0); 317 old_num_coefficient[i-2*den_size] = (double *) cm_analog_get_ptr(i,0); 318 } 319 320 out = (double *) cm_analog_get_ptr(2*den_size+num_size, 0); 321 in = (double *) cm_analog_get_ptr(2*den_size+num_size+1, 0); 322 323 gain = (double *) cm_analog_get_ptr(2*den_size+num_size+2,0); 324 325 }else { /* Allocation was not necessary...retrieve previous values */ 326 327 /* Set pointers to storage locations for in, out, and integrators...*/ 328 329 integrator = (double **) calloc((size_t) den_size, sizeof(double *)); 330 old_integrator = (double **) calloc((size_t) den_size, sizeof(double *)); 331 332 for (i=0; i<den_size; i++) { 333 integrator[i] = (double *) cm_analog_get_ptr(i,0); 334 old_integrator[i] = (double *) cm_analog_get_ptr(i,1); 335 336 } 337 out = (double *) cm_analog_get_ptr(2*den_size+num_size,0); 338 in = (double *) cm_analog_get_ptr(2*den_size+num_size+1,0); 339 340 341 /* Set den_coefficient & gain pointers to ITP values */ 342 /* for denominator coefficients & gain... */ 343 344 old_den_coefficient = (double **) calloc((size_t) den_size, sizeof(double)); 345 den_coefficient = (double **) calloc((size_t) den_size, sizeof(double)); 346 347 for(i=den_size;i<2*den_size;i++){ 348 old_den_coefficient[i-den_size] = (double *) cm_analog_get_ptr(i,1); 349 den_coefficient[i-den_size] = (double *) cm_analog_get_ptr(i,0); 350 *(den_coefficient[i-den_size]) = *(old_den_coefficient[i-den_size]); 351 } 352 353 num_coefficient = (double **) calloc((size_t) num_size, sizeof(double)); 354 old_num_coefficient = (double **) calloc((size_t) num_size, sizeof(double)); 355 356 for(i=2*den_size;i<2*den_size+num_size;i++){ 357 old_num_coefficient[i-2*den_size] = (double *) cm_analog_get_ptr(i,1); 358 num_coefficient[i-2*den_size] = (double *) cm_analog_get_ptr(i,0); 359 *(num_coefficient[i-2*den_size]) = *(old_num_coefficient[i-2*den_size]); 360 } 361 362 /* gain has to be stored each time since it could possibly change 363 if the highest order denominator coefficient isn't zero. This 364 is a hack until the ITP variables work */ 365 366 old_gain = (double *) cm_analog_get_ptr(2*den_size+num_size+2,1); 367 gain = (double *) cm_analog_get_ptr(2*den_size+num_size+2,0); 368 369 *gain = *old_gain; 370 371 /* gain = ITP_VAR(total_gain); */ 372 373 } 374 375 376 /** Test for TIME=0.0; if so, initialize... **/ 377 378 if (TIME == 0.0) { /* First pass...set initial conditions... */ 379 380 /* Initialize integrators to int_ic condition values... */ 381 for (i=0; i<den_size-1; i++) { /* Note...do NOT set the highest */ 382 /* order value...this represents */ 383 /* the "calculated" input to the */ 384 /* actual highest integrator...it */ 385 /* is NOT a true state variable. */ 386 if ( PARAM_NULL(int_ic) ) { 387 // *(integrator[i]) = *(old_integrator[i]) = PARAM(int_ic[0]); 388 *(integrator[i]) = *(old_integrator[i]) = 0; 389 } 390 else { 391 *(integrator[i]) = *(old_integrator[i]) = 392 PARAM(int_ic[den_size - 2 - i]); 393 } 394 } 395 396 397 /*** Read in coefficients and denormalize, if required ***/ 398 399 for (i=0; i<num_size; i++) { 400 *(num_coefficient[i]) = PARAM(num_coeff[num_size - 1 - i]); 401 if ( denormalized_freq != 1.0 ) { 402 *(num_coefficient[i]) = *(num_coefficient[i]) / 403 pow(denormalized_freq,(double) i); 404 } 405 } 406 407 for (i=0; i<den_size; i++) { 408 *(den_coefficient[i]) = PARAM(den_coeff[den_size - 1 - i]); 409 if ( denormalized_freq != 1.0 ) { 410 *(den_coefficient[i]) = *(den_coefficient[i]) / 411 pow(denormalized_freq,(double) i); 412 } 413 } 414 415 416 417 /* Test denominator highest order coefficient...if that value */ 418 /* is other than 1.0, then divide all denominator coefficients */ 419 /* and the gain by that value... */ 420 // if ( (factor = PARAM(den_coeff[den_size-1])) != 1.0 ) { 421 if ( (factor = *den_coefficient[den_size-1]) != 1.0 ) { 422 for (i=0; i<den_size; i++) { 423 *(den_coefficient[i]) = *(den_coefficient[i]) / factor; 424 } 425 *gain = PARAM(gain) / factor; 426 } 427 else { /* No division by coefficient necessary... */ 428 /* only need to adjust gain value. */ 429 *gain = PARAM(gain); 430 } 431 432 } 433 434 435 /**** DC & Transient Analyses **************************/ 436 if (ANALYSIS != MIF_AC) { 437 438 /**** DC Analysis - Not needed JPM 10/29/91 *****************/ 439/* if (ANALYSIS == MIF_DC) { 440 441 ?* Test to see if a term exists for the zero-th order 442 denom coeff... 443 444 ?* division by zero if output 445 ?* num_coefficient[0]/den_coefficient[0], 446 ?* so output init. conds. instead... 447 if ( 0.0 == *(den_coefficient[0])) { 448 449 450 *out = 0.0; 451 for (i=0; i<num_size; i++) { 452 *out = *out + ( *(old_integrator[i]) * 453 *(num_coefficient[i]) ); 454 } 455 *out = *gain * *out; 456 pout_pin = *(old_integrator[1]); 457 458 } 459 460 ?* Zero-th order den term != 0.0, so output 461 ?* num_coeff[0]/den_coeff[0]... 462 else { 463 464 *out = *gain * ( INPUT(in) + 465 in_offset) * ( *(num_coefficient[0]) / 466 *(den_coefficient[0]) ); 467 pout_pin = 0.0; 468 } 469 } 470 471 472 473 else { 474*/ 475 476 477 /**** Transient & DC Analyses ****************************/ 478 479 /*** Read input value for current time, and 480 calculate pseudo-input which includes input 481 offset and gain.... ***/ 482 483 *in = *gain * (INPUT(in)+in_offset); 484 485 486 487 /*** Obtain the "new" input to the Controller 488 Canonical topology, then propagate through 489 the integrators.... ***/ 490 491 /* calculate the "new" input to the first integrator, based on */ 492 /* the old values of each integrator multiplied by their */ 493 /* respective denominator coefficients and then subtracted */ 494 /* from *in.... */ 495 /* Note that this value, which is similar to a state variable, */ 496 /* is stored in *(integrator[den_size-1]). */ 497 498 *(integrator[den_size-1]) = *in; 499 for (i=0; i<den_size-1; i++) { 500 *(integrator[den_size-1]) = 501 *(integrator[den_size-1]) - 502 *(old_integrator[i]) * *(den_coefficient[i]); 503 } 504 505 506 507 508 /* Propagate the new input through each integrator in succession. */ 509 510 for (i=den_size-1; i>0; i--) { 511 cm_analog_integrate(*(integrator[i]),(integrator[i-1]),&null); 512 } 513 514 515 516 /* Calculate the output based on the new integrator values... */ 517 518 *out = 0.0; 519 for (i=0; i<num_size; i++) { 520 *out = *out + ( *(integrator[i]) * 521 *(num_coefficient[i]) ); 522 } 523 pout_pin = *(integrator[1]); 524 525 526 /** Output values for DC & Transient **/ 527 528 OUTPUT(out) = *out; 529 PARTIAL(out,in) = pout_pin; 530 // cm_analog_auto_partial(); // Removed again. Seems to have problems. 531 532 } 533 534 /**** AC Analysis ************************************/ 535 else { 536 537 /*** Calculate Real & Imaginary portions of AC gain ***/ 538 /*** at the current RAD_FREQ point... ***/ 539 540 541 /*** Calculate Numerator Real & Imaginary Components... ***/ 542 543 acc_num.real = 0.0; 544 acc_num.imag = 0.0; 545 546 for (i=0; i<num_size; i++) { 547 frac = modf(i/2.0, ÷_integer); /* Determine the integer portion */ 548 /* of a divide-by-2.0 on the index. */ 549 550 if (modf(divide_integer/2.0,&temp) > 0.0 ) { /* Negative coefficient */ 551 /* values for this iteration. */ 552 if (frac > 0.0 ) { /** Odd Powers of "s" **/ 553 acc_num.imag = acc_num.imag - *(num_coefficient[i]) * pow(RAD_FREQ,i) * (*gain); 554 } 555 else { /** Even Powers of "s" **/ 556 acc_num.real = acc_num.real - *(num_coefficient[i]) * pow(RAD_FREQ,i) * (*gain); 557 } 558 } 559 else { /* Positive coefficient values for this iteration */ 560 if (frac> 0.0 ) { /** Odd Powers of "s" **/ 561 acc_num.imag = acc_num.imag + *(num_coefficient[i]) * pow(RAD_FREQ,i) * (*gain); 562 } 563 else { /** Even Powers of "s" **/ 564 acc_num.real = acc_num.real + *(num_coefficient[i]) * pow(RAD_FREQ,i) * (*gain); 565 } 566 } 567 } 568 569 /*** Calculate Denominator Real & Imaginary Components... ***/ 570 571 acc_den.real = 0.0; 572 acc_den.imag = 0.0; 573 574 for (i=0; i<den_size; i++) { 575 frac = modf(i/2.0, ÷_integer); /* Determine the integer portion */ 576 /* of a divide-by-2.0 on the index. */ 577 if (modf(divide_integer/2.0,&temp) > 0.0 ) { /* Negative coefficient */ 578 /* values for this iteration. */ 579 if (frac > 0.0 ) { /** Odd Powers of "s" **/ 580 acc_den.imag = acc_den.imag - *(den_coefficient[i]) * pow(RAD_FREQ,i); 581 } 582 else { /** Even Powers of "s" **/ 583 acc_den.real = acc_den.real - *(den_coefficient[i]) * pow(RAD_FREQ,i); 584 } 585 } 586 else { /* Positive coefficient values for this iteration */ 587 if (frac > 0.0 ) { /** Odd Powers of "s" **/ 588 acc_den.imag = acc_den.imag + *(den_coefficient[i]) * pow(RAD_FREQ,i); 589 } 590 else { /** Even Powers of "s" **/ 591 acc_den.real = acc_den.real + *(den_coefficient[i]) * pow(RAD_FREQ,i); 592 } 593 } 594 } 595 596 /* divide numerator values by denominator values */ 597 598 ac_gain = cm_complex_div(acc_num, acc_den); 599 600 AC_GAIN(out,in) = ac_gain; 601 } 602 603 /* free all allocated memory */ 604 if(integrator) free(integrator); 605 if(old_integrator) free(old_integrator); 606 if(den_coefficient) free(den_coefficient); 607 if(old_den_coefficient) free(old_den_coefficient); 608 if(num_coefficient) free(num_coefficient); 609 if(old_num_coefficient) free(old_num_coefficient); 610} 611 612 613 614 615 616 617 618