Subversion Repositories shark

#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;

}

void Init_Rtw() {

    HARD_TASK_MODEL     RTW_task,OUTPUT_task;

    PID                 RTW_pid,OUTPUT_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();

    }

    task_activate(RTW_pid);

    task_activate(OUTPUT_pid);

}

int main() {

    Init_Rtw();

    activate_sensors();

    activate_control();

    while(simulation_run);

    Close_RealTime_Workshop();

    sys_end();

    return 0;

}