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

// Optional features

    25

#define CONFIGURE_PDOS  %(configure_pdos)d

    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

#if CONFIGURE_PDOS

    38

%(pdos_configuration_declaration)s

    39

#endif

    40

    41

int rt_fd = -1;

    42

CstructMstrAttach MstrAttach;

    43

char rt_dev_file[64];

    44

long long wait_period_ns = 100000LL;

    45

    46

// EtherCAT

    47

static ec_master_t *master = NULL;

    48

static ec_domain_t *domain1 = NULL;

    49

static int first_sent=0;

    50

%(slaves_declaration)s

    51

    52

/* Beremiz plugin functions */

    53

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

    54

{

    55

    uint32_t abort_code;

    56

    size_t result_size;

    57

    int i, rtstatus;

    58

    59

	MstrAttach.masterindex = %(master_number)d;

    60

    61

	master = ecrt_request_master(MstrAttach.masterindex);

    62

	if (!master) return -1;

    63

    64

	domain1 = ecrt_master_create_domain(master);

    65

	if (!domain1) return -1;

    66

    67

#if CONFIGURE_PDOS

    68

    fprintf(stdout, "Configure PDOs...\n");

    69

	%(slaves_configuration)s

    70

#endif

    71

    72

    if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {

    73

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

    74

        return -1;

    75

    }

    76

    77

	ecrt_master_set_send_interval(master, common_ticktime__);

    78

    79

%(slaves_initialization)s

    80

    81

%(slaves_output_pdos_default_values_extraction)s

    82

    83

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

    84

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

    85

    if (rt_fd < 0) {

    86

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

    87

        return -1;

    88

    }

    89

    90

    // attach the master over rtdm driver

    91

    MstrAttach.domainindex = ecrt_domain_index(domain1);

    92

    rtstatus = ecrt_rtdm_master_attach(rt_fd, &MstrAttach);

    93

    if (rtstatus < 0) {

    94

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

    95

        return -1;

    96

    }

    97

    98

    if (ecrt_master_activate(master))

    99

        return -1;

   100

   101

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

   102

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

   103

        return -1;

   104

    }

   105

   106

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

   107

   108

    first_sent = 0;

   109

   110

    return 0;

   111

}

   112

   113

void __cleanup_%(location)s(void)

   114

{

   115

	if (rt_fd >= 0) {

   116

		rt_dev_close(rt_fd);

   117

	}

   118

	//release master

   119

	ecrt_release_master(master);

   120

    first_sent = 0;

   121

}

   122

   123

void __retrieve_%(location)s(void)

   124

{

   125

//    // send process data

   126

//    ecrt_rtdm_domain_queque(rt_fd);

   127

//    ecrt_rtdm_master_send(rt_fd);

   128

//

   129

//    rt_task_sleep(rt_timer_ns2tsc(wait_period_ns));

   130

   131

    // receive ethercat

   132

    if(first_sent){

   133

        ecrt_rtdm_master_recieve(rt_fd);

   134

        ecrt_rtdm_domain_process(rt_fd);

   135

%(retrieve_variables)s

   136

    }

   137

   138

}

   139

   140

static RTIME _last_occur=0;

   141

RTIME _current_lag=0;

   142

   143

void __publish_%(location)s(void)

   144

{

   145

%(publish_variables)s

   146

    ecrt_rtdm_domain_queque(rt_fd);

   147

    {

   148

        RTIME _current_time = rt_timer_read();

   149

        RTIME deadline = _last_occur ?

   150

            _last_occur + common_ticktime__:

   151

            _current_time; 

   152

        _last_occur = _current_time;

   153

        _current_lag = deadline - _current_time;

   154

        while(_current_time < deadline) {

   155

            _last_occur = _current_time;

   156

            _current_time = rt_timer_read();

   157

        }

   158

    }

   159

    ecrt_rtdm_master_send(rt_fd);

   160

    first_sent = 1;

   161

}