Subversion Repositories shark

Rev

Rev 1415 | 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
 
1415 giacomo 50
#ifdef EXT_MODE
51
#  define rtExtModeSingleTaskUpload(S)    \
52
   {                                      \
53
        int stIdx;                        \
54
        rtExtModeUploadCheckTrigger();    \
55
        for (stIdx=0; stIdx<NUMST; stIdx++) {                   \
56
            if (rtmIsSampleHit(S, stIdx, 0 /*unused*/)) {       \
57
                rtExtModeUpload(stIdx,rtmGetTaskTime(S,stIdx)); \
58
            }                                                   \
59
        }                                                       \
60
   }
61
#else
62
#  define rtExtModeSingleTaskUpload(S) /* Do nothing */
63
#endif
64
 
1402 giacomo 65
#if NCSTATES > 0
66
  extern void rt_ODECreateIntegrationData(RTWSolverInfo *si);
67
  extern void rt_ODEUpdateContinuousStates(RTWSolverInfo *si);
68
 
69
# define rt_CreateIntegrationData(S) \
70
    rt_ODECreateIntegrationData(rtmGetRTWSolverInfo(S));
71
# define rt_UpdateContinuousStates(S) \
72
    rt_ODEUpdateContinuousStates(rtmGetRTWSolverInfo(S));
73
# else
1342 giacomo 74
# define rt_CreateIntegrationData(S)  \
75
      rtsiSetSolverName(rtmGetRTWSolverInfo(S),"FixedStepDiscrete");
76
# define rt_UpdateContinuousStates(S) \
77
      rtmSetT(S, rtsiGetSolverStopTime(rtmGetRTWSolverInfo(S)));
1402 giacomo 78
#endif
1342 giacomo 79
 
80
/*==================================*
81
 * Global data local to this module *
82
 *==================================*/
1368 giacomo 83
 
84
RT_MODEL  *S;
85
const char *status;
86
real_T     finaltime = -2.0;
87
volatile int simulation_run;                                                                                                                            
1342 giacomo 88
static struct {
89
  int_T    stopExecutionFlag;
90
  int_T    isrOverrun;
91
  int_T    overrunFlags[NUMST];
92
  const char_T *errmsg;
93
} GBLbuf;
94
 
95
/* Function: rt_InitInfAndNaN ==================================================
96
 * Abstract:
97
 *      Initialize the rtInf, rtMinusInf, and rtNaN needed by the
98
 *      generated code. NaN is initialized as non-signaling. Assumes IEEE.
99
 */
100
void rt_InitInfAndNaN(int_T realSize)
101
{
102
    short one = 1;
103
    enum {
104
        LittleEndian,
105
        BigEndian
106
    } machByteOrder = (*((char *) &one) == 1) ? LittleEndian : BigEndian;
107
 
108
    switch (realSize) {  
109
      case 4:
110
        switch (machByteOrder) {
111
          case LittleEndian: {
112
              typedef struct {
113
                  uint32_T fraction : 23;
114
                  uint32_T exponent  : 8;
115
                  uint32_T sign      : 1;
116
              } LittleEndianIEEEDouble;
117
 
118
              (*(LittleEndianIEEEDouble*)&rtInf).sign      = 0;
119
              (*(LittleEndianIEEEDouble*)&rtInf).exponent  = 0xFF;
120
              (*(LittleEndianIEEEDouble*)&rtInf).fraction  = 0;
121
              rtMinusInf = rtInf;
122
              rtNaN = rtInf;
123
              (*(LittleEndianIEEEDouble*)&rtMinusInf).sign = 1;
124
              (*(LittleEndianIEEEDouble*)&rtNaN).fraction  = 0x7FFFFF;
125
          }
126
          break;
127
          case BigEndian: {
128
              typedef struct {
129
                  uint32_T sign      : 1;
130
                  uint32_T exponent  : 8;
131
                  uint32_T fraction  : 23;
132
              } BigEndianIEEEDouble;
133
 
134
              (*(BigEndianIEEEDouble*)&rtInf).sign      = 0;
135
              (*(BigEndianIEEEDouble*)&rtInf).exponent  = 0xFF;
136
              (*(BigEndianIEEEDouble*)&rtInf).fraction  = 0;
137
              rtMinusInf = rtInf;
138
              rtNaN = rtInf;
139
              (*(BigEndianIEEEDouble*)&rtMinusInf).sign = 1;
140
              (*(BigEndianIEEEDouble*)&rtNaN).fraction  = 0x7FFFFF;
141
          }
142
          break;
143
        }
144
        break;    
145
 
146
      case 8:
147
        switch (machByteOrder) {
148
          case LittleEndian: {
149
              typedef struct {
150
                  struct {
151
                      uint32_T fraction2;
152
                  } wordH;
153
                  struct {
154
                      uint32_T fraction1 : 20;
155
                      uint32_T exponent  : 11;
156
                      uint32_T sign      : 1;
157
                  } wordL;
158
              } LittleEndianIEEEDouble;
159
 
160
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.sign      = 0;
161
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.exponent  = 0x7FF;
162
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.fraction1 = 0;
163
              (*(LittleEndianIEEEDouble*)&rtInf).wordH.fraction2 = 0;
164
 
165
              rtMinusInf = rtInf;
166
              (*(LittleEndianIEEEDouble*)&rtMinusInf).wordL.sign = 1;
167
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.sign      = 0;
168
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.exponent  = 0x7FF;
169
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.fraction1 = 0xFFFFF;
170
              (*(LittleEndianIEEEDouble*)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
171
          }
172
          break;
173
          case BigEndian: {
174
              typedef struct {
175
                  struct {
176
                      uint32_T sign      : 1;
177
                      uint32_T exponent  : 11;
178
                      uint32_T fraction1 : 20;
179
                  } wordL;
180
                  struct {
181
                      uint32_T fraction2;
182
                  } wordH;
183
              } BigEndianIEEEDouble;
184
 
185
              (*(BigEndianIEEEDouble*)&rtInf).wordL.sign      = 0;
186
              (*(BigEndianIEEEDouble*)&rtInf).wordL.exponent  = 0x7FF;
187
              (*(BigEndianIEEEDouble*)&rtInf).wordL.fraction1 = 0;
188
              (*(BigEndianIEEEDouble*)&rtInf).wordH.fraction2 = 0;
189
 
190
              rtMinusInf = rtInf;
191
              (*(BigEndianIEEEDouble*)&rtMinusInf).wordL.sign = 1;
192
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.sign      = 0;
193
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.exponent  = 0x7FF;
194
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.fraction1 = 0xFFFFF;
195
              (*(BigEndianIEEEDouble*)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
196
          }
197
          break;
198
        }
199
        break;
200
      default:
1369 giacomo 201
        cprintf("Error: Unable to initialize rtInf, rtMinusInf and rtNaN\n");
1342 giacomo 202
        sys_end();
203
        break;
204
    }
205
 
206
} /* end rt_InitInfAndNaN */
207
 
208
/* Function: rtOneStep ========================================================
209
 *
210
 * Abstract:
211
 *      Perform one step of the model. This function is modeled such that
212
 *      it could be called from an interrupt service routine (ISR) with minor
213
 *      modifications.
214
 */
215
static void rt_OneStep(RT_MODEL *S)
216
{
217
    real_T tnext;
1415 giacomo 218
 
1342 giacomo 219
    /***********************************************
220
     * Check and see if base step time is too fast *
221
     ***********************************************/
1415 giacomo 222
 
1342 giacomo 223
    if (GBLbuf.isrOverrun++) {
224
        GBLbuf.stopExecutionFlag = 1;
225
        return;
226
    }
1415 giacomo 227
 
1342 giacomo 228
    /***********************************************
229
     * Check and see if error status has been set  *
230
     ***********************************************/
1415 giacomo 231
 
1342 giacomo 232
    if (rtmGetErrorStatus(S) != NULL) {
233
        GBLbuf.stopExecutionFlag = 1;
234
        return;
235
    }
1415 giacomo 236
 
1342 giacomo 237
    /* enable interrupts here */
1415 giacomo 238
 
1342 giacomo 239
    rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
1415 giacomo 240
                        (boolean_T *)&rtmGetStopRequested(S));
241
 
1342 giacomo 242
    tnext = rt_SimGetNextSampleHit();
243
    rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
1415 giacomo 244
 
1342 giacomo 245
    MdlOutputs(0);
1415 giacomo 246
 
247
    rtExtModeSingleTaskUpload(S);
248
 
249
    if (GBLbuf.errmsg != NULL) {
250
        GBLbuf.stopExecutionFlag = 1;
251
        return;
252
    }
253
 
1342 giacomo 254
    MdlUpdate(0);
255
    rt_SimUpdateDiscreteTaskSampleHits(rtmGetNumSampleTimes(S),
256
                                       rtmGetTimingData(S),
257
                                       rtmGetSampleHitPtr(S),
258
                                       rtmGetTPtr(S));
1415 giacomo 259
 
1342 giacomo 260
    if (rtmGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
261
        rt_UpdateContinuousStates(S);
262
    }
1415 giacomo 263
 
1342 giacomo 264
    GBLbuf.isrOverrun--;
1415 giacomo 265
 
1342 giacomo 266
    rtExtModeCheckEndTrigger();
267
 
268
} /* end rtOneStep */
269
 
1368 giacomo 270
void Init_RealTime_Workshop() {
1342 giacomo 271
 
1368 giacomo 272
    /****************************
1342 giacomo 273
     * Initialize global memory *
274
     ****************************/
1368 giacomo 275
 
1342 giacomo 276
    (void)memset(&GBLbuf, 0, sizeof(GBLbuf));  
277
 
1368 giacomo 278
    /************************
1342 giacomo 279
     * Initialize the model *
280
     ************************/
1368 giacomo 281
 
1342 giacomo 282
    rt_InitInfAndNaN(sizeof(real_T));
283
 
284
    S = MODEL();
285
    if (rtmGetErrorStatus(S) != NULL) {
1369 giacomo 286
        cprintf("Error: Model registration: %s\n",
1342 giacomo 287
                      rtmGetErrorStatus(S));
288
        sys_end();
289
    }
1368 giacomo 290
 
1342 giacomo 291
    if (finaltime >= 0.0 || finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);
292
 
293
    MdlInitializeSizes();
294
    MdlInitializeSampleTimes();
295
 
296
    status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(S),
297
                                    rtmGetStepSize(S),
298
                                    rtmGetSampleTimePtr(S),
299
                                    rtmGetOffsetTimePtr(S),
300
                                    rtmGetSampleHitPtr(S),
301
                                    rtmGetSampleTimeTaskIDPtr(S),
302
                                    rtmGetTStart(S),
303
                                    &rtmGetSimTimeStep(S),
304
                                    &rtmGetTimingData(S));
305
 
306
    if (status != NULL) {
1369 giacomo 307
        cprintf("Error: Failed to initialize sample time engine: %s\n", status);
1342 giacomo 308
        sys_end();
309
    }
1368 giacomo 310
 
1342 giacomo 311
    rt_CreateIntegrationData(S);
312
 
313
    rtExtModeCheckInit();
314
    rtExtModeWaitForStartMsg(rtmGetRTWExtModeInfo(S),
315
                             (boolean_T *)&rtmGetStopRequested(S));
316
 
1368 giacomo 317
    cprintf("\n** Starting the model **\n");
1342 giacomo 318
 
319
    MdlStart();
320
    if (rtmGetErrorStatus(S) != NULL) {
321
      GBLbuf.stopExecutionFlag = 1;
322
    }
323
 
324
     /*************************************************************************
325
     * Execute the model.  You may attach rtOneStep to an ISR, if so replace *
326
     * the call to rtOneStep (below) with a call to a background task        *
327
     * application.                                                          *
328
     *************************************************************************/
329
 
330
    if (rtmGetTFinal(S) == RUN_FOREVER) {
331
        cprintf("\n**May run forever. Model stop time set to infinity.**\n");
332
    }
333
 
1368 giacomo 334
}
1342 giacomo 335
 
1368 giacomo 336
void Close_RealTime_Workshop() {
1342 giacomo 337
 
338
    /********************
339
     * Cleanup and exit *
340
     ********************/
341
 
342
    rtExtModeShutdown();
343
 
344
    if (GBLbuf.errmsg) {
1369 giacomo 345
        cprintf("Error: %s\n",GBLbuf.errmsg);
1342 giacomo 346
        sys_end();
347
    }
348
 
349
    if (GBLbuf.isrOverrun) {
1369 giacomo 350
        cprintf("Error: %s: ISR overrun - base sampling rate is too fast\n",QUOTE(MODEL));
1342 giacomo 351
        sys_end();
352
    }
353
 
354
    if (rtmGetErrorStatus(S) != NULL) {
1369 giacomo 355
        cprintf("Error: %s\n", rtmGetErrorStatus(S));
1342 giacomo 356
        sys_end();
357
    }
358
 
1368 giacomo 359
    MdlTerminate();
1344 giacomo 360
 
1368 giacomo 361
}
362
 
363
TASK RTW_body(void *arg) {
364
 
365
    simulation_run = 1;
366
 
367
    while (!GBLbuf.stopExecutionFlag &&
368
           (rtmGetTFinal(S) == RUN_FOREVER ||
369
            rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON)) {
370
 
371
        rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
372
                               (boolean_T *)&rtmGetStopRequested(S));
373
 
374
        if (rtmGetStopRequested(S)) break;
375
        rt_OneStep(S);
376
 
377
        task_endcycle();
378
 
379
    }
380
 
381
    if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
382
        /* Execute model last time step */
383
        rt_OneStep(S);
384
    }
385
 
386
    simulation_run = 0;
387
 
388
    return NULL;
389
 
390
}
391
 
1403 giacomo 392
TASK INPUT_body(void *arg) {
393
 
1404 giacomo 394
    int input_ports = rtmGetNumU(S);
1403 giacomo 395
    real_T *input;
396
 
1416 giacomo 397
    cprintf("Number of Inputs = %d\n",input_ports);
1403 giacomo 398
 
399
    input = (real_T *)rtmGetU(S);
400
 
401
    while(1) {
402
 
1415 giacomo 403
        input[0] = 1000.0;
404
        input[1] = 1000.0;
1403 giacomo 405
 
406
        task_endcycle();
407
 
408
    }
409
 
410
    return NULL;
411
 
412
}
413
 
1416 giacomo 414
TASK OUTPUT_body(void *arg) {
415
 
416
    int output_ports = rtmGetNumY(S),i;
417
    real_T *output;
418
 
419
    cprintf("Number of Outputs = %d\n",output_ports);
420
 
421
    output = (real_T *)rtmGetY(S);
422
 
423
    while(1) {
424
 
425
        cprintf("T=%6lf ",rtmGetT(S));    
426
 
427
        for (i=0;i<output_ports;i++)
428
                cprintf("O[%d]=%6lf ",i,output[i]);
429
        cprintf("\n");
430
 
431
        task_endcycle();
432
 
433
    }
434
 
435
    return NULL;
436
 
437
}
438
 
1368 giacomo 439
int main() {
440
 
1416 giacomo 441
    HARD_TASK_MODEL     RTW_task,INPUT_task,OUTPUT_task;
442
    PID                 RTW_pid,INPUT_pid,OUTPUT_pid;
1368 giacomo 443
 
1416 giacomo 444
    int RTW_period;
445
    int RTW_wcet;
446
 
447
    int INPUT_period;
448
    int INPUT_wcet;
449
 
450
    int OUTPUT_period;
451
    int OUTPUT_wcet;
452
 
1368 giacomo 453
    Init_RealTime_Workshop();
454
 
1416 giacomo 455
    RTW_period =(int)(rtmGetStepSize(S) * 1000000.0);
456
    RTW_wcet = (int)(rtmGetStepSize(S) * 1000000.0 * 0.45);
457
 
458
    INPUT_period = (int)(10000.0);
459
    INPUT_wcet = (int)(10000.0 * 0.20);
460
 
461
    OUTPUT_period = (int)(20000.0);
462
    OUTPUT_wcet = (int)(20000.0 * 0.25);
463
 
464
    cprintf("Task Setup\n");
465
    cprintf("RTW    (P %8d W %8d)\n",RTW_period,RTW_wcet);
466
    cprintf("INPUT  (P %8d W %8d)\n",INPUT_period,INPUT_wcet);
467
    cprintf("OUTPUT (P %8d W %8d)\n",OUTPUT_period,OUTPUT_wcet);
468
 
1368 giacomo 469
    hard_task_default_model(RTW_task);
1416 giacomo 470
    hard_task_def_mit(RTW_task,RTW_period);
471
    hard_task_def_wcet(RTW_task,RTW_wcet);
1368 giacomo 472
    hard_task_def_usemath(RTW_task);
473
    hard_task_def_ctrl_jet(RTW_task);
474
 
475
    RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL);
476
    if (RTW_pid == NIL) {
1416 giacomo 477
        cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n");
1368 giacomo 478
        sys_end();
479
    }
480
 
1403 giacomo 481
    hard_task_default_model(INPUT_task);
1416 giacomo 482
    hard_task_def_mit(INPUT_task,INPUT_period);
483
    hard_task_def_wcet(INPUT_task,INPUT_wcet);
1403 giacomo 484
    hard_task_def_usemath(INPUT_task);
485
    hard_task_def_ctrl_jet(INPUT_task);
486
 
487
    INPUT_pid = task_create("INPUT",INPUT_body,&INPUT_task,NULL);
488
    if (INPUT_pid == NIL) {
1416 giacomo 489
        cprintf("Error: Cannot create RealTime Workshop [INPUT] Task\n");
1403 giacomo 490
        sys_end();
491
    }
492
 
1416 giacomo 493
    hard_task_default_model(OUTPUT_task);
494
    hard_task_def_mit(OUTPUT_task,OUTPUT_period);
495
    hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet);
496
    hard_task_def_usemath(OUTPUT_task);
497
    hard_task_def_ctrl_jet(OUTPUT_task);
498
 
499
    OUTPUT_pid = task_create("OUTPUT",OUTPUT_body,&OUTPUT_task,NULL);
500
    if (OUTPUT_pid == NIL) {
501
        cprintf("Error: Cannot create RealTime Workshop [OUTPUT] Task\n");
502
        sys_end();
503
    }
504
 
1403 giacomo 505
    task_activate(INPUT_pid);
1368 giacomo 506
    task_activate(RTW_pid);
1416 giacomo 507
    task_activate(OUTPUT_pid);
1368 giacomo 508
 
509
    while(simulation_run);
510
 
511
    Close_RealTime_Workshop();
512
 
513
    sys_end();
514
 
1342 giacomo 515
    return 0;
516
 
1368 giacomo 517
}