57 void *r_ssi2; |
59 void *r_ssi2; |
58 void *r_inc; |
60 void *r_inc; |
59 |
61 |
60 uint32_t k_angle; |
62 uint32_t k_angle; |
61 uint32_t k_pos; |
63 uint32_t k_pos; |
|
64 uint32_t k_preio; |
|
65 uint32_t k_postio; |
|
66 uint32_t k_finished; |
62 |
67 |
63 ec_field_init_t domain1_fields[] = { |
68 ec_field_init_t domain1_fields[] = { |
64 {&r_ssi, "1", "Beckhoff", "EL5001", ec_ipvalue, 0, 1}, |
69 {&r_ssi, "1", "Beckhoff", "EL5001", "InputValue", 0, 1}, |
65 {} |
70 {} |
66 }; |
71 }; |
67 |
72 |
68 ec_field_init_t domain2_fields[] = { |
73 ec_field_init_t domain2_fields[] = { |
69 {&r_ssi2, "1", "Beckhoff", "EL5001", ec_ipvalue, 0, 1}, |
74 {&r_ssi2, "1", "Beckhoff", "EL5001", "InputValue", 0, 1}, |
70 {} |
75 {} |
71 }; |
76 }; |
72 |
77 |
73 /*****************************************************************************/ |
78 /*****************************************************************************/ |
74 |
79 |
75 static void msr_controller_run(void) |
80 static void msr_controller_run(void) |
76 { |
81 { |
|
82 cycles_t offset; |
77 static unsigned int counter = 0; |
83 static unsigned int counter = 0; |
|
84 |
|
85 offset = get_cycles(); |
78 |
86 |
79 if (counter) counter--; |
87 if (counter) counter--; |
80 else { |
88 else { |
81 //EtherCAT_rt_master_debug(master, 2); |
89 //EtherCAT_rt_master_debug(master, 2); |
82 counter = MSR_ABTASTFREQUENZ; |
90 counter = MSR_ABTASTFREQUENZ; |
83 } |
91 } |
84 |
92 |
85 // Prozessdaten lesen und schreiben |
93 k_preio = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; |
86 EtherCAT_rt_domain_queue(domain1); |
94 |
87 EtherCAT_rt_domain_queue(domain2); |
95 #ifdef ASYNC |
88 |
96 // Empfangen |
89 EtherCAT_rt_master_xio(master); |
97 ecrt_master_async_receive(master); |
90 |
98 ecrt_domain_process(domain1); |
91 EtherCAT_rt_domain_process(domain1); |
99 ecrt_domain_process(domain2); |
92 EtherCAT_rt_domain_process(domain2); |
100 |
93 |
101 // Prozessdaten verarbeiten |
94 //k_angle = EC_READ_U16(r_inc); |
|
95 k_pos = EC_READ_U32(r_ssi); |
102 k_pos = EC_READ_U32(r_ssi); |
96 |
103 |
97 //EtherCAT_rt_master_debug(master, 0); |
104 // Senden |
|
105 ecrt_domain_queue(domain1); |
|
106 ecrt_domain_queue(domain2); |
|
107 ecrt_master_async_send(master); |
|
108 #else |
|
109 // Senden und empfangen |
|
110 ecrt_domain_queue(domain1); |
|
111 ecrt_domain_queue(domain2); |
|
112 ecrt_master_sync_io(master); |
|
113 ecrt_domain_process(domain1); |
|
114 ecrt_domain_process(domain2); |
|
115 |
|
116 // Prozessdaten verarbeiten |
|
117 k_pos = EC_READ_U32(r_ssi); |
|
118 #endif |
|
119 |
|
120 k_postio = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; |
|
121 |
|
122 //ecrt_master_debug(master, 0); |
|
123 k_finished = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; |
98 } |
124 } |
99 |
125 |
100 /*****************************************************************************/ |
126 /*****************************************************************************/ |
101 |
127 |
102 int msr_globals_register(void) |
128 int msr_globals_register(void) |
103 { |
129 { |
104 msr_reg_kanal("/angle0", "", &k_angle, TUINT); |
130 msr_reg_kanal("/angle0", "", &k_angle, TUINT); |
105 msr_reg_kanal("/pos0", "", &k_pos, TUINT); |
131 msr_reg_kanal("/pos0", "", &k_pos, TUINT); |
|
132 |
|
133 msr_reg_kanal("/Timing/Pre-IO", "ns", &k_preio, TUINT); |
|
134 msr_reg_kanal("/Timing/Post-IO", "ns", &k_postio, TUINT); |
|
135 msr_reg_kanal("/Timing/Finished", "ns", &k_finished, TUINT); |
106 |
136 |
107 return 0; |
137 return 0; |
108 } |
138 } |
109 |
139 |
110 /*****************************************************************************/ |
140 /*****************************************************************************/ |
146 if (msr_rtlib_init(1, MSR_ABTASTFREQUENZ, 10, &msr_globals_register) < 0) { |
176 if (msr_rtlib_init(1, MSR_ABTASTFREQUENZ, 10, &msr_globals_register) < 0) { |
147 msr_print_warn("msr_modul: can't initialize rtlib!"); |
177 msr_print_warn("msr_modul: can't initialize rtlib!"); |
148 goto out_return; |
178 goto out_return; |
149 } |
179 } |
150 |
180 |
151 if ((master = EtherCAT_rt_request_master(0)) == NULL) { |
181 if ((master = ecrt_request_master(0)) == NULL) { |
152 printk(KERN_ERR "Error requesting master 0!\n"); |
182 printk(KERN_ERR "Error requesting master 0!\n"); |
153 goto out_msr_cleanup; |
183 goto out_msr_cleanup; |
154 } |
184 } |
155 |
185 |
156 //EtherCAT_rt_master_print(master); |
186 //ecrt_master_print(master); |
157 |
187 |
158 printk(KERN_INFO "Registering domains...\n"); |
188 printk(KERN_INFO "Registering domains...\n"); |
159 |
189 |
160 if (!(domain1 = EtherCAT_rt_master_register_domain(master, ec_sync, 100))) |
190 if (!(domain1 = ecrt_master_create_domain(master))) { |
161 { |
|
162 printk(KERN_ERR "EtherCAT: Could not register domain!\n"); |
191 printk(KERN_ERR "EtherCAT: Could not register domain!\n"); |
163 goto out_release_master; |
192 goto out_release_master; |
164 } |
193 } |
165 |
194 |
166 if (!(domain2 = EtherCAT_rt_master_register_domain(master, ec_sync, 100))) |
195 if (!(domain2 = ecrt_master_create_domain(master))) { |
167 { |
|
168 printk(KERN_ERR "EtherCAT: Could not register domain!\n"); |
196 printk(KERN_ERR "EtherCAT: Could not register domain!\n"); |
169 goto out_release_master; |
197 goto out_release_master; |
170 } |
198 } |
171 |
199 |
172 printk(KERN_INFO "Registering domain fields...\n"); |
200 printk(KERN_INFO "Registering domain fields...\n"); |
173 |
201 |
174 if (EtherCAT_rt_register_domain_fields(domain1, domain1_fields)) { |
202 if (ecrt_domain_register_field_list(domain1, domain1_fields)) { |
175 printk(KERN_ERR "Failed to register domain fields.\n"); |
203 printk(KERN_ERR "Failed to register domain fields.\n"); |
176 goto out_release_master; |
204 goto out_release_master; |
177 } |
205 } |
178 |
206 |
179 if (EtherCAT_rt_register_domain_fields(domain2, domain2_fields)) { |
207 if (ecrt_domain_register_field_list(domain2, domain2_fields)) { |
180 printk(KERN_ERR "Failed to register domain fields.\n"); |
208 printk(KERN_ERR "Failed to register domain fields.\n"); |
181 goto out_release_master; |
209 goto out_release_master; |
182 } |
210 } |
183 |
211 |
184 printk(KERN_INFO "Activating master...\n"); |
212 printk(KERN_INFO "Activating master...\n"); |
185 |
213 |
186 //EtherCAT_rt_master_debug(master, 2); |
214 //ecrt_master_debug(master, 2); |
187 |
215 |
188 if (EtherCAT_rt_master_activate(master)) { |
216 if (ecrt_master_activate(master)) { |
189 printk(KERN_ERR "Could not activate master!\n"); |
217 printk(KERN_ERR "Could not activate master!\n"); |
190 goto out_release_master; |
218 goto out_release_master; |
191 } |
219 } |
192 |
220 |
193 //EtherCAT_rt_master_debug(master, 0); |
221 //ecrt_master_debug(master, 0); |
194 |
222 |
195 #if 1 |
223 #if 1 |
196 if (EtherCAT_rt_canopen_sdo_addr_read(master, "6", 0x100A, 1, |
224 if (ecrt_master_sdo_read(master, "6", 0x100A, 1, &version)) { |
197 &version)) { |
|
198 printk(KERN_ERR "Could not read SSI version!\n"); |
225 printk(KERN_ERR "Could not read SSI version!\n"); |
199 goto out_release_master; |
226 goto out_release_master; |
200 } |
227 } |
201 printk(KERN_INFO "Software-version: %u\n", version); |
228 printk(KERN_INFO "Software-version: %u\n", version); |
|
229 #endif |
|
230 |
|
231 #ifdef ASYNC |
|
232 ecrt_domain_queue(domain1); |
|
233 ecrt_domain_queue(domain2); |
|
234 ecrt_master_async_send(master); |
|
235 udelay(100); |
202 #endif |
236 #endif |
203 |
237 |
204 ipipe_init_attr(&attr); |
238 ipipe_init_attr(&attr); |
205 attr.name = "IPIPE-MSR-MODULE"; |
239 attr.name = "IPIPE-MSR-MODULE"; |
206 attr.priority = IPIPE_ROOT_PRIO + 1; |
240 attr.priority = IPIPE_ROOT_PRIO + 1; |