fp@232: /****************************************************************************** fp@232: * fp@232: * $Id$ fp@232: * fp@232: * Copyright (C) 2006 Florian Pose, Ingenieurgemeinschaft IgH fp@232: * fp@232: * This file is part of the IgH EtherCAT Master. fp@232: * fp@232: * The IgH EtherCAT Master is free software; you can redistribute it fp@232: * and/or modify it under the terms of the GNU General Public License fp@246: * as published by the Free Software Foundation; either version 2 of the fp@246: * License, or (at your option) any later version. fp@232: * fp@232: * The IgH EtherCAT Master is distributed in the hope that it will be fp@232: * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of fp@232: * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the fp@232: * GNU General Public License for more details. fp@232: * fp@232: * You should have received a copy of the GNU General Public License fp@232: * along with the IgH EtherCAT Master; if not, write to the Free Software fp@232: * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA fp@232: * fp@246: * The right to use EtherCAT Technology is granted and comes free of fp@246: * charge under condition of compatibility of product made by fp@246: * Licensee. People intending to distribute/sell products based on the fp@246: * code, have to sign an agreement to guarantee that products using fp@246: * software based on IgH EtherCAT master stay compatible with the actual fp@246: * EtherCAT specification (which are released themselves as an open fp@246: * standard) as the (only) precondition to have the right to use EtherCAT fp@246: * Technology, IP and trade marks. fp@246: * fp@232: *****************************************************************************/ fp@232: fp@232: // Linux fp@232: #include fp@232: fp@232: // RTAI fp@232: #include "rtai_sched.h" fp@232: #include "rtai_sem.h" fp@232: fp@232: // RT_lib fp@232: #include fp@232: #include fp@234: #include fp@232: fp@232: // EtherCAT fp@232: #include "../../include/ecrt.h" fp@325: #include "../../include/ecdb.h" fp@232: fp@451: #define MSR_ABTASTFREQUENZ 1000 fp@451: fp@232: #define HZREDUCTION (MSR_ABTASTFREQUENZ / HZ) fp@232: #define TIMERTICKS (1000000000 / MSR_ABTASTFREQUENZ) fp@232: fp@232: /*****************************************************************************/ fp@232: fp@232: // RTAI fp@232: RT_TASK task; fp@232: SEM master_sem; fp@232: cycles_t t_start = 0, t_critical; fp@232: fp@232: // EtherCAT fp@232: ec_master_t *master = NULL; fp@232: ec_domain_t *domain1 = NULL; fp@232: fp@234: // raw process data fp@325: void *r_ana_out; fp@232: fp@232: // Channels fp@325: double k_ana_out; fp@325: fp@325: ec_pdo_reg_t domain1_pdos[] = { fp@325: {"3", Beckhoff_EL4132_Output1, &r_ana_out}, fp@232: {} fp@232: }; fp@232: fp@232: /*****************************************************************************/ fp@232: fp@234: void msr_controller_run(void) fp@234: { fp@234: rt_sem_wait(&master_sem); fp@234: fp@325: // receive fp@325: ecrt_master_receive(master); fp@234: ecrt_domain_process(domain1); fp@325: fp@325: // Process data fp@325: EC_WRITE_S16(r_ana_out, k_ana_out / 10.0 * 0x7FFF); fp@325: fp@325: // Send fp@509: ecrt_domain_queue(domain1); fp@234: ecrt_master_run(master); fp@325: ecrt_master_send(master); fp@234: fp@234: rt_sem_signal(&master_sem); fp@234: fp@234: msr_write_kanal_list(); fp@234: } fp@234: fp@234: /*****************************************************************************/ fp@234: fp@232: void msr_run(long data) fp@232: { fp@234: while (1) { fp@232: t_start = get_cycles(); fp@234: MSR_RTAITHREAD_CODE(msr_controller_run();); fp@232: rt_task_wait_period(); fp@232: } fp@232: } fp@232: fp@232: /*****************************************************************************/ fp@232: fp@232: int msr_reg(void) fp@232: { fp@325: msr_reg_kanal("/ana_out", "", &k_ana_out, TDBL); fp@232: return 0; fp@232: } fp@232: fp@232: /*****************************************************************************/ fp@232: fp@232: int request_lock(void *data) fp@232: { fp@232: // too close to the next RT cycle: deny access... fp@232: if (get_cycles() - t_start > t_critical) return -1; fp@232: fp@232: // allow access fp@232: rt_sem_wait(&master_sem); fp@232: return 0; fp@232: } fp@232: fp@232: /*****************************************************************************/ fp@232: fp@232: void release_lock(void *data) fp@232: { fp@232: rt_sem_signal(&master_sem); fp@232: } fp@232: fp@232: /*****************************************************************************/ fp@232: fp@232: int __init init_mod(void) fp@232: { fp@232: RTIME ticks; fp@232: fp@232: printk(KERN_INFO "=== Starting EtherCAT RTAI MSR sample module... ===\n"); fp@232: fp@232: rt_sem_init(&master_sem, 1); fp@232: t_critical = cpu_khz * 800 / MSR_ABTASTFREQUENZ; // ticks for 80% fp@232: fp@232: if (msr_rtlib_init(1, MSR_ABTASTFREQUENZ, 10, &msr_reg) < 0) { fp@232: printk(KERN_ERR "Failed to initialize rtlib!\n"); fp@232: goto out_return; fp@232: } fp@232: fp@325: if (!(master = ecrt_request_master(0))) { fp@232: printk(KERN_ERR "Failed to request master 0!\n"); fp@232: goto out_msr_cleanup; fp@232: } fp@232: fp@232: ecrt_master_callbacks(master, request_lock, release_lock, NULL); fp@232: fp@232: printk(KERN_INFO "Creating domains...\n"); fp@232: if (!(domain1 = ecrt_master_create_domain(master))) { fp@232: printk(KERN_ERR "Failed to create domains!\n"); fp@232: goto out_release_master; fp@232: } fp@232: fp@325: printk(KERN_INFO "Registering PDOs...\n"); fp@325: if (ecrt_domain_register_pdo_list(domain1, domain1_pdos)) { fp@325: printk(KERN_ERR "Failed to register PDOs.\n"); fp@232: goto out_release_master; fp@232: } fp@232: fp@232: printk(KERN_INFO "Activating master...\n"); fp@232: if (ecrt_master_activate(master)) { fp@232: printk(KERN_ERR "Could not activate master!\n"); fp@232: goto out_release_master; fp@232: } fp@232: fp@232: printk("Starting cyclic sample thread...\n"); fp@232: ticks = start_rt_timer(nano2count(TIMERTICKS)); fp@232: if (rt_task_init(&task, msr_run, 0, 2000, 0, 1, NULL)) { fp@232: printk(KERN_ERR "Failed to init RTAI task!\n"); fp@232: goto out_stop_timer; fp@232: } fp@232: if (rt_task_make_periodic(&task, rt_get_time() + ticks, ticks)) { fp@232: printk(KERN_ERR "Failed to run RTAI task!\n"); fp@232: goto out_stop_task; fp@232: } fp@232: fp@232: printk(KERN_INFO "=== EtherCAT RTAI MSR sample module started. ===\n"); fp@232: return 0; fp@232: fp@232: out_stop_task: fp@232: rt_task_delete(&task); fp@232: out_stop_timer: fp@232: stop_rt_timer(); fp@232: out_release_master: fp@232: ecrt_release_master(master); fp@232: out_msr_cleanup: fp@232: msr_rtlib_cleanup(); fp@232: out_return: fp@232: rt_sem_delete(&master_sem); fp@232: return -1; fp@232: } fp@232: fp@232: /*****************************************************************************/ fp@232: fp@232: void __exit cleanup_mod(void) fp@232: { fp@232: printk(KERN_INFO "=== Unloading EtherCAT RTAI MSR sample module... ===\n"); fp@232: fp@232: rt_task_delete(&task); fp@232: stop_rt_timer(); fp@232: ecrt_release_master(master); fp@232: rt_sem_delete(&master_sem); fp@232: msr_rtlib_cleanup(); fp@232: fp@232: printk(KERN_INFO "=== EtherCAT RTAI MSR sample module unloaded. ===\n"); fp@232: } fp@232: fp@232: /*****************************************************************************/ fp@232: fp@232: MODULE_LICENSE("GPL"); fp@232: MODULE_AUTHOR ("Florian Pose "); fp@232: MODULE_DESCRIPTION ("EtherCAT RTAI MSR sample module"); fp@232: fp@232: module_init(init_mod); fp@232: module_exit(cleanup_mod); fp@232: fp@232: /*****************************************************************************/