| Writing S-Functions | |
Example - Zero Crossing S-Function
The example S-function, sfun_zc_sat demonstrates how to implement a saturation block. This S-function is designed to work with either fixed or variable step solvers. When this S-function inherits a continuous sample time, and a variable step solver is being used, a zero crossings algorithm is used to locate the exact points at which the saturation occurs.
matlabroot/simulink/src/sfun_zc_sat.c
/* File : sfun_zc_sat.c
* Abstract:
*
* Example of an S-function which has nonsampled zero crossings to
* implement a saturation function. This S-function is designed to be
* used with a variable or fixed step solver.
*
* A saturation is described by three equations
*
* (1) y = UpperLimit
* (2) y = u
* (3) y = LowerLimit
*
* and a set of inequalities that specify which equation to use
*
* if UpperLimit < u then use (1)
* if LowerLimit <= u <= UpperLimit then use (2)
* if u < LowerLimit then use (3)
*
* A key fact is that the valid equation 1, 2, or 3, can change at
* any instant. Nonsampled zero crossing support helps the variable step
* solvers locate the exact instants when behavior switches from one equation
* to another.
*
* Copyright 1990-2000 The MathWorks, Inc.
* $Revision: 1.1 $
*/
#define S_FUNCTION_NAME sfun_zc_sat
#define S_FUNCTION_LEVEL 2
#include "simstruc.h"
/*========================*
* General Defines/macros *
*========================*/
/* index to Upper Limit */
#define I_PAR_UPPER_LIMIT 0
/* index to Lower Limit */
#define I_PAR_LOWER_LIMIT 1
/* total number of block parameters */
#define N_PAR 2
/*
* Make access to mxArray pointers for parameters more readable.
*/
#define P_PAR_UPPER_LIMIT ( ssGetSFcnParam(S,I_PAR_UPPER_LIMIT) )
#define P_PAR_LOWER_LIMIT ( ssGetSFcnParam(S,I_PAR_LOWER_LIMIT) )
#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
/* Function: mdlCheckParameters =============================================
* Abstract:
* Check that parameter choices are allowable.
*/
static void mdlCheckParameters(SimStruct *S)
{
int_T i;
int_T numUpperLimit;
int_T numLowerLimit;
const char *msg = NULL;
/*
* check parameter basics
*/
for ( i = 0; i < N_PAR; i++ ) {
if ( mxIsEmpty( ssGetSFcnParam(S,i) ) ||
mxIsSparse( ssGetSFcnParam(S,i) ) ||
mxIsComplex( ssGetSFcnParam(S,i) ) ||
!mxIsNumeric( ssGetSFcnParam(S,i) ) ) {
msg = "Parameters must be real vectors.";
goto EXIT_POINT;
}
}
/*
* Check sizes of parameters.
*/
numUpperLimit = mxGetNumberOfElements( P_PAR_UPPER_LIMIT );
numLowerLimit = mxGetNumberOfElements( P_PAR_LOWER_LIMIT );
if ( ( numUpperLimit != 1 ) &&
( numLowerLimit != 1 ) &&
( numUpperLimit != numLowerLimit ) ) {
msg = "Number of input and output values must be equal.";
goto EXIT_POINT;
}
/*
* Error exit point
*/
EXIT_POINT:
if (msg != NULL) {
ssSetErrorStatus(S, msg);
}
}
#endif /* MDL_CHECK_PARAMETERS */
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* Initialize the sizes array.
*/
static void mdlInitializeSizes(SimStruct *S)
{
int_T numUpperLimit, numLowerLimit, maxNumLimit;
/*
* Set and Check parameter count
*/
ssSetNumSFcnParams(S, N_PAR);
#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
/*
* Get parameter size info.
*/
numUpperLimit = mxGetNumberOfElements( P_PAR_UPPER_LIMIT );
numLowerLimit = mxGetNumberOfElements( P_PAR_LOWER_LIMIT );
if (numUpperLimit > numLowerLimit) {
maxNumLimit = numUpperLimit;
} else {
maxNumLimit = numLowerLimit;
}
/*
* states
*/
ssSetNumContStates(S, 0);
ssSetNumDiscStates(S, 0);
/*
* outputs
* The upper and lower limits are scalar expanded
* so their size determines the size of the output
* only if at least one of them is not scalar.
*/
if (!ssSetNumOutputPorts(S, 1)) return;
if ( maxNumLimit > 1 ) {
ssSetOutputPortWidth(S, 0, maxNumLimit);
} else {
ssSetOutputPortWidth(S, 0, DYNAMICALLY_SIZED);
}
/*
* inputs
* If the upper or lower limits are not scalar then
* the input is set to the same size. However, the
* ssSetOptions below allows the actual width to
* be reduced to 1 if needed for scalar expansion.
*/
if (!ssSetNumInputPorts(S, 1)) return;
ssSetInputPortDirectFeedThrough(S, 0, 1 );
if ( maxNumLimit > 1 ) {
ssSetInputPortWidth(S, 0, maxNumLimit);
} else {
ssSetInputPortWidth(S, 0, DYNAMICALLY_SIZED);
}
/*
* sample times
*/
ssSetNumSampleTimes(S, 1);
/*
* work
*/
ssSetNumRWork(S, 0);
ssSetNumIWork(S, 0);
ssSetNumPWork(S, 0);
/*
* Modes and zero crossings:
* If we have a variable step solver and this block has a continuous
* sample time, then
* o One mode element will be needed for each scalar output
* in order to specify which equation is valid (1), (2), or (3).
* o Two ZC elements will be needed for each scalar output
* in order to help the solver find the exact instants
* at which either of the two possible "equation switches"
* One will be for the switch from eq. (1) to (2);
* the other will be for eq. (2) to (3) and vise versa.
* otherwise
* o No modes and nonsampled zero crossings will be used.
*
*/
ssSetNumModes(S, DYNAMICALLY_SIZED);
ssSetNumNonsampledZCs(S, DYNAMICALLY_SIZED);
/*
* options
* o No mexFunctions and no problematic mxFunctions are called
* so the exception free code option safely gives faster simulations.
* o Scalar expansion of the inputs is desired. The option provides
* this without the need to write mdlSetOutputPortWidth and
* mdlSetInputPortWidth functions.
*/
ssSetOptions(S, ( SS_OPTION_EXCEPTION_FREE_CODE |
SS_OPTION_ALLOW_INPUT_SCALAR_EXPANSION));
} /* end mdlInitializeSizes */
/* Function: mdlInitializeSampleTimes =========================================
* Abstract:
* Specify that the block is continuous.
*/
static void mdlInitializeSampleTimes(SimStruct *S)
{
ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME);
ssSetOffsetTime(S, 0, 0);
}
#define MDL_SET_WORK_WIDTHS
#if defined(MDL_SET_WORK_WIDTHS) && defined(MATLAB_MEX_FILE)
/* Function: mdlSetWorkWidths ===============================================
* The width of the Modes and the ZCs depends on the width of the output.
* This width is not always known in mdlInitializeSizes so it is handled
* here.
*/
static void mdlSetWorkWidths(SimStruct *S)
{
int nModes;
int nNonsampledZCs;
if (ssIsVariableStepSolver(S) &&
ssGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME &&
ssGetOffsetTime(S,0) == 0.0) {
int numOutput = ssGetOutputPortWidth(S, 0);
/*
* modes and zero crossings
* o One mode element will be needed for each scalar output
* in order to specify which equation is valid (1), (2), or (3).
* o Two ZC elements will be needed for each scalar output
* in order to help the solver find the exact instants
* at which either of the two possible "equation switches"
* One will be for the switch from eq. (1) to (2);
* the other will be for eq. (2) to (3) and vise-versa.
*/
nModes = numOutput;
nNonsampledZCs = 2 * numOutput;
} else {
nModes = 0;
nNonsampledZCs = 0;
}
ssSetNumModes(S,nModes);
ssSetNumNonsampledZCs(S,nNonsampledZCs);
}
#endif /* MDL_SET_WORK_WIDTHS */
/* Function: mdlOutputs =======================================================
* Abstract:
*
* A saturation is described by three equations
*
* (1) y = UpperLimit
* (2) y = u
* (3) y = LowerLimit
*
* When this block is used with a fixed-step solver or it has a noncontinuous
* sample time, the equations are used as it
*
* Now consider the case of this block being used with a variable step solver
* and it has a continusous sample time. Solvers work best on smooth problems.
* In order for the solver to work without chattering, limit cycles, or
* similar problems. It is absolutely crucial that the same equation be used
* throughout the duration of a MajorTimeStep. To visualize this, consider
* the case of the Saturation block feeding an Integrator block.
*
* To implement this rule, the mode vector is used to specify the
* valid equation based on the following:
*
* if UpperLimit < u then use (1)
* if LowerLimit <= u <= UpperLimit then use (2)
* if u < LowerLimit then use (3)
*
* The mode vector is changed only at the beginning of a MajorTimeStep.
*
* During a minor time step, the equation specified by the mode vector
* is used without question. Most of the time, the value of u will agree
* with the equation specified by the mode vector. However, sometimes u's
* value will indicate a different equation. Nonetheless, the equation
* specified by the mode vector must be used.
*
* When the mode and u indicate different equations, the corresponding
* calculations are not correct. However, this is not a problem. From
* the ZC function, the solver will know that an equation switch occured
* in the middle of the last MajorTimeStep. The calculations for that
* time step will be discarded. The ZC function will help the solver
* find the exact instant at which the switch occured. Using this knowledge,
* the length of the MajorTimeStep will be reduced so that only one equation
* is valid throughout the entire time step.
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
real_T *y = ssGetOutputPortRealSignal(S,0);
int_T numOutput = ssGetOutputPortWidth(S,0);
int_T iOutput;
/*
* Set index and increment for input signal, upper limit, and lower limit
* parameters so that each gives scalar expansion if needed.
*/
int_T uIdx = 0;
int_T uInc = ( ssGetInputPortWidth(S,0) > 1 );
const real_T *upperLimit = mxGetPr( P_PAR_UPPER_LIMIT );
int_T upperLimitInc = ( mxGetNumberOfElements( P_PAR_UPPER_LIMIT ) > 1 );
const real_T *lowerLimit = mxGetPr( P_PAR_LOWER_LIMIT );
int_T lowerLimitInc = ( mxGetNumberOfElements( P_PAR_LOWER_LIMIT ) > 1 );
UNUSED_ARG(tid); /* not used in single tasking mode */
if (ssGetNumNonsampledZCs(S) == 0) {
/*
* This block is being used with a fixed-step solver or it has
* a noncontinuous sample time, so we always saturate.
*/
for (iOutput = 0; iOutput < numOutput; iOutput++) {
if (*uPtrs[uIdx] >= *upperLimit) {
*y++ = *upperLimit;
} else if (*uPtrs[uIdx] > *lowerLimit) {
*y++ = *uPtrs[uIdx];
} else {
*y++ = *lowerLimit;
}
upperLimit += upperLimitInc;
lowerLimit += lowerLimitInc;
uIdx += uInc;
}
} else {
/*
* This block is being used with a variable-step solver.
*/
int_T *mode = ssGetModeVector(S);
/*
* Specify indices for each equation.
*/
enum { UpperLimitEquation, NonLimitEquation, LowerLimitEquation };
/*
* Update the Mode Vector ONLY at the beginning of a MajorTimeStep
*/
if ( ssIsMajorTimeStep(S) ) {
/*
* Specify the mode, ie the valid equation for each output scalar.
*/
for ( iOutput = 0; iOutput < numOutput; iOutput++ ) {
if ( *uPtrs[uIdx] > *upperLimit ) {
/*
* Upper limit eq is valid.
*/
mode[iOutput] = UpperLimitEquation;
} else if ( *uPtrs[uIdx] < *lowerLimit ) {
/*
* Lower limit eq is valid.
*/
mode[iOutput] = LowerLimitEquation;
} else {
/*
* Nonlimit eq is valid.
*/
mode[iOutput] = NonLimitEquation;
}
/*
* Adjust indices to give scalar expansion if needed.
*/
uIdx += uInc;
upperLimit += upperLimitInc;
lowerLimit += lowerLimitInc;
}
/*
* Reset index to input and limits.
*/
uIdx = 0;
upperLimit = mxGetPr( P_PAR_UPPER_LIMIT );
lowerLimit = mxGetPr( P_PAR_LOWER_LIMIT );
} /* end IsMajorTimeStep */
/*
* For both MinorTimeSteps and MajorTimeSteps calculate each scalar
* output using the equation specified by the mode vector.
*/
for ( iOutput = 0; iOutput < numOutput; iOutput++ ) {
if ( mode[iOutput] == UpperLimitEquation ) {
/*
* Upper limit eq.
*/
*y++ = *upperLimit;
} else if ( mode[iOutput] == LowerLimitEquation ) {
/*
* Lower limit eq.
*/
*y++ = *lowerLimit;
} else {
/*
* Nonlimit eq.
*/
*y++ = *uPtrs[uIdx];
}
/*
* Adjust indices to give scalar expansion if needed.
*/
uIdx += uInc;
upperLimit += upperLimitInc;
lowerLimit += lowerLimitInc;
}
}
} /* end mdlOutputs */
#define MDL_ZERO_CROSSINGS
#if defined(MDL_ZERO_CROSSINGS) && (defined(MATLAB_MEX_FILE) || defined(NRT))
/* Function: mdlZeroCrossings =================================================
* Abstract:
* This will only be called if the number of nonsampled zero crossings is
* greater than 0 which means this block has a continuous sample time and the
* the model is using a variable step solver.
*
* Calculate zero crossing (ZC) signals that help the solver find the
* exact instants at which equation switches occur:
*
* if UpperLimit < u then use (1)
* if LowerLimit <= u <= UpperLimit then use (2)
* if u < LowerLimit then use (3)
*
* The key words are help find. There is no choice of a function that will
* direct the solver to the exact instant of the change. The solver will
* track the zero crossing signal and do a bisection style search for the
* exact instant of equation switch.
*
* There is generally one ZC signal for each pair of signals that can
* switch. The three equations above would broken into two pairs (1)&(2)
* and (2)&(3). The possibility of a "long jump" from (1) to (3) does
* not need to be handled as a separate case. It is implicitly handled.
*
* When a ZCs are calculated, the value is normally used twice. When it is
* first calculated, it is used as the end of the current time step. Later,
* it will be used as the beginning of the following step.
*
* The sign of the ZC signal always indicates an equation from the pair. For
* S-functions, which equation is associated with a positive ZC and which is
* associated with a negative ZC doesn't really matter. If the ZC is positive
* at the beginning and at the end of the time step, this implies that the
* "positive" equation was valid throughout the time step. Likewise, if the
* ZC is negative at the beginning and at the end of the time step, this
* implies that the "negative" equation was valid throughout the time step.
* Like any other nonlinear solver, this is not fool proof, but it is an
* excellent indicator. If the ZC has a different sign at the beginning and
* at the end of the time step, then a equation switch definitely occured
* during the time step.
*
* Ideally, the ZC signal gives an estimate of when an equation switch
* occurred. For example, if the ZC signal is -2 at the beginning and +6 at
* the end, then this suggests that the switch occured
* 25% = 100%*(-2)/(-2-(+6)) of the way into the time step. It will almost
* never be true that 25% is perfectly correct. There is no perfect choice
* for a ZC signal, but there are some good rules. First, choose the ZC
* signal to be continuous. Second, choose the ZC signal to give a monotonic
* measure of the "distance" to a signal switch; strictly monotonic is ideal.
*/
static void mdlZeroCrossings(SimStruct *S)
{
int_T iOutput;
int_T numOutput = ssGetOutputPortWidth(S,0);
real_T *zcSignals = ssGetNonsampledZCs(S);
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
/*
* Set index and increment for the input signal, upper limit, and lower
* limit parameters so that each gives scalar expansion if needed.
*/
int_T uIdx = 0;
int_T uInc = ( ssGetInputPortWidth(S,0) > 1 );
real_T *upperLimit = mxGetPr( P_PAR_UPPER_LIMIT );
int_T upperLimitInc = ( mxGetNumberOfElements( P_PAR_UPPER_LIMIT ) > 1 );
real_T *lowerLimit = mxGetPr( P_PAR_LOWER_LIMIT );
int_T lowerLimitInc = ( mxGetNumberOfElements( P_PAR_LOWER_LIMIT ) > 1 );
/*
* For each output scalar, give the solver a measure of "how close things
* are" to an equation switch.
*/
for ( iOutput = 0; iOutput < numOutput; iOutput++ ) {
/* The switch from eq (1) to eq (2)
*
* if UpperLimit < u then use (1)
* if LowerLimit <= u <= UpperLimit then use (2)
*
* is related to how close u is to UpperLimit. A ZC choice
* that is continuous, strictly monotonic, and is
* u - UpperLimit
* or it is negative.
*/
zcSignals[2*iOutput] = *uPtrs[uIdx] - *upperLimit;
/* The switch from eq (2) to eq (3)
*
* if LowerLimit <= u <= UpperLimit then use (2)
* if u < LowerLimit then use (3)
*
* is related to how close u is to LowerLimit. A ZC choice
* that is continuous, strictly monotonic, and is
* u - LowerLimit.
*/
zcSignals[2*iOutput+1] = *uPtrs[uIdx] - *lowerLimit;
/*
* Adjust indices to give scalar expansion if needed.
*/
uIdx += uInc;
upperLimit += upperLimitInc;
lowerLimit += lowerLimitInc;
}
}
#endif /* end mdlZeroCrossings */
/* Function: mdlTerminate =====================================================
* Abstract:
* No termination needed, but we are required to have this routine.
*/
static void mdlTerminate(SimStruct *S)
{
UNUSED_ARG(S); /* unused input argument */
}
#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 - Variable Step S-Function | Example - Time Varying Continuous Transfer Function | |