├── 15.625MHz ├── README.md └── libwiznet_drv.a ├── LICENSE ├── README.md ├── build.sh ├── clean-all.sh ├── hardware ├── PiCAT-0.2.3.pdf ├── PiCAT-0.2.3.schdoc └── W5500Cape_R0.2.3_rpi0_Gerber_2.zip ├── lib └── libwiznet_drv.a ├── osal ├── Makefile ├── osal.c └── osal.h ├── oshw ├── Makefile ├── nicdrv.c ├── nicdrv.h ├── oshw.c ├── oshw.h └── wiznet_drv.h ├── soem ├── Makefile ├── ethercatbase.c ├── ethercatbase.h ├── ethercatcoe.c ├── ethercatcoe.h ├── ethercatconfig.c ├── ethercatconfig.h ├── ethercatconfiglist.h ├── ethercatdc.c ├── ethercatdc.h ├── ethercatfoe.c ├── ethercatfoe.h ├── ethercatmain.c ├── ethercatmain.h ├── ethercatprint.c ├── ethercatprint.h ├── ethercatsoe.c ├── ethercatsoe.h └── ethercattype.h └── test ├── MAXPOS_CSP ├── Makefile ├── ecat_dc.c ├── ecat_dc.h ├── main.c ├── pdo_def.h ├── servo.c └── servo_def.h ├── slaveInfo ├── Makefile └── slaveinfo.c ├── xmc4800 ├── Makefile ├── XMC48_ECAT_D4.hex ├── main.c └── pdo_def.h └── xmc4800_dc ├── Makefile ├── ecat_dc.c ├── ecat_dc.h ├── main.c └── pdo_def.h /15.625MHz/README.md: -------------------------------------------------------------------------------- 1 | 15.625MHz SPI speed library version 2 | -------------------------------------------------------------------------------- /15.625MHz/libwiznet_drv.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thanhtam-h/soem-w5500-rpi/3ee21f8a2b45d59073d6232553cf67821a33711c/15.625MHz/libwiznet_drv.a -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # soem-w5500-rpi 2 | Opensource realtime EtherCAT master for Raspberry pi 3 | 4 | If you are looking for a **hard-realtime EtherCAT Master** solution for Raspberry pi, this is the only place you have it. All guides are available at http://www.simplerobot.net/ 5 | 6 | 7 | **HARDWARE:** 8 | + Raspberry pi (rpi) any version 9 | + Wiznet W5500 Ethernet chip with SPI interface 10 | 11 | **SOFTWARE:** 12 | + Raspbian or Raspbian lite OS (https://www.raspberrypi.org/downloads/) 13 | + Patched kernel with xenomai 2.x or 3.x, or preempt-rt 14 | + Simple Open Source EtherCAT Master (SOEM) (https://openethercatsociety.github.io/) 15 | + SOEM is patched to support xenomai, preempt-rt 16 | + Realtime compatible W5500 driver for SOEM on rpi 17 | 18 | **BUILD:** 19 | 20 | **DO NOT** use master branch, select a working branch according to your raspberry pi and kernel version (or xenomai version).i.e.: 21 | 22 | + Raspberry pi 0,1 + Xenomai 2: https://github.com/thanhtam-h/soem-w5500-rpi/tree/rpi01-xenomai-2 23 | + Raspberry pi 0,1 + Xenomai 3: https://github.com/thanhtam-h/soem-w5500-rpi/tree/rpi01-xenomai-3 24 | + Raspberry pi 2,3 + Xenomai 2: https://github.com/thanhtam-h/soem-w5500-rpi/tree/rpi23-xenomai-2 25 | + Raspberry pi 2,3 + Xenomai 3: https://github.com/thanhtam-h/soem-w5500-rpi/tree/rpi23-xenomai-3 26 | + Raspberry pi 2,3 + Preempt-rt: https://github.com/thanhtam-h/soem-w5500-rpi/tree/rpi23-rt 27 | 28 | + Raspberry pi 4 + Preempt-rt: https://github.com/thanhtam-h/soem-w5500-rpi/tree/rpi4-rt 29 | + Raspberry pi 4 + Xenomai 3: https://github.com/thanhtam-h/soem-w5500-rpi/tree/rpi4-xenomai-3 30 | 31 | 32 | **TEST:** 33 | * Check information of all slaves on bus: 34 | 35 | cd test/slaveInfo/ 36 | sudo ./slaveinfo wiz 37 | 38 | * Check pdo mapping of all slaves on bus: 39 | 40 | cd test/slaveInfo/ 41 | sudo ./slaveinfo wiz -map 42 | -------------------------------------------------------------------------------- /build.sh: -------------------------------------------------------------------------------- 1 | cd osal 2 | make 3 | cd ../oshw 4 | make 5 | cd ../soem 6 | make 7 | cd ../test/slaveInfo 8 | make 9 | cd ../xmc4800 10 | make 11 | cd ../xmc4800_dc 12 | make 13 | cd .. 14 | cd .. 15 | 16 | 17 | -------------------------------------------------------------------------------- /clean-all.sh: -------------------------------------------------------------------------------- 1 | 2 | 3 | cd osal 4 | make clean 5 | cd ../oshw 6 | make clean 7 | cd ../soem 8 | make clean 9 | cd ../test/slaveInfo 10 | make clean 11 | cd ../xmc4800 12 | make clean 13 | cd ../xmc4800_dc 14 | make clean 15 | cd .. 16 | cd .. 17 | 18 | 19 | -------------------------------------------------------------------------------- /hardware/PiCAT-0.2.3.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thanhtam-h/soem-w5500-rpi/3ee21f8a2b45d59073d6232553cf67821a33711c/hardware/PiCAT-0.2.3.pdf -------------------------------------------------------------------------------- /hardware/PiCAT-0.2.3.schdoc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thanhtam-h/soem-w5500-rpi/3ee21f8a2b45d59073d6232553cf67821a33711c/hardware/PiCAT-0.2.3.schdoc -------------------------------------------------------------------------------- /hardware/W5500Cape_R0.2.3_rpi0_Gerber_2.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thanhtam-h/soem-w5500-rpi/3ee21f8a2b45d59073d6232553cf67821a33711c/hardware/W5500Cape_R0.2.3_rpi0_Gerber_2.zip -------------------------------------------------------------------------------- /lib/libwiznet_drv.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thanhtam-h/soem-w5500-rpi/3ee21f8a2b45d59073d6232553cf67821a33711c/lib/libwiznet_drv.a -------------------------------------------------------------------------------- /osal/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # 'make depend' uses makedepend to automatically generate dependencies 3 | # (dependencies are added to end of Makefile) 4 | # 'make' build executable file 'mycc' 5 | # 'make clean' removes all .o and executable files 6 | # 7 | 8 | # define the C compiler to use 9 | CC = gcc 10 | 11 | AR = ar 12 | 13 | # define any compile-time flags 14 | skin = alchemy 15 | CFLAGS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --cflags) 16 | CFLAGS += -g -O3 -ffunction-sections 17 | 18 | # define any directories containing header files other than /usr/include 19 | # 20 | INCLUDES += -I./ 21 | 22 | # define library paths in addition to /usr/lib 23 | # if I wanted to include libraries not in /usr/lib I'd specify 24 | # their path using -Lpath, something like: 25 | LDLIBS += $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --ldflags) 26 | 27 | AFLAGS = cr 28 | 29 | # define any libraries to link into executable: 30 | # if I want to link in libraries (libx.so or libx.a) I use the -llibname 31 | # option, something like (this will link in libmylib.so and libm.so: 32 | LIBS =-lnative 33 | LIBS +=-lxenomai 34 | LIBS +=-lrtdm 35 | 36 | # define the C source files 37 | SRCS = osal.c 38 | 39 | # define the C object files 40 | # 41 | # This uses Suffix Replacement within a macro: 42 | # $(name:string1=string2) 43 | # For each word in 'name' replace 'string1' with 'string2' 44 | # Below we are replacing the suffix .c of all words in the macro SRCS 45 | # with the .o suffix 46 | # 47 | OBJS = $(SRCS:.c=.o) 48 | 49 | # define the executable file 50 | MAIN = osal 51 | 52 | # 53 | # The following part of the makefile is generic; it can be used to 54 | # build any executable just by changing the definitions above and by 55 | # deleting dependencies appended to the file from 'make depend' 56 | # 57 | 58 | .PHONY: depend clean 59 | 60 | all: $(MAIN) 61 | 62 | $(MAIN): $(OBJS) 63 | $(AR) $(AFLAGS) ../lib/lib$(MAIN).a $(OBJS) 64 | 65 | # this is a suffix replacement rule for building .o's from .c's 66 | # it uses automatic variables $<: the name of the prerequisite of 67 | # the rule(a .c file) and $@: the name of the target of the rule (a .o file) 68 | # (see the gnu make manual section about automatic variables) 69 | .c.o: 70 | $(CC) $(CFLAGS) $(INCLUDES) -c $< -o $@ 71 | 72 | clean: 73 | $(RM) *.o *~ ../lib/lib$(MAIN).a 74 | 75 | 76 | # DO NOT DELETE THIS LINE -- make depend needs it 77 | -------------------------------------------------------------------------------- /osal/osal.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * * *** *** 3 | * *** *** *** 4 | * *** **** ********** *** ***** *** **** ***** 5 | * ********* ********** *** ********* ************ ********* 6 | * **** *** *** *** *** **** *** 7 | * *** *** ****** *** *********** *** **** ***** 8 | * *** *** ****** *** ************* *** **** ***** 9 | * *** **** **** *** *** *** **** *** 10 | * *** ******* ***** ************** ************* ********* 11 | * *** ***** *** ******* ** ** ****** ***** 12 | * t h e r e a l t i m e t a r g e t e x p e r t s 13 | * 14 | * http://www.rt-labs.com 15 | * Copyright (C) 2009. rt-labs AB, Sweden. All rights reserved. 16 | *------------------------------------------------------------------------------ 17 | * $Id: osal.c 416 2013-01-08 21:54:25Z smf.arthur $ 18 | *------------------------------------------------------------------------------ 19 | */ 20 | /* 21 | * Ported to xenomai by Ho Tam, thanhtam.h[at]gmail.com 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | //Xenomai-3 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #ifndef USECS_PER_SEC 38 | #define USECS_PER_SEC 1000000 39 | #endif 40 | 41 | int osal_usleep (uint32 usec) 42 | { 43 | return rt_task_sleep(usec * 1000); 44 | } 45 | 46 | int osal_gettimeofday(struct timeval *tv, struct timezone *tz) 47 | { 48 | uint32_t t_us= (uint32_t) (rt_timer_read()/1000); 49 | tv->tv_sec=t_us/USECS_PER_SEC; 50 | tv->tv_usec = t_us % USECS_PER_SEC; 51 | return 0; 52 | } 53 | 54 | ec_timet osal_current_time (void) 55 | { 56 | struct timeval current_time; 57 | ec_timet return_value; 58 | 59 | osal_gettimeofday (¤t_time, 0); 60 | return_value.sec = current_time.tv_sec; 61 | return_value.usec = current_time.tv_usec; 62 | return return_value; 63 | } 64 | 65 | void osal_timer_start (osal_timert * self, uint32 timeout_usec) 66 | { 67 | struct timeval start_time; 68 | struct timeval timeout; 69 | struct timeval stop_time; 70 | 71 | osal_gettimeofday (&start_time, 0); 72 | timeout.tv_sec = timeout_usec / USECS_PER_SEC; 73 | timeout.tv_usec = timeout_usec % USECS_PER_SEC; 74 | timeradd (&start_time, &timeout, &stop_time); 75 | 76 | self->stop_time.sec = stop_time.tv_sec; 77 | self->stop_time.usec = stop_time.tv_usec; 78 | } 79 | 80 | boolean osal_timer_is_expired (const osal_timert * self) 81 | { 82 | struct timeval current_time; 83 | struct timeval stop_time; 84 | int is_not_yet_expired; 85 | 86 | osal_gettimeofday (¤t_time, 0); 87 | stop_time.tv_sec = self->stop_time.sec; 88 | stop_time.tv_usec = self->stop_time.usec; 89 | is_not_yet_expired = timercmp (¤t_time, &stop_time, <); 90 | 91 | return is_not_yet_expired == FALSE; 92 | } 93 | -------------------------------------------------------------------------------- /osal/osal.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * * *** *** 3 | * *** *** *** 4 | * *** **** ********** *** ***** *** **** ***** 5 | * ********* ********** *** ********* ************ ********* 6 | * **** *** *** *** *** **** *** 7 | * *** *** ****** *** *********** *** **** ***** 8 | * *** *** ****** *** ************* *** **** ***** 9 | * *** **** **** *** *** *** **** *** 10 | * *** ******* ***** ************** ************* ********* 11 | * *** ***** *** ******* ** ** ****** ***** 12 | * t h e r e a l t i m e t a r g e t e x p e r t s 13 | * 14 | * http://www.rt-labs.com 15 | * Copyright (C) 2009. rt-labs AB, Sweden. All rights reserved. 16 | *------------------------------------------------------------------------------ 17 | * $Id: osal.h 452 2013-02-26 21:02:58Z smf.arthur $ 18 | *------------------------------------------------------------------------------ 19 | */ 20 | 21 | #ifndef _osal_ 22 | #define _osal_ 23 | 24 | #include 25 | 26 | /* General types */ 27 | typedef uint8_t boolean; 28 | #define TRUE 1 29 | #define FALSE 0 30 | typedef int8_t int8; 31 | typedef int16_t int16; 32 | typedef int32_t int32; 33 | typedef uint8_t uint8; 34 | typedef uint16_t uint16; 35 | typedef uint32_t uint32; 36 | typedef int64_t int64; 37 | typedef uint64_t uint64; 38 | typedef float float32; 39 | typedef double float64; 40 | 41 | #ifndef PACKED 42 | #ifdef _MSC_VER 43 | #define PACKED_BEGIN __pragma(pack(push, 1)) 44 | #define PACKED 45 | #define PACKED_END __pragma(pack(pop)) 46 | #elif defined(__GNUC__) 47 | #define PACKED_BEGIN 48 | #define PACKED __attribute__((__packed__)) 49 | #define PACKED_END 50 | #endif 51 | #endif 52 | 53 | typedef struct 54 | { 55 | uint32 sec; /*< Seconds elapsed since the Epoch (Jan 1, 1970) */ 56 | uint32 usec; /*< Microseconds elapsed since last second boundary */ 57 | } ec_timet; 58 | 59 | typedef struct osal_timer 60 | { 61 | ec_timet stop_time; 62 | } osal_timert; 63 | 64 | void osal_timer_start (osal_timert * self, uint32 timeout_us); 65 | boolean osal_timer_is_expired (const osal_timert * self); 66 | int osal_usleep (uint32 usec); 67 | ec_timet osal_current_time (void); 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /oshw/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # 'make depend' uses makedepend to automatically generate dependencies 3 | # (dependencies are added to end of Makefile) 4 | # 'make' build executable file 'mycc' 5 | # 'make clean' removes all .o and executable files 6 | # 7 | 8 | # define the C compiler to use 9 | CC = gcc 10 | 11 | AR = ar 12 | 13 | # define any compile-time flags 14 | CFLAGS = -g -O3 -ffunction-sections 15 | 16 | # define any directories containing header files other than /usr/include 17 | # 18 | INCLUDES += -I/usr/xenomai/include 19 | INCLUDES += -I../osal 20 | INCLUDES += -I../soem 21 | 22 | AFLAGS = cr 23 | 24 | 25 | # define the C source files 26 | SRCS = nicdrv.c oshw.c 27 | 28 | # define the C object files 29 | # 30 | # This uses Suffix Replacement within a macro: 31 | # $(name:string1=string2) 32 | # For each word in 'name' replace 'string1' with 'string2' 33 | # Below we are replacing the suffix .c of all words in the macro SRCS 34 | # with the .o suffix 35 | # 36 | OBJS = $(SRCS:.c=.o) 37 | 38 | # define the executable file 39 | MAIN = oshw 40 | 41 | # 42 | # The following part of the makefile is generic; it can be used to 43 | # build any executable just by changing the definitions above and by 44 | # deleting dependencies appended to the file from 'make depend' 45 | # 46 | 47 | .PHONY: depend clean 48 | 49 | all: $(MAIN) 50 | 51 | $(MAIN): $(OBJS) 52 | $(AR) $(AFLAGS) ../lib/lib$(MAIN).a $(OBJS) 53 | 54 | # this is a suffix replacement rule for building .o's from .c's 55 | # it uses automatic variables $<: the name of the prerequisite of 56 | # the rule(a .c file) and $@: the name of the target of the rule (a .o file) 57 | # (see the gnu make manual section about automatic variables) 58 | .c.o: 59 | $(CC) $(CFLAGS) $(INCLUDES) -c $< -o $@ 60 | 61 | clean: 62 | $(RM) *.o *~ ../lib/lib$(MAIN).a 63 | 64 | 65 | # DO NOT DELETE THIS LINE -- make depend needs it -------------------------------------------------------------------------------- /oshw/nicdrv.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : nicdrv.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo "EtherCAT" are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Headerfile for nicdrv.c 44 | */ 45 | 46 | #ifndef _nicdrvh_ 47 | #define _nicdrvh_ 48 | 49 | /** pointer structure to Tx and Rx stacks */ 50 | typedef struct 51 | { 52 | /** socket connection used */ 53 | int *sock; 54 | /** tx buffer */ 55 | ec_bufT (*txbuf)[EC_MAXBUF]; 56 | /** tx buffer lengths */ 57 | int (*txbuflength)[EC_MAXBUF]; 58 | /** temporary receive buffer */ 59 | ec_bufT *tempbuf; 60 | /** rx buffers */ 61 | ec_bufT (*rxbuf)[EC_MAXBUF]; 62 | /** rx buffer status fields */ 63 | int (*rxbufstat)[EC_MAXBUF]; 64 | /** received MAC source address (middle word) */ 65 | int (*rxsa)[EC_MAXBUF]; 66 | } ec_stackT; 67 | 68 | /** pointer structure to buffers for redundant port */ 69 | typedef struct 70 | { 71 | ec_stackT stack; 72 | int sockhandle; 73 | /** rx buffers */ 74 | ec_bufT rxbuf[EC_MAXBUF]; 75 | /** rx buffer status */ 76 | int rxbufstat[EC_MAXBUF]; 77 | /** rx MAC source address */ 78 | int rxsa[EC_MAXBUF]; 79 | /** temporary rx buffer */ 80 | ec_bufT tempinbuf; 81 | } ecx_redportt; 82 | 83 | /** pointer structure to buffers, vars and mutexes for port instantiation */ 84 | typedef struct 85 | { 86 | ec_stackT stack; 87 | int sockhandle; 88 | /** rx buffers */ 89 | ec_bufT rxbuf[EC_MAXBUF]; 90 | /** rx buffer status */ 91 | int rxbufstat[EC_MAXBUF]; 92 | /** rx MAC source address */ 93 | int rxsa[EC_MAXBUF]; 94 | /** temporary rx buffer */ 95 | ec_bufT tempinbuf; 96 | /** temporary rx buffer status */ 97 | int tempinbufs; 98 | /** transmit buffers */ 99 | ec_bufT txbuf[EC_MAXBUF]; 100 | /** transmit buffer lenghts */ 101 | int txbuflength[EC_MAXBUF]; 102 | /** temporary tx buffer */ 103 | ec_bufT txbuf2; 104 | /** temporary tx buffer length */ 105 | int txbuflength2; 106 | /** last used frame index */ 107 | int lastidx; 108 | /** current redundancy state */ 109 | int redstate; 110 | /** pointer to redundancy port and buffers */ 111 | ecx_redportt *redport; 112 | // mtx_t * getindex_mutex; //HoTam 113 | // mtx_t * tx_mutex; 114 | // mtx_t * rx_mutex; 115 | } ecx_portt; 116 | 117 | extern const uint16 priMAC[3]; 118 | extern const uint16 secMAC[3]; 119 | 120 | #ifdef EC_VER1 121 | extern ecx_portt ecx_port; 122 | extern ecx_redportt ecx_redport; 123 | 124 | int ec_setupnic(const char * ifname, int secondary); 125 | int ec_closenic(void); 126 | void ec_setbufstat(int idx, int bufstat); 127 | int ec_getindex(void); 128 | int ec_outframe(int idx, int sock); 129 | int ec_outframe_red(int idx); 130 | int ec_waitinframe(int idx, int timeout); 131 | int ec_srconfirm(int idx,int timeout); 132 | #endif 133 | 134 | void ec_setupheader(void *p); 135 | int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary); 136 | int ecx_closenic(ecx_portt *port); 137 | void ecx_setbufstat(ecx_portt *port, int idx, int bufstat); 138 | int ecx_getindex(ecx_portt *port); 139 | int ecx_outframe(ecx_portt *port, int idx, int sock); 140 | int ecx_outframe_red(ecx_portt *port, int idx); 141 | int ecx_waitinframe(ecx_portt *port, int idx, int timeout); 142 | int ecx_srconfirm(ecx_portt *port, int idx,int timeout); 143 | 144 | #endif 145 | -------------------------------------------------------------------------------- /oshw/oshw.c: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * * *** *** 3 | * *** *** *** 4 | * *** **** ********** *** ***** *** **** ***** 5 | * ********* ********** *** ********* ************ ********* 6 | * **** *** *** *** *** **** *** 7 | * *** *** ****** *** *********** *** **** ***** 8 | * *** *** ****** *** ************* *** **** ***** 9 | * *** **** **** *** *** *** **** *** 10 | * *** ******* ***** ************** ************* ********* 11 | * *** ***** *** ******* ** ** ****** ***** 12 | * t h e r e a l t i m e t a r g e t e x p e r t s 13 | * 14 | * http://www.rt-labs.com 15 | * Copyright (C) 2009. rt-labs AB, Sweden. All rights reserved. 16 | *------------------------------------------------------------------------------ 17 | * $Id: oshw.c 411 2012-12-02 20:16:39Z rtlaka $ 18 | *------------------------------------------------------------------------------ 19 | */ 20 | 21 | #include "oshw.h" 22 | #include 23 | 24 | /** 25 | * Host to Network byte order (i.e. to big endian). 26 | * 27 | * Note that Ethercat uses little endian byte order, except for the Ethernet 28 | * header which is big endian as usual. 29 | */ 30 | uint16 oshw_htons(const uint16 host) 31 | { 32 | uint16 network = htons (host); 33 | return network; 34 | } 35 | 36 | /** 37 | * Network (i.e. big endian) to Host byte order. 38 | * 39 | * Note that Ethercat uses little endian byte order, except for the Ethernet 40 | * header which is big endian as usual. 41 | */ 42 | uint16 oshw_ntohs(const uint16 network) 43 | { 44 | uint16 host = ntohs (network); 45 | return host; 46 | } 47 | 48 | /* Create list over available network adapters. 49 | * @return First element in linked list of adapters 50 | */ 51 | ec_adaptert * oshw_find_adapters(void) 52 | { 53 | ec_adaptert * ret_adapter = NULL; 54 | 55 | /* TODO if needed */ 56 | 57 | return ret_adapter; 58 | } 59 | 60 | /** Free memory allocated memory used by adapter collection. 61 | * @param[in] adapter = First element in linked list of adapters 62 | * EC_NOFRAME. 63 | */ 64 | void oshw_free_adapters(ec_adaptert * adapter) 65 | { 66 | /* TODO if needed */ 67 | } 68 | -------------------------------------------------------------------------------- /oshw/oshw.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * * *** *** 3 | * *** *** *** 4 | * *** **** ********** *** ***** *** **** ***** 5 | * ********* ********** *** ********* ************ ********* 6 | * **** *** *** *** *** **** *** 7 | * *** *** ****** *** *********** *** **** ***** 8 | * *** *** ****** *** ************* *** **** ***** 9 | * *** **** **** *** *** *** **** *** 10 | * *** ******* ***** ************** ************* ********* 11 | * *** ***** *** ******* ** ** ****** ***** 12 | * t h e r e a l t i m e t a r g e t e x p e r t s 13 | * 14 | * http://www.rt-labs.com 15 | * Copyright (C) 2009. rt-labs AB, Sweden. All rights reserved. 16 | *------------------------------------------------------------------------------ 17 | * $Id: oshw.h 452 2013-02-26 21:02:58Z smf.arthur $ 18 | *------------------------------------------------------------------------------ 19 | */ 20 | 21 | /** \file 22 | * \brief 23 | * Headerfile for oshw.c 24 | */ 25 | 26 | #ifndef _oshw_ 27 | #define _oshw_ 28 | 29 | 30 | //#include 31 | #include "ethercattype.h" 32 | #include "nicdrv.h" 33 | #include "ethercatmain.h" 34 | 35 | 36 | uint16 oshw_htons(uint16 hostshort); 37 | uint16 oshw_ntohs(uint16 networkshort); 38 | 39 | ec_adaptert * oshw_find_adapters(void); 40 | void oshw_free_adapters(ec_adaptert * adapter); 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /oshw/wiznet_drv.h: -------------------------------------------------------------------------------- 1 | 2 | /** \file 3 | * \brief 4 | * Header file for wiznet_drv.c 5 | * W5500 EtherCAT driver for raspberry pi 6 | * By Ho Tam, thanhtam.h[at]gmail.com 7 | */ 8 | 9 | #ifndef _wiznet_h_ 10 | #define _wiznet_h_ 11 | 12 | #include 13 | #include 14 | 15 | /* 16 | * Configure driver parameters 17 | * spi_clkDiv: clock divider nummber for rpi SPI, only three values are accepted: 18 | 08: spi speed 31.25 MHz (default) 19 | 16: spi speed 15.625 MHz 20 | 32: spi speed 7.8125 MHz 21 | other values will be 'rounded' down (e.g. 25->16) 22 | 23 | * startup_rst: enable W5500 reset at starting up 24 | 1: enable RST signal (default) 25 | othes: disable RST signal 26 | 27 | * link_wait_us: if RST is enabled, this is waiting time (for slave) after link RESET 28 | */ 29 | int wiznet_hw_config(uint32_t spi_clkDiv /*default 8 ~ 31.25 MHz*/, 30 | int startup_rst /*default 1 (enable)*/, 31 | uint32_t link_wait_us /*default 1000000 (1s)*/); 32 | 33 | int wiznet_macraw_init(uint8_t *enetaddr); 34 | int wiznet_macraw_send(void *packet, int length); 35 | int wiznet_macraw_recv(uint8_t * packet, size_t size); 36 | 37 | #endif //_wiznet_h_ 38 | -------------------------------------------------------------------------------- /soem/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # 'make depend' uses makedepend to automatically generate dependencies 3 | # (dependencies are added to end of Makefile) 4 | # 'make' build executable file 'mycc' 5 | # 'make clean' removes all .o and executable files 6 | # 7 | 8 | # define the C compiler to use 9 | CC = gcc 10 | 11 | AR = ar 12 | 13 | # define any compile-time flags 14 | CFLAGS = -g -O3 -ffunction-sections 15 | 16 | # define any directories containing header files other than /usr/include 17 | # 18 | INCLUDES += -I../oshw 19 | INCLUDES += -I../osal 20 | INCLUDES += -I../soem 21 | 22 | # define library paths in addition to /usr/lib 23 | # if I wanted to include libraries not in /usr/lib I'd specify 24 | # their path using -Lpath, something like: 25 | LFLAGS = 26 | 27 | AFLAGS = cr 28 | 29 | # define any libraries to link into executable: 30 | # if I want to link in libraries (libx.so or libx.a) I use the -llibname 31 | # option, something like (this will link in libmylib.so and libm.so: 32 | LIBS = 33 | 34 | # define the C source files 35 | SRCS = ethercatbase.c ethercatcoe.c ethercatconfig.c ethercatdc.c ethercatfoe.c ethercatmain.c ethercatprint.c ethercatsoe.c 36 | 37 | # define the C object files 38 | # 39 | # This uses Suffix Replacement within a macro: 40 | # $(name:string1=string2) 41 | # For each word in 'name' replace 'string1' with 'string2' 42 | # Below we are replacing the suffix .c of all words in the macro SRCS 43 | # with the .o suffix 44 | # 45 | OBJS = $(SRCS:.c=.o) 46 | 47 | # define the executable file 48 | MAIN = soem 49 | 50 | # 51 | # The following part of the makefile is generic; it can be used to 52 | # build any executable just by changing the definitions above and by 53 | # deleting dependencies appended to the file from 'make depend' 54 | # 55 | 56 | .PHONY: depend clean 57 | 58 | all: $(MAIN) 59 | 60 | $(MAIN): $(OBJS) 61 | $(AR) $(AFLAGS) ../lib/lib$(MAIN).a $(OBJS) 62 | 63 | # this is a suffix replacement rule for building .o's from .c's 64 | # it uses automatic variables $<: the name of the prerequisite of 65 | # the rule(a .c file) and $@: the name of the target of the rule (a .o file) 66 | # (see the gnu make manual section about automatic variables) 67 | .c.o: 68 | $(CC) $(CFLAGS) $(INCLUDES) -c $< -o $@ 69 | 70 | clean: 71 | $(RM) *.o *~ ../lib/lib$(MAIN).a 72 | 73 | 74 | # DO NOT DELETE THIS LINE -- make depend needs it -------------------------------------------------------------------------------- /soem/ethercatbase.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatbase.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Headerfile for ethercatbase.c 44 | */ 45 | 46 | #ifndef _ethercatbase_ 47 | #define _ethercatbase_ 48 | 49 | #ifdef __cplusplus 50 | extern "C" 51 | { 52 | #endif 53 | 54 | int ecx_setupdatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data); 55 | int ecx_adddatagram(ecx_portt *port, void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data); 56 | int ecx_BWR(ecx_portt *port, uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); 57 | int ecx_BRD(ecx_portt *port, uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); 58 | int ecx_APRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 59 | int ecx_ARMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 60 | int ecx_FRMW(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 61 | uint16 ecx_APRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout); 62 | int ecx_FPRD(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 63 | uint16 ecx_FPRDw(ecx_portt *port, uint16 ADP, uint16 ADO, int timeout); 64 | int ecx_APWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout); 65 | int ecx_APWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 66 | int ecx_FPWRw(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 data, int timeout); 67 | int ecx_FPWR(ecx_portt *port, uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 68 | int ecx_LRW(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); 69 | int ecx_LRD(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); 70 | int ecx_LWR(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, int timeout); 71 | int ecx_LRWDC(ecx_portt *port, uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout); 72 | 73 | #ifdef EC_VER1 74 | int ec_setupdatagram(void *frame, uint8 com, uint8 idx, uint16 ADP, uint16 ADO, uint16 length, void *data); 75 | int ec_adddatagram(void *frame, uint8 com, uint8 idx, boolean more, uint16 ADP, uint16 ADO, uint16 length, void *data); 76 | int ec_BWR(uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); 77 | int ec_BRD(uint16 ADP,uint16 ADO,uint16 length,void *data,int timeout); 78 | int ec_APRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 79 | int ec_ARMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 80 | int ec_FRMW(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 81 | uint16 ec_APRDw(uint16 ADP, uint16 ADO, int timeout); 82 | int ec_FPRD(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 83 | uint16 ec_FPRDw(uint16 ADP, uint16 ADO, int timeout); 84 | int ec_APWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout); 85 | int ec_APWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 86 | int ec_FPWRw(uint16 ADP, uint16 ADO, uint16 data, int timeout); 87 | int ec_FPWR(uint16 ADP, uint16 ADO, uint16 length, void *data, int timeout); 88 | int ec_LRW(uint32 LogAdr, uint16 length, void *data, int timeout); 89 | int ec_LRD(uint32 LogAdr, uint16 length, void *data, int timeout); 90 | int ec_LWR(uint32 LogAdr, uint16 length, void *data, int timeout); 91 | int ec_LRWDC(uint32 LogAdr, uint16 length, void *data, uint16 DCrs, int64 *DCtime, int timeout); 92 | #endif 93 | 94 | #ifdef __cplusplus 95 | } 96 | #endif 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /soem/ethercatcoe.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatcoe.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Headerfile for ethercatcoe.c 44 | */ 45 | 46 | #ifndef _ethercatcoe_ 47 | #define _ethercatcoe_ 48 | 49 | #ifdef __cplusplus 50 | extern "C" 51 | { 52 | #endif 53 | 54 | /** max entries in Object Description list */ 55 | #define EC_MAXODLIST 1024 56 | 57 | /** max entries in Object Entry list */ 58 | #define EC_MAXOELIST 256 59 | 60 | /* Storage for object description list */ 61 | typedef struct 62 | { 63 | /** slave number */ 64 | uint16 Slave; 65 | /** number of entries in list */ 66 | uint16 Entries; 67 | /** array of indexes */ 68 | uint16 Index[EC_MAXODLIST]; 69 | /** array of datatypes, see EtherCAT specification */ 70 | uint16 DataType[EC_MAXODLIST]; 71 | /** array of object codes, see EtherCAT specification */ 72 | uint8 ObjectCode[EC_MAXODLIST]; 73 | /** number of subindexes for each index */ 74 | uint8 MaxSub[EC_MAXODLIST]; 75 | /** textual description of each index */ 76 | char Name[EC_MAXODLIST][EC_MAXNAME+1]; 77 | } ec_ODlistt; 78 | 79 | /* storage for object list entry information */ 80 | typedef struct 81 | { 82 | /** number of entries in list */ 83 | uint16 Entries; 84 | /** array of value infos, see EtherCAT specification */ 85 | uint8 ValueInfo[EC_MAXOELIST]; 86 | /** array of value infos, see EtherCAT specification */ 87 | uint16 DataType[EC_MAXOELIST]; 88 | /** array of bit lengths, see EtherCAT specification */ 89 | uint16 BitLength[EC_MAXOELIST]; 90 | /** array of object access bits, see EtherCAT specification */ 91 | uint16 ObjAccess[EC_MAXOELIST]; 92 | /** textual description of each index */ 93 | char Name[EC_MAXOELIST][EC_MAXNAME+1]; 94 | } ec_OElistt; 95 | 96 | #ifdef EC_VER1 97 | void ec_SDOerror(uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode); 98 | int ec_SDOread(uint16 slave, uint16 index, uint8 subindex, 99 | boolean CA, int *psize, void *p, int timeout); 100 | int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, 101 | boolean CA, int psize, void *p, int Timeout); 102 | int ec_RxPDO(uint16 Slave, uint16 RxPDOnumber , int psize, void *p); 103 | int ec_TxPDO(uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout); 104 | int ec_readPDOmap(uint16 Slave, int *Osize, int *Isize); 105 | int ec_readPDOmapCA(uint16 Slave, int *Osize, int *Isize); 106 | int ec_readODlist(uint16 Slave, ec_ODlistt *pODlist); 107 | int ec_readODdescription(uint16 Item, ec_ODlistt *pODlist); 108 | int ec_readOEsingle(uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist); 109 | int ec_readOE(uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist); 110 | #endif 111 | 112 | void ecx_SDOerror(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIdx, int32 AbortCode); 113 | int ecx_SDOread(ecx_contextt *context, uint16 slave, uint16 index, uint8 subindex, 114 | boolean CA, int *psize, void *p, int timeout); 115 | int ecx_SDOwrite(ecx_contextt *context, uint16 Slave, uint16 Index, uint8 SubIndex, 116 | boolean CA, int psize, void *p, int Timeout); 117 | int ecx_RxPDO(ecx_contextt *context, uint16 Slave, uint16 RxPDOnumber , int psize, void *p); 118 | int ecx_TxPDO(ecx_contextt *context, uint16 slave, uint16 TxPDOnumber , int *psize, void *p, int timeout); 119 | int ecx_readPDOmap(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize); 120 | int ecx_readPDOmapCA(ecx_contextt *context, uint16 Slave, int *Osize, int *Isize); 121 | int ecx_readODlist(ecx_contextt *context, uint16 Slave, ec_ODlistt *pODlist); 122 | int ecx_readODdescription(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist); 123 | int ecx_readOEsingle(ecx_contextt *context, uint16 Item, uint8 SubI, ec_ODlistt *pODlist, ec_OElistt *pOElist); 124 | int ecx_readOE(ecx_contextt *context, uint16 Item, ec_ODlistt *pODlist, ec_OElistt *pOElist); 125 | 126 | #ifdef __cplusplus 127 | } 128 | #endif 129 | 130 | #endif 131 | -------------------------------------------------------------------------------- /soem/ethercatconfig.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatconfig.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Headerfile for ethercatconfig.c 44 | */ 45 | 46 | #ifndef _ethercatconfig_ 47 | #define _ethercatconfig_ 48 | 49 | #ifdef __cplusplus 50 | extern "C" 51 | { 52 | #endif 53 | 54 | #define EC_NODEOFFSET 0x1000 55 | #define EC_TEMPNODE 0xffff 56 | 57 | #ifdef EC_VER1 58 | int ec_config_init(uint8 usetable); 59 | int ec_config_map(void *pIOmap); 60 | int ec_config_map_group(void *pIOmap, uint8 group); 61 | int ec_config(uint8 usetable, void *pIOmap); 62 | int ec_recover_slave(uint16 slave, int timeout); 63 | int ec_reconfig_slave(uint16 slave, int timeout); 64 | #endif 65 | 66 | int ecx_config_init(ecx_contextt *context, uint8 usetable); 67 | int ecx_config_map_group(ecx_contextt *context, void *pIOmap, uint8 group); 68 | int ecx_recover_slave(ecx_contextt *context, uint16 slave, int timeout); 69 | int ecx_reconfig_slave(ecx_contextt *context, uint16 slave, int timeout); 70 | 71 | #ifdef __cplusplus 72 | } 73 | #endif 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /soem/ethercatconfiglist.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatconfiglist.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * DEPRICATED Configuration list of known EtherCAT slave devices. 44 | * 45 | * If a slave is found in this list it is configured according to the parameters 46 | * in the list. Otherwise the configuration info is read directly from the slave 47 | * EEPROM (SII or Slave Information Interface). 48 | */ 49 | 50 | /* 51 | explanation of dev: 52 | 1: static device with no IO mapping ie EK1100 53 | 2: input device no mailbox ie simple IO device 54 | 3: output device no mailbox 55 | 4: input device with mailbox configuration 56 | 5: output device with mailbox configuration 57 | 6: input/output device no mailbox 58 | 7: input.output device with mailbox configuration 59 | */ 60 | #define EC_CONFIGEND 0xffffffff 61 | 62 | ec_configlist_t ec_configlist[] = { 63 | {/*Man=*/0x00000000,/*ID=*/0x00000000,/*Name=*/"" ,/*dtype=*/0,/*Ibits=*/ 0,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 64 | {/*Man=*/0x00000002,/*ID=*/0x044c2c52,/*Name=*/"EK1100" ,/*dtype=*/1,/*Ibits=*/ 0,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 65 | {/*Man=*/0x00000002,/*ID=*/0x03ea3052,/*Name=*/"EL1002" ,/*dtype=*/2,/*Ibits=*/ 2,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 66 | {/*Man=*/0x00000002,/*ID=*/0x03ec3052,/*Name=*/"EL1004" ,/*dtype=*/2,/*Ibits=*/ 4,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 67 | {/*Man=*/0x00000002,/*ID=*/0x03f43052,/*Name=*/"EL1012" ,/*dtype=*/2,/*Ibits=*/ 2,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 68 | {/*Man=*/0x00000002,/*ID=*/0x03f63052,/*Name=*/"EL1014" ,/*dtype=*/2,/*Ibits=*/ 4,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 69 | {/*Man=*/0x00000002,/*ID=*/0x03fa3052,/*Name=*/"EL1018" ,/*dtype=*/2,/*Ibits=*/ 8,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 70 | {/*Man=*/0x00000002,/*ID=*/0x07d23052,/*Name=*/"EL2002" ,/*dtype=*/3,/*Ibits=*/ 0,/*Obits=*/ 2,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 71 | {/*Man=*/0x00000002,/*ID=*/0x07d43052,/*Name=*/"EL2004" ,/*dtype=*/3,/*Ibits=*/ 0,/*Obits=*/ 4,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 72 | {/*Man=*/0x00000002,/*ID=*/0x07d83052,/*Name=*/"EL2008" ,/*dtype=*/3,/*Ibits=*/ 0,/*Obits=*/ 8,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 73 | {/*Man=*/0x00000002,/*ID=*/0x07f03052,/*Name=*/"EL2032" ,/*dtype=*/6,/*Ibits=*/ 2,/*Obits=*/ 2,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0}, 74 | {/*Man=*/0x00000002,/*ID=*/0x0c1e3052,/*Name=*/"EL3102" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, 75 | {/*Man=*/0x00000002,/*ID=*/0x0c283052,/*Name=*/"EL3112" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, 76 | {/*Man=*/0x00000002,/*ID=*/0x0c323052,/*Name=*/"EL3122" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, 77 | {/*Man=*/0x00000002,/*ID=*/0x0c463052,/*Name=*/"EL3142" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, 78 | {/*Man=*/0x00000002,/*ID=*/0x0c503052,/*Name=*/"EL3152" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, 79 | {/*Man=*/0x00000002,/*ID=*/0x0c5a3052,/*Name=*/"EL3162" ,/*dtype=*/4,/*Ibits=*/48,/*Obits=*/ 0,/*SM2a*/0x1000,/*SM2f*/0x00000024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/0,/*FM1ac*/1}, 80 | {/*Man=*/0x00000002,/*ID=*/0x0fc03052,/*Name=*/"EL4032" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1100,/*SM2f*/0x00010024,/*SM3a*/0x1180,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, 81 | {/*Man=*/0x00000002,/*ID=*/0x10063052,/*Name=*/"EL4102" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, 82 | {/*Man=*/0x00000002,/*ID=*/0x10103052,/*Name=*/"EL4112" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, 83 | {/*Man=*/0x00000002,/*ID=*/0x101a3052,/*Name=*/"EL4122" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, 84 | {/*Man=*/0x00000002,/*ID=*/0x10243052,/*Name=*/"EL4132" ,/*dtype=*/5,/*Ibits=*/ 0,/*Obits=*/32,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00000022,/*FM0ac*/1,/*FM1ac*/0}, 85 | {/*Man=*/0x00000002,/*ID=*/0x13ed3052,/*Name=*/"EL5101" ,/*dtype=*/7,/*Ibits=*/40,/*Obits=*/24,/*SM2a*/0x1000,/*SM2f*/0x00010024,/*SM3a*/0x1100,/*SM3f*/0x00010020,/*FM0ac*/1,/*FM1ac*/1}, 86 | {/*Man=*/EC_CONFIGEND,/*ID=*/0x00000000,/*Name=*/"" ,/*dtype=*/0,/*Ibits=*/ 0,/*Obits=*/ 0,/*SM2a*/ 0,/*SM2f*/ 0,/*SM3a*/ 0,/*SM3f*/ 0,/*FM0ac*/0,/*FM1ac*/0} 87 | }; 88 | -------------------------------------------------------------------------------- /soem/ethercatdc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatdc.c 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Distributed Clock EtherCAT functions. 44 | * 45 | */ 46 | #include "oshw.h" 47 | #include "osal.h" 48 | #include "ethercattype.h" 49 | #include "ethercatbase.h" 50 | #include "ethercatmain.h" 51 | #include "ethercatdc.h" 52 | 53 | #define PORTM0 0x01 54 | #define PORTM1 0x02 55 | #define PORTM2 0x04 56 | #define PORTM3 0x08 57 | 58 | /** 1st sync pulse delay in ns here 100ms */ 59 | #define SyncDelay ((int32)100000000) 60 | 61 | /** 62 | * Set DC of slave to fire sync0 at CyclTime interval with CyclShift offset. 63 | * 64 | * @param[in] context = context struct 65 | * @param [in] slave Slave number. 66 | * @param [in] act TRUE = active, FALSE = deactivated 67 | * @param [in] CyclTime Cycltime in ns. 68 | * @param [in] CyclShift CyclShift in ns. 69 | */ 70 | void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift) 71 | { 72 | uint8 h, RA; 73 | uint16 wc, slaveh; 74 | int64 t, t1; 75 | int32 tc; 76 | 77 | slaveh = context->slavelist[slave].configadr; 78 | RA = 0; 79 | 80 | /* stop cyclic operation, ready for next trigger */ 81 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); 82 | if (act) 83 | { 84 | RA = 1 + 2; /* act cyclic operation and sync0, sync1 deactivated */ 85 | } 86 | h = 0; 87 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */ 88 | wc = ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */ 89 | t1 = etohll(t1); 90 | 91 | /* Calculate first trigger time, always a whole multiple of CyclTime rounded up 92 | plus the shifttime (can be negative) 93 | This insures best sychronisation between slaves, slaves with the same CyclTime 94 | will sync at the same moment (you can use CyclShift to shift the sync) */ 95 | if (CyclTime > 0) 96 | { 97 | t = ((t1 + SyncDelay) / CyclTime) * CyclTime + CyclTime + CyclShift; 98 | } 99 | else 100 | { 101 | t = t1 + SyncDelay + CyclShift; 102 | /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */ 103 | } 104 | t = htoell(t); 105 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */ 106 | tc = htoel(CyclTime); 107 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */ 108 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */ 109 | } 110 | 111 | /** 112 | * Set DC of slave to fire sync0 and sync1 at CyclTime interval with CyclShift offset. 113 | * 114 | * @param[in] context = context struct 115 | * @param [in] slave Slave number. 116 | * @param [in] act TRUE = active, FALSE = deactivated 117 | * @param [in] CyclTime0 Cycltime SYNC0 in ns. 118 | * @param [in] CyclTime1 Cycltime SYNC1 in ns. This time is a delta time in relation to 119 | the SYNC0 fire. If CylcTime1 = 0 then SYNC1 fires a the same time 120 | as SYNC0. 121 | * @param [in] CyclShift CyclShift in ns. 122 | */ 123 | void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift) 124 | { 125 | uint8 h, RA; 126 | uint16 wc, slaveh; 127 | int64 t, t1; 128 | int32 tc; 129 | 130 | slaveh = context->slavelist[slave].configadr; 131 | RA = 0; 132 | 133 | /* stop cyclic operation, ready for next trigger */ 134 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); 135 | if (act) 136 | { 137 | RA = 1 + 2 + 4; /* act cyclic operation and sync0 + sync1 */ 138 | } 139 | h = 0; 140 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCCUC, sizeof(h), &h, EC_TIMEOUTRET); /* write access to ethercat */ 141 | wc = ecx_FPRD(context->port, slaveh, ECT_REG_DCSYSTIME, sizeof(t1), &t1, EC_TIMEOUTRET); /* read local time of slave */ 142 | t1 = etohll(t1); 143 | 144 | /* Calculate first trigger time, always a whole multiple of CyclTime rounded up 145 | plus the shifttime (can be negative) 146 | This insures best sychronisation between slaves, slaves with the same CyclTime 147 | will sync at the same moment (you can use CyclShift to shift the sync) */ 148 | if (CyclTime0 > 0) 149 | { 150 | t = ((t1 + SyncDelay) / CyclTime0) * CyclTime0 + CyclTime0 + CyclShift; 151 | } 152 | else 153 | { 154 | t = t1 + SyncDelay + CyclShift; 155 | /* first trigger at T1 + CyclTime + SyncDelay + CyclShift in ns */ 156 | } 157 | t = htoell(t); 158 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSTART0, sizeof(t), &t, EC_TIMEOUTRET); /* SYNC0 start time */ 159 | tc = htoel(CyclTime0); 160 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE0, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC0 cycle time */ 161 | tc = htoel(CyclTime1); 162 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCCYCLE1, sizeof(tc), &tc, EC_TIMEOUTRET); /* SYNC1 cycle time */ 163 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSYNCACT, sizeof(RA), &RA, EC_TIMEOUTRET); /* activate cyclic operation */ 164 | } 165 | 166 | /* latched port time of slave */ 167 | static int32 ecx_porttime(ecx_contextt *context, uint16 slave, uint8 port) 168 | { 169 | int32 ts; 170 | switch (port) 171 | { 172 | case 0: 173 | ts = context->slavelist[slave].DCrtA; 174 | break; 175 | case 1: 176 | ts = context->slavelist[slave].DCrtB; 177 | break; 178 | case 2: 179 | ts = context->slavelist[slave].DCrtC; 180 | break; 181 | case 3: 182 | ts = context->slavelist[slave].DCrtD; 183 | break; 184 | default: 185 | ts = 0; 186 | } 187 | return ts; 188 | } 189 | 190 | /* calculate previous active port of a slave */ 191 | static uint8 ecx_prevport(ecx_contextt *context, uint16 slave, uint8 port) 192 | { 193 | uint8 pport = port; 194 | uint8 aport = context->slavelist[slave].activeports; 195 | switch(port) 196 | { 197 | case 0: 198 | if(aport & PORTM2) 199 | pport = 2; 200 | else if (aport & PORTM1) 201 | pport = 1; 202 | else if (aport & PORTM2) 203 | pport = 3; 204 | break; 205 | case 1: 206 | if(aport & PORTM3) 207 | pport = 3; 208 | else if (aport & PORTM0) 209 | pport = 0; 210 | else if (aport & PORTM2) 211 | pport = 2; 212 | break; 213 | case 2: 214 | if(aport & PORTM1) 215 | pport = 1; 216 | else if (aport & PORTM3) 217 | pport = 3; 218 | else if (aport & PORTM0) 219 | pport = 0; 220 | break; 221 | case 3: 222 | if(aport & PORTM0) 223 | pport = 0; 224 | else if (aport & PORTM2) 225 | pport = 2; 226 | else if (aport & PORTM1) 227 | pport = 1; 228 | break; 229 | } 230 | return pport; 231 | } 232 | 233 | /* search unconsumed ports in parent, consume and return first open port */ 234 | static uint8 ecx_parentport(ecx_contextt *context, uint16 parent) 235 | { 236 | uint8 parentport = 0; 237 | uint8 b; 238 | /* search order is important, here 3 - 1 - 2 - 0 */ 239 | b = context->slavelist[parent].consumedports; 240 | if (b & PORTM3) 241 | { 242 | parentport = 3; 243 | b &= (uint8)~PORTM3; 244 | } 245 | else if (b & PORTM1) 246 | { 247 | parentport = 1; 248 | b &= (uint8)~PORTM1; 249 | } 250 | else if (b & PORTM2) 251 | { 252 | parentport = 2; 253 | b &= (uint8)~PORTM2; 254 | } 255 | else if (b & PORTM0) 256 | { 257 | parentport = 0; 258 | b &= (uint8)~PORTM0; 259 | } 260 | context->slavelist[parent].consumedports = b; 261 | return parentport; 262 | } 263 | 264 | /** 265 | * Locate DC slaves, measure propagation delays. 266 | * 267 | * @param[in] context = context struct 268 | * @return boolean if slaves are found with DC 269 | */ 270 | boolean ecx_configdc(ecx_contextt *context) 271 | { 272 | uint16 i, wc, slaveh, parent, child; 273 | uint16 parenthold = 0; 274 | uint16 prevDCslave = 0; 275 | int32 ht, dt1, dt2, dt3; 276 | int64 hrt; 277 | uint8 entryport; 278 | int8 nlist; 279 | int8 plist[4]; 280 | int32 tlist[4]; 281 | 282 | context->slavelist[0].hasdc = FALSE; 283 | context->grouplist[0].hasdc = FALSE; 284 | ht = 0; 285 | ecx_BWR(context->port, 0, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); /* latch DCrecvTimeA of all slaves */ 286 | for (i = 1; i <= *(context->slavecount); i++) 287 | { 288 | context->slavelist[i].consumedports = context->slavelist[i].activeports; 289 | if (context->slavelist[i].hasdc) 290 | { 291 | if (!context->slavelist[0].hasdc) 292 | { 293 | context->slavelist[0].hasdc = TRUE; 294 | context->slavelist[0].DCnext = i; 295 | context->slavelist[i].DCprevious = 0; 296 | context->grouplist[0].hasdc = TRUE; 297 | context->grouplist[0].DCnext = i; 298 | } 299 | else 300 | { 301 | context->slavelist[prevDCslave].DCnext = i; 302 | context->slavelist[i].DCprevious = prevDCslave; 303 | } 304 | /* this branch has DC slave so remove parenthold */ 305 | parenthold = 0; 306 | prevDCslave = i; 307 | slaveh = context->slavelist[i].configadr; 308 | wc = ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME0, sizeof(ht), &ht, EC_TIMEOUTRET); 309 | context->slavelist[i].DCrtA = etohl(ht); 310 | /* 64bit latched DCrecvTimeA of each specific slave */ 311 | wc = ecx_FPRD(context->port, slaveh, ECT_REG_DCSOF, sizeof(hrt), &hrt, EC_TIMEOUTRET); 312 | /* use it as offset in order to set local time around 0 */ 313 | hrt = htoell(-etohll(hrt)); 314 | /* save it in the offset register */ 315 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSOFFSET, sizeof(hrt), &hrt, EC_TIMEOUTRET); 316 | wc = ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME1, sizeof(ht), &ht, EC_TIMEOUTRET); 317 | context->slavelist[i].DCrtB = etohl(ht); 318 | wc = ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME2, sizeof(ht), &ht, EC_TIMEOUTRET); 319 | context->slavelist[i].DCrtC = etohl(ht); 320 | wc = ecx_FPRD(context->port, slaveh, ECT_REG_DCTIME3, sizeof(ht), &ht, EC_TIMEOUTRET); 321 | context->slavelist[i].DCrtD = etohl(ht); 322 | 323 | /* make list of active ports and their time stamps */ 324 | nlist = 0; 325 | if (context->slavelist[i].activeports & PORTM0) 326 | { 327 | plist[nlist] = 0; 328 | tlist[nlist] = context->slavelist[i].DCrtA; 329 | nlist++; 330 | } 331 | if (context->slavelist[i].activeports & PORTM3) 332 | { 333 | plist[nlist] = 3; 334 | tlist[nlist] = context->slavelist[i].DCrtD; 335 | nlist++; 336 | } 337 | if (context->slavelist[i].activeports & PORTM1) 338 | { 339 | plist[nlist] = 1; 340 | tlist[nlist] = context->slavelist[i].DCrtB; 341 | nlist++; 342 | } 343 | if (context->slavelist[i].activeports & PORTM2) 344 | { 345 | plist[nlist] = 2; 346 | tlist[nlist] = context->slavelist[i].DCrtC; 347 | nlist++; 348 | } 349 | /* entryport is port with the lowest timestamp */ 350 | entryport = 0; 351 | if((nlist > 1) && (tlist[1] < tlist[entryport])) 352 | { 353 | entryport = 1; 354 | } 355 | if((nlist > 2) && (tlist[2] < tlist[entryport])) 356 | { 357 | entryport = 2; 358 | } 359 | if((nlist > 3) && (tlist[3] < tlist[entryport])) 360 | { 361 | entryport = 3; 362 | } 363 | entryport = plist[entryport]; 364 | context->slavelist[i].entryport = entryport; 365 | /* consume entryport from activeports */ 366 | context->slavelist[i].consumedports &= (uint8)~(1 << entryport); 367 | 368 | /* finding DC parent of current */ 369 | parent = i; 370 | do 371 | { 372 | child = parent; 373 | parent = context->slavelist[parent].parent; 374 | } 375 | while (!((parent == 0) || (context->slavelist[parent].hasdc))); 376 | /* only calculate propagation delay if slave is not the first */ 377 | if (parent > 0) 378 | { 379 | /* find port on parent this slave is connected to */ 380 | context->slavelist[i].parentport = ecx_parentport(context, parent); 381 | if (context->slavelist[parent].topology == 1) 382 | { 383 | context->slavelist[i].parentport = context->slavelist[parent].entryport; 384 | } 385 | 386 | dt1 = 0; 387 | dt2 = 0; 388 | /* delta time of (parentport - 1) - parentport */ 389 | /* note: order of ports is 0 - 3 - 1 -2 */ 390 | /* non active ports are skipped */ 391 | dt3 = ecx_porttime(context, parent, context->slavelist[i].parentport) - 392 | ecx_porttime(context, parent, 393 | ecx_prevport(context, parent, context->slavelist[i].parentport)); 394 | /* current slave has children */ 395 | /* those childrens delays need to be substacted */ 396 | if (context->slavelist[i].topology > 1) 397 | { 398 | dt1 = ecx_porttime(context, i, 399 | ecx_prevport(context, i, context->slavelist[i].entryport)) - 400 | ecx_porttime(context, i, context->slavelist[i].entryport); 401 | } 402 | /* we are only interrested in positive diference */ 403 | if (dt1 > dt3) dt1 = -dt1; 404 | /* current slave is not the first child of parent */ 405 | /* previous childs delays need to be added */ 406 | if ((child - parent) > 1) 407 | { 408 | dt2 = ecx_porttime(context, parent, 409 | ecx_prevport(context, parent, context->slavelist[i].parentport)) - 410 | ecx_porttime(context, parent, context->slavelist[parent].entryport); 411 | } 412 | if (dt2 < 0) dt2 = -dt2; 413 | 414 | /* calculate current slave delay from delta times */ 415 | /* assumption : forward delay equals return delay */ 416 | context->slavelist[i].pdelay = ((dt3 - dt1) / 2) + dt2 + 417 | context->slavelist[parent].pdelay; 418 | ht = htoel(context->slavelist[i].pdelay); 419 | /* write propagation delay*/ 420 | wc = ecx_FPWR(context->port, slaveh, ECT_REG_DCSYSDELAY, sizeof(ht), &ht, EC_TIMEOUTRET); 421 | } 422 | } 423 | else 424 | { 425 | context->slavelist[i].DCrtA = 0; 426 | context->slavelist[i].DCrtB = 0; 427 | context->slavelist[i].DCrtC = 0; 428 | context->slavelist[i].DCrtD = 0; 429 | parent = context->slavelist[i].parent; 430 | /* if non DC slave found on first position on branch hold root parent */ 431 | if ( (parent > 0) && (context->slavelist[parent].topology > 2)) 432 | parenthold = parent; 433 | /* if branch has no DC slaves consume port on root parent */ 434 | if ( parenthold && (context->slavelist[i].topology == 1)) 435 | { 436 | ecx_parentport(context, parenthold); 437 | parenthold = 0; 438 | } 439 | } 440 | } 441 | 442 | return context->slavelist[0].hasdc; 443 | } 444 | 445 | #ifdef EC_VER1 446 | void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift) 447 | { 448 | ecx_dcsync0(&ecx_context, slave, act, CyclTime, CyclShift); 449 | } 450 | 451 | void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift) 452 | { 453 | ecx_dcsync01(&ecx_context, slave, act, CyclTime0, CyclTime1, CyclShift); 454 | } 455 | 456 | boolean ec_configdc(void) 457 | { 458 | return ecx_configdc(&ecx_context); 459 | } 460 | #endif 461 | -------------------------------------------------------------------------------- /soem/ethercatdc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatdc.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Headerfile for ethercatdc.c 44 | */ 45 | 46 | #ifndef _EC_ECATDC_H 47 | #define _EC_ECATDC_H 48 | 49 | #ifdef __cplusplus 50 | extern "C" 51 | { 52 | #endif 53 | 54 | #ifdef EC_VER1 55 | boolean ec_configdc(); 56 | void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift); 57 | void ec_dcsync01(uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift); 58 | #endif 59 | 60 | boolean ecx_configdc(ecx_contextt *context); 61 | void ecx_dcsync0(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime, uint32 CyclShift); 62 | void ecx_dcsync01(ecx_contextt *context, uint16 slave, boolean act, uint32 CyclTime0, uint32 CyclTime1, uint32 CyclShift); 63 | 64 | #ifdef __cplusplus 65 | } 66 | #endif 67 | 68 | #endif /* _EC_ECATDC_H */ 69 | -------------------------------------------------------------------------------- /soem/ethercatfoe.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatfoe.c 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | * 40 | * 14-06-2010 : fixed bug in FOEread() by Torsten Bitterlich 41 | */ 42 | 43 | /** \file 44 | * \brief 45 | * File over EtherCAT (FoE) module. 46 | * 47 | * SDO read / write and SDO service functions 48 | */ 49 | 50 | #include 51 | #include 52 | #include "osal.h" 53 | #include "oshw.h" 54 | #include "ethercattype.h" 55 | #include "ethercatbase.h" 56 | #include "ethercatmain.h" 57 | #include "ethercatfoe.h" 58 | 59 | #define EC_MAXFOEDATA 512 60 | 61 | /** FOE structure. 62 | * Used for Read, Write, Data, Ack and Error mailbox packets. 63 | */ 64 | PACKED_BEGIN 65 | typedef struct PACKED 66 | { 67 | ec_mbxheadert MbxHeader; 68 | uint8 OpCode; 69 | uint8 Reserved; 70 | union 71 | { 72 | uint32 Password; 73 | uint32 PacketNumber; 74 | uint32 ErrorCode; 75 | }; 76 | union 77 | { 78 | char FileName[EC_MAXFOEDATA]; 79 | uint8 Data[EC_MAXFOEDATA]; 80 | char ErrorText[EC_MAXFOEDATA]; 81 | }; 82 | } ec_FOEt; 83 | PACKED_END 84 | 85 | /** FoE progress hook. 86 | * 87 | * @param[in] context = context struct 88 | * @param[in] hook = Pointer to hook function. 89 | * @return 1 90 | */ 91 | int ecx_FOEdefinehook(ecx_contextt *context, void *hook) 92 | { 93 | context->FOEhook = hook; 94 | return 1; 95 | } 96 | 97 | /** FoE read, blocking. 98 | * 99 | * @param[in] context = context struct 100 | * @param[in] slave = Slave number. 101 | * @param[in] filename = Filename of file to read. 102 | * @param[in] password = password. 103 | * @param[in,out] psize = Size in bytes of file buffer, returns bytes read from file. 104 | * @param[out] p = Pointer to file buffer 105 | * @param[in] timeout = Timeout per mailbox cycle in us, standard is EC_TIMEOUTRXM 106 | * @return Workcounter from last slave response 107 | */ 108 | int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout) 109 | { 110 | ec_FOEt *FOEp, *aFOEp; 111 | int wkc; 112 | int32 dataread = 0; 113 | int32 buffersize, packetnumber, prevpacket = 0; 114 | uint16 fnsize, maxdata, segmentdata; 115 | ec_mbxbuft MbxIn, MbxOut; 116 | uint8 cnt; 117 | boolean worktodo; 118 | 119 | buffersize = *psize; 120 | ec_clearmbx(&MbxIn); 121 | /* Empty slave out mailbox if something is in. Timout set to 0 */ 122 | wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); 123 | ec_clearmbx(&MbxOut); 124 | aFOEp = (ec_FOEt *)&MbxIn; 125 | FOEp = (ec_FOEt *)&MbxOut; 126 | fnsize = strlen(filename); 127 | maxdata = context->slavelist[slave].mbx_l - 12; 128 | if (fnsize > maxdata) 129 | { 130 | fnsize = maxdata; 131 | } 132 | FOEp->MbxHeader.length = htoes(0x0006 + fnsize); 133 | FOEp->MbxHeader.address = htoes(0x0000); 134 | FOEp->MbxHeader.priority = 0x00; 135 | /* get new mailbox count value, used as session handle */ 136 | cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 137 | context->slavelist[slave].mbx_cnt = cnt; 138 | FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */ 139 | FOEp->OpCode = ECT_FOE_READ; 140 | FOEp->Password = htoel(password); 141 | /* copy filename in mailbox */ 142 | memcpy(&FOEp->FileName[0], filename, fnsize); 143 | /* send FoE request to slave */ 144 | wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 145 | if (wkc > 0) /* succeeded to place mailbox in slave ? */ 146 | { 147 | do 148 | { 149 | worktodo = FALSE; 150 | /* clean mailboxbuffer */ 151 | ec_clearmbx(&MbxIn); 152 | /* read slave response */ 153 | wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); 154 | if (wkc > 0) /* succeeded to read slave response ? */ 155 | { 156 | /* slave response should be FoE */ 157 | if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE) 158 | { 159 | if(aFOEp->OpCode == ECT_FOE_DATA) 160 | { 161 | segmentdata = etohs(aFOEp->MbxHeader.length) - 0x0006; 162 | packetnumber = etohl(aFOEp->PacketNumber); 163 | if ((packetnumber == ++prevpacket) && (dataread + segmentdata <= buffersize)) 164 | { 165 | memcpy(p, &aFOEp->Data[0], segmentdata); 166 | dataread += segmentdata; 167 | p = (uint8 *)p + segmentdata; 168 | if (segmentdata == maxdata) 169 | { 170 | worktodo = TRUE; 171 | } 172 | FOEp->MbxHeader.length = htoes(0x0006); 173 | FOEp->MbxHeader.address = htoes(0x0000); 174 | FOEp->MbxHeader.priority = 0x00; 175 | /* get new mailbox count value */ 176 | cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 177 | context->slavelist[slave].mbx_cnt = cnt; 178 | FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */ 179 | FOEp->OpCode = ECT_FOE_ACK; 180 | FOEp->PacketNumber = htoel(packetnumber); 181 | /* send FoE ack to slave */ 182 | wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 183 | if (wkc <= 0) 184 | { 185 | worktodo = FALSE; 186 | } 187 | if (context->FOEhook) 188 | { 189 | context->FOEhook(slave, packetnumber, dataread); 190 | } 191 | } 192 | else 193 | { 194 | /* FoE error */ 195 | wkc = -EC_ERR_TYPE_FOE_BUF2SMALL; 196 | } 197 | } 198 | else 199 | { 200 | if(aFOEp->OpCode == ECT_FOE_ERROR) 201 | { 202 | /* FoE error */ 203 | wkc = -EC_ERR_TYPE_FOE_ERROR; 204 | } 205 | else 206 | { 207 | /* unexpected mailbox received */ 208 | wkc = -EC_ERR_TYPE_PACKET_ERROR; 209 | } 210 | } 211 | } 212 | else 213 | { 214 | /* unexpected mailbox received */ 215 | wkc = -EC_ERR_TYPE_PACKET_ERROR; 216 | } 217 | *psize = dataread; 218 | } 219 | } while (worktodo); 220 | } 221 | 222 | return wkc; 223 | } 224 | 225 | /** FoE write, blocking. 226 | * 227 | * @param[in] context = context struct 228 | * @param[in] slave = Slave number. 229 | * @param[in] filename = Filename of file to write. 230 | * @param[in] password = password. 231 | * @param[in] psize = Size in bytes of file buffer. 232 | * @param[out] p = Pointer to file buffer 233 | * @param[in] timeout = Timeout per mailbox cycle in us, standard is EC_TIMEOUTRXM 234 | * @return Workcounter from last slave response 235 | */ 236 | int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout) 237 | { 238 | ec_FOEt *FOEp, *aFOEp; 239 | int wkc; 240 | int32 packetnumber, sendpacket = 0; 241 | uint16 fnsize, maxdata; 242 | int segmentdata; 243 | ec_mbxbuft MbxIn, MbxOut; 244 | uint8 cnt; 245 | boolean worktodo, dofinalzero; 246 | int tsize; 247 | 248 | ec_clearmbx(&MbxIn); 249 | /* Empty slave out mailbox if something is in. Timout set to 0 */ 250 | wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); 251 | ec_clearmbx(&MbxOut); 252 | aFOEp = (ec_FOEt *)&MbxIn; 253 | FOEp = (ec_FOEt *)&MbxOut; 254 | dofinalzero = FALSE; 255 | fnsize = strlen(filename); 256 | maxdata = context->slavelist[slave].mbx_l - 12; 257 | if (fnsize > maxdata) 258 | { 259 | fnsize = maxdata; 260 | } 261 | FOEp->MbxHeader.length = htoes(0x0006 + fnsize); 262 | FOEp->MbxHeader.address = htoes(0x0000); 263 | FOEp->MbxHeader.priority = 0x00; 264 | /* get new mailbox count value, used as session handle */ 265 | cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 266 | context->slavelist[slave].mbx_cnt = cnt; 267 | FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */ 268 | FOEp->OpCode = ECT_FOE_WRITE; 269 | FOEp->Password = htoel(password); 270 | /* copy filename in mailbox */ 271 | memcpy(&FOEp->FileName[0], filename, fnsize); 272 | /* send FoE request to slave */ 273 | wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 274 | if (wkc > 0) /* succeeded to place mailbox in slave ? */ 275 | { 276 | do 277 | { 278 | worktodo = FALSE; 279 | /* clean mailboxbuffer */ 280 | ec_clearmbx(&MbxIn); 281 | /* read slave response */ 282 | wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); 283 | if (wkc > 0) /* succeeded to read slave response ? */ 284 | { 285 | /* slave response should be FoE */ 286 | if ((aFOEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_FOE) 287 | { 288 | switch (aFOEp->OpCode) 289 | { 290 | case ECT_FOE_ACK: 291 | { 292 | packetnumber = etohl(aFOEp->PacketNumber); 293 | if (packetnumber == sendpacket) 294 | { 295 | if (context->FOEhook) 296 | { 297 | context->FOEhook(slave, packetnumber, psize); 298 | } 299 | tsize = psize; 300 | if (tsize > maxdata) 301 | { 302 | tsize = maxdata; 303 | } 304 | if(tsize || dofinalzero) 305 | { 306 | worktodo = TRUE; 307 | dofinalzero = FALSE; 308 | segmentdata = tsize; 309 | psize -= segmentdata; 310 | /* if last packet was full size, add a zero size packet as final */ 311 | /* EOF is defined as packetsize < full packetsize */ 312 | if (!psize && (segmentdata == maxdata)) 313 | { 314 | dofinalzero = TRUE; 315 | } 316 | FOEp->MbxHeader.length = htoes(0x0006 + segmentdata); 317 | FOEp->MbxHeader.address = htoes(0x0000); 318 | FOEp->MbxHeader.priority = 0x00; 319 | /* get new mailbox count value */ 320 | cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 321 | context->slavelist[slave].mbx_cnt = cnt; 322 | FOEp->MbxHeader.mbxtype = ECT_MBXT_FOE + (cnt << 4); /* FoE */ 323 | FOEp->OpCode = ECT_FOE_DATA; 324 | sendpacket++; 325 | FOEp->PacketNumber = htoel(sendpacket); 326 | memcpy(&FOEp->Data[0], p, segmentdata); 327 | p = (uint8 *)p + segmentdata; 328 | /* send FoE data to slave */ 329 | wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 330 | if (wkc <= 0) 331 | { 332 | worktodo = FALSE; 333 | } 334 | } 335 | } 336 | else 337 | { 338 | /* FoE error */ 339 | wkc = -EC_ERR_TYPE_FOE_PACKETNUMBER; 340 | } 341 | break; 342 | } 343 | case ECT_FOE_BUSY: 344 | { 345 | /* resend if data has been send before */ 346 | /* otherwise ignore */ 347 | if (sendpacket) 348 | { 349 | if (!psize) 350 | { 351 | dofinalzero = TRUE; 352 | } 353 | psize += segmentdata; 354 | p = (uint8 *)p - segmentdata; 355 | --sendpacket; 356 | } 357 | break; 358 | } 359 | case ECT_FOE_ERROR: 360 | { 361 | /* FoE error */ 362 | wkc = -EC_ERR_TYPE_FOE_ERROR; 363 | break; 364 | } 365 | default: 366 | { 367 | /* unexpected mailbox received */ 368 | wkc = -EC_ERR_TYPE_PACKET_ERROR; 369 | } 370 | } 371 | } 372 | else 373 | { 374 | /* unexpected mailbox received */ 375 | wkc = -EC_ERR_TYPE_PACKET_ERROR; 376 | } 377 | } 378 | } while (worktodo); 379 | } 380 | 381 | return wkc; 382 | } 383 | 384 | #ifdef EC_VER1 385 | int ec_FOEdefinehook(void *hook) 386 | { 387 | return ecx_FOEdefinehook(&ecx_context, hook); 388 | } 389 | 390 | int ec_FOEread(uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout) 391 | { 392 | return ecx_FOEread(&ecx_context, slave, filename, password, psize, p, timeout); 393 | } 394 | 395 | int ec_FOEwrite(uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout) 396 | { 397 | return ecx_FOEwrite(&ecx_context, slave, filename, password, psize, p, timeout); 398 | } 399 | #endif 400 | -------------------------------------------------------------------------------- /soem/ethercatfoe.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatfoe.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Headerfile for ethercatfoe.c 44 | */ 45 | 46 | #ifndef _ethercatfoe_ 47 | #define _ethercatfoe_ 48 | 49 | #ifdef __cplusplus 50 | extern "C" 51 | { 52 | #endif 53 | 54 | #ifdef EC_VER1 55 | int ec_FOEdefinehook(void *hook); 56 | int ec_FOEread(uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout); 57 | int ec_FOEwrite(uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout); 58 | #endif 59 | 60 | int ecx_FOEdefinehook(ecx_contextt *context, void *hook); 61 | int ecx_FOEread(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int *psize, void *p, int timeout); 62 | int ecx_FOEwrite(ecx_contextt *context, uint16 slave, char *filename, uint32 password, int psize, void *p, int timeout); 63 | 64 | #ifdef __cplusplus 65 | } 66 | #endif 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /soem/ethercatprint.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatprint.c 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo EtherCAT are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Module to convert EtherCAT errors to readable messages. 44 | * 45 | * SDO abort messages and AL status codes are used to relay slave errors to 46 | * the user application. This module converts the binary codes to readble text. 47 | * For the defined error codes see the EtherCAT protocol documentation. 48 | */ 49 | 50 | #include 51 | #include "oshw.h" 52 | #include "ethercattype.h" 53 | #include "ethercatmain.h" 54 | 55 | #define EC_MAXERRORNAME 127 56 | 57 | /** SDO error list type definition */ 58 | typedef struct 59 | { 60 | /** Error code returned from SDO */ 61 | uint32 errorcode; 62 | /** Readable error description */ 63 | char errordescription[EC_MAXERRORNAME + 1]; 64 | } ec_sdoerrorlist_t; 65 | 66 | /** AL status code list type definition */ 67 | typedef struct 68 | { 69 | /** AL status code */ 70 | uint16 ALstatuscode; 71 | /** Readable description */ 72 | char ALstatuscodedescription[EC_MAXERRORNAME + 1]; 73 | } ec_ALstatuscodelist_t; 74 | 75 | /** SoE error list type definition */ 76 | typedef struct 77 | { 78 | /** SoE error code */ 79 | uint16 errorcode; 80 | /** Readable description */ 81 | char errordescription[EC_MAXERRORNAME + 1]; 82 | } ec_soeerrorlist_t; 83 | 84 | /** MBX error list type definition */ 85 | typedef struct 86 | { 87 | /** MBX error code */ 88 | uint16 errorcode; 89 | /** Readable description */ 90 | char errordescription[EC_MAXERRORNAME + 1]; 91 | } ec_mbxerrorlist_t; 92 | 93 | char estring[EC_MAXERRORNAME]; 94 | 95 | /** SDO error list definition */ 96 | const ec_sdoerrorlist_t ec_sdoerrorlist[] = { 97 | {0x00000000, "No error" }, 98 | {0x05030000, "Toggle bit not changed" }, 99 | {0x05040000, "SDO protocol timeout" }, 100 | {0x05040001, "Client/Server command specifier not valid or unknown" }, 101 | {0x05040005, "Out of memory" }, 102 | {0x06010000, "Unsupported access to an object" }, 103 | {0x06010001, "Attempt to read to a write only object" }, 104 | {0x06010002, "Attempt to write to a read only object" }, 105 | {0x06010003, "Subindex can not be written, SI0 must be 0 for write access" }, 106 | {0x06010004, "SDO Complete access not supported for variable length objects" }, 107 | {0x06010005, "Object length exceeds mailbox size" }, 108 | {0x06010006, "Object mapped to RxPDO, SDO download blocked" }, 109 | {0x06020000, "The object does not exist in the object directory" }, 110 | {0x06040041, "The object can not be mapped into the PDO" }, 111 | {0x06040042, "The number and length of the objects to be mapped would exceed the PDO length" }, 112 | {0x06040043, "General parameter incompatibility reason" }, 113 | {0x06040047, "General internal incompatibility in the device" }, 114 | {0x06060000, "Access failed due to a hardware error" }, 115 | {0x06070010, "Data type does not match, length of service parameter does not match" }, 116 | {0x06070012, "Data type does not match, length of service parameter too high" }, 117 | {0x06070013, "Data type does not match, length of service parameter too low" }, 118 | {0x06090011, "Subindex does not exist" }, 119 | {0x06090030, "Value range of parameter exceeded (only for write access)" }, 120 | {0x06090031, "Value of parameter written too high" }, 121 | {0x06090032, "Value of parameter written too low" }, 122 | {0x06090036, "Maximum value is less than minimum value" }, 123 | {0x08000000, "General error" }, 124 | {0x08000020, "Data cannot be transferred or stored to the application" }, 125 | {0x08000021, "Data cannot be transferred or stored to the application because of local control" }, 126 | {0x08000022, "Data cannot be transferred or stored to the application because of the present device state" }, 127 | {0x08000023, "Object dictionary dynamic generation fails or no object dictionary is present" }, 128 | {0xffffffff, "Unknown" } 129 | }; 130 | 131 | /** AL status code list definition */ 132 | const ec_ALstatuscodelist_t ec_ALstatuscodelist[] = { 133 | {0x0000 , "No error" }, 134 | {0x0001 , "Unspecified error" }, 135 | {0x0002 , "No memory" }, 136 | {0x0011 , "Invalid requested state change" }, 137 | {0x0012 , "Unknown requested state" }, 138 | {0x0013 , "Bootstrap not supported" }, 139 | {0x0014 , "No valid firmware" }, 140 | {0x0015 , "Invalid mailbox configuration" }, 141 | {0x0016 , "Invalid mailbox configuration" }, 142 | {0x0017 , "Invalid sync manager configuration" }, 143 | {0x0018 , "No valid inputs available" }, 144 | {0x0019 , "No valid outputs" }, 145 | {0x001A , "Synchronization error" }, 146 | {0x001B , "Sync manager watchdog" }, 147 | {0x001C , "Invalid sync Manager types" }, 148 | {0x001D , "Invalid output configuration" }, 149 | {0x001E , "Invalid input configuration" }, 150 | {0x001F , "Invalid watchdog configuration" }, 151 | {0x0020 , "Slave needs cold start" }, 152 | {0x0021 , "Slave needs INIT" }, 153 | {0x0022 , "Slave needs PREOP" }, 154 | {0x0023 , "Slave needs SAFEOP" }, 155 | {0x0024 , "Invalid input mapping" }, 156 | {0x0025 , "Invalid output mapping" }, 157 | {0x0026 , "Inconsistent settings" }, 158 | {0x0027 , "Freerun not supported" }, 159 | {0x0028 , "Synchronisation not supported" }, 160 | {0x0029 , "Freerun needs 3buffer mode" }, 161 | {0x002A , "Background watchdog" }, 162 | {0x002B , "No valid Inputs and Outputs" }, 163 | {0x002C , "Fatal sync error" }, 164 | {0x002D , "No sync error" }, // was "Invalid Output FMMU Configuration" 165 | {0x002E , "Invalid input FMMU configuration" }, 166 | {0x0030 , "Invalid DC SYNC configuration" }, 167 | {0x0031 , "Invalid DC latch configuration" }, 168 | {0x0032 , "PLL error" }, 169 | {0x0033 , "DC sync IO error" }, 170 | {0x0034 , "DC sync timeout error" }, 171 | {0x0035 , "DC invalid sync cycle time" }, 172 | {0x0035 , "DC invalid sync0 cycle time" }, 173 | {0x0035 , "DC invalid sync1 cycle time" }, 174 | {0x0042 , "MBX_EOE" }, 175 | {0x0043 , "MBX_COE" }, 176 | {0x0044 , "MBX_FOE" }, 177 | {0x0045 , "MBX_SOE" }, 178 | {0x004F , "MBX_VOE" }, 179 | {0x0050 , "EEPROM no access" }, 180 | {0x0051 , "EEPROM error" }, 181 | {0x0060 , "Slave restarted locally" }, 182 | {0x0061 , "Device identification value updated" }, 183 | {0x00f0 , "Application controller available" }, 184 | {0xffff , "Unknown" } 185 | }; 186 | 187 | /** SoE error list definition */ 188 | const ec_soeerrorlist_t ec_soeerrorlist[] = { 189 | {0x0000, "No error" }, 190 | {0x1001, "No IDN" }, 191 | {0x1009, "Invalid access to element 1" }, 192 | {0x2001, "No Name" }, 193 | {0x2002, "Name transmission too short" }, 194 | {0x2003, "Name transmission too long" }, 195 | {0x2004, "Name cannot be changed (read only)" }, 196 | {0x2005, "Name is write-protected at this time" }, 197 | {0x3002, "Attribute transmission too short" }, 198 | {0x3003, "Attribute transmission too long" }, 199 | {0x3004, "Attribute cannot be changed (read only)" }, 200 | {0x3005, "Attribute is write-protected at this time" }, 201 | {0x4001, "No units" }, 202 | {0x4002, "Unit transmission too short" }, 203 | {0x4003, "Unit transmission too long" }, 204 | {0x4004, "Unit cannot be changed (read only)" }, 205 | {0x4005, "Unit is write-protected at this time" }, 206 | {0x5001, "No minimum input value" }, 207 | {0x5002, "Minimum input value transmission too short" }, 208 | {0x5003, "Minimum input value transmission too long" }, 209 | {0x5004, "Minimum input value cannot be changed (read only)" }, 210 | {0x5005, "Minimum input value is write-protected at this time" }, 211 | {0x6001, "No maximum input value" }, 212 | {0x6002, "Maximum input value transmission too short" }, 213 | {0x6003, "Maximum input value transmission too long" }, 214 | {0x6004, "Maximum input value cannot be changed (read only)" }, 215 | {0x6005, "Maximum input value is write-protected at this time" }, 216 | {0x7002, "Operation data transmission too short" }, 217 | {0x7003, "Operation data transmission too long" }, 218 | {0x7004, "Operation data cannot be changed (read only)" }, 219 | {0x7005, "Operation data is write-protected at this time (state)" }, 220 | {0x7006, "Operation data is smaller than the minimum input value" }, 221 | {0x7007, "Operation data is smaller than the maximum input value" }, 222 | {0x7008, "Invalid operation data:Configured IDN will not be supported" }, 223 | {0x7009, "Operation data write protected by a password" }, 224 | {0x700A, "Operation data is write protected, it is configured cyclically" }, 225 | {0x700B, "Invalid indirect addressing: (e.g., data container, list handling)" }, 226 | {0x700C, "Operation data is write protected, due to other settings" }, 227 | {0x700D, "Reserved" }, 228 | {0x7010, "Procedure command already active" }, 229 | {0x7011, "Procedure command not interruptible" }, 230 | {0x7012, "Procedure command at this time not executable (state)" }, 231 | {0x7013, "Procedure command not executable (invalid or false parameters)" }, 232 | {0x7014, "No data state" }, 233 | {0x8001, "No default value" }, 234 | {0x8002, "Default value transmission too long" }, 235 | {0x8004, "Default value cannot be changed, read only" }, 236 | {0x800A, "Invalid drive number" }, 237 | {0x800A, "General error" }, 238 | {0x800A, "No element addressed" }, 239 | {0xffff, "Unknown" } 240 | }; 241 | 242 | /** MBX error list definition */ 243 | const ec_mbxerrorlist_t ec_mbxerrorlist[] = { 244 | {0x0000, "No error" }, 245 | {0x0001, "Syntax of 6 octet Mailbox Header is wrong" }, 246 | {0x0002, "The mailbox protocol is not supported" }, 247 | {0x0003, "Channel Field contains wrong value"}, 248 | {0x0004, "The service is no supported"}, 249 | {0x0005, "Invalid mailbox header"}, 250 | {0x0006, "Length of received mailbox data is too short"}, 251 | {0x0007, "No more memory in slave"}, 252 | {0x0008, "The lenght of data is inconsistent"}, 253 | {0xffff, "Unknown"} 254 | }; 255 | 256 | /** Look up text string that belongs to SDO error code. 257 | * 258 | * @param[in] sdoerrorcode = SDO error code as defined in EtherCAT protocol 259 | * @return readable string 260 | */ 261 | char* ec_sdoerror2string( uint32 sdoerrorcode) 262 | { 263 | int i = 0; 264 | 265 | while ( (ec_sdoerrorlist[i].errorcode != 0xfffffffful) && 266 | (ec_sdoerrorlist[i].errorcode != sdoerrorcode) ) 267 | { 268 | i++; 269 | } 270 | 271 | return (char*) ec_sdoerrorlist[i].errordescription; 272 | } 273 | 274 | /** Look up text string that belongs to AL status code. 275 | * 276 | * @param[in] ALstatuscode = AL status code as defined in EtherCAT protocol 277 | * @return readable string 278 | */ 279 | char* ec_ALstatuscode2string( uint16 ALstatuscode) 280 | { 281 | int i = 0; 282 | 283 | while ( (ec_ALstatuscodelist[i].ALstatuscode != 0xffff) && 284 | (ec_ALstatuscodelist[i].ALstatuscode != ALstatuscode) ) 285 | { 286 | i++; 287 | } 288 | 289 | return (char *) ec_ALstatuscodelist[i].ALstatuscodedescription; 290 | } 291 | 292 | /** Look up text string that belongs to SoE error code. 293 | * 294 | * @param[in] errorcode = SoE error code as defined in EtherCAT protocol 295 | * @return readable string 296 | */ 297 | char* ec_soeerror2string( uint16 errorcode) 298 | { 299 | int i = 0; 300 | 301 | while ( (ec_soeerrorlist[i].errorcode != 0xffff) && 302 | (ec_soeerrorlist[i].errorcode != errorcode) ) 303 | { 304 | i++; 305 | } 306 | 307 | return (char *) ec_soeerrorlist[i].errordescription; 308 | } 309 | 310 | /** Look up text string that belongs to MBX error code. 311 | * 312 | * @param[in] errorcode = MBX error code as defined in EtherCAT protocol 313 | * @return readable string 314 | */ 315 | char* ec_mbxerror2string( uint16 errorcode) 316 | { 317 | int i = 0; 318 | 319 | while ( (ec_mbxerrorlist[i].errorcode != 0xffff) && 320 | (ec_mbxerrorlist[i].errorcode != errorcode) ) 321 | { 322 | i++; 323 | } 324 | 325 | return (char *) ec_mbxerrorlist[i].errordescription; 326 | } 327 | 328 | /** Look up error in ec_errorlist and convert to text string. 329 | * 330 | * @param[in] context = context struct 331 | * @return readable string 332 | */ 333 | char* ecx_elist2string(ecx_contextt *context) 334 | { 335 | ec_errort Ec; 336 | char timestr[20]; 337 | 338 | if (ecx_poperror(context, &Ec)) 339 | { 340 | sprintf(timestr, "Time:%12.3f", Ec.Time.sec + (Ec.Time.usec / 1000000.0) ); 341 | switch (Ec.Etype) 342 | { 343 | case EC_ERR_TYPE_SDO_ERROR: 344 | { 345 | sprintf(estring, "%s SDO slave:%d index:%4.4x.%2.2x error:%8.8x %s\n", 346 | timestr, Ec.Slave, Ec.Index, Ec.SubIdx, Ec.AbortCode, ec_sdoerror2string(Ec.AbortCode)); 347 | break; 348 | } 349 | case EC_ERR_TYPE_EMERGENCY: 350 | { 351 | sprintf(estring, "%s EMERGENCY slave:%d error:%4.4x\n", 352 | timestr, Ec.Slave, Ec.ErrorCode); 353 | break; 354 | } 355 | case EC_ERR_TYPE_PACKET_ERROR: 356 | { 357 | sprintf(estring, "%s PACKET slave:%d index:%4.4x.%2.2x error:%d\n", 358 | timestr, Ec.Slave, Ec.Index, Ec.SubIdx, Ec.ErrorCode); 359 | break; 360 | } 361 | case EC_ERR_TYPE_SDOINFO_ERROR: 362 | { 363 | sprintf(estring, "%s SDO slave:%d index:%4.4x.%2.2x error:%8.8x %s\n", 364 | timestr, Ec.Slave, Ec.Index, Ec.SubIdx, Ec.AbortCode, ec_sdoerror2string(Ec.AbortCode)); 365 | break; 366 | } 367 | case EC_ERR_TYPE_SOE_ERROR: 368 | { 369 | sprintf(estring, "%s SoE slave:%d IDN:%4.4x error:%4.4x %s\n", 370 | timestr, Ec.Slave, Ec.Index, Ec.AbortCode, ec_soeerror2string(Ec.ErrorCode)); 371 | break; 372 | } 373 | case EC_ERR_TYPE_MBX_ERROR: 374 | { 375 | sprintf(estring, "%s MBX slave:%d error:%4.4x %s\n", 376 | timestr, Ec.Slave, Ec.ErrorCode, ec_mbxerror2string(Ec.ErrorCode)); 377 | break; 378 | } 379 | default: 380 | { 381 | sprintf(estring, "%s error:%8.8x\n", 382 | timestr, Ec.AbortCode); 383 | } 384 | } 385 | return (char*) estring; 386 | } 387 | else 388 | { 389 | return ""; 390 | } 391 | } 392 | 393 | #ifdef EC_VER1 394 | char* ec_elist2string(void) 395 | { 396 | return ecx_elist2string(&ecx_context); 397 | } 398 | #endif 399 | -------------------------------------------------------------------------------- /soem/ethercatprint.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatprint.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Headerfile for ethercatprint.c 44 | */ 45 | 46 | #ifndef _ethercatprint_ 47 | #define _ethercatprint_ 48 | 49 | #ifdef __cplusplus 50 | extern "C" 51 | { 52 | #endif 53 | 54 | char* ec_sdoerror2string( uint16 sdoerrorcode); 55 | char* ec_ALstatuscode2string( uint16 ALstatuscode); 56 | char* ec_soeerror2string( uint16 errorcode); 57 | char* ecx_elist2string(ecx_contextt *context); 58 | 59 | #ifdef EC_VER1 60 | char* ec_elist2string(void); 61 | #endif 62 | 63 | #ifdef __cplusplus 64 | } 65 | #endif 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /soem/ethercatsoe.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatsoe.c 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * Thanks to Hidde Verhoef for testing and improving the SoE module 11 | * 12 | * SOEM is free software; you can redistribute it and/or modify it under 13 | * the terms of the GNU General Public License version 2 as published by the Free 14 | * Software Foundation. 15 | * 16 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 17 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 18 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 19 | * for more details. 20 | * 21 | * As a special exception, if other files instantiate templates or use macros 22 | * or inline functions from this file, or you compile this file and link it 23 | * with other works to produce a work based on this file, this file does not 24 | * by itself cause the resulting work to be covered by the GNU General Public 25 | * License. However the source code for this file must still be made available 26 | * in accordance with section (3) of the GNU General Public License. 27 | * 28 | * This exception does not invalidate any other reasons why a work based on 29 | * this file might be covered by the GNU General Public License. 30 | * 31 | * The EtherCAT Technology, the trade name and logo EtherCAT are the intellectual 32 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 33 | * the sole purpose of creating, using and/or selling or otherwise distributing 34 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 35 | * from Beckhoff Automation GmbH. 36 | * 37 | * In case you did not receive a copy of the EtherCAT Master License along with 38 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 39 | * (www.beckhoff.com). 40 | */ 41 | 42 | /** \file 43 | * \brief 44 | * Servo over EtherCAT (SoE) Module. 45 | */ 46 | 47 | #include 48 | #include 49 | #include "osal.h" 50 | #include "oshw.h" 51 | #include "ethercattype.h" 52 | #include "ethercatbase.h" 53 | #include "ethercatmain.h" 54 | #include "ethercatsoe.h" 55 | 56 | 57 | /** SoE (Servo over EtherCAT) mailbox structure */ 58 | PACKED_BEGIN 59 | typedef struct PACKED 60 | { 61 | ec_mbxheadert MbxHeader; 62 | uint8 opCode :3; 63 | uint8 incomplete :1; 64 | uint8 error :1; 65 | uint8 driveNo :3; 66 | uint8 elementflags; 67 | union 68 | { 69 | uint16 idn; 70 | uint16 fragmentsleft; 71 | }; 72 | } ec_SoEt; 73 | PACKED_END 74 | 75 | /** Report SoE error. 76 | * 77 | * @param[in] context = context struct 78 | * @param[in] Slave = Slave number 79 | * @param[in] idn = IDN that generated error 80 | * @param[in] Error = Error code, see EtherCAT documentation for list 81 | */ 82 | void ecx_SoEerror(ecx_contextt *context, uint16 Slave, uint16 idn, uint16 Error) 83 | { 84 | ec_errort Ec; 85 | 86 | Ec.Time = osal_current_time(); 87 | Ec.Slave = Slave; 88 | Ec.Index = idn; 89 | Ec.SubIdx = 0; 90 | *(context->ecaterror) = TRUE; 91 | Ec.Etype = EC_ERR_TYPE_SOE_ERROR; 92 | Ec.ErrorCode = Error; 93 | ecx_pusherror(context, &Ec); 94 | } 95 | 96 | /** SoE read, blocking. 97 | * 98 | * The IDN object of the selected slave and DriveNo is read. If a response 99 | * is larger than the mailbox size then the response is segmented. The function 100 | * will combine all segments and copy them to the parameter buffer. 101 | * 102 | * @param[in] context = context struct 103 | * @param[in] slave = Slave number 104 | * @param[in] driveNo = Drive number in slave 105 | * @param[in] elementflags = Flags to select what properties of IDN are to be transfered. 106 | * @param[in] idn = IDN. 107 | * @param[in,out] psize = Size in bytes of parameter buffer, returns bytes read from SoE. 108 | * @param[out] p = Pointer to parameter buffer 109 | * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM 110 | * @return Workcounter from last slave response 111 | */ 112 | int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout) 113 | { 114 | ec_SoEt *SoEp, *aSoEp; 115 | uint16 totalsize, framedatasize; 116 | int wkc; 117 | uint8 *bp; 118 | uint8 *mp; 119 | uint16 *errorcode; 120 | ec_mbxbuft MbxIn, MbxOut; 121 | uint8 cnt; 122 | boolean NotLast; 123 | 124 | ec_clearmbx(&MbxIn); 125 | /* Empty slave out mailbox if something is in. Timeout set to 0 */ 126 | wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); 127 | ec_clearmbx(&MbxOut); 128 | aSoEp = (ec_SoEt *)&MbxIn; 129 | SoEp = (ec_SoEt *)&MbxOut; 130 | SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert)); 131 | SoEp->MbxHeader.address = htoes(0x0000); 132 | SoEp->MbxHeader.priority = 0x00; 133 | /* get new mailbox count value, used as session handle */ 134 | cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 135 | context->slavelist[slave].mbx_cnt = cnt; 136 | SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */ 137 | SoEp->opCode = ECT_SOE_READREQ; 138 | SoEp->incomplete = 0; 139 | SoEp->error = 0; 140 | SoEp->driveNo = driveNo; 141 | SoEp->elementflags = elementflags; 142 | SoEp->idn = htoes(idn); 143 | totalsize = 0; 144 | bp = p; 145 | mp = (uint8 *)&MbxIn + sizeof(ec_SoEt); 146 | NotLast = TRUE; 147 | /* send SoE request to slave */ 148 | wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 149 | if (wkc > 0) /* succeeded to place mailbox in slave ? */ 150 | { 151 | while (NotLast) 152 | { 153 | /* clean mailboxbuffer */ 154 | ec_clearmbx(&MbxIn); 155 | /* read slave response */ 156 | wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); 157 | if (wkc > 0) /* succeeded to read slave response ? */ 158 | { 159 | /* slave response should be SoE, ReadRes */ 160 | if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && 161 | (aSoEp->opCode == ECT_SOE_READRES) && 162 | (aSoEp->error == 0) && 163 | (aSoEp->driveNo == driveNo) && 164 | (aSoEp->elementflags == elementflags)) 165 | { 166 | framedatasize = etohs(aSoEp->MbxHeader.length) - sizeof(ec_SoEt) + sizeof(ec_mbxheadert); 167 | totalsize += framedatasize; 168 | /* Does parameter fit in parameter buffer ? */ 169 | if (totalsize <= *psize) 170 | { 171 | /* copy parameter data in parameter buffer */ 172 | memcpy(bp, mp, framedatasize); 173 | /* increment buffer pointer */ 174 | bp += framedatasize; 175 | } 176 | else 177 | { 178 | framedatasize -= totalsize - *psize; 179 | totalsize = *psize; 180 | /* copy parameter data in parameter buffer */ 181 | if (framedatasize > 0) memcpy(bp, mp, framedatasize); 182 | } 183 | 184 | if (!aSoEp->incomplete) 185 | { 186 | NotLast = FALSE; 187 | *psize = totalsize; 188 | } 189 | } 190 | /* other slave response */ 191 | else 192 | { 193 | NotLast = FALSE; 194 | if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && 195 | (aSoEp->opCode == ECT_SOE_READRES) && 196 | (aSoEp->error == 1)) 197 | { 198 | mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16)); 199 | errorcode = (uint16 *)mp; 200 | ecx_SoEerror(context, slave, idn, *errorcode); 201 | } 202 | else 203 | { 204 | ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */ 205 | } 206 | wkc = 0; 207 | } 208 | } 209 | else 210 | { 211 | NotLast = FALSE; 212 | ecx_packeterror(context, slave, idn, 0, 4); /* no response */ 213 | } 214 | } 215 | } 216 | return wkc; 217 | } 218 | 219 | /** SoE write, blocking. 220 | * 221 | * The IDN object of the selected slave and DriveNo is written. If a response 222 | * is larger than the mailbox size then the response is segmented. 223 | * 224 | * @param[in] context = context struct 225 | * @param[in] slave = Slave number 226 | * @param[in] driveNo = Drive number in slave 227 | * @param[in] elementflags = Flags to select what properties of IDN are to be transfered. 228 | * @param[in] idn = IDN. 229 | * @param[in] psize = Size in bytes of parameter buffer. 230 | * @param[out] p = Pointer to parameter buffer 231 | * @param[in] timeout = Timeout in us, standard is EC_TIMEOUTRXM 232 | * @return Workcounter from last slave response 233 | */ 234 | int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout) 235 | { 236 | ec_SoEt *SoEp, *aSoEp; 237 | uint16 framedatasize, maxdata; 238 | int wkc; 239 | uint8 *mp; 240 | uint8 *hp; 241 | uint16 *errorcode; 242 | ec_mbxbuft MbxIn, MbxOut; 243 | uint8 cnt; 244 | boolean NotLast; 245 | 246 | ec_clearmbx(&MbxIn); 247 | /* Empty slave out mailbox if something is in. Timeout set to 0 */ 248 | wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, 0); 249 | ec_clearmbx(&MbxOut); 250 | aSoEp = (ec_SoEt *)&MbxIn; 251 | SoEp = (ec_SoEt *)&MbxOut; 252 | SoEp->MbxHeader.address = htoes(0x0000); 253 | SoEp->MbxHeader.priority = 0x00; 254 | SoEp->opCode = ECT_SOE_WRITEREQ; 255 | SoEp->error = 0; 256 | SoEp->driveNo = driveNo; 257 | SoEp->elementflags = elementflags; 258 | hp = p; 259 | mp = (uint8 *)&MbxOut + sizeof(ec_SoEt); 260 | maxdata = context->slavelist[slave].mbx_l - sizeof(ec_SoEt); 261 | NotLast = TRUE; 262 | while (NotLast) 263 | { 264 | framedatasize = psize; 265 | NotLast = FALSE; 266 | SoEp->idn = htoes(idn); 267 | SoEp->incomplete = 0; 268 | if (framedatasize > maxdata) 269 | { 270 | framedatasize = maxdata; /* segmented transfer needed */ 271 | NotLast = TRUE; 272 | SoEp->incomplete = 1; 273 | SoEp->fragmentsleft = psize / maxdata; 274 | } 275 | SoEp->MbxHeader.length = htoes(sizeof(ec_SoEt) - sizeof(ec_mbxheadert) + framedatasize); 276 | /* get new mailbox counter, used for session handle */ 277 | cnt = ec_nextmbxcnt(context->slavelist[slave].mbx_cnt); 278 | context->slavelist[slave].mbx_cnt = cnt; 279 | SoEp->MbxHeader.mbxtype = ECT_MBXT_SOE + (cnt << 4); /* SoE */ 280 | /* copy parameter data to mailbox */ 281 | memcpy(mp, hp, framedatasize); 282 | hp += framedatasize; 283 | psize -= framedatasize; 284 | /* send SoE request to slave */ 285 | wkc = ecx_mbxsend(context, slave, (ec_mbxbuft *)&MbxOut, EC_TIMEOUTTXM); 286 | if (wkc > 0) /* succeeded to place mailbox in slave ? */ 287 | { 288 | if (!NotLast || !ecx_mbxempty(context, slave, timeout)) 289 | { 290 | /* clean mailboxbuffer */ 291 | ec_clearmbx(&MbxIn); 292 | /* read slave response */ 293 | wkc = ecx_mbxreceive(context, slave, (ec_mbxbuft *)&MbxIn, timeout); 294 | if (wkc > 0) /* succeeded to read slave response ? */ 295 | { 296 | NotLast = FALSE; 297 | /* slave response should be SoE, WriteRes */ 298 | if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && 299 | (aSoEp->opCode == ECT_SOE_WRITERES) && 300 | (aSoEp->error == 0) && 301 | (aSoEp->driveNo == driveNo) && 302 | (aSoEp->elementflags == elementflags)) 303 | { 304 | /* SoE write succeeded */ 305 | } 306 | /* other slave response */ 307 | else 308 | { 309 | if (((aSoEp->MbxHeader.mbxtype & 0x0f) == ECT_MBXT_SOE) && 310 | (aSoEp->opCode == ECT_SOE_READRES) && 311 | (aSoEp->error == 1)) 312 | { 313 | mp = (uint8 *)&MbxIn + (etohs(aSoEp->MbxHeader.length) + sizeof(ec_mbxheadert) - sizeof(uint16)); 314 | errorcode = (uint16 *)mp; 315 | ecx_SoEerror(context, slave, idn, *errorcode); 316 | } 317 | else 318 | { 319 | ecx_packeterror(context, slave, idn, 0, 1); /* Unexpected frame returned */ 320 | } 321 | wkc = 0; 322 | } 323 | } 324 | else 325 | { 326 | ecx_packeterror(context, slave, idn, 0, 4); /* no response */ 327 | } 328 | } 329 | } 330 | } 331 | return wkc; 332 | } 333 | 334 | /** SoE read AT and MTD mapping. 335 | * 336 | * SoE has standard indexes defined for mapping. This function 337 | * tries to read them and collect a full input and output mapping size 338 | * of designated slave. 339 | * 340 | * @param[in] context = context struct 341 | * @param[in] slave = Slave number 342 | * @param[out] Osize = Size in bits of output mapping (MTD) found 343 | * @param[out] Isize = Size in bits of input mapping (AT) found 344 | * @return >0 if mapping succesful. 345 | */ 346 | int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize) 347 | { 348 | int retVal = 0; 349 | int wkc; 350 | int psize; 351 | uint16 entries, itemcount; 352 | ec_SoEmappingt SoEmapping; 353 | ec_SoEattributet SoEattribute; 354 | 355 | *Isize = 0; 356 | *Osize = 0; 357 | psize = sizeof(SoEmapping); 358 | /* read output mapping via SoE */ 359 | wkc = ecx_SoEread(context, slave, 0, EC_SOE_VALUE_B, EC_IDN_MDTCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM); 360 | if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING)) 361 | { 362 | /* command word (uint16) is always mapped but not in list */ 363 | *Osize = 16; 364 | for (itemcount = 0 ; itemcount < entries ; itemcount++) 365 | { 366 | psize = sizeof(SoEattribute); 367 | /* read attribute of each IDN in mapping list */ 368 | wkc = ecx_SoEread(context, slave, 0, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM); 369 | if ((wkc > 0) && (!SoEattribute.list)) 370 | { 371 | /* length : 0 = 8bit, 1 = 16bit .... */ 372 | *Osize += (int)8 << SoEattribute.length; 373 | } 374 | } 375 | } 376 | psize = sizeof(SoEmapping); 377 | /* read input mapping via SoE */ 378 | wkc = ecx_SoEread(context, slave, 0, EC_SOE_VALUE_B, EC_IDN_ATCONFIG, &psize, &SoEmapping, EC_TIMEOUTRXM); 379 | if ((wkc > 0) && (psize >= 4) && ((entries = etohs(SoEmapping.currentlength) / 2) > 0) && (entries <= EC_SOE_MAXMAPPING)) 380 | { 381 | /* status word (uint16) is always mapped but not in list */ 382 | *Isize = 16; 383 | for (itemcount = 0 ; itemcount < entries ; itemcount++) 384 | { 385 | psize = sizeof(SoEattribute); 386 | /* read attribute of each IDN in mapping list */ 387 | wkc = ecx_SoEread(context, slave, 0, EC_SOE_ATTRIBUTE_B, SoEmapping.idn[itemcount], &psize, &SoEattribute, EC_TIMEOUTRXM); 388 | if ((wkc > 0) && (!SoEattribute.list)) 389 | { 390 | /* length : 0 = 8bit, 1 = 16bit .... */ 391 | *Isize += (int)8 << SoEattribute.length; 392 | } 393 | } 394 | } 395 | 396 | /* found some I/O bits ? */ 397 | if ((*Isize > 0) || (*Osize > 0)) 398 | { 399 | retVal = 1; 400 | } 401 | return retVal; 402 | } 403 | 404 | #ifdef EC_VER1 405 | int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout) 406 | { 407 | return ecx_SoEread(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout); 408 | } 409 | 410 | int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout) 411 | { 412 | return ecx_SoEwrite(&ecx_context, slave, driveNo, elementflags, idn, psize, p, timeout); 413 | } 414 | 415 | int ec_readIDNmap(uint16 slave, int *Osize, int *Isize) 416 | { 417 | return ecx_readIDNmap(&ecx_context, slave, Osize, Isize); 418 | } 419 | #endif 420 | -------------------------------------------------------------------------------- /soem/ethercatsoe.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercatsoe.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo “EtherCAT” are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * Headerfile for ethercatsoe.c 44 | */ 45 | 46 | #ifndef _ethercatsoe_ 47 | #define _ethercatsoe_ 48 | 49 | #ifdef __cplusplus 50 | extern "C" 51 | { 52 | #endif 53 | 54 | #define EC_SOE_DATASTATE_B 0x01 55 | #define EC_SOE_NAME_B 0x02 56 | #define EC_SOE_ATTRIBUTE_B 0x04 57 | #define EC_SOE_UNIT_B 0x08 58 | #define EC_SOE_MIN_B 0x10 59 | #define EC_SOE_MAX_B 0x20 60 | #define EC_SOE_VALUE_B 0x40 61 | #define EC_SOE_DEFAULT_B 0x80 62 | 63 | #define EC_SOE_MAXNAME 60 64 | #define EC_SOE_MAXMAPPING 64 65 | 66 | #define EC_IDN_MDTCONFIG 24 67 | #define EC_IDN_ATCONFIG 16 68 | 69 | /** SoE name structure */ 70 | PACKED_BEGIN 71 | typedef struct PACKED 72 | { 73 | /** current length in bytes of list */ 74 | uint16 currentlength; 75 | /** maximum length in bytes of list */ 76 | uint16 maxlength; 77 | char name[EC_SOE_MAXNAME]; 78 | } ec_SoEnamet; 79 | PACKED_END 80 | 81 | /** SoE list structure */ 82 | PACKED_BEGIN 83 | typedef struct PACKED 84 | { 85 | /** current length in bytes of list */ 86 | uint16 currentlength; 87 | /** maximum length in bytes of list */ 88 | uint16 maxlength; 89 | union 90 | { 91 | uint8 byte[8]; 92 | uint16 word[4]; 93 | uint32 dword[2]; 94 | uint64 lword[1]; 95 | }; 96 | } ec_SoElistt; 97 | PACKED_END 98 | 99 | /** SoE IDN mapping structure */ 100 | PACKED_BEGIN 101 | typedef struct PACKED 102 | { 103 | /** current length in bytes of list */ 104 | uint16 currentlength; 105 | /** maximum length in bytes of list */ 106 | uint16 maxlength; 107 | uint16 idn[EC_SOE_MAXMAPPING]; 108 | } ec_SoEmappingt; 109 | PACKED_END 110 | 111 | #define EC_SOE_LENGTH_1 0x00 112 | #define EC_SOE_LENGTH_2 0x01 113 | #define EC_SOE_LENGTH_4 0x02 114 | #define EC_SOE_LENGTH_8 0x03 115 | #define EC_SOE_TYPE_BINARY 0x00 116 | #define EC_SOE_TYPE_UINT 0x01 117 | #define EC_SOE_TYPE_INT 0x02 118 | #define EC_SOE_TYPE_HEX 0x03 119 | #define EC_SOE_TYPE_STRING 0x04 120 | #define EC_SOE_TYPE_IDN 0x05 121 | #define EC_SOE_TYPE_FLOAT 0x06 122 | #define EC_SOE_TYPE_PARAMETER 0x07 123 | 124 | /** SoE attribute structure */ 125 | PACKED_BEGIN 126 | typedef struct PACKED 127 | { 128 | /** evaluation factor for display purposes */ 129 | uint32 evafactor :16; 130 | /** length of IDN element(s) */ 131 | uint32 length :2; 132 | /** IDN is list */ 133 | uint32 list :1; 134 | /** IDN is command */ 135 | uint32 command :1; 136 | /** datatype */ 137 | uint32 datatype :3; 138 | uint32 reserved1 :1; 139 | /** decimals to display if float datatype */ 140 | uint32 decimals :4; 141 | /** write protected in pre-op */ 142 | uint32 wppreop :1; 143 | /** write protected in safe-op */ 144 | uint32 wpsafeop :1; 145 | /** write protected in op */ 146 | uint32 wpop :1; 147 | uint32 reserved2 :1; 148 | } ec_SoEattributet; 149 | PACKED_END 150 | 151 | #ifdef EC_VER1 152 | int ec_SoEread(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout); 153 | int ec_SoEwrite(uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout); 154 | int ec_readIDNmap(uint16 slave, int *Osize, int *Isize); 155 | #endif 156 | 157 | int ecx_SoEread(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int *psize, void *p, int timeout); 158 | int ecx_SoEwrite(ecx_contextt *context, uint16 slave, uint8 driveNo, uint8 elementflags, uint16 idn, int psize, void *p, int timeout); 159 | int ecx_readIDNmap(ecx_contextt *context, uint16 slave, int *Osize, int *Isize); 160 | 161 | #ifdef __cplusplus 162 | } 163 | #endif 164 | 165 | #endif -------------------------------------------------------------------------------- /soem/ethercattype.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Simple Open EtherCAT Master Library 3 | * 4 | * File : ethercattype.h 5 | * Version : 1.3.0 6 | * Date : 24-02-2013 7 | * Copyright (C) 2005-2013 Speciaal Machinefabriek Ketels v.o.f. 8 | * Copyright (C) 2005-2013 Arthur Ketels 9 | * Copyright (C) 2008-2009 TU/e Technische Universiteit Eindhoven 10 | * 11 | * SOEM is free software; you can redistribute it and/or modify it under 12 | * the terms of the GNU General Public License version 2 as published by the Free 13 | * Software Foundation. 14 | * 15 | * SOEM is distributed in the hope that it will be useful, but WITHOUT ANY 16 | * WARRANTY; without even the implied warranty of MERCHANTABILITY or 17 | * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License 18 | * for more details. 19 | * 20 | * As a special exception, if other files instantiate templates or use macros 21 | * or inline functions from this file, or you compile this file and link it 22 | * with other works to produce a work based on this file, this file does not 23 | * by itself cause the resulting work to be covered by the GNU General Public 24 | * License. However the source code for this file must still be made available 25 | * in accordance with section (3) of the GNU General Public License. 26 | * 27 | * This exception does not invalidate any other reasons why a work based on 28 | * this file might be covered by the GNU General Public License. 29 | * 30 | * The EtherCAT Technology, the trade name and logo EtherCAT are the intellectual 31 | * property of, and protected by Beckhoff Automation GmbH. You can use SOEM for 32 | * the sole purpose of creating, using and/or selling or otherwise distributing 33 | * an EtherCAT network master provided that an EtherCAT Master License is obtained 34 | * from Beckhoff Automation GmbH. 35 | * 36 | * In case you did not receive a copy of the EtherCAT Master License along with 37 | * SOEM write to Beckhoff Automation GmbH, Eiserstraße 5, D-33415 Verl, Germany 38 | * (www.beckhoff.com). 39 | */ 40 | 41 | /** \file 42 | * \brief 43 | * General typedefs and defines for EtherCAT. 44 | * 45 | * Defines that could need optimalisation for specific applications 46 | * are the EC_TIMEOUTxxx. Assumptions for the standard settings are a 47 | * standard linux PC or laptop and a wired connection to maximal 100 slaves. 48 | * For use with wireless connections or lots of slaves the timouts need 49 | * increasing. For fast systems running Xenomai and RT-net or alike the 50 | * timeouts need to be shorter. 51 | */ 52 | 53 | #ifndef _EC_TYPE_H 54 | #define _EC_TYPE_H 55 | 56 | #ifdef __cplusplus 57 | extern "C" 58 | { 59 | #endif 60 | 61 | /** Define Little or Big endian target */ 62 | #define EC_LITTLE_ENDIAN 63 | 64 | /** define EC_VER1 if version 1 default context and functions are needed 65 | * comment if application uses only ecx_ functions and own context */ 66 | #define EC_VER1 67 | 68 | #include "osal.h" 69 | 70 | /** return value general error */ 71 | #define EC_ERROR -3 72 | /** return value no frame returned */ 73 | #define EC_NOFRAME -1 74 | /** return value unknown frame received */ 75 | #define EC_OTHERFRAME -2 76 | /** maximum EtherCAT frame length in bytes */ 77 | #define EC_MAXECATFRAME 1518 78 | /** maximum EtherCAT LRW frame length in bytes */ 79 | /* MTU - Ethernet header - length - datagram header - WCK - FCS */ 80 | #define EC_MAXLRWDATA (EC_MAXECATFRAME - 14 - 2 - 10 - 2 - 4) 81 | /** size of DC datagram used in first LRW frame */ 82 | #define EC_FIRSTDCDATAGRAM 20 83 | /** standard frame buffer size in bytes */ 84 | #define EC_BUFSIZE EC_MAXECATFRAME 85 | /** datagram type EtherCAT */ 86 | #define EC_ECATTYPE 0x1000 87 | /** number of frame buffers per channel (tx, rx1 rx2) */ 88 | #define EC_MAXBUF 16 89 | /** timeout value in us for tx frame to return to rx */ 90 | #define EC_TIMEOUTRET 2000 91 | /** timeout value in us for safe data transfer, max. triple retry */ 92 | /** EC_TIMEOUTRET3 is used mostly for ecx_config_init, scan slave, sdo configuration,...*/ 93 | /** default is 10 times of EC_TIMEOUTRET (20ms), some slaves like Elmo drives need longer timer for configuration, changed to 200ms*/ 94 | #define EC_TIMEOUTRET3 (EC_TIMEOUTRET * 100) 95 | /** timeout value in us for return "safe" variant (f.e. wireless) */ 96 | #define EC_TIMEOUTSAFE 20000 97 | /** timeout value in us for EEPROM access */ 98 | #define EC_TIMEOUTEEP 20000 99 | /** timeout value in us for tx mailbox cycle */ 100 | #define EC_TIMEOUTTXM 20000 101 | /** timeout value in us for rx mailbox cycle */ 102 | #define EC_TIMEOUTRXM 700000 103 | /** timeout value in us for check statechange */ 104 | #define EC_TIMEOUTSTATE 2000000 105 | /** size of EEPROM bitmap cache */ 106 | #define EC_MAXEEPBITMAP 128 107 | /** size of EEPROM cache buffer */ 108 | #define EC_MAXEEPBUF EC_MAXEEPBITMAP << 5 109 | /** default number of retries if wkc <= 0 */ 110 | #define EC_DEFAULTRETRIES 3 111 | 112 | /** definition for frame buffers */ 113 | typedef uint8 ec_bufT[EC_BUFSIZE]; 114 | 115 | /** ethernet header definition */ 116 | PACKED_BEGIN 117 | typedef struct PACKED 118 | { 119 | /** destination MAC */ 120 | uint16 da0,da1,da2; 121 | /** source MAC */ 122 | uint16 sa0,sa1,sa2; 123 | /** ethernet type */ 124 | uint16 etype; 125 | } ec_etherheadert; 126 | PACKED_END 127 | 128 | /** ethernet header size */ 129 | #define ETH_HEADERSIZE sizeof(ec_etherheadert) 130 | 131 | /** EtherCAT datagram header definition */ 132 | PACKED_BEGIN 133 | typedef struct PACKED 134 | { 135 | /** length of EtherCAT datagram */ 136 | uint16 elength; 137 | /** EtherCAT command, see ec_cmdtype */ 138 | uint8 command; 139 | /** index, used in SOEM for Tx to Rx recombination */ 140 | uint8 index; 141 | /** ADP */ 142 | uint16 ADP; 143 | /** ADO */ 144 | uint16 ADO; 145 | /** length of data portion in datagram */ 146 | uint16 dlength; 147 | /** interrupt, currently unused */ 148 | uint16 irpt; 149 | } ec_comt; 150 | PACKED_END 151 | 152 | /** EtherCAT header size */ 153 | #define EC_HEADERSIZE sizeof(ec_comt) 154 | /** size of ec_comt.elength item in EtherCAT header */ 155 | #define EC_ELENGTHSIZE sizeof(uint16) 156 | /** offset position of command in EtherCAT header */ 157 | #define EC_CMDOFFSET EC_ELENGTHSIZE 158 | /** size of workcounter item in EtherCAT datagram */ 159 | #define EC_WKCSIZE sizeof(uint16) 160 | /** definition of datagram follows bit in ec_comt.dlength */ 161 | #define EC_DATAGRAMFOLLOWS (1 << 15) 162 | 163 | /** Possible error codes returned. */ 164 | typedef enum 165 | { 166 | /** No error */ 167 | EC_ERR_OK = 0, 168 | /** Library already initialized. */ 169 | EC_ERR_ALREADY_INITIALIZED, 170 | /** Library not initialized. */ 171 | EC_ERR_NOT_INITIALIZED, 172 | /** Timeout occured during execution of the function. */ 173 | EC_ERR_TIMEOUT, 174 | /** No slaves were found. */ 175 | EC_ERR_NO_SLAVES, 176 | /** Function failed. */ 177 | EC_ERR_NOK 178 | } ec_err; 179 | 180 | /** Possible EtherCAT slave states */ 181 | typedef enum 182 | { 183 | /** Init state*/ 184 | EC_STATE_INIT = 0x01, 185 | /** Pre-operational. */ 186 | EC_STATE_PRE_OP = 0x02, 187 | /** Boot state*/ 188 | EC_STATE_BOOT = 0x03, 189 | /** Safe-operational. */ 190 | EC_STATE_SAFE_OP = 0x04, 191 | /** Operational */ 192 | EC_STATE_OPERATIONAL = 0x08, 193 | /** Error or ACK error */ 194 | EC_STATE_ACK = 0x10, 195 | EC_STATE_ERROR = 0x10 196 | } ec_state; 197 | 198 | /** Possible buffer states */ 199 | typedef enum 200 | { 201 | /** Empty */ 202 | EC_BUF_EMPTY = 0x00, 203 | /** Allocated, but not filled */ 204 | EC_BUF_ALLOC = 0x01, 205 | /** Transmitted */ 206 | EC_BUF_TX = 0x02, 207 | /** Received, but not consumed */ 208 | EC_BUF_RCVD = 0x03, 209 | /** Cycle completed */ 210 | EC_BUF_COMPLETE = 0x04 211 | } ec_bufstate; 212 | 213 | /** Ethercat data types */ 214 | typedef enum 215 | { 216 | ECT_BOOLEAN = 0x0001, 217 | ECT_INTEGER8 = 0x0002, 218 | ECT_INTEGER16 = 0x0003, 219 | ECT_INTEGER32 = 0x0004, 220 | ECT_UNSIGNED8 = 0x0005, 221 | ECT_UNSIGNED16 = 0x0006, 222 | ECT_UNSIGNED32 = 0x0007, 223 | ECT_REAL32 = 0x0008, 224 | ECT_VISIBLE_STRING = 0x0009, 225 | ECT_OCTET_STRING = 0x000A, 226 | ECT_UNICODE_STRING = 0x000B, 227 | ECT_TIME_OF_DAY = 0x000C, 228 | ECT_TIME_DIFFERENCE = 0x000D, 229 | ECT_DOMAIN = 0x000F, 230 | ECT_INTEGER24 = 0x0010, 231 | ECT_REAL64 = 0x0011, 232 | ECT_INTEGER64 = 0x0015, 233 | ECT_UNSIGNED24 = 0x0016, 234 | ECT_UNSIGNED64 = 0x001B, 235 | ECT_BIT1 = 0x0030, 236 | ECT_BIT2 = 0x0031, 237 | ECT_BIT3 = 0x0032, 238 | ECT_BIT4 = 0x0033, 239 | ECT_BIT5 = 0x0034, 240 | ECT_BIT6 = 0x0035, 241 | ECT_BIT7 = 0x0036, 242 | ECT_BIT8 = 0x0037 243 | } ec_datatype; 244 | 245 | /** Ethercat command types */ 246 | typedef enum 247 | { 248 | /** No operation */ 249 | EC_CMD_NOP = 0x00, 250 | /** Auto Increment Read */ 251 | EC_CMD_APRD, 252 | /** Auto Increment Write */ 253 | EC_CMD_APWR, 254 | /** Auto Increment Read Write */ 255 | EC_CMD_APRW, 256 | /** Configured Address Read */ 257 | EC_CMD_FPRD, 258 | /** Configured Address Write */ 259 | EC_CMD_FPWR, 260 | /** Configured Address Read Write */ 261 | EC_CMD_FPRW, 262 | /** Broadcast Read */ 263 | EC_CMD_BRD, 264 | /** Broaddcast Write */ 265 | EC_CMD_BWR, 266 | /** Broadcast Read Write */ 267 | EC_CMD_BRW, 268 | /** Logical Memory Read */ 269 | EC_CMD_LRD, 270 | /** Logical Memory Write */ 271 | EC_CMD_LWR, 272 | /** Logical Memory Read Write */ 273 | EC_CMD_LRW, 274 | /** Auto Increment Read Mulitple Write */ 275 | EC_CMD_ARMW, 276 | /** Configured Read Mulitple Write */ 277 | EC_CMD_FRMW 278 | /** Reserved */ 279 | } ec_cmdtype; 280 | 281 | /** Ethercat EEprom command types */ 282 | typedef enum 283 | { 284 | /** No operation */ 285 | EC_ECMD_NOP = 0x0000, 286 | /** Read */ 287 | EC_ECMD_READ = 0x0100, 288 | /** Write */ 289 | EC_ECMD_WRITE = 0x0201, 290 | /** Reload */ 291 | EC_ECMD_RELOAD = 0x0300 292 | } ec_ecmdtype; 293 | 294 | /** EEprom state machine read size */ 295 | #define EC_ESTAT_R64 0x0040 296 | /** EEprom state machine busy flag */ 297 | #define EC_ESTAT_BUSY 0x8000 298 | /** EEprom state machine error flag mask */ 299 | #define EC_ESTAT_EMASK 0x7800 300 | /** EEprom state machine error acknowledge */ 301 | #define EC_ESTAT_NACK 0x2000 302 | 303 | /* Ethercat SSI (Slave Information Interface) */ 304 | 305 | /** Start address SII sections in Eeprom */ 306 | #define ECT_SII_START 0x0040 307 | 308 | enum 309 | { 310 | /** SII category strings */ 311 | ECT_SII_STRING = 10, 312 | /** SII category general */ 313 | ECT_SII_GENERAL = 30, 314 | /** SII category FMMU */ 315 | ECT_SII_FMMU = 40, 316 | /** SII category SM */ 317 | ECT_SII_SM = 41, 318 | /** SII category PDO */ 319 | ECT_SII_PDO = 50 320 | }; 321 | 322 | /** Item offsets in SII general section */ 323 | enum 324 | { 325 | ECT_SII_MANUF = 0x0008, 326 | ECT_SII_ID = 0x000a, 327 | ECT_SII_REV = 0x000c, 328 | ECT_SII_BOOTRXMBX = 0x0014, 329 | ECT_SII_BOOTTXMBX = 0x0016, 330 | ECT_SII_MBXSIZE = 0x0019, 331 | ECT_SII_TXMBXADR = 0x001a, 332 | ECT_SII_RXMBXADR = 0x0018, 333 | ECT_SII_MBXPROTO = 0x001c 334 | }; 335 | 336 | /** Mailbox types definitions */ 337 | enum 338 | { 339 | /** Error mailbox type */ 340 | ECT_MBXT_ERR = 0x00, 341 | /** ADS over EtherCAT mailbox type */ 342 | ECT_MBXT_AOE, 343 | /** Ethernet over EtherCAT mailbox type */ 344 | ECT_MBXT_EOE, 345 | /** CANopen over EtherCAT mailbox type */ 346 | ECT_MBXT_COE, 347 | /** File over EtherCAT mailbox type */ 348 | ECT_MBXT_FOE, 349 | /** Servo over EtherCAT mailbox type */ 350 | ECT_MBXT_SOE, 351 | /** Vendor over EtherCAT mailbox type */ 352 | ECT_MBXT_VOE = 0x0f 353 | }; 354 | 355 | /** CoE mailbox types */ 356 | enum 357 | { 358 | ECT_COES_EMERGENCY = 0x01, 359 | ECT_COES_SDOREQ, 360 | ECT_COES_SDORES, 361 | ECT_COES_TXPDO, 362 | ECT_COES_RXPDO, 363 | ECT_COES_TXPDO_RR, 364 | ECT_COES_RXPDO_RR, 365 | ECT_COES_SDOINFO 366 | }; 367 | 368 | /** CoE SDO commands */ 369 | enum 370 | { 371 | ECT_SDO_DOWN_INIT = 0x21, 372 | ECT_SDO_DOWN_EXP = 0x23, 373 | ECT_SDO_DOWN_INIT_CA = 0x31, 374 | ECT_SDO_UP_REQ = 0x40, 375 | ECT_SDO_UP_REQ_CA = 0x50, 376 | ECT_SDO_SEG_UP_REQ = 0x60, 377 | ECT_SDO_ABORT = 0x80 378 | }; 379 | 380 | /** CoE Object Description commands */ 381 | enum 382 | { 383 | ECT_GET_ODLIST_REQ = 0x01, 384 | ECT_GET_ODLIST_RES = 0x02, 385 | ECT_GET_OD_REQ = 0x03, 386 | ECT_GET_OD_RES = 0x04, 387 | ECT_GET_OE_REQ = 0x05, 388 | ECT_GET_OE_RES = 0x06, 389 | ECT_SDOINFO_ERROR = 0x07 390 | }; 391 | 392 | /** FoE opcodes */ 393 | enum 394 | { 395 | ECT_FOE_READ = 0x01, 396 | ECT_FOE_WRITE, 397 | ECT_FOE_DATA, 398 | ECT_FOE_ACK, 399 | ECT_FOE_ERROR, 400 | ECT_FOE_BUSY 401 | }; 402 | 403 | /** SoE opcodes */ 404 | enum 405 | { 406 | ECT_SOE_READREQ = 0x01, 407 | ECT_SOE_READRES, 408 | ECT_SOE_WRITEREQ, 409 | ECT_SOE_WRITERES, 410 | ECT_SOE_NOTIFICATION, 411 | ECT_SOE_EMERGENCY 412 | }; 413 | 414 | /** Ethercat registers */ 415 | enum 416 | { 417 | ECT_REG_TYPE = 0x0000, 418 | ECT_REG_PORTDES = 0x0007, 419 | ECT_REG_ESCSUP = 0x0008, 420 | ECT_REG_STADR = 0x0010, 421 | ECT_REG_ALIAS = 0x0012, 422 | ECT_REG_DLCTL = 0x0100, 423 | ECT_REG_DLPORT = 0x0101, 424 | ECT_REG_DLALIAS = 0x0103, 425 | ECT_REG_DLSTAT = 0x0110, 426 | ECT_REG_ALCTL = 0x0120, 427 | ECT_REG_ALSTAT = 0x0130, 428 | ECT_REG_ALSTATCODE = 0x0134, 429 | ECT_REG_PDICTL = 0x0140, 430 | ECT_REG_IRQMASK = 0x0200, 431 | ECT_REG_RXERR = 0x0300, 432 | ECT_REG_EEPCFG = 0x0500, 433 | ECT_REG_EEPCTL = 0x0502, 434 | ECT_REG_EEPSTAT = 0x0502, 435 | ECT_REG_EEPADR = 0x0504, 436 | ECT_REG_EEPDAT = 0x0508, 437 | ECT_REG_FMMU0 = 0x0600, 438 | ECT_REG_FMMU1 = ECT_REG_FMMU0 + 0x10, 439 | ECT_REG_FMMU2 = ECT_REG_FMMU1 + 0x10, 440 | ECT_REG_FMMU3 = ECT_REG_FMMU2 + 0x10, 441 | ECT_REG_SM0 = 0x0800, 442 | ECT_REG_SM1 = ECT_REG_SM0 + 0x08, 443 | ECT_REG_SM2 = ECT_REG_SM1 + 0x08, 444 | ECT_REG_SM3 = ECT_REG_SM2 + 0x08, 445 | ECT_REG_SM0STAT = ECT_REG_SM0 + 0x05, 446 | ECT_REG_SM1STAT = ECT_REG_SM1 + 0x05, 447 | ECT_REG_SM1ACT = ECT_REG_SM1 + 0x06, 448 | ECT_REG_SM1CONTR = ECT_REG_SM1 + 0x07, 449 | ECT_REG_DCTIME0 = 0x0900, 450 | ECT_REG_DCTIME1 = 0x0904, 451 | ECT_REG_DCTIME2 = 0x0908, 452 | ECT_REG_DCTIME3 = 0x090C, 453 | ECT_REG_DCSYSTIME = 0x0910, 454 | ECT_REG_DCSOF = 0x0918, 455 | ECT_REG_DCSYSOFFSET = 0x0920, 456 | ECT_REG_DCSYSDELAY = 0x0928, 457 | ECT_REG_DCSYSDIFF = 0x092C, 458 | ECT_REG_DCSPEEDCNT = 0x0930, 459 | ECT_REG_DCTIMEFILT = 0x0934, 460 | ECT_REG_DCCUC = 0x0980, 461 | ECT_REG_DCSYNCACT = 0x0981, 462 | ECT_REG_DCSTART0 = 0x0990, 463 | ECT_REG_DCCYCLE0 = 0x09A0, 464 | ECT_REG_DCCYCLE1 = 0x09A4 465 | }; 466 | 467 | /** standard SDO Sync Manager Communication Type */ 468 | #define ECT_SDO_SMCOMMTYPE 0x1c00 469 | /** standard SDO PDO assignment */ 470 | #define ECT_SDO_PDOASSIGN 0x1c10 471 | /** standard SDO RxPDO assignment */ 472 | #define ECT_SDO_RXPDOASSIGN 0x1c12 473 | /** standard SDO TxPDO assignment */ 474 | #define ECT_SDO_TXPDOASSIGN 0x1c13 475 | 476 | /** Ethercat packet type */ 477 | #define ETH_P_ECAT 0x88A4 478 | 479 | /** Error types */ 480 | typedef enum 481 | { 482 | EC_ERR_TYPE_SDO_ERROR = 0, 483 | EC_ERR_TYPE_EMERGENCY = 1, 484 | EC_ERR_TYPE_PACKET_ERROR = 3, 485 | EC_ERR_TYPE_SDOINFO_ERROR = 4, 486 | EC_ERR_TYPE_FOE_ERROR = 5, 487 | EC_ERR_TYPE_FOE_BUF2SMALL = 6, 488 | EC_ERR_TYPE_FOE_PACKETNUMBER = 7, 489 | EC_ERR_TYPE_SOE_ERROR = 8, 490 | EC_ERR_TYPE_MBX_ERROR = 9 491 | } ec_err_type; 492 | 493 | /** Struct to retrieve errors. */ 494 | typedef struct 495 | { 496 | /** Time at which the error was generated. */ 497 | ec_timet Time; 498 | /** Signal bit, error set but not read */ 499 | boolean Signal; 500 | /** Slave number that generated the error */ 501 | uint16 Slave; 502 | /** CoE SDO index that generated the error */ 503 | uint16 Index; 504 | /** CoE SDO subindex that generated the error */ 505 | uint8 SubIdx; 506 | /** Type of error */ 507 | ec_err_type Etype; 508 | union 509 | { 510 | /** General abortcode */ 511 | int32 AbortCode; 512 | /** Specific error for Emergency mailbox */ 513 | struct 514 | { 515 | uint16 ErrorCode; 516 | uint8 ErrorReg; 517 | uint8 b1; 518 | uint16 w1; 519 | uint16 w2; 520 | }; 521 | }; 522 | } ec_errort; 523 | 524 | /** Helper macros */ 525 | /** Macro to make a word from 2 bytes */ 526 | #define MK_WORD(msb, lsb) ((((uint16)(msb))<<8) | (lsb)) 527 | /** Macro to get hi byte of a word */ 528 | #define HI_BYTE(w) ((w) >> 8) 529 | /** Macro to get low byte of a word */ 530 | #define LO_BYTE(w) ((w) & 0x00ff) 531 | /** Macro to swap hi and low byte of a word */ 532 | #define SWAP(w) ((((w)& 0xff00) >> 8) | (((w) & 0x00ff) << 8)) 533 | /** Macro to get hi word of a dword */ 534 | #define LO_WORD(l) ((l) & 0xffff) 535 | /** Macro to get hi word of a dword */ 536 | #define HI_WORD(l) ((l) >> 16) 537 | 538 | #define get_unaligned(ptr) \ 539 | ({ __typeof__(*(ptr)) __tmp; memcpy(&__tmp, (ptr), sizeof(*(ptr))); __tmp; }) 540 | 541 | #define put_unaligned32(val, ptr) \ 542 | (memcpy((ptr), &(val), 4)) 543 | 544 | #define put_unaligned64(val, ptr) \ 545 | (memcpy((ptr), &(val), 8)) 546 | 547 | #if !defined(EC_BIG_ENDIAN) && defined(EC_LITTLE_ENDIAN) 548 | 549 | #define htoes(A) (A) 550 | #define htoel(A) (A) 551 | #define htoell(A) (A) 552 | #define etohs(A) (A) 553 | #define etohl(A) (A) 554 | #define etohll(A) (A) 555 | 556 | #elif !defined(EC_LITTLE_ENDIAN) && defined(EC_BIG_ENDIAN) 557 | 558 | #define htoes(A) ((((uint16)(A) & 0xff00) >> 8) | \ 559 | (((uint16)(A) & 0x00ff) << 8)) 560 | #define htoel(A) ((((uint32)(A) & 0xff000000) >> 24) | \ 561 | (((uint32)(A) & 0x00ff0000) >> 8) | \ 562 | (((uint32)(A) & 0x0000ff00) << 8) | \ 563 | (((uint32)(A) & 0x000000ff) << 24)) 564 | #define htoell(A) ((((uint64)(A) & (uint64)0xff00000000000000ULL) >> 56) | \ 565 | (((uint64)(A) & (uint64)0x00ff000000000000ULL) >> 40) | \ 566 | (((uint64)(A) & (uint64)0x0000ff0000000000ULL) >> 24) | \ 567 | (((uint64)(A) & (uint64)0x000000ff00000000ULL) >> 8) | \ 568 | (((uint64)(A) & (uint64)0x00000000ff000000ULL) << 8) | \ 569 | (((uint64)(A) & (uint64)0x0000000000ff0000ULL) << 24) | \ 570 | (((uint64)(A) & (uint64)0x000000000000ff00ULL) << 40) | \ 571 | (((uint64)(A) & (uint64)0x00000000000000ffULL) << 56)) 572 | 573 | #define etohs htoes 574 | #define etohl htoel 575 | #define etohll htoell 576 | 577 | #else 578 | 579 | #error "Must define one of EC_BIG_ENDIAN or EC_LITTLE_ENDIAN" 580 | 581 | #endif 582 | 583 | #ifdef __cplusplus 584 | } 585 | #endif 586 | 587 | #endif /* _EC_TYPE_H */ 588 | -------------------------------------------------------------------------------- /test/MAXPOS_CSP/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # 'make depend' uses makedepend to automatically generate dependencies 3 | # (dependencies are added to end of Makefile) 4 | # 'make' build executable file 'mycc' 5 | # 'make clean' removes all .o and executable files 6 | # 7 | 8 | # define the C compiler to use 9 | CC = gcc 10 | 11 | # define any compile-time flags 12 | skin = alchemy 13 | CFLAGS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --cflags) 14 | LDLIBS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --ldflags) 15 | 16 | #SOEM EtherCAT 17 | CFLAGS += -I../../soem 18 | CFLAGS += -I../../osal 19 | CFLAGS += -I../../oshw 20 | CFLAGS += -O3 -Wall -g 21 | 22 | # define any libraries to link into executable: 23 | # if I want to link in libraries (libx.so or libx.a) I use the -llibname 24 | # option, something like (this will link in libmylib.so and libm.so: 25 | 26 | #SOEM EtherCAT 27 | LDLIBS += -L../../lib 28 | LDLIBS +=-lsoem 29 | LDLIBS +=-losal 30 | LDLIBS +=-loshw 31 | LDLIBS +=-lwiznet_drv 32 | 33 | LDLIBS +=-lm 34 | 35 | # define the C source files 36 | SRCS = main.c servo.c ecat_dc.c 37 | 38 | # define the C object files 39 | # 40 | # This uses Suffix Replacement within a macro: 41 | # $(name:string1=string2) 42 | # For each word in 'name' replace 'string1' with 'string2' 43 | # Below we are replacing the suffix .c of all words in the macro SRCS 44 | # with the .o suffix 45 | # 46 | OBJS = $(SRCS:.c=.o) 47 | 48 | # define the executable file 49 | MAIN = MAXPOS_CSP 50 | 51 | # 52 | # The following part of the makefile is generic; it can be used to 53 | # build any executable just by changing the definitions above and by 54 | # deleting dependencies appended to the file from 'make depend' 55 | # 56 | 57 | .PHONY: depend clean 58 | 59 | all: $(MAIN) 60 | 61 | $(MAIN): $(OBJS) 62 | $(CC) $(CFLAGS) -o $(MAIN) $(OBJS) $(LDLIBS) 63 | 64 | # this is a suffix replacement rule for building .o's from .c's 65 | # it uses automatic variables $<: the name of the prerequisite of 66 | # the rule(a .c file) and $@: the name of the target of the rule (a .o file) 67 | # (see the gnu make manual section about automatic variables) 68 | .c.o: 69 | $(CC) $(CFLAGS) $(LDLIBS) -c $< -o $@ 70 | 71 | clean: 72 | $(RM) *.o *~ $(MAIN) 73 | 74 | 75 | # DO NOT DELETE THIS LINE -- make depend needs it -------------------------------------------------------------------------------- /test/MAXPOS_CSP/ecat_dc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * SOEM EtherCAT exmaple 3 | * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 4 | */ 5 | 6 | #include 7 | #include 8 | #include "ecat_dc.h" 9 | 10 | //convert float, double to fixed point 11 | void double_to_fixed(double f_input, int32_t *pValue, int32_t *pBase) 12 | { 13 | if (f_input<1.0) 14 | { 15 | (*pBase)=15; 16 | (*pValue)=(int32_t) (32768.0*f_input); 17 | } 18 | else if (f_input<2.0) 19 | { 20 | (*pBase)=14; 21 | (*pValue)=(int32_t) (16384.0*f_input); 22 | } 23 | else if (f_input<4.0) 24 | { 25 | (*pBase)=13; 26 | (*pValue)=(int32_t) (8192.0*f_input); 27 | } 28 | else if (f_input<8.0) 29 | { 30 | (*pBase)=12; 31 | (*pValue)=(int32_t) (4096.0*f_input); 32 | } 33 | else if (f_input<16.0) 34 | { 35 | (*pBase)=11; 36 | (*pValue)=(int32_t) (2048.0*f_input); 37 | } 38 | else if (f_input<32.0) 39 | { 40 | (*pBase)=10; 41 | (*pValue)=(int32_t) (1024.0*f_input); 42 | } 43 | else if (f_input<64.0) 44 | { 45 | (*pBase)=9; 46 | (*pValue)=(int32_t) (512.0*f_input); 47 | } 48 | else if (f_input<128.0) 49 | { 50 | (*pBase)=8; 51 | (*pValue)=(int32_t) (256.0*f_input); 52 | } 53 | else if (f_input<256.0) 54 | { 55 | (*pBase)=7; 56 | (*pValue)=(int32_t) (128.0*f_input); 57 | } 58 | else if (f_input<512.0) 59 | { 60 | (*pBase)=6; 61 | (*pValue)=(int32_t) (64.0*f_input); 62 | } 63 | else if (f_input<1024.0) 64 | { 65 | (*pBase)=5; 66 | (*pValue)=(int32_t) (32.0*f_input); 67 | } 68 | else if (f_input<2048.0) 69 | { 70 | (*pBase)=4; 71 | (*pValue)=(int32_t) (16.0*f_input); 72 | } 73 | else if (f_input<4096.0) 74 | { 75 | (*pBase)=3; 76 | (*pValue)=(int32_t) (8.0*f_input); 77 | } 78 | else if (f_input<81928.0) 79 | { 80 | (*pBase)=2; 81 | (*pValue)=(int32_t) (4.0*f_input); 82 | } 83 | else if (f_input<16384.0) 84 | { 85 | (*pBase)=1; 86 | (*pValue)=(int32_t) (2.0*f_input); 87 | } 88 | else if (f_input<32768.0) 89 | { 90 | (*pBase)=0; 91 | (*pValue)=(int32_t) (1.0*f_input); 92 | } 93 | } 94 | 95 | 96 | 97 | 98 | #define PI_SAT_VAL 50000 99 | 100 | int32_t _pPart=0, _iPart=0; 101 | int32_t _sync_err=0, _sync_pre_err=0; 102 | 103 | //double Kp=0.1, Ki=0.0005; 104 | double Kp=0.1, Ki=0.001; 105 | 106 | //new style, PI compensation 107 | long long dc_pi_sync(long long reftime, long long cycletime, int32_t shift_time) 108 | { 109 | long long adj_time=0; 110 | int32_t iKp=0, iKp_Base, iKi=0, iKi_Base; 111 | double_to_fixed(Kp, &iKp, &iKp_Base); 112 | double_to_fixed(Ki, &iKi, &iKi_Base); 113 | 114 | _sync_err = (reftime - shift_time) % cycletime; 115 | if(_sync_err> (cycletime /2)) { _sync_err= _sync_err - cycletime; } 116 | 117 | _pPart=_sync_err*iKp; 118 | _iPart+=(_sync_err+_sync_pre_err)*iKi; 119 | _sync_pre_err=_sync_err; 120 | 121 | adj_time = -(_pPart>>iKp_Base) - (_iPart>>iKi_Base); 122 | 123 | // if (adj_time>PI_SAT_VAL) adj_time=PI_SAT_VAL; 124 | // if (adj_time<-PI_SAT_VAL) adj_time=-PI_SAT_VAL; 125 | 126 | return adj_time; 127 | 128 | } 129 | 130 | -------------------------------------------------------------------------------- /test/MAXPOS_CSP/ecat_dc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SOEM EtherCAT exmaple 3 | * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 4 | */ 5 | 6 | #ifndef _ECAT_DC_H_ 7 | #define _ECAT_DC_H_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | //new style, PI compensation 19 | long long dc_pi_sync(long long reftime, long long cycletime, int32_t shift_time); 20 | #endif //_ECAT_DC_H_ 21 | 22 | -------------------------------------------------------------------------------- /test/MAXPOS_CSP/main.c: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Example code for Simple Open EtherCAT master 3 | * 4 | * Usage : simple_test [ifname1] 5 | * ifname is NIC interface, f.e. eth0 6 | * 7 | * This is a minimal test. 8 | * 9 | * (c)Arthur Ketels 2010 - 2011 10 | * 11 | * Port for Raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "ethercattype.h" 26 | #include "nicdrv.h" 27 | #include "ethercatbase.h" 28 | #include "ethercatmain.h" 29 | #include "ethercatdc.h" 30 | #include "ethercatcoe.h" 31 | #include "ethercatfoe.h" 32 | #include "ethercatconfig.h" 33 | #include "ethercatprint.h" 34 | #include "pdo_def.h" 35 | #include "ecat_dc.h" 36 | #include "wiznet_drv.h" 37 | 38 | #define NSEC_PER_SEC 1000000000 39 | #define EC_TIMEOUTMON 500 40 | 41 | #define NUMOFMAXPOS_DRIVE 2 42 | 43 | #define x_USE_DC 44 | 45 | MAXPOS_Drive_pt maxpos_drive_pt[NUMOFMAXPOS_DRIVE]; 46 | 47 | unsigned int cycle_ns = 1000000; /* 1 ms */ 48 | 49 | char IOmap[4096]; 50 | //char IOmap[1024]; 51 | pthread_t thread1; 52 | int expectedWKC; 53 | boolean needlf; 54 | volatile int wkc; 55 | boolean inOP; 56 | uint8 currentgroup = 0; 57 | 58 | RT_TASK motion_task; 59 | RT_TASK print_task; 60 | RT_TASK eccheck_task; 61 | 62 | RTIME now, previous; 63 | long ethercat_time_send, ethercat_time_read=0; 64 | long ethercat_time=0, worst_time=0; 65 | char ecat_ifname[32]="PiCAT"; 66 | int run=1; 67 | int sys_ready=0; 68 | 69 | int started[NUMOFMAXPOS_DRIVE]={0}, ServoState=0; 70 | uint8 servo_ready=0, servo_prestate=0; 71 | int32_t zeropos[NUMOFMAXPOS_DRIVE]={0}; 72 | double gt=0; 73 | 74 | double sine_amp=11500, f=0.2, period; 75 | int recv_fail_cnt=0; 76 | 77 | //variables for pdo re-mapping (sdo write) 78 | int os; 79 | uint32_t ob; 80 | uint16_t ob2; 81 | uint8_t ob3; 82 | 83 | boolean ecat_init(void) 84 | { 85 | int i, oloop, iloop, k, wkc_count; 86 | needlf = FALSE; 87 | inOP = FALSE; 88 | 89 | rt_printf("Starting simple test\n"); 90 | 91 | //wiznet_hw_config(8, 1, 1000000); //select SPI-W5500 parameters, before ec_init 92 | wiznet_hw_config(8, 0, 0); //31.25 Mhz, don't reset link 93 | 94 | if (ec_init(ecat_ifname)) 95 | { 96 | rt_printf("ec_init on %s succeeded.\n", ecat_ifname); //ifname 97 | /* find and auto-config slaves */ 98 | 99 | if ( ec_config_init(FALSE) > 0 ) 100 | { 101 | rt_printf("%d slaves found and configured.\n",ec_slavecount); 102 | 103 | //PDO re-mapping**************************************************************************************************** 104 | for (k=0; k= 1 ) && (strcmp(ec_slave[k+1].name,"MAXPOS") == 0)) //change name for other drives 107 | { 108 | printf("Re mapping for MAXPOS...\n"); 109 | os=sizeof(ob2); ob2 = 0x1600; //RxPDO, check MAXPOS ESI 110 | //0x1c12 is Index of Sync Manager 2 PDO Assignment (output RxPDO), CA (Complete Access) must be TRUE 111 | wkc_count=ec_SDOwrite(k+1, 0x1c12,01,TRUE,os, &ob2,EC_TIMEOUTRXM); //change slave position (k+1) if needed 112 | 113 | if (wkc_count==0) 114 | { 115 | printf("RxPDO assignment error\n"); 116 | return FALSE; 117 | } 118 | 119 | os=sizeof(ob2); ob2 = 0x1a00; //TxPDO, check MAXPOS ESI 120 | //0x1c13 is Index of Sync Manager 3 PDO Assignment (input TxPDO), CA (Complete Access) must be TRUE 121 | wkc_count=ec_SDOwrite(k+1, 0x1c13,01,TRUE,os, &ob2,EC_TIMEOUTRXM); //change slave position (k+1) if needed 122 | if (wkc_count==0) 123 | { 124 | printf("TxPDO assignment error\n"); 125 | return FALSE; 126 | } 127 | } 128 | } 129 | //PDO re-mapping**************************************************************************************************** 130 | 131 | #ifdef _WRITE_MODEOP_SDO 132 | uint8 modeOp=OP_MODE_CYCLIC_SYNC_POSITION; 133 | // write mode of operation by SDO 134 | for (i=0; i 0)) oloop = 1; 157 | //if (oloop > 8) oloop = 8; 158 | iloop = ec_slave[0].Ibytes; 159 | if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; 160 | //if (iloop > 8) iloop = 8; 161 | 162 | rt_printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]); 163 | 164 | rt_printf("Request operational state for all slaves\n"); 165 | expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; 166 | rt_printf("Calculated workcounter %d\n", expectedWKC); 167 | ec_slave[0].state = EC_STATE_OPERATIONAL; 168 | /* send one valid process data to make outputs in slaves happy*/ 169 | ec_send_processdata(); 170 | ec_receive_processdata(EC_TIMEOUTRET); 171 | /* request OP state for all slaves */ 172 | 173 | ec_writestate(0); 174 | ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); //wait for OP 175 | 176 | if (ec_slave[0].state == EC_STATE_OPERATIONAL ) 177 | { 178 | rt_printf("Operational state reached for all slaves.\n"); 179 | wkc_count = 0; 180 | 181 | for (k=0; kModeOfOperation=OP_MODE_CYCLIC_SYNC_POSITION; 186 | } 187 | inOP = TRUE; 188 | } 189 | else 190 | { 191 | rt_printf("Not all slaves reached operational state.\n"); 192 | ec_readstate(); 193 | for(i = 1; i<=ec_slavecount ; i++) 194 | { 195 | if(ec_slave[i].state != EC_STATE_OPERATIONAL) 196 | { 197 | rt_printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", 198 | i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); 199 | } 200 | } 201 | for (i=0; ipre_dc32) //normal case 301 | diff_dc32=cur_dc32-pre_dc32; 302 | else //32-bit data overflow 303 | diff_dc32=(0xffffffff-pre_dc32)+cur_dc32; 304 | pre_dc32=cur_dc32; 305 | cur_DCtime+=diff_dc32; 306 | 307 | toff=dc_pi_sync(cur_DCtime, cycletime, shift_time); 308 | 309 | if (cur_DCtime>max_DCtime) max_DCtime=cur_DCtime; 310 | #endif 311 | 312 | //servo-on 313 | for (i=0; iStatusWord, &controlword); 317 | maxpos_drive_pt[i].ptOutParam->ControlWord=controlword; 318 | if (started[i]) ServoState |= (1<=3000) //wait for 3s after servo-on 328 | { 329 | ready_cnt=10000; 330 | sys_ready=1; 331 | } 332 | 333 | if (sys_ready) 334 | { 335 | ival=(int) (sine_amp*(sin(PI2*f*gt))); 336 | for (i=0; iTargetPosition=ival + zeropos[i]; 340 | else 341 | maxpos_drive_pt[i].ptOutParam->TargetPosition=-ival + zeropos[i]; 342 | } 343 | gt+=period; 344 | } 345 | else 346 | { 347 | for (i=0; iPositionActualValue; 350 | maxpos_drive_pt[i].ptOutParam->TargetPosition=zeropos[i]; 351 | } 352 | } 353 | 354 | 355 | if (sys_ready) 356 | if (worst_timeControlWord=0; //Servo OFF (Disable voltage, transition#9) 369 | } 370 | ec_send_processdata(); 371 | wkc = ec_receive_processdata(EC_TIMEOUTRET); 372 | 373 | rt_task_sleep(cycle_ns); 374 | 375 | rt_printf("End simple test, close socket\n"); 376 | /* stop SOEM, close socket */ 377 | printf("Request safe operational state for all slaves\n"); 378 | ec_slave[0].state = EC_STATE_SAFE_OP; 379 | /* request SAFE_OP state for all slaves */ 380 | ec_writestate(0); 381 | /* wait for all slaves to reach state */ 382 | ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); 383 | ec_slave[0].state = EC_STATE_PRE_OP; 384 | /* request SAFE_OP state for all slaves */ 385 | ec_writestate(0); 386 | /* wait for all slaves to reach state */ 387 | ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); 388 | 389 | ec_close(); 390 | } 391 | 392 | void print_run(void *arg) 393 | { 394 | int i; 395 | unsigned long itime=0; 396 | long stick=0; 397 | rt_task_set_periodic(NULL, TM_NOW, 1e8); 398 | 399 | while (run) 400 | { 401 | rt_task_wait_period(NULL); //wait for next cycle 402 | if (inOP==TRUE) 403 | { 404 | if (!sys_ready) 405 | { 406 | if (stick==0) 407 | rt_printf("waiting for system ready...\n"); 408 | if (stick%10==0) 409 | rt_printf("%i\n", stick/10); 410 | stick++; 411 | } 412 | else 413 | { 414 | itime++; 415 | rt_printf("Time=%06d.%01d, \e[32;1m fail=%ld\e[0m, ecat_T=%ld, maxT=%ld\n", itime/10, itime%10, recv_fail_cnt, ethercat_time/1000, worst_time/1000); 416 | for(i=0; iStatusWord); 420 | rt_printf("Actual Position = %i / %i\n", maxpos_drive_pt[i].ptInParam->PositionActualValue, maxpos_drive_pt[i].ptOutParam->TargetPosition); 421 | rt_printf("\n"); 422 | } 423 | rt_printf("\n"); 424 | 425 | } 426 | 427 | } 428 | } 429 | } 430 | 431 | void catch_signal(int sig) 432 | { 433 | run=0; 434 | usleep(5e5); 435 | rt_task_delete(&motion_task); 436 | rt_task_delete(&print_task); 437 | exit(1); 438 | } 439 | 440 | int main(int argc, char *argv[]) 441 | { 442 | signal(SIGTERM, catch_signal); 443 | signal(SIGINT, catch_signal); 444 | 445 | printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n"); 446 | mlockall(MCL_CURRENT | MCL_FUTURE); 447 | 448 | cycle_ns=1000000; // nanosecond 449 | period=((double) cycle_ns)/((double) NSEC_PER_SEC); //period in second unit 450 | if (argc > 1) 451 | { 452 | sine_amp=atoi(argv[1]); 453 | } 454 | printf("use default adapter %s\n", ecat_ifname); 455 | 456 | cpu_set_t cpu_set_ecat; 457 | CPU_ZERO(&cpu_set_ecat); 458 | CPU_SET(0, &cpu_set_ecat); //assign CPU#0 for ethercat task 459 | cpu_set_t cpu_set_print; 460 | CPU_ZERO(&cpu_set_print); 461 | CPU_SET(1, &cpu_set_print); //assign CPU#1 (or any) for main task 462 | 463 | rt_task_create(&motion_task, "SOEM_motion_task", 0, 90, 0 ); 464 | rt_task_set_affinity(&motion_task, &cpu_set_ecat); //CPU affinity for ethercat task 465 | 466 | rt_task_create(&print_task, "ec_printing", 0, 50, 0 ); 467 | rt_task_set_affinity(&print_task, &cpu_set_print); //CPU affinity for printing task 468 | 469 | rt_task_start(&motion_task, &demo_run, NULL); 470 | rt_task_start(&print_task, &print_run, NULL); 471 | 472 | while (run) 473 | { 474 | usleep(100000); 475 | } 476 | 477 | 478 | printf("End program\n"); 479 | return (0); 480 | } 481 | -------------------------------------------------------------------------------- /test/MAXPOS_CSP/pdo_def.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SOEM EtherCAT exmaple 3 | * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 4 | */ 5 | 6 | 7 | #ifndef _PDO_DEF_ 8 | #define _PDO_DEF_ 9 | 10 | #include "servo_def.h" 11 | 12 | //MAXPOS PDO mapping 13 | 14 | typedef union _mapping_obj{ 15 | uint32_t obj; 16 | struct { 17 | uint16_t index; 18 | uint8_t subindex; 19 | uint8_t size; 20 | }; 21 | } mapping_obj; 22 | 23 | //0x1600 RxPDO 24 | typedef struct PACKED 25 | { 26 | UINT16 ControlWord; //0x6040 27 | INT32 TargetPosition; //0x607A 28 | INT32 PositionOffset; //0x60B0 29 | INT32 VelocityOffset; //0x60B1 30 | INT16 TorqueOffset; //0x60B2 31 | UINT8 ModeOfOperation; //0x6060 32 | UINT32 PhysicalOutput; //0x60FE 33 | UINT16 TouchProbeFunction; //0x60B8 34 | }MAXPOS_DRIVE_RxPDO_t; 35 | 36 | //0x1A00 TxPDO 37 | typedef struct PACKED 38 | { 39 | UINT16 StatusWord; //0x6041 40 | INT32 PositionActualValue; //0x6064 41 | INT32 VelocityActualValue; //0x606C 42 | INT16 TorqueActualValue; //0x6077 43 | UINT8 ModeOfOperationDisplay; //0x6061 44 | UINT32 DigitalInput; //0x60FD 45 | UINT16 TouchProbeStatus; //0x60B9 46 | INT32 TouchProbePosition1; //0x60BA 47 | INT32 TouchProbePosition2; //0x60BB 48 | }MAXPOS_DRIVE_TxPDO_t; 49 | 50 | 51 | typedef struct _MAXPOS_ServoDrive 52 | { 53 | MAXPOS_DRIVE_RxPDO_t OutParam; 54 | MAXPOS_DRIVE_TxPDO_t InParam; 55 | } MAXPOS_ServoDrive_t; 56 | 57 | typedef struct _MAXPOS_Drive_pt 58 | { 59 | MAXPOS_DRIVE_RxPDO_t *ptOutParam; 60 | MAXPOS_DRIVE_TxPDO_t *ptInParam; 61 | } MAXPOS_Drive_pt; 62 | 63 | 64 | #endif //_PDO_DEF_ 65 | 66 | -------------------------------------------------------------------------------- /test/MAXPOS_CSP/servo.c: -------------------------------------------------------------------------------- 1 | /** 2 | * SOEM EtherCAT CiA402 example 3 | * Author: Ho Tam - thanhtam.h[at]gmail.com 4 | * 5 | * Description: implementation of CiA402 state machine for Servo On 6 | * Input: current status word read from servo drive 7 | * Output: next control word or command will be sent to servo drive in next cycle 8 | * Return value: drive has been in Operation or not 9 | */ 10 | 11 | #include "servo_def.h" 12 | 13 | int ServoOn_GetCtrlWrd(uint16_t StatusWord, uint16_t *ControlWord) 14 | { 15 | int _enable=0; 16 | if (bit_is_clear(StatusWord, STATUSWORD_OPERATION_ENABLE_BIT)) //Not ENABLED yet 17 | { 18 | if (bit_is_clear(StatusWord, STATUSWORD_SWITCHED_ON_BIT)) //Not SWITCHED ON yet 19 | { 20 | if (bit_is_clear(StatusWord, STATUSWORD_READY_TO_SWITCH_ON_BIT)) //Not READY to SWITCH ON yet 21 | { 22 | if (bit_is_set(StatusWord, STATUSWORD_FAULT_BIT)) //FAULT exist 23 | { 24 | (*ControlWord)=0x80; //FAULT RESET command 25 | } 26 | else //NO FAULT 27 | { 28 | (*ControlWord)=0x06; //SHUTDOWN command (transition#2) 29 | } 30 | } 31 | else //READY to SWITCH ON 32 | { 33 | (*ControlWord)=0x07; //SWITCH ON command (transition#3) 34 | } 35 | } 36 | else //has been SWITCHED ON 37 | { 38 | (*ControlWord)=0x0F; //ENABLE OPETATION command (transition#4) 39 | _enable=1; 40 | } 41 | } 42 | else //has been ENABLED 43 | { 44 | (*ControlWord)=0x0F; //maintain OPETATION state 45 | _enable=1; 46 | } 47 | return _enable;; 48 | } 49 | -------------------------------------------------------------------------------- /test/MAXPOS_CSP/servo_def.h: -------------------------------------------------------------------------------- 1 | /** 2 | * SOEM EtherCAT CiA402 example 3 | * 4 | * Author: Ho Tam - thanhtam.h[at]gmail.com 5 | */ 6 | 7 | #ifndef _SERVO_DEF_ 8 | #define _SERVO_DEF_ 9 | 10 | #include 11 | 12 | #define _BV(bit) (1 << (bit)) 13 | #define bit_is_set(val, bit) (val & _BV(bit)) 14 | #define bit_is_clear(val, bit) (!(val & _BV(bit))) 15 | #define sbi(val,bit) ((val) |= _BV(bit)) 16 | #define cbi(val,bit) ((val) &= ~_BV(bit)) 17 | 18 | #define STATUSWORD_READY_TO_SWITCH_ON_BIT 0 19 | #define STATUSWORD_SWITCHED_ON_BIT 1 20 | #define STATUSWORD_OPERATION_ENABLE_BIT 2 21 | #define STATUSWORD_FAULT_BIT 3 22 | #define STATUSWORD_VOLTAGE_ENABLE_BIT 4 23 | #define STATUSWORD_QUICK_STOP_BIT 5 24 | #define STATUSWORD_SWITCH_ON_DISABLE_BIT 6 25 | #define STATUSWORD_NO_USED_WARNING_BIT 7 26 | #define STATUSWORD_ELMO_NOT_USED_BIT 8 27 | #define STATUSWORD_REMOTE_BIT 9 28 | #define STATUSWORD_TARGET_REACHED_BIT 10 29 | #define STATUSWORD_INTERNAL_LIMIT_ACTIVE_BIT 11 30 | 31 | /** 32 | * Control CoE FSM(Finite State Machine) 33 | */ 34 | #define EC_CONTROL_COMMAND (0x02) 35 | #define CONTROL_COMMAND_DIABLE_VOLTAGE (0X0000) 36 | #define CONTROL_COMMAND_QUICK_STOP (0x0002) 37 | #define CONTROL_COMMAND_SHUTDOWN (0x0006) 38 | #define CONTROL_COMMAND_SWITCH_ON (0x0007) 39 | #define CONTROL_COMMAND_ENABLE_OPERATION (0x000f) 40 | #define CONTROL_COMMAND_SWITCH_ON_ENABLE_OPERATION (0x000f) 41 | #define CONTROL_COMMAND_DISABLE_OPERATION (0x0007) 42 | #define CONTROL_COMMAND_FAULT_RESET (0x0080) 43 | 44 | /** 45 | * Operation mode 46 | */ 47 | #define EC_OP_MODE (0x03) 48 | 49 | #define OP_MODE_NO_MODE (0x00) 50 | #define OP_MODE_PROFILE_POSITION (0X01) 51 | #define OP_MODE_VELOCITY (0X02) 52 | #define OP_MODE_PROFILE_VELOCITY (0X03) 53 | #define OP_MODE_TORQUE_PROFILE (0X04) 54 | #define OP_MODE_HOMING (0X06) 55 | #define OP_MODE_INTERPOLATED_POSITION (0X07) 56 | #define OP_MODE_CYCLIC_SYNC_POSITION (0X08) 57 | #define OP_MODE_CYCLIC_SYNC_VELOCITY (0X09) 58 | #define OP_MODE_CYCLIC_SYNC_TORQUE (0X0a) 59 | 60 | #define SERVO_TIMEOUT (50000) 61 | 62 | typedef int32_t INT32; 63 | typedef uint32_t UINT32; 64 | typedef int16_t INT16; 65 | typedef uint16_t UINT16; 66 | typedef uint8_t UINT8; 67 | 68 | #ifndef PI 69 | #define PI (3.14159265359) 70 | #define PI2 (6.28318530718) 71 | #define RAG2DEG 57.2957795131 72 | #define DEG2RAD 0.01745329252 73 | #endif 74 | 75 | #ifdef __cplusplus 76 | extern "C"{ 77 | #endif 78 | int ServoOn_GetCtrlWrd(uint16_t StatusWord, uint16_t *ControlWord); 79 | #ifdef __cplusplus 80 | } 81 | #endif 82 | 83 | #endif //_SERVO_DEF_ 84 | 85 | -------------------------------------------------------------------------------- /test/slaveInfo/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # 'make depend' uses makedepend to automatically generate dependencies 3 | # (dependencies are added to end of Makefile) 4 | # 'make' build executable file 'mycc' 5 | # 'make clean' removes all .o and executable files 6 | # 7 | 8 | # define the C compiler to use 9 | CC = gcc 10 | 11 | # define any compile-time flags 12 | skin = alchemy 13 | CFLAGS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --cflags) 14 | 15 | #SOEM EtherCAT 16 | CFLAGS += -I../../soem 17 | CFLAGS += -I../../osal 18 | CFLAGS += -I../../oshw 19 | CFLAGS += -O3 -Wall -g 20 | 21 | 22 | # define any libraries to link into executable: 23 | # if I want to link in libraries (libx.so or libx.a) I use the -llibname 24 | # option, something like (this will link in libmylib.so and libm.so: 25 | 26 | #SOEM EtherCAT 27 | LDLIBS += -L../../lib 28 | LDLIBS +=-lsoem 29 | LDLIBS +=-losal 30 | LDLIBS +=-loshw 31 | LDLIBS +=-lwiznet_drv 32 | LDLIBS += $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --ldflags) 33 | 34 | # define the C source files 35 | SRCS = slaveinfo.c 36 | 37 | # define the C object files 38 | # 39 | # This uses Suffix Replacement within a macro: 40 | # $(name:string1=string2) 41 | # For each word in 'name' replace 'string1' with 'string2' 42 | # Below we are replacing the suffix .c of all words in the macro SRCS 43 | # with the .o suffix 44 | # 45 | OBJS = $(SRCS:.c=.o) 46 | 47 | # define the executable file 48 | MAIN = slaveinfo 49 | 50 | # 51 | # The following part of the makefile is generic; it can be used to 52 | # build any executable just by changing the definitions above and by 53 | # deleting dependencies appended to the file from 'make depend' 54 | # 55 | 56 | .PHONY: depend clean 57 | 58 | all: $(MAIN) 59 | 60 | $(MAIN): $(OBJS) 61 | $(CC) $(CFLAGS) -o $(MAIN) $(OBJS) $(LDLIBS) 62 | 63 | # this is a suffix replacement rule for building .o's from .c's 64 | # it uses automatic variables $<: the name of the prerequisite of 65 | # the rule(a .c file) and $@: the name of the target of the rule (a .o file) 66 | # (see the gnu make manual section about automatic variables) 67 | .c.o: 68 | $(CC) $(CFLAGS) $(LDLIBS) -c $< -o $@ 69 | 70 | clean: 71 | $(RM) *.o *~ $(MAIN) 72 | 73 | 74 | # DO NOT DELETE THIS LINE -- make depend needs it 75 | -------------------------------------------------------------------------------- /test/xmc4800/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # 'make depend' uses makedepend to automatically generate dependencies 3 | # (dependencies are added to end of Makefile) 4 | # 'make' build executable file 'mycc' 5 | # 'make clean' removes all .o and executable files 6 | # 7 | 8 | # define the C compiler to use 9 | CC = gcc 10 | 11 | # define any compile-time flags 12 | skin = alchemy 13 | CFLAGS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --cflags) 14 | LDLIBS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --ldflags) 15 | 16 | #SOEM EtherCAT 17 | CFLAGS += -I../../soem 18 | CFLAGS += -I../../osal 19 | CFLAGS += -I../../oshw 20 | CFLAGS += -O3 -Wall -g 21 | 22 | 23 | # define any libraries to link into executable: 24 | # if I want to link in libraries (libx.so or libx.a) I use the -llibname 25 | # option, something like (this will link in libmylib.so and libm.so: 26 | 27 | 28 | #SOEM EtherCAT 29 | LDLIBS += -L../../lib 30 | LDLIBS +=-lsoem 31 | LDLIBS +=-losal 32 | LDLIBS +=-loshw 33 | LDLIBS +=-lwiznet_drv 34 | 35 | # define the C source files 36 | SRCS = main.c 37 | 38 | # define the C object files 39 | # 40 | # This uses Suffix Replacement within a macro: 41 | # $(name:string1=string2) 42 | # For each word in 'name' replace 'string1' with 'string2' 43 | # Below we are replacing the suffix .c of all words in the macro SRCS 44 | # with the .o suffix 45 | # 46 | OBJS = $(SRCS:.c=.o) 47 | 48 | # define the executable file 49 | MAIN = xmc4800 50 | 51 | # 52 | # The following part of the makefile is generic; it can be used to 53 | # build any executable just by changing the definitions above and by 54 | # deleting dependencies appended to the file from 'make depend' 55 | # 56 | 57 | .PHONY: depend clean 58 | 59 | all: $(MAIN) 60 | 61 | $(MAIN): $(OBJS) 62 | $(CC) $(CFLAGS) -o $(MAIN) $(OBJS) $(LDLIBS) 63 | 64 | # this is a suffix replacement rule for building .o's from .c's 65 | # it uses automatic variables $<: the name of the prerequisite of 66 | # the rule(a .c file) and $@: the name of the target of the rule (a .o file) 67 | # (see the gnu make manual section about automatic variables) 68 | .c.o: 69 | $(CC) $(CFLAGS) $(LDLIBS) -c $< -o $@ 70 | 71 | clean: 72 | $(RM) *.o *~ $(MAIN) 73 | 74 | 75 | # DO NOT DELETE THIS LINE -- make depend needs it -------------------------------------------------------------------------------- /test/xmc4800/main.c: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Example code for Simple Open EtherCAT master 3 | * 4 | * Usage : simple_test [ifname1] 5 | * ifname is NIC interface, f.e. eth0 6 | * 7 | * This is a minimal test. 8 | * 9 | * (c)Arthur Ketels 2010 - 2011 10 | * 11 | * Port for Raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | //#include 23 | #include 24 | #include 25 | 26 | #include "ethercattype.h" 27 | #include "nicdrv.h" 28 | #include "ethercatbase.h" 29 | #include "ethercatmain.h" 30 | #include "ethercatdc.h" 31 | #include "ethercatcoe.h" 32 | #include "ethercatfoe.h" 33 | #include "ethercatconfig.h" 34 | #include "ethercatprint.h" 35 | 36 | #include "pdo_def.h" 37 | 38 | #define NSEC_PER_SEC 1000000000 39 | #define EC_TIMEOUTMON 500 40 | 41 | #define x_USE_DC 42 | #define x_WRITE_MODEOP_SDO 43 | 44 | #define NUMOFXMC 1 45 | 46 | slave_XMC4800_pt xmc_module_pt[NUMOFXMC]; 47 | 48 | 49 | unsigned int cycle_ns = 1000000; /* 1 ms */ 50 | 51 | int print_enable=0; 52 | 53 | char IOmap[4096]; 54 | //char IOmap[1024]; 55 | pthread_t thread1; 56 | int expectedWKC; 57 | boolean needlf; 58 | volatile int wkc; 59 | boolean inOP; 60 | uint8 currentgroup = 0; 61 | 62 | RT_TASK demo_task; 63 | RT_TASK print_task; 64 | RT_TASK eccheck_task; 65 | //RT_MUTEX mutex_desc; 66 | 67 | RTIME now, previous; 68 | long ethercat_time=0, worst_time=0; 69 | char ecat_ifname[32]="wiznet"; 70 | int run=1; 71 | int LED_Byte_idx=8, BTN_Byte_idx=8; //HoTam, for XMC4800 kit 72 | uint8 input_bnt1=0; 73 | uint8 input_bnt2=0; 74 | int sys_ready=0; 75 | 76 | int recv_fail_cnt=0; 77 | 78 | double gt=0; 79 | /// TO DO: This is user-code. 80 | double sine_amp=5000, f=1, period; 81 | 82 | boolean ecat_init(void) 83 | { 84 | int i, oloop, iloop; 85 | needlf = FALSE; 86 | inOP = FALSE; 87 | 88 | rt_printf("Starting simple test\n"); 89 | 90 | if (ec_init(ecat_ifname)) 91 | { 92 | rt_printf("ec_init on %s succeeded.\n", ecat_ifname); //ifname 93 | /* find and auto-config slaves */ 94 | 95 | if ( ec_config_init(FALSE) > 0 ) 96 | { 97 | rt_printf("%d slaves found and configured.\n",ec_slavecount); 98 | 99 | ec_config_map(&IOmap); 100 | 101 | #ifdef _USE_DC 102 | ec_configdc(); 103 | #endif 104 | rt_printf("Slaves mapped, state to SAFE_OP.\n"); 105 | /* wait for all slaves to reach SAFE_OP state */ 106 | ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); 107 | #ifdef _USE_DC 108 | //NEW, FOR DC---- 109 | /* configure DC options for every DC capable slave found in the list */ 110 | rt_printf("DC capable : %d\n",ec_configdc()); 111 | //--------------- 112 | #endif 113 | oloop = ec_slave[0].Obytes; 114 | if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; 115 | //if (oloop > 8) oloop = 8; 116 | iloop = ec_slave[0].Ibytes; 117 | if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; 118 | //if (iloop > 8) iloop = 8; 119 | 120 | rt_printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]); 121 | 122 | rt_printf("Request operational state for all slaves\n"); 123 | expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; 124 | rt_printf("Calculated workcounter %d\n", expectedWKC); 125 | ec_slave[0].state = EC_STATE_OPERATIONAL; 126 | /* send one valid process data to make outputs in slaves happy*/ 127 | ec_send_processdata(); 128 | ec_receive_processdata(EC_TIMEOUTRET); 129 | /* request OP state for all slaves */ 130 | 131 | ec_writestate(0); 132 | ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); //wait for OP 133 | 134 | if (ec_slave[0].state == EC_STATE_OPERATIONAL ) 135 | { 136 | rt_printf("Operational state reached for all slaves.\n"); 137 | 138 | int k; 139 | for (k=0; koutgenbit1=0; 186 | xmc_module_pt[i].ptout_data->outgenbit2=0; 187 | xmc_module_pt[i].ptout_data->outgenbit3=0; 188 | xmc_module_pt[i].ptout_data->outgenbit4=0; 189 | xmc_module_pt[i].ptout_data->outgenbit5=0; 190 | xmc_module_pt[i].ptout_data->outgenbit6=0; 191 | xmc_module_pt[i].ptout_data->outgenbit7=0; 192 | xmc_module_pt[i].ptout_data->outgenbit8=0; 193 | 194 | switch (idx) 195 | { 196 | case 0: 197 | xmc_module_pt[i].ptout_data->outgenbit1=1; 198 | break; 199 | case 1: 200 | xmc_module_pt[i].ptout_data->outgenbit2=1; 201 | break; 202 | case 2: 203 | xmc_module_pt[i].ptout_data->outgenbit3=1; 204 | break; 205 | case 3: 206 | xmc_module_pt[i].ptout_data->outgenbit4=1; 207 | break; 208 | case 4: 209 | xmc_module_pt[i].ptout_data->outgenbit5=1; 210 | break; 211 | case 5: 212 | xmc_module_pt[i].ptout_data->outgenbit6=1; 213 | break; 214 | case 6: 215 | xmc_module_pt[i].ptout_data->outgenbit7=1; 216 | break; 217 | case 7: 218 | xmc_module_pt[i].ptout_data->outgenbit8=1; 219 | break; 220 | } 221 | } 222 | 223 | } 224 | 225 | 226 | // main task 227 | void demo_run(void *arg) 228 | { 229 | RTIME now, previous; 230 | unsigned long led_loop_cnt=0; 231 | unsigned long ready_cnt=0; 232 | int LED_idx=0; 233 | int LED_Step=1; 234 | 235 | wiznet_hw_config(8, 1, 1000000); //select SPI-W5500 parameters, before ec_init 236 | if (ecat_init()==FALSE) 237 | { 238 | run =0; 239 | printf("fail\n"); 240 | return; //all initialization stuffs here 241 | } 242 | 243 | rt_task_sleep(1e6); 244 | rt_task_set_periodic(NULL, TM_NOW, cycle_ns); 245 | 246 | while (run) 247 | { 248 | rt_task_wait_period(NULL); //wait for next cycle 249 | 250 | previous = rt_timer_read(); 251 | 252 | ec_send_processdata(); 253 | wkc = ec_receive_processdata(EC_TIMEOUTRET); 254 | if (wkc<3*(NUMOFXMC)) recv_fail_cnt++; 255 | 256 | now = rt_timer_read(); 257 | ethercat_time = (long) now - previous; 258 | if (sys_ready) 259 | if (worst_time=1000) 263 | { 264 | ready_cnt=6000; 265 | sys_ready=1; 266 | } 267 | 268 | if (sys_ready) 269 | { 270 | if (++led_loop_cnt>=100) 271 | { 272 | led_loop_cnt=0; 273 | LED_update(LED_idx); 274 | LED_idx+=LED_Step; 275 | if (LED_idx>=7) LED_Step=-1; 276 | if (LED_idx<1) LED_Step=1; 277 | } 278 | gt+=period; 279 | } 280 | 281 | } 282 | 283 | rt_printf("End simple test, close socket\n"); 284 | /* stop SOEM, close socket */ 285 | printf("Request safe operational state for all slaves\n"); 286 | ec_slave[0].state = EC_STATE_SAFE_OP; 287 | /* request SAFE_OP state for all slaves */ 288 | ec_writestate(0); 289 | /* wait for all slaves to reach state */ 290 | ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); 291 | ec_slave[0].state = EC_STATE_PRE_OP; 292 | /* request SAFE_OP state for all slaves */ 293 | ec_writestate(0); 294 | /* wait for all slaves to reach state */ 295 | ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE); 296 | 297 | ec_close(); 298 | } 299 | 300 | void print_run(void *arg) 301 | { 302 | int i; 303 | unsigned long itime=0; 304 | long stick=0; 305 | 306 | rt_task_set_periodic(NULL, TM_NOW, 1e8); 307 | 308 | while (run) 309 | { 310 | rt_task_wait_period(NULL); //wait for next cycle 311 | if (inOP==TRUE) 312 | { 313 | if (!sys_ready) 314 | { 315 | if (stick==0) 316 | rt_printf("waiting for system ready...\n"); 317 | if (stick%10==0) 318 | rt_printf("%i\n", stick/10); 319 | stick++; 320 | } 321 | else 322 | { 323 | itime++; 324 | rt_printf("Time=%06d.%01d, \e[32;1m fail=%ld\e[0m, ecat_T=%ld, maxT=%ld\n", itime/10, itime%10, recv_fail_cnt, ethercat_time/1000, worst_time/1000); 325 | for(i=0; iingenbit1, xmc_module_pt[i].ptin_data->ingenbit2); 327 | rt_printf("\n"); 328 | 329 | } 330 | 331 | } 332 | } 333 | } 334 | 335 | 336 | void catch_signal(int sig) 337 | { 338 | run=0; 339 | usleep(1e5); 340 | rt_task_delete(&demo_task); 341 | rt_task_delete(&print_task); 342 | exit(1); 343 | } 344 | 345 | int main(int argc, char *argv[]) 346 | { 347 | signal(SIGTERM, catch_signal); 348 | signal(SIGINT, catch_signal); 349 | 350 | printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n"); 351 | 352 | mlockall(MCL_CURRENT | MCL_FUTURE); 353 | 354 | cycle_ns=1000000; // nanosecond 355 | period=((double) cycle_ns)/((double) NSEC_PER_SEC); //period in second unit 356 | 357 | printf("use default adapter %s\n", ecat_ifname); 358 | 359 | 360 | rt_task_create(&demo_task, "SOEM_demo_task", 0, 90, 0 ); 361 | rt_task_create(&print_task, "ec_printing", 0, 50, 0 ); 362 | 363 | rt_task_start(&demo_task, &demo_run, NULL); 364 | rt_task_start(&print_task, &print_run, NULL); 365 | 366 | while (run) 367 | { 368 | usleep(10000); 369 | } 370 | 371 | 372 | printf("End program\n"); 373 | return (0); 374 | } 375 | -------------------------------------------------------------------------------- /test/xmc4800/pdo_def.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SOEM EtherCAT exmaple 3 | * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 4 | */ 5 | 6 | #ifndef _PDO_DEF_ 7 | #define _PDO_DEF_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | typedef struct PACKED 19 | { 20 | uint8 outgenbit1:1; 21 | uint8 outgenbit2:1; 22 | uint8 outgenbit3:1; 23 | uint8 outgenbit4:1; 24 | uint8 outgenbit5:1; 25 | uint8 outgenbit6:1; 26 | uint8 outgenbit7:1; 27 | uint8 outgenbit8:1; 28 | } out_XMC4800_t; 29 | 30 | typedef struct PACKED 31 | { 32 | uint8 ingenbit1:1; 33 | uint8 ingenbit2:1; 34 | } in_XMC4800_t; 35 | 36 | typedef struct 37 | { 38 | out_XMC4800_t out_data; 39 | in_XMC4800_t in_data; 40 | } slave_XMC4800_t; 41 | 42 | typedef struct 43 | { 44 | out_XMC4800_t *ptout_data; 45 | in_XMC4800_t *ptin_data; 46 | } slave_XMC4800_pt; 47 | 48 | 49 | 50 | #endif //_PDO_DEF_ 51 | 52 | -------------------------------------------------------------------------------- /test/xmc4800_dc/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # 'make depend' uses makedepend to automatically generate dependencies 3 | # (dependencies are added to end of Makefile) 4 | # 'make' build executable file 'mycc' 5 | # 'make clean' removes all .o and executable files 6 | # 7 | 8 | # define the C compiler to use 9 | CC = gcc 10 | 11 | # define any compile-time flags 12 | skin = alchemy 13 | CFLAGS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --cflags) 14 | LDLIBS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --ldflags) 15 | 16 | #SOEM EtherCAT 17 | CFLAGS += -I../../soem 18 | CFLAGS += -I../../osal 19 | CFLAGS += -I../../oshw 20 | CFLAGS += -O3 -Wall -g 21 | 22 | 23 | # define any libraries to link into executable: 24 | # if I want to link in libraries (libx.so or libx.a) I use the -llibname 25 | # option, something like (this will link in libmylib.so and libm.so: 26 | 27 | 28 | #SOEM EtherCAT 29 | LDLIBS += -L../../lib 30 | LDLIBS +=-lsoem 31 | LDLIBS +=-losal 32 | LDLIBS +=-loshw 33 | LDLIBS +=-lwiznet_drv 34 | 35 | # define the C source files 36 | SRCS = main.c ecat_dc.c 37 | 38 | # define the C object files 39 | # 40 | # This uses Suffix Replacement within a macro: 41 | # $(name:string1=string2) 42 | # For each word in 'name' replace 'string1' with 'string2' 43 | # Below we are replacing the suffix .c of all words in the macro SRCS 44 | # with the .o suffix 45 | # 46 | OBJS = $(SRCS:.c=.o) 47 | 48 | # define the executable file 49 | MAIN = xmc4800_dc 50 | 51 | # 52 | # The following part of the makefile is generic; it can be used to 53 | # build any executable just by changing the definitions above and by 54 | # deleting dependencies appended to the file from 'make depend' 55 | # 56 | 57 | .PHONY: depend clean 58 | 59 | all: $(MAIN) 60 | 61 | $(MAIN): $(OBJS) 62 | $(CC) $(CFLAGS) -o $(MAIN) $(OBJS) $(LDLIBS) 63 | 64 | # this is a suffix replacement rule for building .o's from .c's 65 | # it uses automatic variables $<: the name of the prerequisite of 66 | # the rule(a .c file) and $@: the name of the target of the rule (a .o file) 67 | # (see the gnu make manual section about automatic variables) 68 | .c.o: 69 | $(CC) $(CFLAGS) $(LDLIBS) -c $< -o $@ 70 | 71 | clean: 72 | $(RM) *.o *~ $(MAIN) 73 | 74 | 75 | # DO NOT DELETE THIS LINE -- make depend needs it -------------------------------------------------------------------------------- /test/xmc4800_dc/ecat_dc.c: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include "ecat_dc.h" 5 | /* 6 | * SOEM EtherCAT exmaple 7 | * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 8 | */ 9 | 10 | //convert float, double to fixed point 11 | void double_to_fixed(double f_input, int32_t *pValue, int32_t *pBase) 12 | { 13 | if (f_input<1.0) 14 | { 15 | (*pBase)=15; 16 | (*pValue)=(int32_t) (32768.0*f_input); 17 | } 18 | else if (f_input<2.0) 19 | { 20 | (*pBase)=14; 21 | (*pValue)=(int32_t) (16384.0*f_input); 22 | } 23 | else if (f_input<4.0) 24 | { 25 | (*pBase)=13; 26 | (*pValue)=(int32_t) (8192.0*f_input); 27 | } 28 | else if (f_input<8.0) 29 | { 30 | (*pBase)=12; 31 | (*pValue)=(int32_t) (4096.0*f_input); 32 | } 33 | else if (f_input<16.0) 34 | { 35 | (*pBase)=11; 36 | (*pValue)=(int32_t) (2048.0*f_input); 37 | } 38 | else if (f_input<32.0) 39 | { 40 | (*pBase)=10; 41 | (*pValue)=(int32_t) (1024.0*f_input); 42 | } 43 | else if (f_input<64.0) 44 | { 45 | (*pBase)=9; 46 | (*pValue)=(int32_t) (512.0*f_input); 47 | } 48 | else if (f_input<128.0) 49 | { 50 | (*pBase)=8; 51 | (*pValue)=(int32_t) (256.0*f_input); 52 | } 53 | else if (f_input<256.0) 54 | { 55 | (*pBase)=7; 56 | (*pValue)=(int32_t) (128.0*f_input); 57 | } 58 | else if (f_input<512.0) 59 | { 60 | (*pBase)=6; 61 | (*pValue)=(int32_t) (64.0*f_input); 62 | } 63 | else if (f_input<1024.0) 64 | { 65 | (*pBase)=5; 66 | (*pValue)=(int32_t) (32.0*f_input); 67 | } 68 | else if (f_input<2048.0) 69 | { 70 | (*pBase)=4; 71 | (*pValue)=(int32_t) (16.0*f_input); 72 | } 73 | else if (f_input<4096.0) 74 | { 75 | (*pBase)=3; 76 | (*pValue)=(int32_t) (8.0*f_input); 77 | } 78 | else if (f_input<81928.0) 79 | { 80 | (*pBase)=2; 81 | (*pValue)=(int32_t) (4.0*f_input); 82 | } 83 | else if (f_input<16384.0) 84 | { 85 | (*pBase)=1; 86 | (*pValue)=(int32_t) (2.0*f_input); 87 | } 88 | else if (f_input<32768.0) 89 | { 90 | (*pBase)=0; 91 | (*pValue)=(int32_t) (1.0*f_input); 92 | } 93 | } 94 | 95 | 96 | 97 | #define PI_SAT_VAL 50000 98 | 99 | int32_t _pPart=0, _iPart=0; 100 | int32_t _sync_err=0, _sync_pre_err=0; 101 | 102 | double Kp=0.1, Ki=0.0005; 103 | //double Kp=0.1, Ki=0.001; 104 | 105 | //PI compensation 106 | long long dc_pi_sync(long long reftime, long long cycletime, int32_t shift_time) 107 | { 108 | long long adj_time=0; 109 | int32_t iKp=0, iKp_Base, iKi=0, iKi_Base; 110 | double_to_fixed(Kp, &iKp, &iKp_Base); 111 | double_to_fixed(Ki, &iKi, &iKi_Base); 112 | 113 | _sync_err = (reftime - shift_time) % cycletime; 114 | if(_sync_err> (cycletime /2)) { _sync_err= _sync_err - cycletime; } 115 | 116 | _pPart=_sync_err*iKp; 117 | _iPart+=(_sync_err+_sync_pre_err)*iKi; 118 | _sync_pre_err=_sync_err; 119 | 120 | adj_time = -(_pPart>>iKp_Base) - (_iPart>>iKi_Base); 121 | 122 | if (adj_time>PI_SAT_VAL) adj_time=PI_SAT_VAL; 123 | if (adj_time<-PI_SAT_VAL) adj_time=-PI_SAT_VAL; 124 | 125 | return adj_time; 126 | 127 | } 128 | 129 | -------------------------------------------------------------------------------- /test/xmc4800_dc/ecat_dc.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SOEM EtherCAT exmaple 3 | * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 4 | */ 5 | 6 | #ifndef _ECAT_DC_H_ 7 | #define _ECAT_DC_H_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | //PI compensation 19 | long long dc_pi_sync(long long reftime, long long cycletime, int32_t shift_time); 20 | #endif //_ECAT_DC_H_ 21 | 22 | -------------------------------------------------------------------------------- /test/xmc4800_dc/main.c: -------------------------------------------------------------------------------- 1 | /** \file 2 | * \brief Example code for Simple Open EtherCAT master 3 | * 4 | * Usage : simple_test [ifname1] 5 | * ifname is NIC interface, f.e. eth0 6 | * 7 | * This is a minimal test. 8 | * 9 | * (c)Arthur Ketels 2010 - 2011 10 | * 11 | * Port for Raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 12 | */ 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "ethercattype.h" 26 | #include "nicdrv.h" 27 | #include "ethercatbase.h" 28 | #include "ethercatmain.h" 29 | #include "ethercatdc.h" 30 | #include "ethercatcoe.h" 31 | #include "ethercatfoe.h" 32 | #include "ethercatconfig.h" 33 | #include "ethercatprint.h" 34 | #include "pdo_def.h" 35 | #include "ecat_dc.h" 36 | #include "wiznet_drv.h" 37 | 38 | #define NSEC_PER_SEC 1000000000 39 | #define EC_TIMEOUTMON 500 40 | 41 | #define NUMOFXMC 4 42 | 43 | #define _USE_DC 44 | 45 | slave_XMC4800_pt xmc_module_pt[NUMOFXMC]; 46 | 47 | unsigned int cycle_ns = 1000000; /* 1 ms */ 48 | 49 | int print_enable=0; 50 | 51 | char IOmap[4096]; 52 | //char IOmap[1024]; 53 | pthread_t thread1; 54 | int expectedWKC; 55 | boolean needlf; 56 | volatile int wkc; 57 | boolean inOP; 58 | uint8 currentgroup = 0; 59 | 60 | RT_TASK demo_task; 61 | RT_TASK print_task; 62 | RT_TASK eccheck_task; 63 | //RT_MUTEX mutex_desc; 64 | 65 | RTIME now, previous; 66 | long ethercat_time_send, ethercat_time_read=0; 67 | long ethercat_time=0, worst_time=0; 68 | char ecat_ifname[32]="wiznet"; 69 | int run=1; 70 | int LED_Byte_idx=8, BTN_Byte_idx=8; //HoTam, for XMC4800 kit 71 | uint8 input_bnt1=0; 72 | uint8 input_bnt2=0; 73 | int sys_ready=0; 74 | 75 | int recv_fail_cnt=0; 76 | 77 | double gt=0; 78 | /// TO DO: This is user-code. 79 | double sine_amp=5000, f=1, period; 80 | 81 | boolean ecat_init(void) 82 | { 83 | int i, oloop, iloop; 84 | needlf = FALSE; 85 | inOP = FALSE; 86 | 87 | rt_printf("Starting simple test\n"); 88 | 89 | wiznet_hw_config(16, 1, 1000000); //select SPI-W5500 parameters, before ecat_init 90 | 91 | if (ec_init(ecat_ifname)) 92 | { 93 | rt_printf("ec_init on %s succeeded.\n", ecat_ifname); //ifname 94 | /* find and auto-config slaves */ 95 | 96 | if ( ec_config_init(FALSE) > 0 ) 97 | { 98 | rt_printf("%d slaves found and configured.\n",ec_slavecount); 99 | ec_config_map(&IOmap); 100 | #ifdef _USE_DC 101 | ec_configdc(); 102 | #endif 103 | rt_printf("Slaves mapped, state to SAFE_OP.\n"); 104 | /* wait for all slaves to reach SAFE_OP state */ 105 | ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); 106 | #ifdef _USE_DC 107 | //NEW, FOR DC---- 108 | /* configure DC options for every DC capable slave found in the list */ 109 | rt_printf("DC capable : %d\n",ec_configdc()); 110 | //--------------- 111 | #endif 112 | oloop = ec_slave[0].Obytes; 113 | if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; 114 | //if (oloop > 8) oloop = 8; 115 | iloop = ec_slave[0].Ibytes; 116 | if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; 117 | //if (iloop > 8) iloop = 8; 118 | 119 | rt_printf("segments : %d : %d %d %d %d\n",ec_group[0].nsegments ,ec_group[0].IOsegment[0],ec_group[0].IOsegment[1],ec_group[0].IOsegment[2],ec_group[0].IOsegment[3]); 120 | 121 | rt_printf("Request operational state for all slaves\n"); 122 | expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; 123 | rt_printf("Calculated workcounter %d\n", expectedWKC); 124 | ec_slave[0].state = EC_STATE_OPERATIONAL; 125 | /* send one valid process data to make outputs in slaves happy*/ 126 | ec_send_processdata(); 127 | ec_receive_processdata(EC_TIMEOUTRET); 128 | /* request OP state for all slaves */ 129 | 130 | ec_writestate(0); 131 | ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE); //wait for OP 132 | 133 | if (ec_slave[0].state == EC_STATE_OPERATIONAL ) 134 | { 135 | rt_printf("Operational state reached for all slaves.\n"); 136 | 137 | int k; 138 | for (k=0; koutgenbit1=0; 194 | xmc_module_pt[i].ptout_data->outgenbit2=0; 195 | xmc_module_pt[i].ptout_data->outgenbit3=0; 196 | xmc_module_pt[i].ptout_data->outgenbit4=0; 197 | xmc_module_pt[i].ptout_data->outgenbit5=0; 198 | xmc_module_pt[i].ptout_data->outgenbit6=0; 199 | xmc_module_pt[i].ptout_data->outgenbit7=0; 200 | xmc_module_pt[i].ptout_data->outgenbit8=0; 201 | } 202 | 203 | switch (led_idx) 204 | { 205 | case 0: 206 | xmc_module_pt[n].ptout_data->outgenbit1=1; 207 | break; 208 | case 1: 209 | xmc_module_pt[n].ptout_data->outgenbit2=1; 210 | break; 211 | case 2: 212 | xmc_module_pt[n].ptout_data->outgenbit3=1; 213 | break; 214 | case 3: 215 | xmc_module_pt[n].ptout_data->outgenbit4=1; 216 | break; 217 | case 4: 218 | xmc_module_pt[n].ptout_data->outgenbit5=1; 219 | break; 220 | case 5: 221 | xmc_module_pt[n].ptout_data->outgenbit6=1; 222 | break; 223 | case 6: 224 | xmc_module_pt[n].ptout_data->outgenbit7=1; 225 | break; 226 | case 7: 227 | xmc_module_pt[n].ptout_data->outgenbit8=1; 228 | break; 229 | } 230 | 231 | } 232 | 233 | // main task 234 | void demo_run(void *arg) 235 | { 236 | RTIME now, previous; 237 | unsigned long led_loop_cnt=0; 238 | unsigned long ready_cnt=0; 239 | int LED_idx=0; 240 | 241 | //for dc computation 242 | long long toff; 243 | long long cur_DCtime=0, max_DCtime=0; 244 | unsigned long long cur_dc32=0, pre_dc32=0; 245 | int32_t shift_time=380000; //dc event shifted compared to master reference clock 246 | long long diff_dc32; 247 | 248 | //int LED_Step=1; 249 | if (ecat_init()==FALSE) 250 | { 251 | run =0; 252 | printf("fail\n"); 253 | return; //all initialization stuffs here 254 | } 255 | 256 | rt_task_sleep(1e6); 257 | #ifdef _USE_DC 258 | int i; 259 | for (i=0; ipre_dc32) //normal case 312 | diff_dc32=cur_dc32-pre_dc32; 313 | else //32-bit data overflow 314 | diff_dc32=(0xffffffff-pre_dc32)+cur_dc32; 315 | pre_dc32=cur_dc32; 316 | cur_DCtime+=diff_dc32; 317 | 318 | toff=dc_pi_sync(cur_DCtime, cycletime, shift_time); 319 | 320 | if (cur_DCtime>max_DCtime) max_DCtime=cur_DCtime; 321 | #endif 322 | 323 | ready_cnt++; 324 | if (ready_cnt>=1000) 325 | { 326 | ready_cnt=6000; 327 | sys_ready=1; 328 | } 329 | 330 | if (sys_ready) 331 | { 332 | if (++led_loop_cnt>=100) 333 | { 334 | led_loop_cnt=0; 335 | LED_update(LED_idx); 336 | LED_idx++; 337 | if (LED_idx>=(NUMOFXMC*8)) LED_idx=0; 338 | 339 | } 340 | gt+=period; 341 | } 342 | 343 | if (sys_ready) 344 | if (worst_timeingenbit1, xmc_module_pt[i].ptin_data->ingenbit2); 402 | rt_printf("\n"); 403 | 404 | } 405 | 406 | } 407 | } 408 | } 409 | 410 | 411 | void catch_signal(int sig) 412 | { 413 | run=0; 414 | usleep(5e5); 415 | rt_task_delete(&demo_task); 416 | rt_task_delete(&print_task); 417 | exit(1); 418 | } 419 | 420 | int main(int argc, char *argv[]) 421 | { 422 | signal(SIGTERM, catch_signal); 423 | signal(SIGINT, catch_signal); 424 | 425 | printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n"); 426 | 427 | mlockall(MCL_CURRENT | MCL_FUTURE); 428 | 429 | cycle_ns=1000000; // nanosecond 430 | period=((double) cycle_ns)/((double) NSEC_PER_SEC); //period in second unit 431 | 432 | printf("use default adapter %s\n", ecat_ifname); 433 | 434 | rt_task_create(&demo_task, "SOEM_demo_task", 0, 90, 0 ); 435 | rt_task_create(&print_task, "ec_printing", 0, 50, 0 ); 436 | 437 | rt_task_start(&demo_task, &demo_run, NULL); 438 | rt_task_start(&print_task, &print_run, NULL); 439 | 440 | while (run) 441 | { 442 | usleep(10000); 443 | } 444 | 445 | 446 | printf("End program\n"); 447 | return (0); 448 | } 449 | -------------------------------------------------------------------------------- /test/xmc4800_dc/pdo_def.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SOEM EtherCAT exmaple 3 | * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 4 | */ 5 | 6 | #ifndef _PDO_DEF_ 7 | #define _PDO_DEF_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | typedef struct PACKED 19 | { 20 | uint16 outgenint1; 21 | uint16 outgenint2; 22 | uint16 outgenint3; 23 | uint16 outgenint4; 24 | uint8 outgenbit1:1; 25 | uint8 outgenbit2:1; 26 | uint8 outgenbit3:1; 27 | uint8 outgenbit4:1; 28 | uint8 outgenbit5:1; 29 | uint8 outgenbit6:1; 30 | uint8 outgenbit7:1; 31 | uint8 outgenbit8:1; 32 | } out_XMC4800_t; 33 | 34 | typedef struct PACKED 35 | { 36 | uint16 ingenint1; 37 | uint16 ingenint2; 38 | uint16 ingenint3; 39 | uint16 ingenint4; 40 | uint8 ingenbit1:1; 41 | uint8 ingenbit2:1; 42 | uint8 ingenbit3:1; 43 | uint8 ingenbit4:1; 44 | uint8 ingenbit5:1; 45 | uint8 ingenbit6:1; 46 | uint8 ingenbit7:1; 47 | uint8 ingenbit8:1; 48 | } in_XMC4800_t; 49 | 50 | typedef struct 51 | { 52 | out_XMC4800_t out_data; 53 | in_XMC4800_t in_data; 54 | } slave_XMC4800_t; 55 | 56 | typedef struct 57 | { 58 | out_XMC4800_t *ptout_data; 59 | in_XMC4800_t *ptin_data; 60 | } slave_XMC4800_pt; 61 | 62 | 63 | 64 | #endif //_PDO_DEF_ 65 | 66 | --------------------------------------------------------------------------------