scsb_sfun.c

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

    
  /* Initialize the continuous state vector to initial values
     specified in S-function parameter. */
  if (nx>0){
  nx = ssGetNumContStates(S);
  memcpy(ssGetContStates(S),mxGetPr(ssGetSFcnParam(S,PARAM_X0)),
         (nx-1)*sizeof(real_T));
  zero=0;
  cont_states = ssGetContStates(S);
  memcpy(&cont_states[nx-1],&zero,
         sizeof(real_T));
         }
 
  /* If sampled-data analysis is used, put the initial controller
     states in the first part of the discrete-state vector.  Then 
     compute the output of the controller and put those values in the
     second part of the discrete-state vector.*/
  if (use_sd) {
    temp = mxGetPr(ssGetSFcnParam(S,PARAM_X0));
    if (nx>0){
    memcpy(ssGetDiscStates(S),&temp[nx-1],
         (nz)*sizeof(real_T));
    } else {
    memcpy(ssGetDiscStates(S),temp,
         (nz)*sizeof(real_T));
    }
    computeDerivativeAndReset(S);
    z_and_u = ssGetDiscStates(S);

    if (nup>0){
    memcpy(&z_and_u[nz],ssGetPWorkValue(S,PWORK_U_OUT),nup*sizeof(real_T)); 
    } 
  }
  
  
}


static void mdlOutputs(SimStruct *S,int_T tid)
{
  
  real_T *cont_out,*disc_out,use_sd;
  int_T nx,nz;  
  
  use_sd = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_USE_SD)); 
  
  nx = ssGetNumContStates(S);
  if (use_sd){
      nz = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NZ));
  }
  
  /* y = x, copy state vector x to output vector y. */
  cont_out = ssGetOutputPortRealSignal(S,0);
  memcpy(cont_out,ssGetContStates(S),
         (ssGetNumContStates(S)-1)*sizeof(real_T));
  if (use_sd){
    disc_out = &cont_out[nx-1];
    memcpy(disc_out,ssGetDiscStates(S),
             nz*sizeof(real_T));
  }
}

#define MDL_UPDATE  /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
static void mdlUpdate(SimStruct *S,int_T tid)
{
  int_T i,j,nx,nz,nup,use_reset,reset_value,use_sd,sd_port,reset_port,clock_value,nAR;
  real_T prod_i;
  real_T *x,*CI,*dI,*z_and_u;
  mxArray *mxCI;
  InputPtrsType      u;
  DTypeId test;



  nx = ssGetNumContStates(S);
  x = ssGetContStates(S);

  /* Check if state reset should be applied. */

  use_reset = ssGetIWorkValue(S,IWORK_USE_RESET);
  if (use_reset) {
    reset_port = ssGetIWorkValue(S,IWORK_RESET_PORT);
    
    /* This code was added by JPK 2/2004.  The purpose is to 
     * determine what the data type of the reset input is and 
     * to handle it accordingly.*/
    test= ssGetInputPortDataType(S,reset_port);
    if (test==8) {
        /* Boolean data type case */
        u     = ssGetInputPortSignalPtrs(S,reset_port);
        if((**(InputBooleanPtrsType) u)==1){        
            reset_value=1;
        }else{
            reset_value=0;
        }      
    } else {
       /* Double data type case */
        if(**ssGetInputPortRealSignalPtrs(S,reset_port)==1){        
            reset_value=1;
        }else{
            reset_value=0;
        } 
    }    
    
    if (ssGetIWorkValue(S,IWORK_LAST_RESET) != reset_value) {
      /* If the reset signal changes, record the new reset signal value. */
      ssSetIWorkValue(S,IWORK_LAST_RESET,reset_value);
      /* Apply state reset. */
      computeDerivativeAndReset(S);
      memcpy(x,ssGetPWorkValue(S,PWORK_X_RESET),(nx-1)*sizeof(real_T));
      /* Update block output since state has changed. */
      mdlOutputs(S,tid);
    }
  }
  
  /* If there is a clock pulse, update controller state and output. */

  z_and_u = ssGetDiscStates(S);
  use_sd = ssGetIWorkValue(S,IWORK_USE_SD);
  
  
  if (use_sd) {
    nz = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NZ));
    nup = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NUP));
    sd_port = ssGetIWorkValue(S,IWORK_SD_PORT);
    clock_value = (int_T) **ssGetInputPortRealSignalPtrs(S,sd_port);
    if ((ssGetIWorkValue(S,IWORK_LAST_SD) < .5)&&(clock_value > .5)) {
      /* If the clock signal goes hi, update the controller. */
      ssSetIWorkValue(S,IWORK_LAST_SD,clock_value);
      computeDerivativeAndReset(S);
      memcpy(z_and_u,ssGetPWorkValue(S,PWORK_Z),nz*sizeof(real_T));
      memcpy(&z_and_u[nz],ssGetPWorkValue(S,PWORK_U_OUT),nup*sizeof(real_T));      
      /* Update block output since state has changed. */
      mdlOutputs(S,tid);
    }
    else if ((ssGetIWorkValue(S,IWORK_LAST_SD) > .5)&&(clock_value < .5)) {
        ssSetIWorkValue(S,IWORK_LAST_SD,clock_value);
    }
  }
  

  /* Check if simulation should be stopped. */
  
  mxCI = ssGetSFcnParam(S,PARAM_AR_CI);
  if (!mxIsEmpty(mxCI)) {
    nAR = mxGetM(mxCI);
    CI = mxGetPr(mxCI);
    dI = mxGetPr(ssGetSFcnParam(S,PARAM_AR_DI));
    /* Compute product CI*x for analysis region. */
    for (i = 0; i < nAR; i++) {
      /* Multiply ith row of CI with x */
      prod_i = 0;
      
      /* If sampled-data analysis is to be performed, analysis
         region includes controller state dimensions. */
      if (use_sd) {
        for (j = 0; j < nx-1; j++) {
            prod_i += CI[j*nAR+i]*x[j];
        }
        for (j = nx-1; j < nx-1+nz;j++) {
            prod_i += CI[j*nAR+i]*z_and_u[j-(nx-1)];
        }
      }  
      else {
        for (j = 0; j < nx-1; j++) {
            prod_i += CI[j*nAR+i]*x[j];      
        }
      }
      
      /* if CIi*x > dIi, then state is out of analysis region. */
      if (prod_i > dI[i]) {
        /* Stop simulation if state is outside of analysis region. */
        ssSetStopRequested(S,1);
        ssWarning(S,"System has left the user-defined Analysis Region. Simulation halted.");
        ssSetErrorStatus(S,"System has left the user-defined Analysis Region. Simulation halted. ");
      }
    }
  }

}
#endif /* MDL_UPDATE */

#define MDL_DERIVATIVES
/* Function: mdlDerivatives =================================================
 * Abstract:
 *    In this function, you compute the S-function block's derivatives.
 *    The derivatives are placed in the derivative vector, ssGetdX(S).
 */
static void mdlDerivatives(SimStruct *S)
{
  /* Call the SCSB wrapper function to compute state derivative and
     state reset vectors. */
 int_T nx;

 nx = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NX));


  computeDerivativeAndReset(S);
 
  /* Copy the derivative computed above from pointer work vector. */
  memcpy(ssGetdX(S),ssGetPWorkValue(S,PWORK_X_DOT),
         (nx+1)*sizeof(real_T));
}


#define MDL_ZERO_CROSSINGS
static void mdlZeroCrossings(SimStruct *S)
{
  real_T *zcSignals;
  int_T use_reset,use_sd,reset_value,reset_port,sd_port,sd_value;

  use_reset = ssGetIWorkValue(S,IWORK_USE_RESET);
  use_sd = ssGetIWorkValue(S,IWORK_USE_SD);
  if (use_reset) {
    reset_port = ssGetIWorkValue(S,IWORK_RESET_PORT);
    /* Assume that the reset signal changes between discrete values of 0
       and 1. Thus, we set the zero crossing detection at 0.5 */
    reset_value = (int_T) **ssGetInputPortRealSignalPtrs(S,reset_port);
    zcSignals = ssGetNonsampledZCs(S);
    zcSignals[0] = (real_T) reset_value - 0.5;
    
    if (use_sd) {
        sd_port = ssGetIWorkValue(S,IWORK_SD_PORT);
        /* Assume that the SD signal changes between discrete values of 0
            and 1. Thus, we set the zero crossing detection at 0.5 */
        sd_value = (int_T) **ssGetInputPortRealSignalPtrs(S,sd_port);
        zcSignals = ssGetNonsampledZCs(S);
        zcSignals[1] = (real_T) sd_value - 0.5;
    } 
  }
  else if (use_sd) {
        sd_port = ssGetIWorkValue(S,IWORK_SD_PORT);
        /* Assume that the SD signal changes between discrete values of 0
            and 1. Thus, we set the zero crossing detection at 0.5 */
        sd_value = (int_T) **ssGetInputPortRealSignalPtrs(S,sd_port);
        zcSignals = ssGetNonsampledZCs(S);
        zcSignals[0] = (real_T) sd_value - 0.5;
  }
}


static void mdlTerminate(SimStruct *S)
{
  /* Free mxArrays and other arrays that we allocated in mdlStart. */
  free(ssGetPWorkValue(S,PWORK_X_DOT)); 
  free(ssGetPWorkValue(S,PWORK_X_RESET)); 
  free(ssGetPWorkValue(S,PWORK_Z)); 
  free(ssGetPWorkValue(S,PWORK_U_OUT));
}


/*========================*
 * Other helper functions *
 *========================*/

/* Function: mxIsVector =======================================================
 * Abstract:
 *    Return 1 if the given mxArray is a vector of length L, i.e. the
 *    mxArray has the size of 1 in one dimension and the size L in the
 *    other dimension. Return 0 otherwise.
 */
int_T mxIsVector(mxArray *A,int_T L)
{
  int_T m,n,st;

  m = mxGetM(A);
  n = mxGetN(A);
  st = ((m == 1) && (n == L)) || ((m == L) && (n == 1));
  return st;
}

/* Function: controller_state_update ==========================================
   Abstract:
        Use the difference equations specified in the user-defined m-file to
        update the state of the discrete-time controller.
*/      

/* Function: computeDerivativeAndReset ========================================
 * Abstract:
 *    Call the SCSB wrapper function, which, in turn, calls the
 *    use-defined switching function to compute the state derivative
 *    and reset vectors given the current state x and discrete input
 *    u. The results are stored in memory allocated inside the pointer
 *    work vector.
 */
void computeDerivativeAndReset(SimStruct *S)
{
  
  mxArray *ArgIn[9];
  mxArray *ArgOut[2];
  mxArray *x_dot_temp;
  int_T i,nx,nz,nu,nup,u_port,use_sd,use_reset,buflen;
  real_T *mxUPr,*z_and_u,*u,*z_idx,one,*u_idx,*x_in,*timer,*x_dot;
  InputRealPtrsType uPtrs;

  use_reset = ssGetIWorkValue(S,IWORK_USE_RESET);
  use_sd = ssGetIWorkValue(S,IWORK_USE_SD);
  
  ArgIn[0] = ssGetSFcnParam(S,PARAM_SWFUNC);

  /* Copy x vector to mxArray to be used as an argument to the wrapper
     function. */
  nx = ssGetNumContStates(S);
  if (use_sd) {
    nz = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NZ));
    ArgIn[1] = mxCreateDoubleMatrix(nx-1+nz,1,mxREAL);
    x_in = mxGetPr(ArgIn[1]);    
    memcpy(x_in,ssGetContStates(S),(nx-1)*sizeof(real_T));     
    memcpy(&x_in[nx-1],ssGetDiscStates(S),nz*sizeof(real_T));
  }
  else {
    nz = 0;    
    ArgIn[1] = mxCreateDoubleMatrix(nx-1,1,mxREAL);    
    x_in = mxGetPr(ArgIn[1]);
    memcpy(x_in,ssGetContStates(S),(nx-1)*sizeof(real_T));
  
  }

  /* Copy u vector to mxArray to be used as an argument to the wrapper
     function. */
  nu = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NU));
  ArgIn[2] = mxCreateDoubleMatrix(nu,1,mxREAL);
  if (nu > 0) {
    u_port = ssGetIWorkValue(S,IWORK_U_PORT);
    uPtrs = ssGetInputPortRealSignalPtrs(S,u_port);
    mxUPr = mxGetPr(ArgIn[2]);
    for (i = 0; i < nu; i++) {
      mxUPr[i] = *uPtrs[i];
    }
  }

  ArgIn[3] = ssGetSFcnParam(S,PARAM_P0);
  ArgIn[4] = ssGetSFcnParam(S,PARAM_USE_RESET);
  ArgIn[5] = ssGetSFcnParam(S,PARAM_USE_PARAM);
  ArgIn[6] = ssGetSFcnParam(S,PARAM_USE_SD);
  if (use_sd) {
     nup = (int_T) *mxGetPr(ssGetSFcnParam(S,PARAM_NUP)); 
     z_and_u = ssGetDiscStates(S); 
     u = &z_and_u[nz];
     ArgIn[7] = mxCreateDoubleMatrix(nup,1,mxREAL);
     memcpy(mxGetPr(ArgIn[7]),u,nup*sizeof(real_T));
  }   
  else {
    ArgIn[7] = mxCreateDoubleMatrix(1,1,mxREAL);
  }
  
  /* Pass the simulation time to the user-defined m-file.
     This will allow the simulation of time varying systems.*/
  ArgIn[8] = mxCreateDoubleMatrix(1,1,mxREAL);
  timer = mxGetPr(ArgIn[8]);     
  memcpy(timer,&ssGetContStates(S)[nx-1],(1)*sizeof(real_T));   
  
  
  /* Call the SCSB wrapper function, which calls the switching
     function to return the derivative and the reset vectors.
       [msg,x_dot,x_reset] = swfunc_wrapper(swfunc,x,u,p0,use_reset,use_param)
   */
   
  mexCallMATLAB(2,ArgOut,9,ArgIn,"scsb_wrapper");   

  mxDestroyArray(ArgIn[1]);
  mxDestroyArray(ArgIn[2]);
  mxDestroyArray(ArgIn[7]);
  mxDestroyArray(ArgIn[8]);
  
  /* Perform error checking to check data integrity and prevent
     possible segmentation fault error. */

  /* Check if derivative is a vector of class 'double' and of
     appropriate length. */
  if (!mxIsDouble(ArgOut[0])) {
    ssSetErrorStatus(S,"Switching function must return derivative vector of class 'double'. ");
    mxDestroyArray(ArgOut[0]);
    mxDestroyArray(ArgOut[1]);
    return;
  }   
  if (use_sd) {
    if (!mxIsVector(ArgOut[0],nx-1+nz+nup)) {
        ssSetErrorStatus(S,"Switching function must return derivative vector with appropriate length. ");
        mxDestroyArray(ArgOut[0]);
        mxDestroyArray(ArgOut[1]);
        return;
    }
  }   
  if (!use_sd) {
    if (!mxIsVector(ArgOut[0],nx-1)) {
        ssSetErrorStatus(S,"Switching function must return derivative vector with appropriate length. ");
        mxDestroyArray(ArgOut[0]);
        mxDestroyArray(ArgOut[1]);
        return;
    }
  }
  if (use_reset) {
    /* Check if reset is a vector of class 'double' and of appropriate
       length. */
    if (!mxIsDouble(ArgOut[1])) {
      ssSetErrorStatus(S,"Switching function must return reset vector of class 'double'. ");
      mxDestroyArray(ArgOut[0]);
      mxDestroyArray(ArgOut[1]);
      return;
    }
    if (!mxIsVector(ArgOut[1],nx-1)) {
      ssSetErrorStatus(S,"Switching function must return reset vector with appropriate length. ");
      mxDestroyArray(ArgOut[0]);
      mxDestroyArray(ArgOut[1]);
      return;
    }
  }
    
  /* If everything checks out, copy output arguments into memory
     allocated in the pointer work vector. */   
  buflen = nx*sizeof(real_T);
  x_dot_temp = mxCreateDoubleMatrix(nx,1,mxREAL);
  x_dot = mxGetPr(x_dot_temp);
  memcpy(x_dot,mxGetPr(ArgOut[0]),(nx-1)*sizeof(real_T));
  one = 1;
  memcpy(&x_dot[nx-1],&one,sizeof(real_T));
  memcpy(ssGetPWorkValue(S,PWORK_X_DOT),mxGetPr(x_dot_temp),buflen);
  if (use_reset) {
    memcpy(ssGetPWorkValue(S,PWORK_X_RESET),mxGetPr(ArgOut[1]),(nx-1)*sizeof(real_T));
  }

  mxDestroyArray(x_dot_temp);   
  
  if (use_sd) {
    z_idx = mxGetPr(ArgOut[0]);
    z_idx = &z_idx[nx-1];
    u_idx = mxGetPr(ArgOut[0]);
    u_idx = &u_idx[nx-1+nz];
    memcpy(ssGetPWorkValue(S,PWORK_Z),z_idx,nz*sizeof(real_T));
    memcpy(ssGetPWorkValue(S,PWORK_U_OUT),u_idx,nup*sizeof(real_T));
  }

  /* Free the mxArrays returned by the above MATLAB function call. */
  mxDestroyArray(ArgOut[0]);
  mxDestroyArray(ArgOut[1]);
  return;
}

#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