scsb_sfun.c~

CheckMate is a MATLAB-based tool for modeling, simulating and investigating properties of hybrid dyn

/* File: scsb_sfun.c
 * Abstract:
 *   C-MEX S-function for the Switched Continuous System Block (SCSB)
 *   in the CheckMate library. To compile the S-function, type "mex
 *   scsb_sfun.c" in the MATLAB command window.
 * Author: Alongkrit Chutinan
 * Date: March 21, 2002
 * Altered by Jim Kapinski
 * April, 2002
 * Altered by Ansgar Fehnker
 * July, 2003
 * Note:
 *   This function has been adapted for the template provided by The
 *   MathWorks.  For more details about S-functions, see
 *   simulink/src/sfuntmpl_doc.c.
 */

#define S_FUNCTION_NAME  scsb_sfun
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"

/* Data indices for S-Function parameter. */
#define NPARAM          12
#define PARAM_NX        0
#define PARAM_NUP       1
#define PARAM_NZ        2
#define PARAM_NU        3
#define PARAM_X0        4
#define PARAM_SWFUNC    5
#define PARAM_P0        6
#define PARAM_USE_RESET 7
#define PARAM_USE_PARAM 8
#define PARAM_USE_SD    9
#define PARAM_AR_CI     10
#define PARAM_AR_DI     11

/* Data indices for pointer work vector. */
#define NPWORK             4
#define PWORK_X_DOT        0
#define PWORK_X_RESET      1
#define PWORK_Z            2
#define PWORK_U_OUT        3

/* Data indices for integer work vector. */
#define NIWORK              8
#define IWORK_U_PORT        0
#define IWORK_RESET_PORT    1
#define IWORK_USE_RESET     2
#define IWORK_LAST_RESET    3
#define IWORK_USE_PARAM     4
#define IWORK_SD_PORT       5
#define IWORK_USE_SD        6
#define IWORK_LAST_SD       7


void computeDerivativeAndReset(SimStruct *S);

/*====================*
 * S-function methods *
 *====================*/

#define MDL_CHECK_PARAMETERS   /* Change to #undef to remove function */
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
/* Function: mdlCheckParameters =============================================
 * Abstract:
 *    mdlCheckParameters verifies new parameter settings whenever parameter
 *    change or are re-evaluated during a simulation. When a simulation is
 *    running, changes to S-function parameters can occur at any time during
 *    the simulation loop.
 *
 *    This method can be called at any point after mdlInitializeSizes.
 *    You should add a call to this method from mdlInitalizeSizes
 *    to check the parameters. After setting the number of parameters
 *    you expect in your S-function via ssSetNumSFcnParams(S,n), you should:
 *     #if defined(MATLAB_MEX_FILE)
 *       if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
 *           mdlCheckParameters(S);
 *           if (ssGetErrorStatus(S) != NULL) return;
 *       } else {
 *           return;     Simulink will report a parameter mismatch error
 *       }
 *     #endif
 *
 *     When a Simulation is running, changes to S-function parameters can
 *     occur either at the start of a simulation step, or during a
 *     simulation step. When changes to S-function parameters occur during
 *     a simulation step, this method is called twice, for the same
 *     parameter changes. The first call during the simulation step is
 *     used to verify that the parameters are correct. After verifying the
 *     new parameters, the simulation continues using the original
 *     parameter values until the next simulation step at which time the
 *     new parameter values will be used. Redundant calls are needed to
 *     maintain simulation consistency.  Note that you cannot access the
 *     work, state, input, output, etc. vectors in this method. This
 *     method should only be used to validate the parameters. Processing
 *     of the parameters should be done in mdlProcessParameters.
 *
 *     See matlabroot/simulink/src/sfun_errhdl.c for an example. 
 */
static void mdlCheckParameters(SimStruct *S)
{
  mxArray *mxTemp;
  int_T nx,nup,nz,nu,nAR,use_sd,use_param;
  real_T temp;

  use_sd = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_USE_SD));
  use_param = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_USE_PARAM));
  
  /* nx must be scalar integer. */
  mxTemp = ssGetSFcnParam(S,PARAM_NX);
  if (!mxIsNumeric(mxTemp)) {
    ssSetErrorStatus(S,"Number of continuous states must be numeric. ");
    return;
  }
  if ((mxGetM(mxTemp) != 1) || (mxGetN(mxTemp) != 1)) {
    ssSetErrorStatus(S,"Number of continuous states must be scalar. ");
    return;
  }
  temp = mxGetScalar(mxTemp);
  nx = (int_T) temp;
  if (temp != (real_T) nx) {
    ssSetErrorStatus(S,"Number of continuous states must be integer. ");
    return;
  }
  if (nx < 0) {
     ssSetErrorStatus(S,"Number of continuous states must be non-negative. ");
     return;
  }
  if (use_sd) {

    /* nup must be scalar integer. */
    mxTemp = ssGetSFcnParam(S,PARAM_NUP);
    if (!mxIsNumeric(mxTemp)) {
        ssSetErrorStatus(S,"Number of discrete-time controller outputs must be numeric. ");
        return;
    }
    if ((mxGetM(mxTemp) != 1) || (mxGetN(mxTemp) != 1)) {
        ssSetErrorStatus(S,"Number of discrete-time controller outputs must be scalar. ");
        return;
    }
    temp = mxGetScalar(mxTemp);
    nup = (int_T) temp;
    if (temp != (real_T) nup) {
        ssSetErrorStatus(S,"Number of discrete-time controller outputs must be integer. ");
        return;
    }
    if (nup < 0) {
        ssSetErrorStatus(S,"Number of discrete-time controller outputs must be non-negative. ");
        return;
    }
  
    /* nz must be scalar integer. */
    mxTemp = ssGetSFcnParam(S,PARAM_NZ);
    if (!mxIsNumeric(mxTemp)) {
        ssSetErrorStatus(S,"Number of discrete-time controller states must be numeric. ");
        return;
    }
    if ((mxGetM(mxTemp) != 1) || (mxGetN(mxTemp) != 1)) {
        ssSetErrorStatus(S,"Number of discrete-time controller states must be scalar. ");
        return;
    }
    temp = mxGetScalar(mxTemp);
    nz = (int_T) temp;
    if (temp != (real_T) nz) {
        ssSetErrorStatus(S,"Number of discrete-time controller states must be integer. ");
        return;
    }
    if (nz < 0) {
        ssSetErrorStatus(S,"Number of discrete-time controller states must be non-negative. ");
        return;
    }
  }
  /* nu must be scalar integer. */
  mxTemp = ssGetSFcnParam(S,PARAM_NU);
  if (!mxIsNumeric(mxTemp)) {
    ssSetErrorStatus(S,"Number of discrete inputs must be numeric. ");
    return;
  }
  if ((mxGetM(mxTemp) != 1) || (mxGetN(mxTemp) != 1)) {
    ssSetErrorStatus(S,"Number of discrete inputs must be scalar. ");
    return;
  }
  temp = mxGetScalar(mxTemp);
  nu = (int_T) temp;
  if (temp != (real_T) nu) {
    ssSetErrorStatus(S,"Number of discrete inputs must be integer. ");
    return;
  }
  if (nu < 0) {
    ssSetErrorStatus(S,"Number of discrete inputs must be non-negative. ");
    return;
  }
/* x0 must be a column vector of length nx. */
  mxTemp = ssGetSFcnParam(S,PARAM_X0);
  if (!mxIsNumeric(mxTemp)) {
    ssSetErrorStatus(S,"Initial conditions must be numeric. ");
    return;
  }
  
  /*Check Initial conditions.  If Sampled-Data Analysis is used, then the initial
  conditions should be in the form [x0;z0]*/
  if (!use_sd) {
    if ((nx == 0 && !mxIsEmpty(mxTemp)) || 
          (nx !=0 && (mxGetM(mxTemp) != nx || mxGetN(mxTemp) != 1))) {
        ssSetErrorStatus(S,"Initial condition and number of continuous states must be consistent. ");
        return;
    }
  }
  
  if (use_sd) {
     if (((nx == 0 && nz == 0 )&& !mxIsEmpty(mxTemp)) || 
          (nx !=0 && (mxGetM(mxTemp) != nx +nz || mxGetN(mxTemp) != 1))) {
        ssSetErrorStatus(S,"Initial condition and number of continuous states must be consistent. ");
        return;
     }
  }
  
  
  /* swfunc must be a string */
  mxTemp = ssGetSFcnParam(S,PARAM_SWFUNC);
  if (!mxIsChar(mxTemp)) {
    ssSetErrorStatus(S,"Switching Function must be a string. ");
    return;
  }
  
  if (use_param) {
      /* p0 must be numeric. */
    mxTemp = ssGetSFcnParam(S,PARAM_P0);
    if (!mxIsNumeric(mxTemp)) {
        ssSetErrorStatus(S,"Default parameter must be numeric. ");
        return;
    }
  }

  /* reset flag must be scalar and real. */
  mxTemp = ssGetSFcnParam(S,PARAM_USE_RESET);
  if (!mxIsNumeric(mxTemp)) {
    ssSetErrorStatus(S,"Reset flag must be numeric. ");
    return;
  }
  if ((mxGetM(mxTemp) != 1) || (mxGetN(mxTemp) != 1)) {
    ssSetErrorStatus(S,"Reset flag must be scalar. ");
    return;
  }

  
  /* AR CI must be a double matrix with nx columns. */
  mxTemp = ssGetSFcnParam(S,PARAM_AR_CI);
  if (!mxIsDouble(mxTemp)) {
    ssSetErrorStatus(S,"Matrix CI for analysis region must be of class 'double'. ");
    return;
  }
  if (use_sd) {
    if (!mxIsEmpty(mxTemp) && mxGetN(mxTemp) != (nx+nz)) {
        ssSetErrorStatus(S,"Matrix CI for analysis region must have same number of columns as number of plant and controller states. ");
        return;
    }
  }
  if (!use_sd) {
    if (!mxIsEmpty(mxTemp) && mxGetN(mxTemp) != nx) {
        ssSetErrorStatus(S,"Matrix CI for analysis region must have same number of columns as number of continuous states. ");
        return;
    }
  }
  if (mxIsEmpty(mxTemp)) {
    nAR = 0;
  }
  else {
    nAR = mxGetM(mxTemp);
  }

  /* AR dI must be a double column vector with nAR rows. */
  mxTemp = ssGetSFcnParam(S,PARAM_AR_DI);
  if (!mxIsDouble(mxTemp)) {
    ssSetErrorStatus(S,"Vector dI for analysis region must be of class 'double'. ");
    return;
  }
  if ((nAR == 0 && !mxIsEmpty(mxTemp)) || 
      (nAR != 0 && (mxGetM(mxTemp) != nAR || mxGetN(mxTemp) != 1))) {
    ssSetErrorStatus(S,"Vector dI for analysis region must be consistent with matrix CI. ");
    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.).
 *
 *    The direct feedthrough flag can be either 1=yes or 0=no. It should be
 *    set to 1 if the input, "u", is used in the mdlOutput function. Setting
 *    this to 0 is akin to making a promise that "u" will not be used in the
 *    mdlOutput function. If you break the promise, then unpredictable results
 *    will occur.
 *
 *    The NumContStates, NumDiscStates, NumInputs, NumOutputs, NumRWork,
 *    NumIWork, NumPWork NumModes, and NumNonsampledZCs widths can be set to:
 *       DYNAMICALLY_SIZED    - In this case, they will be set to the actual
 *                              input width, unless you are have a
 *                              mdlSetWorkWidths to set the widths.
 *       0 or positive number - This explicitly sets item to the specified
 *                              value.
 */
static void mdlInitializeSizes(SimStruct *S)
{
  int_T nx,nup,nz,nu,use_reset,use_sd;
  int_T ninput,noutput;
  int_T u_port,reset_port,x_port,sd_port;
  int_T zero_crossings;

  
  ssSetNumSFcnParams(S,NPARAM);  /* Number of expected parameters */
  if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
    return; /* Parameter mismatch will be reported by Simulink */
  }
  else {
    mdlCheckParameters(S);
    if (ssGetErrorStatus(S) != NULL) return;
  }



  /* Assume that all parameters have been checked above. */
  nx = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NX));
  nu = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NU));
  use_reset = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_USE_RESET));
  use_sd = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_USE_SD));
  if (use_sd) {
    nup = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NUP));
    nz = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NZ));
  }
    
  /* Initialize number of continuous and discrete states.  There are only
  discrete states if sampled-data analysis is used.*/
  if (use_sd) {
    ssSetNumContStates(S,nx+1);
    ssSetNumDiscStates(S,nz+nup);
  }
  else {
  ssSetNumContStates(S,nx+1);
  ssSetNumDiscStates(S,0);
  }
  
  /* Compute port index for the input u. Port index is -1 if there is
     no input signal u. */
  if (nu > 0)
    u_port = 0;
  else
    u_port = -1;
  
  /* Compute port index for the SD clock input.*/  
  if (use_sd)
    sd_port = u_port + 1;
  else
    sd_port = u_port;  
    
  /* Compute port index for the reset signal. Port index is -1 if