FreeRTOS的SOEM-master（1.4.1）移植
1. 本次移植系统及前提
工作需要移植soem做EtherCAT主站，就跟大家分享如何移植soem，板块网口一个，实现soem与lwip能共用，下面是移植时系统配置：
- stm32f07做裸机开发
- FreeRTOS做为实时系统
- 移植lwip做tcp通讯
2. 移植三步骤（osal、oshw、soem）
移植时候参考了，下面几个资料，都讲的非常好：
- NUC980 DIY项目大挑战 - EtherCAT实现:https://club.rt-thread.org/ask/article/160414965e80294e.html : link
- 在RT-Thread上移植EtherCAT开源主站SOEM1.4.0:https://blog.youkuaiyun.com/lg28870983/article/details/124647952 link
- 基于STM32构建EtherCAT主站:https://blog.youkuaiyun.com/cln512/article/details/122093655: link
- EtherCATマスターSOEMをマイコンに移植する https://lipoyang.hatenablog.com/entry/2019/12/08/101951: link
- 知网搜：基于嵌入式平台的EtherCAT主站实现研究
2.1 官方soem移植下载
soem移植源码兼容很多版本：erika、intime、linux、macosx、rtems、rtk、vxworks、win32
官方soem下载源文件你可以在github下载（1.4.0）版本：https://github.com/OpenEtherCATsociety/SOEM: link
但为了移植统一，建议你下载上面几篇文章移植版本再根据自己工程修改：.
2.2 FreeRTOS版本SOEM移植
Some移植主要是三个文件 osal.c，oshw.c和nicdrv.c。
- osal.c 主要是微秒及的延时和定时函数；
- oshw.c 主要是网络端和本机端数据的大小端转换；
- nicdrv.c 主要是网络数据收发
Some已经给出了很多操作系统移植，我的移植是基于rtk，这个是嵌入式系统实现，和我们的FreeRTOS开发环境最接近。
可以将下面三个文件夹中其他操作系统移植文件删掉，留下rtk：
最终成这样：
少的：netdev_hook.c与netdev_hook.h是后面添加的
2.2.1 osal.c修改
主要内容是实现osal_usleep和osal_gettimeofday两个函数。
由于我的系统FreeRTOS移植系统时钟采用SysTick定时器，TIM10定时器作为HAL基础时钟时，所有将TIM10时钟与SOEM移植时钟共享，就不会浪费多一个定时器去做SOEM的系统时钟。
查看HAL的系统时钟是哪一个定时器：
可知道:hal定时器是每隔1ms产生中断，定时器__HAL_TIM_GET_COUNTER(&htim10)就是us数；
则osal_usleep与osal_gettimeofday可以修改成这样：
/*
* Licensed under the GNU General Public License version 2 with exceptions. See
* LICENSE file in the project root for full license information
*/
#include "osal.h"
#include <time.h>
#include <string.h>
#include <sys/time.h>
#include "stdlib.h"
#include "FreeRTOSConfig.h"
//此处由于与lwip有重定义所以除掉，没有的话可以开启
//#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.
*/
#include "stm32f4xx_hal.h"
extern TIM_HandleTypeDef htim10;
#define false 0
//延迟ms
void MY_MsDelay(uint32_t Delay)
{
uint16_t start_ms,start_us;
if(Delay==0)
{
return;
}
//不超过1ms
start_ms=HAL_GetTick();
start_us =__HAL_TIM_GET_COUNTER(&htim10);
while((HAL_GetTick() - start_ms) < Delay)
{
}
while(__HAL_TIM_GET_COUNTER(&htim10) <start_us)
{
}
}
//延迟us
void MY_UsDelay(uint32_t Delay)
{
uint16_t wait_ms,wait_us,start_us,realse_us;
if(Delay==0)
{
return;
}
wait_ms=Delay/1000;
wait_us=Delay%1000;
realse_us =__HAL_TIM_GET_COUNTER(&htim10);
if(wait_ms!=0)
{
MY_MsDelay(wait_ms);
}
if(wait_us!=0)
{
start_us =__HAL_TIM_GET_COUNTER(&htim10);
if((start_us+wait_us)>999)
{
wait_us=start_us+wait_us-1000;
while(__HAL_TIM_GET_COUNTER(&htim10)>=start_us || __HAL_TIM_GET_COUNTER(&htim10)<wait_us)
{
}
}else
{
while(__HAL_TIM_GET_COUNTER(&htim10)-start_us <wait_us)
{
}
}
}
}
void udelay (uint32_t us)
{
MY_UsDelay(us);
}
uint16 test_mss;
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);
// configASSERT(tp != NULL);
tp->tv_sec=HAL_GetTick()/1000;
tp->tv_usec =(HAL_GetTick()%1000)*1000+__HAL_TIM_GET_COUNTER(&htim10);
test_mss=__HAL_TIM_GET_COUNTER(&htim10);
return 0;
}
int osal_usleep (uint32 usec)
{
udelay(usec);
return 0;
}
int osal_gettimeofday(struct timeval *tv, struct timezone *tz)
{
return gettimeofday(tv, tz);
}
ec_timet osal_current_time (void)
{
struct timeval current_time;
ec_timet return_value;
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;
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;
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 == false;
}
void *osal_malloc(size_t size)
{
return malloc(size);
}
void osal_free(void *ptr)
{
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;
}
2.2.2 oshw.c修改
不需要做什么
2.2.3 nicdrv.c修改及增加netdev_hook.c
主要修改就是调用自己的网络发送和接收函数，我把它们命名为net_send和net_recv。这两个函数最好的实现是直接操作网卡（或者叫emac），我现在的实现参考了tcpdump的方法，在协议栈中加钩子（hook）实现，这样对原来系统影响最小，网口除了EtherCAT，还可以当正常的网口用。
ecx_setupnic函数中创建mutex（这个按照FreeRTOS格式改一下即可），安装网络钩子
ecx_closenic函数中删除mutex，卸载网络钩子
其原理就是soem在ecx_setupnic网络初始化时候，找到lwip网络端口结构体：now_netif = netif_find(ifname); 在now_netif结构体中有：netif->linkoutput与netif->input为发送与接收处理钩子函数，将其保存并指引到新的发送与接收钩子函数处理，绕过原来lwip协议。 在ecx_closenic网络注销中，将钩子函数退出来写入lwip原来钩子处理函数。
感兴趣可以学习lwip的网络收发过程。
nicdrv.c文件：
/*
* Licensed under the GNU General Public License version 2 with exceptions. See
* LICENSE file in the project root for full license information
*/
/** \file
* \brief
* EtherCAT RAW socket driver.
*
* Low level interface functions to send and receive EtherCAT packets.
* EtherCAT has the property that packets are only send by the master,
* and the send packets always return in the receive buffer.
* There can be multiple packets "on the wire" before they return.
* To combine the received packets with the original send packets a buffer
* system is installed. The identifier is put in the index item of the
* EtherCAT header. The index is stored and compared when a frame is received.
* If there is a match the packet can be combined with the transmit packet
* and returned to the higher level function.
*
* The socket layer can exhibit a reversal in the packet order (rare).
* If the Tx order is A-B-C the return order could be A-C-B. The indexed buffer
* will reorder the packets automatically.
*
* The "redundant" option will configure two sockets and two NIC interfaces.
* Slaves are connected to both interfaces, one on the IN port and one on the
* OUT port. Packets are send via both interfaces. Any one of the connections
* (also an interconnect) can be removed and the slaves are still serviced with
* packets. The software layer will detect the possible failure modes and
* compensate. If needed the packets from interface A are resent through interface B.
* This layer is fully transparent for the higher layers.
*/
//#include <kern.h>
//#include <ioctl.h>
#include <stdio.h>
#include <string.h>
#include "osal.h"
#include "oshw.h"
#include "netdev_hook.h"
//#include "lw_mac/lw_emac.h"
#ifndef MAX
#define MAX(a,b) (((a) > (b)) ? (a) : (b))
#define MIN(a,b) (((a) < (b)) ? (a) : (b))
#endif
/** Redundancy modes */
enum
{
/** No redundancy, single NIC mode */
ECT_RED_NONE,
/** Double redundant NIC connection */
ECT_RED_DOUBLE
};
/** Primary source MAC address used for EtherCAT.
* This address is not the MAC address used from the NIC.
* EtherCAT does not care about MAC addressing, but it is used here to
* differentiate the route the packet traverses through the EtherCAT
* segment. This is needed to find out the packet flow in redundant
* configurations. */
const uint16 priMAC[3] = { 0x0101, 0x0101, 0x0101 };
/** Secondary source MAC address used for EtherCAT. */
const uint16 secMAC[3] = { 0x0404, 0x0404, 0x0404 };
/** second MAC word is used for identification */
#define RX_PRIM priMAC[1]
/** second MAC word is used for identification */
#define RX_SEC secMAC[1]
static void ecx_clear_rxbufstat(int *rxbufstat)
{
int i;
for(i = 0; i < EC_MAXBUF; i++)
{
rxbufstat[i] = EC_BUF_EMPTY;
}
}
/** 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;
// port->getindex_mutex = mtx_create();
// port->tx_mutex = mtx_create();
// port->rx_mutex = mtx_create();
// 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 = mtx_create();
// port->tx_mutex = mtx_create();
// port->rx_mutex = mtx_create();
// sys_mutex_new(port->getindex_mutex);
// sys_mutex_new(port->tx_mutex);
// sys_mutex_new(port->rx_mutex);
port->getindex_mutex=xSemaphoreCreateMutex();
port->tx_mutex=xSemaphoreCreateMutex();
port->rx_mutex=xSemaphoreCreateMutex();
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){
printf("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)
{
vSemaphoreDelete(port->getindex_mutex);
vSemaphoreDelete(port->tx_mutex);
vSemaphoreDelete(port->rx_mutex);
// sys_mutex_free(port->getindex_mutex);
// sys_mutex_free(port->tx_mutex);
// sys_mutex_free(port->rx_mutex);
uninstall_hook(port);
return 0;
}
/** Fill buffer with ethernet header structure.
* Destination MAC is always broadcast.
* Ethertype is always ETH_P_ECAT.
* @param[out] p = buffer
*/
void ec_setupheader(void *p)
{
ec_etherheadert *bp;
bp = p;
bp->da0 = oshw_htons(0xffff);
bp->da1 = oshw_htons(0xffff);
bp->da2 = oshw_htons(0xffff);
bp->sa0 = oshw_htons(priMAC[0]);
bp->sa1 = oshw_htons(priMAC[1]);
bp->sa2 = oshw_htons(priMAC[2]);
bp->etype = oshw_htons(ETH_P_ECAT);
}
/** Get new frame identifier index and allocate corresponding rx buffer.
* @param[in] port = port context struct
* @return new index.
*/
uint8 ecx_getindex(ecx_portt *port)
{
uint8 idx;
uint8 cnt;
// mtx_lock (port->getindex_mutex);
// sys_mutex_lock(port->getindex_mutex);
xSemaphoreTake(port->getindex_mutex, portMAX_DELAY);
idx = port->lastidx + 1;
/* index can't be larger than buffer array */
if (idx >= EC_MAXBUF)
{
idx = 0;
}
cnt = 0;
/* try to find unused index */
while ((port->rxbufstat[idx] != EC_BUF_EMPTY) && (cnt < EC_MAXBUF))
{
idx++;
cnt++;
if (idx >= EC_MAXBUF)
{
idx = 0;
}
}
port->rxbufstat[idx] = EC_BUF_ALLOC;
if (port->redstate != ECT_RED_NONE)
{
port->redport->rxbufstat[idx] = EC_BUF_ALLOC;
}
port->lastidx = idx;
// mtx_unlock (port->getindex_mutex);
// sys_mutex_unlock(port->getindex_mutex);
xSemaphoreGive(port->getindex_mutex);
return idx;
}
/** Set rx buffer status.
* @param[in] port = port context struct
* @param[in] idx = index in buffer array
* @param[in] bufstat = status to set
*/
void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat)
{
port->rxbufstat[idx] = bufstat;
if (port->redstate != ECT_RED_NONE)
{
port->redport->rxbufstat[idx] = bufstat;
}
}
/** Transmit buffer over socket (non blocking).
* @param[in] port = port context struct
* @param[in] idx = index in tx buffer array
* @param[in] stacknumber = 0=Primary 1=Secondary stack
* @return socket send result
*/
int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber)
{
int lp, rval;
ec_stackT *stack;
if (!stacknumber)
{
stack = &(port->stack);
}
else
{
stack = &(port->redport->stack);
}
lp = (*stack->txbuflength)[idx];
(*stack->rxbufstat)[idx] = EC_BUF_TX;
rval = net_send((*stack->txbuf)[idx], lp);
return rval;
}
/** Transmit buffer over socket (non blocking).
* @param[in] port = port context struct
* @param[in] idx = index in tx buffer array
* @return socket send result
*/
int ecx_outframe_red(ecx_portt *port, uint8 idx)
{
ec_comt *datagramP;
ec_etherheadert *ehp;
int rval;
ehp = (ec_etherheadert *)&(port->txbuf[idx]);
/* rewrite MAC source address 1 to primary */
ehp->sa1 = oshw_htons(priMAC[1]);
/* transmit over primary socket*/
rval = ecx_outframe(port, idx, 0);
if (port->redstate != ECT_RED_NONE)
{
//mtx_lock (port->tx_mutex);
// sys_mutex_lock(port->tx_mutex);
xSemaphoreTake(port->tx_mutex, portMAX_DELAY);
ehp = (ec_etherheadert *)&(port->txbuf2);
/* use dummy frame for secondary socket transmit (BRD) */
datagramP = (ec_comt*)&(port->txbuf2[ETH_HEADERSIZE]);
/* write index to frame */
datagramP->index = idx;
/* rewrite MAC source address 1 to secondary */
ehp->sa1 = oshw_htons(secMAC[1]);
/* transmit over secondary socket */
//send(sockhandle2, &ec_txbuf2, ec_txbuflength2 , 0);
// OBS! redundant not ACTIVE for BFIN, just added to compile
if(net_send(&(port->txbuf2), port->txbuflength2) == -1)
{
port->redport->rxbufstat[idx] = EC_BUF_EMPTY;
}
port->redport->rxbufstat[idx] = EC_BUF_TX;
//mtx_unlock (port->tx_mutex);
// sys_mutex_unlock(port->tx_mutex);
xSemaphoreGive(port->tx_mutex);
}
return rval;
}
/** Non blocking read of socket. Put frame in temporary buffer.
* @param[in] port = port context struct
* @param[in] stacknumber = 0=primary 1=secondary stack
* @return >0 if frame is available and read
*/
static int ecx_recvpkt(ecx_portt *port, int stacknumber)
{
int lp, bytesrx;
ec_stackT *stack;
if (!stacknumber)
{
stack = &(port->stack);
}
else
{
stack = &(port->redport->stack);
}
lp = sizeof(port->tempinbuf);
bytesrx = net_recv((*stack->tempbuf), lp);
port->tempinbufs = bytesrx;
return (bytesrx > 0);
}
/** Non blocking receive frame function. Uses RX buffer and index to combine
* read frame with transmitted frame. To compensate for received frames that
* are out-of-order all frames are stored in their respective indexed buffer.
* If a frame was placed in the buffer previously, the function retrieves it
* from that buffer index without calling ec_recvpkt. If the requested index
* is not already in the buffer it calls ec_recvpkt to fetch it. There are
* three options now, 1 no frame read, so exit. 2 frame read but other
* than requested index, store in buffer and exit. 3 frame read with matching
* index, store in buffer, set completed flag in buffer status and exit.
*
* @param[in] port = port context struct
* @param[in] idx = requested index of frame
* @param[in] stacknumber = 0=primary 1=secondary stack
* @return Workcounter if a frame is found with corresponding index, otherwise
* EC_NOFRAME or EC_OTHERFRAME.
*/
int ecx_inframe(ecx_portt *port, uint8 idx, int stacknumber)
{
uint16 l;
int rval;
uint8 idxf;
ec_etherheadert *ehp;
ec_comt *ecp;
ec_stackT *stack;
ec_bufT *rxbuf;
if (!stacknumber)
{
stack = &(port->stack);
}
else
{
stack = &(port->redport->stack);
}
rval = EC_NOFRAME;
rxbuf = &(*stack->rxbuf)[idx];
/* check if requested index is already in buffer ? */
if ((idx < EC_MAXBUF) && ( (*stack->rxbufstat)[idx] == EC_BUF_RCVD))
{
l = (*rxbuf)[0] + ((uint16)((*rxbuf)[1] & 0x0f) << 8);
/* return WKC */
rval = ((*rxbuf)[l] + ((uint16)(*rxbuf)[l + 1] << 8));
/* mark as completed */
(*stack->rxbufstat)[idx] = EC_BUF_COMPLETE;
}
else
{
// mtx_lock (port->rx_mutex);
// sys_mutex_lock(port->rx_mutex);
xSemaphoreTake(port->rx_mutex, portMAX_DELAY);
/* non blocking call to retrieve frame from socket */
if (ecx_recvpkt(port, stacknumber))
{
rval = EC_OTHERFRAME;
ehp =(ec_etherheadert*)(stack->tempbuf);
/* check if it is an EtherCAT frame */
if (ehp->etype == oshw_htons(ETH_P_ECAT))
{
ecp =(ec_comt*)(&(*stack->tempbuf)[ETH_HEADERSIZE]);
l = etohs(ecp->elength) & 0x0fff;
idxf = ecp->index;
/* found index equals requested index ? */
if (idxf == idx)
{
/* yes, put it in the buffer array (strip ethernet header) */
memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idx] - ETH_HEADERSIZE);
/* return WKC */
rval = ((*rxbuf)[l] + ((uint16)((*rxbuf)[l + 1]) << 8));
/* mark as completed */
(*stack->rxbufstat)[idx] = EC_BUF_COMPLETE;
/* store MAC source word 1 for redundant routing info */
(*stack->rxsa)[idx] = oshw_ntohs(ehp->sa1);
}
else
{
/* check if index exist and someone is waiting for it */
if (idxf < EC_MAXBUF && (*stack->rxbufstat)[idxf] == EC_BUF_TX)
{
rxbuf = &(*stack->rxbuf)[idxf];
/* put it in the buffer array (strip ethernet header) */
memcpy(rxbuf, &(*stack->tempbuf)[ETH_HEADERSIZE], (*stack->txbuflength)[idxf] - ETH_HEADERSIZE);
/* mark as received */
(*stack->rxbufstat)[idxf] = EC_BUF_RCVD;
(*stack->rxsa)[idxf] = oshw_ntohs(ehp->sa1);
}
else
{
/* strange things happened */
}
}
}
}
// mtx_unlock (port->rx_mutex);
// sys_mutex_unlock(port->rx_mutex);
xSemaphoreGive(port->rx_mutex);
}
/* WKC if matching frame found */
return rval;
}
/** Blocking redundant receive frame function. If redundant mode is not active then
* it skips the secondary stack and redundancy functions. In redundant mode it waits
* for both (primary and secondary) frames to come in. The result goes in an decision
* tree that decides, depending on the route of the packet and its possible missing arrival,
* how to reroute the original packet to get the data in an other try.
*
* @param[in] port = port context struct
* @param[in] idx = requested index of frame
* @param[in] timer = absolute timeout time
* @return Workcounter if a frame is found with corresponding index, otherwise
* EC_NOFRAME.
*/
static int ecx_waitinframe_red(ecx_portt *port, uint8 idx, osal_timert timer)
{
int wkc = EC_NOFRAME;
int wkc2 = EC_NOFRAME;
int primrx, secrx;
/* if not in redundant mode then always assume secondary is OK */
if (port->redstate == ECT_RED_NONE)
{
wkc2 = 0;
}
do
{
/* only read frame if not already in */
if (wkc <= EC_NOFRAME)
{
wkc = ecx_inframe(port, idx, 0);
}
/* only try secondary if in redundant mode */
if (port->redstate != ECT_RED_NONE)
{
/* only read frame if not already in */
if (wkc2 <= EC_NOFRAME)
wkc2 = ecx_inframe(port, idx, 1);
}
/* wait for both frames to arrive or timeout */
} while (((wkc <= EC_NOFRAME) || (wkc2 <= EC_NOFRAME)) && (osal_timer_is_expired(&timer) == FALSE));
/* only do redundant functions when in redundant mode */
if (port->redstate != ECT_RED_NONE)
{
/* primrx if the received MAC source on primary socket */
primrx = 0;
if (wkc > EC_NOFRAME)
{
primrx = port->rxsa[idx];
}
/* secrx if the received MAC source on psecondary socket */
secrx = 0;
if (wkc2 > EC_NOFRAME)
{
secrx = port->redport->rxsa[idx];
}
/* primary socket got secondary frame and secondary socket got primary frame */
/* normal situation in redundant mode */
if ( ((primrx == RX_SEC) && (secrx == RX_PRIM)) )
{
/* copy secondary buffer to primary */
memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE);
wkc = wkc2;
}
/* primary socket got nothing or primary frame, and secondary socket got secondary frame */
/* we need to resend TX packet */
if ( ((primrx == 0) && (secrx == RX_SEC)) ||
((primrx == RX_PRIM) && (secrx == RX_SEC)) )
{
osal_timert read_timer;
/* If both primary and secondary have partial connection retransmit the primary received
* frame over the secondary socket. The result from the secondary received frame is a combined
* frame that traversed all slaves in standard order. */
if ( (primrx == RX_PRIM) && (secrx == RX_SEC) )
{
/* copy primary rx to tx buffer */
memcpy(&(port->txbuf[idx][ETH_HEADERSIZE]), &(port->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE);
}
osal_timer_start(&read_timer, EC_TIMEOUTRET);
/* resend secondary tx */
ecx_outframe(port, idx, 1);
do
{
/* retrieve frame */
wkc2 = ecx_inframe(port, idx, 1);
} while ((wkc2 <= EC_NOFRAME) && (osal_timer_is_expired(&read_timer) == FALSE));
if (wkc2 > EC_NOFRAME)
{
/* copy secondary result to primary rx buffer */
memcpy(&(port->rxbuf[idx]), &(port->redport->rxbuf[idx]), port->txbuflength[idx] - ETH_HEADERSIZE);
wkc = wkc2;
}
}
}
/* return WKC or EC_NOFRAME */
return wkc;
}
/** Blocking receive frame function. Calls ec_waitinframe_red().
* @param[in] port = port context struct
* @param[in] idx = requested index of frame
* @param[in] timeout = timeout in us
* @return Workcounter if a frame is found with corresponding index, otherwise
* EC_NOFRAME.
*/
int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout)
{
int wkc;
osal_timert timer;
osal_timer_start (&timer, timeout);
wkc = ecx_waitinframe_red(port, idx, timer);
return wkc;
}
/** Blocking send and receive frame function. Used for non processdata frames.
* A datagram is build into a frame and transmitted via this function. It waits
* for an answer and returns the workcounter. The function retries if time is
* left and the result is WKC=0 or no frame received.
*
* The function calls ec_outframe_red() and ec_waitinframe_red().
*
* @param[in] port = port context struct
* @param[in] idx = index of frame
* @param[in] timeout = timeout in us
* @return Workcounter or EC_NOFRAME
*/
int ecx_srconfirm(ecx_portt *port, uint8 idx, int timeout)
{
int wkc = EC_NOFRAME;
osal_timert timer;
osal_timer_start(&timer, timeout);
do
{
osal_timert read_timer;
/* tx frame on primary and if in redundant mode a dummy on secondary */
ecx_outframe_red(port, idx);
osal_timer_start(&read_timer, MIN(timeout, EC_TIMEOUTRET));
/* get frame from primary or if in redundant mode possibly from secondary */
wkc = ecx_waitinframe_red(port, idx, read_timer);
/* wait for answer with WKC>0 or otherwise retry until timeout */
} while ((wkc <= EC_NOFRAME) && (osal_timer_is_expired(&timer) == FALSE));
return wkc;
}
#ifdef EC_VER1
int ec_setupnic(const char *ifname, int secondary)
{
return ecx_setupnic(&ecx_port, ifname, secondary);
}
int ec_closenic(void)
{
return ecx_closenic(&ecx_port);
}
uint8 ec_getindex(void)
{
return ecx_getindex(&ecx_port);
}
void ec_setbufstat(uint8 idx, int bufstat)
{
ecx_setbufstat(&ecx_port, idx, bufstat);
}
int ec_outframe(uint8 idx, int stacknumber)
{
return ecx_outframe(&ecx_port, idx, stacknumber);
}
int ec_outframe_red(uint8 idx)
{
return ecx_outframe_red(&ecx_port, idx);
}
int ec_inframe(uint8 idx, int stacknumber)
{
return ecx_inframe(&ecx_port, idx, stacknumber);
}
int ec_waitinframe(uint8 idx, int timeout)
{
return ecx_waitinframe(&ecx_port, idx, timeout);
}
int ec_srconfirm(uint8 idx, int timeout)
{
return ecx_srconfirm(&ecx_port, idx, timeout);
}
#endif
nicdrv.h文件：
```c
/*
* Licensed under the GNU General Public License version 2 with exceptions. See
* LICENSE file in the project root for full license information
*/
/** \file
* \brief
* Headerfile for nicdrv.c
*/
#ifndef _nicdrvh_
#define _nicdrvh_
#include "ethercattype.h"
/** pointer structure to Tx and Rx stacks */
typedef struct
{
/** socket connection used */
int *sock;
/** tx buffer */
ec_bufT (*txbuf)[EC_MAXBUF];
/** tx buffer lengths */
int (*txbuflength)[EC_MAXBUF];
/** temporary receive buffer */
ec_bufT *tempbuf;
/** rx buffers */
ec_bufT (*rxbuf)[EC_MAXBUF];
/** rx buffer status fields */
int (*rxbufstat)[EC_MAXBUF];
/** received MAC source address (middle word) */
int (*rxsa)[EC_MAXBUF];
} ec_stackT;
/** pointer structure to buffers for redundant port */
typedef struct
{
ec_stackT stack;
int sockhandle;
/** rx buffers */
ec_bufT rxbuf[EC_MAXBUF];
/** rx buffer status */
int rxbufstat[EC_MAXBUF];
/** rx MAC source address */
int rxsa[EC_MAXBUF];
/** temporary rx buffer */
ec_bufT tempinbuf;
} ecx_redportt;
/** pointer structure to buffers, vars and mutexes for port instantiation */
#include "sys.h"
#include "sys_arch.h"
typedef struct
{
ec_stackT stack;
int sockhandle;
/** rx buffers */
ec_bufT rxbuf[EC_MAXBUF];
/** rx buffer status */
int rxbufstat[EC_MAXBUF];
/** rx MAC source address */
int rxsa[EC_MAXBUF];
/** temporary rx buffer */
ec_bufT tempinbuf;
/** temporary rx buffer status */
int tempinbufs;
/** transmit buffers */
ec_bufT txbuf[EC_MAXBUF];
/** transmit buffer lengths */
int txbuflength[EC_MAXBUF];
/** temporary tx buffer */
ec_bufT txbuf2;
/** temporary tx buffer length */
int txbuflength2;
/** last used frame index */
uint8 lastidx;
/** current redundancy state */
int redstate;
/** pointer to redundancy port and buffers */
ecx_redportt *redport;
// mtx_t * getindex_mutex;
// mtx_t * tx_mutex;
// mtx_t * rx_mutex;
// sys_mutex_t * getindex_mutex;
// sys_mutex_t * tx_mutex;
// sys_mutex_t * rx_mutex;
SemaphoreHandle_t getindex_mutex;
SemaphoreHandle_t tx_mutex;
SemaphoreHandle_t rx_mutex;
// uart3DMATxCpltMutex = xSemaphoreCreateMutex()
// xSemaphoreTake(TCP_TxCpltMutex, portMAX_DELAY);
// xSemaphoreGive(TCP_TxCpltMutex);
} ecx_portt;
extern const uint16 priMAC[3];
extern const uint16 secMAC[3];
#ifdef EC_VER1
extern ecx_portt ecx_port;
extern ecx_redportt ecx_redport;
int ec_setupnic(const char * ifname, int secondary);
int ec_closenic(void);
void ec_setbufstat(uint8 idx, int bufstat);
uint8 ec_getindex(void);
int ec_outframe(uint8 idx, int stacknumber);
int ec_outframe_red(uint8 idx);
int ec_waitinframe(uint8 idx, int timeout);
int ec_srconfirm(uint8 idx,int timeout);
#endif
void ec_setupheader(void *p);
int ecx_setupnic(ecx_portt *port, const char * ifname, int secondary);
int ecx_closenic(ecx_portt *port);
void ecx_setbufstat(ecx_portt *port, uint8 idx, int bufstat);
uint8 ecx_getindex(ecx_portt *port);
int ecx_outframe(ecx_portt *port, uint8 idx, int stacknumber);
int ecx_outframe_red(ecx_portt *port, uint8 idx);
int ecx_waitinframe(ecx_portt *port, uint8 idx, int timeout);
int ecx_srconfirm(ecx_portt *port, uint8 idx,int timeout);
#endif
netdev_hook.c文件：
#include "netif.h"
#include "nicdrv.h"
#include "FreeRTOS.h"
#include "FreeRTOSConfig.h"
#include "stdint.h"
#include "string.h"
/******************************************************************************
* 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;
/******************************************************************************
* store netif and old function addr
*/
static struct netif *netif = 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 uint8_t *ptr, size_t buflen)
{
unsigned char *buf = (unsigned char *)ptr;
int i, j;
configASSERT(ptr != NULL);
for (i = 0; i < buflen; i += 16)
{
printf("%08X: ", i);
for (j = 0; j < 16; j++)
if (i + j < buflen)
printf("%02X ", buf[i + j]);
else
printf(" ");
printf(" ");
for (j = 0; j < 16; j++)
if (i + j < buflen)
printf("%c", __is_print(buf[i + j]) ? buf[i + j] : '.');
printf("\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 netif *now_netif;
netfrm_head = 0;
netfrm_tail = 0;
netfrm_full = 0;
if(netif == NULL){
now_netif = netif_find(ifname);
if (now_netif == NULL){
printf("hook install error 'device == NULL'\n");
return 0;
}
if ((now_netif == NULL) || (now_netif->linkoutput == NULL)){
printf("hook install error '(netif == NULL) || (netif->linkoutput == NULL)'\n");
return 0;
}
}else{
printf("device %s hook already installed, must be uninstall it before intall new on\n", ifname);
}
netif = now_netif;
//install netdev hook
taskENTER_CRITICAL(); //进入临界段
link_output = netif->linkoutput;
netif->linkoutput = _netif_linkoutput;
input = netif->input;
netif->input = _netif_input;
taskEXIT_CRITICAL(); //退出临界段
printf("hook installed on %s\n", ifname);
return 1;
}
/******************************************************************************
* hook uninstall
*/
int uninstall_hook(ecx_portt *port)
{
//uninstall netdev hook
if(netif != NULL){
taskENTER_CRITICAL(); //进入临界段
netif->input = input;
netif->linkoutput = link_output;
taskEXIT_CRITICAL(); //退出临界段
netif = NULL;
}
printf("hook uninstalled\n");
return 1;
}
/******************************************************************************
* netdev send/recv api
*/
int net_send(unsigned char *data, int len)
{
int ret = -1;
struct pbuf *p;
p = pbuf_alloc(PBUF_TRANSPORT, len, PBUF_POOL);
if (p != NULL)
{
pbuf_take(p, data, len);
if(hook_tx_dump){
printf("send --- len=%d>>>\n",len);
hex_dump(p->payload, p->tot_len);
}
_netif_linkoutput(netif,p);
pbuf_free(p);
ret = len;
}
else{
printf("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;
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){
printf("recv <<<---\n");
hex_dump(data, total);
printf("head = %d, tail = %d, full = %d\n", netfrm_head, netfrm_tail, netfrm_full);
}
return total;
}
//extern osSemaphoreId s_xSemaphore;
extern ETH_HandleTypeDef heth;
void net_hook_test(void)
{
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";
// unsigned char frame[] = "\xff\xff\xff\xff\xff\xff\xfc\x34\x97\x4a\xa6\xd2\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";
if(install_hook(NULL, "st0")==1)
{
uint32_t data = heth.Instance->MACFFR;
heth.Instance->MACFFR = data | 0x80000000;
net_send(frame,60);
osal_usleep(500);
// osSemaphoreRelease(s_xSemaphore);
vTaskDelay(1000);
// net_recv();
// etharp_output
uninstall_hook(NULL);
}
}
netdev_hook.h文件：
#ifndef _netdev_hook_h_
#define _netdev_hook_h_
#ifdef __cplusplus
extern "C"
{
#endif
#include "osal.h"
#include "oshw.h"
int install_hook(ecx_portt *port, const char *ifname);
int uninstall_hook(ecx_portt *port);
int net_send(unsigned char *data, int len);
int net_recv(unsigned char *data, int len);
void net_hook_test(void);
#ifdef __cplusplus
}
#endif
#endif
2.2.4 soem的剪切
stm32f407的flash有限，所以必须对文件定义堆栈跟空间进行缩减：
找的这几个定义修改成这样：或者更具自工程自己修改
2.2.5 确保lwip开启mac混杂模式
确保有这段代码：
macinit.ReceiveAll = ETH_RECEIVEALL_ENABLE;
macinit.PromiscuousMode = ETH_PROMISCUOUS_MODE_ENABLE;
3. 测试代码
好了整体移植成功后，新建任务测试一下：
/* 创建伺服电机控制任务 */
xReturn = xTaskCreate((TaskFunction_t)soem_motor_Task, //伺服电机
(const char* )"soem_motor_Task",
(uint16_t )1024*2,
(void* )NULL,
(UBaseType_t )6,
(TaskHandle_t* )&soem_motor_Task_Handle);
if(pdPASS == xReturn)
printf("创建PHY_Link_Check任务成功!\r\n");
soem_motor.c测试代码：
/*
* PVD.c
*
* Created on: 2022年9月22日
* Author: shengmidao
*/
#include "soem_motor.h"
#include <string.h>
#include <stdio.h>
#include "nicdrv.h"
#include <lwip/pbuf.h>
#include <lwip/inet.h>
#include "ethercat.h"
#include "FreeRTOSConfig.h"
#include "osal.h"
#include "oshw.h"
#include "netif.h"
static void test_oshw_htons (void)
{
uint16 network;
uint16 host;
host = 0x1234;
network = oshw_htons (host);
configASSERT(network == htons (host));
}
static void test_oshw_ntohs (void)
{
uint16 host;
uint16 network;
network = 0x1234;
host = oshw_ntohs (network);
configASSERT(host == ntohs (network));
}
//static uint8_t frame_data[] =
//{
// /*0x50,0xFA,0x84,0x15,0x3C,0x3C,*/ /* ??MAC */
// //0xff,0xFf,0xff,0xff,0xff,0xff,
// 0x00,0xE0,0x4C,0x36,0x10,0x00,
// 0x0,0x80,0xE1,0x0,0x0,0x0, /* ??MAC */
// 0x8,0x0, /* ip?? */
// //0x7,0x10, /* ip?? */
// 0x45,0x0,0x0,0x26/*l*/,0x0,0x0,0x0,0x0,0xFF,0x11,0x0,0x0, /* UDP?? */
// 0xC0,0xA8,0x1f,0xef, /* ??IP */
// 0xC0,0xA8,0x1f,0x63, /* ??IP */
// 0x22,0xB0, /* ???? */
// 0x22,0xB1, /* ???? */
// 0x0,0x12, /* UDP??*/
// 0x0,0x0, /* UDP?? */
// 0x68,0x65,0x6C,0x6C,0x6F,0x20,0x7A,0x6F,0x72,0x62 /* ?? */
//};
//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);
//}
//static void test_osal_timer_timeout_us (const uint32 timeout_us)
//{
// osal_timert timer;
// RT_ASSERT (timeout_us > 4000);
// osal_timer_start (&timer, timeout_us);
// RT_ASSERT (osal_timer_is_expired (&timer) == FALSE);
// osal_usleep (timeout_us - 2000);
// RT_ASSERT (osal_timer_is_expired (&timer) == FALSE);
// osal_usleep (4000);
// RT_ASSERT (osal_timer_is_expired (&timer));
//}
//static void test_osal_timer_timeout_us2 (const uint32 timeout_us)
//{
// osal_timert timer;
// struct timeval tp,tp1;
//
// RT_ASSERT (timeout_us > 4000);
// osal_gettimeofday(&tp1, 0);
//
// osal_timer_start (&timer, timeout_us);
// if(osal_timer_is_expired (&timer)){
// osal_gettimeofday(&tp, 0);
// //rt_kprintf("\ttime1 = %d,%03d,%03d(us)\n", tp1.tv_sec,tp1.tv_usec/1000,tp1.tv_usec%1000);
// rt_kprintf("\tFail-1 = %d,%03d,%03d(us)\n", tp1.tv_sec,tp1.tv_usec/1000,tp1.tv_usec%1000);
// goto fail;
// }
// osal_usleep (timeout_us - 2000);
// if(osal_timer_is_expired (&timer)){
// osal_gettimeofday(&tp, 0);
// //rt_kprintf("\ttime1 = %d,%03d,%03d(us)\n", tp1.tv_sec,tp1.tv_usec/1000,tp1.tv_usec%1000);
// rt_kprintf("\tFail-2 = %d,%03d,%03d(us)\n", tp1.tv_sec,tp1.tv_usec/1000,tp1.tv_usec%1000);
// goto fail;
// }
// osal_usleep (4000);
// if(osal_timer_is_expired (&timer)==FALSE){
// osal_gettimeofday(&tp, 0);
// //rt_kprintf("\ttime1 = %d,%03d,%03d(us)\n", tp1.tv_sec,tp1.tv_usec/1000,tp1.tv_usec%1000);
// rt_kprintf("\tFail-3 = %d,%03d,%03d(us)\n", tp1.tv_sec,tp1.tv_usec/1000,tp1.tv_usec%1000);
// goto fail;
// }
// osal_gettimeofday(&tp, 0);
// rt_kprintf("\ttime1 = %d,%03d,%03d(us)\n", tp1.tv_sec,tp1.tv_usec/1000,tp1.tv_usec%1000);
// rt_kprintf("\ttime2 = %d,%03d,%03d(us)\n", tp.tv_sec,tp.tv_usec/1000,tp.tv_usec%1000);
// rt_kprintf("%d us test pass\n", timeout_us);
// return;
//fail:
// rt_kprintf("%d us test fail\n", timeout_us);
//}
//static void test_osal_timer (void)
//{
// test_osal_timer_timeout_us (10*1000); /* 10ms */
// test_osal_timer_timeout_us (100*1000); /* 100ms */
// test_osal_timer_timeout_us (1000*1000); /* 1s */
// test_osal_timer_timeout_us (2000*1000); /* 2s */
//}
#define USECS_PER_SEC 1000000
#define USECS_PER_TICK 1000
#ifndef ABS
#define ABS(x) ((x) < 0 ? -(x) : (x))
#endif
static int32 time_difference_us (const ec_timet stop, const ec_timet start)
{
int32 difference_us;
configASSERT (stop.sec >= start.sec);
if (stop.sec == start.sec)
{
configASSERT (stop.usec >= start.usec);
}
difference_us = (stop.sec - start.sec) * USECS_PER_SEC;
difference_us += ((int32)stop.usec - (int32)start.usec);
configASSERT (difference_us >= 0);
return difference_us;
}
/**
* Test osal_current_time() by using it for measuring how long an osal_usleep()
* takes, in specified number of microseconds.
*/
ec_timet start_test;
ec_timet stop_test;
int32 sleep_test;
static void test_osal_current_time_for_delay_us (const int32 sleep_time_us)
{
ec_timet start;
ec_timet stop;
int32 measurement_us;
int32 deviation_us;
const int32 usleep_accuracy_us = USECS_PER_TICK;
boolean is_deviation_within_tolerance;
printf("sleep_us(%ld)...\n", sleep_time_us);
sleep_test=sleep_time_us;
start = osal_current_time ();
start_test=start;
osal_usleep (sleep_time_us);
stop = osal_current_time ();
stop_test=stop;
printf("\tstart time = %ld,%03ld,%03ld(us)\n", start.sec,start.usec/1000,start.usec%1000);
printf("\tstop time = %ld,%03ld,%03ld(us)\n", stop.sec,stop.usec/1000,stop.usec%1000);
measurement_us = time_difference_us (stop, start);
deviation_us = ABS (measurement_us - sleep_time_us);
is_deviation_within_tolerance = deviation_us <= usleep_accuracy_us;
if(is_deviation_within_tolerance){
printf("\terr=%ld pass\n", measurement_us - sleep_time_us);
}else{
printf("\terr=%ld fail\n", measurement_us - sleep_time_us);
}
}
static void test_osal_current_time (void)
{
test_osal_current_time_for_delay_us (1000);
test_osal_current_time_for_delay_us (2000);
test_osal_current_time_for_delay_us (3000);
test_osal_current_time_for_delay_us (0);
test_osal_current_time_for_delay_us (1);
test_osal_current_time_for_delay_us (500);
test_osal_current_time_for_delay_us (USECS_PER_TICK);
test_osal_current_time_for_delay_us (USECS_PER_TICK-1);
test_osal_current_time_for_delay_us (USECS_PER_TICK+1);
test_osal_current_time_for_delay_us (2 * 1000 * 1000); /* 2s */
test_osal_current_time_for_delay_us (400);
test_osal_current_time_for_delay_us (600);
test_osal_current_time_for_delay_us (800);
}
char IOmap[4096];
ec_ODlistt ODlist;
ec_OElistt OElist;
boolean printSDO = TRUE;
boolean printMAP = TRUE;
char usdo[128];
#define OTYPE_VAR 0x0007
#define OTYPE_ARRAY 0x0008
#define OTYPE_RECORD 0x0009
#define ATYPE_Rpre 0x01
#define ATYPE_Rsafe 0x02
#define ATYPE_Rop 0x04
#define ATYPE_Wpre 0x08
#define ATYPE_Wsafe 0x10
#define ATYPE_Wop 0x20
char* dtype2string(uint16 dtype, uint16 bitlen)
{
static char str[32] = { 0 };
switch(dtype)
{
case ECT_BOOLEAN:
sprintf(str, "BOOLEAN");
break;
case ECT_INTEGER8:
sprintf(str, "INTEGER8");
break;
case ECT_INTEGER16:
sprintf(str, "INTEGER16");
break;
case ECT_INTEGER32:
sprintf(str, "INTEGER32");
break;
case ECT_INTEGER24:
sprintf(str, "INTEGER24");
break;
case ECT_INTEGER64:
sprintf(str, "INTEGER64");
break;
case ECT_UNSIGNED8:
sprintf(str, "UNSIGNED8");
break;
case ECT_UNSIGNED16:
sprintf(str, "UNSIGNED16");
break;
case ECT_UNSIGNED32:
sprintf(str, "UNSIGNED32");
break;
case ECT_UNSIGNED24:
sprintf(str, "UNSIGNED24");
break;
case ECT_UNSIGNED64:
sprintf(str, "UNSIGNED64");
break;
case ECT_REAL32:
sprintf(str, "REAL32");
break;
case ECT_REAL64:
sprintf(str, "REAL64");
break;
case ECT_BIT1:
sprintf(str, "BIT1");
break;
case ECT_BIT2:
sprintf(str, "BIT2");
break;
case ECT_BIT3:
sprintf(str, "BIT3");
break;
case ECT_BIT4:
sprintf(str, "BIT4");
break;
case ECT_BIT5:
sprintf(str, "BIT5");
break;
case ECT_BIT6:
sprintf(str, "BIT6");
break;
case ECT_BIT7:
sprintf(str, "BIT7");
break;
case ECT_BIT8:
sprintf(str, "BIT8");
break;
case ECT_VISIBLE_STRING:
sprintf(str, "VISIBLE_STR(%d)", bitlen);
break;
case ECT_OCTET_STRING:
sprintf(str, "OCTET_STR(%d)", bitlen);
break;
default:
sprintf(str, "dt:0x%4.4X (%d)", dtype, bitlen);
}
return str;
}
char* otype2string(uint16 otype)
{
static char str[32] = { 0 };
switch(otype)
{
case OTYPE_VAR:
sprintf(str, "VAR");
break;
case OTYPE_ARRAY:
sprintf(str, "ARRAY");
break;
case OTYPE_RECORD:
sprintf(str, "RECORD");
break;
default:
sprintf(str, "ot:0x%4.4X", otype);
}
return str;
}
char* access2string(uint16 access)
{
static char str[32] = { 0 };
sprintf(str, "%s%s%s%s%s%s",
((access & ATYPE_Rpre) != 0 ? "R" : "_"),
((access & ATYPE_Wpre) != 0 ? "W" : "_"),
((access & ATYPE_Rsafe) != 0 ? "R" : "_"),
((access & ATYPE_Wsafe) != 0 ? "W" : "_"),
((access & ATYPE_Rop) != 0 ? "R" : "_"),
((access & ATYPE_Wop) != 0 ? "W" : "_"));
return str;
}
char* SDO2string(uint16 slave, uint16 index, uint8 subidx, uint16 dtype)
{
int l = sizeof(usdo) - 1, i;
uint8 *u8;
int8 *i8;
uint16 *u16;
int16 *i16;
uint32 *u32;
int32 *i32;
uint64 *u64;
int64 *i64;
float *sr;
double *dr;
char es[32];
memset(&usdo, 0, 128);
ec_SDOread(slave, index, subidx, FALSE, &l, &usdo, EC_TIMEOUTRXM);
if (EcatError)
{
return ec_elist2string();
}
else
{
static char str[64] = { 0 };
switch(dtype)
{
case ECT_BOOLEAN:
u8 = (uint8*) &usdo[0];
if (*u8) sprintf(str, "TRUE");
else sprintf(str, "FALSE");
break;
case ECT_INTEGER8:
i8 = (int8*) &usdo[0];
sprintf(str, "0x%2.2x / %d", *i8, *i8);
break;
case ECT_INTEGER16:
i16 = (int16*) &usdo[0];
sprintf(str, "0x%4.4x / %d", *i16, *i16);
break;
case ECT_INTEGER32:
case ECT_INTEGER24:
i32 = (int32*) &usdo[0];
sprintf(str, "0x%8.8lx / %ld", *i32, *i32);
break;
case ECT_INTEGER64:
i64 = (int64*) &usdo[0];
sprintf(str, "0x%16.16"PRIx64" / %"PRId64, *i64, *i64);
break;
case ECT_UNSIGNED8:
u8 = (uint8*) &usdo[0];
sprintf(str, "0x%2.2x / %u", *u8, *u8);
break;
case ECT_UNSIGNED16:
u16 = (uint16*) &usdo[0];
sprintf(str, "0x%4.4x / %u", *u16, *u16);
break;
case ECT_UNSIGNED32:
case ECT_UNSIGNED24:
u32 = (uint32*) &usdo[0];
sprintf(str, "0x%8.8lx / %lu", *u32, *u32);
break;
case ECT_UNSIGNED64:
u64 = (uint64*) &usdo[0];
sprintf(str, "0x%16.16"PRIx64" / %"PRIu64, *u64, *u64);
break;
case ECT_REAL32:
sr = (float*) &usdo[0];
sprintf(str, "%f", *sr);
sprintf(str, "%f", *sr);
break;
case ECT_REAL64:
dr = (double*) &usdo[0];
sprintf(str, "%f", *dr);
break;
case ECT_BIT1:
case ECT_BIT2:
case ECT_BIT3:
case ECT_BIT4:
case ECT_BIT5:
case ECT_BIT6:
case ECT_BIT7:
case ECT_BIT8:
u8 = (uint8*) &usdo[0];
sprintf(str, "0x%x / %u", *u8, *u8);
break;
case ECT_VISIBLE_STRING:
strcpy(str, "\"");
strcat(str, usdo);
strcat(str, "\"");
break;
case ECT_OCTET_STRING:
str[0] = 0x00;
for (i = 0 ; i < l ; i++)
{
sprintf(es, "0x%2.2x ", usdo[i]);
strcat( str, es);
}
break;
default:
sprintf(str, "Unknown type");
}
return str;
}
}
/** Read PDO assign structure */
int si_PDOassign(uint16 slave, uint16 PDOassign, int mapoffset, int bitoffset)
{
uint16 idxloop, nidx, subidxloop, rdat, idx, subidx;
uint8 subcnt;
int wkc, bsize = 0, rdl;
int32 rdat2;
uint8 bitlen, obj_subidx;
uint16 obj_idx;
int abs_offset, abs_bit;
rdl = sizeof(rdat); rdat = 0;
/* read PDO assign subindex 0 ( = number of PDO's) */
wkc = ec_SDOread(slave, PDOassign, 0x00, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
rdat = etohs(rdat);
/* positive result from slave ? */
if ((wkc > 0) && (rdat > 0))
{
/* number of available sub indexes */
nidx = rdat;
bsize = 0;
/* read all PDO's */
for (idxloop = 1; idxloop <= nidx; idxloop++)
{
rdl = sizeof(rdat); rdat = 0;
/* read PDO assign */
wkc = ec_SDOread(slave, PDOassign, (uint8)idxloop, FALSE, &rdl, &rdat, EC_TIMEOUTRXM);
/* result is index of PDO */
idx = etohs(rdat);
if (idx > 0)
{
rdl = sizeof(subcnt); subcnt = 0;
/* read number of subindexes of PDO */
wkc = ec_SDOread(slave,idx, 0x00, FALSE, &rdl, &subcnt, EC_TIMEOUTRXM);
subidx = subcnt;
/* for each subindex */
for (subidxloop = 1; subidxloop <= subidx; subidxloop++)
{
rdl = sizeof(rdat2); rdat2 = 0;
/* read SDO that is mapped in PDO */
wkc = ec_SDOread(slave, idx, (uint8)subidxloop, FALSE, &rdl, &rdat2, EC_TIMEOUTRXM);
rdat2 = etohl(rdat2);
/* extract bitlength of SDO */
bitlen = LO_BYTE(rdat2);
bsize += bitlen;
obj_idx = (uint16)(rdat2 >> 16);
obj_subidx = (uint8)((rdat2 >> 8) & 0x000000ff);
abs_offset = mapoffset + (bitoffset / 8);
abs_bit = bitoffset % 8;
ODlist.Slave = slave;
ODlist.Index[0] = obj_idx;
OElist.Entries = 0;
wkc = 0;
/* read object entry from dictionary if not a filler (0x0000:0x00) */
if(obj_idx || obj_subidx)
wkc = ec_readOEsingle(0, obj_subidx, &ODlist, &OElist);
printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen);
if((wkc > 0) && OElist.Entries)
{
printf(" %-12s %s\n", dtype2string(OElist.DataType[obj_subidx], bitlen), OElist.Name[obj_subidx]);
}
else
printf("\n");
bitoffset += bitlen;
};
};
};
};
/* return total found bitlength (PDO) */
return bsize;
}
int si_map_sdo(int slave)
{
int wkc, rdl;
int retVal = 0;
uint8 nSM, iSM, tSM;
int Tsize, outputs_bo, inputs_bo;
uint8 SMt_bug_add;
printf("PDO mapping according to CoE :\n");
SMt_bug_add = 0;
outputs_bo = 0;
inputs_bo = 0;
rdl = sizeof(nSM); nSM = 0;
/* read SyncManager Communication Type object count */
wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, 0x00, FALSE, &rdl, &nSM, EC_TIMEOUTRXM);
/* positive result from slave ? */
if ((wkc > 0) && (nSM > 2))
{
/* make nSM equal to number of defined SM */
nSM--;
/* limit to maximum number of SM defined, if true the slave can't be configured */
if (nSM > EC_MAXSM)
nSM = EC_MAXSM;
/* iterate for every SM type defined */
for (iSM = 2 ; iSM <= nSM ; iSM++)
{
rdl = sizeof(tSM); tSM = 0;
/* read SyncManager Communication Type */
wkc = ec_SDOread(slave, ECT_SDO_SMCOMMTYPE, iSM + 1, FALSE, &rdl, &tSM, EC_TIMEOUTRXM);
if (wkc > 0)
{
if((iSM == 2) && (tSM == 2)) // SM2 has type 2 == mailbox out, this is a bug in the slave!
{
SMt_bug_add = 1; // try to correct, this works if the types are 0 1 2 3 and should be 1 2 3 4
printf("Activated SM type workaround, possible incorrect mapping.\n");
}
if(tSM)
tSM += SMt_bug_add; // only add if SMt > 0
if (tSM == 3) // outputs
{
/* read the assign RXPDO */
printf(" SM%1d outputs\n addr b index: sub bitl data_type name\n", iSM);
Tsize = si_PDOassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].outputs - (uint8 *)&IOmap[0]), outputs_bo );
outputs_bo += Tsize;
}
if (tSM == 4) // inputs
{
/* read the assign TXPDO */
printf(" SM%1d inputs\n addr b index: sub bitl data_type name\n", iSM);
Tsize = si_PDOassign(slave, ECT_SDO_PDOASSIGN + iSM, (int)(ec_slave[slave].inputs - (uint8 *)&IOmap[0]), inputs_bo );
inputs_bo += Tsize;
}
}
}
}
/* found some I/O bits ? */
if ((outputs_bo > 0) || (inputs_bo > 0))
retVal = 1;
return retVal;
}
int si_siiPDO(uint16 slave, uint8 t, int mapoffset, int bitoffset)
{
uint16 a , w, c, e, er, Size;
uint8 eectl;
uint16 obj_idx;
uint8 obj_subidx;
uint8 obj_name;
uint8 obj_datatype;
uint8 bitlen;
int totalsize;
ec_eepromPDOt eepPDO;
ec_eepromPDOt *PDO;
int abs_offset, abs_bit;
char str_name[EC_MAXNAME + 1];
eectl = ec_slave[slave].eep_pdi;
Size = 0;
totalsize = 0;
PDO = &eepPDO;
PDO->nPDO = 0;
PDO->Length = 0;
PDO->Index[1] = 0;
for (c = 0 ; c < EC_MAXSM ; c++) PDO->SMbitsize[c] = 0;
if (t > 1)
t = 1;
PDO->Startpos = ec_siifind(slave, ECT_SII_PDO + t);
if (PDO->Startpos > 0)
{
a = PDO->Startpos;
w = ec_siigetbyte(slave, a++);
w += (ec_siigetbyte(slave, a++) << 8);
PDO->Length = w;
c = 1;
/* traverse through all PDOs */
do
{
PDO->nPDO++;
PDO->Index[PDO->nPDO] = ec_siigetbyte(slave, a++);
PDO->Index[PDO->nPDO] += (ec_siigetbyte(slave, a++) << 8);
PDO->BitSize[PDO->nPDO] = 0;
c++;
/* number of entries in PDO */
e = ec_siigetbyte(slave, a++);
PDO->SyncM[PDO->nPDO] = ec_siigetbyte(slave, a++);
a++;
obj_name = ec_siigetbyte(slave, a++);
a += 2;
c += 2;
if (PDO->SyncM[PDO->nPDO] < EC_MAXSM) /* active and in range SM? */
{
str_name[0] = 0;
if(obj_name)
ec_siistring(str_name, slave, obj_name);
if (t)
printf(" SM%1d RXPDO 0x%4.4X %s\n", PDO->SyncM[PDO->nPDO], PDO->Index[PDO->nPDO], str_name);
else
printf(" SM%1d TXPDO 0x%4.4X %s\n", PDO->SyncM[PDO->nPDO], PDO->Index[PDO->nPDO], str_name);
printf(" addr b index: sub bitl data_type name\n");
/* read all entries defined in PDO */
for (er = 1; er <= e; er++)
{
c += 4;
obj_idx = ec_siigetbyte(slave, a++);
obj_idx += (ec_siigetbyte(slave, a++) << 8);
obj_subidx = ec_siigetbyte(slave, a++);
obj_name = ec_siigetbyte(slave, a++);
obj_datatype = ec_siigetbyte(slave, a++);
bitlen = ec_siigetbyte(slave, a++);
abs_offset = mapoffset + (bitoffset / 8);
abs_bit = bitoffset % 8;
PDO->BitSize[PDO->nPDO] += bitlen;
a += 2;
/* skip entry if filler (0x0000:0x00) */
if(obj_idx || obj_subidx)
{
str_name[0] = 0;
if(obj_name)
ec_siistring(str_name, slave, obj_name);
printf(" [0x%4.4X.%1d] 0x%4.4X:0x%2.2X 0x%2.2X", abs_offset, abs_bit, obj_idx, obj_subidx, bitlen);
printf(" %-12s %s\n", dtype2string(obj_datatype, bitlen), str_name);
}
bitoffset += bitlen;
totalsize += bitlen;
}
PDO->SMbitsize[ PDO->SyncM[PDO->nPDO] ] += PDO->BitSize[PDO->nPDO];
Size += PDO->BitSize[PDO->nPDO];
c++;
}
else /* PDO deactivated because SM is 0xff or > EC_MAXSM */
{
c += 4 * e;
a += 8 * e;
c++;
}
if (PDO->nPDO >= (EC_MAXEEPDO - 1)) c = PDO->Length; /* limit number of PDO entries in buffer */
}
while (c < PDO->Length);
}
if (eectl) ec_eeprom2pdi(slave); /* if eeprom control was previously pdi then restore */
return totalsize;
}
int si_map_sii(int slave)
{
int retVal = 0;
int Tsize, outputs_bo, inputs_bo;
printf("PDO mapping according to SII :\n");
outputs_bo = 0;
inputs_bo = 0;
/* read the assign RXPDOs */
Tsize = si_siiPDO(slave, 1, (int)(ec_slave[slave].outputs - (uint8*)&IOmap), outputs_bo );
outputs_bo += Tsize;
/* read the assign TXPDOs */
Tsize = si_siiPDO(slave, 0, (int)(ec_slave[slave].inputs - (uint8*)&IOmap), inputs_bo );
inputs_bo += Tsize;
/* found some I/O bits ? */
if ((outputs_bo > 0) || (inputs_bo > 0))
retVal = 1;
return retVal;
}
void si_sdo(int cnt)
{
int i, j;
ODlist.Entries = 0;
memset(&ODlist, 0, sizeof(ODlist));
if( ec_readODlist(cnt, &ODlist))
{
printf(" CoE Object Description found, %d entries.\n",ODlist.Entries);
for( i = 0 ; i < ODlist.Entries ; i++)
{
uint8_t max_sub;
char name[128] = { 0 };
ec_readODdescription(i, &ODlist);
while(EcatError) printf(" - %s\n", ec_elist2string());
snprintf(name, sizeof(name) - 1, "\"%s\"", ODlist.Name[i]);
if (ODlist.ObjectCode[i] == OTYPE_VAR)
{
printf("0x%04x %-40s [%s]\n", ODlist.Index[i], name,
otype2string(ODlist.ObjectCode[i]));
}
else
{
printf("0x%04x %-40s [%s maxsub(0x%02x / %d)]\n",
ODlist.Index[i], name, otype2string(ODlist.ObjectCode[i]),
ODlist.MaxSub[i], ODlist.MaxSub[i]);
}
memset(&OElist, 0, sizeof(OElist));
ec_readOE(i, &ODlist, &OElist);
while(EcatError) printf("- %s\n", ec_elist2string());
if(ODlist.ObjectCode[i] != OTYPE_VAR)
{
int l = sizeof(max_sub);
ec_SDOread(cnt, ODlist.Index[i], 0, FALSE, &l, &max_sub, EC_TIMEOUTRXM);
}
else {
max_sub = ODlist.MaxSub[i];
}
for( j = 0 ; j < max_sub+1 ; j++)
{
if ((OElist.DataType[j] > 0) && (OElist.BitLength[j] > 0))
{
snprintf(name, sizeof(name) - 1, "\"%s\"", OElist.Name[j]);
printf(" 0x%02x %-40s [%-16s %6s] ", j, name,
dtype2string(OElist.DataType[j], OElist.BitLength[j]),
access2string(OElist.ObjAccess[j]));
if ((OElist.ObjAccess[j] & 0x0007))
{
printf("%s", SDO2string(cnt, ODlist.Index[i], j, OElist.DataType[j]));
}
printf("\n");
}
}
}
}
else
{
while(EcatError) printf("%s", ec_elist2string());
}
}
void soem_run(const char *param)
{
int cnt, i, j, nSM;
uint16 ssigen;
int expectedWKC;
printf("Starting slaveinfo\n");
if (ec_init(param))
{
printf("ec_init on %s succeeded.\n",param);
ec_configdc();
if ( ec_config(FALSE, &IOmap) > 0 )
{
while(EcatError) printf("%s", ec_elist2string());
printf("%d slaves found and configured.\n",ec_slavecount);
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(i = 1; i<=ec_slavecount ; i++)
{
if(ec_slave[i].state != EC_STATE_SAFE_OP)
{
printf("Slave %d State=%2x StatusCode=%4x : %s\n",
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
}
}
}
ec_readstate();
for( cnt = 1 ; cnt <= ec_slavecount ; cnt++)
{
printf("\nSlave:%d\n Name:%s\n Output size: %dbits\n Input size: %dbits\n State: %d\n Delay: %ld[ns]\n Has DC: %d\n",
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
ec_slave[cnt].state, ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
if (ec_slave[cnt].hasdc) printf(" DCParentport:%d\n", ec_slave[cnt].parentport);
printf(" Activeports:%d.%d.%d.%d\n", (ec_slave[cnt].activeports & 0x01) > 0 ,
(ec_slave[cnt].activeports & 0x02) > 0 ,
(ec_slave[cnt].activeports & 0x04) > 0 ,
(ec_slave[cnt].activeports & 0x08) > 0 );
printf(" Configured address: %4.4x\n", ec_slave[cnt].configadr);
printf(" Man: %8.8x ID: %8.8x Rev: %8.8x\n", (int)ec_slave[cnt].eep_man, (int)ec_slave[cnt].eep_id, (int)ec_slave[cnt].eep_rev);
for(nSM = 0 ; nSM < EC_MAXSM ; nSM++)
{
if(ec_slave[cnt].SM[nSM].StartAddr > 0)
printf(" SM%1d A:%4.4x L:%4d F:%8.8lx Type:%d\n",nSM, etohs(ec_slave[cnt].SM[nSM].StartAddr), etohs(ec_slave[cnt].SM[nSM].SMlength),
etohl(ec_slave[cnt].SM[nSM].SMflags), ec_slave[cnt].SMtype[nSM]);
}
for(j = 0 ; j < ec_slave[cnt].FMMUunused ; j++)
{
printf(" FMMU%1d Ls:%8.8lx Ll:%4d Lsb:%d Leb:%d Ps:%4.4x Psb:%d Ty:%2.2x Act:%2.2x\n", j,
etohl(ec_slave[cnt].FMMU[j].LogStart), etohs(ec_slave[cnt].FMMU[j].LogLength), ec_slave[cnt].FMMU[j].LogStartbit,
ec_slave[cnt].FMMU[j].LogEndbit, etohs(ec_slave[cnt].FMMU[j].PhysStart), ec_slave[cnt].FMMU[j].PhysStartBit,
ec_slave[cnt].FMMU[j].FMMUtype, ec_slave[cnt].FMMU[j].FMMUactive);
}
printf(" FMMUfunc 0:%d 1:%d 2:%d 3:%d\n",
ec_slave[cnt].FMMU0func, ec_slave[cnt].FMMU1func, ec_slave[cnt].FMMU2func, ec_slave[cnt].FMMU3func);
printf(" MBX length wr: %d rd: %d MBX protocols : %2.2x\n", ec_slave[cnt].mbx_l, ec_slave[cnt].mbx_rl, ec_slave[cnt].mbx_proto);
ssigen = ec_siifind(cnt, ECT_SII_GENERAL);
/* SII general section */
if (ssigen)
{
ec_slave[cnt].CoEdetails = ec_siigetbyte(cnt, ssigen + 0x07);
ec_slave[cnt].FoEdetails = ec_siigetbyte(cnt, ssigen + 0x08);
ec_slave[cnt].EoEdetails = ec_siigetbyte(cnt, ssigen + 0x09);
ec_slave[cnt].SoEdetails = ec_siigetbyte(cnt, ssigen + 0x0a);
if((ec_siigetbyte(cnt, ssigen + 0x0d) & 0x02) > 0)
{
ec_slave[cnt].blockLRW = 1;
ec_slave[0].blockLRW++;
}
ec_slave[cnt].Ebuscurrent = ec_siigetbyte(cnt, ssigen + 0x0e);
ec_slave[cnt].Ebuscurrent += ec_siigetbyte(cnt, ssigen + 0x0f) << 8;
ec_slave[0].Ebuscurrent += ec_slave[cnt].Ebuscurrent;
}
printf(" CoE details: %2.2x FoE details: %2.2x EoE details: %2.2x SoE details: %2.2x\n",
ec_slave[cnt].CoEdetails, ec_slave[cnt].FoEdetails, ec_slave[cnt].EoEdetails, ec_slave[cnt].SoEdetails);
printf(" Ebus current: %d[mA]\n only LRD/LWR:%d\n",
ec_slave[cnt].Ebuscurrent, ec_slave[cnt].blockLRW);
if ((ec_slave[cnt].mbx_proto & ECT_MBXPROT_COE) && printSDO)
si_sdo(cnt);
if(printMAP)
{
if (ec_slave[cnt].mbx_proto & ECT_MBXPROT_COE)
si_map_sdo(cnt);
else
si_map_sii(cnt);
}
}
}
else
{
printf("No slaves found!\n");
}
printf("End slaveinfo, close socket\n");
/* stop SOEM, close socket */
ec_close();
}else{
printf("ec_init on %s failed.\n",param);
}
}
#include "lwip.h"
extern struct netif gnetif;
void soem_motor_Task(void* parameter)
{
while(1)
{
printf("SOEM (Simple Open EtherCAT Master)\n");
//
printf("test_oshw_htons...");
test_oshw_htons ();
printf("Ok\n");
//
printf("test_oshw_htons...");
test_oshw_ntohs ();
printf("Ok\n");
printf("test_osal_current_time...\n");
// test_osal_current_time ();
printf("Test finished\n");
// ReceiveAll
// ETH_MACDMAConfig
// EthHandle.Instance->MACFFR
// ethernetif_input
// ethernet_input
// HAL_ETH_GetReceivedFrame_IT
// tcpip_input
// HAL_ETH_RxCpltCallback
// ETH_InitStructure
// ETH_DeInit
// IP_SOF_BROADCAST
// net_hook_test();
// HAL_ETH_Start
// ethernetif_input
// tcpip_thread
//slaveinfo test
// gnetif.name
soem_run("st0");
// EC_MAXODLIST
// EC_MAXSLAVE
// EC_MAXBUF
vTaskDelay(5000);
}
}
soem_motor.h测试代码：
#ifndef _SOEM_MOTOR_H
#define _SOEM_MOTOR_H
void soem_motor_Task(void* parameter);
#endif
4. 周期同步位置模式CSP控制代码
下面是测试csp周期位置模式测试代码：
周期发送位置就动起来了
/*
* PVD.c
*
* Created on: 2022年9月22日
* Author: shengmidao
*/
#include "soem_motor.h"
#include <string.h>
#include <stdio.h>
#include "nicdrv.h"
#include <lwip/pbuf.h>
#include <lwip/inet.h>
#include "ethercat.h"
#include "FreeRTOSConfig.h"
#include "osal.h"
#include "oshw.h"
#include "netif.h"
#include "netdev_hook.h"
//csp代码
#define __is_print(ch) ((unsigned int)((ch) - ' ') < 127u - ' ')
static void hex_dump(const uint8_t *ptr, size_t buflen)
{
unsigned char *buf = (unsigned char *)ptr;
int i, j;
configASSERT(ptr != NULL);
for (i = 0; i < buflen; i += 16)
{
printf("%08X: ", i);
for (j = 0; j < 16; j++)
if (i + j < buflen)
printf("%02X ", buf[i + j]);
else
printf(" ");
printf(" ");
for (j = 0; j < 16; j++)
if (i + j < buflen)
printf("%c", __is_print(buf[i + j]) ? buf[i + j] : '.');
printf("\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);
}
int safe_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, int size, void *data)
{
int wkc, cnt=0;
do{
wkc = ec_SDOwrite(Slave, Index, SubIndex, FALSE, size, data, EC_TIMEOUTRXM);
cnt++;
}while(wkc<=0 && cnt<10);
return wkc;
}
int safe_SDCwrite_b(uint16 Slave, uint16 Index, uint8 SubIndex, uint8 b)
{
return safe_SDOwrite(Slave, Index, SubIndex, 1, &b);
}
int safe_SDCwrite_w(uint16 Slave, uint16 Index, uint8 SubIndex, uint16 w)
{
return safe_SDOwrite(Slave, Index, SubIndex, 2, &w);
}
int safe_SDCwrite_dw(uint16 Slave, uint16 Index, uint8 SubIndex, uint32 dw)
{
return safe_SDOwrite(Slave, Index, SubIndex, 4, &dw);
}
int safe_SDOread(uint16 Slave, uint16 Index, uint8 SubIndex, int size, void *data)
{
int wkc, cnt=0;
do{
wkc = ec_SDOread(Slave, Index, SubIndex, FALSE, &size, data, EC_TIMEOUTRXM);
}while(wkc<=0 && cnt<10);
return wkc;
}
int safe_SDOread_b(uint16 Slave, uint16 Index, uint8 SubIndex, uint8 b)
{
return safe_SDOread(Slave, Index, SubIndex, 1, &b);
}
int safe_SDOread_w(uint16 Slave, uint16 Index, uint8 SubIndex, uint16 w)
{
return safe_SDOread(Slave, Index, SubIndex, 2, &w);
}
int safe_SDOread_dw(uint16 Slave, uint16 Index, uint8 SubIndex, uint32 dw)
{
return safe_SDOread(Slave, Index, SubIndex, 4, &dw);
}
void viewSDO(uint16_t slave, uint16_t index, uint16_t subindex, int bytes)
{
uint32_t dw = 0;
int wkc;
safe_SDOread(slave, index, subindex, bytes, &dw);
printf("SDO read=%s, SDO[0x%04x.%02x] = 0x%08x\n", wkc?"success":"fail",index, subindex, dw);
}
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, htoes(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, htoes(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); //pp模式
}
}
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; //5687->0x1637: 5744->0x1670:0x270
//printf("servo_switch_op: slave %d [6041]=%04x\n",slave,servos->servo_data[idx].status_word );
if(servos->servo_data[idx].status_word & 0x8) //故障处理
{ //0x21f //0x218
if(sta==0x0218)
{
servos->servo_data[idx].control_word = 0x80;
// 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 );
//base on cia402 control_word status_word bit0~9
/* 上电初始化 0x0000
* 初*-伺服无故障 0x0250
* 伺*-伺服准备好 0x0006 0x0231
* 伺*-等待使能伺服 0x0007 0x0233
* 等*-伺服运行 0x000F 0x0237
*
* 伺*-等待打开伺服使能 0x0007 0x0233
* 等*-伺服准备好 0x0006 0x0231
* 伺*-伺服无故障 0x0000 0x0250
*
* 伺服运行-伺服准备好 0x0006 0x0231
* 伺服运行-伺服无故障 0x0000 0x0250
* 等待打开伺服使能-伺服无故障 0x0000 0x0250
* 伺服运行-快速停机 0x0002 0x0217
*
* 快速停机-伺服无故障 0x0250
* -故障停机 0x021F
* 故障停机-故障 0x0218
* 故障-伺服无故障 0x80 0x0250
* 快速停机-伺服运行 0x0F 0x0237
*
*/
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;
case 0x217:
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;
}
}
#define EC_TIMEOUTMON 500
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;
long test_poit;
uint16 test_error;
void sv660n_config(char *ifname)
{
needlf = FALSE;
inOP = FALSE;
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);
ec_readstate();
//显示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);
ec_readstate();
// //故障复位
// safe_SDOread(1,0x603f,0,2,&test_error); //30081
// if(test_error==30081)
// {
// safe_SDCwrite_w(1,0x6040, 0, 0x80);
// }
// ec_readstate();
//现在应该在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);
ec_readstate();
//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);
// send one valid process data to make outputs in slaves happy
ec_slave[0].state = EC_STATE_OPERATIONAL;
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET*3);
ec_slave[0].state = EC_STATE_OPERATIONAL;
ec_writestate(0);
ec_readstate();
int chk = 200;
// wait for all slaves to reach OP state
do
{
ec_slave[0].state = EC_STATE_OPERATIONAL;
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("<<<Operational>>> state reached for all slaves.\n");
inOP = TRUE;
osal_timert t;
osal_timer_start(&t, 1000);
// cyclic loop
for(int i = 1; i <= 20000; i++)
{
servo_switch_op();
if(servos->servo_data[0].control_word==7){
servos->servo_data[0].dest_pos = servos->servo_data[0].cur_pos;
test_poit=servos->servo_data[0].dest_pos;
//printf("cur pos = %ld\n", servos->servo_data[0].cur_pos);
}
if(servos->servo_data[0].control_word==0xf){
if(i<=300)
{
servos->servo_data[0].dest_pos += (i*10);
}else
{
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_timer_start(&t, 1000);
while(osal_timer_is_expired(&t)==FALSE);
servos->servo_data[0].control_word = 0x0002;
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
// servos->servo_data[0].control_word = 0x0;
// ec_send_processdata();
// ec_receive_processdata(EC_TIMEOUTRET);
//
// servos->servo_data[0].control_word = 0x0;
// ec_send_processdata();
// ec_receive_processdata(EC_TIMEOUTRET);
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();
}
//伺服控制线程
#include "lwip.h"
extern struct netif gnetif;
void soem_motor_Task(void* parameter)
{
while(1)
{
//测试代码程序
// printf("SOEM (Simple Open EtherCAT Master)\n");
//
// //
// printf("test_oshw_htons...");
// test_oshw_htons ();
// printf("Ok\n");
//
// //
// printf("test_oshw_htons...");
// test_oshw_ntohs ();
// printf("Ok\n");
//
// printf("test_osal_current_time...\n");
// test_osal_current_time ();
//
// printf("Test finished\n");
//
sv660n_config("st0");
vTaskDelay(10000);
}
}
5. 周期同步扭矩模式CST控制代码
下面是测试cst周期扭矩模式测试代码：
周期发送扭矩就动起来了
/*
* PVD.c
*
* Created on: 2022年9月22日
* Author: shengmidao
*/
#include "soem_motor.h"
#include <string.h>
#include <stdio.h>
#include "nicdrv.h"
#include <lwip/pbuf.h>
#include <lwip/inet.h>
#include "ethercat.h"
#include "FreeRTOSConfig.h"
#include "osal.h"
#include "oshw.h"
#include "netif.h"
#include "netdev_hook.h"
//csp代码
#define __is_print(ch) ((unsigned int)((ch) - ' ') < 127u - ' ')
static void hex_dump(const uint8_t *ptr, size_t buflen)
{
unsigned char *buf = (unsigned char *)ptr;
int i, j;
configASSERT(ptr != NULL);
for (i = 0; i < buflen; i += 16)
{
printf("%08X: ", i);
for (j = 0; j < 16; j++)
if (i + j < buflen)
printf("%02X ", buf[i + j]);
else
printf(" ");
printf(" ");
for (j = 0; j < 16; j++)
if (i + j < buflen)
printf("%c", __is_print(buf[i + j]) ? buf[i + j] : '.');
printf("\n");
}
}
static char IOmap[4096];
typedef struct __attribute__((__packed__))
{
unsigned char mode_byte;
unsigned short control_word;
long dest_pos;
long dest_V;
unsigned short dest_T;
unsigned short error_word;
unsigned short status_word;
long cur_pos;
long cur_V;
unsigned short cur_T;
}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);
}
int safe_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, int size, void *data)
{
int wkc, cnt=0;
do{
wkc = ec_SDOwrite(Slave, Index, SubIndex, FALSE, size, data, EC_TIMEOUTRXM);
cnt++;
}while(wkc<=0 && cnt<10);
return wkc;
}
int safe_SDCwrite_b(uint16 Slave, uint16 Index, uint8 SubIndex, uint8 b)
{
return safe_SDOwrite(Slave, Index, SubIndex, 1, &b);
}
int safe_SDCwrite_w(uint16 Slave, uint16 Index, uint8 SubIndex, uint16 w)
{
return safe_SDOwrite(Slave, Index, SubIndex, 2, &w);
}
int safe_SDCwrite_dw(uint16 Slave, uint16 Index, uint8 SubIndex, uint32 dw)
{
return safe_SDOwrite(Slave, Index, SubIndex, 4, &dw);
}
int safe_SDOread(uint16 Slave, uint16 Index, uint8 SubIndex, int size, void *data)
{
int wkc, cnt=0;
do{
wkc = ec_SDOread(Slave, Index, SubIndex, FALSE, &size, data, EC_TIMEOUTRXM);
}while(wkc<=0 && cnt<10);
return wkc;
}
int safe_SDOread_b(uint16 Slave, uint16 Index, uint8 SubIndex, uint8 b)
{
return safe_SDOread(Slave, Index, SubIndex, 1, &b);
}
int safe_SDOread_w(uint16 Slave, uint16 Index, uint8 SubIndex, uint16 w)
{
return safe_SDOread(Slave, Index, SubIndex, 2, &w);
}
int safe_SDOread_dw(uint16 Slave, uint16 Index, uint8 SubIndex, uint32 dw)
{
return safe_SDOread(Slave, Index, SubIndex, 4, &dw);
}
void viewSDO(uint16_t slave, uint16_t index, uint16_t subindex, int bytes)
{
uint32_t dw = 0;
int wkc;
safe_SDOread(slave, index, subindex, bytes, &dw);
printf("SDO read=%s, SDO[0x%04x.%02x] = 0x%08x\n", wkc?"success":"fail",index, subindex, dw);
}
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, htoes(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_dw(slave, 0x1600, ++ind, htoel(0x60ff0020));//60ffh(目标速度)
safe_SDCwrite_dw(slave, 0x1600, ++ind, htoel(0x60710010));//6071h(目标扭矩)
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, htoes(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_dw(slave, 0x1a00, ++ind, htoel(0x606c0020));//6064h(速度反馈)
safe_SDCwrite_dw(slave, 0x1a00, ++ind, htoel(0x60770010));//6064h(扭矩反馈)
safe_SDCwrite_b(slave, 0x1a00, 0, ind);
//1c13.0
safe_SDCwrite_b(slave, 0x1c13, 0, 1);
safe_SDCwrite_b(slave, 0x6060, 0, 1); //pp模式
}
}
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; //5687->0x1637: 5744->0x1670:0x270
//printf("servo_switch_op: slave %d [6041]=%04x\n",slave,servos->servo_data[idx].status_word );
if(servos->servo_data[idx].status_word & 0x8) //故障处理
{ //0x21f //0x218
if(sta==0x0218)
{
servos->servo_data[idx].control_word = 0x80;
// 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 );
//base on cia402 control_word status_word bit0~9
/* 上电初始化 0x0000
* 初*-伺服无故障 0x0250
* 伺*-伺服准备好 0x0006 0x0231
* 伺*-等待使能伺服 0x0007 0x0233
* 等*-伺服运行 0x000F 0x0237
*
* 伺*-等待打开伺服使能 0x0007 0x0233
* 等*-伺服准备好 0x0006 0x0231
* 伺*-伺服无故障 0x0000 0x0250
*
* 伺服运行-伺服准备好 0x0006 0x0231
* 伺服运行-伺服无故障 0x0000 0x0250
* 等待打开伺服使能-伺服无故障 0x0000 0x0250
* 伺服运行-快速停机 0x0002 0x0217
*
* 快速停机-伺服无故障 0x0250
* -故障停机 0x021F
* 故障停机-故障 0x0218
* 故障-伺服无故障 0x80 0x0250
* 快速停机-伺服运行 0x0F 0x0237
*
*/
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;
case 0x217:
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;
}
}
#define EC_TIMEOUTMON 500
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;
uint16 test_error;
void sv660n_config(char *ifname,char *mode)
{
needlf = FALSE;
inOP = FALSE;
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);
ec_readstate();
//显示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);
ec_readstate();
// //故障复位
// safe_SDOread(1,0x603f,0,2,&test_error); //30081
// if(test_error==30081)
// {
// safe_SDCwrite_w(1,0x6040, 0, 0x80);
// }
// ec_readstate();
//现在应该在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);
ec_readstate();
//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 = 10; //cst 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);
// send one valid process data to make outputs in slaves happy
ec_slave[0].state = EC_STATE_OPERATIONAL;
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET*3);
ec_slave[0].state = EC_STATE_OPERATIONAL;
ec_writestate(0);
ec_readstate();
int chk = 200;
// wait for all slaves to reach OP state
do
{
ec_slave[0].state = EC_STATE_OPERATIONAL;
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("<<<Operational>>> state reached for all slaves.\n");
inOP = TRUE;
osal_timert t;
osal_timer_start(&t, 1000);
// cyclic loop
for(int i = 1; i <= 20000; i++)
{
servo_switch_op();
if(servos->servo_data[0].control_word==7){
servos->servo_data[0].dest_T = 0;
//printf("cur pos = %ld\n", servos->servo_data[0].cur_pos);
}
if(servos->servo_data[0].control_word==0xf){
if(i<=100)
{
servos->servo_data[0].dest_T = (i);
}else
{
servos->servo_data[0].dest_T = 100;
}
}
while(osal_timer_is_expired(&t)==FALSE);
osal_timer_start(&t, 400);
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_timer_start(&t, 1000);
while(osal_timer_is_expired(&t)==FALSE);
servos->servo_data[0].control_word = 0x0002;
ec_send_processdata();
ec_receive_processdata(EC_TIMEOUTRET);
inOP = FALSE;
}
else
{
ec_readstate();
for(int i = 1; i<=ec_slavecount ; i++)
{
if(ec_slave[i].state != EC_STATE_OPERATIONAL)
{
}
}
}
ec_slave[0].state = EC_STATE_INIT;
ec_writestate(0);
}
} else {
}
echo_time();
ec_close();
}else{
}
view_slave_data();
}
//伺服控制线程
#include "lwip.h"
extern struct netif gnetif;
void soem_motor_Task(void* parameter)
{
while(1)
{
//测试代码程序
// printf("SOEM (Simple Open EtherCAT Master)\n");
//
// //
// printf("test_oshw_htons...");
// test_oshw_htons ();
// printf("Ok\n");
//
// //
// printf("test_oshw_htons...");
// test_oshw_ntohs ();
// printf("Ok\n");
//
// printf("test_osal_current_time...\n");
// test_osal_current_time ();
//
// printf("Test finished\n");
//
// soem_run("st0");
sv660n_config("st0","CSP"); //CSP CSV CST
vTaskDelay(10000);
}
}
6. 汇川使用注意点
一定要开启DC同步时钟，将ec_dcsync0(1, TRUE, 2000000, 3000)开启
如果汇川驱动器显示无法从safe_op进入op状态，无法进入OP的原因就是同步失败导致的了。官方手册给出的错误代码原因是网络抖动造成的从站同步信号丢失，说明设置的DC周期太短了，在当前主站性能和网络条件下无法实现，所以适当增加DC周期即可解决。如：从默认的2000us改为10000us，如：ec_dcsync0(1, TRUE, 10000000, 3000)
7. 相关资料
ethercat协议+ CIA402协议+FreeRTOS例子：资料: link
链接稍后再挂。
都看到这里，来都来了，不点赞关注下，有什么问题可以在评论区提问
扫一扫
1713
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
