You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
618 lines
20 KiB
618 lines
20 KiB
/* $Id$ */
|
|
/*.......1.........2.........3.........4.........5.........6.........7.........8
|
|
================================================================================
|
|
|
|
FILE s_xfer/cfunc.mod
|
|
|
|
Copyright 1991
|
|
Georgia Tech Research Corporation, Atlanta, Ga. 30332
|
|
All Rights Reserved
|
|
|
|
PROJECT A-8503-405
|
|
|
|
|
|
AUTHORS
|
|
|
|
17 Mar 1991 Jeffrey P. Murray
|
|
|
|
|
|
MODIFICATIONS
|
|
|
|
18 Apr 1991 Harry Li
|
|
27 Sept 1991 Jeffrey P. Murray
|
|
|
|
|
|
SUMMARY
|
|
|
|
This file contains the functional description of the s-domain
|
|
transfer function (s_xfer) code model.
|
|
|
|
|
|
INTERFACES
|
|
|
|
FILE ROUTINE CALLED
|
|
|
|
CMmacros.h cm_message_send();
|
|
|
|
CM.c void *cm_analog_alloc()
|
|
void *cm_analog_get_ptr()
|
|
int cm_analog_integrate()
|
|
|
|
|
|
|
|
|
|
REFERENCED FILES
|
|
|
|
Inputs from and outputs to ARGS structure.
|
|
|
|
|
|
NON-STANDARD FEATURES
|
|
|
|
NONE
|
|
|
|
===============================================================================*/
|
|
|
|
/*=== INCLUDE FILES ====================*/
|
|
|
|
#include <math.h>
|
|
|
|
|
|
|
|
/*=== CONSTANTS ========================*/
|
|
|
|
|
|
|
|
|
|
|
|
/*=== MACROS ===========================*/
|
|
|
|
|
|
|
|
|
|
/*=== LOCAL VARIABLES & TYPEDEFS =======*/
|
|
|
|
|
|
|
|
|
|
|
|
/*=== FUNCTION PROTOTYPE DEFINITIONS ===*/
|
|
|
|
|
|
|
|
|
|
|
|
/*=============================================================================
|
|
|
|
FUNCTION cm_complex_div
|
|
|
|
AUTHORS
|
|
|
|
27 Sept 1991 Jeffrey P. Murray
|
|
|
|
MODIFICATIONS
|
|
|
|
NONE
|
|
|
|
SUMMARY
|
|
|
|
Performs a complex division.
|
|
|
|
INTERFACES
|
|
|
|
FILE ROUTINE CALLED
|
|
|
|
N/A N/A
|
|
|
|
|
|
RETURNED VALUE
|
|
|
|
A Mif_Complex_t value representing the result of the complex division.
|
|
|
|
GLOBAL VARIABLES
|
|
|
|
NONE
|
|
|
|
NON-STANDARD FEATURES
|
|
|
|
NONE
|
|
|
|
==============================================================================*/
|
|
#include <stdlib.h>
|
|
|
|
/*=== Static CM_COMPLEX_DIV ROUTINE ===*/
|
|
|
|
/**** Cm_complex_div Function - FAKE ***********/
|
|
/* */
|
|
/* Function will not be used in finished */
|
|
/* system...provides a stub for performing */
|
|
/* a simple complex division. */
|
|
/* 12/3/90 JPM */
|
|
/* */
|
|
/***********************************************/
|
|
|
|
static Mif_Complex_t cm_complex_div(Mif_Complex_t x, Mif_Complex_t y)
|
|
{
|
|
double mag_x, phase_x, mag_y, phase_y;
|
|
|
|
Mif_Complex_t out;
|
|
|
|
mag_x = sqrt( (x.real * x.real) + (x.imag * x.imag) );
|
|
phase_x = atan2(x.imag, x.real);
|
|
|
|
mag_y = sqrt( (y.real * y.real) + (y.imag * y.imag) );
|
|
phase_y = atan2(y.imag, y.real);
|
|
|
|
mag_x = mag_x/mag_y;
|
|
phase_x = phase_x - phase_y;
|
|
|
|
out.real = mag_x * cos(phase_x);
|
|
out.imag = mag_x * sin(phase_x);
|
|
|
|
return out;
|
|
}
|
|
|
|
|
|
|
|
/*==============================================================================
|
|
|
|
FUNCTION cm_s_xfer()
|
|
|
|
AUTHORS
|
|
|
|
17 Mar 1991 Jeffrey P. Murray
|
|
|
|
MODIFICATIONS
|
|
|
|
18 Apr 1991 Harry Li
|
|
27 Sept 1991 Jeffrey P. Murray
|
|
|
|
SUMMARY
|
|
|
|
This function implements the s_xfer code model.
|
|
|
|
INTERFACES
|
|
|
|
FILE ROUTINE CALLED
|
|
|
|
CMmacros.h cm_message_send();
|
|
|
|
CM.c void *cm_analog_alloc()
|
|
void *cm_analog_get_ptr()
|
|
int cm_analog_integrate()
|
|
|
|
RETURNED VALUE
|
|
|
|
Returns inputs and outputs via ARGS structure.
|
|
|
|
GLOBAL VARIABLES
|
|
|
|
NONE
|
|
|
|
NON-STANDARD FEATURES
|
|
|
|
NONE
|
|
|
|
==============================================================================*/
|
|
|
|
/*=== CM_S_XFER ROUTINE ===*/
|
|
|
|
/****************************************
|
|
* S-Domain Transfer Function - *
|
|
* Code Body *
|
|
* *
|
|
* Last Modified - 9/27/91 JPM *
|
|
****************************************/
|
|
|
|
void cm_s_xfer(ARGS) /* structure holding parms, inputs, outputs, etc. */
|
|
{
|
|
double *out; /* pointer to the output */
|
|
double *in; /* pointer to the input */
|
|
double in_offset; /* input offset */
|
|
double *gain; /* pointer to the gain */
|
|
double **den_coefficient; /* dynamic array that holds the denominator
|
|
coefficients */
|
|
double **old_den_coefficient;/* dynamic array that holds the old
|
|
denonminator coefficients */
|
|
double **num_coefficient; /* dynamic array that holds the numerator
|
|
coefficients */
|
|
double **old_num_coefficient;/* dynamic array that holds the old numerator
|
|
coefficients */
|
|
double factor; /* gain factor in case the highest
|
|
denominator coefficient is not 1 */
|
|
double **integrator; /* outputs of the integrators */
|
|
double **old_integrator; /* previous integrator outputs */
|
|
double null; /* dummy pointer for use with the
|
|
integrate function */
|
|
double pout_pin; /* partial out wrt in */
|
|
/*double total_gain;*/ /* not used, currently-used with ITP stuff */
|
|
double temp; /* temporary variable used with the
|
|
correct type of AC value */
|
|
double frac; /* holds fractional part of a divide */
|
|
double divide_integer; /* integer part of a modf used in AC */
|
|
double denormalized_freq; /* denormalization constant...the nominal
|
|
corner or center frequencies specified
|
|
by the model coefficients will be
|
|
denormalized by this amount. Thus, if
|
|
coefficients were obtained which specified
|
|
a 1 rad/sec cornere frequency, specifying
|
|
a value of 1000.0 for denormalized_freq
|
|
will cause the model to shift the corner
|
|
freq. to 2.0 * pi * 1000.0 */
|
|
double *old_gain; /* pointer to the gain if the highest order
|
|
denominator coefficient is not factored out */
|
|
|
|
Mif_Complex_t ac_gain, acc_num, acc_den;
|
|
|
|
int i; /* generic loop counter index */
|
|
int den_size; /* size of the denominator coefficient array */
|
|
int num_size; /* size of the numerator coefficient array */
|
|
|
|
char *num_size_error="\n***ERROR***\nS_XFER: Numerator coefficient array size greater than\ndenominator coefficiant array size.\n";
|
|
|
|
|
|
|
|
/** Retrieve frequently used parameters (used by all analyses)... **/
|
|
|
|
in_offset = PARAM(in_offset);
|
|
num_size = PARAM_SIZE(num_coeff);
|
|
den_size = PARAM_SIZE(den_coeff);
|
|
if ( PARAM_NULL(denormalized_freq) ) {
|
|
denormalized_freq = 1.0;
|
|
}
|
|
else {
|
|
denormalized_freq = PARAM(denormalized_freq);
|
|
}
|
|
|
|
if ( num_size > den_size ) {
|
|
cm_message_send(num_size_error);
|
|
return;
|
|
}
|
|
|
|
/** Test for INIT; if so, allocate storage, otherwise, retrieve previous **/
|
|
/** timepoint input values as necessary in subsequent analysis sections... **/
|
|
|
|
if (INIT==1) { /* First pass...allocate storage for previous values... */
|
|
|
|
/* Allocate rotational storage for integrator outputs, in & out */
|
|
|
|
|
|
/***** The following two lines may be unnecessary in the final version *****/
|
|
|
|
/* We have to allocate memory and use cm_analog_alloc, because the ITP variables
|
|
are not functional */
|
|
|
|
integrator = (double **) calloc(den_size, sizeof(double *));
|
|
old_integrator = (double **) calloc(den_size, sizeof(double *));
|
|
|
|
/* Allocate storage for coefficient values */
|
|
|
|
den_coefficient = (double **) calloc(den_size,sizeof(double *));
|
|
old_den_coefficient = (double **) calloc(den_size,sizeof(double *));
|
|
|
|
num_coefficient = (double **) calloc(num_size,sizeof(double *));
|
|
old_num_coefficient = (double **) calloc(num_size,sizeof(double *));
|
|
|
|
for (i=0; i < (2*den_size + num_size + 3); i++)
|
|
cm_analog_alloc(i,sizeof(double));
|
|
|
|
/* ITP_VAR_SIZE(den) = den_size; */
|
|
|
|
/* gain = (double *) calloc(1,sizeof(double));
|
|
ITP_VAR(total_gain) = gain;
|
|
ITP_VAR_SIZE(total_gain) = 1.0; */
|
|
|
|
// Retrieve pointers
|
|
|
|
for (i=0; i<den_size; i++) {
|
|
integrator[i] = (double *) cm_analog_get_ptr(i,0);
|
|
old_integrator[i] = (double *) cm_analog_get_ptr(i,0);
|
|
}
|
|
|
|
for(i=den_size;i<2*den_size;i++) {
|
|
den_coefficient[i-den_size] = (double *) cm_analog_get_ptr(i,0);
|
|
old_den_coefficient[i-den_size] = (double *) cm_analog_get_ptr(i,0);
|
|
}
|
|
|
|
for(i=2*den_size;i<2*den_size+num_size;i++) {
|
|
num_coefficient[i-2*den_size] = (double *) cm_analog_get_ptr(i,0);
|
|
old_num_coefficient[i-2*den_size] = (double *) cm_analog_get_ptr(i,0);
|
|
}
|
|
|
|
out = (double *) cm_analog_get_ptr(2*den_size+num_size, 0);
|
|
in = (double *) cm_analog_get_ptr(2*den_size+num_size+1, 0);
|
|
|
|
gain = (double *) cm_analog_get_ptr(2*den_size+num_size+2,0);
|
|
|
|
}else { /* Allocation was not necessary...retrieve previous values */
|
|
|
|
/* Set pointers to storage locations for in, out, and integrators...*/
|
|
|
|
integrator = (double **) calloc(den_size,sizeof(double *));
|
|
old_integrator = (double **) calloc(den_size,sizeof(double *));
|
|
|
|
for (i=0; i<den_size; i++) {
|
|
integrator[i] = (double *) cm_analog_get_ptr(i,0);
|
|
old_integrator[i] = (double *) cm_analog_get_ptr(i,1);
|
|
|
|
}
|
|
out = (double *) cm_analog_get_ptr(2*den_size+num_size,0);
|
|
in = (double *) cm_analog_get_ptr(2*den_size+num_size+1,0);
|
|
|
|
|
|
/* Set den_coefficient & gain pointers to ITP values */
|
|
/* for denominator coefficients & gain... */
|
|
|
|
old_den_coefficient = (double **) calloc(den_size,sizeof(double));
|
|
den_coefficient = (double **) calloc(den_size,sizeof(double));
|
|
|
|
for(i=den_size;i<2*den_size;i++){
|
|
old_den_coefficient[i-den_size] = (double *) cm_analog_get_ptr(i,1);
|
|
den_coefficient[i-den_size] = (double *) cm_analog_get_ptr(i,0);
|
|
*(den_coefficient[i-den_size]) = *(old_den_coefficient[i-den_size]);
|
|
}
|
|
|
|
num_coefficient = (double **) calloc(num_size,sizeof(double));
|
|
old_num_coefficient = (double **) calloc(num_size,sizeof(double));
|
|
|
|
for(i=2*den_size;i<2*den_size+num_size;i++){
|
|
old_num_coefficient[i-2*den_size] = (double *) cm_analog_get_ptr(i,1);
|
|
num_coefficient[i-2*den_size] = (double *) cm_analog_get_ptr(i,0);
|
|
*(num_coefficient[i-2*den_size]) = *(old_num_coefficient[i-2*den_size]);
|
|
}
|
|
|
|
/* gain has to be stored each time since it could possibly change
|
|
if the highest order denominator coefficient isn't zero. This
|
|
is a hack until the ITP variables work */
|
|
|
|
old_gain = (double *) cm_analog_get_ptr(2*den_size+num_size+2,1);
|
|
gain = (double *) cm_analog_get_ptr(2*den_size+num_size+2,0);
|
|
|
|
*gain = *old_gain;
|
|
|
|
/* gain = ITP_VAR(total_gain); */
|
|
|
|
}
|
|
|
|
|
|
/** Test for TIME=0.0; if so, initialize... **/
|
|
|
|
if (TIME == 0.0) { /* First pass...set initial conditions... */
|
|
|
|
/* Initialize integrators to int_ic condition values... */
|
|
for (i=0; i<den_size-1; i++) { /* Note...do NOT set the highest */
|
|
/* order value...this represents */
|
|
/* the "calculated" input to the */
|
|
/* actual highest integrator...it */
|
|
/* is NOT a true state variable. */
|
|
if ( PARAM_NULL(int_ic) ) {
|
|
// *(integrator[i]) = *(old_integrator[i]) = PARAM(int_ic[0]);
|
|
*(integrator[i]) = *(old_integrator[i]) = 0;
|
|
}
|
|
else {
|
|
*(integrator[i]) = *(old_integrator[i]) =
|
|
PARAM(int_ic[den_size - 2 - i]);
|
|
}
|
|
}
|
|
|
|
|
|
/*** Read in coefficients and denormalize, if required ***/
|
|
|
|
for (i=0; i<num_size; i++) {
|
|
*(num_coefficient[i]) = PARAM(num_coeff[num_size - 1 - i]);
|
|
if ( denormalized_freq != 1.0 ) {
|
|
*(num_coefficient[i]) = *(num_coefficient[i]) /
|
|
pow(denormalized_freq,(double) i);
|
|
}
|
|
}
|
|
|
|
for (i=0; i<den_size; i++) {
|
|
*(den_coefficient[i]) = PARAM(den_coeff[den_size - 1 - i]);
|
|
if ( denormalized_freq != 1.0 ) {
|
|
*(den_coefficient[i]) = *(den_coefficient[i]) /
|
|
pow(denormalized_freq,(double) i);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/* Test denominator highest order coefficient...if that value */
|
|
/* is other than 1.0, then divide all denominator coefficients */
|
|
/* and the gain by that value... */
|
|
// if ( (factor = PARAM(den_coeff[den_size-1])) != 1.0 ) {
|
|
if ( (factor = *den_coefficient[den_size-1]) != 1.0 ) {
|
|
for (i=0; i<den_size; i++) {
|
|
*(den_coefficient[i]) = *(den_coefficient[i]) / factor;
|
|
}
|
|
*gain = PARAM(gain) / factor;
|
|
}
|
|
else { /* No division by coefficient necessary... */
|
|
/* only need to adjust gain value. */
|
|
*gain = PARAM(gain);
|
|
}
|
|
|
|
}
|
|
|
|
|
|
/**** DC & Transient Analyses **************************/
|
|
if (ANALYSIS != MIF_AC) {
|
|
|
|
/**** DC Analysis - Not needed JPM 10/29/91 *****************/
|
|
/* if (ANALYSIS == MIF_DC) {
|
|
|
|
?* Test to see if a term exists for the zero-th order
|
|
denom coeff...
|
|
|
|
?* division by zero if output
|
|
?* num_coefficient[0]/den_coefficient[0],
|
|
?* so output init. conds. instead...
|
|
if ( 0.0 == *(den_coefficient[0])) {
|
|
|
|
|
|
*out = 0.0;
|
|
for (i=0; i<num_size; i++) {
|
|
*out = *out + ( *(old_integrator[i]) *
|
|
*(num_coefficient[i]) );
|
|
}
|
|
*out = *gain * *out;
|
|
pout_pin = *(old_integrator[1]);
|
|
|
|
}
|
|
|
|
?* Zero-th order den term != 0.0, so output
|
|
?* num_coeff[0]/den_coeff[0]...
|
|
else {
|
|
|
|
*out = *gain * ( INPUT(in) +
|
|
in_offset) * ( *(num_coefficient[0]) /
|
|
*(den_coefficient[0]) );
|
|
pout_pin = 0.0;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
else {
|
|
*/
|
|
|
|
|
|
/**** Transient & DC Analyses ****************************/
|
|
|
|
/*** Read input value for current time, and
|
|
calculate pseudo-input which includes input
|
|
offset and gain.... ***/
|
|
|
|
*in = *gain * (INPUT(in)+in_offset);
|
|
|
|
|
|
|
|
/*** Obtain the "new" input to the Controller
|
|
Canonical topology, then propagate through
|
|
the integrators.... ***/
|
|
|
|
/* calculate the "new" input to the first integrator, based on */
|
|
/* the old values of each integrator multiplied by their */
|
|
/* respective denominator coefficients and then subtracted */
|
|
/* from *in.... */
|
|
/* Note that this value, which is similar to a state variable, */
|
|
/* is stored in *(integrator[den_size-1]). */
|
|
|
|
*(integrator[den_size-1]) = *in;
|
|
for (i=0; i<den_size-1; i++) {
|
|
*(integrator[den_size-1]) =
|
|
*(integrator[den_size-1]) -
|
|
*(old_integrator[i]) * *(den_coefficient[i]);
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Propagate the new input through each integrator in succession. */
|
|
|
|
for (i=den_size-1; i>0; i--) {
|
|
cm_analog_integrate(*(integrator[i]),(integrator[i-1]),&null);
|
|
}
|
|
|
|
|
|
|
|
/* Calculate the output based on the new integrator values... */
|
|
|
|
*out = 0.0;
|
|
for (i=0; i<num_size; i++) {
|
|
*out = *out + ( *(integrator[i]) *
|
|
*(num_coefficient[i]) );
|
|
}
|
|
pout_pin = *(integrator[1]);
|
|
|
|
|
|
/** Output values for DC & Transient **/
|
|
|
|
OUTPUT(out) = *out;
|
|
PARTIAL(out,in) = pout_pin;
|
|
// cm_analog_auto_partial(); // Removed again. Seems to have problems.
|
|
|
|
}
|
|
|
|
/**** AC Analysis ************************************/
|
|
else {
|
|
|
|
/*** Calculate Real & Imaginary portions of AC gain ***/
|
|
/*** at the current RAD_FREQ point... ***/
|
|
|
|
|
|
/*** Calculate Numerator Real & Imaginary Components... ***/
|
|
|
|
acc_num.real = 0.0;
|
|
acc_num.imag = 0.0;
|
|
|
|
for (i=0; i<num_size; i++) {
|
|
frac = modf(i/2.0, ÷_integer); /* Determine the integer portion */
|
|
/* of a divide-by-2.0 on the index. */
|
|
|
|
if (modf(divide_integer/2.0,&temp) > 0.0 ) { /* Negative coefficient */
|
|
/* values for this iteration. */
|
|
if (frac > 0.0 ) { /** Odd Powers of "s" **/
|
|
acc_num.imag = acc_num.imag - *(num_coefficient[i]) * pow(RAD_FREQ,i) * (*gain);
|
|
}
|
|
else { /** Even Powers of "s" **/
|
|
acc_num.real = acc_num.real - *(num_coefficient[i]) * pow(RAD_FREQ,i) * (*gain);
|
|
}
|
|
}
|
|
else { /* Positive coefficient values for this iteration */
|
|
if (frac> 0.0 ) { /** Odd Powers of "s" **/
|
|
acc_num.imag = acc_num.imag + *(num_coefficient[i]) * pow(RAD_FREQ,i) * (*gain);
|
|
}
|
|
else { /** Even Powers of "s" **/
|
|
acc_num.real = acc_num.real + *(num_coefficient[i]) * pow(RAD_FREQ,i) * (*gain);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*** Calculate Denominator Real & Imaginary Components... ***/
|
|
|
|
acc_den.real = 0.0;
|
|
acc_den.imag = 0.0;
|
|
|
|
for (i=0; i<den_size; i++) {
|
|
frac = modf(i/2.0, ÷_integer); /* Determine the integer portion */
|
|
/* of a divide-by-2.0 on the index. */
|
|
if (modf(divide_integer/2.0,&temp) > 0.0 ) { /* Negative coefficient */
|
|
/* values for this iteration. */
|
|
if (frac > 0.0 ) { /** Odd Powers of "s" **/
|
|
acc_den.imag = acc_den.imag - *(den_coefficient[i]) * pow(RAD_FREQ,i);
|
|
}
|
|
else { /** Even Powers of "s" **/
|
|
acc_den.real = acc_den.real - *(den_coefficient[i]) * pow(RAD_FREQ,i);
|
|
}
|
|
}
|
|
else { /* Positive coefficient values for this iteration */
|
|
if (frac > 0.0 ) { /** Odd Powers of "s" **/
|
|
acc_den.imag = acc_den.imag + *(den_coefficient[i]) * pow(RAD_FREQ,i);
|
|
}
|
|
else { /** Even Powers of "s" **/
|
|
acc_den.real = acc_den.real + *(den_coefficient[i]) * pow(RAD_FREQ,i);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* divide numerator values by denominator values */
|
|
|
|
ac_gain = cm_complex_div(acc_num, acc_den);
|
|
|
|
AC_GAIN(out,in) = ac_gain;
|
|
}
|
|
|
|
/* free all allocated memory */
|
|
if(integrator) free(integrator);
|
|
if(old_integrator) free(old_integrator);
|
|
if(den_coefficient) free(den_coefficient);
|
|
if(old_den_coefficient) free(old_den_coefficient);
|
|
if(num_coefficient) free(num_coefficient);
|
|
if(old_num_coefficient) free(old_num_coefficient);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|