sf_trgen.cc

#include <list>
#include <math.h>
#include <exception>
#include <typeinfo>
#include <iterator>
#include <trgen.h>

#define S_FUNCTION_LEVEL 2
#define S_FUNCTION_NAME  sf_trgen
#include "simstruc.h"

#ifdef __cplusplus
extern "C" { // use the C fcn-call standard for all functions  
#endif       // defined within this scope                     

// Dialog parameters
#define DP_PATH_IDX 0
#define PATH_PARAM(S) (ssGetSFcnParam(S,DP_PATH_IDX))

#define DP_INITPOS_IDX 1
#define INITPOS_PARAM(S) (ssGetSFcnParam(S,DP_INITPOS_IDX))

#define DP_TRAJECTORY_CONSTRAINTS_IDX 2
#define TRAJECTORY_CONSTRAINTS_PARAM(S) (ssGetSFcnParam(S,DP_TRAJECTORY_CONSTRAINTS_IDX))

// Work vectors
#define PW_TRAJECTORY_IDX 0
#define TRAJECTORY_OBJECT(S) ((Trajectory *)ssGetPWork(S)[PW_TRAJECTORY_IDX])

/*
 * Need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */
#include "simstruc.h"

#define IS_PARAM_DOUBLE(pVal) (mxIsNumeric(pVal) && !mxIsLogical(pVal) &&\
!mxIsEmpty(pVal) && !mxIsSparse(pVal) && !mxIsComplex(pVal) && mxIsDouble(pVal))

/*====================*
 * S-function methods *
 *====================*/

#define MDL_CHECK_PARAMETERS
#if defined(MDL_CHECK_PARAMETERS)  && defined(MATLAB_MEX_FILE)
/*
 * Check to make sure that each parameter is 1-d and positive
 */
static void mdlCheckParameters(SimStruct *S)
{

    const mxArray *pPath = PATH_PARAM(S);
    const mxArray *initpos = INITPOS_PARAM(S);
    const mxArray *wc = TRAJECTORY_CONSTRAINTS_PARAM(S);

    if (mxGetM(pPath) != 2) {
        ssSetErrorStatus(S, "Parameter to S-function must be a two row array");
        return;
    }
    if (mxGetN(pPath) < 1) {
        ssSetErrorStatus(S, "Parameter to S-function must have at least one points (columns)");
        return;
    }
    if (mxGetN(initpos)*mxGetM(initpos) != 3) {
        ssSetErrorStatus(S, "The third parameter (initpos) must have three elements.");
        return;
    }
    if (mxGetN(wc)*mxGetM(wc) != 6) {
        ssSetErrorStatus(S, "The fourth parameter (constraints) must have six elements.");
        return;
    }
}
#endif


/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    /* See sfuntmpl.doc for more details on the macros below */

    ssSetNumSFcnParams(S, 3);  /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
    if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
        mdlCheckParameters(S);
        if (ssGetErrorStatus(S) != NULL) {
            return;
        }
    } else {
        return; /* Parameter mismatch will be reported by Simulink */
    }
#endif
    ssSetSFcnParamTunable(S, DP_PATH_IDX, 1);
    ssSetSFcnParamTunable(S, DP_INITPOS_IDX, 0);
    ssSetSFcnParamTunable(S, DP_TRAJECTORY_CONSTRAINTS_IDX, 0);

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 0)) return;
    
    if (!ssSetNumOutputPorts(S, 1)) return;
    ssSetOutputPortWidth(S, 0, 5);


    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 1); // reserve element in the pointers vector
    ssSetNumModes(S, 0); // to store a C++ object
    ssSetNumNonsampledZCs(S, 0);

    ssSetOptions(S, 0);
}



/* 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.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, 0.05);
    ssSetOffsetTime(S, 0, 0.0);
    ssSetModelReferenceSampleTimeDefaultInheritance(S);
}


#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START) 
  /* 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)
  {
      const double *initpos = mxGetPr(INITPOS_PARAM(S));
      const double *wc = mxGetPr(TRAJECTORY_CONSTRAINTS_PARAM(S));
      Pos initPos;
      TrajectoryConstraints trajectoryConstraints;
      Trajectory *trajectory;
      initPos.x = initpos[0];
      initPos.y = initpos[1];
      initPos.phi = initpos[2];
      trajectoryConstraints.maxv = wc[0];
      trajectoryConstraints.maxomega = wc[1];
      trajectoryConstraints.maxangacc = wc[2];
      trajectoryConstraints.maxacc = wc[3];
      trajectoryConstraints.maxcenacc = wc[4];
      trajectoryConstraints.maxe = wc[5];
      trajectory = new Trajectory(trajectoryConstraints);
      trajectory->finalHeading = (double)90.0/180*M_PI;
      //ssGetPWork(S)[PW_REG_IDX] = (void*)new PosRegCmu(); // store new C++ object in the
                                                 // pointers vector

      ssGetPWork(S)[PW_TRAJECTORY_IDX] = (void*)trajectory;
      
      double *path = mxGetPr(PATH_PARAM(S));
      for (unsigned i=0; i<mxGetN(PATH_PARAM(S)); i++) {
          trajectory->addPoint(path[2*i], path[2*i+1]);
          if (i>0) {
              char cmd[300];
              sprintf(cmd, "plot([%g %g], [%g %g], '--bo')",
                      path[2*i-2], path[2*i], path[2*i-1], path[2*i+1]);
              mexEvalString(cmd);
          }
      }

      trajectory->prepare(initPos);
      trajectory->plot("-m");
  }
#endif /*  MDL_START */


/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block. Generally outputs are placed in the output vector, ssGetY(S).
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    //PosRegCmu *reg = REG_OBJECT(S);   // retrieve C++ object from
    real_T  *y = ssGetOutputPortRealSignal(S,0); 
    Trajectory *trajectory = TRAJECTORY_OBJECT(S);
    Pos rp;
    
    trajectory->getRefPos(ssGetT(S), rp);
    y[0] = rp.x;
    y[1] = rp.y;
    y[2] = rp.phi;
    y[3] = rp.v;
    y[4] = rp.omega;

    //ssPrintf("%g %g %g %g %g\n", y[0], y[1], y[2], y[3], y[4]);

    UNUSED_ARG(tid);
}                                                

/* 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.
 */
static void mdlTerminate(SimStruct *S)
{
    delete TRAJECTORY_OBJECT(S);
}                                              // function
/*======================================================*
 * See sfuntmpl.doc for the optional S-function methods *
 *======================================================*/

/*=============================*
 * Required S-function trailer *
 *=============================*/

#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

#ifdef __cplusplus
} // end of extern "C" scope
#endif

---