Subversion Repositories shark

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1624 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
#include "modules/cabs.h"
12
#include "percorso.h"
13
 
14
#include "rtw.h"
15
 
16
/*=========*
17
 * Defines *
18
 *=========*/
19
 
20
#ifndef TRUE
21
#define FALSE (0)
22
#define TRUE  (1)
23
#endif
24
 
25
#ifndef EXIT_FAILURE
26
#define EXIT_FAILURE  1
27
#endif
28
#ifndef EXIT_SUCCESS
29
#define EXIT_SUCCESS  0
30
#endif
31
 
32
#define QUOTE1(name) #name
33
#define QUOTE(name) QUOTE1(name)    /* need to expand name    */
34
 
35
#define RUN_FOREVER -1.0
36
 
37
#define EXPAND_CONCAT(name1,name2) name1 ## name2
38
#define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)
39
#define RT_MODEL            CONCAT(MODEL,_rtModel)
40
 
41
extern RT_MODEL *MODEL(void);
42
 
43
extern void MdlInitializeSizes(void);
44
extern void MdlInitializeSampleTimes(void);
45
extern void MdlStart(void);
46
extern void MdlOutputs(int_T tid);
47
extern void MdlUpdate(int_T tid);
48
extern void MdlTerminate(void);
49
 
50
real_T rtInf;
51
real_T rtMinusInf;
52
real_T rtNaN;
53
 
54
CAB    input_cid[NINPUTCAB];
55
CAB    output_cid[NOUTPUTCAB];
56
 
57
#ifdef EXT_MODE
58
#  define rtExtModeSingleTaskUpload(S)    \
59
   {                                      \
60
        int stIdx;                        \
61
        rtExtModeUploadCheckTrigger();    \
62
        for (stIdx=0; stIdx<NUMST; stIdx++) {                   \
63
            if (rtmIsSampleHit(S, stIdx, 0 /*unused*/)) {       \
64
                rtExtModeUpload(stIdx,rtmGetTaskTime(S,stIdx)); \
65
            }                                                   \
66
        }                                                       \
67
   }
68
#else
69
#  define rtExtModeSingleTaskUpload(S) /* Do nothing */
70
#endif
71
 
72
#if NCSTATES > 0
73
  extern void rt_ODECreateIntegrationData(RTWSolverInfo *si);
74
  extern void rt_ODEUpdateContinuousStates(RTWSolverInfo *si);
75
 
76
# define rt_CreateIntegrationData(S) \
77
    rt_ODECreateIntegrationData(rtmGetRTWSolverInfo(S));
78
# define rt_UpdateContinuousStates(S) \
79
    rt_ODEUpdateContinuousStates(rtmGetRTWSolverInfo(S));
80
# else
81
# define rt_CreateIntegrationData(S)  \
82
      rtsiSetSolverName(rtmGetRTWSolverInfo(S),"FixedStepDiscrete");
83
# define rt_UpdateContinuousStates(S) \
84
      rtmSetT(S, rtsiGetSolverStopTime(rtmGetRTWSolverInfo(S)));
85
#endif
86
 
87
/*==================================*
88
 * Global data local to this module *
89
 *==================================*/
90
 
91
RT_MODEL  *S;
92
const char *status;
93
real_T     finaltime = -2.0;
94
volatile int simulation_run;                                                                                                                            
95
static struct {
96
  int_T    stopExecutionFlag;
97
  int_T    isrOverrun;
98
  int_T    overrunFlags[NUMST];
99
  const char_T *errmsg;
100
} GBLbuf;
101
 
102
/* Function: rt_InitInfAndNaN ==================================================
103
 * Abstract:
104
 *      Initialize the rtInf, rtMinusInf, and rtNaN needed by the
105
 *      generated code. NaN is initialized as non-signaling. Assumes IEEE.
106
 */
107
void rt_InitInfAndNaN(int_T realSize)
108
{
109
    short one = 1;
110
    enum {
111
        LittleEndian,
112
        BigEndian
113
    } machByteOrder = (*((char *) &one) == 1) ? LittleEndian : BigEndian;
114
 
115
    switch (realSize) {  
116
      case 4:
117
        switch (machByteOrder) {
118
          case LittleEndian: {
119
              typedef struct {
120
                  uint32_T fraction : 23;
121
                  uint32_T exponent  : 8;
122
                  uint32_T sign      : 1;
123
              } LittleEndianIEEEDouble;
124
 
125
              (*(LittleEndianIEEEDouble*)&rtInf).sign      = 0;
126
              (*(LittleEndianIEEEDouble*)&rtInf).exponent  = 0xFF;
127
              (*(LittleEndianIEEEDouble*)&rtInf).fraction  = 0;
128
              rtMinusInf = rtInf;
129
              rtNaN = rtInf;
130
              (*(LittleEndianIEEEDouble*)&rtMinusInf).sign = 1;
131
              (*(LittleEndianIEEEDouble*)&rtNaN).fraction  = 0x7FFFFF;
132
          }
133
          break;
134
          case BigEndian: {
135
              typedef struct {
136
                  uint32_T sign      : 1;
137
                  uint32_T exponent  : 8;
138
                  uint32_T fraction  : 23;
139
              } BigEndianIEEEDouble;
140
 
141
              (*(BigEndianIEEEDouble*)&rtInf).sign      = 0;
142
              (*(BigEndianIEEEDouble*)&rtInf).exponent  = 0xFF;
143
              (*(BigEndianIEEEDouble*)&rtInf).fraction  = 0;
144
              rtMinusInf = rtInf;
145
              rtNaN = rtInf;
146
              (*(BigEndianIEEEDouble*)&rtMinusInf).sign = 1;
147
              (*(BigEndianIEEEDouble*)&rtNaN).fraction  = 0x7FFFFF;
148
          }
149
          break;
150
        }
151
        break;    
152
 
153
      case 8:
154
        switch (machByteOrder) {
155
          case LittleEndian: {
156
              typedef struct {
157
                  struct {
158
                      uint32_T fraction2;
159
                  } wordH;
160
                  struct {
161
                      uint32_T fraction1 : 20;
162
                      uint32_T exponent  : 11;
163
                      uint32_T sign      : 1;
164
                  } wordL;
165
              } LittleEndianIEEEDouble;
166
 
167
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.sign      = 0;
168
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.exponent  = 0x7FF;
169
              (*(LittleEndianIEEEDouble*)&rtInf).wordL.fraction1 = 0;
170
              (*(LittleEndianIEEEDouble*)&rtInf).wordH.fraction2 = 0;
171
 
172
              rtMinusInf = rtInf;
173
              (*(LittleEndianIEEEDouble*)&rtMinusInf).wordL.sign = 1;
174
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.sign      = 0;
175
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.exponent  = 0x7FF;
176
              (*(LittleEndianIEEEDouble*)&rtNaN).wordL.fraction1 = 0xFFFFF;
177
              (*(LittleEndianIEEEDouble*)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
178
          }
179
          break;
180
          case BigEndian: {
181
              typedef struct {
182
                  struct {
183
                      uint32_T sign      : 1;
184
                      uint32_T exponent  : 11;
185
                      uint32_T fraction1 : 20;
186
                  } wordL;
187
                  struct {
188
                      uint32_T fraction2;
189
                  } wordH;
190
              } BigEndianIEEEDouble;
191
 
192
              (*(BigEndianIEEEDouble*)&rtInf).wordL.sign      = 0;
193
              (*(BigEndianIEEEDouble*)&rtInf).wordL.exponent  = 0x7FF;
194
              (*(BigEndianIEEEDouble*)&rtInf).wordL.fraction1 = 0;
195
              (*(BigEndianIEEEDouble*)&rtInf).wordH.fraction2 = 0;
196
 
197
              rtMinusInf = rtInf;
198
              (*(BigEndianIEEEDouble*)&rtMinusInf).wordL.sign = 1;
199
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.sign      = 0;
200
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.exponent  = 0x7FF;
201
              (*(BigEndianIEEEDouble*)&rtNaN).wordL.fraction1 = 0xFFFFF;
202
              (*(BigEndianIEEEDouble*)&rtNaN).wordH.fraction2 = 0xFFFFFFFF;
203
          }
204
          break;
205
        }
206
        break;
207
      default:
208
        cprintf("Error: Unable to initialize rtInf, rtMinusInf and rtNaN\n");
209
        sys_end();
210
        break;
211
    }
212
 
213
} /* end rt_InitInfAndNaN */
214
 
215
/* Function: rtOneStep ========================================================
216
 *
217
 * Abstract:
218
 *      Perform one step of the model. This function is modeled such that
219
 *      it could be called from an interrupt service routine (ISR) with minor
220
 *      modifications.
221
 */
222
static void rt_OneStep(RT_MODEL *S)
223
{
224
    real_T tnext;
225
 
226
    /***********************************************
227
     * Check and see if base step time is too fast *
228
     ***********************************************/
229
 
230
    if (GBLbuf.isrOverrun++) {
231
        GBLbuf.stopExecutionFlag = 1;
232
        return;
233
    }
234
 
235
    /***********************************************
236
     * Check and see if error status has been set  *
237
     ***********************************************/
238
 
239
    if (rtmGetErrorStatus(S) != NULL) {
240
        GBLbuf.stopExecutionFlag = 1;
241
        return;
242
    }
243
 
244
    /* enable interrupts here */
245
 
246
    rtExtModeOneStep(rtmGetRTWExtModeInfo(S),
247
                        (boolean_T *)&rtmGetStopRequested(S));
248
 
249
    tnext = rt_SimGetNextSampleHit();
250
    rtsiSetSolverStopTime(rtmGetRTWSolverInfo(S),tnext);
251
 
252
    MdlOutputs(0);
253
 
254
    rtExtModeSingleTaskUpload(S);
255
 
256
    if (GBLbuf.errmsg != NULL) {
257
        GBLbuf.stopExecutionFlag = 1;
258
        return;
259
    }
260
 
261
    MdlUpdate(0);
262
    rt_SimUpdateDiscreteTaskSampleHits(rtmGetNumSampleTimes(S),
263
                                       rtmGetTimingData(S),
264
                                       rtmGetSampleHitPtr(S),
265
                                       rtmGetTPtr(S));
266
 
267
    if (rtmGetSampleTime(S,0) == CONTINUOUS_SAMPLE_TIME) {
268
        rt_UpdateContinuousStates(S);
269
    }
270
 
271
    GBLbuf.isrOverrun--;
272
 
273
    rtExtModeCheckEndTrigger();
274
 
275
} /* end rtOneStep */
276
 
277
void Init_RealTime_Workshop() {
278
 
279
    /****************************
280
     * Initialize global memory *
281
     ****************************/
282
 
283
    (void)memset(&GBLbuf, 0, sizeof(GBLbuf));  
284
 
285
    /************************
286
     * Initialize the model *
287
     ************************/
288
 
289
    rt_InitInfAndNaN(sizeof(real_T));
290
 
291
    S = MODEL();
292
    if (rtmGetErrorStatus(S) != NULL) {
293
        cprintf("Error: Model registration: %s\n",
294
                      rtmGetErrorStatus(S));
295
        sys_end();
296
    }
297
 
298
    if (finaltime >= 0.0 || finaltime == RUN_FOREVER) rtmSetTFinal(S,finaltime);
299
 
300
    MdlInitializeSizes();
301
    MdlInitializeSampleTimes();
302
 
303
    status = rt_SimInitTimingEngine(rtmGetNumSampleTimes(S),
304
                                    rtmGetStepSize(S),
305
                                    rtmGetSampleTimePtr(S),
306
                                    rtmGetOffsetTimePtr(S),
307
                                    rtmGetSampleHitPtr(S),
308
                                    rtmGetSampleTimeTaskIDPtr(S),
309
                                    rtmGetTStart(S),
310
                                    &rtmGetSimTimeStep(S),
311
                                    &rtmGetTimingData(S));
312
 
313
    if (status != NULL) {
314
        cprintf("Error: Failed to initialize sample time engine: %s\n", status);
315
        sys_end();
316
    }
317
 
318
    rt_CreateIntegrationData(S);
319
 
320
    rtExtModeCheckInit();
321
    rtExtModeWaitForStartMsg(rtmGetRTWExtModeInfo(S),
322
                             (boolean_T *)&rtmGetStopRequested(S));
323
 
324
    cprintf("\n** Starting the model **\n");
325
 
326
    MdlStart();
327
    if (rtmGetErrorStatus(S) != NULL) {
328
      GBLbuf.stopExecutionFlag = 1;
329
    }
330
 
331
     /*************************************************************************
332
     * Execute the model.  You may attach rtOneStep to an ISR, if so replace *
333
     * the call to rtOneStep (below) with a call to a background task        *
334
     * application.                                                          *
335
     *************************************************************************/
336
 
337
    if (rtmGetTFinal(S) == RUN_FOREVER) {
338
        cprintf("\n**May run forever. Model stop time set to infinity.**\n");
339
    }
340
 
341
}
342
 
343
void Close_RealTime_Workshop() {
344
 
345
    /********************
346
     * Cleanup and exit *
347
     ********************/
348
 
349
    rtExtModeShutdown();
350
 
351
    if (GBLbuf.errmsg) {
352
        cprintf("Error: %s\n",GBLbuf.errmsg);
353
        sys_end();
354
    }
355
 
356
    if (GBLbuf.isrOverrun) {
357
        cprintf("Error: %s: ISR overrun - base sampling rate is too fast\n",QUOTE(MODEL));
358
        sys_end();
359
    }
360
 
361
    if (rtmGetErrorStatus(S) != NULL) {
362
        cprintf("Error: %s\n", rtmGetErrorStatus(S));
363
        sys_end();
364
    }
365
 
366
    MdlTerminate();
367
 
368
}
369
 
370
TASK RTW_body(void *arg) {
371
 
372
    real_T *input, *p;
373
 
374
    input = (real_T *)rtmGetU(S);
375
    simulation_run = 1;
376
 
377
    while (!GBLbuf.stopExecutionFlag &&
378
           (rtmGetTFinal(S) == RUN_FOREVER ||
379
            rtmGetTFinal(S)-rtmGetT(S) > rtmGetT(S)*DBL_EPSILON)) {
380
 
381
        rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(S),
382
                               (boolean_T *)&rtmGetStopRequested(S));
383
 
384
        if (rtmGetStopRequested(S)) break;
385
 
386
        /* Get PAR0 */
387
        if (input_cid[0] != -1) {
388
          p = (real_T *)cab_getmes(input_cid[0]);
389
          input[0] = *p;
390
          cab_unget(input_cid[0], p);
391
        }
392
 
393
        /* Get PAR1 */
394
        if (input_cid[1] != -1) {
395
          p = (real_T *)cab_getmes(input_cid[1]);
396
          input[1] = *p;
397
          cab_unget(input_cid[1], p);
398
        }
399
 
400
        rt_OneStep(S);
401
 
402
        task_endcycle();
403
 
404
    }
405
 
406
    if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) {
407
        /* Execute model last time step */
408
        rt_OneStep(S);
409
    }
410
 
411
    simulation_run = 0;
412
 
413
    return NULL;
414
 
415
}
416
 
417
TASK OUTPUT_body(void *arg) {
418
 
419
  real_T *output, *p;
420
  real_T th;
421
 
422
  POINT pos;
423
  PATH_ELEM *e;
424
  real_T distance = 0;
425
  real_T angle = 0;
426
  real_T curve = 0;
427
 
428
  output_cid[0] = cab_create("DISTANCE", sizeof(real_T), 2);
429
  output_cid[1] = cab_create("ANGLE", sizeof(real_T), 2);
430
  output_cid[2] = cab_create("CURVE", sizeof(real_T), 2);
431
 
432
  output = (real_T *)rtmGetY(S);
433
 
434
  while(1) {
435
 
436
    pos.x = output[0];
437
    pos.y = output[1];
438
    th = output[2];
439
 
440
    e = find_closest_elem(&pos, 1.0);
441
    if (e != 0) {
442
      distance = get_distance_from_elem(&pos, e);
443
      angle = th - get_angle_from_elem(&pos,e);
444
      curve = is_curve(e);
445
    }
446
 
447
    /* Send Distance */
448
    p = (real_T *)cab_reserve(output_cid[0]);
449
    *p = distance;
450
    cab_putmes(output_cid[0], p);
451
 
452
    /* Send Angle */
453
    p = (real_T *)cab_reserve(output_cid[1]);
454
    *p = angle;
455
    cab_putmes(output_cid[1], p);
456
 
457
    /* Send Curve */
458
    p = (real_T *)cab_reserve(output_cid[2]);
459
    *p = curve;
460
    cab_putmes(output_cid[2], p);
461
 
462
    task_endcycle();
463
  }
464
  return NULL;
465
}
466
 
467
TASK OUTPUT_w_body(void *arg) {
468
 
469
  real_T *output, *p;
470
 
471
  real_T wr = 0;
472
  real_T wl = 0;
473
 
474
  output_cid[0] = cab_create("WR", sizeof(real_T), 2);
475
  output_cid[1] = cab_create("WL", sizeof(real_T), 2);
476
 
477
  output = (real_T *)rtmGetY(S);
478
 
479
  while(1) {
480
 
481
    wr = output[3];
482
    wl = output[4];  
483
 
484
    /* Send wr */
485
    p = (real_T *)cab_reserve(output_cid[3]);
486
    *p = wr;
487
    cab_putmes(output_cid[3], p);
488
 
489
    /* Send wl */
490
    p = (real_T *)cab_reserve(output_cid[4]);
491
    *p = wl;
492
    cab_putmes(output_cid[4], p);
493
 
494
    task_endcycle();
495
 
496
  }
497
 
498
  return NULL;
499
 
500
}
501
 
502
void Init_Rtw() {
503
 
504
    HARD_TASK_MODEL     RTW_task,OUTPUT_task;
505
    PID                 RTW_pid,OUTPUT_pid,OUTPUT_w_pid;
506
 
507
    int i;
508
 
509
    int RTW_period;
510
    int RTW_wcet;
511
 
512
    int OUTPUT_period;
513
    int OUTPUT_wcet;
514
 
515
    Init_RealTime_Workshop();
516
    Init_All_Path();
517
 
518
    for (i=0;i<NINPUTCAB;i++) input_cid[i] = -1;
519
    for (i=0;i<NOUTPUTCAB;i++) output_cid[i] = -1;
520
 
521
    RTW_period =(int)(rtmGetStepSize(S) * 1000000.0);
522
    RTW_wcet = (int)(rtmGetStepSize(S) * 1000000.0 * 0.45);
523
 
524
    OUTPUT_period = (int)(10000.0);
525
    OUTPUT_wcet = (int)(10000.0 * 0.10);
526
 
527
    cprintf("Task Setup\n");
528
    cprintf("RTW    (P %8d W %8d)\n",RTW_period,RTW_wcet);
529
    cprintf("OUTPUT (P %8d W %8d)\n",OUTPUT_period,OUTPUT_wcet);
530
 
531
    hard_task_default_model(RTW_task);
532
    hard_task_def_mit(RTW_task,RTW_period);
533
    hard_task_def_wcet(RTW_task,RTW_wcet);
534
    hard_task_def_usemath(RTW_task);
535
    hard_task_def_ctrl_jet(RTW_task);
536
 
537
    RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL);
538
    if (RTW_pid == NIL) {
539
        cprintf("Error: Cannot create RealTime Workshop [RTW] Task\n");
540
        sys_end();
541
    }
542
 
543
    hard_task_default_model(OUTPUT_task);
544
    hard_task_def_mit(OUTPUT_task,OUTPUT_period);
545
    hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet);
546
    hard_task_def_usemath(OUTPUT_task);
547
    hard_task_def_ctrl_jet(OUTPUT_task);
548
 
549
    OUTPUT_pid = task_create("OUTPUT",OUTPUT_body,&OUTPUT_task,NULL);
550
    if (OUTPUT_pid == NIL) {
551
        cprintf("Error: Cannot create RealTime Workshop [OUTPUT] Task\n");
552
        sys_end();
553
    }
554
 
555
    hard_task_def_mit(OUTPUT_task,OUTPUT_period);
556
    hard_task_def_wcet(OUTPUT_task,OUTPUT_wcet);
557
 
558
    OUTPUT_w_pid = task_create("OUTPUTW",OUTPUT_w_body,&OUTPUT_task,NULL);
559
    if (OUTPUT_pid == NIL) {
560
        cprintf("Error: Cannot create RealTime Workshop [OUTPUTW] Task\n");
561
        sys_end();
562
    }
563
 
564
    task_activate(RTW_pid);
565
    task_activate(OUTPUT_pid);
566
    task_activate(OUTPUT_w_pid);
567
 
568
}
569
 
570
int main() {
571
 
572
    Init_Rtw();
573
 
574
    activate_sensors();
575
    activate_control();
576
 
577
    while(simulation_run);
578
 
579
    Close_RealTime_Workshop();
580
 
581
    sys_end();
582
 
583
    return 0;
584
 
585
}