【FreeRTOS的SOEM-master（1.4.1）移植】
2024-05-20 20:20:50
FreeRTOS的SOEM-master（1.4.1）移植
- 1. 本次移植系统及前提
- 2. 移植三步骤（osal、oshw、soem）
- 2.1 官方soem移植下载
- 2.2 FreeRTOS版本SOEM移植
- 2.2.1 osal.c修改
- 2.2.2 oshw.c修改
- 2.2.3 nicdrv.c修改及增加netdev_hook.c
- 2.2.4 soem的剪切
- 2.2.5 确保lwip开启mac混杂模式
- 3. 测试代码
- 4. 周期同步位置模式CSP控制代码
- 5. 周期同步扭矩模式CST控制代码
- 6. 相关资料
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.csdn.net/lg28870983/article/details/124647952 link
- 基于STM32构建EtherCAT主站:https://blog.csdn.net/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;}//不超过1msstart_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 compileif(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);
#endifvoid 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);#endifnetdev_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 10static 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]);elseprintf(" ");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 hooktaskENTER_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 hookif(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_outputuninstall_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#endif2.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))
#endifstatic 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 0x20char* 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]);}elseprintf("\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 4printf("Activated SM type workaround, possible incorrect mapping.\n");}if(tSM)tSM += SMt_bug_add; // only add if SMt > 0if (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);elseprintf(" 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);elsesi_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.namesoem_run("st0");
// EC_MAXODLIST
// EC_MAXSLAVE
// EC_MAXBUFvTaskDelay(5000);}
}
soem_motor.h测试代码：
#ifndef _SOEM_MOTOR_H
#define _SOEM_MOTOR_Hvoid soem_motor_Task(void* parameter);#endif4. 周期同步位置模式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]);elseprintf(" ");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.0safe_SDCwrite_b(slave, 0x1c12, 0, 0);safe_SDCwrite_w(slave, 0x1c12, 1, htoes(0x1600));//1600ind = 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.0safe_SDCwrite_b(slave, 0x1c12, 0, 1);//tpdo-------------//1c13.0safe_SDCwrite_b(slave, 0x1c13, 0x00, 0);safe_SDCwrite_w(slave, 0x1c13, 0x01, htoes(0x1a00));//1a00ind = 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.0safe_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 //0x218if(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 slavesec_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 1while(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 fmmuec_config_map(IOmap);/* request safe_op for slave */ec_slave[0].state = EC_STATE_SAFE_OP;ec_writestate(0);ec_readstate();//safe-opexpectedWKC = (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 happyec_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 statedo{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 loopfor(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 slavesec_writestate(0);}} else {// printf("No slaves found!\n");}echo_time();
// printf("End soem, close socket\n");// stop SOEM, close socketec_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]);elseprintf(" ");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.0safe_SDCwrite_b(slave, 0x1c12, 0, 0);safe_SDCwrite_w(slave, 0x1c12, 1, htoes(0x1600));//1600ind = 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.0safe_SDCwrite_b(slave, 0x1c12, 0, 1);//tpdo-------------//1c13.0safe_SDCwrite_b(slave, 0x1c13, 0x00, 0);safe_SDCwrite_w(slave, 0x1c13, 0x01, htoes(0x1a00));//1a00ind = 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.0safe_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 //0x218if(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 slavesec_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 1while(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 fmmuec_config_map(IOmap);/* request safe_op for slave */ec_slave[0].state = EC_STATE_SAFE_OP;ec_writestate(0);ec_readstate();//safe-opexpectedWKC = (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 happyec_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 statedo{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 loopfor(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 CSTvTaskDelay(10000);}
}6. 相关资料
ethercat协议+ CIA402协议+FreeRTOS例子：资料: link
链接稍后再挂。
都看到这里，来都来了，不点赞关注下，有什么问题可以在评论区提问
【FreeRTOS的SOEM-master（1.4.1）移植】相关推荐
- EtherCAT主站SOEM在Ubuntu上的移植
EtherCAT主站SOEM在Ubuntu上的移植 原文:https://blog.csdn.net/ethercat_i7/article/details/52794069?utm_medium=d ...
- FreeRTOS系统下LwIP-1.4.1的移植
转载至 基于FreeRTOS与MQTT的物联网技术应用系列--步进电机控制(四)FreeRTOS系统下LwIP-1.4.1的移植 怕以后查找麻烦,特粘贴至此~~~~~~ 本文使用的网卡PHY芯片型号是 ...
- FreeRTOS 之一 源码目录文件 及 移植详解
写在前面 2018/1/15更新了文章中的部分错误. FreeRTOS源码为最新版的10.0.1.FreeRTOS 10包含两个重要的新功能:流缓冲区和消息缓冲区. 从10.0.0开始,FreeRTO ...
- 基于FreeRTOS与MQTT的物联网技术应用系列——步进电机控制（四）FreeRTOS系统下LwIP-1.4.1的移植
本文在前一篇的基础上,进行基于FreeRTOS的LwIP-1.4.1版本移植. 本文使用的网卡PHY芯片型号是DP83848,工作在MII接口模式,时钟频率是25MHz. 现在的LwIP版本已经发展到 ...
- 【FreeRTOS】在Cortex-M4开发板上移植FreeRTOS并且实现LED灯闪烁（保姆级教程）
相信有不少小伙伴手上只有M4的开发板,想要移植FreeRTOS:但是,网上大部分都是M3移植教程.因此,陷入深深的迷茫中,难不成只能使用仿真了???
- N32G457VEL7移植FreeRTOS代码
代码 先附上代码链接(在分支里) 或者直接拉取: git@gitee.com:sanfro/mcu.git 移植 内核移植的东西比如滴答定时器初始化等等,FreeRTOS都帮忙写好了,移植方法其实与S ...
- 基于STM32的实时操作系统FreeRTOS移植教程（手动移植）
前言:此文为笔者FreeRTOS专栏下的第一篇基础性教学文章,其主要目的为:帮助读者朋友快速搭建出属于自己的公版FreeRTOS系统,实现后续在实时操作系统FreeRTOS上的开发与运用.操作系统的学 ...
- FreeRTOS移植到STM32
一.找一个STM32的裸机工程模板 我们以STM32F103裸机程序为例 随便找的一个裸机程序 二.去官网上下载FreeRTOS V9.0.0 源码 在移植之前,我们首先要获取到 FreeRTOS 的 ...
- STM32F4基础工程移植FreeRTOS
本篇目标:基于上一篇的基础工程(stm32f4_first_demo)上,移植freertos,建立移植工程(stm32f4_freertos). 资料准备: Freertos源文件 最后工程:项目工 ...
- 树莓派 pico 移植 FreeRTOS
文章目录 1.FreeRTOS 2.创建工程 3.移植FreeRTOS 4.编写代码 1.FreeRTOS FreeRTOS 是一个迷你的实时操作系统内核.作为一个轻量级的操作系统,功能包括:任务管理 ...
最新文章
- 个人信息泄露致电信诈骗猖獗 专家:治理亟须完善立法
- C# 创建、部署和调用WebService的示例
- php r echo_PHP中echo(),print(),print_r()的区别是什么？
- ubuntu boot空间不足_安装 Ubuntu 双系统
- 树莓派3代刷ubuntu mate在命令行下配置wifi不能连接的一个诡异的bug的解决
- ARIMA时间序列分析
- 在主线程执行_深入理解JavaScript执行机制
- C#Excel上传批量导入sqlserver
- iPhone 4 Cydia使用教程！精选Cydia源！cydia怎么添加源！Cydia源使用方法！越狱后使用cydia全攻略！...
- Win7扫雷的H5完整复刻实现（二） / js扫雷算法处理空白连通域与点击不同方块的实现
- STM32F401的PWM输出
- python画频率直方图_用matplotlib画直方图（histogram）
- 分享免费的主流电商平台商品图片批量下载方法
- 2021年危险化学品生产单位主要负责人考试资料及危险化学品生产单位主要负责人找解析
- 打开EXCEL运行脚本，报无法运行宏问题
- MySQL 数据库设计范式/优化
- sqlserver数据库可疑解决办法
- ArcGis缓冲区分析
- 服务器里的文件删了能恢复吗,远程把服务器文件删了怎么恢复
- 创业公司CTO/技术总监的岗位职责及任职要求
热门文章
- WPF中使用StackPanel，Border进行简单布局
- 地图之美（地图制图）
- 现Revit BuiltInParameter.Model中枚举项，都可以用LabelUitls转成本地语言
- java ror_Java会因为RoR的流行而过时吗?
- 如何让Linux上的GPG error 无法验证的这个公钥 NO_PUBKEY D97A3AE911FXXXXX 出错信息消失？
- 服务器系统如何校验md5值,什么是md5校验工具,md5校验工具怎么用?
- 2022年电工杯A题高比例风电电力系统储能运行及配置分析参考代码
- 给“网吧”做管理系统
- Thinkpad T61宽屏风扇拆缷与清理灰尘手记
- 第八章《Unity游戏优化》内存管理