Example of a Time-Varying Continuous Transfer Function :: Implementing Block Features (Writing S-Functions)

Writing S-Functions
Example of a Time-Varying Continuous Transfer Function
The S-function stvctf is an example of a time-varying continuous transfer function. It demonstrates how to work with the solvers so that the simulation maintains consistency, which means that the block maintains smooth and consistent signals for the integrators although the equations that are being integrated are changing.
matlabroot/simulink/src/stvctf.c
/*
 * File : stvctf.c
 * Abstract:
 *      Time Varying Continuous Transfer Function block
 *
 *      This S-function implements a continuous time transfer function
 *      whose transfer function polynomials are passed in via the input
 *      vector.  This is useful for continuous time adaptive control
 *      applications.
 *
 *      This S-function is also an example of how to use banks to avoid
 *      problems with computing derivatives when a continuous output has 
 *      discontinuities. The consistency checker can be used to verify that 
 *      your S-function is correct with respect to always maintaining smooth 
 *      and consistent signals for the integrators. By consistent we mean that
 *      two mdlOutputs calls at major time t and minor time t are always the 
 *      same. The consistency checker is enabled on the diagnostics page of the
 *      Configuraion parameters dialog box. The update method of this S-function
 *      modifies the coefficients of the transfer function, which cause the
 *      output to "jump." To have the simulation work properly, we need to let
 *      the solver know of these discontinuities by setting 
 *      ssSetSolverNeedsReset and then we need to use multiple banks of 
 *      coefficients so the coefficients used in the major time step output 
 *      and the minor time step outputs are the same. In the simulation loop 
 *      we have:
 *        Loop:
 *          o Output in major time step at time t
 *          o Update in major time step at time t
 *          o Integrate (minor time step):
 *              o Consistency check: recompute outputs at time t and compare
 *                with current outputs.
 *              o Derivatives at time t
 *              o One or more Output,Derivative evaluations at time t+k
 *                where k <= step_size to be taken.
 *              o Compute state, x
 *              o t = t + step_size
 *            End_Integrate
 *        End_Loop
 *      Another purpose of the consistency checker is to verify that when 
 *      the solver needs to try a smaller step_size, the recomputing of 
 *      the output and derivatives at time t doesn't change. Step size 
 *      reduction occurs when tolerances aren't met for the current step size.
 *      The ideal ordering would be to update after integrate. To achieve 
 *      this we have two banks of coefficients. And the use of the new 
 *      coefficients, which were computed in update, is delayed until after 
 *      the integrate phase is complete.
 *
 *  This block has multiple sample times and will not work correctly
 *  in a multitasking environment. It is designed to be used in
 *  a single tasking (or variable step) simulation environment.
 *  Because this block accesses the input signal in both tasks,
 *  it cannot specify the sample times of the input and output ports
 *  (SS_OPTION_PORT_SAMPLE_TIMES_ASSIGNED).
 *
 * See simulink/src/sfuntmpl_doc.c.
 *
 * Copyright 1990-2000 The MathWorks, Inc.
 */

#define S_FUNCTION_NAME  stvctf
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"

/*
 * Defines for easy access to the numerator and denominator polynomials
 * parameters
 */
#define NUM(S)  ssGetSFcnParam(S, 0)
#define DEN(S)  ssGetSFcnParam(S, 1)
#define TS(S)   ssGetSFcnParam(S, 2)
#define NPARAMS 3


#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
  /* Function: mdlCheckParameters =============================================
   * Abstract:
   *    Validate our parameters to verify:
   *     o The numerator must be of a lower order than the denominator.
   *     o The sample time must be a real positive nonzero value.
   */
  static void mdlCheckParameters(SimStruct *S)
  {
      int_T i;

      for (i = 0; i < NPARAMS; i++) {
          real_T *pr;
          int_T   el;
          int_T   nEls;
          if (mxIsEmpty(    ssGetSFcnParam(S,i)) ||
              mxIsSparse(   ssGetSFcnParam(S,i)) ||
              mxIsComplex(  ssGetSFcnParam(S,i)) ||
              !mxIsNumeric( ssGetSFcnParam(S,i)) ) {
              ssSetErrorStatus(S,"Parameters must be real finite vectors");
              return;
          } 
          pr   = mxGetPr(ssGetSFcnParam(S,i));
          nEls = mxGetNumberOfElements(ssGetSFcnParam(S,i));
          for (el = 0; el < nEls; el++) {
              if (!mxIsFinite(pr[el])) {
                  ssSetErrorStatus(S,"Parameters must be real finite vectors");
                  return;
              }
          }
      }

      if (mxGetNumberOfElements(NUM(S)) > mxGetNumberOfElements(DEN(S)) &&
          mxGetNumberOfElements(DEN(S)) > 0  && *mxGetPr(DEN(S)) != 0.0) {
          ssSetErrorStatus(S,"The denominator must be of higher order than "
                           "the numerator, nonempty and with first "
                           "element nonzero");
          return;
      }

      /* xxx verify finite */
      if (mxGetNumberOfElements(TS(S)) != 1 || mxGetPr(TS(S))[0] <= 0.0) {
          ssSetErrorStatus(S,"Invalid sample time specified");
          return;
      }
  }
#endif /* MDL_CHECK_PARAMETERS */


/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    int_T nContStates;
    int_T nCoeffs;

    /* See sfuntmpl_doc.c for more details on the macros below. */
 
    ssSetNumSFcnParams(S, NPARAMS);  /* Number of expected parameters. */
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Parameter mismatch will be reported by Simulink. */
    }
#endif


    /*
     * Define the characteristics of the block:
     *
     *   Number of continuous states:     length of denominator - 1
     *   Inputs port width                2 * (NumContStates+1) + 1
     *   Output port width                1
     *   DirectFeedThrough:               0 (Although this should be computed.
     *                                       We'll assume coefficients entered
     *                                       are strictly proper).
     *   Number of sample times:          2 (continuous and discrete)
     *   Number of Real work elements:    4*NumCoeffs
     *                                    (Two banks for num and den coeff's:
     *                                     NumBank0Coeffs
     *                                     DenBank0Coeffs
     *                                     NumBank1Coeffs
     *                                     DenBank1Coeffs)
     *   Number of Integer work elements: 2 (indicator of active bank 0 or 1
     *                                       and flag to indicate when banks
     *                                       have been updated).
     *
     * The number of inputs arises from the following:
     *   o 1 input (u)
     *   o the numerator and denominator polynomials each have NumContStates+1
     *     coefficients
     */
    nCoeffs     = mxGetNumberOfElements(DEN(S));
    nContStates = nCoeffs - 1;

    ssSetNumContStates(S, nContStates);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 1)) return;
    ssSetInputPortWidth(S, 0, 1 + (2*nCoeffs));
    ssSetInputPortDirectFeedThrough(S, 0, 0);
    ssSetInputPortSampleTime(S, 0, mxGetPr(TS(S))[0]);
    ssSetInputPortOffsetTime(S, 0, 0);

    if (!ssSetNumOutputPorts(S,1)) return;
    ssSetOutputPortWidth(S, 0, 1);
    ssSetOutputPortSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOutputPortOffsetTime(S, 0, 0);

    ssSetNumSampleTimes(S, 2);

    ssSetNumRWork(S, 4 * nCoeffs);
    ssSetNumIWork(S, 2);
    ssSetNumPWork(S, 0);

    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Take care when specifying exception free code - see sfuntmpl_doc.c */
    ssSetOptions(S, (SS_OPTION_EXCEPTION_FREE_CODE));

} /* end mdlInitializeSizes */



/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *      This function is used to specify the sample time(s) for the
 *      S-function.  This S-function has two sample times.  The
 *      first, a continous sample time, is used for the input to the
 *      transfer function, u.  The second, a discrete sample time
 *      provided by the user, defines the rate at which the transfer
 *      function coefficients are updated.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    /*
     * the first sample time, continuous
     */
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);

    /*
     * the second, discrete sample time, is user provided
     */
    ssSetSampleTime(S, 1, mxGetPr(TS(S))[0]);
    ssSetOffsetTime(S, 1, 0.0);

} /* end mdlInitializeSampleTimes */



#define MDL_INITIALIZE_CONDITIONS
/* Function: mdlInitializeConditions ==========================================
 * Abstract:
 *      Initalize the states, numerator and denominator coefficients.
 */
static void mdlInitializeConditions(SimStruct *S)
{
    int_T  i;
    int_T  nContStates = ssGetNumContStates(S);
    real_T *x0           = ssGetContStates(S);
    int_T  nCoeffs       = nContStates + 1;
    real_T *numBank0     = ssGetRWork(S);
    real_T *denBank0     = numBank0 + nCoeffs;
    int_T *activeBank    = ssGetIWork(S);

    /*
     * The continuous states are all initialized to zero.
     */
    for (i = 0; i < nContStates; i++) {
        x0[i]       = 0.0;
        numBank0[i] = 0.0;
        denBank0[i] = 0.0;
    }
    numBank0[nContStates] = 0.0;
    denBank0[nContStates] = 0.0;

    /*
     * Set up the initial numerator and denominator.
     */
    {
        const real_T *numParam   = mxGetPr(NUM(S));
        int          numParamLen = mxGetNumberOfElements(NUM(S));

        const real_T *denParam   = mxGetPr(DEN(S));
        int          denParamLen = mxGetNumberOfElements(DEN(S));
        real_T       den0        = denParam[0];

        for (i = 0; i < denParamLen; i++) {
            denBank0[i] = denParam[i] / den0;
        }

        for (i = 0; i < numParamLen; i++) {
            numBank0[i] = numParam[i] / den0;
        }
    }

    /*
     * Normalize if this transfer function has direct feedthrough.
     */
    for (i = 1; i < nCoeffs; i++) {
        numBank0[i] -= denBank0[i]*numBank0[0];
    }

    /*
     * Indicate bank0 is active (i.e. bank1 is oldest).
     */
    *activeBank = 0;

} /* end mdlInitializeConditions */



/* Function: mdlOutputs =======================================================
 * Abstract:
 *      The outputs for this block are computed by using a controllable state-
 *      space representation of the transfer function.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    if (ssIsContinuousTask(S,tid)) {
        int               i;
        real_T            *num;
        int               nContStates = ssGetNumContStates(S);
        real_T            *x          = ssGetContStates(S);
        int_T             nCoeffs     = nContStates + 1;
        InputRealPtrsType uPtrs       = ssGetInputPortRealSignalPtrs(S,0);
        real_T            *y          = ssGetOutputPortRealSignal(S,0);
        int_T             *activeBank = ssGetIWork(S);

        /*
         * Switch banks because we've updated them in mdlUpdate and we're no 
         * longer in a minor time step.
         */
        if (ssIsMajorTimeStep(S)) {
            int_T *banksUpdated = ssGetIWork(S) + 1;
            if (*banksUpdated) {
                *activeBank = !(*activeBank);
                *banksUpdated = 0;
                /*
                 * Need to tell the solvers that the derivatives are no
                 * longer valid.
                 */
                ssSetSolverNeedsReset(S);
            }
        }
        num = ssGetRWork(S) + (*activeBank) * (2*nCoeffs);

        /*
         * The continuous system is evaluated using a controllable state space
         * representation of the transfer function.  This implies that the
         * output of the system is equal to:
         *
         *     y(t) = Cx(t) + Du(t)
         *          = [ b1 b2 ... bn]x(t) + b0u(t)
         *
         * where b0, b1, b2, ... are the coefficients of the numerator 
         * polynomial:
         *
         *    B(s) = b0 s^n + b1 s^n-1 + b2 s^n-2 + ... + bn-1 s + bn
         */
        *y = *num++ * (*uPtrs[0]);
        for (i = 0; i < nContStates; i++) {
            *y += *num++ * *x++;
        }
    }

} /* end mdlOutputs */


#define MDL_UPDATE
/* Function: mdlUpdate ========================================================
 * Abstract:
 *      Every time through the simulation loop, update the
 *      transfer function coefficients. Here we update the oldest bank.
 */
static void mdlUpdate(SimStruct *S, int_T tid)
{
    UNUSED_ARG(tid); /* not used in single tasking mode */

    if (ssIsSampleHit(S, 1, tid)) {
        int_T             i;
        InputRealPtrsType uPtrs        = ssGetInputPortRealSignalPtrs(S,0);
        int_T             uIdx         = 1;/*1st coeff is after signal input*/
        int_T             nContStates  = ssGetNumContStates(S);
        int_T             nCoeffs      = nContStates + 1;
        int_T             bankToUpdate = !ssGetIWork(S)[0];
        real_T            *num         = ssGetRWork(S)+bankToUpdate*2*nCoeffs;
        real_T            *den         = num + nCoeffs;

        real_T            den0;
        int_T             allZero;


        /*
         * Get the first denominator coefficient.  It will be used
         * for normalizing the numerator and denominator coefficients.
         *
         * If all inputs are zero, we probably could have unconnected
         * inputs, so use the parameter as the first denominator coefficient.
         */
        den0 = *uPtrs[uIdx+nCoeffs];
        if (den0 == 0.0) {
            den0 = mxGetPr(DEN(S))[0];
        }

        /*
         * Grab the numerator.
         */
        allZero = 1;
        for (i = 0; (i < nCoeffs) && allZero; i++) {
            allZero &= *uPtrs[uIdx+i] == 0.0;
        }

        if (allZero) { /* if numerator is all zero */
            const real_T *numParam   = mxGetPr(NUM(S));
            int_T        numParamLen = mxGetNumberOfElements(NUM(S));
            /*
             * Move the input to the denominator input and
             * get the denominator from the input parameter.
             */
            uIdx += nCoeffs;
            num += nCoeffs - numParamLen;
            for (i = 0; i < numParamLen; i++) {
                *num++ = *numParam++ / den0;
            }
        } else {
            for (i = 0; i < nCoeffs; i++) {
                *num++ = *uPtrs[uIdx++] / den0;
            }
        }

        /*
         * Grab the denominator.
         */
        allZero = 1;
        for (i = 0; (i < nCoeffs) && allZero; i++) {
            allZero &= *uPtrs[uIdx+i] == 0.0;
        }

        if (allZero) {  /* If denominator is all zero. */
            const real_T *denParam   = mxGetPr(DEN(S));
            int_T        denParamLen = mxGetNumberOfElements(DEN(S));

            den0 = denParam[0];
            for (i = 0; i < denParamLen; i++) {
                *den++ = *denParam++ / den0;
            }
        } else {
            for (i = 0; i < nCoeffs; i++) {
                *den++ = *uPtrs[uIdx++] / den0;
            }
        }

        /*
         * Normalize if this transfer function has direct feedthrough.
         */
        num = ssGetRWork(S) + bankToUpdate*2*nCoeffs;
        den = num + nCoeffs;
        for (i = 1; i < nCoeffs; i++) {
            num[i] -= den[i]*num[0];
        }

        /*
         * Indicate oldest bank has been updated.
         */
        ssGetIWork(S)[1] = 1;
    }

} /* end mdlUpdate */



#define MDL_DERIVATIVES
/* Function: mdlDerivatives ===================================================
 * Abstract:
 *      The derivatives for this block are computed by using a controllable 
 *      state-space representation of the transfer function.
 */
static void mdlDerivatives(SimStruct *S) 
{
    int_T             i;
    int_T             nContStates = ssGetNumContStates(S);
    real_T            *x          = ssGetContStates(S);
    real_T            *dx         = ssGetdX(S);
    int_T             nCoeffs     = nContStates + 1;
    int_T             activeBank  = ssGetIWork(S)[0];
    const real_T      *num        = ssGetRWork(S) + activeBank*(2*nCoeffs);
    const real_T      *den        = num + nCoeffs;
    InputRealPtrsType uPtrs       = ssGetInputPortRealSignalPtrs(S,0);

    /*
     * The continuous system is evaluated using a controllable state-space
     * representation of the transfer function.  This implies that the
     * next continuous states are computed using:
     *
     *     dx = Ax(t) + Bu(t)
     *        = [-a1 -a2 ... -an] [x1(t)] + [u(t)]
     *          [  1  0  ...   0] [x2(t)] + [0]
     *          [  0  1  ...   0] [x3(t)] + [0]
     *          [  .  .  ...   .]   .     +  .
     *          [  .  .  ...   .]   .     +  .
     *          [  .  .  ...   .]   .     +  .
     *          [  0  0  ... 1 0] [xn(t)] + [0]
     *
     * where a1, a2, ... are the coefficients of the numerator polynomial:
     *
     *    A(s) = s^n + a1 s^n-1 + a2 s^n-2 + ... + an-1 s + an
     */
    dx[0] = -den[1] * x[0] + *uPtrs[0];
    for (i = 1; i < nContStates; i++) {
        dx[i] = x[i-1];
        dx[0] -= den[i+1] * x[i];
    }

} /* end mdlDerivatives */



/* Function: mdlTerminate =====================================================
 * Abstract:
 *      Called when the simulation is terminated.
 *      For this block, there are no end of simulation tasks.
 */
static void mdlTerminate(SimStruct *S)
{
    UNUSED_ARG(S); /* unused input argument */
} /* end mdlTerminate */


#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif
Example of a Zero Crossing S-Function S-Function Callback Methods