scsb_sfun.c~

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

    }
  }
  
  /* 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
/* Function: mdlZeroCrossings ===============================================
 * Abstract:
 *    If your S-function has registered CONTINUOUS_SAMPLE_TIME and there
 *    are signals entering the S-function or internally generated signals
 *    which have discontinuities, you can use this method to locate the
 *    discontinuities. When called, this method must update the
 *    ssGetNonsampleZCs(S) vector.
 */
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;
  }
}


/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was allocated
 *    in mdlStart, this is the place to free it.
 *
 *    Suppose your S-function allocates a few few chunks of memory in mdlStart
 *    and saves them in PWork. The following code fragment would free this
 *    memory.
 *        {
 *            int i;
 *            for (i = 0; i<ssGetNumPWork(S); i++) {
 *                if (ssGetPWorkValue(S,i) != NULL) {
 *                    free(ssGetPWorkValue(S,i));
 *                }
 *            }
 *        }
 */
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