ert_linux/ert_linux_main.tlc

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