EtherCAT从站学习笔记

原创已于 2026-04-14 08:59:55 修改 · 2.4k 阅读

41 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#学习 #笔记

于 2023-12-02 15:29:35 首次发布

EtherCAT从站学习笔记

0.写在最前

从上学期间开始接触EtherCAT协议，直到工作后，做的都是跟EtherCAT主站相关的工作，对于从站只知道大概的架构，其余的知之甚少，且一直对从站较为感兴趣，这段时间抽空周末时间研究了下从站，之前一些不懂的地方也搞懂了，写点文档粗略地记录下，不对的地方也请各路大神指正。

主要研究了AX58100+STM32的实现方式，稍微也看了下瑞萨RZT1平台的实现方式，资料下载地址如下：

AX58100：AX58100 - 2/3端口EtherCAT从站控制芯片(ESC) 内建两组快速以太网PHYs | 亚信电子 (asix.com.tw)

瑞萨RZT1：RZ/T1 - Microprocessors for Real-Time Control of Industrial Equipment and Networking at the Same Time | Renesas

1.EtherCAT从站架构

EtherCAT总线是目前工业上常用的基于以太网的现场总线，由德国倍福公司研发。EtherCAT为了实现其“On the fly”的特性，没有采用以太网常用的第三层报文中常用的TCP/IP寻址方式，从站由专门的通信芯片实现，该芯片被称为ESC（EtherCAT Slave Controller），若从站功能较为简单，如数字量IO从站，仅需ESC芯片就可满足需求，若从站功能较为复杂，需配合一个MCU来实现，该MCU支持与ESC通信所需要的串口或并口就可以。还有一部分MCU芯片，本身就支持ESC功能（如瑞萨RZT1，RZN2L，TI AM335x，AM43XX等），这样一片MCU边可以实现从站。
ESC加MCU独立芯片 MCU集成ESC

2.代码分析

2.1 MCU及ESC初始化

在MCU启动后，进行必要的初始化，使MCU与ESC建立交互，如SPI，并口等外设的初始化，及其他外设初始化，如GPIO，ESC代码用Hw_Init函数进行初始化。

AX58100+STM32以SPI方式进行交互的初始化代码如下：

UINT8 HW_Init(void)
{
    UINT16 intMask;

	/*initialize the led and switch port*/
	GPIO_init();
	
	/* initialize the SSP registers for the ESC SPI */
	SPI1_Init();
	
    /*initialize ADC configration*/
	Adc_Init();

    do
    {
        intMask = 0x93;
        HW_EscWriteWord(intMask, ESC_AL_EVENTMASK_OFFSET);
        intMask = 0;
        HW_EscReadWord(intMask, ESC_AL_EVENTMASK_OFFSET);
    } while (intMask != 0x93);

    intMask = 0x00;
    HW_EscWriteDWord(intMask, ESC_AL_EVENTMASK_OFFSET);

#if AL_EVENT_ENABLED
    INIT_ESC_INT;
    ENABLE_ESC_INT();
#endif

#if DC_SUPPORTED&& _STM32_IO8
    INIT_SYNC0_INT
    INIT_SYNC1_INT

    ENABLE_SYNC0_INT;
    ENABLE_SYNC1_INT;
#endif

    INIT_ECAT_TIMER;
    START_ECAT_TIMER;

#if INTERRUPTS_SUPPORTED
    /* enable all interrupts */
    ENABLE_GLOBAL_INT;
#endif

    return 0;
}

初始化GPIO、SPI1、ADC等外设，而后读写AL寄存器，这里需注意代码中对AL配置寄存器（0x204）写0x93没有特殊意义，仅为验证MCU与ESC通信是否正常，写入0x93再读出，判断与写入的值是否匹配。而后再将AL配置寄存器清0复位。再往下为从站需要的四个中断，分别为PDI、SYNC0、SYNC1、1ms定时器中断，其中1ms定时器中断不是必须的，若没有定时器可以在主循环中读取MCU的Tick或其他方式来达到该效果。

AX58100+STM32以FSMC通信初始化代码如下，与上述SPI通信方式最大不同就是交互接口初始化的不同。

UINT16 HW_Init(void)
{
    /* the memory interface to the ESC, the ESC-interrupt and the ECAT-timer for the
       watchdog monitoring should be initialized here microcontroller specific*/
    /* initialize ESC DPRAM pointer microcontroller specific to the beginning of the physical memory of the ESC,
       the macro MAKE_PTR_TO_ESC should be defined in mcihw.h */
	//PDI接口初始化
	SRAM_Init();
	//TIMER初始化
	TIM_Configuration(10);	//1ms--timer9
	//ADC接口初始化
	ADC_Configuration();
    
	//PDI读写测试
	do
    {
        intMask = 0x0093;
        HW_EscWriteDWord(intMask, ESC_AL_EVENTMASK_OFFSET);
        intMask = 0;
		HW_EscReadDWord(intMask, ESC_AL_EVENTMASK_OFFSET);
    } while(intMask != 0x0093);// PDI接口测试

	//IRQ中断初始化
    INIT_ESC_INT;/* initialize the PDI - interrupt source*/
    /* initialize the AL_Event Mask Register */
    /* the AL Event-Mask register is initialized with 0, so that no ESC interrupt is generated yet,
       the AL Event-Mask register will be adapted in the function StartInputHandler in ecatslv.c
        when the state transition from PREOP to SAFEOP is made */
    HW_EscWriteWord(intMask, ESC_AL_EVENTMASK_OFFSET);

    /* enable the ESC-interrupt microcontroller specific,
        the macro ENABLE_ESC_INT should be defined in ecat_def.h */
    ENABLE_ESC_INT();

	//SYNC0&&SYNC1中断初始化
    INIT_SYNC0_INT;
    INIT_SYNC1_INT;
    ENABLE_SYNC0_INT;
    ENABLE_SYNC1_INT;
	INIT_ECAT_TIMER;
    START_ECAT_TIMER;
    return 0;
}

瑞萨RZT1芯片初始化代码如下，分别初始化了必要的外设，包括ESC所需的PDI、SYNC0、SYNC1中断。

/*******************************************************************************
* Outline      : main processing
* Function Name: main
* Description  : SHM sample program of RZ/T1.
* Arguments    : none
* Return Value : none
*******************************************************************************/
int main (void)
{
	// --- Initialize board depend ---
	board_init();

    /* Initialize the port function */
    port_init();

    /* Initialize the ECM function */
    ecm_init();

   /* Initialize the ICU settings */
    icu_init();

    cmt_standby();

    /* ---- CMT Setting ---- */
    /* Initialize CMT */
    R_CMT_Init(CMT_CH_0, CMT_CKS_DIVISION_512);
    R_CMT_Init(CMT_CH_1, CMT_CKS_DIVISION_512);

    /* Start CMT */
    R_CMT_CreatePeriodic(CMT_CH_0, 20, ledout);
    R_CMT_CreatePeriodic(CMT_CH_1, 10, inputinc);

	/*----------------------------------------------------------------------*/
	/* setup EtherCAT														*/
	/*----------------------------------------------------------------------*/
	// --- Initialize EtherCAT ---
	EtherCAT_init();

	/* set esc register base address */
	pEsc = (MEM_ADDR ESCMEM *) ESC_BASE;

	/* initialize the EtherCAT slave interface */
	HW_Init();

    ......
}

RZT1平台初始化ESC芯片相关代码如下，与AX58100不同，没有集成PHY芯片，因而还需为ESC功能初始化PHY芯片，另外还包括Link引脚状态，PHY地址，EEPROM大小，发送时钟延迟等。

void EtherCAT_init(void)
{
    volatile uint32_t dummy32;
	volatile uint16_t tmp16;

	// ====== Unlock ethernet system register ======
	ETHERC.ETSPCMD.LONG			= 0x000000A5;
	ETHERC.ETSPCMD.LONG			= 0x00000001;
	ETHERC.ETSPCMD.LONG			= 0x0000FFFE;
	ETHERC.ETSPCMD.LONG			= 0x00000001;

	//---------------------------------
	// Initialize PHY Link
	//---------------------------------
	ETHERSW.ETHPHYLNK.LONG = 0x0000000C;			// PHYLINK0 Active Level : Low
													// PHYLINK1 Active Level : Low
	//---------------------------------
	// Initialize EtherCAT
	//---------------------------------
	// --- PHY address offset ---
	ECATC.CATOFFADD.LONG = 0x000000001;				// address offset : 1

	// --- EEPROM size ---
	ECATC.CATEMMD.LONG = 0x000000000;				// EEPROM  0:lower 16Kbit, 1:upper 32Kbit

	// --- TXC delay ---
	ECATC.CATTXCSFT.LONG = 0x000000000;				// [1:0] ETH0_TXC delay 00:0ns, 01:10ns, 10:20ns, 11:30ns
													// [3:2] ETH1_TXC delay 00:0ns, 01:10ns, 10:20ns, 11:30ns

	// ====== Lock ethernet system register ======
	ETHERC.ETSPCMD.LONG = 0x00000000;

	//=========================================
	// Module stop control
	//=========================================
	// ====== Unlock system register ======
	SYSTEM.PRCR.LONG			= 0x0000A502;					// Enable MSTPCRB write access

	//------------------------------------
	// Release EtherCAT, PHY Clock module
	//------------------------------------
	SYSTEM.MSTPCRB.LONG			&= ~(0x00088000);				// Module start
																//  [15] EtherCAT
																//  [19] CLKOUT25Mn
	dummy32 = SYSTEM.MSTPCRB.LONG;

	// ====== Lock MPC register ======
	SYSTEM.PRCR.LONG = 0x0000A500;								// Disable MSTPCRB write access

	// ====== Unlock ethernet system register ======
	ETHERC.ETSPCMD.LONG			= 0x000000A5;
	ETHERC.ETSPCMD.LONG			= 0x00000001;
	ETHERC.ETSPCMD.LONG			= 0x0000FFFE;
	ETHERC.ETSPCMD.LONG			= 0x00000001;

	//=========================================
	// Initialize MACSEL
	//=========================================
	ETHERC.MACSEL.LONG 			= 0x00000001;					// Port 0 : EtherCAT
																// Port 1 : EtherCAT
																// Port 2 : EtherMAC (w/o Switch)
	ETHERC.EMACRST.LONG   = 0x00000001;				// EtherMAC
	//=========================================
	// Initialize MII converter
	//=========================================
	ETHERC.MII_CTRL0.LONG		= 0x00000100;					// MII full
	ETHERC.MII_CTRL1.LONG		= 0x00000100;					// MII full
	ETHERC.MII_CTRL2.LONG		= 0x00000100;					// MII full

	//=========================================
	// EtherCAT Release reset
	//=========================================
	ETHERC.ETHSFTRST.LONG 		= 0x00000001;		// [0] EtherCAT

    // Wait Loading ESC EEPROM 
	while(1){
		if(ECATC.ESC_DL_STATUS.BIT.PDIOPE == 1)
			break;									// Loading successful, PDI operations
		tmp16 = ECATC.EEP_CONT_STAT.WORD & 0x2800;
		if(tmp16 == 0x0800)							// ACMDERR,CKSUMERR = 01
		   break;									// EEPROM is loaded, but it is blank
		if(tmp16 == 0x2800)							// ACMDERR,CKSUMERR = 11
		   break;									// I2C bus error
	}
	//=========================================
	// PHY Release reset
	//=========================================
	ETHERC.ETHSFTRST.LONG 		= 0x0000001D;					// [0] EtherCAT
																// [2] PHYRESETOUT
																// [3] PHYRESETOUT2
																// [4] MII Converter
	// ====== Lock ethernet system register ======
	ETHERC.ETSPCMD.LONG			= 0x00000000;

	// Wait after reset
	ether_wait_ns(PHY_RESET_WAIT_TIME);

	//---------------------------------
	// Change PHY KSZ8041TL setting
	//---------------------------------
	//
	//  Deafult LED0 pin status does not match RZ/T1 PHYLINK specification.
	//
	ether_write_phyreg(0, 0x1E, 0x4000);			// LED mode = [01]
	ether_write_phyreg(1, 0x1E, 0x4000);			// LED0 : Link
	//
	//  To satisfy EthreCAT requirement, the PHYs must not modify the preamble length.
	//
	ether_write_phyreg(0, 0x14, 0x0080);			// MII 100Mbps Preamble Restore mode
	ether_write_phyreg(1, 0x14, 0x0080);
}

RZT1平台HW代码给出的初始化代码复位了AL配置寄存器，

UINT16 HW_Init(void)
{
	volatile UINT32 dummy32;

    /* the memory interface to the ESC, the ESC-interrupt and the ECAT-timer for the
       watchdog monitoring should be initialized here microcontroller specific*/
    /* initialize ESC DPRAM pointer microcontroller specific to the beginning of the physical memory of the ESC,
       the macro MAKE_PTR_TO_ESC should be defined in renesashw.h */
    //TODO: pEsc = MAKE_PTR_TO_ESC;

    /* initialize the AL_Event Mask Register */
	ResetALEventMask( 0 );

	//=========================================
	// Module stop control
	//=========================================
	// ====== Unlock system register ======
	SYSTEM.PRCR.LONG = 0x0000A502;					// Enable MSTPCRA write access

	SYSTEM.MSTPCRA.LONG &= ~(0x00000004);			// Module start
													//  [2] CMT unit 2
	dummy32 = SYSTEM.MSTPCRA.LONG;

	// ====== Lock MPC register ======
	SYSTEM.PRCR.LONG = 0x0000A500;					// Disable MSTPCRA write access

	// --- Initialize timer ---
	CMT5.CMCR.BIT.CKS   = 0;					// CLK <= PCLKD/8
	CMT5.CMCR.BIT.CMIE  = 0;					// Disable interrupt
	CMT5.CMCOR          = 0xffff;				// Compare value = 0
	CMT.CMSTR2.BIT.STR5 = 1;					// Count start [CMT5]

    return 0;
}

2.2 ESC通信接口初始化

ESC与MCU进行通信的接口称为过程数据接口（Process Data Interface）或物理设备接口（Physical Device Interface），简写为PDI，MCU以此接口来访问ESC的寄存器空间。常见的有SPI，8位或16位同步、异步并口，对于集成ESC的MCU还可直接在片上寻址来完成与ESC交互。PDI接口的配置由ESC的0x140寄存器配置，该寄存器的值在ESC上电时装载EEPROM的值。

这里列举AX58100的0x140-0x141寄存器的定义：
在这里插入图片描述

EEPROM前8个字的定义如下：

字地址	参数名	描述
0	PDI控制	PDI控制寄存器（0x140:0x141）的初始值
1	PDI配置	PDI配置寄存器（0x150:0x151）的初始值
2	SYNC信号脉冲宽度	SYNC信号脉宽寄存器（0x982:0x983）初始值
3	扩展PDI配置	扩展PDI配置寄存器（0x152:0x153）初始值
4	站点别名	站点别名配置寄存器（0x12:0x13）初始值
5,6	保留	0
7	校验和	字0~6校验和

EEPROM信息，可以用ESI文件的方式记录，然后用TwinCAT软件经由ESC烧进EEPROM，EEPROM的前4个或5个字的配置，在从站ESI文件的ConfigData节点，以AX58100为例，可配置的方式如下：

<ConfigData>050E0344102700000000</ConfigData>

0x140=0x05 PDI通信方式为SPI

0x141=0x0E AL状态由PDI设置，端口0增强链接功能不打开，端口1 2 3增强链接功能打开

0x150=0x03 SPI模式3，PDI中断推挽输出下降沿中断，SPI片选低电平有效，数据输出采样模式非延迟模式

0x151=0x44 SYNC0信号推挽输出下降沿中断，SYNC0输出，SYNC1信号推挽输出下降沿中断，SYNC1输出
在这里插入图片描述

<ConfigData>080000cc0a000000000000000000</ConfigData>

0x140=0x08 PDI通信方式为16位异步并口

0x141=0x00 AL状态由PDI设置，端口增强链接功能不打开

0x150=0x00 意义见58100手册

0x151=0xCC SYNC0和SYNC1信号分别为推挽输出下降沿中断，信号输出，中断映射到AL时间请求寄存器

对于瑞萨RZT1芯片，该配置为

<ConfigData>800E46EE10000000</ConfigData>

0x140=0x80 PDI通信方式为片上总线

0x141=0x0E AL状态由PDI设置，端口0增强链接功能不打开，端口1 2 3增强链接功能打开

0x150=0x46 意义见手册

0x151=0xEE SYNC信号极性为推挽输出，上升沿有效，信号输出，中断映射到AL事件请求寄存器
在这里插入图片描述

2.3 SPI通信格式

并口或片上集成ESC两种类型的PDI通信方式，最终在代码形式上都可以转化为在MCU片上寻址的方式操作ESC寄存器空间，而SPI通信不同，存在通信协议，经查阅，ET1200和AX58100几乎相同，该协议应该也是倍福规定的标准协议，所有ESC应该均类似。该协议如下：
在这里插入图片描述

SPI通信的有2字节地址和3字节地址模式两种，发起请求先发起命令及地址请求，然后传输数据，以2字节模式为例，读请求先发命令和地址，而后每给ESC发一个非0xFF的数据，ESC返回一个地址的数据，地址并自增，发0xFF为结束，写请求同读请求类似，发完地址和写命令后，每写一个数据写入ESC，写请求以取消片选信号为结束标志。

SPI访问ESC代码如下：

static void AddressingEsc(UINT16 Address, UINT8 Command)
{
    UBYTETOWORD tmp;
    VARVOLATILE UINT8 dummy;
    tmp.Word = (Address << 3) | Command;
    /* select the SPI */
    SELECT_SPI;

    /* there have to be at least 15 ns after the SPI1_SEL signal was active (0) before
       the transmission shall be started */
    /* send the first address/command byte to the ESC */	
    dummy = WR_CMD(tmp.Byte[1]);
	
    /* send the second address/command byte to the ESC */
    dummy =	WR_CMD(tmp.Byte[0]);

    /* if the SPI transmission rate is higher than 15 MBaud, the Busy detection shall be
       done here */
}
static void ISR_AddressingEsc(UINT16 Address, UINT8 Command)
{
    VARVOLATILE UINT8 dummy;
    UBYTETOWORD tmp;
    tmp.Word = (Address << 3) | Command;

    /* select the SPI */
    SELECT_SPI;

    /* there have to be at least 15 ns after the SPI1_SEL signal was active (0) before
       the transmission shall be started */
	
    /* send the first address/command byte to the ESC */
    dummy= WR_CMD(tmp.Byte[1]);
	
    /* send the second address/command byte to the ESC */

    dummy= WR_CMD(tmp.Byte[0]);
    /* if the SPI transmission rate is higher than 15 MBaud, the Busy detection shall be
       done here */
}
void HW_EscRead(MEM_ADDR *pData, UINT16 Address, UINT16 Len)
{
    /* HBu 24.01.06: if the SPI will be read by an interrupt routine too the
                     mailbox reading may be interrupted but an interrupted
                     reading will remain in a SPI transmission fault that will
                     reset the internal Sync Manager status. Therefore the reading
                     will be divided in 1-byte reads with disabled interrupt */
    UINT16 i = Len;
    UINT8 *pTmpData = (UINT8 *)pData;

    /* loop for all bytes to be read */
    while (i-- > 0)
    {
#if AL_EVENT_ENABLED
        /* the reading of data from the ESC can be interrupted by the
           AL Event ISR, in that case the address has to be reinitialized,
           in that case the status flag will indicate an error because
           the reading operation was interrupted without setting the last
           sent byte to 0xFF */
        DISABLE_AL_EVENT_INT;
#endif
        AddressingEsc(Address, ESC_RD);

        /* when reading the last byte the DI pin shall be 1 */
        *pTmpData++ = WR_CMD(0xFF);
        /* enable the ESC interrupt to get the AL Event ISR the chance to interrupt,
           if the next byte is the last the transmission shall not be interrupted,
           otherwise a sync manager could unlock the buffer, because the last was
           read internally */
#if AL_EVENT_ENABLED
        ENABLE_AL_EVENT_INT;
#endif
        /* there has to be at least 15 ns + CLK/2 after the transmission is finished
           before the SPI1_SEL signal shall be 1 */
        DESELECT_SPI;
        /* next address */
        Address++;
        /* reset transmission flag */
        //SPI1_IF = 0;
    }
}
void HW_EscReadIsr(MEM_ADDR *pData, UINT16 Address, UINT16 Len)
{
    UINT16 i = Len;
    UINT8 data = 0;

    UINT8 *pTmpData = (UINT8 *)pData;

    /* send the address and command to the ESC */
    ISR_AddressingEsc(Address, ESC_RD);
    /* loop for all bytes to be read */
    while (i-- > 0)
    {
        if (i == 0)
        {
            /* when reading the last byte the DI pin shall be 1 */
            data = 0xFF;
        }

		*pTmpData++ = WR_CMD(data);
    }

    /* there has to be at least 15 ns + CLK/2 after the transmission is finished
       before the SPI1_SEL signal shall be 1 */
    DESELECT_SPI;
}
void HW_EscWrite(MEM_ADDR *pData, UINT16 Address, UINT16 Len)
{
    UINT16 i = Len;
    VARVOLATILE UINT8 dummy;

    UINT8 *pTmpData = (UINT8 *)pData;

    /* loop for all bytes to be written */
    while (i-- > 0)
    {
#if AL_EVENT_ENABLED
        /* the reading of data from the ESC can be interrupted by the
           AL Event ISR, so every byte will be written separate */
        DISABLE_AL_EVENT_INT;
#endif
        /* HBu 24.01.06: wrong parameter ESC_RD */
        AddressingEsc(Address, ESC_WR);

        /* enable the ESC interrupt to get the AL Event ISR the chance to interrupt */
        /* SPI1_BUF must be read, otherwise the module will not transfer the next received data from SPIxSR to SPIxRXB.*/
        dummy= WR_CMD(*pTmpData++);
			
#if AL_EVENT_ENABLED
        ENABLE_AL_EVENT_INT;
#endif
        DESELECT_SPI;
        /* next address */
        Address++;
    }
}
void HW_EscWriteIsr(MEM_ADDR *pData, UINT16 Address, UINT16 Len)
{
    UINT16 i = Len;
    VARVOLATILE UINT16 dummy;
    UINT8 *pTmpData = (UINT8 *)pData;

    /* send the address and command to the ESC */
    ISR_AddressingEsc( Address, ESC_WR );
    /* loop for all bytes to be written */
    while (i-- > 0)
    {
        /* start transmission */
        dummy= WR_CMD(*pTmpData);
        /* increment data pointer */
        pTmpData++;
    }

    /* there has to be at least 15 ns + CLK/2 after the transmission is finished
       before the SPI1_SEL signal shall be 1 */
    DESELECT_SPI;
}

这里接口带ISR的为在中断服务中程序中调用的，与ISR的接口不同在于操作时没有关闭芯片的中断，而对于片上寻址与ESC交互的方式，这两种接口无异。

2.4 主循环逻辑分析

根据SSC文档ET9300，代码逻辑如下，Hw_Init为硬件相关的初始化，MainInit为初始化从站协议栈的全局变量，而后循环执行MainLoop接口。
在这里插入图片描述

int main(void)
{
    /* initialize the Hardware and the EtherCAT Slave Controller */
    HW_Init();

    MainInit();

    /*Initialize Axes structures*/
    CiA402_Init();

    /*Create basic mapping*/
    APPL_GenerateMapping(&nPdInputSize, &nPdOutputSize);
    bRunApplication = TRUE;
    do
    {
        MainLoop();
    } while (bRunApplication == TRUE);

    CiA402_DeallocateAxis();

    HW_Release();
    return 0;
}

MainLoop接口为协议栈及Freerun模式下的主循环，其中第10行if条件，只在freerun模式下满足，主要执行该模式下的AL事件检查，当检查到有主站输出到从站的PDO事件时，调用PDO_OutputMapping获取主站下发的数据，而后执行ECAT_Application，根据PDO数据执行相关操作（如控制电机、IO等），而后调用PDO_InputMapping将要返回给主站的PDO数据写到ESC缓冲区。

第58行，如上所属，若ESC代码Hw_Init接口没有开启1ms定时器任务，则需在该循环中，让ECAT_CheckTimer接口每1ms执行一次，该接口主要是检查从站状态z状态切换是否超时，以及过程数据看门狗功能相关数据。

第71行，ECAT_Main接口主要是处理AL状态的状态机，即控制ESC状态切换，还有邮箱通信功能。

第74行，COE_Main接口时对CANopen协议下，SDO通信相关的处理。

第79行，CiA402_StateMachine时402状态机，主要是围绕0x6040控制字和0x6041状态字的状态机。

上述三个接口理论上应该是不需要修改的。
在这里插入图片描述

void MainLoop(void)
{
    /*return if initialization not finished */
    if (bInitFinished == FALSE)
        return;

    /* FreeRun-Mode:  bEscIntEnabled = FALSE, bDcSyncActive = FALSE
       Synchron-Mode: bEscIntEnabled = TRUE, bDcSyncActive = FALSE
       DC-Mode:       bEscIntEnabled = TRUE, bDcSyncActive = TRUE */
    if (
        (!bEscIntEnabled || !bEcatFirstOutputsReceived) &&     /* SM-Synchronous, but not SM-event received */
         !bDcSyncActive                                        /* DC-Synchronous */
        )
    {
        /* if the application is running in ECAT Synchron Mode the function ECAT_Application is called
           from the ESC interrupt routine (in mcihw.c or spihw.c),
           in ECAT Synchron Mode it should be additionally checked, if the SM-event is received
           at least once (bEcatFirstOutputsReceived = 1), otherwise no interrupt is generated
           and the function ECAT_Application has to be called here (with interrupts disabled,
           because the SM-event could be generated while executing ECAT_Application) */
        if (!bEscIntEnabled)
        {
            /* application is running in ECAT FreeRun Mode,
               first we have to check, if outputs were received */
            UINT16 ALEvent = HW_GetALEventRegister();
            ALEvent = SWAPWORD(ALEvent);
            if (ALEvent & PROCESS_OUTPUT_EVENT)
            {
                /* set the flag for the state machine behaviour */
                bEcatFirstOutputsReceived = TRUE;
                if (bEcatOutputUpdateRunning)
                {
                    /* update the outputs */
                    PDO_OutputMapping();
                }
            }
            else if (nPdOutputSize == 0)
            {
                /* if no outputs are transmitted, the watchdog must be reset, when the inputs were read */
                if (ALEvent & PROCESS_INPUT_EVENT)
                {
                    /* Outputs were updated, set flag for watchdog monitoring */
                    bEcatFirstOutputsReceived = TRUE;
                }
            }
        }

        DISABLE_ESC_INT();
        ECAT_Application();

        if (bEcatInputUpdateRunning)
        {
            /* EtherCAT slave is at least in SAFE-OPERATIONAL, update inputs */
            PDO_InputMapping();
        }
        ENABLE_ESC_INT();
    }
#if !ECAT_TIMER_INT
    /* there is no interrupt routine for the hardware timer so check the timer register if the desired cycle elapsed */
    {
        UINT32 CurTimer = (UINT32)HW_GetTimer();

        if (CurTimer >= ECAT_TIMER_INC_P_MS)
        {
            ECAT_CheckTimer();
            HW_ClearTimer();
        }
    }
#endif
    /* call EtherCAT functions */
    ECAT_Main();

    /* call lower prior application part */
    COE_Main();
    CheckIfEcatError();

	if (bEcatInputUpdateRunning)
    {
        CiA402_StateMachine();
    }
}

如下代码为瑞萨RZT1有关1ms定时相关代码，不使用定时器中断回调的情况下，使用其CMT计数器来模拟，而在ECAT_CheckTimer函数中，在bEcatWaitForAlControlRes为真时表示有状态切换，对EsmTimeoutCounter可视为状态切换超时时间。DC_CheckWatchdog接口主要是对PDO通信进行检查，在从safeop到op进行切换时，i16WaitForPllRunningCnt累积到一定值后，才可控制状态进入OP，而从站每次收到PDO后，会将Sync0WdCounter看门狗计数置为0，这里当看门狗计数超过规定值Sync0WdValue时，bSmSyncSequenceValid被改为False，则ECAT_Main状态机会将从站状态掉回SafeOP，并报0x1A丢同步错误。

/**
\return     Current timer value

 \brief    This function gets the value of the hardware timer. The timer ticks per ms shall be set in "ECAT_TIMER_INC_P_MS"
*////////////////////////////////////////////////////////////////////////////////////////
UINT16 HW_GetTimer()
{
  return (UINT16)(CMT5.CMCNT - BaseTime);
}

/////////////////////////////////////////////////////////////////////////////////////////
/**
 \brief    Reset the hardware timer
*////////////////////////////////////////////////////////////////////////////////////////
void HW_ClearTimer()
{
	BaseTime = CMT5.CMCNT;
}

void ECAT_CheckTimer(void)
{
    /*decrement the state transition timeout counter*/
    if(bEcatWaitForAlControlRes &&  (EsmTimeoutCounter > 0))
    {
        EsmTimeoutCounter--;
    }

    ECAT_SetLedIndication();

    DC_CheckWatchdog();

    /* Increment the counter every ms between two updates based on the system time (32Bit overrun is handled in COE_SyncTimeStamp) */
    if (!b32BitDc || ((u64Timestamp & 0xFFFFFFFF) <= 4293000000))
    {
        /* the timestamp is stored in ns */
        u64Timestamp = u64Timestamp + 1000000;
    }
    else if(b32BitDc)
    {
        /* in case of a 32Bit DC and almost expired time stamp check for a DC overrun*/
        u32CheckForDcOverrunCnt = CHECK_DC_OVERRUN_IN_MS;
    }

    u32CheckForDcOverrunCnt++;
}

oid DC_CheckWatchdog(void)
{
/*ECATCHANGE_START(V5.12) ECAT5*/
    DISABLE_ESC_INT();
/*ECATCHANGE_END(V5.12) ECAT5*/

    if(bDcSyncActive && bEcatInputUpdateRunning)
    {
        /*If Sync0 watchdog is enabled and expired*/
        if((Sync0WdValue > 0) && (Sync0WdCounter >= Sync0WdValue))
        {
			/*Sync0 watchdog expired*/
			bDcRunning = FALSE;        
        }
        else
        {
            if(Sync0WdCounter < Sync0WdValue)
            {
                Sync0WdCounter++;
            }
            bDcRunning = TRUE;
        }
        if(bDcRunning)
        {
            /*Check the Sync1 cycle if Sync1 Wd is enabled*/
            if(Sync1WdValue > 0)
            {
                if(Sync1WdCounter < Sync1WdValue)
                {
                    Sync1WdCounter++;
                }
                else
                {
                    /*Sync1 watchdog expired*/
                    bDcRunning = FALSE;
                }
            }
        }
        if(bDcRunning)
        {
           if(sSyncManOutPar.u16SmEventMissedCounter < sErrorSettings.u16SyncErrorCounterLimit)
            {
                bSmSyncSequenceValid = TRUE;

                /*Wait for PLL is active increment the Pll valid counter*/
                if (i16WaitForPllRunningTimeout > 0)
                {
                    i16WaitForPllRunningCnt++;
                }
            }
            else if (bSmSyncSequenceValid)
            {
				bSmSyncSequenceValid = FALSE;

                /*Wait for PLL is active reset the Pll valid counter*/
                if (i16WaitForPllRunningTimeout > 0)
                {
                    i16WaitForPllRunningCnt = 0;
                }
            }
        }
        else if(bSmSyncSequenceValid)
        {
           bSmSyncSequenceValid = FALSE;
        }
    }
    /*ECATCHANGE_START(V5.12) ECAT5*/
    ENABLE_ESC_INT();
    /*ECATCHANGE_END(V5.12) ECAT5*/
}

2.5 邮箱通信

在这里插入图片描述

主循环中的ECAT_Main函数，主要功能为状态切换和邮箱通信，邮箱相关的代码如下。

第8行MBX_Main为邮箱通信的主函数，该函数具体定义在如下代码第117行，先从接收到的邮箱数据队列中取出邮箱数据，然后再执行MailboxServiceInd来处理，MailboxServiceInd接口在该代码第148行，可以看到这里暂时只实现了CoE功能，猜测该接口是邮箱应用层功能的最上层接口。

第144行MBX_CheckAndCopyMailbox是检查从站读邮箱是否有待读数据，其中bReceiveMbxIsLocked条件也是在该接口中被设置为TRUE的，该变量被置为TRUE条件主要有两个，一是从站的发送邮箱满或有未处理的数据，二是ESC已经没有足够的空间来存储邮箱数据，考虑到主站发起邮箱通信，一般是阻塞通信，即邮箱不反馈，不再发起下一个邮箱请求，因而猜测该接口在这里一般是不执行的。

第10行读SM1从站发送邮箱的SM激活状态，供代码第62行使用。

第58行的if语句里，也是邮箱通信相关的，bMbxRunning变量，会在从站切换到Preop状态后置为TRUE，回到Init状态后变为FALSE，也就是标准里规定的邮箱通信只在Preop以上的状态才可使用。

第62行检查0x80E.0，若该位为0表示SM1邮箱通道没有被激活，这里是需要报错的，执行AL_ControlInd接口里边在执行CheckSmSettings接口检查邮箱状态，报相应的错误。

第64行检查0x220.5，判断是否有主站读邮箱SM事件产生，有事件产生后，先给发送邮箱缓冲区首地址写入一个值，锁定邮箱，实际上是让发送缓冲区变为一个未满的状态，此时0x80D.3变为0，主站轮询到该位置不为0，则会认为从站发送邮箱未满，不会去读邮箱。检测到从站读邮箱后，执行MBX_MailboxReadInd检查队列是否还有未发送给从站的邮箱数据，有则调用MBX_CopyToSendMailbox将数据放到发送邮箱。

第82行的邮箱通信暂未研究，待进一步研究。

第104行判断是否有主站写从站接收邮箱（SM0）的事件，若有该事件，调用MBX_CheckAndCopyMailbox将邮箱数据放到接收队列，供代码第8行MBX_Main函数处理，或没有足够的空间，或发送邮箱有未处理完的数据，这两种条件下，从站接收邮箱被锁定。

void ECAT_Main(void)
{
    UINT16 ALEventReg;
    UINT16 EscAlControl = 0x0000;
    UINT16 sm1Activate = SM_SETTING_ENABLE_VALUE;

    /* check if services are stored in the mailbox */
    MBX_Main();

    if (bMbxRunning)
    {
        /* Slave is at least in PREOP, Mailbox is running */
        /* get the Activate-Byte of SM 1 (Register 0x80E) to check if a mailbox repeat request was received */
        HW_EscReadWord(sm1Activate, (ESC_SYNCMAN_ACTIVE_OFFSET + SIZEOF_SM_REGISTER));
        sm1Activate = SWAPWORD(sm1Activate);
    }

    /* Read AL Event-Register from ESC */
    ALEventReg = HW_GetALEventRegister();
    ALEventReg = SWAPWORD(ALEventReg);

    if ((ALEventReg & AL_CONTROL_EVENT) && !bEcatWaitForAlControlRes)
    {
        /* AL Control event is set, get the AL Control register sent by the Master to acknowledge the event
          (that the corresponding bit in the AL Event register will be reset) */
        HW_EscReadWord(EscAlControl, ESC_AL_CONTROL_OFFSET);
        EscAlControl = SWAPWORD(EscAlControl);

        /* reset AL Control event and the SM Change event (because the Sync Manager settings will be checked
           in AL_ControlInd, too)*/
        ALEventReg &= ~((AL_CONTROL_EVENT) | (SM_CHANGE_EVENT));

        AL_ControlInd((UINT8)EscAlControl, 0); /* in AL_ControlInd the state transition will be checked and done */

        /* SM-Change-Event was handled too */
    }

    if ((ALEventReg & SM_CHANGE_EVENT) && !bEcatWaitForAlControlRes && (nAlStatus & STATE_CHANGE) == 0 && (nAlStatus & ~STATE_CHANGE) != STATE_INIT)
    {
        /* the SM Change event is set (Bit 4 of Register 0x220), when the Byte 6 (Enable, Lo-Byte of Register 0x806, 0x80E, 0x816,...)
           of a Sync Manager channel was written */
        ALEventReg &= ~(SM_CHANGE_EVENT);

        /* AL_ControlInd is called with the actual state, so that the correct SM settings will be checked */
        AL_ControlInd(nAlStatus & STATE_MASK, 0);
    }

    if (bEcatWaitForAlControlRes)
    {
        AL_ControlRes();
    }
    /*The order of mailbox event processing was changed to prevent race condition errors.
        The SM1 activate Byte (Register 0x80E) was read before reading AL Event register.
        1. Handle Mailbox Read event
        2. Handle repeat toggle request
        3. Handle Mailbox write event
    */
    if (bMbxRunning)
    {
        /*SnycManger change event (0x220:4) could be acknowledged by reading the SM1 control register without notification to the local application
        => check if the SyncManger 1 is still enabled*/
        if (!(sm1Activate & SM_SETTING_ENABLE_VALUE))
            AL_ControlInd(nAlStatus & STATE_MASK, 0);

        if (ALEventReg & (MAILBOX_READ_EVENT))
        {
            /* SM 1 (Mailbox Read) event is set, when the mailbox was read from the master,
               to acknowledge the event the first byte of the mailbox has to be written,
               by writing the first byte the mailbox is locked, too */
            u16dummy = 0;
            HW_EscWriteWord(u16dummy, u16EscAddrSendMbx);

            /* the Mailbox Read event in the variable ALEventReg shall be reset before calling
               MBX_MailboxReadInd, where a new mailbox datagram (if available) could be stored in the send mailbox */
            ALEventReg &= ~(MAILBOX_READ_EVENT);
            MBX_MailboxReadInd();
        }

        DISABLE_MBX_INT;
        /* bMbxRepeatToggle holds the last state of the Repeat Bit (Bit 1) */

        if (((sm1Activate & SM_SETTING_REPAET_REQ_MASK) && !bMbxRepeatToggle)
            ||(!(sm1Activate & SM_SETTING_REPAET_REQ_MASK) && bMbxRepeatToggle))
        {
            /* Repeat Bit (Bit 1) has toggled, there is a repeat request, in MBX_MailboxRepeatReq the correct
               response will put in the send mailbox again */
            MBX_MailboxRepeatReq();
            /* acknowledge the repeat request after the send mailbox was updated by writing the Repeat Bit
               in the Repeat Ack Bit (Bit 1) of the PDI Ctrl-Byte of SM 1 (Register 0x80F) */
            if (bMbxRepeatToggle)
                sm1Activate |= SM_SETTING_REPEAT_ACK; //set repeat acknowledge bit (bit 9)
            else
                sm1Activate &= ~SM_SETTING_REPEAT_ACK; //clear repeat acknowledge bit (bit 9)

            sm1Activate = SWAPWORD(sm1Activate);
            HW_EscWriteWord(sm1Activate, (ESC_SYNCMAN_ACTIVE_OFFSET + SIZEOF_SM_REGISTER));
        }
        ENABLE_MBX_INT;

        /* Reload the AlEvent because it may be changed due to a SM disable, enable in case of an repeat request */
        ALEventReg = HW_GetALEventRegister();
        ALEventReg = SWAPWORD(ALEventReg);

        if (ALEventReg & (MAILBOX_WRITE_EVENT))
        {
            /* SM 0 (Mailbox Write) event is set, when the mailbox was written from the master,
               to acknowledge the event the first byte of the mailbox has to be read,
               which will be done in MBX_CheckAndCopyMailbox */
            /* the Mailbox Write event in the variable ALEventReg shall be reset before calling
               MBX_CheckAndCopyMailbox, where the received mailbox datagram will be processed */
            ALEventReg &= ~(MAILBOX_WRITE_EVENT);
            MBX_CheckAndCopyMailbox();
        }
    }
}

void MBX_Main(void)
{
    TMBX MBXMEM *pMbx = NULL;

    do
    {
        UINT8 result = 0;

        pMbx = GetOutOfMbxQueue(&sMbxReceiveQueue);
        if (pMbx)
            result = MailboxServiceInd(pMbx);

        if (result != 0)
        {
            /* Mailbox error response: type 0 (mailbox service protocol) */
            pMbx->MbxHeader.Length = 4;
            pMbx->MbxHeader.Flags[MBX_OFFS_TYPE] &= ~(MBX_MASK_TYPE);
            pMbx->Data[0] = SWAPWORD(MBXSERVICE_MBXERRORCMD);
            pMbx->Data[1] = SWAPWORD(result);
            MBX_MailboxSendReq(pMbx, 0);
        }
    } while (pMbx != NULL);

    if (bReceiveMbxIsLocked)
    {
        /* the work on the receive mailbox is locked, check if it can be unlocked (if all
           mailbox commands has been sent */
        MBX_CheckAndCopyMailbox();
    }
}

UINT8 MailboxServiceInd(TMBX MBXMEM *pMbx)
{
    UINT8 result;

    switch ((pMbx->MbxHeader.Flags[MBX_OFFS_TYPE] & MBX_MASK_TYPE) >> MBX_SHIFT_TYPE)
    {
    case MBX_TYPE_COE:
        /* CoE datagram received */
        result = COE_ServiceInd((TCOEMBX MBXMEM *)pMbx);
        break;
    default:
        result = MBXERR_UNSUPPORTEDPROTOCOL;
        break;
    }

    return result;
}

2.6 AL状态管理

AL状态管理主要是处理主站发起的AL状态切换请求，并在相应的状态检查相关配置信息及其他控制状态切换的条件，状态管理的代码主要在ECAT_Main接口中，相关代码如下，代码去除了邮箱通信相关的部分。

void ECAT_Main(void)
{
    UINT16 ALEventReg;
    UINT16 EscAlControl = 0x0000;
    UINT16 sm1Activate = SM_SETTING_ENABLE_VALUE;

	...

    /* Read AL Event-Register from ESC */
    ALEventReg = HW_GetALEventRegister();
    ALEventReg = SWAPWORD(ALEventReg);

    if ((ALEventReg & AL_CONTROL_EVENT) && !bEcatWaitForAlControlRes)
    {
        /* AL Control event is set, get the AL Control register sent by the Master to acknowledge the event
          (that the corresponding bit in the AL Event register will be reset) */
        HW_EscReadWord(EscAlControl, ESC_AL_CONTROL_OFFSET);
        EscAlControl = SWAPWORD(EscAlControl);

        /* reset AL Control event and the SM Change event (because the Sync Manager settings will be checked
           in AL_ControlInd, too)*/
        ALEventReg &= ~((AL_CONTROL_EVENT) | (SM_CHANGE_EVENT));

        AL_ControlInd((UINT8)EscAlControl, 0); /* in AL_ControlInd the state transition will be checked and done */

        /* SM-Change-Event was handled too */
    }

    if ((ALEventReg & SM_CHANGE_EVENT) && !bEcatWaitForAlControlRes && (nAlStatus & STATE_CHANGE) == 0 && (nAlStatus & ~STATE_CHANGE) != STATE_INIT)
    {
        /* the SM Change event is set (Bit 4 of Register 0x220), when the Byte 6 (Enable, Lo-Byte of Register 0x806, 0x80E, 0x816,...)
           of a Sync Manager channel was written */
        ALEventReg &= ~(SM_CHANGE_EVENT);

        /* AL_ControlInd is called with the actual state, so that the correct SM settings will be checked */
        AL_ControlInd(nAlStatus & STATE_MASK, 0);
    }

    if (bEcatWaitForAlControlRes)
    {
        AL_ControlRes();
    }

    ...
}

第13行检查AL事件寄存器，若主站写0x120寄存器发起状态请求，则该标志（0x220.0）被置位，第18行读出主站请求的状态，执行AL_ControlInd接口，执行状态切换。

AL_ControlInd接口的主要功能是控制状态切换及检查当前状态的配置条件，代码如下，这里只简单分析状态切换的逻辑。

第17行表示，若当前的状态nAlStatus的bit4为1，则表示从站有错误没有被主站确认（未下发写0x120.4为1）的Ack错误报文，该情况下，除请求Init状态，请求其他状态均不响应。

切换Preop：第49行，检查邮箱SM通道配置是否正确，第89行检查EEPROM是否被正确加载，若成功加载则开启邮箱通信，第193、第205行若从SafeOP及OP状态切换到Preop，则关闭过程数据通信。

切换SafeOP：第51行若从Preop切换到SafeOP，则根据配置的PDO信息，计算过程数据长度及生成各PDO对象字典到过程数据的映射信息，以便在过程数据通信阶段获取PDO数据，第70行若从OP切换到SafeOP，或不存在状态转换，检查SafeOP当前状态，则检查所有SM通道配置是否正确。第183行若从OP切换到SafeOP，停止获取主站下发的PDO数据。

切换OP：第159行当从SafeOP切换到OP时，开启过程数据接收，主要将bEcatOutputUpdateRunning及i16WaitForPllRunningTimeout置位，第322行将bEcatWaitForAlControlRes置为TRUE，等待切换OP状态，该标志在ECAT_Main函数中的第39行使用，主要等待在1ms定时起接口中看门狗接口DC_CheckWatchdog，及PDI、Sync中断相应中完成相关变量赋值，这里不详细分析。

void AL_ControlInd(UINT8 alControl, UINT16 alStatusCode)
{
    UINT16        result = 0;
    UINT8         bErrAck = 0;
    UINT8         stateTrans;
    /*deactivate ESM timeout counter*/
    EsmTimeoutCounter = -1;
    bApplEsmPending = TRUE;

    /* reset the Error Flag in case of acknowledge by the Master */
    if (alControl & STATE_CHANGE)
    {
        bErrAck = 1;
        nAlStatus &= ~STATE_CHANGE;
        /*enable SM2 is moved to state transition block. First check SM Settings.*/
    }
    else if ((nAlStatus & STATE_CHANGE)
    // HBu 17.04.08: the error has to be acknowledged before when sending the same (or a higher) state
    //               (the error was acknowledged with the same state before independent of the acknowledge flag)
    /*Error Acknowledge with 0xX1 is allowed*/
           && (alControl & STATE_MASK) != STATE_INIT)
        /* the error flag (Bit 4) is set in the AL-Status and the ErrAck bit (Bit 4)
           is not set in the AL-Control, so the state cannot be set to a higher state
           and the new state request will be ignored */
        return;
    else
    {
        nAlStatus &= STATE_MASK;
    }

    /* generate a variable for the state transition
      (Bit 0-3: new state (AL Control), Bit 4-7: old state (AL Status) */
    alControl &= STATE_MASK;
    stateTrans = nAlStatus;
    stateTrans <<= 4;
    stateTrans += alControl;


    /* check the SYNCM settings depending on the state transition */
    switch (stateTrans)
    {
    case INIT_2_PREOP:
    case OP_2_PREOP:
    case SAFEOP_2_PREOP:
    case PREOP_2_PREOP:
        /* in PREOP only the SYNCM settings for SYNCM0 and SYNCM1 (mailbox)
           are checked, if result is unequal 0, the slave will stay in or
           switch to INIT and set the ErrorInd Bit (bit 4) of the AL-Status */
        result = CheckSmSettings(MAILBOX_READ + 1);
        break;
    case PREOP_2_SAFEOP:
        {
        /* before checking the SYNCM settings for SYNCM2 and SYNCM3 (process data)
           the expected length of input data (nPdInputSize) and output data (nPdOutputSize)
            could be adapted (changed by PDO-Assign and/or PDO-Mapping)
            if result is unequal 0, the slave will stay in PREOP and set
            the ErrorInd Bit (bit 4) of the AL-Status */
        result = APPL_GenerateMapping(&nPdInputSize, &nPdOutputSize);

        if (result != 0)
            break;
        }
    case SAFEOP_2_OP:
    case OP_2_SAFEOP:
    case SAFEOP_2_SAFEOP:
    case OP_2_OP:
        /* in SAFEOP or OP the SYNCM settings for all SYNCM are checked
           if result is unequal 0, the slave will stay in or
           switch to PREOP and set the ErrorInd Bit (bit 4) of the AL-Status */
        result = CheckSmSettings(nMaxSyncMan);
        break;
    }

    if (result == 0)
    {
        /* execute the corresponding local management service(s) depending on the state transition */
        nEcatStateTrans = 0;
        switch (stateTrans)
        {
        case INIT_2_BOOT:
            result = ALSTATUSCODE_BOOTNOTSUPP;
            break;

        case BOOT_2_INIT:
            result = ALSTATUSCODE_BOOTNOTSUPP;
            BackToInitTransition();
            break;
        case INIT_2_PREOP:
            UpdateEEPROMLoadedState();
            if (EepromLoaded == FALSE)
            {
                //return an error if the EEPROM was not loaded correct  (device restart is required after the new EEPORM update)
                result = ALSTATUSCODE_EE_ERROR;
            }
            if (result == 0)
            {
                /* MBX_StartMailboxHandler (in mailbox.c) checks if the areas of the mailbox
                sync managers SYNCM0 and SYNCM1 overlap each other
                if result is unequal 0, the slave will stay in INIT
                and sets the ErrorInd Bit (bit 4) of the AL-Status */
                result = MBX_StartMailboxHandler();
                if (result == 0)
                {
                    bApplEsmPending = FALSE;
                    /* additionally there could be an application specific check (in ecatappl.c)
                    if the state transition from INIT to PREOP should be done
                    if result is unequal 0, the slave will stay in INIT
                    and sets the ErrorInd Bit (bit 4) of the AL-Status */
                    result = APPL_StartMailboxHandler();
                    if (result == 0)
                    {
                        bMbxRunning = TRUE;
                    }
                }
    
                if(result != 0 && result != NOERROR_INWORK)
                {
                    /*Stop APPL Mbx handler if APPL Start Mbx handler was called before*/
                    if(!bApplEsmPending)
                        APPL_StopMailboxHandler();
    
                    MBX_StopMailboxHandler();
                }
            }
            break;

        case PREOP_2_SAFEOP:
            /* start the input handler (function is defined above) */
            result = StartInputHandler();
            if (result == 0)
            {
                bApplEsmPending = FALSE;
                result = APPL_StartInputHandler(&u16ALEventMask);

                if (result == 0)
                {
                    /* initialize the AL Event Mask register (0x204) */
/*ECATCHANGE_START(V5.11) HW1*/
                    SetALEventMask(u16ALEventMask);
/*ECATCHANGE_END(V5.11) HW1*/

                    bEcatInputUpdateRunning = TRUE;
                }
            }

            /*if one start input handler returned an error stop the input handler*/
            if (result != 0 && result != NOERROR_INWORK)
            {
                if (!bApplEsmPending)
                {
                    /*Call only the APPL stop handler if the APPL start handler was called before*/
                    /*The application can react to the state transition in the function APPL_StopInputHandler */
                    APPL_StopInputHandler();
                }

                StopInputHandler();
            }
            break;
        case SAFEOP_2_OP:
            /* start the output handler (function is defined above) */
            result = StartOutputHandler();
            if (result == 0)
            {
                bApplEsmPending = FALSE;
                result = APPL_StartOutputHandler();

                if (result == 0)
                {
                    /*Device is in OPERATINAL*/
                    bEcatOutputUpdateRunning = TRUE;
                }

            }

            if (result != 0 && result != NOERROR_INWORK)
            {
                if(!bApplEsmPending)
                    APPL_StopOutputHandler();

                StopOutputHandler();
            }
            break;
        case OP_2_SAFEOP:
            /* stop the output handler (function is defined above) */
            APPL_StopOutputHandler();

            StopOutputHandler();

            bApplEsmPending = FALSE;

            break;

        case OP_2_PREOP:
            /* stop the output handler (function is defined above) */
            result = APPL_StopOutputHandler();

            StopOutputHandler();

            bApplEsmPending = FALSE;

            if (result != 0)
                break;

            stateTrans = SAFEOP_2_PREOP;
        case SAFEOP_2_PREOP:
            /* stop the input handler (function is defined above) */
            APPL_StopInputHandler();
           
            StopInputHandler();

            bApplEsmPending = FALSE;

            break;
        case OP_2_INIT:
            /* stop the output handler (function is defined above) */
            result = APPL_StopOutputHandler();

            StopOutputHandler();

            bApplEsmPending = FALSE;

            if (result != 0)
                break;
            
            stateTrans = SAFEOP_2_INIT;
        case SAFEOP_2_INIT:
            /* stop the input handler (function is defined above) */
            result = APPL_StopInputHandler();
            
            StopInputHandler();

            bApplEsmPending = FALSE;

            if (result != 0)
                break;
            stateTrans = PREOP_2_INIT;
        case PREOP_2_INIT:
            MBX_StopMailboxHandler();
            result = APPL_StopMailboxHandler();

            BackToInitTransition();
            break;
        case INIT_2_INIT:
            BackToInitTransition();
        case PREOP_2_PREOP:
        case SAFEOP_2_SAFEOP:
        case OP_2_OP:
            if(bErrAck)
                APPL_AckErrorInd(stateTrans);

            if(!bLocalErrorFlag)
            {
                /*no local error flag is currently active, enable SM*/
                if (nAlStatus & (STATE_SAFEOP | STATE_OP))
                {
                    if (nPdOutputSize > 0)
                    {
/*ECATCHANGE_START(V5.11) HW1*/
                        EnableSyncManChannel(PROCESS_DATA_OUT);
/*ECATCHANGE_END(V5.11) HW1*/
                    }
                    else if(nPdInputSize > 0)
                    {
/*ECATCHANGE_START(V5.11) HW1*/
                        EnableSyncManChannel(PROCESS_DATA_IN);
/*ECATCHANGE_END(V5.11) HW1*/
                    }
                }
            }
            result = NOERROR_NOSTATECHANGE;
            break;
        case INIT_2_SAFEOP:
        case INIT_2_OP:
        case PREOP_2_OP:
        case PREOP_2_BOOT:
        case SAFEOP_2_BOOT:
        case OP_2_BOOT:
        case BOOT_2_PREOP:
        case BOOT_2_SAFEOP:
        case BOOT_2_OP:
            result = ALSTATUSCODE_INVALIDALCONTROL;
            break;

        default:
            result = ALSTATUSCODE_UNKNOWNALCONTROL;
            break;
        }
    }
    else
    {
        /* the checking of the sync manager settings was not successful
            switch back the state to PREOP or INIT */
        switch (nAlStatus)
        {
        case STATE_OP:
            /* stop the output handler (function is defined above) */
            APPL_StopOutputHandler();
            StopOutputHandler();
        case STATE_SAFEOP:
            /* stop the input handler (function is defined above) */
            APPL_StopInputHandler();

            StopInputHandler();
        case STATE_PREOP:
            if (result == ALSTATUSCODE_INVALIDMBXCFGINPREOP)
            {
                /* the mailbox sync manager settings were wrong, switch back to INIT */
                MBX_StopMailboxHandler();
                APPL_StopMailboxHandler();

                nAlStatus = STATE_INIT;
            }
            else
                nAlStatus = STATE_PREOP;
        }
    }

    if (result == NOERROR_INWORK)
    {
        /* state transition is still in work
            ECAT_StateChange must be called from the application */
        bEcatWaitForAlControlRes = TRUE;
        /* state transition has to be stored */
        nEcatStateTrans = stateTrans;

        /*Init ESM timeout counter (will be decremented with the local 1ms timer)*/
        switch(nEcatStateTrans)
        {
            case INIT_2_PREOP:
            case INIT_2_BOOT:
                EsmTimeoutCounter = PREOPTIMEOUT;
            break;
            case PREOP_2_SAFEOP:
            case SAFEOP_2_OP:
                EsmTimeoutCounter = SAFEOP2OPTIMEOUT;
                break;
           default:
                EsmTimeoutCounter = 200; //Set default timeout value to 200ms
                break;
        }
        EsmTimeoutCounter -= 50; //subtract 50ms from the timeout to react before the master runs into a timeout.

    }
    else if (alControl != (nAlStatus & STATE_MASK))
    {
        /* The slave state has changed */

        if ((result != 0 || alStatusCode != 0) && ((alControl | nAlStatus) & STATE_OP))
        {
            /* the local application requested to leave the state OP so we have to disable the SM2
               and make the state change from OP to SAFEOP by calling StopOutputHandler */

            //only execute StopOutputHandler() if Output update is still running
            if (bEcatOutputUpdateRunning)
            {
                APPL_StopOutputHandler();

                StopOutputHandler();
            }

            if (nPdOutputSize > 0)
            {
                /* disable the Sync Manager Channel 2 (outputs) */
/*ECATCHANGE_START(V5.11) HW1*/
                DisableSyncManChannel(PROCESS_DATA_OUT);
/*ECATCHANGE_END(V5.11) HW1*/
            }
            else if (nPdInputSize > 0)
            {
                /*disable Sync Manager 3 (inputs) if no outputs available*/
/*ECATCHANGE_START(V5.11) HW1*/
                DisableSyncManChannel(PROCESS_DATA_IN);
/*ECATCHANGE_END(V5.11) HW1*/
            }

        }
        if (result != 0)
        {
            if (nAlStatus == STATE_OP)
                nAlStatus = STATE_SAFEOP;
            /* save the failed status to be able to decide, if the AL Status Code shall be
               reset in case of a coming successful state transition */
            nAlStatus |= STATE_CHANGE;
        }
        else
        {
            /* state transition was successful */
            if (alStatusCode != 0)
            {
                /* state change request from the user */
                result = alStatusCode;
                alControl |= STATE_CHANGE;
            }
            /* acknowledge the new state */
            nAlStatus = alControl;
        }

        bEcatWaitForAlControlRes = FALSE;

        /* write the AL Status register */
        SetALStatus(nAlStatus, result);
    }
    else
    {
        /* Error acknowledgement without a state transition */

        bEcatWaitForAlControlRes = FALSE;

        /* AL-Status has to be updated and AL-Status-Code has to be reset
           if the the error bit was acknowledged */
        SetALStatus(nAlStatus, 0);
    }

}

2.7 中断服务及过程数据

在这里插入图片描述

ESC用0x204寄存器来配置PDI中断，0x220寄存器的每个bit表示每个事件的是否触发，若0x204将响应的bit置为1，则该事件产生时ESC会产生中断信号，通知ESC检查0x220寄存器从而进行相应处理。

对于支持DC的从站，当从站同步方式为DC或SM时，该PDI中断在代码中被设置成过程数据产生的中断，即过程数据对应的SM通道的中断，如代码所示，当从站有RXPDO时，则PDI中断配置为SM2产生中断，当只有TxPDO时，中断被配置成SM3产生中断。在切换状态到SafeOP时，设置该中断。

...

/* If no free run is supported the EscInt is always enabled*/
if ((SyncType0x1C32 != SYNCTYPE_FREERUN) || (SyncType0x1C33 != SYNCTYPE_FREERUN))
{
    /* ECAT Synchron Mode, the ESC interrupt is enabled */
    bEscIntEnabled = TRUE;
}

/* Update value for AL Event Mask register (0x204) */
if (bEscIntEnabled)
{
    if (nPdOutputSize > 0)
    {
        u16ALEventMask = PROCESS_OUTPUT_EVENT;
    }
    else if(nPdInputSize > 0)
    {
        u16ALEventMask = PROCESS_INPUT_EVENT;
    }

}

...
    
case PREOP_2_SAFEOP:
	/* start the input handler (function is defined above) */
	result = StartInputHandler();
	if (result == 0)
	{
		bApplEsmPending = FALSE;
		result = APPL_StartInputHandler(&u16ALEventMask);

		if (result == 0)
		{
			/* initialize the AL Event Mask register (0x204) */
			/*ECATCHANGE_START(V5.11) HW1*/
			SetALEventMask(u16ALEventMask);
			/*ECATCHANGE_END(V5.11) HW1*/

			bEcatInputUpdateRunning = TRUE;
		}
	}

对于ESC所用的四个中断，这里暂时只分析PDI和Sync0这两个中断。

对于PDI中断，首先检查0x220中断产生标志，若产生RxPDO接收中断，则拷贝RxPDO数据（第32行），若同步方式不是DC模式，此时直接执行应用（第57行），执行完应用后，将TxPDO数据拷贝到ESC中（第67行），SM同步方式是以接收PDO数据帧作为同步方式是在代码的这里体现的。

void PDI_Isr(void)
{
    if (bEscIntEnabled)
    {
        /* get the AL event register */
        UINT16 ALEvent = HW_GetALEventRegister_Isr();
        ALEvent = SWAPWORD(ALEvent);

        if (ALEvent & PROCESS_OUTPUT_EVENT)
        {
            if (bDcRunning && bDcSyncActive)
            {
                /* Reset SM/Sync0 counter. Will be incremented on every Sync0 event*/
                u16SmSync0Counter = 0;
            }
            if (sSyncManOutPar.u16SmEventMissedCounter > 0)
                sSyncManOutPar.u16SmEventMissedCounter--;


            /*ECATCHANGE_START(V5.11) ECAT6*/
            //calculate the bus cycle time if required
            HandleBusCycleCalculation();
            /*ECATCHANGE_END(V5.11) ECAT6*/

            /* Outputs were updated, set flag for watchdog monitoring */
            bEcatFirstOutputsReceived = TRUE;

            /* handle output process data event*/
            if (bEcatOutputUpdateRunning)
            {
                /* slave is in OP, update the outputs */
                PDO_OutputMapping();
            }
            else
            {
                /* Just acknowledge the process data event in the 
                   INIT,PreOP and SafeOP state */
                HW_EscReadWordIsr(u16dummy, nEscAddrOutputData);
                HW_EscReadWordIsr(u16dummy, (nEscAddrOutputData + nPdOutputSize - 2));
            }
        }

        /*ECATCHANGE_START(V5.11) ECAT4*/
        if ((ALEvent & PROCESS_INPUT_EVENT) && (nPdOutputSize == 0))
        {
            //calculate the bus cycle time if required
            HandleBusCycleCalculation();
        }
        /*ECATCHANGE_END(V5.11) ECAT4*/

        /*
            Call ECAT_Application() in SM Sync mode
        */
        if (sSyncManOutPar.u16SyncType == SYNCTYPE_SM_SYNCHRON)
        {
            /* The Application is synchronized to process data Sync Manager event*/
            ECAT_Application();
        }

        if (bEcatInputUpdateRunning 
        /*ECATCHANGE_START(V5.11) ESM7*/
        && ((sSyncManInPar.u16SyncType == SYNCTYPE_SM_SYNCHRON) || (sSyncManInPar.u16SyncType == SYNCTYPE_SM2_SYNCHRON))
        /*ECATCHANGE_END(V5.11) ESM7*/
        )
        {
            /* EtherCAT slave is at least in SAFE-OPERATIONAL, update inputs */
            PDO_InputMapping();
        }

        /*
            Check if cycle exceed
        */
        /*if next SM event was triggered during runtime increment cycle exceed counter*/
        ALEvent = HW_GetALEventRegister_Isr();
        ALEvent = SWAPWORD(ALEvent);

        if (ALEvent & PROCESS_OUTPUT_EVENT)
        {
            sSyncManOutPar.u16CycleExceededCounter++;
            sSyncManInPar.u16CycleExceededCounter = sSyncManOutPar.u16CycleExceededCounter;

            /* Acknowledge the process data event*/
            HW_EscReadWordIsr(u16dummy, nEscAddrOutputData);
            HW_EscReadWordIsr(u16dummy, (nEscAddrOutputData + nPdOutputSize - 2));
        }
    } //if(bEscIntEnabled)
}

对于Sync0中断，首先需要清除看门狗计数Sync0WdCounter，第13行if语句中整个代码逻辑是检查过程数据丢帧情况的，若u16SmSync0Counter计数大于设定的u16SmSync0Value值，则丢帧计数增加，第34行若PDO通信只有TxPDO，第40行检查AL事件若有PDO输入事件，则表示PDO正常接收，无丢帧。若有RxPDO，则u16SmSync0Counter直接加1，该变量在上述PDI中断回调中第14行，若检查到PDI中断触发时，RxPDO被正常接收到，u16SmSync0Counter被清零，这里的逻辑也直接验证了使用从站时的一个要求，即在从站的sync0信号到来前的一段时间，需要将PDO发送至从站，Sync0与PDI的时间差，对于EtherCAT通信是一个很重要的参数，一般在使用主站时都可以设置这个参数，主站需要尽可能在运行时使每个从站都接近这个设定值。第63行，拷贝RxPDO数据，而后第67行执行根据RxPDO数据执行应用，这里的代码只是演示代码，应用相关的程序较少，执行速度快，若是电机控制程序，这里猜测估计不会将耗时很久的任务放在这里执行。第73行拷贝TxPDO数据到ESC，这里有个问题是，拷贝到ESC的TxPDO数据，并不会在本周期通信内被ESC放入数据帧，因为这不符合EtherCAT的“On the fly”的通信特性，本周期拷贝到SM的数据，得下一个通信周期才能被数据帧读走，则也是EtherCAT通信的一个特点，通信上总是慢一个周期，再加上从接收数据帧，到应用响应，又会延后至少一个周期，这样从控制数据开始算，总体通信总是慢至少2个周期。

void Sync0_Isr(void)
{
    Sync0WdCounter = 0;

    if (bDcSyncActive)
    {
        if (bEcatInputUpdateRunning)
        {
            LatchInputSync0Counter++;
        }

        /*ECATCHANGE_START(V5.11) ECAT4*/
        if (u16SmSync0Value > 0)
        {
            /* Check if Sm-Sync sequence is invalid */
            if (u16SmSync0Counter > u16SmSync0Value)
            {
                /*ECATCHANGE_START(V5.11) COE3*/
                if ((nPdOutputSize > 0) && (sSyncManOutPar.u16SmEventMissedCounter <= sErrorSettings.u16SyncErrorCounterLimit))
                {
                    /*ECATCHANGE_END(V5.11) COE3*/
                    sSyncManOutPar.u16SmEventMissedCounter = sSyncManOutPar.u16SmEventMissedCounter + 3;
                }

                /*ECATCHANGE_START(V5.11) COE3*/
                if ((nPdInputSize > 0) && (nPdOutputSize == 0) && (sSyncManInPar.u16SmEventMissedCounter <= sErrorSettings.u16SyncErrorCounterLimit))
                {
                    /*ECATCHANGE_END(V5.11) COE3*/
                    sSyncManInPar.u16SmEventMissedCounter = sSyncManInPar.u16SmEventMissedCounter + 3;
                }

            } // if (u16SmSync0Counter > u16SmSync0Value)

            if ((nPdOutputSize == 0) && (nPdInputSize > 0))
            {
                /* Input only with DC, check if the last input data was read*/
                UINT16 ALEvent = HW_GetALEventRegister_Isr();
                ALEvent = SWAPWORD(ALEvent);

                if ((ALEvent & PROCESS_INPUT_EVENT) == 0)
                {
                    /* no input data was read by the master, increment the sm missed counter*/
                    u16SmSync0Counter++;
                }
                else
                {
                    /* Reset SM/Sync0 counter*/
                    u16SmSync0Counter = 0;

                    sSyncManInPar.u16SmEventMissedCounter = 0;
                }
            }
            else
            {
                u16SmSync0Counter++;
            }
        }//SM -Sync monitoring enabled
        /*ECATCHANGE_END(V5.11) ECAT4*/

        if (!bEscIntEnabled && bEcatOutputUpdateRunning)
        {
            /* Output mapping was not done by the PDI ISR */
            PDO_OutputMapping();
        }

        /* Application is synchronized to SYNC0 event*/
        ECAT_Application();

        if (bEcatInputUpdateRunning 
           && (LatchInputSync0Value > 0) && (LatchInputSync0Value == LatchInputSync0Counter)) /* Inputs shall be latched on a specific Sync0 event */
        {
            /* EtherCAT slave is at least in SAFE-OPERATIONAL, update inputs */
            PDO_InputMapping();

            if (LatchInputSync0Value == 1)
            {
                /* if inputs are latched on every Sync0 event (otherwise the counter is reset on the next Sync1 event) */
                LatchInputSync0Counter = 0;
            }
        }
    }
}

2.8 从站应用

上边分析PDI及Sync0中断时，涉及到应用相关，应用层任务总接口是ECAT_Application接口，代码如下，这里以402轴控相关应用做简要分析，代码的调用链是ECAT_Application->APPL_Application->CiA402_Application->CiA402_DummyMotionControl，CiA402_Application接口中，第26行代码，该应用只针对CSP和CSV两种运行模式进行处理，而第106行的if是将0x6060应用模式，赋值给0x6061实际运行模式，只有在DC模式下才执行，实测该代码若运行在SM模式下，无法运行应用，在工作中也遇到过类似问题，第三方从站没有明确在ESI文件中表明不支持非DC模式，但可以正常进OP，进了OP却没法控制，猜测是这里移植代码时没有注意到这一点，或该从站不支持DC，ESI文件忘记修改。CiA402_DummyMotionControl是实际应用，若运行在CSP模式下，简单将0x607A目标位置和当前位置差分做为实际速度，对实际速度进行一定比例减小，表示不实时跟随目标位置，简单理解可以理解为最终的结果是将实际位置延后一定的周期赋值为目标位置，而在CSV模式下，则是直接根据目标速度乘以一个比例然后进行位置累加。

这里的电机控制应用只是做了一个简单演示，实际从站进行控制一定是MCU将接收到的位置指令下发给电机控制模块，而后从位置采集模块获取当前位置。

void ECAT_Application(void)
{
	/*Axis configuration is written in state change from PREOP to SAFEOP
    => trigger CiA402 Application if device is in SAFEOP or OP
    (Motion Controller function is only triggered if DC Synchronisation is active and valid mode of operation is set)*/
    if (bEcatInputUpdateRunning)
    {
        APPL_Application();
    }
    /* PDO Input mapping is called from the specific trigger ISR */
}

void APPL_Application(void)
{
    UINT16 i;

    for (i = 0; i < MAX_AXES; i++)
    {
        if (LocalAxes[i].bAxisIsActive)
            CiA402_Application(&LocalAxes[i]);
    }
}

void CiA402_Application(TCiA402Axis *pCiA402Axis)
{
    /*clear "Drive follows the command value" flag if the target values from the master overwritten by the local application*/
    if (pCiA402Axis->u16PendingOptionCode != 0 &&
        (pCiA402Axis->Objects.objModesOfOperationDisplay == CYCLIC_SYNC_POSITION_MODE ||
        pCiA402Axis->Objects.objModesOfOperationDisplay == CYCLIC_SYNC_VELOCITY_MODE))
    {
        pCiA402Axis->Objects.objStatusWord &= ~ STATUSWORD_DRIVE_FOLLOWS_COMMAND;
    }
    else
        pCiA402Axis->Objects.objStatusWord |= STATUSWORD_DRIVE_FOLLOWS_COMMAND;

    switch (pCiA402Axis->u16PendingOptionCode)
    {
    case 0x605A:
        /*state transition 11 is pending analyse shutdown option code (0x605A)*/
        {
            UINT16 ramp = pCiA402Axis->Objects.objQuickStopOptionCode;
            /*masked and execute specified quick stop ramp characteristic */
            if(pCiA402Axis->Objects.objQuickStopOptionCode > 4 && pCiA402Axis->Objects.objQuickStopOptionCode <9)
            {
                if(pCiA402Axis->Objects.objQuickStopOptionCode == 5)
                    ramp = 1;
                if(pCiA402Axis->Objects.objQuickStopOptionCode == 6)
                    ramp = 2;
                if(pCiA402Axis->Objects.objQuickStopOptionCode == 7)
                    ramp = 3;
                if(pCiA402Axis->Objects.objQuickStopOptionCode == 8)
                    ramp = 4;
            }

            if(CiA402_TransitionAction(ramp,pCiA402Axis))
            {
                /*quick stop ramp is finished complete state transition*/
                pCiA402Axis->u16PendingOptionCode = 0x0;
                if(pCiA402Axis->Objects.objQuickStopOptionCode > 0 && pCiA402Axis->Objects.objQuickStopOptionCode < 5)
                {
                    pCiA402Axis->i16State = STATE_SWITCH_ON_DISABLED;    //continue state transition 12
                }
                else if(pCiA402Axis->Objects.objQuickStopOptionCode > 4 && pCiA402Axis->Objects.objQuickStopOptionCode < 9)
                    pCiA402Axis->Objects.objStatusWord |= STATUSWORD_TARGET_REACHED;
            }
        }
        break;
    case 0x605B:
        /*state transition 8 is pending analyse shutdown option code (0x605B)*/
        {
            if(CiA402_TransitionAction(pCiA402Axis->Objects.objShutdownOptionCode,pCiA402Axis))
            {
                /*shutdown ramp is finished complete state transition*/
                pCiA402Axis->u16PendingOptionCode = 0x0;
                pCiA402Axis->i16State = STATE_READY_TO_SWITCH_ON;    //continue state transition 8
            }
        }
        break;
    case 0x605C:
        /*state transition 5 is pending analyse Disable operation option code (0x605C)*/
        {
            if(CiA402_TransitionAction(pCiA402Axis->Objects.objDisableOperationOptionCode,pCiA402Axis))
            {
                /*disable operation ramp is finished complete state transition*/
                pCiA402Axis->u16PendingOptionCode = 0x0;
                pCiA402Axis->i16State = STATE_SWITCHED_ON;    //continue state transition 5
            }
        }
        break;
    case 0x605E:
        /*state transition 14 is pending analyse Fault reaction option code (0x605E)*/
        {
            if(CiA402_TransitionAction(pCiA402Axis->Objects.objFaultReactionCode,pCiA402Axis))
            {
                /*fault reaction ramp is finished complete state transition*/
                pCiA402Axis->u16PendingOptionCode = 0x0;
                pCiA402Axis->i16State = STATE_FAULT;    //continue state transition 14
            }
        }
        break;
    default:
        //pending transition code is invalid => values from the master are used
        pCiA402Axis->Objects.objStatusWord |= STATUSWORD_DRIVE_FOLLOWS_COMMAND;
        break;
    }
    if(bDcSyncActive
        && (pCiA402Axis->u32CycleTime != 0)
        && ((pCiA402Axis->Objects.objSupportedDriveModes >> (pCiA402Axis->Objects.objModesOfOperation - 1)) & 0x1)) //Mode of Operation (0x6060) - 1 specifies the Bit within Supported Drive Modes (0x6502)
    {
        CiA402_DummyMotionControl(pCiA402Axis);
    }
}

void CiA402_DummyMotionControl(TCiA402Axis *pCiA402Axis)
{
	INT32 i32TargetVelocity = 0;
    float IncFactor = (float)0.0010922 * (float) pCiA402Axis->u32CycleTime;

    /*Motion Controller shall only be triggered if application is trigger by DC Sync Signals,
      and a valid mode of operation is set*/

    /*calculate actual position */
    pCiA402Axis->fCurPosition += ((double)pCiA402Axis->Objects.objVelocityActualValue) * IncFactor;
    pCiA402Axis->Objects.objPositionActualValue = (INT32)(pCiA402Axis->fCurPosition);

    if(pCiA402Axis->bAxisFunctionEnabled &&
       pCiA402Axis->bLowLevelPowerApplied &&
       pCiA402Axis->bHighLevelPowerApplied &&
       !pCiA402Axis->bBrakeApplied)
    {
        if((pCiA402Axis->Objects.objSoftwarePositionLimit.i32MaxLimit> pCiA402Axis->Objects.objPositionActualValue
            || pCiA402Axis->Objects.objPositionActualValue > pCiA402Axis->Objects.objTargetPosition) &&
            (pCiA402Axis->Objects.objSoftwarePositionLimit.i32MinLimit < pCiA402Axis->Objects.objPositionActualValue
            || pCiA402Axis->Objects.objPositionActualValue < pCiA402Axis->Objects.objTargetPosition))
        {
            pCiA402Axis->Objects.objStatusWord &= ~STATUSWORD_INTERNAL_LIMIT;

            switch(pCiA402Axis->Objects.objModesOfOperationDisplay)
            {
            case CYCLIC_SYNC_POSITION_MODE:
                if(IncFactor != 0)
                    i32TargetVelocity = (pCiA402Axis->Objects.objTargetPosition - pCiA402Axis->Objects.objPositionActualValue) / ((long)IncFactor);
                else
                    i32TargetVelocity = 0;
                break;
            case CYCLIC_SYNC_VELOCITY_MODE:
                if(pCiA402Axis->i16State == STATE_OPERATION_ENABLED)
                    i32TargetVelocity = pCiA402Axis->Objects.objTargetVelocity;
                else
                    i32TargetVelocity = 0;
                break;
            default:
                break;
            }
        }
        else
        {
            pCiA402Axis->Objects.objStatusWord |= STATUSWORD_INTERNAL_LIMIT;
        }
    }
    pCiA402Axis->Objects.objVelocityActualValue= i32TargetVelocity;

    /*Accept new mode of operation*/
    pCiA402Axis->Objects.objModesOfOperationDisplay = pCiA402Axis->Objects.objModesOfOperation;

	temp = i32TargetVelocity;
	position = (INT32)(pCiA402Axis->fCurPosition);	
}