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 |
98 counter--; |
107 counter--; |
99 } |
108 } |
100 else { |
109 else { |
101 counter = FREQUENCY; |
110 counter = FREQUENCY; |
102 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; |
103 } |
132 } |
104 |
133 |
105 rt_task_wait_period(); |
134 rt_task_wait_period(); |
106 } |
135 } |
107 } |
136 } |
129 |
158 |
130 int __init init_mod(void) |
159 int __init init_mod(void) |
131 { |
160 { |
132 RTIME tick_period, requested_ticks, now; |
161 RTIME tick_period, requested_ticks, now; |
133 |
162 |
134 printk(KERN_INFO "=== Starting EtherCAT RTAI sample module... ===\n"); |
163 printk(KERN_INFO PFX "Starting...\n"); |
135 |
164 |
136 rt_sem_init(&master_sem, 1); |
165 rt_sem_init(&master_sem, 1); |
137 |
166 |
138 t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000; |
167 t_critical = cpu_khz * 1000 / FREQUENCY - cpu_khz * INHIBIT_TIME / 1000; |
139 |
168 |
140 if (!(master = ecrt_request_master(0))) { |
169 if (!(master = ecrt_request_master(0))) { |
141 printk(KERN_ERR "Requesting master 0 failed!\n"); |
170 printk(KERN_ERR PFX "Requesting master 0 failed!\n"); |
142 goto out_return; |
171 goto out_return; |
143 } |
172 } |
144 |
173 |
145 ecrt_master_callbacks(master, request_lock, release_lock, NULL); |
174 ecrt_master_callbacks(master, request_lock, release_lock, NULL); |
146 |
175 |
147 printk(KERN_INFO "Creating domain...\n"); |
176 printk(KERN_INFO PFX "Creating domain...\n"); |
148 if (!(domain1 = ecrt_master_create_domain(master))) { |
177 if (!(domain1 = ecrt_master_create_domain(master))) { |
149 printk(KERN_ERR "Domain creation failed!\n"); |
178 printk(KERN_ERR PFX "Domain creation failed!\n"); |
150 goto out_release_master; |
179 goto out_release_master; |
151 } |
180 } |
152 |
181 |
153 printk(KERN_INFO "Registering PDOs...\n"); |
182 printk(KERN_INFO PFX "Registering PDOs...\n"); |
154 if (ecrt_domain_register_pdo_list(domain1, domain1_pdos)) { |
183 if (ecrt_domain_register_pdo_list(domain1, domain1_pdo_regs)) { |
155 printk(KERN_ERR "PDO registration failed!\n"); |
184 printk(KERN_ERR PFX "PDO registration failed!\n"); |
156 goto out_release_master; |
185 goto out_release_master; |
157 } |
186 } |
158 |
187 |
159 printk(KERN_INFO "Activating master...\n"); |
188 printk(KERN_INFO PFX "Activating master...\n"); |
160 if (ecrt_master_activate(master)) { |
189 if (ecrt_master_activate(master)) { |
161 printk(KERN_ERR "Failed to activate master!\n"); |
190 printk(KERN_ERR PFX "Failed to activate master!\n"); |
162 goto out_release_master; |
191 goto out_release_master; |
163 } |
192 } |
164 |
193 |
165 printk("Starting cyclic sample thread...\n"); |
194 printk(KERN_INFO PFX "Starting cyclic sample thread...\n"); |
166 requested_ticks = nano2count(TIMERTICKS); |
195 requested_ticks = nano2count(TIMERTICKS); |
167 tick_period = start_rt_timer(requested_ticks); |
196 tick_period = start_rt_timer(requested_ticks); |
168 printk(KERN_INFO "RT timer started with %i/%i ticks.\n", |
197 printk(KERN_INFO PFX "RT timer started with %i/%i ticks.\n", |
169 (int) tick_period, (int) requested_ticks); |
198 (int) tick_period, (int) requested_ticks); |
170 |
199 |
171 if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) { |
200 if (rt_task_init(&task, run, 0, 2000, 0, 1, NULL)) { |
172 printk(KERN_ERR "Failed to init RTAI task!\n"); |
201 printk(KERN_ERR PFX "Failed to init RTAI task!\n"); |
173 goto out_stop_timer; |
202 goto out_stop_timer; |
174 } |
203 } |
175 |
204 |
176 now = rt_get_time(); |
205 now = rt_get_time(); |
177 if (rt_task_make_periodic(&task, now + tick_period, tick_period)) { |
206 if (rt_task_make_periodic(&task, now + tick_period, tick_period)) { |
178 printk(KERN_ERR "Failed to run RTAI task!\n"); |
207 printk(KERN_ERR PFX "Failed to run RTAI task!\n"); |
179 goto out_stop_task; |
208 goto out_stop_task; |
180 } |
209 } |
181 |
210 |
182 printk(KERN_INFO "=== EtherCAT RTAI sample module started. ===\n"); |
211 printk(KERN_INFO PFX "Initialized.\n"); |
183 return 0; |
212 return 0; |
184 |
213 |
185 out_stop_task: |
214 out_stop_task: |
186 rt_task_delete(&task); |
215 rt_task_delete(&task); |
187 out_stop_timer: |
216 out_stop_timer: |
195 |
224 |
196 /*****************************************************************************/ |
225 /*****************************************************************************/ |
197 |
226 |
198 void __exit cleanup_mod(void) |
227 void __exit cleanup_mod(void) |
199 { |
228 { |
200 printk(KERN_INFO "=== Stopping EtherCAT RTAI sample module... ===\n"); |
229 printk(KERN_INFO PFX "Unloading...\n"); |
201 |
230 |
202 rt_task_delete(&task); |
231 rt_task_delete(&task); |
203 stop_rt_timer(); |
232 stop_rt_timer(); |
204 ecrt_release_master(master); |
233 ecrt_release_master(master); |
205 rt_sem_delete(&master_sem); |
234 rt_sem_delete(&master_sem); |
206 |
235 |
207 printk(KERN_INFO "=== EtherCAT RTAI sample module stopped. ===\n"); |
236 printk(KERN_INFO PFX "Stopped.\n"); |
208 } |
237 } |
209 |
238 |
210 /*****************************************************************************/ |
239 /*****************************************************************************/ |
211 |
240 |
212 MODULE_LICENSE("GPL"); |
241 MODULE_LICENSE("GPL"); |