Subversion Repositories shark

Rev

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