19 *****************************************************************************/ |
19 *****************************************************************************/ |
20 |
20 |
21 // Linux |
21 // Linux |
22 #include <linux/module.h> |
22 #include <linux/module.h> |
23 #include <linux/ipipe.h> |
23 #include <linux/ipipe.h> |
|
24 #include <linux/slab.h> |
|
25 #include <linux/vmalloc.h> |
24 |
26 |
25 // RT_lib |
27 // RT_lib |
26 #include <msr_main.h> |
28 #include <msr_main.h> |
27 #include <msr_utils.h> |
29 #include <msr_utils.h> |
28 #include <msr_messages.h> |
30 #include <msr_messages.h> |
29 #include <msr_float.h> |
31 #include <msr_float.h> |
30 #include <msr_reg.h> |
32 #include <msr_reg.h> |
|
33 #include <msr_time.h> |
31 #include "msr_param.h" |
34 #include "msr_param.h" |
32 #include "msr_jitter.h" |
|
33 |
35 |
34 // EtherCAT |
36 // EtherCAT |
35 #include "../include/EtherCAT_rt.h" |
37 #include "../include/EtherCAT_rt.h" |
36 #include "../include/EtherCAT_si.h" |
38 #include "../include/EtherCAT_si.h" |
37 |
39 |
38 // Defines/Makros |
40 // Defines/Makros |
39 #define TSC2US(T1, T2) ((T2 - T1) * 1000UL / cpu_khz) |
|
40 #define HZREDUCTION (MSR_ABTASTFREQUENZ / HZ) |
41 #define HZREDUCTION (MSR_ABTASTFREQUENZ / HZ) |
41 |
42 |
42 /*****************************************************************************/ |
43 /*****************************************************************************/ |
43 /* Globale Variablen */ |
44 /* Globale Variablen */ |
44 |
|
45 // RT_lib |
|
46 extern struct timeval process_time; |
|
47 struct timeval msr_time_increment; // Increment per Interrupt |
|
48 |
45 |
49 // Adeos |
46 // Adeos |
50 static struct ipipe_domain this_domain; |
47 static struct ipipe_domain this_domain; |
51 static struct ipipe_sysinfo sys_info; |
48 static struct ipipe_sysinfo sys_info; |
52 |
49 |
53 // EtherCAT |
50 // EtherCAT |
54 ec_master_t *master = NULL; |
51 ec_master_t *master = NULL; |
55 ec_slave_t *s_in1, *s_out1, *s_ssi, *s_inc; |
52 ec_domain_t *domain1 = NULL; |
56 |
53 |
57 uint16_t angle0; |
54 // Prozessdaten |
58 |
55 uint8_t *dig_out1; |
59 ec_slave_init_t slaves[] = { |
56 uint16_t *ssi_value; |
60 {&s_in1, "1", "Beckhoff", "EL3102", 0}, |
57 uint16_t *inc_value; |
61 {&s_out1, "2", "Beckhoff", "EL2004", 0}, |
58 |
62 {&s_ssi, "3", "Beckhoff", "EL5001", 0}, |
59 uint32_t angle0; |
63 {&s_inc, "0:4", "Beckhoff", "EL5101", 0} |
60 |
|
61 ec_field_init_t domain1_fields[] = { |
|
62 {(void **) &dig_out1, "2", "Beckhoff", "EL2004", ec_opvalue, 0, 1}, |
|
63 {(void **) &ssi_value, "3", "Beckhoff", "EL5001", ec_ipvalue, 0, 1}, |
|
64 {(void **) &inc_value, "0:4", "Beckhoff", "EL5101", ec_ipvalue, 0, 1}, |
|
65 {} |
64 }; |
66 }; |
65 |
67 |
66 #define SLAVE_COUNT (sizeof(slaves) / sizeof(ec_slave_init_t)) |
68 /*****************************************************************************/ |
67 |
|
68 /****************************************************************************** |
|
69 * |
|
70 * Function: msr_controller_run() |
|
71 * |
|
72 *****************************************************************************/ |
|
73 |
69 |
74 static void msr_controller_run(void) |
70 static void msr_controller_run(void) |
75 { |
71 { |
76 static unsigned int counter = 0; |
|
77 |
|
78 msr_jitter_run(MSR_ABTASTFREQUENZ); |
|
79 |
|
80 EC_WRITE_EL20XX(s_out1, 3, EC_READ_EL31XX(s_in1, 0) < 0); |
|
81 |
|
82 if (!counter) { |
|
83 EtherCAT_rt_debug_level(master, 2); |
|
84 } |
|
85 |
|
86 // Prozessdaten lesen und schreiben |
72 // Prozessdaten lesen und schreiben |
87 EtherCAT_rt_domain_xio(master, 0, 40); |
73 EtherCAT_rt_domain_xio(domain1); |
88 |
74 |
89 if (counter) { |
75 angle0 = (uint32_t) *inc_value; |
90 counter--; |
76 } |
91 } |
77 |
92 else { |
78 /*****************************************************************************/ |
93 EtherCAT_rt_debug_level(master, 0); |
|
94 printk("SSI status=0x%X value=%u\n", |
|
95 EC_READ_EL5001_STATE(s_ssi), EC_READ_EL5001_VALUE(s_ssi)); |
|
96 printk("INC status=0x%X value=%u\n", |
|
97 EC_READ_EL5101_STATE(s_inc), EC_READ_EL5101_VALUE(s_inc)); |
|
98 |
|
99 counter = MSR_ABTASTFREQUENZ * 5; |
|
100 } |
|
101 |
|
102 angle0 = EC_READ_EL5101_VALUE(s_inc); |
|
103 } |
|
104 |
|
105 /****************************************************************************** |
|
106 * |
|
107 * Function: msr_run(_interrupt) |
|
108 * |
|
109 * Beschreibung: Routine wird zyklisch im Timerinterrupt ausgeführt |
|
110 * (hier muß alles rein, was Echtzeit ist ...) |
|
111 * |
|
112 * Parameter: Zeiger auf msr_data |
|
113 * |
|
114 * Rückgabe: |
|
115 * |
|
116 * Status: exp |
|
117 * |
|
118 *****************************************************************************/ |
|
119 |
79 |
120 void msr_run(unsigned irq) |
80 void msr_run(unsigned irq) |
121 { |
81 { |
122 static int counter = 0; |
82 static int counter = 0; |
123 |
83 |
124 timeval_add(&process_time, &process_time, &msr_time_increment); |
|
125 MSR_ADEOS_INTERRUPT_CODE(msr_controller_run(); msr_write_kanal_list();); |
84 MSR_ADEOS_INTERRUPT_CODE(msr_controller_run(); msr_write_kanal_list();); |
126 |
85 |
127 ipipe_control_irq(irq,0,IPIPE_ENABLE_MASK); //Interrupt bestŽätigen |
86 ipipe_control_irq(irq, 0, IPIPE_ENABLE_MASK); // Interrupt bestŽätigen |
128 if (counter++ > HZREDUCTION) { |
87 if (++counter >= HZREDUCTION) { |
129 ipipe_propagate_irq(irq); //und weiterreichen |
88 ipipe_propagate_irq(irq); // und weiterreichen |
130 counter = 0; |
89 counter = 0; |
131 } |
90 } |
132 } |
91 } |
133 |
92 |
134 /*****************************************************************************/ |
93 /*****************************************************************************/ |
142 &msr_run, NULL, IPIPE_HANDLE_MASK); |
101 &msr_run, NULL, IPIPE_HANDLE_MASK); |
143 |
102 |
144 ipipe_tune_timer(1000000000UL / MSR_ABTASTFREQUENZ, 0); |
103 ipipe_tune_timer(1000000000UL / MSR_ABTASTFREQUENZ, 0); |
145 } |
104 } |
146 |
105 |
147 /****************************************************************************** |
106 /*****************************************************************************/ |
148 * |
|
149 * Function: msr_register_channels |
|
150 * |
|
151 * Beschreibung: KanŽäle registrieren |
|
152 * |
|
153 * Parameter: |
|
154 * |
|
155 * RŽückgabe: |
|
156 * |
|
157 * Status: exp |
|
158 * |
|
159 *****************************************************************************/ |
|
160 |
107 |
161 int msr_globals_register(void) |
108 int msr_globals_register(void) |
162 { |
109 { |
163 //msr_reg_kanal("/value", "V", &value, TDBL); |
110 msr_reg_kanal("/angle0", "", &angle0, TUINT); |
164 //msr_reg_kanal("/dig1", "", &dig1, TINT); |
|
165 msr_reg_kanal("/angle0", "", &angle0, TINT); |
|
166 |
111 |
167 return 0; |
112 return 0; |
168 } |
113 } |
169 |
114 |
170 /****************************************************************************** |
115 /*****************************************************************************/ |
171 * the init/clean material |
|
172 *****************************************************************************/ |
|
173 |
116 |
174 int __init init_rt_module(void) |
117 int __init init_rt_module(void) |
175 { |
118 { |
176 struct ipipe_domain_attr attr; //ipipe |
119 struct ipipe_domain_attr attr; //ipipe |
|
120 const ec_field_init_t *field; |
177 |
121 |
178 // Als allererstes die RT-lib initialisieren |
122 // Als allererstes die RT-lib initialisieren |
179 if (msr_rtlib_init(1,MSR_ABTASTFREQUENZ,10,&msr_globals_register) < 0) { |
123 if (msr_rtlib_init(1, MSR_ABTASTFREQUENZ, 10, &msr_globals_register) < 0) { |
180 msr_print_warn("msr_modul: can't initialize rtlib!"); |
124 msr_print_warn("msr_modul: can't initialize rtlib!"); |
181 goto out_return; |
125 goto out_return; |
182 } |
126 } |
183 |
|
184 msr_jitter_init(); |
|
185 |
|
186 printk(KERN_INFO "=== Starting EtherCAT environment... ===\n"); |
|
187 |
127 |
188 if ((master = EtherCAT_rt_request_master(0)) == NULL) { |
128 if ((master = EtherCAT_rt_request_master(0)) == NULL) { |
189 printk(KERN_ERR "Error requesting master 0!\n"); |
129 printk(KERN_ERR "Error requesting master 0!\n"); |
190 goto out_msr_cleanup; |
130 goto out_msr_cleanup; |
191 } |
131 } |
192 |
132 |
193 if (EtherCAT_rt_register_slave_list(master, slaves, SLAVE_COUNT)) { |
133 EtherCAT_rt_master_print(master); |
194 printk(KERN_ERR "EtherCAT: Could not register slaves!\n"); |
134 |
195 goto out_release_master; |
135 printk(KERN_INFO "Registering domain...\n"); |
196 } |
136 |
197 |
137 if (!(domain1 = EtherCAT_rt_master_register_domain(master, ec_sync, 100))) |
198 if (EtherCAT_rt_activate_slaves(master) < 0) { |
138 { |
199 printk(KERN_ERR "EtherCAT: Could not activate slaves!\n"); |
139 printk(KERN_ERR "EtherCAT: Could not register domain!\n"); |
200 goto out_release_master; |
140 goto out_release_master; |
201 } |
141 } |
202 |
142 |
203 if (EtherCAT_rt_canopen_sdo_write(master, s_ssi, 0x4067, 0, 1, 2)) { |
143 printk(KERN_INFO "Registering domain fields...\n"); |
|
144 |
|
145 for (field = domain1_fields; field->data; field++) |
|
146 { |
|
147 if (!EtherCAT_rt_register_slave_field(domain1, |
|
148 field->address, |
|
149 field->vendor, |
|
150 field->product, |
|
151 field->data, |
|
152 field->field_type, |
|
153 field->field_index, |
|
154 field->field_count)) { |
|
155 printk(KERN_ERR "EtherCAT: Could not register field!\n"); |
|
156 goto out_release_master; |
|
157 } |
|
158 } |
|
159 |
|
160 printk(KERN_INFO "Activating master...\n"); |
|
161 |
|
162 if (EtherCAT_rt_master_activate(master)) { |
|
163 printk(KERN_ERR "EtherCAT: Could not activate master!\n"); |
|
164 goto out_release_master; |
|
165 } |
|
166 |
|
167 #if 0 |
|
168 if (EtherCAT_rt_canopen_sdo_write(master, "0:4", 0x4067, 0, 1, 2)) { |
204 printk(KERN_ERR "EtherCAT: Could not set SSI baud rate!\n"); |
169 printk(KERN_ERR "EtherCAT: Could not set SSI baud rate!\n"); |
205 goto out_release_master; |
170 goto out_release_master; |
206 } |
171 } |
207 |
172 |
208 if (EtherCAT_rt_canopen_sdo_write(master, s_ssi, 0x4061, 4, 1, 1)) { |
173 if (EtherCAT_rt_canopen_sdo_write(master, "0:4", 0x4061, 4, 1, 1)) { |
209 printk(KERN_ERR "EtherCAT: Could not set SSI feature bit!\n"); |
174 printk(KERN_ERR "EtherCAT: Could not set SSI feature bit!\n"); |
210 goto out_release_master; |
175 goto out_release_master; |
211 } |
176 } |
212 |
177 #endif |
213 do_gettimeofday(&process_time); |
|
214 msr_time_increment.tv_sec = 0; |
|
215 msr_time_increment.tv_usec = (unsigned int) (1000000 / MSR_ABTASTFREQUENZ); |
|
216 |
178 |
217 ipipe_init_attr(&attr); |
179 ipipe_init_attr(&attr); |
218 attr.name = "IPIPE-MSR-MODULE"; |
180 attr.name = "IPIPE-MSR-MODULE"; |
219 attr.priority = IPIPE_ROOT_PRIO + 1; |
181 attr.priority = IPIPE_ROOT_PRIO + 1; |
220 attr.entry = &domain_entry; |
182 attr.entry = &domain_entry; |