Subversion Repositories shark

Rev

Blame | Last modification | View Log | RSS feed

#include "tmwtypes.h"
#include "rtmodel.h"
#include "rt_sim.h"
#include "rt_logging.h"
#include "rt_nonfinite.h"

#include "ext_work.h"
#include "rt_nonfinite.h"

#include "kernel/kern.h"
#include "modules/cabs.h"
#include "percorso.h"

#include "rtw.h"

/*=========*
 * Defines *
 *=========*/


#ifndef TRUE
#define FALSE (0)
#define TRUE  (1)
#endif

#ifndef EXIT_FAILURE
#define EXIT_FAILURE  1
#endif
#ifndef EXIT_SUCCESS
#define EXIT_SUCCESS  0
#endif

#define QUOTE1(name) #name
#define QUOTE(name) QUOTE1(name)    /* need to expand name    */

#define RUN_FOREVER -1.0

#define EXPAND_CONCAT(name1,name2) name1 ## name2
#define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)
#define RT_MODEL            CONCAT(MODEL,_rtModel)

extern RT_MODEL *MODEL(void);
                                                                                                                             
extern void MdlInitializeSizes(void);
extern void MdlInitializeSampleTimes(void);
extern void MdlStart(void);
extern void MdlOutputs(int_T tid);
extern void MdlUpdate(int_T tid);
extern void MdlTerminate(void);

real_T rtInf;
real_T rtMinusInf;
real_T rtNaN;

CAB    input_cid[NINPUTCAB];
CAB    output_cid[NOUTPUTCAB];

#ifdef EXT_MODE
#  define rtExtModeSingleTaskUpload(S)    \
   {                                      \
        int stIdx;                        \
        rtExtModeUploadCheckTrigger();    \
        for (stIdx=0; stIdx<NUMST; stIdx++) {                   \
            if (rtmIsSampleHit(S, stIdx, 0 /*unused*/)) {       \
                rtExtModeUpload(stIdx,rtmGetTaskTime(S,stIdx)); \
            }                                                   \
        }                                                       \
   }

#else
#  define rtExtModeSingleTaskUpload(S) /* Do nothing */
#endif

#if NCSTATES > 0
  extern void rt_ODECreateIntegrationData(RTWSolverInfo *si);
  extern void rt_ODEUpdateContinuousStates(RTWSolverInfo *si);
                                                                                                                             
# define rt_CreateIntegrationData(S) \
    rt_ODECreateIntegrationData(rtmGetRTWSolverInfo(S));

# define rt_UpdateContinuousStates(S) \
    rt_ODEUpdateContinuousStates(rtmGetRTWSolverInfo(S));

# else
# define rt_CreateIntegrationData(S)  \
      rtsiSetSolverName(rtmGetRTWSolverInfo(S),"FixedStepDiscrete");

# define rt_UpdateContinuousStates(S) \
      rtmSetT(S, rtsiGetSolverStopTime(rtmGetRTWSolverInfo(S)));

#endif

/*==================================*
 * Global data local to this module *
 *==================================*/


RT_MODEL  *S;
const char *status;
real_T     finaltime = -2.0;
volatile int simulation_run;                                                                                                                            
static struct {
  int_T    stopExecutionFlag;
  int_T    isrOverrun;
  int_T    overrunFlags[NUMST];
  const char_T *errmsg;
} GBLbuf;

/* Function: rt_InitInfAndNaN ==================================================
 * Abstract:
 *      Initialize the rtInf, rtMinusInf, and rtNaN needed by the
 *      generated code. NaN is initialized as non-signaling. Assumes IEEE.
 */

void rt_InitInfAndNaN(int_T realSize)
{
    short one = 1;
    enum {
        LittleEndian,
        BigEndian
    } machByteOrder = (*((char *) &one) == 1) ? LittleEndian : BigEndian;

    switch (realSize) {  
      case 4:
        switch (machByteOrder) {
          case LittleEndian: {
              typedef struct {
                  uint32_T fraction : 23;
                  uint32_T exponent  : 8;
                  uint32_T sign      : 1;
              } LittleEndianIEEEDouble;
       
              (*(LittleEndianIEEEDouble*)&rtInf).sign      = 0;
              (*(LittleEndianIEEEDouble*)&rtInf).exponent  = 0xFF;
              (*(LittleEndianIEEEDouble*)&rtInf).fraction  = 0;
              rtMinusInf = rtInf;
              rtNaN = rtInf;
              (*(LittleEndianIEEEDouble*)&rtMinusInf).sign = 1;
              (*(LittleEndianIEEEDouble*)&rtNaN).fraction  = 0x7FFFFF;
          }
          break;
          case BigEndian: {
              typedef struct {
                  uint32_T sign      : 1;
                  uint32_T exponent  : 8;
                  uint32_T fraction  : 23;
              } BigEndianIEEEDouble;
       
              (*(BigEndianIEEEDouble*)&rtInf).sign      = 0;
              (*(BigEndianIEEEDouble*)&rtInf).exponent  = 0xFF;
              (*(BigEndianIEEEDouble*)&rtInf).fraction  = 0;
              rtMinusInf = rtInf;
              rtNaN = rtInf;
              (*(BigEndianIEEEDouble*)&rtMinusInf).sign = 1;
              (*(BigEndianIEEEDouble*)&rtNaN).fraction  = 0x7FFFFF;
          }
          break;
        }
        break;    
   
      case 8:
        switch (machByteOrder) {
          case LittleEndian: {
              typedef struct {
                  struct {
                      uint32_T fraction2;
                  } wordH;
                  struct {
                      uint32_T fraction1 : 20;
                      uint32_T exponent  : 11;
                      uint32_T sign      : 1;
                  } wordL;
              } LittleEndianIEEEDouble;
             
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.sign      = 0;
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.exponent  = 0x7FF;
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.fraction1 = 0;
              (*(LittleEndianIEEEDouble*)&rtInf).wordH.fraction2 = 0;
             
              rtMinusInf = rtInf;
              (*(LittleEndianIEEEDouble*)&rtMinusInf).wordL.sign = 1;
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.sign      = 0;
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.exponent  = 0x7FF;
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.fraction1 = 0xFFFFF;
              (*(LittleEndianIEEEDouble*)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
          }
          break;
          case BigEndian: {
              typedef struct {
                  struct {
                      uint32_T sign      : 1;
                      uint32_T exponent  : 11;
                      uint32_T fraction1 : 20;
                  } wordL;
                  struct {
                      uint32_T fraction2;
                  } wordH;
              } BigEndianIEEEDouble;
             
              (*(BigEndianIEEEDouble*)&rtInf).wordL.sign      = 0;
              (*(BigEndianIEEEDouble*)&rtInf).wordL.exponent  = 0x7FF;
              (*(BigEndianIEEEDouble*)&rtInf).wordL.fraction1 = 0;
              (*(BigEndianIEEEDouble*)&rtInf).wordH.fraction2 = 0;
       
              rtMinusInf = rtInf;
              (*(BigEndianIEEEDouble*)&rtMinusInf).wordL.sign = 1;
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.sign      = 0;
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.exponent  = 0x7FF;
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.fraction1 = 0xFFFFF;
              (*(BigEndianIEEEDouble*)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
          }
          break;
        }
        break;
      default:
        cprintf("Error: Unable to initialize rtInf, rtMinusInf and rtNaN\n");
        sys_end();
        break;
    }

} /* end rt_InitInfAndNaN */

/* Function: rtOneStep ========================================================
 *
 * Abstract:
 *      Perform one step of the model. This function is modeled such that
 *      it could be called from an interrupt service routine (ISR) with minor
 *      modifications.
 */

static void rt_OneStep(RT_MODEL *S)
{
    real_T tnext;
                                                                                                                             
    /***********************************************
     * Check and see if base step time is too fast *
     ***********************************************/

                                                                                                                             
    if (GBLbuf.isrOverrun++) {
        GBLbuf.stopExecutionFlag = 1;
        return;
    }
                                                                                                                             
    /***********************************************
     * Check and see if error status has been set  *
     ***********************************************/

                                                                                                                             
    if (rtmGetErrorStatus(S) != NULL) {
        GBLbuf.stopExecutionFlag = 1;
        return;
    }
                                                                                                                             
    /* enable interrupts here */
                                                                                                                             
    rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
                        (boolean_T *)&rtmGetStopRequested(S));
                                                                                                                             
    tnext = rt_SimGetNextSampleHit();
    rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
                                                                                                                             
    MdlOutputs(0);
                                                                                                                             
    rtExtModeSingleTaskUpload(S);
                                                                                                                             
    if (GBLbuf.errmsg != NULL) {
        GBLbuf.stopExecutionFlag = 1;
        return;
    }
                                                                                                                             
    MdlUpdate(0);
    rt_SimUpdateDiscreteTaskSampleHits(rtmGetNumSampleTimes(S),
                                       rtmGetTimingData(S),
                                       rtmGetSampleHitPtr(S),
                                       rtmGetTPtr(S));
                                                                                                                             
    if (rtmGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
        rt_UpdateContinuousStates(S);
    }
                                                                                                                             
    GBLbuf.isrOverrun--;
                                                                                                                             
    rtExtModeCheckEndTrigger();

} /* end rtOneStep */

void Init_RealTime_Workshop() {

    /****************************
     * Initialize global memory *
     ****************************/

   
    (void)memset(&GBLbuf, 0, sizeof(GBLbuf));  

    /************************
     * Initialize the model *
     ************************/

   
    rt_InitInfAndNaN(sizeof(real_T));

    S = MODEL();
    if (rtmGetErrorStatus(S) != NULL) {
        cprintf("Error: Model registration: %s\n",
                      rtmGetErrorStatus(S));
        sys_end();
    }
   
    if (finaltime >= 0.0 || finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);

    MdlInitializeSizes();
    MdlInitializeSampleTimes();
   
    status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(S),
                                    rtmGetStepSize(S),
                                    rtmGetSampleTimePtr(S),
                                    rtmGetOffsetTimePtr(S),
                                    rtmGetSampleHitPtr(S),
                                    rtmGetSampleTimeTaskIDPtr(S),
                                    rtmGetTStart(S),
                                    &rtmGetSimTimeStep(S),
                                    &rtmGetTimingData(S));

    if (status != NULL) {
        cprintf("Error: Failed to initialize sample time engine: %s\n", status);
        sys_end();
    }
   
    rt_CreateIntegrationData(S);

    rtExtModeCheckInit();
    rtExtModeWaitForStartMsg(rtmGetRTWExtModeInfo(S),
                             (boolean_T *)&rtmGetStopRequested(S));

    cprintf("\n** Starting the model **\n");

    MdlStart();
    if (rtmGetErrorStatus(S) != NULL) {
      GBLbuf.stopExecutionFlag = 1;
    }

     /*************************************************************************
     * Execute the model.  You may attach rtOneStep to an ISR, if so replace *
     * the call to rtOneStep (below) with a call to a background task        *
     * application.                                                          *
     *************************************************************************/


    if (rtmGetTFinal(S) == RUN_FOREVER) {
        cprintf("\n**May run forever. Model stop time set to infinity.**\n");
    }

}

void Close_RealTime_Workshop() {

    /********************
     * Cleanup and exit *
     ********************/


    rtExtModeShutdown();

    if (GBLbuf.errmsg) {
        cprintf("Error: %s\n",GBLbuf.errmsg);
        sys_end();
    }

    if (GBLbuf.isrOverrun) {
        cprintf("Error: %s: ISR overrun - base sampling rate is too fast\n",QUOTE(MODEL));
        sys_end();
    }

    if (rtmGetErrorStatus(S) != NULL) {
        cprintf("Error: %s\n", rtmGetErrorStatus(S));
        sys_end();
    }

    MdlTerminate();

}

TASK RTW_body(void *arg) {
 
    real_T *input, *p;

    input = (real_T *)rtmGetU(S);
    simulation_run = 1;

    while (!GBLbuf.stopExecutionFlag &&
           (rtmGetTFinal(S) == RUN_FOREVER ||
            rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON)) {
                                                                                                                             
        rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
                               (boolean_T *)&rtmGetStopRequested(S));
                                                                                                                             
        if (rtmGetStopRequested(S)) break;
                                                                                                                             
        /* Get PAR0 */
        if (input_cid[0] != -1) {
          p = (real_T *)cab_getmes(input_cid[0]);
          input[0] = *p;
          cab_unget(input_cid[0], p);
        }

        /* Get PAR1 */
        if (input_cid[1] != -1) {
          p = (real_T *)cab_getmes(input_cid[1]);
          input[1] = *p;
          cab_unget(input_cid[1], p);
        }
                                                                                                                             
        rt_OneStep(S);

        task_endcycle();

    }

    if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
        /* Execute model last time step */
        rt_OneStep(S);
    }

    simulation_run = 0;

    return NULL;

}

TASK OUTPUT_body(void *arg) {

  real_T *output, *p;
  real_T th;

  POINT pos;
  PATH_ELEM *e;
  real_T distance = 0;
  real_T angle = 0;
  real_T curve = 0;

  output_cid[0] = cab_create("DISTANCE", sizeof(real_T), 2);
  output_cid[1] = cab_create("ANGLE", sizeof(real_T), 2);
  output_cid[2] = cab_create("CURVE", sizeof(real_T), 2);

  output = (real_T *)rtmGetY(S);

  while(1) {

    pos.x = output[0];
    pos.y = output[1];
    th = output[2];
   
    e = find_closest_elem(&pos, 1.0);
    if (e != 0) {
      distance = get_distance_from_elem(&pos, e);
      angle = th - get_angle_from_elem(&pos,e);
      curve = is_curve(e);
    }
   
    /* Send Distance */
    p = (real_T *)cab_reserve(output_cid[0]);
    *p = distance;
    cab_putmes(output_cid[0], p);

    /* Send Angle */
    p = (real_T *)cab_reserve(output_cid[1]);
    *p = angle;
    cab_putmes(output_cid[1], p);

    /* Send Curve */
    p = (real_T *)cab_reserve(output_cid[2]);
    *p = curve;
    cab_putmes(output_cid[2], p);

    task_endcycle();
  }
  return NULL;
}

TASK OUTPUT_w_body(void *arg) {

  real_T *output, *p;

  real_T wr = 0;
  real_T wl = 0;

  output_cid[0] = cab_create("WR", sizeof(real_T), 2);
  output_cid[1] = cab_create("WL", sizeof(real_T), 2);

  output = (real_T *)rtmGetY(S);

  while(1) {

    wr = output[3];
    wl = output[4];  
 
    /* Send wr */
    p = (real_T *)cab_reserve(output_cid[3]);
    *p = wr;
    cab_putmes(output_cid[3], p);

    /* Send wl */
    p = (real_T *)cab_reserve(output_cid[4]);
    *p = wl;
    cab_putmes(output_cid[4], p);

    task_endcycle();

  }

  return NULL;

}

void Init_Rtw() {

    HARD_TASK_MODEL     RTW_task,OUTPUT_task;
    PID                 RTW_pid,OUTPUT_pid,OUTPUT_w_pid;
                                                                                                                             
    int i;
                                                                                                                             
    int RTW_period;
    int RTW_wcet;
                                                                                                                             
    int OUTPUT_period;
    int OUTPUT_wcet;

    Init_RealTime_Workshop();
    Init_All_Path();

    for (i=0;i<NINPUTCAB;i++) input_cid[i] = -1;
    for (i=0;i<NOUTPUTCAB;i++) output_cid[i] = -1;
                                                                                                                             
    RTW_period =(int)(rtmGetStepSize(S) * 1000000.0);
    RTW_wcet = (int)(rtmGetStepSize(S) * 1000000.0 * 0.45);
                                                                                                                             
    OUTPUT_period = (int)(10000.0);
    OUTPUT_wcet = (int)(10000.0 * 0.10);
                                                                                                                             
    cprintf("Task Setup\n");
    cprintf("RTW    (P %8d W %8d)\n",RTW_period,RTW_wcet);
    cprintf("OUTPUT (P %8d W %8d)\n",OUTPUT_period,OUTPUT_wcet);
                                                                                                                             
    hard_task_default_model(RTW_task);
    hard_task_def_mit(RTW_task,RTW_period);
    hard_task_def_wcet(RTW_task,RTW_wcet);
    hard_task_def_usemath(RTW_task);
    hard_task_def_ctrl_jet(RTW_task);
                                                                                                                             
    RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL);
    if (RTW_pid == NIL) {
        cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n");
        sys_end();
    }

    hard_task_default_model(OUTPUT_task);
    hard_task_def_mit(OUTPUT_task,OUTPUT_period);
    hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet);
    hard_task_def_usemath(OUTPUT_task);
    hard_task_def_ctrl_jet(OUTPUT_task);
                                                                                                                             
    OUTPUT_pid = task_create("OUTPUT",OUTPUT_body,&OUTPUT_task,NULL);
    if (OUTPUT_pid == NIL) {
        cprintf("Error: Cannot create RealTime Workshop [OUTPUT] Task\n");
        sys_end();
    }
                               
    hard_task_def_mit(OUTPUT_task,OUTPUT_period);
    hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet);
   
    OUTPUT_w_pid = task_create("OUTPUTW",OUTPUT_w_body,&OUTPUT_task,NULL);
    if (OUTPUT_pid == NIL) {
        cprintf("Error: Cannot create RealTime Workshop [OUTPUTW] Task\n");
        sys_end();
    }
                                                                                             
    task_activate(RTW_pid);
    task_activate(OUTPUT_pid);
    task_activate(OUTPUT_w_pid);

}

int main() {

    Init_Rtw();

    activate_sensors();
    activate_control();

    while(simulation_run);
 
    Close_RealTime_Workshop();

    sys_end();

    return 0;

}