Subversion Repositories shark

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1624 giacomo 1
#include <math.h>
2
 
3
#include "tmwtypes.h"
4
#ifdef USE_RTMODEL
5
# include "simstruc_types.h"
6
#else
7
# include "simstruc.h"
8
#endif
9
#include "rt_sim.h"
10
 
11
#include "kernel/kern.h"
12
 
13
#ifndef RT_MALLOC /* statically declare data */
14
 
15
/*==========*
16
 * Struct's *
17
 *==========*/
18
 
19
/*
20
 * TimingData
21
 */
22
typedef struct TimingData_Tag {
23
    real_T period[NUMST];       /* Task periods in seconds                   */
24
    real_T offset[NUMST];       /* Task offsets in seconds                   */
25
    real_T clockTick[NUMST];    /* Flint task time tick counter              */
26
    int_T  taskTick[NUMST];     /* Counter for determining task hits         */
27
    int_T  nTaskTicks[NUMST];   /* Number base rate ticks for a task hit     */
28
    int_T  firstDiscIdx;        /* First discrete task index                 */
29
} TimingData;
30
 
31
/*=========================*
32
 * Data local to this file *
33
 *=========================*/
34
 
35
static TimingData td;
36
 
37
/*==================*
38
 * Visible routines *
39
 *==================*/
40
 
41
/* Function: rt_SimInitTimingEngine ============================================
42
 * Abstract:
43
 *      This function is for use with single tasking or multitasking
44
 *      real-time systems.  
45
 *
46
 *      Initializes the timing engine for a fixed-step real-time system.
47
 *      It is assumed that start time is 0.0.
48
 *
49
 * Returns:
50
 *      NULL     - success
51
 *      non-NULL - error string
52
 */
53
const char *rt_SimInitTimingEngine(int_T       rtmNumSampTimes,
54
                                   real_T      rtmStepSize,
55
                                   real_T      *rtmSampleTimePtr,
56
                                   real_T      *rtmOffsetTimePtr,
57
                                   int_T       *rtmSampleHitPtr,
58
                                   int_T       *rtmSampleTimeTaskIDPtr,
59
                                   real_T      rtmTStart,
60
                                   SimTimeStep *rtmSimTimeStepPtr,
61
                                   void        **rtmTimingDataPtr)
62
{
63
    int_T     i;
64
    int       *tsMap     = rtmSampleTimeTaskIDPtr;
65
    real_T    *period    = rtmSampleTimePtr;
66
    real_T    *offset    = rtmOffsetTimePtr;
67
    int_T     *sampleHit = rtmSampleHitPtr;
68
    real_T    stepSize   = rtmStepSize;
69
 
70
    if (rtmTStart != 0.0) {
71
        return("Start time must be zero for real-time systems");
72
    }
73
 
74
    *rtmSimTimeStepPtr = MAJOR_TIME_STEP;
75
 
76
    *rtmTimingDataPtr = (void*)&td;
77
 
78
    for (i = 0; i < NUMST; i++) {
79
        tsMap[i]         = i;
80
        td.period[i]     = period[i];
81
        td.offset[i]     = offset[i];
82
        td.nTaskTicks[i] = (int_T)floor(period[i]/stepSize + 0.5);
83
        if (td.period[i] == CONTINUOUS_SAMPLE_TIME ||
84
            td.offset[i] == 0.0) {
85
            td.taskTick[i]  = 0;
86
            td.clockTick[i] = 0.0;
87
            sampleHit[i]    = 1;
88
        } else {
89
            td.taskTick[i]  = (int_T)floor((td.period[i]-td.offset[i]) /
90
                                            stepSize+0.5);
91
            td.clockTick[i] = -1.0;
92
            sampleHit[i]    = 0;
93
        }
94
    }
95
 
96
    /* Correct first sample time if continuous task */
97
    td.period[0]     = stepSize;
98
    td.nTaskTicks[0] = 1;
99
 
100
    /* Set first discrete task index */
101
#if NUMST == 1    
102
    td.firstDiscIdx = (int_T)(period[0] == CONTINUOUS_SAMPLE_TIME);
103
#else
104
    td.firstDiscIdx = ((int_T)(period[0] == CONTINUOUS_SAMPLE_TIME) +
105
                       (int_T)(period[1] == CONTINUOUS_SAMPLE_TIME));
106
#endif
107
 
108
    return(NULL); /* success */
109
 
110
} /* end rt_SimInitTimingEngine */
111
 
112
 
113
#if !defined(MULTITASKING)
114
 
115
/*###########################################################################*/
116
/*########################### SINGLE TASKING ################################*/
117
/*###########################################################################*/
118
 
119
/* Function: rt_SimGetNextSampleHit ============================================
120
 * Abstract:
121
 *      For a single tasking real-time system, return time of next sample hit.
122
 */
123
time_T rt_SimGetNextSampleHit(void)
124
{
125
    time_T timeOfNextHit;
126
    td.clockTick[0] += 1;
127
    timeOfNextHit = td.clockTick[0] * td.period[0];
128
 
129
#   if NUMST > 1
130
    {
131
        int i;
132
        for (i = 1; i < NUMST; i++) {
133
            if (++td.taskTick[i] == td.nTaskTicks[i]) {
134
                td.taskTick[i] = 0;
135
                td.clockTick[i]++;
136
            }
137
        }
138
    }
139
#   endif
140
 
141
    return(timeOfNextHit);
142
 
143
} /* end rt_SimGetNextSampleHit */
144
 
145
 
146
 
147
/* Function: rt_SimUpdateDiscreteTaskSampleHits ================================
148
 * Abstract:
149
 *      This function is for use with single tasking real-time systems.  
150
 *      
151
 *      If the number of sample times is greater than one, then we need to
152
 *      update the discrete task sample hits for the next time step. Note,
153
 *      task 0 always has a hit since it's sample time is the fundamental
154
 *      sample time.
155
 */
156
void rt_SimUpdateDiscreteTaskSampleHits(int_T  rtmNumSampTimes,
157
                                        void   *rtmTimingData,
158
                                        int_T  *rtmSampleHitPtr,
159
                                        real_T *rtmTPtr)
160
{
161
    int_T *sampleHit = rtmSampleHitPtr;
162
    int   i;
163
 
164
    UNUSED_PARAMETER(rtmTimingData);
165
    UNUSED_PARAMETER(rtmNumSampTimes);
166
 
167
    for (i = td.firstDiscIdx; i < NUMST; i++) {
168
        int_T hit = (td.taskTick[i] == 0);
169
        if (hit) {
170
            rttiSetTaskTime(rtmTPtr, i,
171
                            td.clockTick[i]*td.period[i] + td.offset[i]);
172
        }
173
        sampleHit[i] = hit;
174
    }
175
} /* rt_SimUpdateDiscreteTaskSampleHits */
176
 
177
 
178
 
179
#else /* defined(MULTITASKING) */
180
 
181
/*###########################################################################*/
182
/*############################## MULTITASKING ###############################*/
183
/*###########################################################################*/
184
 
185
 
186
/* Function: rt_SimUpdateDiscreteEvents ========================================
187
 * Abstract:
188
 *      This function is for use with multitasking real-time systems.
189
 *
190
 *      This function updates the status of the RT_MODEL sampleHits
191
 *      flags and the perTaskSampleHits matrix which is used to determine
192
 *      when special sample hits occur.
193
 *
194
 *      The RT_MODEL contains a matrix, called perTaskSampleHits.
195
 *      This matrix is used by the ssIsSpecialSampleHit macro. The row and
196
 *      column indices are both task id's (equivalent to the root RT_MODEL
197
 *      sample time indices). This is a upper triangle matrix. This routine
198
 *      only updates the slower task hits (kept in column j) for row
199
 *      i if we have a sample hit in row i.
200
 *
201
 *                       column j
202
 *           tid   0   1   2   3   4   5  
203
 *               -------------------------
204
 *             0 |   | X | X | X | X | X |
205
 *         r     -------------------------
206
 *         o   1 |   |   | X | X | X | X |      This matrix(i,j) answers:
207
 *         w     -------------------------      If we are in task i, does
208
 *             2 |   |   |   | X | X | X |      slower task j have a hit now?
209
 *         i     -------------------------
210
 *             3 |   |   |   |   | X | X |
211
 *               -------------------------
212
 *             4 |   |   |   |   |   | X |      X = 0 or 1
213
 *               -------------------------
214
 *             5 |   |   |   |   |   |   |
215
 *               -------------------------
216
 *
217
 *      How macros index this matrix:
218
 *
219
 *          ssSetSampleHitInTask(S, j, i, X)   => matrix(i,j) = X
220
 *
221
 *          ssIsSpecialSampleHit(S, my_sti, promoted_sti, tid) =>
222
 *              (tid_for(promoted_sti) == tid && !minor_time_step &&
223
 *               matrix(tid,tid_for(my_sti))
224
 *              )
225
 *
226
 *            my_sti       = My (the block's) original sample time index.
227
 *            promoted_sti = The block's promoted sample time index resulting
228
 *                           from a transition via a ZOH from a fast to a
229
 *                           slow block or a transition via a unit delay from
230
 *                           a slow to a fast block.
231
 *
232
 *      The perTaskSampleHits array, of dimension n*n, is accessed using
233
 *      perTaskSampleHits[j + i*n] where n is the total number of sample
234
 *      times, 0 <= i < n, and 0 <= j < n.  The C language stores arrays in
235
 *      row-major order, that is, row 0 followed by row 1, etc.
236
 *
237
 */
238
time_T rt_SimUpdateDiscreteEvents(int_T  rtmNumSampTimes,
239
                                  void   *rtmTimingData,
240
                                  int_T  *rtmSampleHitPtr,
241
                                  int_T  *rtmPerTaskSampleHits)
242
{
243
    int   i, j;
244
    int_T *sampleHit = rtmSampleHitPtr;
245
 
246
    UNUSED_PARAMETER(rtmTimingData);
247
 
248
    /*
249
     * Run this loop in reverse so that we do lower priority events first.
250
     */
251
    i = NUMST;
252
    while (--i >= 0) {
253
        if (td.taskTick[i] == 0) {
254
            /*
255
             * Got a sample hit, reset the counter, and update the clock
256
             * tick counter.
257
             */
258
            sampleHit[i] = 1;
259
            td.clockTick[i]++;
260
 
261
            /*
262
             * Record the state of all "slower" events
263
             */
264
            for (j = i + 1; j < NUMST; j++) {
265
                rttiSetSampleHitInTask(rtmPerTaskSampleHits, rtmNumSampTimes,
266
                                       j, i, sampleHit[j]);
267
            }
268
        } else {
269
            /*
270
             * no sample hit, increment the counter
271
             */
272
            sampleHit[i] = 0;
273
        }
274
 
275
        if (++td.taskTick[i] == td.nTaskTicks[i]) { /* update for next time */
276
            td.taskTick[i] = 0;
277
        }
278
    }
279
 
280
    return(td.clockTick[0]*td.period[0]);
281
 
282
} /* rt_SimUpdateDiscreteEvents */
283
 
284
 
285
 
286
/* Function: rt_SimUpdateDiscreteTaskTime ======================================
287
 * Abstract:
288
 *      This function is for use with multitasking systems.
289
 *
290
 *      After a discrete task output and update has been performed, this
291
 *      function must be called to update the discrete task time for next
292
 *      time around.
293
 */
294
void rt_SimUpdateDiscreteTaskTime(real_T *rtmTPtr,
295
                                  void   *rtmTimingData,
296
                                  int    tid)
297
{
298
    UNUSED_PARAMETER(rtmTimingData);
299
    rttiSetTaskTime(rtmTPtr, tid,
300
                    td.clockTick[tid]*td.period[tid] + td.offset[tid]);
301
}
302
 
303
#endif /* MULTITASKING */
304
 
305
#else
306
 
307
#include "mrt_sim.c" /* dynamically allocate data */
308
 
309
#endif /* RT_MALLOC */
310
 
311
/*
312
 *******************************************************************************
313
 * FUNCTIONS MAINTAINED FOR BACKWARDS COMPATIBILITY WITH THE SimStruct
314
 *******************************************************************************
315
 */
316
#ifndef USE_RTMODEL
317
const char *rt_InitTimingEngine(SimStruct *S)
318
{
319
    const char_T *retVal = rt_SimInitTimingEngine(
320
        ssGetNumSampleTimes(S),
321
        ssGetStepSize(S),
322
        ssGetSampleTimePtr(S),
323
        ssGetOffsetTimePtr(S),
324
        ssGetSampleHitPtr(S),
325
        ssGetSampleTimeTaskIDPtr(S),
326
        ssGetTStart(S),
327
        &ssGetSimTimeStep(S),
328
        &ssGetTimingData(S));
329
    return(retVal);
330
}
331
 
332
# ifdef RT_MALLOC
333
void rt_DestroyTimingEngine(SimStruct *S)
334
{
335
    rt_SimDestroyTimingEngine(ssGetTimingData(S));
336
}
337
# endif
338
 
339
# if !defined(MULTITASKING)
340
void rt_UpdateDiscreteTaskSampleHits(SimStruct *S)
341
{
342
    rt_SimUpdateDiscreteTaskSampleHits(
343
        ssGetNumSampleTimes(S),
344
        ssGetTimingData(S),
345
        ssGetSampleHitPtr(S),
346
        ssGetTPtr(S));
347
}
348
 
349
#   ifndef RT_MALLOC
350
 
351
time_T rt_GetNextSampleHit(void)
352
{
353
    return(rt_SimGetNextSampleHit());
354
}
355
 
356
#   else /* !RT_MALLOC */
357
 
358
time_T rt_GetNextSampleHit(SimStruct *S)
359
{
360
    return(rt_SimGetNextSampleHit(ssGetTimingData(S),
361
                                  ssGetNumSampleTimes(S)));
362
}
363
 
364
#   endif
365
 
366
# else /* MULTITASKING */
367
 
368
time_T rt_UpdateDiscreteEvents(SimStruct *S)
369
{
370
    return(rt_SimUpdateDiscreteEvents(
371
               ssGetNumSampleTimes(S),
372
               ssGetTimingData(S),
373
               ssGetSampleHitPtr(S),
374
               ssGetPerTaskSampleHitsPtr(S)));
375
}
376
 
377
void rt_UpdateDiscreteTaskTime(SimStruct *S, int tid)
378
{
379
    rt_SimUpdateDiscreteTaskTime(ssGetTPtr(S), ssGetTimingData(S), tid);
380
}
381
 
382
#endif
383
#endif
384
 
385
/* EOF: rt_sim.c */