author | laurent |
Mon, 19 Mar 2012 00:45:19 +0100 | |
changeset 2042 | 563ccc918ded |
parent 2039 | 3a218f6bd805 |
child 2075 | 21eb91dc7071 |
permissions | -rw-r--r-- |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
1 |
/* |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
2 |
* Etherlab execution code |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
3 |
* |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
4 |
* */ |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
5 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
6 |
#include <rtdm/rtdm.h> |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
7 |
#include <native/task.h> |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
8 |
#include <native/timer.h> |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
9 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
10 |
#include "ecrt.h" |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
11 |
#include "ec_rtdm.h" |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
12 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
13 |
#ifdef _WINDOWS_H |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
14 |
#include "iec_types.h" |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
15 |
#else |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
16 |
#include "iec_std_lib.h" |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
17 |
#endif |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
18 |
|
2036
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
19 |
extern unsigned long long common_ticktime__; |
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
20 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
21 |
// declaration of interface variables |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
22 |
%(located_variables_declaration)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
23 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
24 |
// Optional features |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
25 |
#define CONFIGURE_PDOS %(configure_pdos)d |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
26 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
27 |
// process data |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
28 |
uint8_t *domain1_pd = NULL; |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
29 |
%(used_pdo_entry_offset_variables_declaration)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
30 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
31 |
const static ec_pdo_entry_reg_t domain1_regs[] = { |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
32 |
%(used_pdo_entry_configuration)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
33 |
{} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
34 |
}; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
35 |
/*****************************************************************************/ |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
36 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
37 |
#if CONFIGURE_PDOS |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
38 |
%(pdos_configuration_declaration)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
39 |
#endif |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
40 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
41 |
int rt_fd = -1; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
42 |
CstructMstrAttach MstrAttach; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
43 |
char rt_dev_file[64]; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
44 |
long long wait_period_ns = 100000LL; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
45 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
46 |
// EtherCAT |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
47 |
static ec_master_t *master = NULL; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
48 |
static ec_domain_t *domain1 = NULL; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
49 |
%(slaves_declaration)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
50 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
51 |
/* Beremiz plugin functions */ |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
52 |
int __init_%(location)s(int argc,char **argv) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
53 |
{ |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
54 |
uint32_t abort_code; |
2039
3a218f6bd805
Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents:
2036
diff
changeset
|
55 |
size_t result_size; |
3a218f6bd805
Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents:
2036
diff
changeset
|
56 |
int i, rtstatus; |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
57 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
58 |
MstrAttach.masterindex = %(master_number)d; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
59 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
60 |
master = ecrt_request_master(MstrAttach.masterindex); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
61 |
if (!master) return -1; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
62 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
63 |
domain1 = ecrt_master_create_domain(master); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
64 |
if (!domain1) return -1; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
65 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
66 |
#if CONFIGURE_PDOS |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
67 |
fprintf(stdout, "Configure PDOs...\n"); |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
68 |
%(slaves_configuration)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
69 |
#endif |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
70 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
71 |
if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
72 |
fprintf(stderr, "PDO entry registration failed!\n"); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
73 |
return -1; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
74 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
75 |
|
2036
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
76 |
ecrt_master_set_send_interval(master, common_ticktime__); |
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
77 |
|
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
78 |
%(slaves_initialization)s |
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
79 |
|
2039
3a218f6bd805
Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents:
2036
diff
changeset
|
80 |
%(slaves_output_pdos_default_values_extraction)s |
3a218f6bd805
Adding support for uploading default value for unused entries in output pdos from slaves before starting master
laurent
parents:
2036
diff
changeset
|
81 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
82 |
sprintf(&rt_dev_file[0],"%%s%%u",EC_RTDM_DEV_FILE_NAME,0); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
83 |
rt_fd = rt_dev_open( &rt_dev_file[0], 0); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
84 |
if (rt_fd < 0) { |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
85 |
fprintf(stderr, "Can't open %%s\n", &rt_dev_file[0]); |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
86 |
return -1; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
87 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
88 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
89 |
// attach the master over rtdm driver |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
90 |
MstrAttach.domainindex = ecrt_domain_index(domain1); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
91 |
rtstatus = ecrt_rtdm_master_attach(rt_fd, &MstrAttach); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
92 |
if (rtstatus < 0) { |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
93 |
fprintf(stderr, "Cannot attach to master over rtdm\n"); |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
94 |
return -1; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
95 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
96 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
97 |
if (ecrt_master_activate(master)) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
98 |
return -1; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
99 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
100 |
if (!(domain1_pd = ecrt_domain_data(domain1))) { |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
101 |
fprintf(stderr, "domain1_pd: 0x%%.6lx\n", (unsigned long)domain1_pd); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
102 |
return -1; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
103 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
104 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
105 |
fprintf(stdout, "Master %(master_number)d activated...\n"); |
2032
766078d83e22
Adding support for DS402 node subplugin (only simple state transition implemented)
laurent
parents:
2028
diff
changeset
|
106 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
107 |
return 0; |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
108 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
109 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
110 |
void __cleanup_%(location)s(void) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
111 |
{ |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
112 |
if (rt_fd >= 0) { |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
113 |
rt_dev_close(rt_fd); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
114 |
} |
2024
08d67a0af2c1
Master release code added
Donggu Kang <dgkang@higenmotor.com>
parents:
2022
diff
changeset
|
115 |
//release master |
2025
87f428326f9b
Master relase code added
Donggu Kang <dgkang@higenmotor.com>
parents:
2022
diff
changeset
|
116 |
ecrt_release_master(master); |
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
117 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
118 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
119 |
void __retrieve_%(location)s(void) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
120 |
{ |
2036
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
121 |
// send process data |
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
122 |
ecrt_rtdm_domain_queque(rt_fd); |
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
123 |
ecrt_rtdm_master_send(rt_fd); |
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
124 |
|
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
125 |
rt_task_sleep(rt_timer_ns2tsc(wait_period_ns)); |
7c31fab22c74
Adding support for setting TargetPosition only if PositionSetPoint value from MCL is valid
laurent
parents:
2032
diff
changeset
|
126 |
|
2022
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
127 |
// receive ethercat |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
128 |
ecrt_rtdm_master_recieve(rt_fd); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
129 |
ecrt_rtdm_domain_process(rt_fd); |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
130 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
131 |
%(retrieve_variables)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
132 |
} |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
133 |
|
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
134 |
void __publish_%(location)s(void) |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
135 |
{ |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
136 |
%(publish_variables)s |
c2295d311402
First working implementation of Beremiz plugin for etherlab
laurent
parents:
diff
changeset
|
137 |
} |