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 "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) {
 
    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;
        rt_OneStep(S);

        task_endcycle();

    }

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

    simulation_run = 0;

    return NULL;

}

TASK INPUT_body(void *input_port) {

    real_T *input, *p;
    int in = (int)(input_port);

    if (in >= NINPUTCAB) return NULL;

    input = (real_T *)rtmGetU(S);

    while(1) {

        /* Get CAB message */
        p = (real_T *)cab_getmes(input_cid[in]);

        /* Set INPUT port */
        input[in] = *p;

        /* Release CAB message */
        cab_unget(input_cid[in], p);

        task_endcycle();

    }

    return NULL;

}

TASK OUTPUT_body(void *output_port) {

    real_T *output, *p;
    int out = (int)(output_port);
    char cname[20];

    if (out >= NOUTPUTCAB) return NULL;                                    
                                                                                       
    sprintf(cname,"OUTPUT%d",out);
    output_cid[out] = cab_create(cname, sizeof(real_T), 2);
                                                                                                                             
    output = (real_T *)rtmGetY(S);
                                                                                                                             
    while(1) {
                                                                                                                             
        /* Reserve a message */
        p = (real_T *)cab_reserve(output_cid[out]);

        /* Save data */
        *p = output[out];

        /* Put CAB message */
        cab_putmes(output_cid[out], p);
                                                                                                                             
        task_endcycle();
                                                                                                                             
    }
                                                                                                                             
    return NULL;
                                                                                                                             
}

int main() {

    HARD_TASK_MODEL     RTW_task,INPUT_task,OUTPUT_task;
    PID                 RTW_pid,INPUT_pid,OUTPUT_pid;

    int i;

    int RTW_period;
    int RTW_wcet;

    int INPUT_period;
    int INPUT_wcet;

    int OUTPUT_period;
    int OUTPUT_wcet;

    Init_RealTime_Workshop();

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

    INPUT_period = (int)(10000.0);
    INPUT_wcet = (int)(10000.0 * 0.10);

    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("INPUT  (P %8d W %8d)\n",INPUT_period,INPUT_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(INPUT_task);
    hard_task_def_mit(INPUT_task,INPUT_period);
    hard_task_def_wcet(INPUT_task,INPUT_wcet);
    /* Set input port number */
    hard_task_def_arg(INPUT_task,(void *)(0));
    hard_task_def_usemath(INPUT_task);
    hard_task_def_ctrl_jet(INPUT_task);
                                                                                                                             
    INPUT_pid = task_create("INPUT0",INPUT_body,&INPUT_task,NULL);
    if (INPUT_pid == NIL) {
        cprintf("Error: Cannot create RealTime Workshop [INPUT0] 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);
    /* Set output port number */
    hard_task_def_arg(OUTPUT_task,(void *)(0));
    hard_task_def_usemath(OUTPUT_task);
    hard_task_def_ctrl_jet(OUTPUT_task);
                                                                                                                             
    OUTPUT_pid = task_create("OUTPUT0",OUTPUT_body,&OUTPUT_task,NULL);
    if (OUTPUT_pid == NIL) {
        cprintf("Error: Cannot create RealTime Workshop [OUTPUT0] Task\n");
        sys_end();
    }

    task_activate(INPUT_pid);
    task_activate(RTW_pid);
    task_activate(OUTPUT_pid);

    activate_sensors();
    activate_actuators();

    while(simulation_run);
 
    Close_RealTime_Workshop();

    sys_end();

    return 0;

}