[etherlab-users] etherlab mini example
Antonio Del Cinque
adelcinque at gmail.com
Tue May 12 18:30:17 CEST 2009
Hi all,
I modified the etherlab mini example to test the ethercat master with our
ethercat slaves, which are custom devices, implementing the CanOpen dsp402
Drive profile. The configuration seems all right, and the slave is actually
going to the OP state, but the working counter reports a 2/3 actual/expected
value, and the domain process data are not reflecting actual values on the
slave.
I'm using an intel PRO/100 interface, a vanilla linux 2.6.24.6 kernel (I'm
planning to add rtai or xenomai after a successful test with the vanilla
kernel), and the ethercat-devel-r1718 master.
I don't know where the problem may reside. Please, can anyone give a look at
the following commands outputs and code?
Thank you very much
Antonio
Command outputs
===========================================
/ # insmod /lib/modules/2.6.24.6/ethercat/ec_mini.ko
[ 56.256713] ec_mini: Starting...
[ 56.260529] EtherCAT: Requesting master 0...
[ 56.267747] EtherCAT: Successfully requested master 0.
[ 56.273488] ec_mini: Registering domain...
[ 56.278978] ec_mini: Configuring PDOs...
[ 56.283259] ec_mini: Registering PDO entries...
[ 56.288097] ec_mini: Activating master...
[ 56.292414] EtherCAT: Domain0: Logical address 0x00000000, 16 byte,
expected working counter 3.
[ 56.302129] EtherCAT: Datagram domain0-0: Logical offset 0x00000000, 16
byte, type LRW.
[ 56.311247] EtherCAT: Stopping EoE processing.
[ 56.316059] EtherCAT: Master thread exited.
[ 56.320484] EtherCAT: Starting EtherCAT-OP thread.
[ 56.326125] EtherCAT: Starting EoE processing.
[ 56.331456] ec_mini: Starting cyclic sample thread.
[ 56.336632] ec_mini: Started.
/ # [ 56.374385] ec_mini: 1 slave(s).
[ 56.377739] ec_mini: AL states: 0x02.
[ 56.381336] ec_mini: Link is up.
[ 56.384546] ec_mini: AM_MP: State 0x02.
[ 56.388368] ec_mini: AM_MP: online.
[ 62.374365] ec_mini: AM_MP: State 0x01.
[ 78.374358] ec_mini: AM_MP: State 0x02.
[ 81.374354] EtherCAT: Domain 0: Working counter changed to 2/3.
[ 81.380226] ec_mini: Domain1: WC 2.
[ 81.383874] ec_mini: Domain1: State 1.
[ 86.374362] ec_mini: AM_MP: State 0x04.
[ 90.374359] ec_mini: AM_MP: State 0x08.
[ 90.378130] ec_mini: AM_MP: operational.
[ 90.382960] EtherCAT: Slave states: OP.
[ 92.374355] ec_mini: AL states: 0x08.
/ # ethercat slaves -v
=== Slave 0 ===
State: OP
Flag: +
Identity:
Vendor Id: 0x65547241
Product code: 0x3030504d
Revision number: 0x00000002
Serial number: 0x00000000
Ports:
0: MII.
1: MII.
2: Not implemented.
3: Not implemented.
DL information:
FMMU bit operation: no
Distributed clocks: yes (64 bit)
Mailboxes:
Bootstrap RX: 0x0000/0, TX: 0x0000/0
Standard RX: 0x1000/192, TX: 0x1200/192
Supported protocols: EoE, CoE, FoE
General:
Group: ET1100 SPI
Image name: DRIVE
Order number: MP_ECAT
Device name: Motopiattello - Modo interpolato
CoE details:
Enable SDO: yes
Enable SDO Info: no
Enable PDO Assign: no
Enable PDO Configuration: no
Enable Upload at startup: no
Enable SDO complete access: no
Flags:
Enable SafeOp: no
Enable notLRW: no
Current consumption: 0 mA
/ # ethercat config -v
Alias: 0
Position: 0
Vendor Id: 0x65547241
Product code: 0x3030504d
Attached slave: 0 (OP)
SM2 (Output)
PDO 0x1600
PDO entry 0x607a:00, 32 bit
PDO entry 0x6040:00, 16 bit
PDO entry 0x6060:00, 8 bit
PDO entry 0x6098:00, 8 bit
SM3 (Input)
PDO 0x1a00
PDO entry 0x6064:00, 32 bit
PDO entry 0x6041:00, 16 bit
PDO entry 0x6061:00, 8 bit
PDO entry 0x6098:00, 8 bit
SDO configuration:
None.
/ # ethercat pdos -v
SM0: PhysAddr 0x1000, DefaultSize 192, ControlRegister 0x26, Enable 1
SM1: PhysAddr 0x1200, DefaultSize 192, ControlRegister 0x22, Enable 1
SM2: PhysAddr 0x1300, DefaultSize 128, ControlRegister 0x64, Enable 1
RxPDO 0x1600 "RxPdo_0"
PDO entry 0x607a:00, 32 bit, "IP Data Record"
PDO entry 0x6040:00, 16 bit, "Control Word"
PDO entry 0x6060:00, 8 bit, "Mode of Operation"
PDO entry 0x6098:00, 8 bit, "Homing Method"
SM3: PhysAddr 0x1500, DefaultSize 128, ControlRegister 0x20, Enable 1
TxPDO 0x1a00 "TxPdo_0"
PDO entry 0x6064:00, 32 bit, "Position Actual Value"
PDO entry 0x6041:00, 16 bit, "Status Word"
PDO entry 0x6061:00, 8 bit, "Mode of Operation Display"
PDO entry 0x6098:00, 8 bit, "Homing Method"
/ # ethercat xml
<?xml version="1.0" ?>
<EtherCATInfo>
<!-- Slave 0 -->
<Vendor>
<Id>1700033089</Id>
</Vendor>
<Descriptions>
<Devices>
<Device>
<Type ProductCode="#x3030504d"
RevisionNo="#x00000002">MP_ECAT</Type>
<Name><![CDATA[Motopiattello - Modo interpolato]]></Name>
<Sm Enable="1" StartAddress="4096" ControlByte="38"
DefaultSize="192" />
<Sm Enable="1" StartAddress="4608" ControlByte="34"
DefaultSize="192" />
<Sm Enable="1" StartAddress="4864" ControlByte="100"
DefaultSize="128" />
<Sm Enable="1" StartAddress="5376" ControlByte="32"
DefaultSize="128" />
<RxPdo Sm="2" Fixed="1" Mandatory="1">
<Index>#x1600</Index>
<Name>RxPdo_0</Name>
<Entry>
<Index>#x607a</Index>
<SubIndex>0</SubIndex>
<BitLen>32</BitLen>
<Name>IP Data Record</Name>
<DataType>UINT32</DataType>
</Entry>
<Entry>
<Index>#x6040</Index>
<SubIndex>0</SubIndex>
<BitLen>16</BitLen>
<Name>Control Word</Name>
<DataType>UINT16</DataType>
</Entry>
<Entry>
<Index>#x6060</Index>
<SubIndex>0</SubIndex>
<BitLen>8</BitLen>
<Name>Mode of Operation</Name>
<DataType>UINT8</DataType>
</Entry>
<Entry>
<Index>#x6098</Index>
<SubIndex>0</SubIndex>
<BitLen>8</BitLen>
<Name>Homing Method</Name>
<DataType>UINT8</DataType>
</Entry>
</RxPdo>
<TxPdo Sm="3" Fixed="1" Mandatory="1">
<Index>#x1a00</Index>
<Name>TxPdo_0</Name>
<Entry>
<Index>#x6064</Index>
<SubIndex>0</SubIndex>
<BitLen>32</BitLen>
<Name>Position Actual Value</Name>
<DataType>UINT32</DataType>
</Entry>
<Entry>
<Index>#x6041</Index>
<SubIndex>0</SubIndex>
<BitLen>16</BitLen>
<Name>Status Word</Name>
<DataType>UINT16</DataType>
</Entry>
<Entry>
<Index>#x6061</Index>
<SubIndex>0</SubIndex>
<BitLen>8</BitLen>
<Name>Mode of Operation Display</Name>
<DataType>UINT8</DataType>
</Entry>
<Entry>
<Index>#x6098</Index>
<SubIndex>0</SubIndex>
<BitLen>8</BitLen>
<Name>Homing Method</Name>
<DataType>UINT8</DataType>
</Entry>
</TxPdo>
</Device>
</Devices>
</Descriptions>
</EtherCATInfo>
/ # ethercat domains -v
Domain0: LogBaseAddr 0x00000000, Size 16, WorkingCounter 2/3
SlaveConfig 0:0, SM2 (Output), LogAddr 0x00000000, Size 8
0x00 0x00 0x00 0x00 0x01 0x00 0x00 0x00
SlaveConfig 0:0, SM3 ( Input), LogAddr 0x00000008, Size 8
0x00 0x00 0x00 0x00 0x00 0x00 0x00 0x00
mini.c modified code
===========================================
#include <linux/module.h>
#include <linux/timer.h>
#include <linux/spinlock.h>
#include <linux/interrupt.h>
#include <linux/err.h>
#include "../../include/ecrt.h" // EtherCAT realtime interface
/*****************************************************************************/
// Module parameters
#define FREQUENCY 1
// Optional features
#define CONFIGURE_PDOS 1
#define EL3152_ALT_PDOS 0
#define EXTERNAL_MEMORY 1
#define SDO_ACCESS 0
#define VOE_ACCESS 0
#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_am_mp = NULL;
static ec_slave_config_state_t sc_am_mp_state = {};
// Timer
static struct timer_list timer;
/*****************************************************************************/
// process data
static uint8_t *domain1_pd; // process data memory
#define AM_MP_ECAT_SLAVE_POS 0, 0
#define AM_MP_ECAT 0x65547241, 0x3030504D
// offsets for PDO entries
static unsigned int off_am_mp_ip_data_record;
static unsigned int off_am_mp_control_word;
static unsigned int off_am_mp_mode_of_operation;
static unsigned int off_am_mp_homing_method;
static unsigned int off_am_mp_position_actual_value;
static unsigned int off_am_mp_status_word;
static unsigned int off_am_mp_mode_of_operation_display;
const static ec_pdo_entry_reg_t domain1_regs[] = {
{AM_MP_ECAT_SLAVE_POS, AM_MP_ECAT, 0x607A, 0,
&off_am_mp_ip_data_record},
{AM_MP_ECAT_SLAVE_POS, AM_MP_ECAT, 0x6040, 0, &off_am_mp_control_word},
{AM_MP_ECAT_SLAVE_POS, AM_MP_ECAT, 0x6060, 0,
&off_am_mp_mode_of_operation},
{AM_MP_ECAT_SLAVE_POS, AM_MP_ECAT, 0x6098, 0, &off_am_mp_homing_method},
{AM_MP_ECAT_SLAVE_POS, AM_MP_ECAT, 0x6064, 0,
&off_am_mp_position_actual_value},
{AM_MP_ECAT_SLAVE_POS, AM_MP_ECAT, 0x6041, 0, &off_am_mp_status_word},
{AM_MP_ECAT_SLAVE_POS, AM_MP_ECAT, 0x6061, 0,
&off_am_mp_mode_of_operation_display},
{}
};
static unsigned int counter = 0;
/*****************************************************************************/
#if CONFIGURE_PDOS
static ec_pdo_entry_info_t am_mp_rxpdo_0_entries[] = {
{0x607A, 0, 32}, // ip data record
{0x6040, 0, 16}, // control word
{0x6060, 0, 8}, // mode of operation
{0x6098, 0, 8} // homing method
};
static ec_pdo_entry_info_t am_mp_txpdo_0_entries[] = {
{0x6064, 0, 32}, // position actual value
{0x6041, 0, 16}, // status word
{0x6061, 0, 8}, // mode of operation display
{0x6098, 0, 8} // homing method
};
static ec_pdo_info_t am_mp_pdos[] = {
{0x1600, 4, am_mp_rxpdo_0_entries},
{0x1A00, 4, am_mp_txpdo_0_entries}
};
static ec_sync_info_t am_mp_syncs[] = {
{2, EC_DIR_OUTPUT, 1, am_mp_pdos},
{3, EC_DIR_INPUT, 1, am_mp_pdos + 1},
{0xff}
};
#endif
/*****************************************************************************/
#if SDO_ACCESS
static ec_sdo_request_t *sdo;
#endif
#if VOE_ACCESS
static ec_voe_handler_t *voe;
#endif
/*****************************************************************************/
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_am_mp, &s);
spin_unlock(&master_lock);
if (s.al_state != sc_am_mp_state.al_state)
printk(KERN_INFO PFX "AM_MP: State 0x%02X.\n", s.al_state);
if (s.online != sc_am_mp_state.online)
printk(KERN_INFO PFX "AM_MP: %s.\n", s.online ? "online" :
"offline");
if (s.operational != sc_am_mp_state.operational)
printk(KERN_INFO PFX "AM_MP: %soperational.\n",
s.operational ? "" : "Not ");
sc_am_mp_state = s;
}
/*****************************************************************************/
#if SDO_ACCESS
void read_sdo(void)
{
switch (ecrt_sdo_request_state(sdo)) {
case EC_REQUEST_UNUSED: // request was not used yet
ecrt_sdo_request_read(sdo); // trigger first read
break;
case EC_REQUEST_BUSY:
printk(KERN_INFO PFX "Still busy...\n");
break;
case EC_REQUEST_SUCCESS:
printk(KERN_INFO PFX "SDO value: 0x%04X\n",
EC_READ_U16(ecrt_sdo_request_data(sdo)));
ecrt_sdo_request_read(sdo); // trigger next read
break;
case EC_REQUEST_ERROR:
printk(KERN_INFO PFX "Failed to read SDO!\n");
ecrt_sdo_request_read(sdo); // retry reading
break;
}
}
#endif
/*****************************************************************************/
#if VOE_ACCESS
void read_voe(void)
{
switch (ecrt_voe_handler_execute(voe)) {
case EC_REQUEST_UNUSED:
ecrt_voe_handler_read(voe); // trigger first read
break;
case EC_REQUEST_BUSY:
printk(KERN_INFO PFX "VoE read still busy...\n");
break;
case EC_REQUEST_SUCCESS:
printk(KERN_INFO PFX "VoE received.\n");
// get data via ecrt_voe_handler_data(voe)
ecrt_voe_handler_read(voe); // trigger next read
break;
case EC_REQUEST_ERROR:
printk(KERN_INFO PFX "Failed to read VoE data!\n");
ecrt_voe_handler_read(voe); // retry reading
break;
}
}
#endif
/*****************************************************************************/
void cyclic_task(unsigned long data)
{
// 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;
// TODO: calculate new process data
// 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();
#endif
#if VOE_ACCESS
read_voe();
#endif
}
// write process data
EC_WRITE_U32((uint32_t *)(domain1_pd + off_am_mp_control_word), 0x0001);
// send process data
spin_lock(&master_lock);
ecrt_domain_queue(domain1);
ecrt_master_send(master);
spin_unlock(&master_lock);
// restart timer
timer.expires += HZ / FREQUENCY;
add_timer(&timer);
}
/*****************************************************************************/
int request_lock(void *data)
{
spin_lock(&master_lock);
return 0; // access allowed
}
/*****************************************************************************/
void release_lock(void *data)
{
spin_unlock(&master_lock);
}
/*****************************************************************************/
int __init init_mini_module(void)
{
int ret = -1;
#if CONFIGURE_PDOS
ec_slave_config_t *sc;
#endif
#if EXTERNAL_MEMORY
unsigned int size;
#endif
printk(KERN_INFO PFX "Starting...\n");
master = ecrt_request_master(0);
if (!master) {
ret = -EBUSY;
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_am_mp = ecrt_master_slave_config(
master, AM_MP_ECAT_SLAVE_POS, AM_MP_ECAT))) {
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_am_mp, EC_END, am_mp_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");
// status word...
if (!(sdo = ecrt_slave_config_create_sdo_request(sc_am_mp, 0x6041, 0,
2))) {
printk(KERN_ERR PFX "Failed to create SDO request.\n");
goto out_release_master;
}
ecrt_sdo_request_timeout(sdo, 500); // ms
#endif
#if VOE_ACCESS
// TODO...
#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;
}
#if EXTERNAL_MEMORY
if ((size = ecrt_domain_size(domain1))) {
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");
init_timer(&timer);
timer.function = cyclic_task;
timer.expires = jiffies + 10;
add_timer(&timer);
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 ret;
}
/*****************************************************************************/
void __exit cleanup_mini_module(void)
{
printk(KERN_INFO PFX "Stopping...\n");
del_timer_sync(&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_DESCRIPTION("EtherCAT minimal test environment");
module_init(init_mini_module);
module_exit(cleanup_mini_module);
/*****************************************************************************/
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.etherlab.org/pipermail/etherlab-users/attachments/20090512/491a95ef/attachment-0002.htm>
More information about the Etherlab-users
mailing list