1

/*

     2

 * Etherlab execution code

     3

 *

     4

 * */

     5

     6

#include <rtdm/rtdm.h>

     7

#include <native/task.h>

     8

#include <native/timer.h>

     9

    10

#include "ecrt.h"

    11

#include "ec_rtdm.h"

    12

    13

#ifdef _WINDOWS_H

    14

  #include "iec_types.h"

    15

#else

    16

  #include "iec_std_lib.h"

    17

#endif

    18

    19

extern unsigned long long common_ticktime__;

    20

    21

// declaration of interface variables

    22

%(located_variables_declaration)s

    23

    24

// process data

    25

uint8_t *domain1_pd = NULL;

    26

%(used_pdo_entry_offset_variables_declaration)s

    27

    28

const static ec_pdo_entry_reg_t domain1_regs[] = {

    29

%(used_pdo_entry_configuration)s

    30

    {}

    31

};

    32

/*****************************************************************************/

    33

    34

%(pdos_configuration_declaration)s

    35

    36

int rt_fd = -1;

    37

CstructMstrAttach MstrAttach;

    38

char rt_dev_file[64];

    39

long long wait_period_ns = 100000LL;

    40

    41

// EtherCAT

    42

static ec_master_t *master = NULL;

    43

static ec_domain_t *domain1 = NULL;

    44

static int first_sent=0;

    45

%(slaves_declaration)s

    46

    47

/* Beremiz plugin functions */

    48

int __init_%(location)s(int argc,char **argv)

    49

{

    50

    uint32_t abort_code;

    51

    size_t result_size;

    52

    int i, rtstatus;

    53

    54

	MstrAttach.masterindex = %(master_number)d;

    55

    56

	master = ecrt_request_master(MstrAttach.masterindex);

    57

	if (!master) return -1;

    58

    59

	domain1 = ecrt_master_create_domain(master);

    60

	if (!domain1) return -1;

    61

    64

    65

    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {

    66

        fprintf(stderr, "PDO entry registration failed!\n");

    67

        return -1;

    68

    }

    69

    70

	ecrt_master_set_send_interval(master, common_ticktime__);

    71

    73

%(slaves_initialization)s

    74

    76

%(slaves_output_pdos_default_values_extraction)s

    77

    78

    sprintf(&rt_dev_file[0],"%%s%%u",EC_RTDM_DEV_FILE_NAME,0);

    79

    rt_fd = rt_dev_open( &rt_dev_file[0], 0);

    80

    if (rt_fd < 0) {

    81

        fprintf(stderr, "Can't open %%s\n", &rt_dev_file[0]);

    82

        return -1;

    83

    }

    84

    85

    // attach the master over rtdm driver

    86

    MstrAttach.domainindex = ecrt_domain_index(domain1);

    87

    rtstatus = ecrt_rtdm_master_attach(rt_fd, &MstrAttach);

    88

    if (rtstatus < 0) {

    89

        fprintf(stderr, "Cannot attach to master over rtdm\n");

    90

        return -1;

    91

    }

    92

    93

    if (ecrt_master_activate(master))

    94

        return -1;

    95

    96

    if (!(domain1_pd = ecrt_domain_data(domain1))) {

    97

        fprintf(stderr, "domain1_pd:  0x%%.6lx\n", (unsigned long)domain1_pd);

    98

        return -1;

    99

    }

   100

   101

    fprintf(stdout, "Master %(master_number)d activated...\n");

   102

   103

    first_sent = 0;

   104

   105

    return 0;

   106

}

   107

   108

void __cleanup_%(location)s(void)

   109

{

   110

	if (rt_fd >= 0) {

   111

		rt_dev_close(rt_fd);

   112

	}

   113

	//release master

   114

	ecrt_release_master(master);

   115

    first_sent = 0;

   116

}

   117

   118

void __retrieve_%(location)s(void)

   119

{

   120

    // receive ethercat

   121

    if(first_sent){

   122

        ecrt_rtdm_master_recieve(rt_fd);

   123

        ecrt_rtdm_domain_process(rt_fd);

   124

%(retrieve_variables)s

   125

    }

   126

   127

}

   128

   129

static RTIME _last_occur=0;

   130

static RTIME _last_publish=0;

   131

RTIME _current_lag=0;

   132

RTIME _max_jitter=0;

   133

static inline RTIME max(RTIME a,RTIME b){return a>b?a:b;}

   134

   135

void __publish_%(location)s(void)

   136

{

   137

%(publish_variables)s

   138

    ecrt_rtdm_domain_queque(rt_fd);

   139

    {

   140

        RTIME current_time = rt_timer_read();

   141

        // Limit spining max 1/5 of common_ticktime

   142

        RTIME maxdeadline = current_time + (common_ticktime__ / 5);

   143

        RTIME deadline = _last_occur ? 

   144

            _last_occur + common_ticktime__ : 

   145

            current_time + _max_jitter; 

   146

        if(deadline > maxdeadline) deadline = maxdeadline;

   147

        _current_lag = deadline - current_time;

   148

        if(_last_publish != 0){

   149

            RTIME period = current_time - _last_publish;

   150

            if(period > common_ticktime__ )

   151

                _max_jitter = max(_max_jitter, period - common_ticktime__);

   152

            else

   153

                _max_jitter = max(_max_jitter, common_ticktime__ - period);

   154

        }

   155

        _last_publish = current_time;

   156

        _last_occur = current_time;

   157

        while(current_time < deadline) {

   158

            _last_occur = current_time; //Drift backward by default

   159

            current_time = rt_timer_read();

   160

        }

   161

        if( _max_jitter * 10 < common_ticktime__ && _current_lag < _max_jitter){

   162

            //Consuming security margin ?

   163

            _last_occur = current_time; //Drift forward

   164

        }

   165

    }

   166

    ecrt_rtdm_master_send(rt_fd);

   167

    first_sent = 1;

   168

}