Subversion Repositories shark

Rev

Rev 1344 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1342 giacomo 1
#include "tmwtypes.h"
2
#include "rtmodel.h"
3
#include "rt_sim.h"
4
#include "rt_logging.h"
5
#include "rt_nonfinite.h"
6
 
7
#include "ext_work.h"
8
#include "rt_nonfinite.h"
9
 
10
#include "kernel/kern.h"
11
 
12
/*=========*
13
 * Defines *
14
 *=========*/
15
 
16
#ifndef TRUE
17
#define FALSE (0)
18
#define TRUE  (1)
19
#endif
20
 
21
#ifndef EXIT_FAILURE
22
#define EXIT_FAILURE  1
23
#endif
24
#ifndef EXIT_SUCCESS
25
#define EXIT_SUCCESS  0
26
#endif
27
 
28
#define QUOTE1(name) #name
29
#define QUOTE(name) QUOTE1(name)    /* need to expand name    */
30
 
31
#define RUN_FOREVER -1.0
32
 
33
#define EXPAND_CONCAT(name1,name2) name1 ## name2
34
#define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)
35
#define RT_MODEL            CONCAT(MODEL,_rtModel)
36
 
37
extern RT_MODEL *MODEL(void);
38
 
39
extern void MdlInitializeSizes(void);
40
extern void MdlInitializeSampleTimes(void);
41
extern void MdlStart(void);
42
extern void MdlOutputs(int_T tid);
43
extern void MdlUpdate(int_T tid);
44
extern void MdlTerminate(void);
45
 
46
real_T rtInf;
47
real_T rtMinusInf;
48
real_T rtNaN;
49
 
50
# define rt_CreateIntegrationData(S)  \
51
      rtsiSetSolverName(rtmGetRTWSolverInfo(S),"FixedStepDiscrete");
52
# define rt_UpdateContinuousStates(S) \
53
      rtmSetT(S, rtsiGetSolverStopTime(rtmGetRTWSolverInfo(S)));
54
 
55
/*==================================*
56
 * Global data local to this module *
57
 *==================================*/
1368 giacomo 58
 
59
RT_MODEL  *S;
60
const char *status;
61
real_T     finaltime = -2.0;
62
volatile int simulation_run;                                                                                                                            
1342 giacomo 63
static struct {
64
  int_T    stopExecutionFlag;
65
  int_T    isrOverrun;
66
  int_T    overrunFlags[NUMST];
67
  const char_T *errmsg;
68
} GBLbuf;
69
 
70
/* Function: rt_InitInfAndNaN ==================================================
71
 * Abstract:
72
 *      Initialize the rtInf, rtMinusInf, and rtNaN needed by the
73
 *      generated code. NaN is initialized as non-signaling. Assumes IEEE.
74
 */
75
void rt_InitInfAndNaN(int_T realSize)
76
{
77
    short one = 1;
78
    enum {
79
        LittleEndian,
80
        BigEndian
81
    } machByteOrder = (*((char *) &one) == 1) ? LittleEndian : BigEndian;
82
 
83
    switch (realSize) {  
84
      case 4:
85
        switch (machByteOrder) {
86
          case LittleEndian: {
87
              typedef struct {
88
                  uint32_T fraction : 23;
89
                  uint32_T exponent  : 8;
90
                  uint32_T sign      : 1;
91
              } LittleEndianIEEEDouble;
92
 
93
              (*(LittleEndianIEEEDouble*)&rtInf).sign      = 0;
94
              (*(LittleEndianIEEEDouble*)&rtInf).exponent  = 0xFF;
95
              (*(LittleEndianIEEEDouble*)&rtInf).fraction  = 0;
96
              rtMinusInf = rtInf;
97
              rtNaN = rtInf;
98
              (*(LittleEndianIEEEDouble*)&rtMinusInf).sign = 1;
99
              (*(LittleEndianIEEEDouble*)&rtNaN).fraction  = 0x7FFFFF;
100
          }
101
          break;
102
          case BigEndian: {
103
              typedef struct {
104
                  uint32_T sign      : 1;
105
                  uint32_T exponent  : 8;
106
                  uint32_T fraction  : 23;
107
              } BigEndianIEEEDouble;
108
 
109
              (*(BigEndianIEEEDouble*)&rtInf).sign      = 0;
110
              (*(BigEndianIEEEDouble*)&rtInf).exponent  = 0xFF;
111
              (*(BigEndianIEEEDouble*)&rtInf).fraction  = 0;
112
              rtMinusInf = rtInf;
113
              rtNaN = rtInf;
114
              (*(BigEndianIEEEDouble*)&rtMinusInf).sign = 1;
115
              (*(BigEndianIEEEDouble*)&rtNaN).fraction  = 0x7FFFFF;
116
          }
117
          break;
118
        }
119
        break;    
120
 
121
      case 8:
122
        switch (machByteOrder) {
123
          case LittleEndian: {
124
              typedef struct {
125
                  struct {
126
                      uint32_T fraction2;
127
                  } wordH;
128
                  struct {
129
                      uint32_T fraction1 : 20;
130
                      uint32_T exponent  : 11;
131
                      uint32_T sign      : 1;
132
                  } wordL;
133
              } LittleEndianIEEEDouble;
134
 
135
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.sign      = 0;
136
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.exponent  = 0x7FF;
137
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.fraction1 = 0;
138
              (*(LittleEndianIEEEDouble*)&rtInf).wordH.fraction2 = 0;
139
 
140
              rtMinusInf = rtInf;
141
              (*(LittleEndianIEEEDouble*)&rtMinusInf).wordL.sign = 1;
142
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.sign      = 0;
143
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.exponent  = 0x7FF;
144
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.fraction1 = 0xFFFFF;
145
              (*(LittleEndianIEEEDouble*)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
146
          }
147
          break;
148
          case BigEndian: {
149
              typedef struct {
150
                  struct {
151
                      uint32_T sign      : 1;
152
                      uint32_T exponent  : 11;
153
                      uint32_T fraction1 : 20;
154
                  } wordL;
155
                  struct {
156
                      uint32_T fraction2;
157
                  } wordH;
158
              } BigEndianIEEEDouble;
159
 
160
              (*(BigEndianIEEEDouble*)&rtInf).wordL.sign      = 0;
161
              (*(BigEndianIEEEDouble*)&rtInf).wordL.exponent  = 0x7FF;
162
              (*(BigEndianIEEEDouble*)&rtInf).wordL.fraction1 = 0;
163
              (*(BigEndianIEEEDouble*)&rtInf).wordH.fraction2 = 0;
164
 
165
              rtMinusInf = rtInf;
166
              (*(BigEndianIEEEDouble*)&rtMinusInf).wordL.sign = 1;
167
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.sign      = 0;
168
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.exponent  = 0x7FF;
169
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.fraction1 = 0xFFFFF;
170
              (*(BigEndianIEEEDouble*)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
171
          }
172
          break;
173
        }
174
        break;
175
      default:
176
        cprintf("Unable to initialize rtInf, rtMinusInf and rtNaN\n");
177
        sys_end();
178
        break;
179
    }
180
 
181
} /* end rt_InitInfAndNaN */
182
 
183
/* Function: rtOneStep ========================================================
184
 *
185
 * Abstract:
186
 *      Perform one step of the model. This function is modeled such that
187
 *      it could be called from an interrupt service routine (ISR) with minor
188
 *      modifications.
189
 */
190
static void rt_OneStep(RT_MODEL *S)
191
{
192
    real_T tnext;
193
 
194
    /***********************************************
195
     * Check and see if base step time is too fast *
196
     ***********************************************/
197
 
198
    if (GBLbuf.isrOverrun++) {
199
        GBLbuf.stopExecutionFlag = 1;
200
        return;
201
    }
202
 
203
    /***********************************************
204
     * Check and see if error status has been set  *
205
     ***********************************************/
206
 
207
    if (rtmGetErrorStatus(S) != NULL) {
208
        GBLbuf.stopExecutionFlag = 1;
209
        return;
210
    }
211
 
212
    /* enable interrupts here */
213
 
214
    rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
215
                     (boolean_T *)&rtmGetStopRequested(S));
216
 
217
    tnext = rt_SimGetNextSampleHit();
218
    rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
219
 
220
    MdlOutputs(0);
221
 
222
    MdlUpdate(0);
223
    rt_SimUpdateDiscreteTaskSampleHits(rtmGetNumSampleTimes(S),
224
                                       rtmGetTimingData(S),
225
                                       rtmGetSampleHitPtr(S),
226
                                       rtmGetTPtr(S));
227
 
228
    if (rtmGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
229
        rt_UpdateContinuousStates(S);
230
    }
231
 
232
    GBLbuf.isrOverrun--;
233
 
234
    rtExtModeCheckEndTrigger();
235
 
236
} /* end rtOneStep */
237
 
1368 giacomo 238
void Init_RealTime_Workshop() {
1342 giacomo 239
 
1368 giacomo 240
    /****************************
1342 giacomo 241
     * Initialize global memory *
242
     ****************************/
1368 giacomo 243
 
1342 giacomo 244
    (void)memset(&GBLbuf, 0, sizeof(GBLbuf));  
245
 
1368 giacomo 246
    /************************
1342 giacomo 247
     * Initialize the model *
248
     ************************/
1368 giacomo 249
 
1342 giacomo 250
    rt_InitInfAndNaN(sizeof(real_T));
251
 
252
    S = MODEL();
253
    if (rtmGetErrorStatus(S) != NULL) {
254
        cprintf("Error during model registration: %s\n",
255
                      rtmGetErrorStatus(S));
256
        sys_end();
257
    }
1368 giacomo 258
 
1342 giacomo 259
    if (finaltime >= 0.0 || finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);
260
 
261
    MdlInitializeSizes();
262
    MdlInitializeSampleTimes();
263
 
264
    status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(S),
265
                                    rtmGetStepSize(S),
266
                                    rtmGetSampleTimePtr(S),
267
                                    rtmGetOffsetTimePtr(S),
268
                                    rtmGetSampleHitPtr(S),
269
                                    rtmGetSampleTimeTaskIDPtr(S),
270
                                    rtmGetTStart(S),
271
                                    &rtmGetSimTimeStep(S),
272
                                    &rtmGetTimingData(S));
273
 
274
    if (status != NULL) {
275
        cprintf("Failed to initialize sample time engine: %s\n", status);
276
        sys_end();
277
    }
1368 giacomo 278
 
1342 giacomo 279
    rt_CreateIntegrationData(S);
280
 
281
    rtExtModeCheckInit();
282
    rtExtModeWaitForStartMsg(rtmGetRTWExtModeInfo(S),
283
                             (boolean_T *)&rtmGetStopRequested(S));
284
 
1368 giacomo 285
    cprintf("\n** Starting the model **\n");
1342 giacomo 286
 
287
    MdlStart();
288
    if (rtmGetErrorStatus(S) != NULL) {
289
      GBLbuf.stopExecutionFlag = 1;
290
    }
291
 
292
     /*************************************************************************
293
     * Execute the model.  You may attach rtOneStep to an ISR, if so replace *
294
     * the call to rtOneStep (below) with a call to a background task        *
295
     * application.                                                          *
296
     *************************************************************************/
297
 
298
    if (rtmGetTFinal(S) == RUN_FOREVER) {
299
        cprintf("\n**May run forever. Model stop time set to infinity.**\n");
300
    }
301
 
1368 giacomo 302
}
1342 giacomo 303
 
1368 giacomo 304
void Close_RealTime_Workshop() {
1342 giacomo 305
 
306
    /********************
307
     * Cleanup and exit *
308
     ********************/
309
 
310
    rtExtModeShutdown();
311
 
312
    if (GBLbuf.errmsg) {
313
        cprintf("%s\n",GBLbuf.errmsg);
314
        sys_end();
315
    }
316
 
317
    if (GBLbuf.isrOverrun) {
318
        cprintf("%s: ISR overrun - base sampling rate is too fast\n",QUOTE(MODEL));
319
        sys_end();
320
    }
321
 
322
    if (rtmGetErrorStatus(S) != NULL) {
323
        cprintf("%s\n", rtmGetErrorStatus(S));
324
        sys_end();
325
    }
326
 
1368 giacomo 327
    MdlTerminate();
1344 giacomo 328
 
1368 giacomo 329
}
330
 
331
TASK RTW_body(void *arg) {
332
 
333
    simulation_run = 1;
334
 
335
    while (!GBLbuf.stopExecutionFlag &&
336
           (rtmGetTFinal(S) == RUN_FOREVER ||
337
            rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON)) {
338
 
339
        rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
340
                               (boolean_T *)&rtmGetStopRequested(S));
341
 
342
        cprintf(".(%dus)",(int)(rtmGetT(S)*1000000));
343
 
344
        if (rtmGetStopRequested(S)) break;
345
        rt_OneStep(S);
346
 
347
        task_endcycle();
348
 
349
    }
350
 
351
    if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
352
        /* Execute model last time step */
353
        rt_OneStep(S);
354
    }
355
 
356
    simulation_run = 0;
357
 
358
    return NULL;
359
 
360
}
361
 
362
int main() {
363
 
364
    HARD_TASK_MODEL     RTW_task;
365
    PID                 RTW_pid;
366
 
367
    Init_RealTime_Workshop();
368
 
369
    hard_task_default_model(RTW_task);
370
    hard_task_def_mit(RTW_task,rtmGetStepSize(S) * 1000000);
371
    hard_task_def_wcet(RTW_task,10000);
372
    hard_task_def_usemath(RTW_task);
373
    hard_task_def_ctrl_jet(RTW_task);
374
 
375
    RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL);
376
    if (RTW_pid == NIL) {
377
        cprintf("Error creating RealTime Workshop Task\n");
378
        sys_end();
379
    }
380
 
381
    task_activate(RTW_pid);
382
 
383
    while(simulation_run);
384
 
385
    Close_RealTime_Workshop();
386
 
387
    sys_end();
388
 
1342 giacomo 389
    return 0;
390
 
1368 giacomo 391
}