ert_linux/ert_linux_multitasking_main.tlc

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