45 #include "../../include/ecdb.h" |
45 #include "../../include/ecdb.h" |
46 |
46 |
47 /*****************************************************************************/ |
47 /*****************************************************************************/ |
48 |
48 |
49 // RTAI task frequency in Hz |
49 // RTAI task frequency in Hz |
50 #define FREQUENCY 4000 |
50 #define FREQUENCY 2000 |
51 #define INHIBIT_TIME 20 |
51 #define INHIBIT_TIME 20 |
52 |
52 |
53 #define TIMERTICKS (1000000000 / FREQUENCY) |
53 #define TIMERTICKS (1000000000 / FREQUENCY) |
54 |
54 |
|
55 #define PFX "ec_rtai_sample: " |
|
56 |
55 /*****************************************************************************/ |
57 /*****************************************************************************/ |
56 |
58 |
57 // RTAI |
59 // RTAI |
58 RT_TASK task; |
60 static RT_TASK task; |
59 SEM master_sem; |
61 static SEM master_sem; |
60 cycles_t t_last_cycle = 0, t_critical; |
62 static cycles_t t_last_cycle = 0, t_critical; |
61 |
63 |
62 // EtherCAT |
64 // EtherCAT |
63 ec_master_t *master = NULL; |
65 static ec_master_t *master = NULL; |
64 ec_domain_t *domain1 = NULL; |
66 static ec_domain_t *domain1 = NULL; |
|
67 static ec_master_status_t master_status, old_status = {}; |
65 |
68 |
66 // data fields |
69 // data fields |
67 void *r_dig_out; |
70 static void *r_dig_out; |
68 |
71 static void *r_ana_out; |
69 ec_pdo_reg_t domain1_pdos[] = { |
72 static void *r_count; |
70 {"2", Beckhoff_EL2004_Outputs, &r_dig_out}, |
73 //static void *r_freq; |
|
74 |
|
75 const static ec_pdo_reg_t domain1_pdo_regs[] = { |
|
76 {"2", Beckhoff_EL2004_Outputs, &r_dig_out}, |
|
77 {"3", Beckhoff_EL4132_Output1, &r_ana_out}, |
|
78 {"#888:1", Beckhoff_EL5101_Value, &r_count}, |
|
79 //{"4", Beckhoff_EL5101_Frequency, &r_freq}, |
71 {} |
80 {} |
72 }; |
81 }; |
73 |
82 |
74 /*****************************************************************************/ |
83 /*****************************************************************************/ |
75 |
84 |
89 // process data |
98 // process data |
90 EC_WRITE_U8(r_dig_out, blink ? 0x0F : 0x00); |
99 EC_WRITE_U8(r_dig_out, blink ? 0x0F : 0x00); |
91 |
100 |
92 rt_sem_wait(&master_sem); |
101 rt_sem_wait(&master_sem); |
93 ecrt_domain_queue(domain1); |
102 ecrt_domain_queue(domain1); |
94 ecrt_master_run(master); |
|
95 ecrt_master_send(master); |
103 ecrt_master_send(master); |
96 rt_sem_signal(&master_sem); |
104 rt_sem_signal(&master_sem); |
97 |
105 |
98 if (counter) { |
106 if (counter) { |
99 counter--; |
107 counter--; |
100 } |
108 } |
101 else { |
109 else { |
102 counter = FREQUENCY; |
110 counter = FREQUENCY; |
103 blink = !blink; |
111 blink = !blink; |
|
112 |
|
113 rt_sem_wait(&master_sem); |
|
114 ecrt_master_get_status(master, &master_status); |
|
115 rt_sem_signal(&master_sem); |
|
116 |
|
117 if (master_status.bus_status != old_status.bus_status) { |
|
118 printk(KERN_INFO PFX "bus status changed to %i.\n", |
|
119 master_status.bus_status); |
|
120 } |
|
121 if (master_status.bus_tainted != old_status.bus_tainted) { |
|
122 printk(KERN_INFO PFX "tainted flag changed to %u.\n", |
|
123 master_status.bus_tainted); |
|
124 } |
|
125 if (master_status.slaves_responding != |
|
126 old_status.slaves_responding) { |
|
127 printk(KERN_INFO PFX "slaves_responding changed to %u.\n", |
|
128 master_status.slaves_responding); |
|
129 } |
|
130 |
|
131 old_status = master_status; |
104 } |
132 } |
105 |
133 |
106 rt_task_wait_period(); |
134 rt_task_wait_period(); |
107 } |
135 } |
108 } |
136 } |
130 |
158 |
131 int __init init_mod(void) |
159 int __init init_mod(void) |
132 { |
160 { |
133 RTIME tick_period, requested_ticks, now; |
161 RTIME tick_period, requested_ticks, now; |
134 |
162 |
135 printk(KERN_INFO "=== Starting EtherCAT RTAI sample module... ===\n"); |
163 printk(KERN_INFO PFX "Starting...\n"); |
136 |
164 |
137 rt_sem_init(&master_sem, 1); |
165 rt_sem_init(&master_sem, 1); |
138 |
166 |
139 t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000; |
167 t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000; |
140 |
168 |
141 if (!(master = ecrt_request_master(0))) { |
169 if (!(master = ecrt_request_master(0))) { |
142 printk(KERN_ERR "Requesting master 0 failed!\n"); |
170 printk(KERN_ERR PFX "Requesting master 0 failed!\n"); |
143 goto out_return; |
171 goto out_return; |
144 } |
172 } |
145 |
173 |
146 ecrt_master_callbacks(master, request_lock, release_lock, NULL); |
174 ecrt_master_callbacks(master, request_lock, release_lock, NULL); |
147 |
175 |
148 printk(KERN_INFO "Creating domain...\n"); |
176 printk(KERN_INFO PFX "Creating domain...\n"); |
149 if (!(domain1 = ecrt_master_create_domain(master))) { |
177 if (!(domain1 = ecrt_master_create_domain(master))) { |
150 printk(KERN_ERR "Domain creation failed!\n"); |
178 printk(KERN_ERR PFX "Domain creation failed!\n"); |
151 goto out_release_master; |
179 goto out_release_master; |
152 } |
180 } |
153 |
181 |
154 printk(KERN_INFO "Registering PDOs...\n"); |
182 printk(KERN_INFO PFX "Registering PDOs...\n"); |
155 if (ecrt_domain_register_pdo_list(domain1, domain1_pdos)) { |
183 if (ecrt_domain_register_pdo_list(domain1, domain1_pdo_regs)) { |
156 printk(KERN_ERR "PDO registration failed!\n"); |
184 printk(KERN_ERR PFX "PDO registration failed!\n"); |
157 goto out_release_master; |
185 goto out_release_master; |
158 } |
186 } |
159 |
187 |
160 printk(KERN_INFO "Activating master...\n"); |
188 printk(KERN_INFO PFX "Activating master...\n"); |
161 if (ecrt_master_activate(master)) { |
189 if (ecrt_master_activate(master)) { |
162 printk(KERN_ERR "Failed to activate master!\n"); |
190 printk(KERN_ERR PFX "Failed to activate master!\n"); |
163 goto out_release_master; |
191 goto out_release_master; |
164 } |
192 } |
165 |
193 |
166 printk("Starting cyclic sample thread...\n"); |
194 printk(KERN_INFO PFX "Starting cyclic sample thread...\n"); |
167 requested_ticks = nano2count(TIMERTICKS); |
195 requested_ticks = nano2count(TIMERTICKS); |
168 tick_period = start_rt_timer(requested_ticks); |
196 tick_period = start_rt_timer(requested_ticks); |
169 printk(KERN_INFO "RT timer started with %i/%i ticks.\n", |
197 printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n", |
170 (int) tick_period, (int) requested_ticks); |
198 (int) tick_period, (int) requested_ticks); |
171 |
199 |
172 if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) { |
200 if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) { |
173 printk(KERN_ERR "Failed to init RTAI task!\n"); |
201 printk(KERN_ERR PFX "Failed to init RTAI task!\n"); |
174 goto out_stop_timer; |
202 goto out_stop_timer; |
175 } |
203 } |
176 |
204 |
177 now = rt_get_time(); |
205 now = rt_get_time(); |
178 if (rt_task_make_periodic(&task, now + tick_period, tick_period)) { |
206 if (rt_task_make_periodic(&task, now + tick_period, tick_period)) { |
179 printk(KERN_ERR "Failed to run RTAI task!\n"); |
207 printk(KERN_ERR PFX "Failed to run RTAI task!\n"); |
180 goto out_stop_task; |
208 goto out_stop_task; |
181 } |
209 } |
182 |
210 |
183 printk(KERN_INFO "=== EtherCAT RTAI sample module started. ===\n"); |
211 printk(KERN_INFO PFX "Initialized.\n"); |
184 return 0; |
212 return 0; |
185 |
213 |
186 out_stop_task: |
214 out_stop_task: |
187 rt_task_delete(&task); |
215 rt_task_delete(&task); |
188 out_stop_timer: |
216 out_stop_timer: |
189 stop_rt_timer(); |
217 stop_rt_timer(); |
190 out_release_master: |
218 out_release_master: |
|
219 printk(KERN_ERR PFX "Releasing master...\n"); |
191 ecrt_release_master(master); |
220 ecrt_release_master(master); |
192 out_return: |
221 out_return: |
193 rt_sem_delete(&master_sem); |
222 rt_sem_delete(&master_sem); |
|
223 printk(KERN_ERR PFX "Failed to load. Aborting.\n"); |
194 return -1; |
224 return -1; |
195 } |
225 } |
196 |
226 |
197 /*****************************************************************************/ |
227 /*****************************************************************************/ |
198 |
228 |
199 void __exit cleanup_mod(void) |
229 void __exit cleanup_mod(void) |
200 { |
230 { |
201 printk(KERN_INFO "=== Stopping EtherCAT RTAI sample module... ===\n"); |
231 printk(KERN_INFO PFX "Stopping...\n"); |
202 |
232 |
203 rt_task_delete(&task); |
233 rt_task_delete(&task); |
204 stop_rt_timer(); |
234 stop_rt_timer(); |
205 ecrt_release_master(master); |
235 ecrt_release_master(master); |
206 rt_sem_delete(&master_sem); |
236 rt_sem_delete(&master_sem); |
207 |
237 |
208 printk(KERN_INFO "=== EtherCAT RTAI sample module stopped. ===\n"); |
238 printk(KERN_INFO PFX "Unloading.\n"); |
209 } |
239 } |
210 |
240 |
211 /*****************************************************************************/ |
241 /*****************************************************************************/ |
212 |
242 |
213 MODULE_LICENSE("GPL"); |
243 MODULE_LICENSE("GPL"); |