50 static struct ipipe_sysinfo sys_info; |
51 static struct ipipe_sysinfo sys_info; |
51 |
52 |
52 // EtherCAT |
53 // EtherCAT |
53 ec_master_t *master = NULL; |
54 ec_master_t *master = NULL; |
54 ec_domain_t *domain1 = NULL; |
55 ec_domain_t *domain1 = NULL; |
55 ec_domain_t *domain2 = NULL; |
|
56 |
56 |
57 // Prozessdaten |
57 // Prozessdaten |
|
58 #ifdef BLOCK1 |
58 void *r_ssi, *r_ssi_st; |
59 void *r_ssi, *r_ssi_st; |
59 void *r_ssi2; |
60 #else |
60 void *r_inc; |
61 void *r_inc; |
61 |
62 #endif |
62 uint32_t k_angle; |
63 |
63 uint32_t k_ssi_pos; |
64 uint32_t k_ssi_pos; |
64 uint32_t k_ssi_status; |
65 uint32_t k_ssi_status; |
|
66 uint32_t k_angle; |
65 uint32_t k_preio; |
67 uint32_t k_preio; |
66 uint32_t k_postio; |
68 uint32_t k_postio; |
67 uint32_t k_finished; |
69 uint32_t k_finished; |
68 |
70 |
|
71 #ifdef BLOCK1 |
69 ec_field_init_t domain1_fields[] = { |
72 ec_field_init_t domain1_fields[] = { |
70 {&r_ssi, "1", "Beckhoff", "EL5001", "InputValue", 0}, |
73 {&r_ssi, "1", "Beckhoff", "EL5001", "InputValue", 0}, |
71 {&r_ssi_st, "1", "Beckhoff", "EL5001", "Status", 0}, |
74 {&r_ssi_st, "1", "Beckhoff", "EL5001", "Status", 0}, |
72 {} |
75 {} |
73 }; |
76 }; |
74 |
77 #else |
75 ec_field_init_t domain2_fields[] = { |
78 ec_field_init_t domain1_fields[] = { |
76 {&r_ssi2, "1", "Beckhoff", "EL5001", "InputValue", 0}, |
79 {&r_inc, "4", "Beckhoff", "EL5101", "InputValue", 0}, |
77 {} |
80 {} |
78 }; |
81 }; |
|
82 #endif |
79 |
83 |
80 /*****************************************************************************/ |
84 /*****************************************************************************/ |
81 |
85 |
82 static void msr_controller_run(void) |
86 static void msr_controller_run(void) |
83 { |
87 { |
96 |
100 |
97 #ifdef ASYNC |
101 #ifdef ASYNC |
98 // Empfangen |
102 // Empfangen |
99 ecrt_master_async_receive(master); |
103 ecrt_master_async_receive(master); |
100 ecrt_domain_process(domain1); |
104 ecrt_domain_process(domain1); |
101 ecrt_domain_process(domain2); |
|
102 |
105 |
103 // Prozessdaten verarbeiten |
106 // Prozessdaten verarbeiten |
|
107 #ifdef BLOCK1 |
104 k_ssi_pos = EC_READ_U32(r_ssi); |
108 k_ssi_pos = EC_READ_U32(r_ssi); |
105 k_ssi_status = EC_READ_U32(r_ssi_st); |
109 k_ssi_status = EC_READ_U32(r_ssi_st); |
|
110 #else |
|
111 k_angle = EC_READ_U16(r_inc); |
|
112 #endif |
106 |
113 |
107 // Senden |
114 // Senden |
108 ecrt_domain_queue(domain1); |
115 ecrt_domain_queue(domain1); |
109 ecrt_domain_queue(domain2); |
|
110 ecrt_master_async_send(master); |
116 ecrt_master_async_send(master); |
111 #else |
117 #else |
112 // Senden und empfangen |
118 // Senden und empfangen |
113 ecrt_domain_queue(domain1); |
119 ecrt_domain_queue(domain1); |
114 ecrt_domain_queue(domain2); |
|
115 ecrt_master_sync_io(master); |
120 ecrt_master_sync_io(master); |
116 ecrt_domain_process(domain1); |
121 ecrt_domain_process(domain1); |
117 ecrt_domain_process(domain2); |
|
118 |
122 |
119 // Prozessdaten verarbeiten |
123 // Prozessdaten verarbeiten |
|
124 #ifdef BLOCK1 |
120 k_ssi_pos = EC_READ_U32(r_ssi); |
125 k_ssi_pos = EC_READ_U32(r_ssi); |
121 k_ssi_status = EC_READ_U32(r_ssi_st); |
126 k_ssi_status = EC_READ_U32(r_ssi_st); |
122 #endif |
127 #else |
|
128 k_angle = EC_READ_U16(r_inc); |
|
129 #endif |
|
130 |
|
131 #endif // ASYNC |
123 |
132 |
124 k_postio = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; |
133 k_postio = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; |
125 |
134 |
126 //ecrt_master_debug(master, 0); |
135 //ecrt_master_debug(master, 0); |
127 k_finished = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; |
136 k_finished = (uint32_t) (get_cycles() - offset) * 1e6 / cpu_khz; |
193 if (!(domain1 = ecrt_master_create_domain(master))) { |
202 if (!(domain1 = ecrt_master_create_domain(master))) { |
194 printk(KERN_ERR "Could not register domain!\n"); |
203 printk(KERN_ERR "Could not register domain!\n"); |
195 goto out_release_master; |
204 goto out_release_master; |
196 } |
205 } |
197 |
206 |
198 if (!(domain2 = ecrt_master_create_domain(master))) { |
|
199 printk(KERN_ERR "Could not register domain!\n"); |
|
200 goto out_release_master; |
|
201 } |
|
202 |
|
203 printk(KERN_INFO "Registering domain fields...\n"); |
207 printk(KERN_INFO "Registering domain fields...\n"); |
204 |
208 |
205 if (ecrt_domain_register_field_list(domain1, domain1_fields)) { |
209 if (ecrt_domain_register_field_list(domain1, domain1_fields)) { |
206 printk(KERN_ERR "Failed to register domain fields.\n"); |
210 printk(KERN_ERR "Failed to register domain fields.\n"); |
207 goto out_release_master; |
211 goto out_release_master; |
208 } |
212 } |
209 |
213 |
210 if (ecrt_domain_register_field_list(domain2, domain2_fields)) { |
|
211 printk(KERN_ERR "Failed to register domain fields.\n"); |
|
212 goto out_release_master; |
|
213 } |
|
214 |
|
215 printk(KERN_INFO "Activating master...\n"); |
214 printk(KERN_INFO "Activating master...\n"); |
216 |
215 |
217 if (ecrt_master_activate(master)) { |
216 if (ecrt_master_activate(master)) { |
218 printk(KERN_ERR "Could not activate master!\n"); |
217 printk(KERN_ERR "Could not activate master!\n"); |
219 goto out_release_master; |
218 goto out_release_master; |
220 } |
219 } |
|
220 |
|
221 ecrt_master_print(master); |
221 |
222 |
222 //ecrt_master_debug(master, 2); |
223 //ecrt_master_debug(master, 2); |
223 if (ecrt_master_fetch_sdo_lists(master)) { |
224 if (ecrt_master_fetch_sdo_lists(master)) { |
224 printk(KERN_ERR "Failed to fetch SDO lists!\n"); |
225 printk(KERN_ERR "Failed to fetch SDO lists!\n"); |
225 goto out_deactivate; |
226 goto out_deactivate; |
226 } |
227 } |
227 //ecrt_master_debug(master, 0); |
228 //ecrt_master_debug(master, 0); |
228 |
229 |
229 ecrt_master_print(master); |
230 ecrt_master_print(master); |
230 |
231 |
231 #if 1 |
232 #ifdef BLOCK1 |
232 if (ecrt_master_sdo_read(master, "1", 0x100A, 1, &version)) { |
233 if (ecrt_master_sdo_read(master, "1", 0x100A, 1, &version)) { |
233 printk(KERN_ERR "Could not read SSI version!\n"); |
234 printk(KERN_ERR "Could not read SSI version!\n"); |
234 goto out_deactivate; |
235 goto out_deactivate; |
235 } |
236 } |
236 printk(KERN_INFO "Software-version: %u\n", version); |
237 printk(KERN_INFO "Software-version: %u\n", version); |