ert_linux/ert_linux_multitasking_main.tlc

   1 %%\r
   2 %%\r
   3 %% description:\r
   4 %%  This TLC script generates ert_main.c\r
   5 %%  case.\r
   6 %%\r
   7 %% authors: Michal Sojka <sojkam1@fel.cvut.cz>\r
   8 %%          Lukas Hamacek <hamacl1@fel.cvut.cz>\r
   9 %%\r
  10 %% Department of Control Engineering\r
  11 %% Faculty of Electrical Engineering\r
  12 %% Czech Technical University in Prague\r
  13 %%\r
  14 \r
  15 %% GENERATEDECLARATIONS\r
  16 %%  This function generates main function declarations.\r
  17 %%\r
  18 \r
  19 %assign ::tid01Eq = LibGetTID01EQ()\r
  20 \r
  21 %function generateDeclarations() Output\r
  22 \r
  23   /* Multirate - Multitasking case main file */\r
  24   #define _BSD_SOURCE                    /* For usleep() */\r
  25   #define _POSIX_C_SOURCE 200112L        /* For clock_gettime() & clock_nanosleep() */\r
  26   #include <stdio.h>                     /* This ert_main.c example uses printf/fflush */\r
  27   #include <pthread.h>                   /* Thread library header file */\r
  28   #include <sched.h>                     /* OS scheduler header file */\r
  29   #include <semaphore.h>                 /* Semaphores library header file */\r
  30   #include <time.h>\r
  31   #include <stdlib.h>\r
  32   #include <unistd.h>\r
  33   #include "%<modelName>.h"              /* Model's header file */\r
  34   #include "rtwtypes.h"                  /* MathWorks types */\r
  35   %if extMode == 1\r
  36     #include "ext_work.h"                  /* External mode header file */\r
  37   %endif\r
  38 \r
  39   /*==================*\r
  40    * Required defines *\r
  41    *==================*/\r
  42 \r
  43   #ifndef MODEL\r
  44   # error Must specify a model name.  Define MODEL=name.\r
  45   #else\r
  46   /* create generic macros that work with any model */\r
  47   # define EXPAND_CONCAT(name1,name2) name1 ## name2\r
  48   # define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)\r
  49   # define MODEL_INITIALIZE CONCAT(MODEL,_initialize)\r
  50   # define MODEL_STEP       CONCAT(MODEL,_step)\r
  51   # define MODEL_TERMINATE  CONCAT(MODEL,_terminate)\r
  52   # define RT_MDL           CONCAT(MODEL,_M)\r
  53   #endif\r
  54 \r
  55   /* Error checking */\r
  56   #define STRINGIZE(num) #num\r
  57   #define POS(line) __FILE__ ":" STRINGIZE(line)\r
  58   #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
  59   #define CHECKE(expr) do { if ((expr) == -1) { perror(#expr " at " POS(__LINE__)); exit(1); } } while (0);\r
  60 \r
  61   /**\r
  62    * Maximal priority used by base rate thread.\r
  63    */\r
  64   #define MAX_PRIO (sched_get_priority_min(SCHED_FIFO) + %<numSampleTimes>)\r
  65 \r
  66   /**\r
  67    * Thread handle of the base rate thread.\r
  68    * Fundamental sample time = %<fundamentalStepSize>s\r
  69    */\r
  70   pthread_t base_rate_thread;\r
  71 \r
  72   /**\r
  73    * Thread handles of and semaphores for sub rate threads. The array\r
  74    * is indexed by TID, i.e. the first one or two elements are unused.\r
  75   %foreach i = numSampleTimes\r
  76       %if i == 0 || i == 1 && tid01Eq\r
  77       %continue\r
  78     %endif\r
  79     %assign s = sampleTime%<i>\r
  80     %assign o = offset%<i>\r
  81    * TID%<i>: sample time = %<s>s, offset = %<o>s\r
  82   %endforeach\r
  83    */\r
  84   struct sub_rate {\r
  85   pthread_t thread;\r
  86   sem_t     sem;\r
  87   } sub_rate[%<numSampleTimes>];\r
  88 \r
  89   /**\r
  90    * Flag if the simulation has been terminated.\r
  91    */\r
  92   int simulationFinished = 0;\r
  93 \r
  94 %endfunction\r
  95 \r
  96 %function printfunc() Output\r
  97   /% printf("%s\n", __func__); %/\r
  98 %endfunction\r
  99 \r
 100 %function generateRtOneStep() Output\r
 101   %foreach j = numSampleTimes - 1\r
 102     %assign i = j + 1\r
 103     %if i == 1 && tid01Eq\r
 104       %continue\r
 105     %endif\r
 106     void *sub_rate%<i>(void *arg)\r
 107     {\r
 108       while(!simulationFinished) {\r
 109         sem_wait(&sub_rate[%<i>].sem);    /* sem_val = 1 */\r
 110         %<printfunc()>\r
 111         %<modelName>_step%<i>();\r
 112         sem_wait(&sub_rate[%<i>].sem);    /* sem_val = 0 */\r
 113       }\r
 114     }\r
 115   %endforeach\r
 116 %endfunction\r
 117 \r
 118 \r
 119 %% GENERATEMAIN\r
 120 %%  This function generates code of the main function function.\r
 121 %%\r
 122 %function generateMain() Output\r
 123   /**\r
 124    * This is the thread function of the base rate loop.\r
 125    * Fundamental sample time = %<fundamentalStepSize>s\r
 126    */\r
 127   void * base_rate()\r
 128   {\r
 129     %assign  sec = FEVAL("uint32", fundamentalStepSize)\r
 130     %assign nsec = FEVAL("uint32", (fundamentalStepSize - FEVAL("floor", fundamentalStepSize))*1000000000)\r
 131     struct timespec now, next;\r
 132     struct timespec period = { %<sec>, %<nsec> }; /* %<fundamentalStepSize> seconds */\r
 133     boolean_T eventFlags[%<numSampleTimes>];             /* Model has %<numSampleTimes> rates */\r
 134     int_T taskCounter[%<numSampleTimes>] = %<FcnInitializeTaskCounter()>;\r
 135     int_T OverrunFlags[%<numSampleTimes>];\r
 136     int step_sem_value;\r
 137     int_T i;\r
 138 \r
 139     %if extMode == 1\r
 140       %<SLibGenERTExtModeInit()>\r
 141     %endif\r
 142 \r
 143     clock_gettime(CLOCK_MONOTONIC, &next);\r
 144 \r
 145     /* Main loop, running until all the threads are terminated */\r
 146     while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {\r
 147       %<printfunc()>\r
 148       /* Check subrate overrun, set rates that need to run this time step*/\r
 149       %<LibCallSetEventForThisBaseStep("eventFlags")>\\r
 150 \r
 151       /* Trigger sub-rate threads */\r
 152       %foreach i = numSampleTimes\r
 153         %if i == 0 || i == 1 && tid01Eq\r
 154           %continue\r
 155         %endif\r
 156         %assign s = sampleTime%<i>\r
 157         %assign o = offset%<i>\r
 158         /* Sampling rate %<i>, sample time = %<s>, offset = %<o> */\r
 159         if (eventFlags[%<i>]) {\r
 160           eventFlags[%<i>] = FALSE;\r
 161           sem_getvalue(&sub_rate[%<i>].sem, &step_sem_value);\r
 162           if (step_sem_value) {\r
 163             rtmSetErrorStatus(%<modelName>_M, "Overrun");\r
 164             printf("Sub rate %<i> overrun, sample time=%<s>s, offset=%<o>s is too fast\n");\r
 165             break;\r
 166           }\r
 167           sem_post(&sub_rate[%<i>].sem);\r
 168           sem_post(&sub_rate[%<i>].sem);\r
 169         }\r
 170       %endforeach\r
 171 \r
 172       /* Execute base rate step */\r
 173       %if solverMode == "SingleTasking"\r
 174         %<modelName>_step();\r
 175       %else\r
 176         %<modelName>_step0();\r
 177       %endif\r
 178 \r
 179       %if extMode == 1\r
 180         rtExtModeCheckEndTrigger();\r
 181       %endif\r
 182 \r
 183       do {\r
 184         next.tv_sec += period.tv_sec;\r
 185         next.tv_nsec += period.tv_nsec;\r
 186         if (next.tv_nsec >= 1000000000) {\r
 187           next.tv_sec++;\r
 188           next.tv_nsec -= 1000000000;\r
 189         }\r
 190         clock_gettime(CLOCK_MONOTONIC, &now);\r
 191         if (now.tv_sec > next.tv_sec ||\r
 192             (now.tv_sec == next.tv_sec && now.tv_nsec > next.tv_nsec)) {\r
 193           uint32_T usec = (now.tv_sec - next.tv_sec) * 1000000 + (now.tv_nsec - next.tv_nsec)/1000;\r
 194           fprintf(stderr, "Base rate (%<fundamentalStepSize>s) overrun by %d us\n", usec);\r
 195           next = now;\r
 196           continue;\r
 197         }\r
 198       } while (0);\r
 199       clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next, NULL);\r
 200 \r
 201     }\r
 202 \r
 203     simulationFinished = 1;\r
 204     /* Final step */\r
 205     for (i = %<1 + tid01Eq>; i < %<numSampleTimes>; i++) {\r
 206       sem_post(&sub_rate[i].sem);\r
 207       sem_post(&sub_rate[i].sem);\r
 208     }\r
 209   }\r
 210 \r
 211   /**\r
 212    * This is the main function of the model.\r
 213    * Multirate - Multitasking case main file\r
 214    */\r
 215   int_T main(int_T argc, const char_T *argv[])\r
 216   {\r
 217     const char_T *errStatus;\r
 218     int_T i;\r
 219     pthread_attr_t attr;\r
 220     struct sched_param sched_param;\r
 221 \r
 222     %if extMode == 1\r
 223       /* External mode */\r
 224       rtERTExtModeParseArgs(argc, argv);\r
 225     %else\r
 226       (void)(argc);\r
 227       (void *)(argv);\r
 228     %endif\r
 229 \r
 230     /* Initialize model */\r
 231     %<modelName>_initialize();\r
 232     simulationFinished = 0;\r
 233 \r
 234     /* Prepare task attributes */\r
 235     CHECK0(pthread_attr_init(&attr));\r
 236     CHECK0(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED));\r
 237     CHECK0(pthread_attr_setschedpolicy(&attr, SCHED_FIFO));\r
 238 \r
 239     %foreach i = numSampleTimes\r
 240       %if i == 0 || i == 1 && tid01Eq\r
 241         %continue\r
 242       %endif\r
 243       %assign s = sampleTime%<i>\r
 244       %assign o = offset%<i>\r
 245 \r
 246       /* Initializing the step semaphore of the loop %<i> */\r
 247       CHECKE(sem_init(&sub_rate[%<i>].sem, 0, 0));\r
 248 \r
 249       /* Starting loop %<i> thread for sample time = %<s>s, offset = %<o>s. */\r
 250       sched_param.sched_priority = MAX_PRIO - %<i>;\r
 251       CHECK0(pthread_attr_setschedparam(&attr, &sched_param));\r
 252       CHECK0(pthread_create(&sub_rate[%<i>].thread, &attr, sub_rate%<i>, (void*)%<i>));\r
 253 \r
 254     %endforeach\r
 255 \r
 256     /* Starting the base rate thread */\r
 257     sched_param.sched_priority = MAX_PRIO;\r
 258     CHECK0(pthread_attr_setschedparam(&attr, &sched_param));\r
 259     CHECK0(pthread_create(&base_rate_thread, &attr, base_rate, NULL));\r
 260     CHECK0(pthread_attr_destroy(&attr));\r
 261 \r
 262     %if extMode == 1\r
 263       /* External mode */\r
 264       while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {\r
 265         rtExtModeOneStep(rtmGetRTWExtModeInfo(RT_MDL), NUMST, (boolean_T *)&rtmGetStopRequested(RT_MDL));\r
 266         usleep(%<FEVAL("uint32", fundamentalStepSize * 1000000)>);\r
 267       }\r
 268     %endif\r
 269 \r
 270     /* Wait for threads to finish */\r
 271     pthread_join(base_rate_thread, NULL);\r
 272     %foreach i = numSampleTimes\r
 273       %if i == 0 || i == 1 && tid01Eq\r
 274         %continue\r
 275       %endif\r
 276       pthread_join(sub_rate[%<i>].thread, NULL);\r
 277     %endforeach\r
 278 \r
 279     %if extMode == 1\r
 280       rtExtModeShutdown(%<numSampleTimes>);\r
 281     %endif\r
 282 \r
 283     /* Terminate model */\r
 284     %<modelName>_terminate();\r
 285 \r
 286     errStatus = rtmGetErrorStatus(%<modelName>_M);\r
 287     if(errStatus != NULL && strcmp(errStatus, "Simulation finished")) {\r
 288       %%printf("%s\n", rtmGetErrorStatus(%<modelName>_M));\r
 289       if(!strcmp(errStatus, "Overrun")) {\r
 290         printf("ISR overrun - sampling rate too fast\n");\r
 291       }\r
 292       return(1);\r
 293     }\r
 294     return 0;\r
 295   }\r
 296 \r
 297 /* Local Variables: */\r
 298 /* compile-command: "make -f %<modelName>.mk" */\r
 299 /* End: */\r
 300 \r
 301 %endfunction\r