%%\r
%% date: 3nd Feb 2009\r
%%\r
-%% author: Lukas Hamacek\r
+%% author: Lukas Hamacek, Michal Sojka\r
%% e-mail: hamacl1@fel.cvut.cz\r
%% Department of Control Engineering\r
%% Faculty of Electrical Engineering\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
-\r
+ #define _POSIX_C_SOURCE 199309L /* For clock_gettime() */\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 "pthread_periodic.h" /* Periodic threads library header file*/\r
+ #include <time.h>\r
+ #include <stdlib.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 main loop thread.\r
+ * Maximal priority used by base rate thread.\r
*/\r
- #define MAX_PRIO sched_get_priority_max(SCHED_FIFO) - 4\r
+ #define MAX_PRIO (sched_get_priority_min(SCHED_FIFO) + %<numSampleTimes>)\r
\r
/**\r
- * Thread handler of the main loop thread.\r
+ * Thread handle of the base rate thread.\r
* Fundamental sample time = %<fundamentalStepSize>s\r
*/\r
- pthread_t main_loop_thread;\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
-\r
- /**\r
- * Thread handler of the sample time %<i> loop thread.\r
- * Sample time = %<s>s, offset = %<o>s\r
- */\r
- pthread_t rt_OneStep%<i>_thread;\r
-\r
- /**\r
- * Semaphore used to suspend rt_OneStep%<i> loop until next simulation step.\r
- */\r
- sem_t step%<i>_semaphore;\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
%endfunction\r
\r
-\r
-%% GENERATERTONESTEP\r
-%% This function generates code of rt_OneStep function.\r
-%%\r
%function generateRtOneStep() Output\r
-\r
- %foreach i = numSampleTimes\r
- %assign s = sampleTime%<i>\r
- %assign o = offset%<i>\r
-\r
- /**\r
- * This is thread function of the simulation one step cycle.\r
- * Sample time = %<s>s, offset = %<o>s\r
- */\r
- void * rt_OneStep%<i>()\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
- /* Setting up the pririty of the sample time loop */\r
- struct sched_param scheduling_parameters;\r
- scheduling_parameters.sched_priority = MAX_PRIO - 1 - %<i>;\r
- if (0 != pthread_setschedparam(pthread_self(), SCHED_FIFO, &scheduling_parameters))\r
- {\r
- %%log_message("[ERROR] Failed to set sample time thread priority, Login as root and start again.", VERBOSEL);\r
- printf("[ERROR] Failed to set the loop %<i> thread priority. Login as root and start again.\n");\r
- }\r
-\r
- while(!simulationFinished) {\r
- sem_wait(&step%<i>_semaphore); /* sem_val = 1 */\r
- %<modelName>_step%<i>();\r
- sem_wait(&step%<i>_semaphore); /* sem_val = 0 */\r
- }\r
+ while(!simulationFinished) {\r
+ sem_wait(&sub_rate[%<i>].sem); /* sem_val = 1 */\r
+ printf("%s\n", __func__);\r
+ %<modelName>_step%<i>();\r
+ %if extMode == 1\r
+ rtExtModeUpload(%<i>, %<RTMGetTaskTimeForTID(i)>);\r
+ %endif\r
+ sem_wait(&sub_rate[%<i>].sem); /* sem_val = 0 */\r
+ }\r
}\r
%endforeach\r
%endfunction\r
%% This function generates code of the main function function.\r
%%\r
%function generateMain() Output\r
-\r
/**\r
* This is the thread function of the main loop.\r
* Fundamental sample time = %<fundamentalStepSize>s\r
*/\r
- void * main_loop()\r
+ void * base_rate()\r
{\r
- struct timespec start;\r
- struct timespec period;\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
- /* Setting up the pririty of the main loop */\r
- struct sched_param scheduling_parameters;\r
- scheduling_parameters.sched_priority = sched_get_priority_max(SCHED_FIFO) - 4;\r
- if (0 != pthread_setschedparam(pthread_self(), SCHED_FIFO, &scheduling_parameters))\r
- {\r
- %%log_message("[ERROR] Failed to set main thread priority, Login as root and start again", VERBOSEL);\r
- printf("[ERROR] Failed to set main thread priority. Login as root and start again.\n");\r
- }\r
-\r
- period.tv_sec = (unsigned long long)%<fundamentalStepSize>;\r
- period.tv_nsec = (unsigned long long)((%<fundamentalStepSize>-period.tv_sec)*1000000000);\r
+ %if extMode == 1\r
+ /* External mode */\r
+ rtSetTFinalForExtMode(&rtmGetTFinal(RT_MDL));\r
+ rtExtModeCheckInit(NUMST);\r
+ rtExtModeWaitForStartPkt(rtmGetRTWExtModeInfo(RT_MDL), NUMST, (boolean_T *)&rtmGetStopRequested(RT_MDL));\r
+ %endif\r
\r
- clock_gettime(CLOCK_REALTIME, &start);\r
- pthread_make_periodic_np(pthread_self(), &start, &period);\r
+ (void)printf("\n** starting the model **\n");\r
\r
- int step_sem_value;\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
- pthread_wait_np();\r
- %<modelName>_SetEventsForThisBaseStep(eventFlags);\r
-\r
- /* Base sampling rate */\r
- sem_getvalue(&step0_semaphore, &step_sem_value);\r
- if(step_sem_value) {\r
- rtmSetErrorStatus(%<modelName>_M, "Overrun");\r
- printf("Loop 0 overrun, sample time=%<sampleTime0>s, offset=%<offset0>s is too fast\n");\r
- break;\r
- }\r
- sem_post(&step0_semaphore);\r
- sem_post(&step0_semaphore);\r
-\r
- %foreach i = numSampleTimes-1\r
- %assign s = sampleTime%<i+1>\r
- %assign o = offset%<i+1>\r
- /* Sampling rate %<i+1>, sample time = %<s>, offset = %<o> */\r
- if (eventFlags[%<i+1>]) {\r
- sem_getvalue(&step%<i+1>_semaphore, &step_sem_value);\r
- if(step_sem_value) {\r
+ printf("%s\n", __func__);\r
+ /* Check subrate overrun, set rates that need to run this time step*/\r
+ %<LibCallSetEventForThisBaseStep("eventFlags")>\\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
+ /* 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("Loop %<i+1> overrun, sample time=%<s>s, offset=%<o>s is too fast\n");\r
+ printf("Sub rate %<i> overrun, sample time=%<s>s, offset=%<o>s is too fast\n");\r
break;\r
}\r
- sem_post(&step%<i+1>_semaphore);\r
- sem_post(&step%<i+1>_semaphore);\r
+ sem_post(&sub_rate[%<i>].sem);\r
+ sem_post(&sub_rate[%<i>].sem);\r
}\r
%endforeach\r
\r
%if extMode == 1\r
+ /* external mode */\r
+ rtExtModeUpload(0, %<RTMGetTaskTimeForTID(0)>);\r
+ %if tid01Eq\r
+ rtExtModeUpload(1, %<RTMGetTaskTimeForTID(1)>);\r
+ %endif\r
+\r
rtExtModeCheckEndTrigger();\r
+ rtExtModePauseIfNeeded(rtmGetRTWExtModeInfo(RT_MDL),\r
+ NUMST,\r
+ (boolean_T *)&rtmGetStopRequested(RT_MDL));\r
+ rtExtModeOneStep(rtmGetRTWExtModeInfo(RT_MDL),\r
+ NUMST,\r
+ (boolean_T *)&rtmGetStopRequested(RT_MDL));\r
%endif\r
+\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
+ uint64_T nsec = (now.tv_sec - next.tv_sec) * 1000000000 + now.tv_nsec - next.tv_nsec;\r
+ fprintf(stderr, "Base rate (%<s>s) overrun by %d us\n", (int)(nsec/1000));\r
+ }\r
+ clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);\r
+\r
}\r
\r
simulationFinished = 1;\r
/* Final step */\r
- %foreach i = numSampleTimes\r
- sem_post(&step%<i>_semaphore);\r
- sem_post(&step%<i>_semaphore);\r
- %endforeach\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
*/\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
(void *)(argv);\r
%endif\r
\r
- /* Initialize periodic thread library */\r
- pthread_periodic_init_np();\r
-\r
/* Initialize model */\r
- %<modelName>_initialize(1);\r
+ %<modelName>_initialize();\r
simulationFinished = 0;\r
\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
- if(sem_init(&step%<i>_semaphore, 0, 0) < 0) {\r
- printf("[ERROR] Step semaphore %<i> initialization failed!\n");\r
- %%log_message("[ERROR] Step semaphore %<i> initialization failed", VERBOSEL);\r
- return(1);\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
- pthread_create(&rt_OneStep%<i>_thread, NULL, rt_OneStep%<i>, NULL);\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 main loop */\r
- pthread_create(&main_loop_thread, NULL, main_loop, NULL);\r
- pthread_join(main_loop_thread, NULL);\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
+\r
+ /* Wait for threads to finish */\r
+ CHECK0(pthread_attr_destroy(&attr));\r
+ pthread_join(base_rate_thread, NULL);\r
%foreach i = numSampleTimes\r
- pthread_join(rt_OneStep%<i>_thread, NULL);\r
+ %if i == 0 || i == 1 && tid01Eq\r
+ %continue\r
+ %endif\r
+ pthread_join(sub_rate[%<i>].thread, NULL);\r
%endforeach\r
\r
/* Terminate model */\r
%<modelName>_terminate();\r
\r
- const char_T *errStatus = rtmGetErrorStatus(%<modelName>_M);\r
- int_T i;\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
}\r
return 0;\r
}\r
-%endfunction\r
\r
-%% [EOF] linux_ert_target_miltitasking_main.tlc\r
+/* Local Variables: */\r
+/* compile-command: "make -f %<modelName>.mk" */\r
+/* End: */\r
+\r
+%endfunction\r
projects / ert_linux.git / commitdiff