-%%\r
-%%\r
-%% description:\r
-%% This TLC script generates ert_main.c\r
-%% case.\r
-%%\r
-%% authors: Michal Sojka <sojkam1@fel.cvut.cz>\r
-%% Lukas Hamacek <hamacl1@fel.cvut.cz>\r
-%%\r
-%% Department of Control Engineering\r
-%% Faculty of Electrical Engineering\r
-%% Czech Technical University in Prague\r
-%%\r
-\r
-%% GENERATEDECLARATIONS\r
-%% This function generates main function declarations.\r
-%%\r
-\r
-%assign ::tid01Eq = LibGetTID01EQ()\r
-\r
-%function generateDeclarations() Output\r
-\r
- /* Multirate - Multitasking case main file */\r
- #define _BSD_SOURCE /* For usleep() */\r
- #define _POSIX_C_SOURCE 200112L /* For clock_gettime() & clock_nanosleep() */\r
- #include <stdio.h> /* This ert_main.c example uses printf/fflush */\r
- #include <pthread.h> /* Thread library header file */\r
- #include <sched.h> /* OS scheduler header file */\r
- #include <semaphore.h> /* Semaphores library header file */\r
- #include <time.h>\r
- #include <stdlib.h>\r
- #include <unistd.h>\r
- #include "%<modelName>.h" /* Model's header file */\r
- #include "rtwtypes.h" /* MathWorks types */\r
- %if extMode == 1\r
- #include "ext_work.h" /* External mode header file */\r
- %endif\r
-\r
- /*==================*\r
- * Required defines *\r
- *==================*/\r
-\r
- #ifndef MODEL\r
- # error Must specify a model name. Define MODEL=name.\r
- #else\r
- /* create generic macros that work with any model */\r
- # define EXPAND_CONCAT(name1,name2) name1 ## name2\r
- # define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)\r
- # define MODEL_INITIALIZE CONCAT(MODEL,_initialize)\r
- # define MODEL_STEP CONCAT(MODEL,_step)\r
- # define MODEL_TERMINATE CONCAT(MODEL,_terminate)\r
- # define RT_MDL CONCAT(MODEL,_M)\r
- #endif\r
-\r
- /* Error checking */\r
- #define STRINGIZE(num) #num\r
- #define POS(line) __FILE__ ":" STRINGIZE(line)\r
- #define CHECK0(expr) do { int __err = (expr); if (__err) { fprintf(stderr, "Error: %s returned '%s' at " POS(__LINE__) "\n", #expr, strerror(__err)); exit(1); } } while (0);\r
- #define CHECKE(expr) do { if ((expr) == -1) { perror(#expr " at " POS(__LINE__)); exit(1); } } while (0);\r
-\r
- /**\r
- * Maximal priority used by base rate thread.\r
- */\r
- #define MAX_PRIO (sched_get_priority_min(SCHED_FIFO) + %<numSampleTimes>)\r
-\r
- /**\r
- * Thread handle of the base rate thread.\r
- * Fundamental sample time = %<fundamentalStepSize>s\r
- */\r
- pthread_t base_rate_thread;\r
-\r
- /**\r
- * Thread handles of and semaphores for sub rate threads. The array\r
- * is indexed by TID, i.e. the first one or two elements are unused.\r
- %foreach i = numSampleTimes\r
- %if i == 0 || i == 1 && tid01Eq\r
- %continue\r
- %endif\r
- %assign s = sampleTime%<i>\r
- %assign o = offset%<i>\r
- * TID%<i>: sample time = %<s>s, offset = %<o>s\r
- %endforeach\r
- */\r
- struct sub_rate {\r
- pthread_t thread;\r
- sem_t sem;\r
- } sub_rate[%<numSampleTimes>];\r
-\r
- /**\r
- * Flag if the simulation has been terminated.\r
- */\r
- int simulationFinished = 0;\r
-\r
-%endfunction\r
-\r
-%function printfunc() Output\r
- /% printf("%s\n", __func__); %/\r
-%endfunction\r
-\r
-%function generateRtOneStep() Output\r
- %foreach j = numSampleTimes - 1\r
- %assign i = j + 1\r
- %if i == 1 && tid01Eq\r
- %continue\r
- %endif\r
- void *sub_rate%<i>(void *arg)\r
- {\r
- while(!simulationFinished) {\r
- sem_wait(&sub_rate[%<i>].sem); /* sem_val = 1 */\r
- %<printfunc()>\r
- %<modelName>_step%<i>();\r
- sem_wait(&sub_rate[%<i>].sem); /* sem_val = 0 */\r
- }\r
- }\r
- %endforeach\r
-%endfunction\r
-\r
-\r
-%% GENERATEMAIN\r
-%% This function generates code of the main function function.\r
-%%\r
-%function generateMain() Output\r
- /**\r
- * This is the thread function of the base rate loop.\r
- * Fundamental sample time = %<fundamentalStepSize>s\r
- */\r
- void * base_rate()\r
- {\r
- %assign sec = FEVAL("uint32", fundamentalStepSize)\r
- %assign nsec = FEVAL("uint32", (fundamentalStepSize - FEVAL("floor", fundamentalStepSize))*1000000000)\r
- struct timespec now, next;\r
- struct timespec period = { %<sec>, %<nsec> }; /* %<fundamentalStepSize> seconds */\r
- boolean_T eventFlags[%<numSampleTimes>]; /* Model has %<numSampleTimes> rates */\r
- int_T taskCounter[%<numSampleTimes>] = %<FcnInitializeTaskCounter()>;\r
- int_T OverrunFlags[%<numSampleTimes>];\r
- int step_sem_value;\r
- int_T i;\r
-\r
- %if extMode == 1\r
- %<SLibGenERTExtModeInit()>\r
- %endif\r
-\r
- clock_gettime(CLOCK_MONOTONIC, &next);\r
-\r
- /* Main loop, running until all the threads are terminated */\r
- while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {\r
- %<printfunc()>\r
- /* Check subrate overrun, set rates that need to run this time step*/\r
- %<LibCallSetEventForThisBaseStep("eventFlags")>\\r
-\r
- /* Trigger sub-rate threads */\r
- %foreach i = numSampleTimes\r
- %if i == 0 || i == 1 && tid01Eq\r
- %continue\r
- %endif\r
- %assign s = sampleTime%<i>\r
- %assign o = offset%<i>\r
- /* Sampling rate %<i>, sample time = %<s>, offset = %<o> */\r
- if (eventFlags[%<i>]) {\r
- eventFlags[%<i>] = FALSE;\r
- sem_getvalue(&sub_rate[%<i>].sem, &step_sem_value);\r
- if (step_sem_value) {\r
- rtmSetErrorStatus(%<modelName>_M, "Overrun");\r
- printf("Sub rate %<i> overrun, sample time=%<s>s, offset=%<o>s is too fast\n");\r
- break;\r
- }\r
- sem_post(&sub_rate[%<i>].sem);\r
- sem_post(&sub_rate[%<i>].sem);\r
- }\r
- %endforeach\r
-\r
- /* Execute base rate step */\r
- %if solverMode == "SingleTasking"\r
- %<modelName>_step();\r
- %else\r
- %<modelName>_step0();\r
- %endif\r
-\r
- %if extMode == 1\r
- rtExtModeCheckEndTrigger();\r
- %endif\r
-\r
- do {\r
- next.tv_sec += period.tv_sec;\r
- next.tv_nsec += period.tv_nsec;\r
- if (next.tv_nsec >= 1000000000) {\r
- next.tv_sec++;\r
- next.tv_nsec -= 1000000000;\r
- }\r
- clock_gettime(CLOCK_MONOTONIC, &now);\r
- if (now.tv_sec > next.tv_sec ||\r
- (now.tv_sec == next.tv_sec && now.tv_nsec > next.tv_nsec)) {\r
- uint32_T usec = (now.tv_sec - next.tv_sec) * 1000000 + (now.tv_nsec - next.tv_nsec)/1000;\r
- fprintf(stderr, "Base rate (%<fundamentalStepSize>s) overrun by %d us\n", usec);\r
- next = now;\r
- continue;\r
- }\r
- } while (0);\r
- clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);\r
-\r
- }\r
-\r
- simulationFinished = 1;\r
- /* Final step */\r
- for (i = %<1 + tid01Eq>; i < %<numSampleTimes>; i++) {\r
- sem_post(&sub_rate[i].sem);\r
- sem_post(&sub_rate[i].sem);\r
- }\r
- }\r
-\r
- /**\r
- * This is the main function of the model.\r
- * Multirate - Multitasking case main file\r
- */\r
- int_T main(int_T argc, const char_T *argv[])\r
- {\r
- const char_T *errStatus;\r
- int_T i;\r
- pthread_attr_t attr;\r
- struct sched_param sched_param;\r
-\r
- %if extMode == 1\r
- /* External mode */\r
- rtERTExtModeParseArgs(argc, argv);\r
- %else\r
- (void)(argc);\r
- (void *)(argv);\r
- %endif\r
-\r
- /* Initialize model */\r
- %<modelName>_initialize();\r
- simulationFinished = 0;\r
-\r
- /* Prepare task attributes */\r
- CHECK0(pthread_attr_init(&attr));\r
- CHECK0(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED));\r
- CHECK0(pthread_attr_setschedpolicy(&attr, SCHED_FIFO));\r
-\r
- %foreach i = numSampleTimes\r
- %if i == 0 || i == 1 && tid01Eq\r
- %continue\r
- %endif\r
- %assign s = sampleTime%<i>\r
- %assign o = offset%<i>\r
-\r
- /* Initializing the step semaphore of the loop %<i> */\r
- CHECKE(sem_init(&sub_rate[%<i>].sem, 0, 0));\r
-\r
- /* Starting loop %<i> thread for sample time = %<s>s, offset = %<o>s. */\r
- sched_param.sched_priority = MAX_PRIO - %<i>;\r
- CHECK0(pthread_attr_setschedparam(&attr, &sched_param));\r
- CHECK0(pthread_create(&sub_rate[%<i>].thread, &attr, sub_rate%<i>, (void*)%<i>));\r
-\r
- %endforeach\r
-\r
- /* Starting the base rate thread */\r
- sched_param.sched_priority = MAX_PRIO;\r
- CHECK0(pthread_attr_setschedparam(&attr, &sched_param));\r
- CHECK0(pthread_create(&base_rate_thread, &attr, base_rate, NULL));\r
- CHECK0(pthread_attr_destroy(&attr));\r
-\r
- %if extMode == 1\r
- /* External mode */\r
- while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {\r
- rtExtModeOneStep(rtmGetRTWExtModeInfo(RT_MDL), NUMST, (boolean_T *)&rtmGetStopRequested(RT_MDL));\r
- usleep(%<FEVAL("uint32", fundamentalStepSize * 1000000)>);\r
- }\r
- %endif\r
-\r
- /* Wait for threads to finish */\r
- pthread_join(base_rate_thread, NULL);\r
- %foreach i = numSampleTimes\r
- %if i == 0 || i == 1 && tid01Eq\r
- %continue\r
- %endif\r
- pthread_join(sub_rate[%<i>].thread, NULL);\r
- %endforeach\r
-\r
- %if extMode == 1\r
- rtExtModeShutdown(%<numSampleTimes>);\r
- %endif\r
-\r
- /* Terminate model */\r
- %<modelName>_terminate();\r
-\r
- errStatus = rtmGetErrorStatus(%<modelName>_M);\r
- if(errStatus != NULL && strcmp(errStatus, "Simulation finished")) {\r
- %%printf("%s\n", rtmGetErrorStatus(%<modelName>_M));\r
- if(!strcmp(errStatus, "Overrun")) {\r
- printf("ISR overrun - sampling rate too fast\n");\r
- }\r
- return(1);\r
- }\r
- return 0;\r
- }\r
-\r
-/* Local Variables: */\r
-/* compile-command: "make -f %<modelName>.mk" */\r
-/* End: */\r
-\r
-%endfunction\r
+%%
+%%
+%% description:
+%% This TLC script generates ert_main.c
+%% case.
+%%
+%% authors: Michal Sojka <sojkam1@fel.cvut.cz>
+%% Lukas Hamacek <hamacl1@fel.cvut.cz>
+%%
+%% Department of Control Engineering
+%% Faculty of Electrical Engineering
+%% Czech Technical University in Prague
+%%
+
+%% GENERATEDECLARATIONS
+%% This function generates main function declarations.
+%%
+
+%assign ::tid01Eq = LibGetTID01EQ()
+
+%function generateDeclarations() Output
+
+ /* Multirate - Multitasking case main file */
+ #define _BSD_SOURCE /* For usleep() */
+ #define _POSIX_C_SOURCE 200112L /* For clock_gettime() & clock_nanosleep() */
+ #include <stdio.h> /* This ert_main.c example uses printf/fflush */
+ #include <pthread.h> /* Thread library header file */
+ #include <sched.h> /* OS scheduler header file */
+ #include <semaphore.h> /* Semaphores library header file */
+ #include <time.h>
+ #include <stdlib.h>
+ #include <unistd.h>
+ #include "%<modelName>.h" /* Model's header file */
+ #include "rtwtypes.h" /* MathWorks types */
+ %if extMode == 1
+ #include "ext_work.h" /* External mode header file */
+ %endif
+
+ /*==================*
+ * Required defines *
+ *==================*/
+
+ #ifndef MODEL
+ # error Must specify a model name. Define MODEL=name.
+ #else
+ /* create generic macros that work with any model */
+ # define EXPAND_CONCAT(name1,name2) name1 ## name2
+ # define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)
+ # define MODEL_INITIALIZE CONCAT(MODEL,_initialize)
+ # define MODEL_STEP CONCAT(MODEL,_step)
+ # define MODEL_TERMINATE CONCAT(MODEL,_terminate)
+ # define RT_MDL CONCAT(MODEL,_M)
+ #endif
+
+ /* Error checking */
+ #define STRINGIZE(num) #num
+ #define POS(line) __FILE__ ":" STRINGIZE(line)
+ #define CHECK0(expr) do { int __err = (expr); if (__err) { fprintf(stderr, "Error: %s returned '%s' at " POS(__LINE__) "\n", #expr, strerror(__err)); exit(1); } } while (0);
+ #define CHECKE(expr) do { if ((expr) == -1) { perror(#expr " at " POS(__LINE__)); exit(1); } } while (0);
+
+ /**
+ * Maximal priority used by base rate thread.
+ */
+ #define MAX_PRIO (sched_get_priority_min(SCHED_FIFO) + %<numSampleTimes>)
+
+ /**
+ * Thread handle of the base rate thread.
+ * Fundamental sample time = %<fundamentalStepSize>s
+ */
+ pthread_t base_rate_thread;
+
+ /**
+ * Thread handles of and semaphores for sub rate threads. The array
+ * is indexed by TID, i.e. the first one or two elements are unused.
+ %foreach i = numSampleTimes
+ %if i == 0 || i == 1 && tid01Eq
+ %continue
+ %endif
+ %assign s = sampleTime%<i>
+ %assign o = offset%<i>
+ * TID%<i>: sample time = %<s>s, offset = %<o>s
+ %endforeach
+ */
+ struct sub_rate {
+ pthread_t thread;
+ sem_t sem;
+ } sub_rate[%<numSampleTimes>];
+
+ /**
+ * Flag if the simulation has been terminated.
+ */
+ int simulationFinished = 0;
+
+%endfunction
+
+%function printfunc() Output
+ /% printf("%s\n", __func__); %/
+%endfunction
+
+%function generateRtOneStep() Output
+ %foreach j = numSampleTimes - 1
+ %assign i = j + 1
+ %if i == 1 && tid01Eq
+ %continue
+ %endif
+ void *sub_rate%<i>(void *arg)
+ {
+ while(!simulationFinished) {
+ sem_wait(&sub_rate[%<i>].sem); /* sem_val = 1 */
+ %<printfunc()>
+ %<modelName>_step%<i>();
+ sem_wait(&sub_rate[%<i>].sem); /* sem_val = 0 */
+ }
+ }
+ %endforeach
+%endfunction
+
+
+%% GENERATEMAIN
+%% This function generates code of the main function function.
+%%
+%function generateMain() Output
+ /**
+ * This is the thread function of the base rate loop.
+ * Fundamental sample time = %<fundamentalStepSize>s
+ */
+ void * base_rate()
+ {
+ %assign sec = FEVAL("uint32", fundamentalStepSize)
+ %assign nsec = FEVAL("uint32", (fundamentalStepSize - FEVAL("floor", fundamentalStepSize))*1000000000)
+ struct timespec now, next;
+ struct timespec period = { %<sec>, %<nsec> }; /* %<fundamentalStepSize> seconds */
+ boolean_T eventFlags[%<numSampleTimes>]; /* Model has %<numSampleTimes> rates */
+ int_T taskCounter[%<numSampleTimes>] = %<FcnInitializeTaskCounter()>;
+ int_T OverrunFlags[%<numSampleTimes>];
+ int step_sem_value;
+ int_T i;
+
+ %if extMode == 1
+ %<SLibGenERTExtModeInit()>
+ %endif
+
+ clock_gettime(CLOCK_MONOTONIC, &next);
+
+ /* Main loop, running until all the threads are terminated */
+ while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
+ %<printfunc()>
+ /* Check subrate overrun, set rates that need to run this time step*/
+ %<LibCallSetEventForThisBaseStep("eventFlags")>\
+
+ /* Trigger sub-rate threads */
+ %foreach i = numSampleTimes
+ %if i == 0 || i == 1 && tid01Eq
+ %continue
+ %endif
+ %assign s = sampleTime%<i>
+ %assign o = offset%<i>
+ /* Sampling rate %<i>, sample time = %<s>, offset = %<o> */
+ if (eventFlags[%<i>]) {
+ eventFlags[%<i>] = FALSE;
+ sem_getvalue(&sub_rate[%<i>].sem, &step_sem_value);
+ if (step_sem_value) {
+ rtmSetErrorStatus(%<modelName>_M, "Overrun");
+ printf("Sub rate %<i> overrun, sample time=%<s>s, offset=%<o>s is too fast\n");
+ break;
+ }
+ sem_post(&sub_rate[%<i>].sem);
+ sem_post(&sub_rate[%<i>].sem);
+ }
+ %endforeach
+
+ /* Execute base rate step */
+ %if solverMode == "SingleTasking"
+ %<modelName>_step();
+ %else
+ %<modelName>_step0();
+ %endif
+
+ %if extMode == 1
+ rtExtModeCheckEndTrigger();
+ %endif
+
+ do {
+ next.tv_sec += period.tv_sec;
+ next.tv_nsec += period.tv_nsec;
+ if (next.tv_nsec >= 1000000000) {
+ next.tv_sec++;
+ next.tv_nsec -= 1000000000;
+ }
+ clock_gettime(CLOCK_MONOTONIC, &now);
+ if (now.tv_sec > next.tv_sec ||
+ (now.tv_sec == next.tv_sec && now.tv_nsec > next.tv_nsec)) {
+ uint32_T usec = (now.tv_sec - next.tv_sec) * 1000000 + (now.tv_nsec - next.tv_nsec)/1000;
+ fprintf(stderr, "Base rate (%<fundamentalStepSize>s) overrun by %d us\n", usec);
+ next = now;
+ continue;
+ }
+ } while (0);
+ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);
+
+ }
+
+ simulationFinished = 1;
+ /* Final step */
+ for (i = %<1 + tid01Eq>; i < %<numSampleTimes>; i++) {
+ sem_post(&sub_rate[i].sem);
+ sem_post(&sub_rate[i].sem);
+ }
+ }
+
+ /**
+ * This is the main function of the model.
+ * Multirate - Multitasking case main file
+ */
+ int_T main(int_T argc, const char_T *argv[])
+ {
+ const char_T *errStatus;
+ int_T i;
+ pthread_attr_t attr;
+ struct sched_param sched_param;
+
+ %if extMode == 1
+ /* External mode */
+ rtERTExtModeParseArgs(argc, argv);
+ %else
+ (void)(argc);
+ (void *)(argv);
+ %endif
+
+ /* Initialize model */
+ %<modelName>_initialize();
+ simulationFinished = 0;
+
+ /* Prepare task attributes */
+ CHECK0(pthread_attr_init(&attr));
+ CHECK0(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED));
+ CHECK0(pthread_attr_setschedpolicy(&attr, SCHED_FIFO));
+
+ %foreach i = numSampleTimes
+ %if i == 0 || i == 1 && tid01Eq
+ %continue
+ %endif
+ %assign s = sampleTime%<i>
+ %assign o = offset%<i>
+
+ /* Initializing the step semaphore of the loop %<i> */
+ CHECKE(sem_init(&sub_rate[%<i>].sem, 0, 0));
+
+ /* Starting loop %<i> thread for sample time = %<s>s, offset = %<o>s. */
+ sched_param.sched_priority = MAX_PRIO - %<i>;
+ CHECK0(pthread_attr_setschedparam(&attr, &sched_param));
+ CHECK0(pthread_create(&sub_rate[%<i>].thread, &attr, sub_rate%<i>, (void*)%<i>));
+
+ %endforeach
+
+ /* Starting the base rate thread */
+ sched_param.sched_priority = MAX_PRIO;
+ CHECK0(pthread_attr_setschedparam(&attr, &sched_param));
+ CHECK0(pthread_create(&base_rate_thread, &attr, base_rate, NULL));
+ CHECK0(pthread_attr_destroy(&attr));
+
+ %if extMode == 1
+ /* External mode */
+ while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
+ rtExtModeOneStep(rtmGetRTWExtModeInfo(RT_MDL), NUMST, (boolean_T *)&rtmGetStopRequested(RT_MDL));
+ usleep(%<FEVAL("uint32", fundamentalStepSize * 1000000)>);
+ }
+ %endif
+
+ /* Wait for threads to finish */
+ pthread_join(base_rate_thread, NULL);
+ %foreach i = numSampleTimes
+ %if i == 0 || i == 1 && tid01Eq
+ %continue
+ %endif
+ pthread_join(sub_rate[%<i>].thread, NULL);
+ %endforeach
+
+ %if extMode == 1
+ rtExtModeShutdown(%<numSampleTimes>);
+ %endif
+
+ /* Terminate model */
+ %<modelName>_terminate();
+
+ errStatus = rtmGetErrorStatus(%<modelName>_M);
+ if(errStatus != NULL && strcmp(errStatus, "Simulation finished")) {
+ %%printf("%s\n", rtmGetErrorStatus(%<modelName>_M));
+ if(!strcmp(errStatus, "Overrun")) {
+ printf("ISR overrun - sampling rate too fast\n");
+ }
+ return(1);
+ }
+ return 0;
+ }
+
+/* Local Variables: */
+/* compile-command: "make -f %<modelName>.mk" */
+/* End: */
+
+%endfunction
projects / ert_linux.git / commitdiff