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)

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 */

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
     there is no reset signal.  Note that the sampled-data clock input is now the first
     input port after the discrete-location inputs. */
  if (use_reset)
    reset_port = sd_port + 1;
  else
    reset_port = -1;
  
  ninput = (nu > 0) + use_sd + use_reset;
   
  if (!ssSetNumInputPorts(S,ninput)) return;

  /* Initialize size and direct feedthrough status for u port. */
  if (u_port != -1) {
    ssSetInputPortWidth(S,u_port,nu);
    ssSetInputPortDirectFeedThrough(S,u_port,0);
  }

  /* Initialize size and direct feedthrough status for sampled-data clock input port. */
  if (use_sd) {
    ssSetInputPortWidth(S,sd_port,1);
    ssSetInputPortDirectFeedThrough(S,sd_port,0);
  }

  /* Initialize size, type, and direct feedthrough status for reset port. */
  if (reset_port != -1) {
    ssSetInputPortWidth(S,reset_port,1);
    ssSetInputPortDirectFeedThrough(S,reset_port,0);
   
    ssSetInputPortDataType(S,reset_port,DYNAMICALLY_TYPED);
  }

  /* Compute port index for the state output port x. Port index is -1
     if the output x is not available. */
  if ((nx > 0)||(nz>0))
    x_port = 0;
  else
    x_port = -1;

  noutput = (nx > 0)||(nz > 0);

  if (!ssSetNumOutputPorts(S,noutput)) return;
  
  /* Initialize the size for the x port.  If sampled-data analysis is used,
  then make the output the continuous-time state, the controller output, and 
  the discrete-time controller state.*/
  if (x_port != -1) {
    if (use_sd)
        ssSetOutputPortWidth(S,x_port,nx+nz);
    else
        ssSetOutputPortWidth(S,x_port,nx);
   }
        
  ssSetNumSampleTimes(S,1);
  ssSetNumRWork(S,0);
  ssSetNumDWork(S,0);
  ssSetNumIWork(S,NIWORK);
  ssSetNumPWork(S,NPWORK);

  
  zero_crossings = 0;
  if (use_reset) {
    /* Declare that we will track one zero crossing signal, namely the
       reset signal. */
    zero_crossings = 1;   
  }
  if (use_sd) {
    zero_crossings++;
  }
  ssSetNumNonsampledZCs(S,zero_crossings);  
  
}


static void mdlInitializeSampleTimes(SimStruct *S)
{
  /* Specify that we have a continuous sample time. */
  ssSetSampleTime(S,0,CONTINUOUS_SAMPLE_TIME);
  ssSetOffsetTime(S,0,0.0);
}


#define MDL_START  /* Change to #undef to remove function */

static void mdlStart(SimStruct *S)
{
  int_T nx,nu,nz,nup,use_reset,u_port,reset_port,use_param,use_sd,sd_port;
  real_T *z_and_u,*temp,zero,*cont_states;  
  
  /* Assume that all parameters have been checked previously. */
  use_reset = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_USE_RESET));
  use_param = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_USE_PARAM));
  use_sd = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_USE_SD)); 
  nx = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NX));
  if (use_sd) {
    nup = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NUP));
    nz = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NZ));
  }
  nu = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NU));
  
  
  /* 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
     there is no reset signal.  Note that the sampled-data clock input is now the first
     input port after the discrete-location inputs. */
  if (use_reset)
    reset_port = sd_port + 1;
  else
    reset_port = -1;
  
  /* Store port indices in the integer work vector so that we don't
     have to compute them again. */
  ssSetIWorkValue(S,IWORK_U_PORT,u_port);
  ssSetIWorkValue(S,IWORK_RESET_PORT,reset_port);
  ssSetIWorkValue(S,IWORK_USE_RESET,use_reset);
  ssSetIWorkValue(S,IWORK_USE_PARAM,use_param);
  ssSetIWorkValue(S,IWORK_USE_SD,use_sd);
  ssSetIWorkValue(S,IWORK_SD_PORT,sd_port);
  
  
  /* Initialize pointer work vector and allocate some mxArrays to
     store arguments for the SCSB wrapper function. */
  ssSetPWorkValue(S,PWORK_X_DOT,calloc((nx+1),sizeof(real_T)));
  ssSetPWorkValue(S,PWORK_X_RESET,calloc(nx,sizeof(real_T)));
  if (use_sd){
    ssSetPWorkValue(S,PWORK_Z,calloc(nz,sizeof(real_T)));
    ssSetPWorkValue(S,PWORK_U_OUT,calloc(nup,sizeof(real_T)));
  }