1

/*

     2

     3

Template C code used to produce target Ethercat C code

     4

     5

Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT

     6

     7

Distributed under the terms of the GNU Lesser General Public License as

     8

published by the Free Software Foundation; either version 2 of the License, or

     9

(at your option) any later version.

    10

    11

See COPYING file for copyrights details.

    12

    13

*/

    14

    15

#include <rtdm/rtdm.h>

    16

#include <native/task.h>

    17

#include <native/timer.h>

    18

    19

#include "ecrt.h"

    20

    21

#include "beremiz.h"

    22

#include "iec_types_all.h"

    23

    24

// declaration of interface variables

    25

%(located_variables_declaration)s

    26

    27

// process data

    28

uint8_t *domain1_pd = NULL;

    29

%(used_pdo_entry_offset_variables_declaration)s

    30

    31

const static ec_pdo_entry_reg_t domain1_regs[] = {

    32

%(used_pdo_entry_configuration)s

    33

    {}

    34

};

    35

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

    36

    37

%(pdos_configuration_declaration)s

    38

    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

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

    47

{\

    48

    char sbuf[256];\

    49

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

    50

    LogMessage(level, sbuf, slen);\

    51

}

    52

    53

/* Beremiz plugin functions */

    54

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

    55

{

    56

    uint32_t abort_code;

    57

    size_t result_size;

    58

    59

    abort_code = 0;

    60

    result_size = 0;

    61

    62

    master = ecrt_request_master(%(master_number)d);

    63

    if (!master) {

    64

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

    65

        return -1;

    66

    }

    67

    68

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

    69

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

    70

        goto ecat_failed;

    71

    }

    72

    73

    // slaves PDO configuration

    74

%(slaves_configuration)s

    75

    76

    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {

    77

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

    78

        goto ecat_failed;

    79

    }

    80

    81

    ecrt_master_set_send_interval(master, common_ticktime__);

    82

    83

    // slaves initialization

    84

%(slaves_initialization)s

    85

    86

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

    87

%(slaves_output_pdos_default_values_extraction)s

    88

    89

    if (ecrt_master_activate(master)){

    90

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

    91

        goto ecat_failed;

    92

    }

    93

    94

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

    95

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

    96

        goto ecat_failed;

    97

    }

    98

    99

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

   100

   101

    first_sent = 0;

   102

   103

    return 0;

   104

   105

ecat_failed:

   106

    ecrt_release_master(master);

   107

    return -1;

   108

   109

}

   110

   111

void __cleanup_%(location)s(void)

   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_master_receive(master);

   123

        ecrt_domain_process(domain1);

   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_domain_queue(domain1);

   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_master_send(master);

   167

    first_sent = 1;

   168

}