Rev 1404 | Rev 1416 | 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 | |||
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 | |||
1415 | giacomo | 374 | cprintf(".(%dus out = %lf)",(int)(rtmGetT(S)*1000000.0),*(real_T *)(rtmGetY(S))); |
1368 | giacomo | 375 | |
376 | if (rtmGetStopRequested(S)) break; |
||
377 | rt_OneStep(S); |
||
378 | |||
379 | task_endcycle(); |
||
380 | |||
381 | } |
||
382 | |||
383 | if (!GBLbuf.stopExecutionFlag && !rtmGetStopRequested(S)) { |
||
384 | /* Execute model last time step */ |
||
385 | rt_OneStep(S); |
||
386 | } |
||
387 | |||
388 | simulation_run = 0; |
||
389 | |||
390 | return NULL; |
||
391 | |||
392 | } |
||
393 | |||
1403 | giacomo | 394 | TASK INPUT_body(void *arg) { |
395 | |||
1404 | giacomo | 396 | int input_ports = rtmGetNumU(S); |
1403 | giacomo | 397 | real_T *input; |
398 | |||
399 | cprintf("Number of inputs = %d\n",input_ports); |
||
400 | |||
401 | input = (real_T *)rtmGetU(S); |
||
402 | |||
403 | while(1) { |
||
404 | |||
1415 | giacomo | 405 | input[0] = 1000.0; |
406 | input[1] = 1000.0; |
||
1403 | giacomo | 407 | |
408 | task_endcycle(); |
||
409 | |||
410 | } |
||
411 | |||
412 | return NULL; |
||
413 | |||
414 | } |
||
415 | |||
1368 | giacomo | 416 | int main() { |
417 | |||
1403 | giacomo | 418 | HARD_TASK_MODEL RTW_task,INPUT_task; |
419 | PID RTW_pid,INPUT_pid; |
||
1368 | giacomo | 420 | |
421 | Init_RealTime_Workshop(); |
||
422 | |||
423 | hard_task_default_model(RTW_task); |
||
1415 | giacomo | 424 | hard_task_def_mit(RTW_task,20000/*(int)(rtmGetStepSize(S) * 1000000.0)*/); |
425 | hard_task_def_wcet(RTW_task,9000); |
||
1368 | giacomo | 426 | hard_task_def_usemath(RTW_task); |
427 | hard_task_def_ctrl_jet(RTW_task); |
||
428 | |||
429 | RTW_pid = task_create("RTW",RTW_body,&RTW_task,NULL); |
||
430 | if (RTW_pid == NIL) { |
||
1369 | giacomo | 431 | cprintf("Error: Cannot create RealTime Workshop Task\n"); |
1368 | giacomo | 432 | sys_end(); |
433 | } |
||
434 | |||
1403 | giacomo | 435 | hard_task_default_model(INPUT_task); |
1415 | giacomo | 436 | hard_task_def_mit(INPUT_task,20000); |
437 | hard_task_def_wcet(INPUT_task,9000); |
||
1403 | giacomo | 438 | hard_task_def_usemath(INPUT_task); |
439 | hard_task_def_ctrl_jet(INPUT_task); |
||
440 | |||
441 | INPUT_pid = task_create("INPUT",INPUT_body,&INPUT_task,NULL); |
||
442 | if (INPUT_pid == NIL) { |
||
443 | cprintf("Error: Cannot create RealTime Workshop Task\n"); |
||
444 | sys_end(); |
||
445 | } |
||
446 | |||
447 | task_activate(INPUT_pid); |
||
1368 | giacomo | 448 | task_activate(RTW_pid); |
449 | |||
450 | while(simulation_run); |
||
451 | |||
452 | Close_RealTime_Workshop(); |
||
453 | |||
454 | sys_end(); |
||
455 | |||
1342 | giacomo | 456 | return 0; |
457 | |||
1368 | giacomo | 458 | } |