在RT-Thread上移植EtherCAT开源主站SOEM1.4.0
本文已参与「开源摘星计划」，欢迎正在阅读的你加入。活动链接：https://github.com/weopenprojects/WeOpen-Star
目录
1．背景
2．EtherCAT主站软件方案
3. 移植过程
3.1 RT-Thread 下载
3.2 Some移植
3.2.1 osal.c移植
3.2.2 oshw.c移植
3.2.3 nicdrv.c移植
3.2.4 net_hook.c实现
3.2.5 some基本功能测试
4. 运动控制测试
5. 总结
1．背景
最近计划DIY一个EtherCAT控制器，一直在看资料和选型，初步定了NUC980的方案，主要是看中NUC980的RAM比较大，采购还算方便（最近缺芯，大家都懂）。
选定硬件之后，NUC980跑什么系统呢？从以往经验来看，ARM9一般跑linux比较多，资源也好找，同时官方提供BSP。但这次任务有点特殊，EtherCAT对实时性要求比较高，linux不是最合适的。结合MCU开发经验，就准备上个RTOS，以前M3和M4的芯片，主要用FreeRTOS，最近也开始用RT-Thread。新唐官方也推出了NUC980的RT-Thread版BSP，对开发者非常友好，最终决定了NUC980+RT-Thread的方案。
2．EtherCAT主站软件方案
EtherCAT本身还是比较复杂的，我们就不自己造轮子了，考虑用开源方案，毕竟硬件成本这么低，商用方案是真的用不起啊！
现在开源主站主要就两种，SOEM和IGH（相关的资料网上很多，这儿就不展开了），IGH只支持linux，所以只能选SOME，最新版本是SOEM1.4，本次移植就基于该版本。
3. 移植过程
3.1 RT-Thread 下载
这个可以到github下载，最近Gitee也更新了。移植EtherCAT之前，首先把Nuvoton的BSP跑起来。这个参考官方的文档就可以了。本次移植基于最新发布的RT-Thread release 4.1.0。
3.2 Some移植
下载soem-1.4.0，将整个目录放在rt-thread项目里，下图是我的目录，供参考
在some-1.4.0及其子目录中需要手工编辑SConscript脚本。
import os
from building import *
objs = []
cwd = GetCurrentDir()
list = os.listdir(cwd)
for item in list:
if os.path.isfile(os.path.join(cwd, item, 'SConscript')):
objs = objs + SConscript(os.path.join(item, 'SConscript'))
Return('objs')
Some移植主要是三个文件 osal.c，oshw.c和nicdrv.c。
osal.c 主要是微秒及的延时和定时函数；
oshw.c 主要是网络端和本机端数据的大小端转换；
nicdrv.c 主要是网络数据收发。
Some已经给出了很多操作系统移植，我的移植是基于rtk，这个是嵌入式系统实现，和我们的开发环境最接近。
3.2.1 osal.c移植
主要内容是实现osal_usleep和osal_gettimeofday两个函数。
我开始思路是自定义一个定时器用于EtherCAT，当时用了Timer4。等实现差不多了，发现系统时钟用的是Timer5，很多地方功能重复。最终和系统共用Timer5，省了个Timer，代码也简化了不少。下面就是改动过的相关代码，osal_timer_init这个初始化函数要在启动EhterCAT功能之前调用。
#include
#include
#include
#include
#include
#include "NuMicro.h"
#include "drv_sys.h"
static rt_uint32_t us_ticks;
void osal_timer_init(void)
{
rt_uint32_t cmp = ETIMER_GetCompareData(5);
us_ticks = 1 * cmp / (1000000 / RT_TICK_PER_SECOND);
rt_kprintf("rt-thread hwtimer5 1us = %d ticks\n", us_ticks);
}
#define timercmp(a, b, CMP) \
(((a)->tv_sec == (b)->tv_sec) ? \
((a)->tv_usec CMP (b)->tv_usec) : \
((a)->tv_sec CMP (b)->tv_sec))
#define timeradd(a, b, result) \
do { \
(result)->tv_sec = (a)->tv_sec + (b)->tv_sec; \
(result)->tv_usec = (a)->tv_usec + (b)->tv_usec; \
if ((result)->tv_usec >= 1000000) \
{ \
++(result)->tv_sec; \
(result)->tv_usec -= 1000000; \
} \
} while (0)
#define timersub(a, b, result) \
do { \
(result)->tv_sec = (a)->tv_sec - (b)->tv_sec; \
(result)->tv_usec = (a)->tv_usec - (b)->tv_usec; \
if ((result)->tv_usec < 0) { \
--(result)->tv_sec; \
(result)->tv_usec += 1000000; \
} \
} while (0)
#define USECS_PER_SEC 1000000
#define USECS_PER_TICK (USECS_PER_SEC / CFG_TICKS_PER_SECOND)
/* Workaround for rt-labs defect 776.
* Default implementation of udelay() didn't work correctly when tick was
* shorter than one millisecond.
*/
//void udelay (uint32_t us)
//{
tick_t ticks = (us / USECS_PER_TICK) + 1;
task_delay (ticks);
//}
//int gettimeofday(struct timeval *tp, void *tzp)
//{
// tick_t tick = tick_get();
// tick_t ticks_left;
// ASSERT (tp != NULL);
// tp->tv_sec = tick / CFG_TICKS_PER_SECOND;
// ticks_left = tick % CFG_TICKS_PER_SECOND;
// tp->tv_usec = ticks_left * USECS_PER_TICK;
// ASSERT (tp->tv_usec < USECS_PER_SEC);
// return 0;
//}
int osal_usleep (uint32 usec)
{
//udelay(usec);
/*ajustment for precision*/
//usec -= usec / 4080;
usec -= usec / 1500;
/*rt_hw_us_delay work for a delay less than 16us*/
do{
if(usec>=1000)
{
rt_hw_us_delay(1000);
usec -= 1000;
}else{
rt_hw_us_delay(usec);
usec = 0;
}
}while(usec>0);
return 0;
}
int osal_gettimeofday(struct timeval *tv, struct timezone *tz)
{
// return gettimeofday(tv, tz);
RT_ASSERT (tv != NULL);
rt_uint32_t timer_tick,rt_tick;
rt_base_t level = rt_hw_interrupt_disable();
timer_tick = ETIMER_GetCounter(5);
rt_tick = rt_tick_get();
rt_hw_interrupt_enable(level);
tv->tv_sec = rt_tick/1000;
tv->tv_usec = (rt_tick % 1000)*1000 + timer_tick / us_ticks;
return 0;
}
ec_timet osal_current_time (void)
{
struct timeval current_time;
ec_timet return_value;
osal_gettimeofday (¤t_time, 0);
return_value.sec = current_time.tv_sec;
return_value.usec = current_time.tv_usec;
return return_value;
}
void osal_timer_start (osal_timert * self, uint32 timeout_usec)
{
struct timeval start_time;
struct timeval timeout;
struct timeval stop_time;
osal_gettimeofday (&start_time, 0);
timeout.tv_sec = timeout_usec / USECS_PER_SEC;
timeout.tv_usec = timeout_usec % USECS_PER_SEC;
timeradd (&start_time, &timeout, &stop_time);
self->stop_time.sec = stop_time.tv_sec;
self->stop_time.usec = stop_time.tv_usec;
}
boolean osal_timer_is_expired (osal_timert * self)
{
struct timeval current_time;
struct timeval stop_time;
int is_not_yet_expired;
osal_gettimeofday (¤t_time, 0);
stop_time.tv_sec = self->stop_time.sec;
stop_time.tv_usec = self->stop_time.usec;
is_not_yet_expired = timercmp (¤t_time, &stop_time, <);
return is_not_yet_expired == 0;
}
void *osal_malloc(size_t size)
{
return rt_malloc(size);
}
void osal_free(void *ptr)
{
rt_free(ptr);
}
int osal_thread_create(void *thandle, int stacksize, void *func, void *param)
{
// thandle = task_spawn ("worker", func, 6,stacksize, param);
// if(!thandle)
// {
// return 0;
// }
return 1;
}
int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param)
{
// thandle = task_spawn ("worker_rt", func, 15 ,stacksize, param);
// if(!thandle)
// {
// return 0;
// }
return 1;
}
3.2.2 oshw.c移植
不需做什么工作。
3.2.3 nicdrv.c移植
主要修改就是调用自己的网络发送和接收函数，我把它们命名为net_send和net_recv。这两个函数最好的实现是直接操作网卡（或者叫emac），我现在的实现参考了tcpdump的方法，在协议栈中加钩子（hook）实现，这样对原来系统影响最小，网口除了EtherCAT，还可以当正常的网口用。
ecx_setupnic函数中创建mutex（这个按照rt-thread格式改一下即可），安装网络钩子
ecx_closenic函数中删除mutex，卸载网络钩子。
/** Basic setup to connect NIC to socket.
* @param[in] port = port context struct
* @param[in] ifname = Name of NIC device, f.e. "eth0"
* @param[in] secondary = if >0 then use secondary stack instead of primary
* @return >0 if succeeded
*/
int ecx_setupnic(ecx_portt *port, const char *ifname, int secondary)
{
int i;
int rVal;
int *psock;
// rVal = bfin_EMAC_init((uint8_t *)priMAC);
// if (rVal != 0)
// return 0;
if (secondary)
{
/* secondary port struct available? */
if (port->redport)
{
/* when using secondary socket it is automatically a redundant setup */
psock = &(port->redport->sockhandle);
*psock = -1;
port->redstate = ECT_RED_DOUBLE;
port->redport->stack.sock = &(port->redport->sockhandle);
port->redport->stack.txbuf = &(port->txbuf);
port->redport->stack.txbuflength = &(port->txbuflength);
port->redport->stack.tempbuf = &(port->redport->tempinbuf);
port->redport->stack.rxbuf = &(port->redport->rxbuf);
port->redport->stack.rxbufstat = &(port->redport->rxbufstat);
port->redport->stack.rxsa = &(port->redport->rxsa);
ecx_clear_rxbufstat(&(port->redport->rxbufstat[0]));
}
else
{
/* fail */
return 0;
}
}
else
{
port->getindex_mutex = rt_mutex_create ("getindex_mutex", RT_IPC_FLAG_PRIO);
port->tx_mutex = rt_mutex_create ("tx_mutex", RT_IPC_FLAG_PRIO);
port->rx_mutex = rt_mutex_create ("rx_mutex", RT_IPC_FLAG_PRIO);
port->sockhandle = -1;
port->lastidx = 0;
port->redstate = ECT_RED_NONE;
port->stack.sock = &(port->sockhandle);
port->stack.txbuf = &(port->txbuf);
port->stack.txbuflength = &(port->txbuflength);
port->stack.tempbuf = &(port->tempinbuf);
port->stack.rxbuf = &(port->rxbuf);
port->stack.rxbufstat = &(port->rxbufstat);
port->stack.rxsa = &(port->rxsa);
ecx_clear_rxbufstat(&(port->rxbufstat[0]));
psock = &(port->sockhandle);
if(install_hook(port, ifname)==0){
rt_kprintf("ecx_setupnic fail\n");
return 0; //fail
}
}
/* setup ethernet headers in tx buffers so we don't have to repeat it */
for (i = 0; i < EC_MAXBUF; i++)
{
ec_setupheader(&(port->txbuf[i]));
port->rxbufstat[i] = EC_BUF_EMPTY;
}
ec_setupheader(&(port->txbuf2));
return 1;
}
/** Close sockets used
* @param[in] port = port context struct
* @return 0
*/
int ecx_closenic(ecx_portt *port)
{
rt_mutex_delete(port->getindex_mutex);
rt_mutex_delete(port->tx_mutex);
rt_mutex_delete(port->rx_mutex);
uninstall_hook(port);
return 0;
}
3.2.4 net_hook.c实现
主要实现EtherCAT数据帧收发，中间加了个环形缓冲区用于接收。具体原理就是在网卡加个钩子函数，有数据来的时候先经过钩子函数，我们把EtherCAT数据帧截住，不传给原来的lwip协议栈；如果要发送数据，就直接调用发送函数，绕过lwip协议栈。这样也不影响lwip协议栈工作。
#include
#include "netif/ethernetif.h"
#include "nicdrv.h"
#include
#include
/******************************************************************************
* receive fifo buf
*/
#define HOOK_RX_BUFSIZE 10
static uint8_t netfrmbuf[HOOK_RX_BUFSIZE][1540];
static int netfrmbuf_cnt[HOOK_RX_BUFSIZE];
static int netfrm_head = 0;
static int netfrm_tail = 0;
static int netfrm_full = 0;
int hook_rx_dump = 0;
int hook_tx_dump = 0;
static inline void tx_led_flash(void)
{
static int c = 0;
//rt_kprintf("c = %d\n",c);
if(++c%2)
rt_pin_write(NU_GET_PININDEX(NU_PB, 8), PIN_LOW);
else
rt_pin_write(NU_GET_PININDEX(NU_PB, 8), PIN_HIGH);
}
/******************************************************************************
* store netif and old function addr
*/
static struct netif *netif = RT_NULL;
static netif_linkoutput_fn link_output;
static netif_input_fn input;
/******************************************************************************
* hex dump
*/
#define __is_print(ch) ((unsigned int)((ch) - ' ') < 127u - ' ')
static void hex_dump(const rt_uint8_t *ptr, rt_size_t buflen)
{
unsigned char *buf = (unsigned char *)ptr;
int i, j;
RT_ASSERT(ptr != RT_NULL);
for (i = 0; i < buflen; i += 16)
{
rt_kprintf("%08X: ", i);
for (j = 0; j < 16; j++)
if (i + j < buflen)
rt_kprintf("%02X ", buf[i + j]);
else
rt_kprintf(" ");
rt_kprintf(" ");
for (j = 0; j < 16; j++)
if (i + j < buflen)
rt_kprintf("%c", __is_print(buf[i + j]) ? buf[i + j] : '.');
rt_kprintf("\n");
}
}
/******************************************************************************
* rx/tx hook function
*/
/* get tx data */
static err_t _netif_linkoutput(struct netif *netif, struct pbuf *p)
{
return link_output(netif, p);
}
/* get rx data */
static err_t _netif_input(struct pbuf *p, struct netif *inp)
{
if(p->tot_len>=14)
{
char *data = p->payload;
if(data[12]=='\x88' && data[13]=='\xa4') //filter for ethercat frame
{
if(netfrm_full == 0){
pbuf_copy_partial(p, netfrmbuf[netfrm_tail], p->tot_len, 0);
netfrmbuf_cnt[netfrm_tail] = p->tot_len;
netfrm_tail = (netfrm_tail+1) % HOOK_RX_BUFSIZE;
if(netfrm_tail==netfrm_head)
netfrm_full = 1;
}
//rt_kprintf("tail = %d, full = %d\n", netfrm_tail, netfrm_full);
}
}
return input(p, inp);
}
/******************************************************************************
* hook install
*/
int install_hook(ecx_portt *port, const char *ifname)
{
struct eth_device *device;
rt_base_t level;
netfrm_head = 0;
netfrm_tail = 0;
netfrm_full = 0;
if(netif == RT_NULL){
rt_kprintf("hook installing on %s\n", ifname);
device = (struct eth_device *)rt_device_find(ifname);
if (device == RT_NULL){
rt_kprintf("hook install error 'device == RT_NULL'\n");
return 0;
}
if ((device->netif == RT_NULL) || (device->netif->linkoutput == RT_NULL)){
rt_kprintf("hook install error '(device->netif == RT_NULL) || (device->netif->linkoutput == RT_NULL)'\n");
return 0;
}
rt_kprintf("device %s found: 0x%x \n", ifname, (uint32_t)device);
}else{
rt_kprintf("device %s hook already installed, must be uninstall it before intall new on\n", ifname);
}
netif = device->netif;
//install netdev hook
level = rt_hw_interrupt_disable();
link_output = netif->linkoutput;
netif->linkoutput = _netif_linkoutput;
input = netif->input;
netif->input = _netif_input;
rt_hw_interrupt_enable(level);
rt_kprintf("hook installed on %s\n", ifname);
return 1;
}
/******************************************************************************
* hook uninstall
*/
int uninstall_hook(ecx_portt *port)
{
rt_base_t level;
//uninstall netdev hook
if(netif != RT_NULL){
level = rt_hw_interrupt_disable();
netif->input = input;
netif->linkoutput = link_output;
rt_hw_interrupt_enable(level);
netif = RT_NULL;
}
rt_kprintf("hook uninstalled\n");
return 1;
}
/******************************************************************************
* netdev send/recv api
*/
int net_send(unsigned char *data, int len)
{
int ret = -1;
struct pbuf *p;
tx_led_flash();
p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_POOL);
if (p != NULL)
{
pbuf_take(p, data, len);
if(hook_tx_dump){
rt_kprintf("send --- len=%d>>>\n",len);
hex_dump(p->payload, p->tot_len);
}
_netif_linkoutput(netif,p);
pbuf_free(p);
ret = len;
}
else{
rt_kprintf("net_send alloc buffer error\n");
}
return ret;
}
int net_recv(unsigned char *data, int len)
{
if(netfrm_full == 0 && netfrm_tail==netfrm_head){
return 0;
}
int total = netfrmbuf_cnt[netfrm_head];
if(total > len) total = len;
rt_memcpy(data, netfrmbuf[netfrm_head], total);
netfrm_head = (netfrm_head+1) % HOOK_RX_BUFSIZE;
if(netfrm_tail==netfrm_head)
netfrm_full = 0;
if(hook_rx_dump){
rt_kprintf("recv <<<---\n");
hex_dump(data, total);
rt_kprintf("head = %d, tail = %d, full = %d\n", netfrm_head, netfrm_tail, netfrm_full);
}
return total;
}
void net_hook_test(int argc, char **argv)
{
unsigned char frame[] = "\xff\xff\xff\xff\xff\xff\x01\x01\x01\x01\x01\x01\x88\xa4\x0d\x10\
\x08\x01\x00\x00\x03\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\
\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\
\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
install_hook(NULL, "e0");
net_send(frame,60);
rt_thread_mdelay(20000);
uninstall_hook(NULL);
}
MSH_CMD_EXPORT(net_hook_test, net_hook_test sample on 'e0'); 3.2.5 some基本功能测试
采用官方的slave_info测试代码，测试主要分为时钟测试和soem EtherCAT协议栈基本功能测试。在终端中输入 soem_test + 回车即可。
我接了一个汇川IS620N驱动器，下面是输出的部分内容：
Slave:1
Name:IS620N
Output size: 96bits
Input size: 224bits
State: 4
Delay: 0[ns]
Has DC: 1
DCParentport:0
Activeports:1.0.0.0
Configured address: 1001
Man: 00100000 ID: 000c0108 Rev: 00010001
SM0 A:1000 L: 128 F:00010026 Type:1
SM1 A:1400 L: 128 F:00010022 Type:2
SM2 A:1800 L: 12 F:00010064 Type:3
SM3 A:1c00 L: 28 F:00010020 Type:4
FMMU0 Ls:00000000 Ll: 12 Lsb:0 Leb:7 Ps:1800 Psb:0 Ty:02 Act:01
FMMU1 Ls:0000000c Ll: 28 Lsb:0 Leb:7 Ps:1c00 Psb:0 Ty:01 Act:01
FMMUfunc 0:1 1:2 2:0 3:0
MBX length wr: 128 rd: 128 MBX protocols : 04
CoE details: 0d FoE details: 00 EoE details: 00 SoE details: 00
Ebus current: 0[mA]
only LRD/LWR:04. 运动控制测试
基础工作做好以后，我们就能真正的控制电机运行了。在控制电机运行之前，还需要了解CIA402相关的规范，启动伺服需要按照规范要求，按顺序来。
程序主要流程如下，具体代码见附件。
a)初始化时钟 osal_timer_init
b)初始化网卡ec_init
c)等待进入INIT态
d)初始化驱动器（is620n）ec_config_init
e)DC配置
f)申请并等待进入Pre-OP态
g)配置过程数据TxPDO/RxPDO（自定义函数process_data_config）
h)配置FMMU ec_config_map
i)申请并等待进入Safe-OP态
j)设置CSP模式
k)发送和接收过程数据1次，触发SLAVE
l)申请并等待进入OP态
m)进入过程数据收发循环
#define __is_print(ch) ((unsigned int)((ch) - ' ') < 127u - ' ')
static void hex_dump(const rt_uint8_t *ptr, rt_size_t buflen)
{
unsigned char *buf = (unsigned char *)ptr;
int i, j;
RT_ASSERT(ptr != RT_NULL);
for (i = 0; i < buflen; i += 16)
{
rt_kprintf("%08X: ", i);
for (j = 0; j < 16; j++)
if (i + j < buflen)
rt_kprintf("%02X ", buf[i + j]);
else
rt_kprintf(" ");
rt_kprintf(" ");
for (j = 0; j < 16; j++)
if (i + j < buflen)
rt_kprintf("%c", __is_print(buf[i + j]) ? buf[i + j] : '.');
rt_kprintf("\n");
}
}
static char IOmap[4096];
typedef struct __attribute__((__packed__))
{
unsigned char mode_byte;
unsigned short control_word;
long dest_pos;
unsigned short error_word;
unsigned short status_word;
long cur_pos;
}SERVO_DATA_T;
typedef struct
{
SERVO_DATA_T servo_data[3];
}SERVOS_T;
SERVOS_T *servos = (SERVOS_T *)IOmap;
void view_slave_data()
{
hex_dump(IOmap,32);
}
static void echo_time()
{
struct timeval tp;
osal_gettimeofday(&tp, 0);
printf("****cur time = %d,%03d,%03d(us)\n", tp.tv_sec,tp.tv_usec/1000,tp.tv_usec%1000);
}
#define EC_TIMEOUTMON 500
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;
void process_data_config()
{
u8_t ind;
for(int slave = 1; slave <= *ecx_context.slavecount; slave++)
{
//rpdo------------
//1c12.0
safe_SDCwrite_b(slave, 0x1c12, 0, 0);
safe_SDCwrite_w(slave, 0x1c12, 1, 0x1600);
//1600
ind = 0;
safe_SDCwrite_b(slave, 0x1600, 0, 0);
safe_SDCwrite_dw(slave, 0x1600, ++ind, htoel(0x60600008));//6060h(控制模式)
safe_SDCwrite_dw(slave, 0x1600, ++ind, htoel(0x60400010));//6040h(控制字)
safe_SDCwrite_dw(slave, 0x1600, ++ind, htoel(0x607a0020));//607Ah(目标位置)
safe_SDCwrite_b(slave, 0x1600, 0, ind);
//1c12.0
safe_SDCwrite_b(slave, 0x1c12, 0, 1);
//tpdo-------------
//1c13.0
safe_SDCwrite_b(slave, 0x1c13, 0x00, 0);
safe_SDCwrite_w(slave, 0x1c13, 0x01, 0x1a00);
//1a00
ind = 0;
safe_SDCwrite_b(slave, 0x1a00, 0, 0);
safe_SDCwrite_dw(slave, 0x1a00, ++ind, htoel(0x603F0010));//603Fh(错误码)
safe_SDCwrite_dw(slave, 0x1a00, ++ind, htoel(0x60410010));//6041h(状态字)
safe_SDCwrite_dw(slave, 0x1a00, ++ind, htoel(0x60640020));//6064h(位置反馈)
safe_SDCwrite_b(slave, 0x1a00, 0, ind);
//1c13.0
safe_SDCwrite_b(slave, 0x1c13, 0, 1);
safe_SDCwrite_b(slave, 0x6060, 0, 1);
//viewSDO(slave, 0x6060, 0, 1);
}
}
void servo_switch_op()
{
int sta;
for(int slave = 1; slave <= *ecx_context.slavecount; slave++)
{
int idx = slave - 1;
sta = servos->servo_data[idx].status_word & 0x3ff;
//printf("servo_switch_op: slave %d [6041]=%04x\n",slave,servos->servo_data[idx].status_word );
if(servos->servo_data[idx].status_word & 0x8){
servos->servo_data[idx].control_word = 0x8;
// printf("***slave %d control=%04x\n",slave,servos->servo_data[idx].control_word );
continue;
}
//printf("servo_switch_op: slave %d sta=%04x\n", slave, sta );
switch(sta)
{
case 0x250:
case 0x270:
servos->servo_data[idx].control_word = 0x6;
break;
case 0x231:
servos->servo_data[idx].control_word = 0x7;
break;
case 0x233:
servos->servo_data[idx].control_word = 0xf;
break;
default:
//servos->servo_data[idx].control_word = 0x6;
break;
}
//printf("slave %d control=%04x\n",slave,servos->servo_data[idx].control_word );
}
}
void servo_switch_idle()
{
int sta;
for(int slave = 1; slave <= *ecx_context.slavecount; slave++)
{
servos->servo_data[slave-1].control_word = 0x0;
}
}
void sv660n_config(char *ifname)
{
needlf = FALSE;
inOP = FALSE;
osal_timer_init();
ecx_context.manualstatechange = 1;
printf("========================\n");
printf("sv660 config\n");
echo_time();
if (ec_init(ifname))
{
printf("ec_init on %s succeeded.\n",ifname);
//init status
printf("\nRequest init state for all slaves\n");
ec_slave[0].state = EC_STATE_INIT;
//request INIT state for all slaves
ec_writestate(0);
//显示1状态
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_INIT, EC_TIMEOUTSTATE * 3);
if (ec_slave[0].state != EC_STATE_INIT ){
printf("Not all slaves reached init state.\n");
ec_readstate();
for(int i = 1; i<=ec_slavecount ; i++){
if(ec_slave[i].state != EC_STATE_INIT){
printf("Slave %d State=0x%2x StatusCode=0x%04x : %s\n", i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
echo_time();
//if ( ec_config(FALSE, &IOmap) > 0 )
wkc = ec_config_init(0/*usetable*/);
if (wkc > 0)
{
ec_configdc();
ec_dcsync0(1, TRUE, 2000000, 50); // SYNC0 on slave 1
while(EcatError) printf("%s", ec_elist2string());
printf("%d slaves found and configured.\n",ec_slavecount);
/* request pre_op for slave */
printf("\nRequest pre_op state for all slaves\n");
ec_slave[0].state = EC_STATE_PRE_OP | EC_STATE_ACK;
ec_writestate(0);
//故障复位
// safe_SDCwrite_w(1,0x200d, 0x2, 1);
// safe_SDCwrite_w(1,0x200d, 0x2, 0);
//现在应该在pre_op状态
//显示2状态
process_data_config(); //config tpdo/rpdo
//config fmmu
ec_config_map(IOmap);
/* request safe_op for slave */
ec_slave[0].state = EC_STATE_SAFE_OP;
ec_writestate(0);
//safe-op
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
/* wait for all slaves to reach SAFE_OP state */
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 3);
if (ec_slave[0].state != EC_STATE_SAFE_OP ){
printf("Not all slaves reached safe operational state.\n");
ec_readstate();
for(int i = 1; i<=ec_slavecount ; i++){
if(ec_slave[i].state != EC_STATE_SAFE_OP){
printf("Slave %d State=0x%2x StatusCode=0x%04x : %s\n", i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}else{
//显示4状态
//启动伺服
servos->servo_data[0].mode_byte = 8; //csp mode
//op status
printf("Request operational state for all slaves\n");
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
ec_slave[0].state = EC_STATE_OPERATIONAL;
// send one valid process data to make outputs in slaves happy
ec_send_processdata();
wkc = ec_receive_processdata(EC_TIMEOUTRET*3);
printf("--->workcounter %d\n", wkc);
//view_slave_data();
// request OP state for all slaves
ec_writestate(0);
int chk = 200;
// wait for all slaves to reach OP state
do
{
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
printf("--->workcounter %d\n", wkc);
ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
}
while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
if (ec_slave[0].state == EC_STATE_OPERATIONAL )
{
printf("<<>> state reached for all slaves.\n");
inOP = TRUE;
osal_timert t;
osal_timer_start(&t, 1000);
// cyclic loop
for(int i = 1; i <= 10000; i++)
{
servo_switch_op();
if(servos->servo_data[0].control_word==7){
servos->servo_data[0].dest_pos = servos->servo_data[0].cur_pos;
//printf("cur pos = %ld\n", servos->servo_data[0].cur_pos);
}
if(servos->servo_data[0].control_word==0xf){
servos->servo_data[0].dest_pos += 3000;
}
while(osal_timer_is_expired(&t)==FALSE);
osal_timer_start(&t, 1000);
ec_send_processdata();
wkc = ec_receive_processdata(EC_TIMEOUTRET);
if(wkc >= expectedWKC){
//printf("~~~~WKC %d \n", wkc);
}
if(wkc <=0 ){
printf("Error.\n");
break;
}
//osal_usleep(1000);
}
inOP = FALSE;
}
else
{
printf("Not all slaves reached operational state.\n");
ec_readstate();
for(int i = 1; i<=ec_slavecount ; i++)
{
if(ec_slave[i].state != EC_STATE_OPERATIONAL)
{
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
//init status
printf("\nRequest init state for all slaves\n");
ec_slave[0].state = EC_STATE_INIT;
//request INIT state for all slaves
ec_writestate(0);
}
} else {
printf("No slaves found!\n");
}
echo_time();
printf("End soem, close socket\n");
// stop SOEM, close socket
ec_close();
}else{
printf("ec_init on %s failed.\n",ifname);
}
printf("IOMAP addr = 0x%08x\n", (uint32_t)IOmap);
printf("========================\n");
view_slave_data();
}
在进入数据数据收发循环后，按次序发送控制字启动伺服（6040h发送6，7和15），然后就可以不断发送新的控制位置让电机转起来了！
5. 总结
整个移植过程还是充满了挑战，主要也是因为今年才开始接触EtherCAT，很多概念是边学边用，网上也参考了不少同学的帖子。很多人反应汇川的伺服用SOEM驱动DC同步模式总是有问题，确实遇到了很多奇奇怪怪的问题。经过这两个月的折腾，总算开了个头，基础打好了。
下一步可优化的就是现在的网络移植改用直接操作emac，这样可以减少网络抖动。
附件程序里还参考本站贴子移植了uffs文件系统，编译如果有问题，可能还需要下载uffs、ramdisk、optparse和netutils包。
完整程序包下载