/* 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       7void 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)));  }