Rev 1402 | Rev 1404 | 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 | |||
1403 | giacomo | 352 | cprintf(".(%dus out = %lf)",(int)(rtmGetT(S)*1000000),*(real_T *)(rtmGetY(S))); |
1368 | giacomo | 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 | |||
1403 | giacomo | 372 | TASK INPUT_body(void *arg) { |
373 | |||
374 | int input_ports = rtmGetNumInputPorts(S); |
||
375 | real_T *input; |
||
376 | |||
377 | cprintf("Number of inputs = %d\n",input_ports); |
||
378 | |||
379 | input = (real_T *)rtmGetU(S); |
||
380 | |||
381 | while(1) { |
||
382 | |||
383 | input[0] = 1; |
||
384 | input[1] = 1; |
||
385 | |||
386 | task_endcycle(); |
||
387 | |||
388 | } |
||
389 | |||
390 | return NULL; |
||
391 | |||
392 | } |
||
393 | |||
1368 | giacomo | 394 | int main() { |
395 | |||
1403 | giacomo | 396 | HARD_TASK_MODEL RTW_task,INPUT_task; |
397 | PID RTW_pid,INPUT_pid; |
||
1368 | giacomo | 398 | |
399 | Init_RealTime_Workshop(); |
||
400 | |||
401 | hard_task_default_model(RTW_task); |
||
402 | hard_task_def_mit(RTW_task,rtmGetStepSize(S) * 1000000); |
||
403 | hard_task_def_wcet(RTW_task,10000); |
||
404 | hard_task_def_usemath(RTW_task); |
||
405 | hard_task_def_ctrl_jet(RTW_task); |
||
406 | |||
407 | RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL); |
||
408 | if (RTW_pid == NIL) { |
||
1369 | giacomo | 409 | cprintf("Error: Cannot create RealTime Workshop Task\n"); |
1368 | giacomo | 410 | sys_end(); |
411 | } |
||
412 | |||
1403 | giacomo | 413 | hard_task_default_model(INPUT_task); |
414 | hard_task_def_mit(INPUT_task,100000); |
||
415 | hard_task_def_wcet(INPUT_task,10000); |
||
416 | hard_task_def_usemath(INPUT_task); |
||
417 | hard_task_def_ctrl_jet(INPUT_task); |
||
418 | |||
419 | INPUT_pid = task_create("INPUT",INPUT_body,&INPUT_task,NULL); |
||
420 | if (INPUT_pid == NIL) { |
||
421 | cprintf("Error: Cannot create RealTime Workshop Task\n"); |
||
422 | sys_end(); |
||
423 | } |
||
424 | |||
425 | task_activate(INPUT_pid); |
||
1368 | giacomo | 426 | task_activate(RTW_pid); |
427 | |||
428 | while(simulation_run); |
||
429 | |||
430 | Close_RealTime_Workshop(); |
||
431 | |||
432 | sys_end(); |
||
433 | |||
1342 | giacomo | 434 | return 0; |
435 | |||
1368 | giacomo | 436 | } |