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

    12

#include "beremiz.h"

    13

#include "iec_types_all.h"

    14

    15

// declaration of interface variables

    16

%(located_variables_declaration)s

    17

    18

// process data

    19

uint8_t *domain1_pd = NULL;

    20

%(used_pdo_entry_offset_variables_declaration)s

    21

    22

const static ec_pdo_entry_reg_t domain1_regs[] = {

    23

%(used_pdo_entry_configuration)s

    24

    {}

    25

};

    26

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

    27

    28

%(pdos_configuration_declaration)s

    29

    30

long long wait_period_ns = 100000LL;

    31

    32

// EtherCAT

    33

static ec_master_t *master = NULL;

    34

static ec_domain_t *domain1 = NULL;

    35

static int first_sent=0;

    36

%(slaves_declaration)s

    37

#define SLOGF(level, format, args...)\

    38

{\

    39

    char sbuf[256];\

    40

    int slen = snprintf(sbuf , sizeof(sbuf) , format , ##args);\

    41

    LogMessage(level, sbuf, slen);\

    42

}

    43

    44

/* Beremiz plugin functions */

    45

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

    46

{

    47

    uint32_t abort_code;

    48

    size_t result_size;

    49

    50

    master = ecrt_request_master(%(master_number)d);

    51

    if (!master) {

    52

        SLOGF(LOG_CRITICAL, "EtherCAT master request failed!");

    53

        return -1;

    54

    }

    55

    56

    if(!(domain1 = ecrt_master_create_domain(master))){

    57

        SLOGF(LOG_CRITICAL, "EtherCAT Domain Creation failed!");

    58

        goto ecat_failed;

    59

    }

    60

    61

    // slaves PDO configuration

    62

%(slaves_configuration)s

    63

    64

    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {

    65

        SLOGF(LOG_CRITICAL, "EtherCAT PDO registration failed!");

    66

        goto ecat_failed;

    67

    }

    68

    69

    ecrt_master_set_send_interval(master, common_ticktime__);

    70

    71

    // slaves initialization

    72

%(slaves_initialization)s

    73

    74

    // extracting default value for not mapped entry in output PDOs

    75

%(slaves_output_pdos_default_values_extraction)s

    76

    77

    if (ecrt_master_activate(master)){

    78

        SLOGF(LOG_CRITICAL, "EtherCAT Master activation failed");

    79

        goto ecat_failed;

    80

    }

    81

    82

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

    83

        SLOGF(LOG_CRITICAL, "Failed to map EtherCAT process data");

    84

        goto ecat_failed;

    85

    }

    86

    87

    SLOGF(LOG_INFO, "Master %(master_number)d activated.");

    88

    89

    first_sent = 0;

    90

    91

    return 0;

    92

    93

ecat_failed:

    94

    ecrt_release_master(master);

    95

    return -1;

    96

    97

}

    98

    99

void __cleanup_%(location)s(void)

   100

{

   101

    //release master

   102

    ecrt_release_master(master);

   103

    first_sent = 0;

   104

}

   105

   106

void __retrieve_%(location)s(void)

   107

{

   108

    // receive ethercat

   109

    if(first_sent){

   110

        ecrt_master_receive(master);

   111

        ecrt_domain_process(domain1);

   112

%(retrieve_variables)s

   113

    }

   114

   115

}

   116

   117

static RTIME _last_occur=0;

   118

static RTIME _last_publish=0;

   119

RTIME _current_lag=0;

   120

RTIME _max_jitter=0;

   121

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

   122

   123

void __publish_%(location)s(void)

   124

{

   125

%(publish_variables)s

   126

    ecrt_domain_queue(domain1);

   127

    {

   128

        RTIME current_time = rt_timer_read();

   129

        // Limit spining max 1/5 of common_ticktime

   130

        RTIME maxdeadline = current_time + (common_ticktime__ / 5);

   131

        RTIME deadline = _last_occur ? 

   132

            _last_occur + common_ticktime__ : 

   133

            current_time + _max_jitter; 

   134

        if(deadline > maxdeadline) deadline = maxdeadline;

   135

        _current_lag = deadline - current_time;

   136

        if(_last_publish != 0){

   137

            RTIME period = current_time - _last_publish;

   138

            if(period > common_ticktime__ )

   139

                _max_jitter = max(_max_jitter, period - common_ticktime__);

   140

            else

   141

                _max_jitter = max(_max_jitter, common_ticktime__ - period);

   142

        }

   143

        _last_publish = current_time;

   144

        _last_occur = current_time;

   145

        while(current_time < deadline) {

   146

            _last_occur = current_time; //Drift backward by default

   147

            current_time = rt_timer_read();

   148

        }

   149

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

   150

            //Consuming security margin ?

   151

            _last_occur = current_time; //Drift forward

   152

        }

   153

    }

   154

    ecrt_master_send(master);

   155

    first_sent = 1;

   156

}