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

    abort_code = 0;

    51

    result_size = 0;

    52

    53

    master = ecrt_request_master(%(master_number)d);

    54

    if (!master) {

    55

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

    56

        return -1;

    57

    }

    58

    59

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

    60

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

    61

        goto ecat_failed;

    62

    }

    63

    64

    // slaves PDO configuration

    65

%(slaves_configuration)s

    66

    67

    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {

    68

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

    69

        goto ecat_failed;

    70

    }

    71

    72

    ecrt_master_set_send_interval(master, common_ticktime__);

    73

    74

    // slaves initialization

    75

%(slaves_initialization)s

    76

    77

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

    78

%(slaves_output_pdos_default_values_extraction)s

    79

    80

    if (ecrt_master_activate(master)){

    81

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

    82

        goto ecat_failed;

    83

    }

    84

    85

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

    86

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

    87

        goto ecat_failed;

    88

    }

    89

    90

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

    91

    92

    first_sent = 0;

    93

    94

    return 0;

    95

    96

ecat_failed:

    97

    ecrt_release_master(master);

    98

    return -1;

    99

   100

}

   101

   102

void __cleanup_%(location)s(void)

   103

{

   104

    //release master

   105

    ecrt_release_master(master);

   106

    first_sent = 0;

   107

}

   108

   109

void __retrieve_%(location)s(void)

   110

{

   111

    // receive ethercat

   112

    if(first_sent){

   113

        ecrt_master_receive(master);

   114

        ecrt_domain_process(domain1);

   115

%(retrieve_variables)s

   116

    }

   117

   118

}

   119

   120

static RTIME _last_occur=0;

   121

static RTIME _last_publish=0;

   122

RTIME _current_lag=0;

   123

RTIME _max_jitter=0;

   124

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

   125

   126

void __publish_%(location)s(void)

   127

{

   128

%(publish_variables)s

   129

    ecrt_domain_queue(domain1);

   130

    {

   131

        RTIME current_time = rt_timer_read();

   132

        // Limit spining max 1/5 of common_ticktime

   133

        RTIME maxdeadline = current_time + (common_ticktime__ / 5);

   134

        RTIME deadline = _last_occur ? 

   135

            _last_occur + common_ticktime__ : 

   136

            current_time + _max_jitter; 

   137

        if(deadline > maxdeadline) deadline = maxdeadline;

   138

        _current_lag = deadline - current_time;

   139

        if(_last_publish != 0){

   140

            RTIME period = current_time - _last_publish;

   141

            if(period > common_ticktime__ )

   142

                _max_jitter = max(_max_jitter, period - common_ticktime__);

   143

            else

   144

                _max_jitter = max(_max_jitter, common_ticktime__ - period);

   145

        }

   146

        _last_publish = current_time;

   147

        _last_occur = current_time;

   148

        while(current_time < deadline) {

   149

            _last_occur = current_time; //Drift backward by default

   150

            current_time = rt_timer_read();

   151

        }

   152

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

   153

            //Consuming security margin ?

   154

            _last_occur = current_time; //Drift forward

   155

        }

   156

    }

   157

    ecrt_master_send(master);

   158

    first_sent = 1;

   159

}