scsb_sfun.c~

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

     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);  
  
}


/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *
 *    This function is used to specify the sample time(s) for your S-function.
 *    You must register the same number of sample times as specified in
 *    ssSetNumSampleTimes. If you specify that you have no sample times, then
 *    the S-function is assumed to have one inherited sample time.
 *
 *    The sample times are specified as pairs "[sample_time, offset_time]"
 *    via the following macros:
 *      ssSetSampleTime(S, sampleTimePairIndex, sample_time)
 *      ssSetOffsetTime(S, offsetTimePairIndex, offset_time)
 *    Where sampleTimePairIndex starts at 0.
 *
 *    The valid sample time pairs are (upper case values are macros defined
 *    in simstruc.h):
 *
 *      [CONTINUOUS_SAMPLE_TIME,  0.0                       ]
 *      [CONTINUOUS_SAMPLE_TIME,  FIXED_IN_MINOR_STEP_OFFSET]
 *      [discrete_sample_period,  offset                    ]
 *      [VARIABLE_SAMPLE_TIME  ,  0.0                       ]
 *
 *    Alternatively, you can specify that the sample time is inherited from the
 *    driving block in which case the S-function can have only one sample time
 *    pair:
 *
 *      [INHERITED_SAMPLE_TIME,  0.0                       ]
 *    or
 *      [INHERITED_SAMPLE_TIME,  FIXED_IN_MINOR_STEP_OFFSET]
 *
 *    The following guidelines may help aid in specifying sample times:
 *
 *      o A continuous function that changes during minor integration steps
 *        should register the [CONTINUOUS_SAMPLE_TIME, 0.0] sample time.
 *      o A continuous function that does not change during minor integration
 *        steps should register the
 *              [CONTINUOUS_SAMPLE_TIME, FIXED_IN_MINOR_STEP_OFFSET]
 *        sample time.
 *      o A discrete function that changes at a specified rate should register
 *        the discrete sample time pair
 *              [discrete_sample_period, offset]
 *        where
 *              discrete_sample_period > 0.0 and
 *              0.0 <= offset < discrete_sample_period
 *      o A discrete function that changes at a variable rate should
 *        register the variable step discrete [VARIABLE_SAMPLE_TIME, 0.0]
 *        sample time. The mdlGetTimeOfNextVarHit function is called to get
 *        the time of the next sample hit for the variable step discrete task.
 *        Note, the VARIABLE_SAMPLE_TIME can be used with variable step
 *        solvers only.
 *      o Discrete blocks which can operate in triggered subsystems.  For your 
 *        block to operate correctly in a triggered subsystem or a periodic 
 *        system it must register [INHERITED_SAMPLE_TIME, 0.0]. In a triggered
 *        subsystem after sample times have been propagated throughout the
 *        block diagram, the assigned sample time to the block will be 
 *        [INHERITED_SAMPLE_TIME, INHERITED_SAMPLE_TIME]. Typically discrete
 *        blocks which can be periodic or reside within triggered subsystems
 *        need to register the inherited sample time and the option
 *        SS_DISALLOW_CONSTANT_SAMPLE_TIME. Then in mdlSetWorkWidths, they
 *        need to verify that they were assigned a discrete or triggered
 *        sample time. To do this:
 *          mdlSetWorkWidths:
 *            if (ssGetSampleTime(S, 0) == CONTINUOUS_SAMPLE_TIME) {
 *              ssSetErrorStatus(S, "This block cannot be assigned a "
 *                               "continuous sample time");
 *            }
 *
 *    If your function has no intrinsic sample time, then you should indicate
 *    that your sample time is inherited according to the following guidelines:
 *
 *      o A function that changes as its input changes, even during minor
 *        integration steps should register the [INHERITED_SAMPLE_TIME, 0.0]
 *        sample time.
 *      o A function that changes as its input changes, but doesn't change
 *        during minor integration steps (i.e., held during minor steps) should
 *        register the [INHERITED_SAMPLE_TIME, FIXED_IN_MINOR_STEP_OFFSET]
 *        sample time.
 *
 *    To check for a sample hit during execution (in mdlOutputs or mdlUpdate),
 *    you should use the ssIsSampleHit or ssIsContinuousTask macros.
 *    For example, if your first sample time is continuous, then you
 *    used the following code-fragment to check for a sample hit. Note,
 *    you would get incorrect results if you used ssIsSampleHit(S,0,tid).
 *        if (ssIsContinuousTask(S,tid)) {
 *        }
 *    If say, you wanted to determine if the third (discrete) task has a hit,
 *    then you would use the following code-fragment:
 *        if (ssIsSampleHit(S,2,tid) {
 *        }
 *
 */
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 */
/* Function: mdlStart =======================================================
 * Abstract:
 *    This function is called once at start of model execution. If you
 *    have states that should be initialized once, this is the place
 *    to do it.
 */
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)));
  }
    
  /* 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)); 
    } 
  }
  
  
}


/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block. Generally outputs are placed in the output vector(s),
 *    ssGetOutputPortSignal.
 */
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)
/* Function: mdlUpdate ======================================================
 * Abstract:
 *    This function is called once for every major integration time step.
 *    Discrete states are typically updated here, but this function is useful
 *    for performing any tasks that should only take place once per
 *    integration step.
 */
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);