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