ert_mac_main.tlc

%%
%%
%% description:
%%  This TLC script generates ert_main.c
%%  case for MAC.
%%
%% authors: Constantin Wiesener <wiesener@control.tu-berlin.de>
%%
%% Department of Control Systems Engineering
%% Faculty of Electrical Engineering
%% TU Berlin

%% GENERATEDECLARATIONS
%%  This function generates main function declarations.
%%
%function DisableGenerateExampleMain() void
  
  %if GenerateSampleERTMain
    %assign ::CompiledModel.GenerateSampleERTMain = TLC_FALSE
    %warning Overriding example ert_main.c!
  %endif

%endfunction

%assign ::tid01Eq = LibGetTID01EQ()

%function generateDeclarations() Output

    // Multirate - Multitasking case main file
    #define _BSD_SOURCE                    // For usleep()
    #include <stdio.h>                     // This ert_main.c example uses printf/fflush
    #include <pthread.h>                   // Thread library header file
    #include <mach/mach.h>
    #include <mach/mach_init.h>
    #include <mach/mach_time.h>
    #include <mach/thread_policy.h>
    #include <dispatch/dispatch.h>
    #include <stdlib.h>
    #include <stdbool.h>
    #include <unistd.h>
    #include <sys/mman.h>                  // For mlockall()
    #include <csignal>
    #include "%<modelName>.h"              // Model's header file
    #include "rtwtypes.h"                  // MathWorks types
    %if extMode == 1
    #include "ext_work.h"                // External mode header file
    %endif

    #ifndef TRUE
    #define TRUE true
    #define FALSE false
    #endif

    //!
    //! Required defines 
    //!

    #ifndef MODEL
    # error Must specify a model name.  Define MODEL=name.
    #else
    // create generic macros that work with any model
    # define EXPAND_CONCAT(name1,name2) name1 ## name2
    # define CONCAT(name1,name2) EXPAND_CONCAT(name1,name2)
    # define MODEL_INITIALIZE CONCAT(MODEL,_initialize)
    # define MODEL_STEP       CONCAT(MODEL,_step)
    # define MODEL_TERMINATE  CONCAT(MODEL,_terminate)
    # define RT_MDL           CONCAT(MODEL,_M)
    #endif

    //! Error checking
    #define STRINGIZE(num) #num
    #define POS(line) __FILE__ ":" STRINGIZE(line)
    #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);
    #define CHECKE(expr) do { if ((expr) == -1) { perror(#expr " at " POS(__LINE__)); exit(1); } } while (0);

    // Signal handler for ABORT during simulation
    void abortHandler(int sig) {
        fprintf(stderr, "Simulation aborted by pressing CTRL+C\n");
        rtmSetStopRequested(%<modelName>_M, 1);
    }

    //!
    //! Thread handle of the base rate thread.
    //! Fundamental sample time = %<fundamentalStepSize>s
    //!
    pthread_t base_rate_thread;

    //
    // Thread handles of and semaphores for sub rate threads. The array
    // is indexed by TID, i.e. the first one or two elements are unused.
    %foreach i = numSampleTimes
        %if i == 0 || i == 1 && tid01Eq
        %continue
      %endif
      %assign s = sampleTime%<i>
      %assign o = offset%<i>
    // TID%<i>: sample time = %<s>s, offset = %<o>s
     %endforeach  
    struct sub_rate {
    pthread_t thread;
    bool is_running;
    dispatch_semaphore_t sem;
    } sub_rate[%<numSampleTimes>];
    
    //! Prototype for realtime scheduling setting function
    int move_pthread_to_realtime_scheduling_class(double Tcomputation, double Tconstraint);
    
    //! Flag if the simulation has been terminated.
    //!
    int simulationFinished = 0;

    %if extMode == 1
    //! Indication that the base rate thread has started
    dispatch_semaphore_t ext_mode_ready;    
    %endif
%endfunction

    %function printfunc() Output
    /% printf("%s\n", __func__); %/
    %endfunction

    %function generateRtOneStep() Output
  %foreach j = numSampleTimes - 1
    %assign i = j + 1
    %if i == 1 && tid01Eq
      %continue
    %endif
    void *sub_rate%<i>(void *arg)
    {
        %assign comp = FEVAL("double", 0.5 * sampleTime%<i> * 1000.0)
        %assign const = FEVAL("double", comp * 2)
        // set thread to zero period with a computation cost of %<comp> ms and a constraint of %<const> ms
        move_pthread_to_realtime_scheduling_class(1.0, 2.0);

        while(!simulationFinished) {
            dispatch_semaphore_wait(sub_rate[%<i>].sem, DISPATCH_TIME_FOREVER);    // sem_val = 1
            %<printfunc()>
            // fprintf(stderr, "Subrate %<i> executed\n");
            sub_rate[%<i>].is_running = true;
            %<modelName>_step%<i>();
            sub_rate[%<i>].is_running = false;

        }
        return NULL;
    }
  %endforeach
    %endfunction

    %% GENERATEMAIN
    %%  This function generates code of the main function function.
    %%
    %function generateMain() Output
    //!
    //! This is the thread function of the base rate loop.
    //! Fundamental sample time = %<fundamentalStepSize>s
    //!
    #ifdef __cplusplus
    void * base_rate(void *)
    #else
    void * base_rate()
    #endif
    {
        %assign comp = FEVAL("double", 0.5 * fundamentalStepSize * 1000.0)
        %assign const = FEVAL("double", comp * 2)
        // set thread to zero period with a computation cost of %<comp> ms and a constraint of %<const> ms
        move_pthread_to_realtime_scheduling_class(1.0, 2.0);
        //fprintf(stderr, "Computation %f ms Constraints %f \n", 1.0, 2.0);
        uint64_t next, now = 0;
        uint64_t period =  1000000000ULL * %<fundamentalStepSize>; // %<fundamentalStepSize> seconds
        boolean_T eventFlags[%<numSampleTimes>];             // Model has %<numSampleTimes> rates
        uint32_T taskCounter[%<numSampleTimes>] = %<FcnInitializeTaskCounter()>;
        int_T OverrunFlags[%<numSampleTimes>];
        int_T i;

        %if extMode == 1
          %<SLibGenERTExtModeInit()>
          dispatch_semaphore_signal(ext_mode_ready);
        %endif

        //! get current CPU tick
        next = mach_absolute_time();

        // Main loop, running until all the threads are terminated
        while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
          %<printfunc()>
          // Check subrate overrun, set rates that need to run this time step
          %foreach i = numSampleTimes
        %if i == 0 || i == 1 && tid01Eq
          %continue
        %endif
           
            if (taskCounter[%<i>] == 0) {
              if (eventFlags[%<i>]) {
                OverrunFlags[0] = false;
                OverrunFlags[%<i>] = true;

                // Sampling too fast
                rtmSetErrorStatus(%<modelName>_M, "Overrun");
                return 0;
              }

              eventFlags[%<i>] = true;
            }
            %assign T = FEVAL("int32", FEVAL("floor", sampleTime%<i> / fundamentalStepSize))
            taskCounter[%<i>]++;
            if (taskCounter[%<i>] == %<T>) {
              taskCounter[%<i>]= 0;
            }
        %endforeach

          // Trigger sub-rate threads
          %foreach i = numSampleTimes
        %if i == 0 || i == 1 && tid01Eq
          %continue
        %endif
        %assign s = sampleTime%<i>
        %assign o = offset%<i>
        // Sampling rate %<i>, sample time = %<s>, offset = %<o>
        if (eventFlags[%<i>]) {
          eventFlags[%<i>] = FALSE;

          if (sub_rate[%<i>].is_running) {
            rtmSetErrorStatus(%<modelName>_M, "Overrun");
            printf("Sub rate %<i> overrun, sample time=%<s>s, offset=%<o>s is too fast\n");
            break;
          }
          dispatch_semaphore_signal(sub_rate[%<i>].sem);
        }
          %endforeach

          // Execute base rate step 
          %if solverMode == "SingleTasking"
        %<modelName>_step();
          %else
        %<modelName>_step0();
          %endif

          %if extMode == 1
          rtExtModeCheckEndTrigger();
          %endif

          do {
            // increment for next time step
            next += period;
            now = mach_absolute_time();
            // #ifdef __cplusplus
            // fprintf(stderr, "Elapsed time %d us\n", static_cast<int>(now - next) / 1000);
            // #else
            // fprintf(stderr, "Elapsed time %d us\n", ((int)(now - next)) / 1000);
            // #endif
            if (now > next){
                    #ifdef __cplusplus
                    fprintf(stderr, "Base rate (%<fundamentalStepSize>s) overrun by %d us\n", static_cast<int>(now - next) / 1000);
                    #else
                    fprintf(stderr, "Base rate (%<fundamentalStepSize>s) overrun by %d us\n", ((int)(now - next)) / 1000);
                    #endif
                    
                    next = now;
                    continue;
                }
            } while (0);
            mach_wait_until(next);
        }

        simulationFinished = 1;
        // Final step for sub rate step functions
        for (i = %<1 + tid01Eq>; i < %<numSampleTimes>; i++) {
          dispatch_semaphore_signal(sub_rate[i].sem);
        }
        return 0;
    }

    //!
    //! This is the main function of the model.
    //! Multirate - Multitasking case main file
    //!
    int_T main(int_T argc, const char_T *argv[])
    {
        const char_T *errStatus;
        int_T i;

        %if extMode == 1
        // External mode
        signal(SIGINT, abortHandler); // important for letting the destructor be called.

        rtExtModeParseArgs(argc, argv, NULL);
        ext_mode_ready = dispatch_semaphore_create(1);

        %else
        (void)(argc);
        (void *)(argv);
        %endif

        // Initialize model
        %<modelName>_initialize();
        simulationFinished = 0;

        %foreach i = numSampleTimes
            %if i == 0 || i == 1 && tid01Eq
                %continue
            %endif
        // Initializing the step semaphore of the loop %<i> */
        sub_rate[%<i>].sem = dispatch_semaphore_create(1); // init with value of 1

        // Starting loop %<i> thread for sample time = %<s>s, offset = %<o>s.
        CHECK0(pthread_create(&sub_rate[%<i>].thread, NULL, sub_rate%<i>, (void*)%<i>));
        %endforeach

        // Starting the base rate thread
        CHECK0(pthread_create(&base_rate_thread, NULL, base_rate, NULL));

        %if extMode == 1
        // External mode
        dispatch_semaphore_wait(ext_mode_ready, DISPATCH_TIME_FOREVER);
        while(rtmGetErrorStatus(%<modelName>_M) == NULL && !rtmGetStopRequested(%<modelName>_M)) {
            rtExtModeOneStep(%<modelName>_M->extModeInfo, NUMST, (boolean_T *)&rtmGetStopRequested(RT_MDL));
            usleep(%<FEVAL("uint32", fundamentalStepSize * 1000000)>);
        }
        %endif
        if (rtmGetErrorStatus(%<modelName>_M) != NULL){
            fprintf(stderr,"ERROR %s\n", rtmGetErrorStatus(%<modelName>_M));
        }

        // Wait for threads to finish
        pthread_join(base_rate_thread, NULL); 
        %foreach i = numSampleTimes
          %if i == 0 || i == 1 && tid01Eq
        %continue
          %endif
        pthread_join(sub_rate[%<i>].thread, NULL);
        %endforeach

        %if extMode == 1
          rtExtModeShutdown(%<numSampleTimes>);
        %endif

        // Terminate model
        %<modelName>_terminate();

        // close semaphores
        %foreach i = numSampleTimes
          %if i == 0 || i == 1 && tid01Eq
        %continue
          %endif
        dispatch_release(sub_rate[%<i>].sem);
        %endforeach
        
        %if extMode == 1
        dispatch_release(ext_mode_ready);
        %endif

        errStatus = rtmGetErrorStatus(%<modelName>_M);
        if(errStatus != NULL && strcmp(errStatus, "Simulation finished")) {
          %%printf("%s\n", rtmGetErrorStatus(%<modelName>_M));
          if(!strcmp(errStatus, "Overrun")) {
            printf("ISR overrun - sampling rate too fast\n");
          }
          return(1);
        }
        return 0;
    }


    int move_pthread_to_realtime_scheduling_class(double Tcomputation, double Tconstraint)
    {
        mach_timebase_info_data_t timebase_info;
        mach_timebase_info(&timebase_info);

        const uint64_t NANOS_PER_MSEC = 1000000ULL;
        double clock2abs = ((double)timebase_info.denom / (double)timebase_info.numer) * NANOS_PER_MSEC;

        thread_time_constraint_policy_data_t policy;
        policy.period      = 0;
        policy.computation = (uint32_t)(Tcomputation * clock2abs); // 5 ms of work
        policy.constraint  = (uint32_t)(Tconstraint * clock2abs);
        policy.preemptible = FALSE;

        int kr = thread_policy_set(pthread_mach_thread_np(pthread_self()),
                                   THREAD_TIME_CONSTRAINT_POLICY,
                                   (thread_policy_t)&policy,
                                   THREAD_TIME_CONSTRAINT_POLICY_COUNT);

        if (kr != KERN_SUCCESS) {
            mach_error("thread_policy_set:", kr);
            fprintf(stderr, "failed to enter Mach real-time scheduling class\n");

            return -1;
        }


        fprintf(stderr, "Entered Mach real-time scheduling class with Tcomp = %3.0f ms and Tcons = %3.0f ms\n",
                Tcomputation, Tconstraint);

        return 0;
    }
// Local Variables:
// compile-command: "make -f %<modelName>.mk"
// End: 

%endfunction