/****************************************************************************** * * $Id: mini.c 1604 2008-12-29 15:19:16Z fp $ * * Copyright (C) 2006-2008 Florian Pose, Ingenieurgemeinschaft IgH * * This file is part of the IgH EtherCAT Master. * * The IgH EtherCAT Master is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License version 2, as * published by the Free Software Foundation. * * The IgH EtherCAT Master is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General * Public License for more details. * * You should have received a copy of the GNU General Public License along * with the IgH EtherCAT Master; if not, write to the Free Software * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA * * Using the EtherCAT technology and brand is permitted in compliance with * the industrial property and similar rights of Beckhoff Automation GmbH. * *****************************************************************************/ #include // #include #include #include #include #include #include "../../include/ecrt.h" // EtherCAT realtime interface /*****************************************************************************/ // Module parameters #define FREQUENCY 1000 // Optional features #define CONFIGURE_PDOS 1 #define EXTERNAL_MEMORY 1 #define SDO_ACCESS 1 #define PFX "ec_mini: " /*****************************************************************************/ // EtherCAT static ec_master_t *master = NULL; static ec_master_state_t master_state = {}; spinlock_t master_lock = SPIN_LOCK_UNLOCKED; static ec_domain_t *domain1 = NULL; static ec_domain_state_t domain1_state = {}; static ec_slave_config_t *sc_accelnet = NULL; static ec_slave_config_state_t sc_accelnet_state = {}; // Timer //static struct timer_list timer; /*****************************************************************************/ // process data static uint8_t *domain1_pd; // process data memory #define AccelnetSlavePos 0, 0 #define AccelnetSlavePos2 0, 1 #define Accelnet_AEP055 0x000000ab, 0x00000380 // offsets for PDO entries static unsigned int off_accelnet_rx = -1; static unsigned int off_accelnet_rx1 = -1; static unsigned int off_accelnet_tx = -1; static unsigned int off_accelnet_tx1 = -1; //static unsigned int off_modeOfOperation = -1; const static ec_pdo_entry_reg_t domain1_regs[] = { {AccelnetSlavePos, Accelnet_AEP055, 0x6061, 0, &off_accelnet_rx}, // {AccelnetSlavePos, Accelnet_AEP055, 0x606C, 0, &off_accelnet_rx1}, {AccelnetSlavePos, Accelnet_AEP055, 0x607A, 0, &off_accelnet_tx}, // {AccelnetSlavePos, Accelnet_AEP055, 0x60FF, 0, &off_accelnet_tx1}, // {AccelnetSlavePos, Accelnet_AEP055, 0x6060, 0, &off_modeOfOperation}, {} }; static unsigned int counter = 0; // static unsigned int blink = 0; /*****************************************************************************/ #if CONFIGURE_PDOS static ec_pdo_entry_info_t accelnet_pdo_entries[] = { // Read PDO {0x6061, 0, 8}, // mode of operation display {0x6041, 0, 16}, // status {0x6064, 0, 32}, // actual position // {0x606C, 0, 32}, // actual velocity {0x60FA, 0, 32}, // commanded velocity {0x60FF, 0, 32}, // target velocity // Write PDO // {0x6040, 0, 16}, // control word // {0x6060, 0, 8}, // mode of operation {0x607A, 0, 32}, // target position {0x60FF, 0, 32}, // target velocity }; static ec_pdo_info_t accelnet_pdos[] = { {0x1A00, 3, accelnet_pdo_entries}, {0x1A01, 2, accelnet_pdo_entries+3}, {0x1600, 2, accelnet_pdo_entries+5}, {0x1601, 2, accelnet_pdo_entries+7}, }; static ec_sync_info_t accelnet_syncs[] = { {2, EC_DIR_OUTPUT, 2, accelnet_pdos+2}, {3, EC_DIR_INPUT, 2, accelnet_pdos}, {0xff} }; #endif /*****************************************************************************/ #if SDO_ACCESS static ec_sdo_request_t *sdoModeOfOper; static ec_sdo_request_t *sdoControlWord; #endif /*****************************************************************************/ unsigned long uSecClock(void) { struct timeval tv; unsigned long result; do_gettimeofday(&tv); result = (tv.tv_sec % 1000000) * (unsigned long)1000000 + tv.tv_usec; // printf("sec = %d, usec = %d, result = %lld\n",tv.tv_sec,tv.tv_usec,result); return result; } void check_domain1_state(void) { ec_domain_state_t ds; spin_lock(&master_lock); ecrt_domain_state(domain1, &ds); spin_unlock(&master_lock); if (ds.working_counter != domain1_state.working_counter) printk(KERN_INFO PFX "Domain1: WC %u.\n", ds.working_counter); if (ds.wc_state != domain1_state.wc_state) printk(KERN_INFO PFX "Domain1: State %u.\n", ds.wc_state); domain1_state = ds; } /*****************************************************************************/ void check_master_state(void) { ec_master_state_t ms; spin_lock(&master_lock); ecrt_master_state(master, &ms); spin_unlock(&master_lock); if (ms.slaves_responding != master_state.slaves_responding) printk(KERN_INFO PFX "%u slave(s).\n", ms.slaves_responding); if (ms.al_states != master_state.al_states) printk(KERN_INFO PFX "AL states: 0x%02X.\n", ms.al_states); if (ms.link_up != master_state.link_up) printk(KERN_INFO PFX "Link is %s.\n", ms.link_up ? "up" : "down"); master_state = ms; } /*****************************************************************************/ void check_slave_config_states(void) { ec_slave_config_state_t s; spin_lock(&master_lock); ecrt_slave_config_state(sc_accelnet, &s); spin_unlock(&master_lock); if (s.al_state != sc_accelnet_state.al_state) printk(KERN_INFO PFX "AccelNet: State 0x%02X.\n", s.al_state); if (s.online != sc_accelnet_state.online) printk(KERN_INFO PFX "AccelNet: %s.\n", s.online ? "online" : "offline"); if (s.operational != sc_accelnet_state.operational) printk(KERN_INFO PFX "AccelNet: %soperational.\n", s.operational ? "" : "Not "); sc_accelnet_state = s; } /*****************************************************************************/ #if SDO_ACCESS int read_sdo(ec_sdo_request_t *sdo, void* bufferPt) { void* data = NULL; unsigned size = 0; switch (ecrt_sdo_request_state(sdo)) { case EC_SDO_REQUEST_UNUSED: // request was not used yet printk(KERN_INFO PFX "SDO do Request\n"); ecrt_sdo_request_read(sdo); // trigger first read break; case EC_SDO_REQUEST_BUSY: printk(KERN_INFO PFX "SDO Still busy...\n"); break; case EC_SDO_REQUEST_SUCCESS: data = ecrt_sdo_request_data(sdo); size = ecrt_sdo_request_data_size(sdo); memcpy(bufferPt,data,size); // printk(KERN_INFO PFX "SDO value: 0x%04X\n", EC_READ_U16(data)); ecrt_sdo_request_read(sdo); // trigger next read return 1; break; case EC_SDO_REQUEST_ERROR: printk(KERN_INFO PFX "SDO: Failed to read SDO!\n"); ecrt_sdo_request_read(sdo); // retry reading break; } return 0; } int write_sdo(ec_sdo_request_t *sdo, void* bufferPt) { void* data = NULL; unsigned size = 0; data = ecrt_sdo_request_data(sdo); size = ecrt_sdo_request_data_size(sdo); switch (ecrt_sdo_request_state(sdo)) { case EC_SDO_REQUEST_UNUSED: // request was not used yet printk(KERN_INFO PFX "SDO do Write Request: state = UNUSED\n"); memcpy(data,bufferPt,size); ecrt_sdo_request_write(sdo); // trigger first read return 1; break; case EC_SDO_REQUEST_BUSY: printk(KERN_INFO PFX "SDO Still busy...\n"); break; case EC_SDO_REQUEST_SUCCESS: // printk(KERN_INFO PFX "SDO value: 0x%04X\n", EC_READ_U16(data)); memcpy(data,bufferPt,size); printk(KERN_INFO PFX "SDO do Write Request: state = SUCCESS\n"); ecrt_sdo_request_write(sdo); // trigger next read return 1; break; case EC_SDO_REQUEST_ERROR: printk(KERN_INFO PFX "SDO: Failed to read SDO!\n"); ecrt_sdo_request_read(sdo); // retry reading break; } return 0; } #endif /*****************************************************************************/ int lastPosition = -1; int lastMode = -1; int stepCounter = 0; int innerCounter = 0; #define dataLength 100 static int data[dataLength][5]; unsigned long startTime; static int dataIndex = 0; // void cyclic_task(unsigned long data) enum hrtimer_restart cyclic_task(struct hrtimer* timer) { unsigned long val; unsigned long currentTime; ktime_t ktm; unsigned modeOfOperation = 0; unsigned status = 0; int actualPosition = 0; int actualVelocity = 0; int commandedVelocity = 0; //uint16_t* controlWordPt = NULL; //uint8_t* modeOfOperationPt = NULL; int32_t* targetVelocityPt = NULL; int32_t* targetPositionPt = NULL; uint16_t controlWord = 0; // receive process data spin_lock(&master_lock); ecrt_master_receive(master); ecrt_domain_process(domain1); spin_unlock(&master_lock); // check process data state (optional) //check_domain1_state(); if (counter) { counter--; } else { // do this at 1 Hz counter = FREQUENCY; // calculate new process data // blink = !blink; // check for master state (optional) check_master_state(); // check for islave configuration state(s) (optional) check_slave_config_states(); #if SDO_ACCESS // read process data SDO // read_sdo(sdoControlWord); #endif } // read - write process data innerCounter ++; modeOfOperation = (*(uint8_t*) (domain1_pd + off_accelnet_rx + 0)); status = (*(uint16_t*) (domain1_pd + off_accelnet_rx + 1)); actualPosition = (*(int*) (domain1_pd + off_accelnet_rx + 3)); actualVelocity = (*(int*) (domain1_pd + off_accelnet_rx + 7)); commandedVelocity = (*(int*) (domain1_pd + off_accelnet_rx + 11)); targetPositionPt = ((int32_t*) (domain1_pd + off_accelnet_tx + 0)); targetVelocityPt = ((int32_t*) (domain1_pd + off_accelnet_tx + 4)); //if(innerCounter == 1) // read_sdo(sdoControlWord, &controlWord); // start Read if(innerCounter == 100) { printk(KERN_INFO PFX "At %lu microSec, innerCounter %d\n",uSecClock(), innerCounter); printk(KERN_INFO PFX "AcualPos %d, modeOfOp %X, Status %X\n", actualPosition,modeOfOperation,status); printk(KERN_INFO PFX "controlWord %X, targetVelocity %d, targetPosition %d\n" ,controlWord,*targetVelocityPt,*targetPositionPt); // *controlWordPt = 15; controlWord = 15; write_sdo(sdoControlWord, &controlWord); controlWord = 0; modeOfOperation = 9; write_sdo(sdoModeOfOper, &modeOfOperation); // *modeOfOperationPt = 3; //*modeOfOperationPt = 9; *targetVelocityPt = 0; } if(innerCounter == 200) { printk(KERN_INFO PFX "At %lu microSec, innerCounter %d\n",uSecClock(), innerCounter); printk(KERN_INFO PFX "AcualPos %d, modeOfOp %X, Status %X\n", actualPosition,modeOfOperation,status); printk(KERN_INFO PFX "controlWord %X, targetVelocity %d, targetPosition %d\n" ,controlWord,*targetVelocityPt,*targetPositionPt); if(read_sdo(sdoControlWord, &controlWord)) printk(KERN_INFO PFX "controlWordSDO 2 %X\n" ,controlWord); if(read_sdo(sdoModeOfOper, &modeOfOperation)) printk(KERN_INFO PFX "modeOfOperation 2 %d\n" ,modeOfOperation); } if(innerCounter >= 1000 && dataIndex < dataLength) { currentTime = uSecClock(); if(innerCounter == 1000) startTime = currentTime; if(innerCounter < 1050) *targetVelocityPt = innerCounter; else *targetVelocityPt = 1050; data[dataIndex][0] = innerCounter; data[dataIndex][1] = currentTime - startTime; data[dataIndex][2] = *targetVelocityPt; data[dataIndex][3] = commandedVelocity; data[dataIndex][4] = actualVelocity; dataIndex++; } if(dataIndex == dataLength) { int i; //*controlWordPt = 0; *targetVelocityPt = 0; printk(KERN_INFO PFX "1: Counter|TimeDiff|Write|Read\n"); for(i = 0; i < dataLength; i++) { printk(KERN_INFO PFX "%7d |%7d |%7d |%7d |%7d\n",data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); } dataIndex++; controlWord = 0; write_sdo(sdoControlWord, &controlWord); modeOfOperation = 3; write_sdo(sdoModeOfOper, &modeOfOperation); } if(innerCounter == 2500) { if(read_sdo(sdoControlWord, &controlWord)) printk(KERN_INFO PFX "controlWordSDO 4 %X\n" ,controlWord); if(read_sdo(sdoModeOfOper, &modeOfOperation)) printk(KERN_INFO PFX "modeOfOperation 4 %d\n" ,modeOfOperation); } if(innerCounter == 3000) { if(read_sdo(sdoControlWord, &controlWord)) printk(KERN_INFO PFX "controlWordSDO 5 %X\n" ,controlWord); if(read_sdo(sdoModeOfOper, &modeOfOperation)) printk(KERN_INFO PFX "modeOfOperation 5 %d\n" ,modeOfOperation); } // send process data spin_lock(&master_lock); ecrt_domain_queue(domain1); ecrt_master_send(master); spin_unlock(&master_lock); // restart timer //timer.expires += 2; // HZ / FREQUENCY; //add_timer(&timer); // ktm = ktime_set(0,100000); // 0.1 msec // ktm = ktime_set(0,333000); // 0.333 msec // ktm = ktime_set(0,500000); // 0.5 msec // ktm = ktime_set(0,1000000); // 1 msec ktm = ktime_set(0,10000000); // 10 msec // ktm = ktime_set(0,20000000); // 20 msec val = hrtimer_forward(timer, timer->expires, ktm); // printk(KERN_INFO PFX "forward returns %lX\n",val); //if(dataIndex == dataLength) // return HRTIMER_NORESTART; return HRTIMER_RESTART; } /*****************************************************************************/ int request_lock(void *data) { spin_lock(&master_lock); return 0; // access allowed } /*****************************************************************************/ void release_lock(void *data) { spin_unlock(&master_lock); } /*****************************************************************************/ static struct hrtimer timer; int __init init_mini_module(void) { ktime_t ktm; #if EXTERNAL_MEMORY unsigned int size; #endif printk(KERN_INFO PFX "Starting...\n"); if (!(master = ecrt_request_master(0))) { printk(KERN_ERR PFX "Requesting master 0 failed!\n"); goto out_return; } ecrt_master_callbacks(master, request_lock, release_lock, NULL); printk(KERN_INFO PFX "Registering domain...\n"); if (!(domain1 = ecrt_master_create_domain(master))) { printk(KERN_ERR PFX "Domain creation failed!\n"); goto out_release_master; } if (!(sc_accelnet = ecrt_master_slave_config( master, AccelnetSlavePos, Accelnet_AEP055))) { printk(KERN_ERR PFX "Failed to get slave configuration.\n"); goto out_release_master; } #if CONFIGURE_PDOS printk(KERN_INFO PFX "Configuring PDOs...\n"); if (ecrt_slave_config_pdos(sc_accelnet, EC_END, accelnet_syncs)) { printk(KERN_ERR PFX "Failed to configure PDOs.\n"); goto out_release_master; } #endif #if SDO_ACCESS printk(KERN_INFO PFX "Creating SDO requests...\n"); if (!(sdoControlWord = ecrt_slave_config_create_sdo_request(sc_accelnet, 0x6040, 0, 2))) { printk(KERN_ERR PFX "Failed to create SDO request for ControlWord.\n"); goto out_release_master; } ecrt_sdo_request_timeout(sdoControlWord, 500); // ms if (!(sdoModeOfOper = ecrt_slave_config_create_sdo_request(sc_accelnet, 0x6060, 0, 1))) { printk(KERN_ERR PFX "Failed to create SDO request for ModeOfOper.\n"); goto out_release_master; } ecrt_sdo_request_timeout(sdoModeOfOper, 500); // ms #endif printk(KERN_INFO PFX "Registering PDO entries...\n"); if (ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) { printk(KERN_ERR PFX "PDO entry registration failed!\n"); goto out_release_master; } size = ecrt_domain_size(domain1); printk(KERN_INFO PFX "size %d\n", size); printk(KERN_INFO PFX "offset rx %d\n", off_accelnet_rx); printk(KERN_INFO PFX "offset rx1 %d\n", off_accelnet_rx1); printk(KERN_INFO PFX "offset tx %d\n", off_accelnet_tx); printk(KERN_INFO PFX "Offset tx1 %d\n", off_accelnet_tx1); #if EXTERNAL_MEMORY if (size > 0) { if (!(domain1_pd = (uint8_t *) kmalloc(size, GFP_KERNEL))) { printk(KERN_ERR PFX "Failed to allocate %u bytes of process data" " memory!\n", size); goto out_release_master; } ecrt_domain_external_memory(domain1, domain1_pd); } #endif printk(KERN_INFO PFX "Activating master...\n"); if (ecrt_master_activate(master)) { printk(KERN_ERR PFX "Failed to activate master!\n"); #if EXTERNAL_MEMORY goto out_free_process_data; #else goto out_release_master; #endif } #if !EXTERNAL_MEMORY // Get internal process data for domain domain1_pd = ecrt_domain_data(domain1); #endif printk(KERN_INFO PFX "Starting cyclic sample thread.\n"); // printk(KERN_INFO PFX "jiffies = %d\n", jiffies); printk(KERN_INFO PFX "HZ = %d\n", HZ); printk(KERN_INFO PFX "FREQUENCY = %d\n",FREQUENCY); // init_timer(&timer); // timer.function = cyclic_task; // timer.expires = jiffies + 10; // add_timer(&timer); hrtimer_init(&timer,CLOCK_MONOTONIC,HRTIMER_MODE_REL); timer.function = cyclic_task; ktm = ktime_set(1,100000000); // 100 msec hrtimer_start(&timer,ktm,HRTIMER_MODE_REL); printk(KERN_INFO PFX "Started.\n"); return 0; #if EXTERNAL_MEMORY out_free_process_data: kfree(domain1_pd); #endif out_release_master: printk(KERN_ERR PFX "Releasing master...\n"); ecrt_release_master(master); out_return: printk(KERN_ERR PFX "Failed to load. Aborting.\n"); return -1; } /*****************************************************************************/ void __exit cleanup_mini_module(void) { printk(KERN_INFO PFX "Stopping...\n"); // del_timer_sync(&timer); hrtimer_cancel(&timer); #if EXTERNAL_MEMORY kfree(domain1_pd); #endif printk(KERN_INFO PFX "Releasing master...\n"); ecrt_release_master(master); printk(KERN_INFO PFX "Unloading.\n"); } /*****************************************************************************/ MODULE_LICENSE("GPL"); MODULE_AUTHOR("Florian Pose "); MODULE_DESCRIPTION("EtherCAT minimal test environment"); module_init(init_mini_module); module_exit(cleanup_mini_module); /*****************************************************************************/