home *** CD-ROM | disk | FTP | other *** search
/ Peanuts NeXT Software Archives / Peanuts-2.iso / Developer / hardware / dsp / drbub / analog / pzsim.c < prev    next >
Encoding:
C/C++ Source or Header  |  1991-09-07  |  17.3 KB  |  579 lines
  1. /**********************************************************************
  2. *       This program calculates the magnitude of a transfer function
  3. *       given all the pole and zero locations.  A scalar constant
  4. *       K can also be entered. 
  5. *                                               Steven Punte
  6. *                                               Sept. 24 1987
  7. ***********************************************************************/
  8.  
  9. #include        <stdio.h>
  10. #include        <math.h>
  11. #define         TRUE            1
  12. #define         FALSE           0
  13. #define         MAX_ARRAY       50      /* Max number of poles or zeros */
  14. #define         PIE             3.141592653
  15. #define         STR_LNGTH       20
  16. #define         scanf           my_scanf
  17.  
  18. #define         abs(X)          ( X > 0 ? X : -X )
  19. #define         sqr(X)          ( X )*( X )
  20. #define         scal(X, Y)      while( abs( X ) > 1000) { X *= 0.0001; Y += 4; } \
  21.                                 while( abs( X ) < 0.0001) { X *= 10000; Y -= 4; }       
  22. #define         clear(X)        X.real = 0; X.imag = 0
  23. #define         cmov(X, Y)      Y.real = X.real; Y.imag = X.imag
  24. #define         cset(X, Y, Z)   X.real = Y; X.imag = Z
  25. #define         cmpl(X)         X.imag = - X.imag
  26. #define         cadd(X, Y, Z)   Z.real = X.real + Y.real;\
  27.                                 Z.imag = X.imag + Y.imag
  28. #define         csub(X, Y, Z)   Z.real = X.real - Y.real;\
  29.                                 Z.imag = X.imag - Y.imag
  30. #define         cmul(X, Y, Z)   { \
  31.                                 struct cmplx_num tmp; \
  32.                                 tmp.real = X.real * Y.real - X.imag * Y.imag ; \
  33.                                 tmp.imag = X.real * Y.imag + X.imag * Y.real ; \
  34.                                 Z.real = tmp.real; \
  35.                                 Z.imag = tmp.imag; \
  36.                                 }
  37. #define         cdiv(X, Y, Z)   { \
  38.                                 struct cmplx_num tmp; \
  39.                                 double den; \
  40.                                 den = sqr( Y.real ) + sqr( Y.imag ); \
  41.                                 tmp.real = X.real * Y.real + X.imag * Y.imag ; \
  42.                                 tmp.imag = X.imag * Y.real - X.real * Y.imag ; \
  43.                                 Z.imag = tmp.imag / den; \
  44.                                 Z.real = tmp.real / den; \
  45.                                 }
  46. #define         cmag(X)         sqrt( sqr( X.real) + sqr( X.imag))
  47.                                 
  48.  
  49. struct cmplx_num        {
  50.                         double real;
  51.                         double imag;
  52.                         };
  53.  
  54. int flag_S      =       FALSE;
  55. int flag_E      =       FALSE;
  56. int flag_PT     =       FALSE;
  57. int flag_LOG    =       FALSE;
  58. int flag_RADS   =       FALSE;
  59. int flag_PHASE  =       FALSE;
  60. int flag_STEP   =       FALSE;
  61.  
  62. double start;   /* Starting Frequency */
  63. double endd;    /* Ending Frequency */
  64. int points;             /* Number of partitions */
  65. double K_coeff = 1.0;   /* Front Scaler coeffficient (1.0 default value) */
  66. int K_exp = 0;
  67.  
  68. int numb_poles = 0;     /* Number of poles (either real or complex) */
  69. int numb_zeros = 0;     /* Number of zeros (either real or complex) */
  70.  
  71. int i;                  /* Index for summation calculations */
  72. char str[ STR_LNGTH ] ; /* String buffer for input reading */
  73. int data_set    = 0;    /* Number of sets of data to be processed */
  74. double ctemp;           /* Used for complex divide */
  75.  
  76. struct cmplx_num poles[ MAX_ARRAY ];
  77. struct cmplx_num zeros[ MAX_ARRAY ];
  78. struct {
  79.         double x[500];
  80.         double y[500];
  81.         int numb_point;
  82.         char name[ STR_LNGTH ];
  83.         } output[ 5 ];
  84.  
  85. main()
  86. {
  87. int done_chk;
  88.  
  89. do {
  90.         done_chk = read_set();
  91.         check_input();
  92.  
  93.         if( flag_STEP == TRUE ) step_response();
  94.         else transfer_respones();
  95.  
  96.         data_set++;
  97.         } while( done_chk != EOF );
  98.  
  99. display();
  100.  
  101. } /* End of main */
  102.  
  103.  
  104.  
  105. transfer_respones()
  106. {
  107. double delta;           /* Frequency increment in hertz */
  108. double x;               /* Frequency being analysized in hertz */
  109. double ampl;
  110. double angle;
  111. double gamma;
  112. int pnt;
  113. struct cmplx_num cgamma, tmp;
  114. struct cmplx_num  K_scale;
  115. double scale_data();
  116.  
  117. gamma = scale_data();
  118. cset( K_scale, K_coeff, 0);
  119.  
  120. delta = (endd - start) / points;
  121.  
  122. /* Main loop; Calculates magnitude at each frequency */
  123.  
  124. pnt = 0;
  125. for( x = start; x < endd ; x += delta ) {
  126.  
  127.         struct cmplx_num  numerator;    /* Numerator of magnitude calculation */
  128.         struct cmplx_num  denominator;  /* Denominator of mangitude calculation */
  129.         struct cmplx_num  magnitude;    /* Final resulting magnitude */
  130.         struct cmplx_num  temp1, temp2;
  131.         struct cmplx_num  omega;
  132.  
  133.         if( flag_RADS == FALSE ) { cset( omega, 0, (2*PIE*x / gamma ) ); }
  134.         else { cset( omega, 0, ( x / gamma )); }
  135.  
  136.         cset( numerator, 1, 0 );
  137.         cset( denominator, 1, 0 );
  138.         
  139.         /* Main calculations here */
  140.         for( i = 0; i < numb_zeros ; i++ ) {
  141.                 csub( omega, zeros[ i ], temp1 );
  142.                 cmul( temp1, numerator, numerator); 
  143.                 }
  144.         for( i = 0; i < numb_poles ; i++ ) { 
  145.                 csub( omega, poles[ i ], temp1 );
  146.                 cmul( temp1, denominator, denominator); 
  147.                 }
  148.  
  149.         /* In case of underflow */
  150.         if( cmag( denominator) == 0.0 ) {
  151.                 cset( magnitude, 100000000000000.0, 0.0 );
  152.                 }
  153.         else {
  154.                 cdiv( numerator, denominator, magnitude );
  155.                 cmul( magnitude, K_scale, magnitude );
  156.                 }
  157.  
  158.         if( flag_PHASE == TRUE ) {
  159.                 angle = 180 / PIE * atan2( magnitude.imag, magnitude.real );
  160. /*
  161.                 printf( "%15.5f  %15.5f \n", x, angle );
  162. */
  163.                 output[ data_set ].x[ pnt ] = x;
  164.                 output[ data_set ].y[ pnt ] = angle;
  165.                 pnt++;
  166.  
  167.                 }
  168.  
  169.         else {
  170.                 ampl = cmag( magnitude );
  171.  
  172.                 if( flag_LOG == TRUE ) {
  173.                         if( ampl == 0.0 ) ampl = -1000; 
  174.                         else ampl = -20 * log10( ampl );
  175.                         }
  176.         
  177. /*
  178.                 printf( "%15.5f  %15.5f \n", x, ampl );
  179. */
  180.                 output[ data_set ].x[ pnt ] = x;
  181.                 output[ data_set ].y[ pnt ] = ampl;
  182.                 pnt++;
  183.                 }
  184.         }
  185. if( flag_PHASE == TRUE ) strcpy( output[ data_set ].name, "Phase" );
  186. else strcpy( output[ data_set ].name, "Magnitude" );
  187.  
  188. output[ data_set ].numb_point = pnt;
  189.  
  190. } /* End of transfer response */
  191.  
  192. step_response()
  193. {
  194. double delta;           /* Frequency increment in hertz */
  195. double x;               /* Frequency being analysized in hertz */
  196. double gamma ;
  197. struct cmplx_num C[ MAX_ARRAY ];
  198. int k;
  199. int pnt;
  200.  
  201. gamma = scale_data();
  202.  
  203. for( k = 0; k < numb_poles ; k++) {
  204.         struct cmplx_num  numerator;    /* Numerator of magnitude calculation */
  205.         struct cmplx_num  denominator;  /* Denominator of mangitude calculation */
  206.         struct cmplx_num  magnitude;    /* Final resulting magnitude */
  207.         struct cmplx_num  temp1, temp2;
  208.  
  209.         cset( numerator, K_coeff, 0 );
  210.         cset( denominator, 1, 0);
  211.  
  212.         for( i = 0; i < numb_zeros ; i++ ) {
  213.                 csub( poles[k], zeros[i], temp1 );
  214.                 cmul( temp1, numerator, numerator );
  215.                 }
  216.  
  217.         for( i = 0; i < numb_poles ; i++ ) {
  218.                 if( i != k) {
  219.                         csub( poles[k], poles[i], temp1 );
  220.                         cmul( temp1, denominator, denominator );
  221.                         }
  222.                 }
  223.         cdiv( numerator, denominator, C[k]);
  224.         }
  225.         
  226. delta = (endd - start) / points;
  227.  
  228. /* Main loop; Calculates magnitude at each frequency */
  229.  
  230. for( x = start; x < endd ; x += delta ) {
  231.  
  232.         struct cmplx_num  temp1, temp2, temp3, sum;
  233.         cset( sum, 0, 0);
  234.  
  235.         for( k = 0; k < numb_poles ; k++) {
  236.  
  237.                 cset( temp1, ( x * gamma ), 0);
  238.                 cmul( poles[k], temp1, temp2);
  239.                 cset( temp1, cos( temp2.imag), sin( temp2.imag) );
  240.                 cset( temp2, exp( temp2.real), 0);
  241.                 cmul( temp1, temp2, temp3 );
  242.                 cset( temp1, 1, 0);
  243.                 csub( temp3, temp1, temp2);
  244.                 cmul( temp2, C[k], temp3);
  245.                 cdiv( temp3, poles[k], temp1);
  246.  
  247.                 cadd( sum, temp1, temp2);
  248.                 cmov( temp2, sum);
  249.  
  250.                 }
  251.         /*
  252.         printf( "%15.5f  %15.5f  %f15.5 \n", x, sum.real, sum.imag );
  253.         printf( "%15.5f  %15.5f  \n", x, sum.real);
  254.         */
  255.         output[ data_set ].x[ pnt ] = x;
  256.         output[ data_set ].y[ pnt ] = sum.real;
  257.         pnt++;
  258.         }
  259. strcpy( output[ data_set ].name, "Step" );
  260. output[ data_set ].numb_point = pnt;
  261. }
  262.  
  263. /* Stack overflow error routing and exit */
  264. overflow_error()
  265. {
  266.         fprintf( stderr, "Internal Array Size has been exceeded \n");
  267.         fprintf( stderr, "Recompile with larger \"MAX_ARRAY\" size. \n");
  268.         fprintf( stderr, "Fatal Error \n");
  269.         exit(1);
  270. }
  271.  
  272.  
  273.  
  274. /* Loops over all input lines, and enters data into proper variables */
  275. read_set()
  276. {
  277. double scan_modifier();
  278. int expon;      /* The tens power exponent of the metric scale factor */
  279. *str = EOF;
  280.  
  281. flag_LOG        =       FALSE;
  282. flag_RADS       =       FALSE;
  283. flag_PHASE      =       FALSE;
  284. flag_STEP       =       FALSE;
  285. K_coeff = 1.0;  /* Front Scaler coeffficient (1.0 default value) */
  286.  
  287. numb_poles = 0; /* Number of poles (either real or complex) */
  288. numb_zeros = 0; /* Number of poles (either real or complex) */
  289.  
  290. while( (scanf( "%s", str) != EOF) && ( *str != '#') ) {
  291.  
  292.         if( !strcmp( str, "FS") || !strcmp( str, "TS") || !strcmp( str, "S" )) {
  293.                 scanf( "%f", &start);
  294.                 start *= scan_modifier( stdin, &expon );
  295.                 flag_S = TRUE;
  296.                 }
  297.  
  298.         else if( !strcmp( str, "FE") || !strcmp( str, "TE") || !strcmp( str, "E" )) {
  299.                 scanf( "%f", &endd);
  300.                 endd *= scan_modifier( stdin, &expon );
  301.                 flag_E = TRUE;
  302.                 }
  303.  
  304.         else if( !strcmp( str, "FP") || !strcmp( str, "TP") || !strcmp( str, "PT" )) {
  305.                 scanf( "%d", &points);
  306.                 points *= (int)scan_modifier( stdin, &expon );
  307.                 flag_PT = TRUE;
  308.                 }
  309.  
  310.         else if( !strcmp( str, "K") ) {
  311.                 scanf( "%f", &K_coeff);
  312.                 scan_modifier( stdin, &expon );
  313.                 K_exp = expon;
  314.                 }
  315.  
  316.         else if( !strcmp( str, "P") ) {
  317.                 if( numb_poles >= MAX_ARRAY) overflow_error();
  318.                 scanf( "%f", &poles[ numb_poles ].real);
  319.                 poles[ numb_poles ].real *= scan_modifier( stdin, &expon );
  320.                 poles[ numb_poles++ ].imag = 0.0;
  321.                 }
  322.  
  323.         else if( !strcmp( str, "Z") ) {
  324.                 if( numb_poles >= MAX_ARRAY) overflow_error();
  325.                 scanf( "%f", &zeros[ numb_zeros ].real);
  326.                 zeros[ numb_zeros ].real *= scan_modifier( stdin, &expon );
  327.                 zeros[ numb_zeros++ ].imag = 0.0;
  328.                 }
  329.  
  330.         else if( !strcmp( str, "PC") ) {
  331.                 double real, imag, temp;
  332.                 if( numb_poles >= MAX_ARRAY - 1) overflow_error();
  333.                 /* Assumes as complex conjugate pair */
  334.                 scanf( "%f", &real);
  335.                 scanf( "%f", &imag);
  336.                 temp = scan_modifier( stdin, &expon );
  337.                 real *= temp;
  338.                 imag *= temp;
  339.                 poles[ numb_poles ].real = real;
  340.                 poles[ numb_poles++ ].imag = imag;
  341.                 /* Also enter complex pair */
  342.                 poles[ numb_poles ].real = real;
  343.                 poles[ numb_poles++ ].imag = - imag;
  344.                 }
  345.  
  346.         else if( !strcmp( str, "ZC") ) {
  347.                 double real, imag, temp;
  348.                 if( numb_poles >= MAX_ARRAY - 1) overflow_error();
  349.                 /* Assumes as complex conjugate pair */
  350.                 scanf( "%f", &real);
  351.                 scanf( "%f", &imag);
  352.                 temp = scan_modifier( stdin, &expon );
  353.                 real *= temp;
  354.                 imag *= temp;
  355.                 zeros[ numb_zeros ].real = real;
  356.                 zeros[ numb_zeros++ ].imag = imag;
  357.                 /* Also enter complex pair */
  358.                 zeros[ numb_zeros ].real = real;
  359.                 zeros[ numb_zeros++ ].imag = - imag;
  360.                 }
  361.  
  362.         else if( !strcmp( str, "LOG") ) {
  363.                 scan_modifier( stdin, &expon );
  364.                 flag_LOG = TRUE;
  365.                 }
  366.  
  367.         else if( !strcmp( str, "RADS") ) {
  368.                 scan_modifier( stdin, &expon );
  369.                 flag_RADS = TRUE;
  370.                 }
  371.  
  372.         else if( !strcmp( str, "PHASE") ) {
  373.                 scan_modifier( stdin, &expon );
  374.                 flag_PHASE = TRUE;
  375.                 }
  376.  
  377.         else if( !strcmp( str, "STEP") ) {
  378.                 scan_modifier( stdin, &expon );
  379.                 flag_STEP = TRUE;
  380.                 }
  381.  
  382.         else {
  383.                 fprintf( stderr, "Syntax Error, ");
  384.                 fprintf( stderr, "Unrecognized input string \"%s\". ", str );
  385.                 fprintf( stderr, "String ignored. \n");
  386.                 }
  387.         }
  388.  
  389. if( *str != '#' ) *str = EOF ;
  390. return( *str );
  391. } /* End of read subroutine */
  392.  
  393. /* Checks if essential input informaiton is present */
  394. check_input()
  395. {
  396. if( flag_S == FALSE ) {
  397.         fprintf( stderr, "Starting point unspecified! " );
  398.         fprintf( stderr, "Fatal Error. \n");
  399.         exit(1);
  400.         }
  401.  
  402. if( flag_E == FALSE ) {
  403.         fprintf( stderr, "Ending point unspecified! " );
  404.         fprintf( stderr, "Fatal Error. \n");
  405.         exit(1);
  406.         }
  407.  
  408. if( flag_PT == FALSE ) {
  409.         fprintf( stderr, "Number of data points unspecified! \n" );
  410.         fprintf( stderr, "Default of 20 will be used. \n");
  411.         points = 20;
  412.         }
  413. }
  414.  
  415.  
  416. /* Looks for some type of value modifie, such as kilo (x1000)
  417. *  or micro (x0.000001).  Then will be expecting a colon as endd
  418. *  of line marker.  Returns a double persion number for
  419. *  scaling purposes.    
  420. */
  421.  
  422. double scan_modifier( input, expon )
  423. FILE *input;
  424. int *expon;
  425. {
  426. double scale = 1.0;
  427. char string[ STR_LNGTH ];
  428.  
  429. while( (fscanf( input, "%s", string) != EOF) && (*string != ';' ) ) {
  430.  
  431.         switch( *string) {
  432.  
  433.         case 'K':
  434.                 scale = 1000;
  435.                 *expon = 3;
  436.                 break;
  437.  
  438.         case 'M':
  439.                 scale = 1000000;
  440.                 *expon = 6;
  441.                 break;
  442.  
  443.         case 'G':
  444.                 scale = 1000000000;
  445.                 *expon = 9;
  446.                 break;
  447.  
  448.         case 'T':
  449.                 scale = 1000000000000;
  450.                 *expon = 12;
  451.                 break;
  452.  
  453.         case 'm':
  454.                 scale = 0.001;
  455.                 *expon = -3;
  456.                 break;
  457.  
  458.         case 'u':
  459.                 scale = 0.000001;
  460.                 *expon = -6;
  461.                 break;
  462.  
  463.         case 'n':
  464.                 scale = 0.000000001;
  465.                 *expon = -9;
  466.                 break;
  467.  
  468.         case 'p':
  469.                 scale = 0.000000000001;
  470.                 *expon = -12;
  471.                 break;
  472.  
  473.         case 'E':
  474.                 {
  475.                 int K;
  476.                 fscanf( input, "%d", &K);
  477.                 *expon = K;
  478.                 if( K > 30 ) K = 30;
  479.                 if( K < -30) K = -30;
  480.                 while( K-- > 0 ) scale *= 10;
  481.                 while( K++ < 0 ) scale *= 0.1;
  482.                 }
  483.                 break;
  484.  
  485.         default:
  486.                 fprintf( stderr, "Unrecognized string \"%s\" \n", string);
  487.                 fprintf( stderr, "Metric modifying unit expected. \n" );
  488.                 fprintf( stderr, "String Ignored. \n");
  489.                 
  490.  
  491.                 }
  492.         }
  493. return( scale );
  494. }
  495.  
  496. display()
  497. {
  498. int i, j;
  499.  
  500. /*
  501. printf("%d \n", data_set);
  502. */
  503.  
  504. for(i = 0; i < data_set ; i++ ) {
  505.         printf( "%d %s \n", output[i].numb_point, output[i].name );
  506.         printf( "%d \n", output[i].numb_point);
  507.         for(j = 0; j < output[ i ].numb_point ; j++) {
  508.                 printf( "%15f, %15f \n", output[i].x[j], output[i].y[j]);
  509.                 }
  510.         }
  511. }
  512.  
  513.  
  514. double scale_data()
  515. {
  516. double net_mean ;
  517. int mean_order  ;
  518. double gamma, mag;
  519. struct cmplx_num cgamma;
  520.  
  521. net_mean        = 0.0;
  522. mean_order      = 0;
  523.  
  524. /* Find geometric mean of all non zeros and poles */
  525. for( i = 0; i < numb_zeros ; i++ ) {
  526.         mag = cmag( zeros[i] );
  527.         if( mag > 0 ) {
  528.                 net_mean += log( mag);
  529.                 mean_order++;
  530.                 }
  531.         }
  532. for( i = 0; i < numb_poles ; i++ ) {
  533.         mag = cmag( poles[i] );
  534.         if( mag > 0 ) {
  535.                 net_mean += log( mag);
  536.                 mean_order++;
  537.                 }
  538.         }
  539. gamma = exp( net_mean / (double)mean_order );
  540.  
  541. /* Set complex scaling constant, and scale all poles and zeros */
  542. cset( cgamma, (1 / gamma), 0);
  543.  
  544. for( i = 0; i < numb_zeros ; i++ ) {
  545.         cmul( zeros[i], cgamma, zeros[i]);
  546.         }
  547. for( i = 0; i < numb_poles ; i++ ) {
  548.         cmul( poles[i], cgamma, poles[i]);
  549.         }
  550.  
  551. /* Rescale K_coeff, note: care for overflow must be observed */
  552. K_coeff = exp( (double)(numb_zeros - numb_poles)*log( gamma ) + log( K_coeff ) + 2.30258509 * (double)K_exp );
  553.  
  554. return( gamma );
  555. } /* End of data scale routine */
  556.  
  557.  
  558. #undef          scanf
  559. /* A lousy fucking patch for micro-soft C.  It doesn't
  560. *  seem to read floating point numbers properly.      
  561. */
  562. my_scanf( str, reg)
  563. char *str;
  564. union {
  565.         int x;
  566.         double y;
  567.         char t;
  568.         } *reg;
  569.         
  570. {
  571. double atof();
  572. if( !strcmp( str, "%f") ) {
  573.         char temp[40];
  574.         scanf( "%s", temp);
  575.         reg->y = atof( temp);
  576.         }
  577. else scanf( str, reg);
  578. }
  579.