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