CiA402 Example

Scope

This example covers how to enable the CiA402 device profile.

Related Files

ecSubDeviceCiA402.c
ecSubDeviceCiA402.h
ESL_cia402Demo.c
ESL_cia402Demo.h
ESL_cia402Obd.c
ESL_cia402Obd.h
EtherCAT_SubDevice_CiA402.c

CiA402 Example

This example configures a 3 axis motor control application. If the device or the ESI file (EtherCAT SubDevice Information) does not support the CiA402 device profile, the subsequent registration calls should be disabled. The following functions are implemented in ESL_cia402Demo.c.

EC_SLV_APP_motionControl

Motion control is done in this function. The control of modes CSP, CSV or CST is shown.

EC_SLV_APP_cia402Application

This CiA402 application function is synchronized to Sync0 ISR in DC synchronous mode and to Pdi ISR in SM synchronous mode. This function is registered using EC_API_SLV_CiA402_registerApplication(). This function calls EC_API_SLV_preSeqInputPDBuffer() and EC_API_SLV_preSeqOutputPDBuffer() to acquire the input and output process data buffers and calls EC_API_SLV_postSeqInputPDBuffer() and EC_API_SLV_postSeqOutputPDBuffer() to release the acquired buffers. The output process data buffer is being updated by the PDO_OutputMapping(), which is called in Sync0 ISR in DC mode and in Pdi ISR in SM mode. Similarly, input process data from the application is accessed by the PDO_InputMapping(), which is called in Sync0 ISR in DC mode and in Pdi ISR in SM mode.
The sequence of tasks performed in Sync0 ISR in DC mode is given below.

• Get output process data from SM2 and update local output process data buffer, handled by PDO_OutputMapping().
• Application Task - CiA402 application which acquires both local input and output process data buffers.
• Set input process data into SM3 from the local input process data buffer, handled by PDO_InputMapping().
  

The function contains an implementation of the CiA402 State Machine (although an external one can be registered). This state machine works alongside the CiA402 application. The application reads the control object (0x6040) and writes the status object (0x6041). The application activates the axis and checks if there is any error pending. If an error is pending then it calls to transition actions to resolve the error and clears the error status if required.

EC_SLV_APP_cia402LocalError

If the CiA402 State machine changes to an error state this function is called. It is used to notify the error, not to take any action.

Basic Setup

The example starts by initializing operating system (OSAL) and hardware abstraction layers (HWAL). The EtherCAT functionality itself is implemented within the EC_SLV_APP_loopTask that is started from the main application entry point, both are implemented in EtherCAT_SubDevice_CiA402.c.

Hardware Setup

EC_SLV_APP_loopTask starts with the initialization of the GPIO driver and the configuration of the PHY and LED pins in the function EC_SLV_APP_initBoardFunctions in ecSubDeviceCiA402.c.

Next PHY parameters, such as address, polarity and RX link are configured in EC_SLV_APP_registerStacklessBoardFunctions.

Program flow of the EtherCAT SubDevice CiA402 Example Application

Initialization of Stack, PRU, and Device Identification

Before entering the main stack loop, the example application itself is initialized in EC_SLV_APP_CIA_applicationInit. This function creates a new EtherCAT SubDevice instance, registers a board status LED callback, sets vendor ID, product code and name, revision number, etc.

Program flow of the EtherCAT SubDevice CiA402 Example Application

After the basic initialization, EC_SLV_APP_CIA_applicationInit continues with the setup of the CiA402 example drives and motion control example.

CiA402 Basics

CiA402 Object Dictionary

In CiA402 profile, for multiple axis SubDevices the objects range from 0x6000 to 0x67FF shall be shifted by 0x800 per axis. The below table describes the object index range for 3 axis.

Axis	Range
Axis 1	0x6000 to 0x67FF
Axis 2	0x6800 to 0x6FFF
Axis 3	0x7000 to 0x77FF

For a 3-axis setup, the CiA402 objects is allocated as follows.

Objects	Axis 1	Axis 2	Axis 3
Controlword	0x6040	0x6840	0x7040
Statusword	0x6041	0x6841	0x7041
Modes of operation	0x6060	0x6860	0x7060
Modes of operation display	0x6061	0x6861	0x7061
Target position	0x607A	0x687A	0x707A
Target velocity	0x60FF	0x68FF	0x70FF
Target torque	0x6071	0x6871	0x7071
Position actual value	0x6064	0x6864	0x7064
Velocity actual value	0x606C	0x686C	0x706C
Torque actual value	0x6077	0x6877	0x7077

CiA402 State Machine

CiA402 State Machine or Finite State Automation (FSA) is independent to the EtherCAT State Machine (ESM) and defines internal and external behavior for each state. The state of the drive determines which commands are accepted and whether high power is applied. States are changed when a controlword from the host-controller is received. They can also be changed due to internal events. The current state is indicated by the statusword. The controlword and different command values (e.g. velocity) are mapped into default RxPDOs (receive process data objects). The statusword and different actual values (e.g. position) are mapped into TxPDOs.

For a detailed description of the state machine transition please refer to the chapter 5.1 of the ETG6010 CiA402 implementation directive.

CiA402 State Machine

When the EtherCAT SubDevice has reached the OP state, there shall be an automatic state transition from Not ready to switch on state to Switch on disabled state in the CiA402 state machine. The states in bold letters are stable states in the drive, and transition can be requested only from the control device/MainDevice. Below is the list of controlword commands for the corresponding state transitions in the CiA402 state machine.

Controlword	CiA402 State Transitions
0x06	Switch on disabled -> Ready to switch on
0x07	Ready to switch on -> Switched on
0x0F	Switched on -> Operation enabled

Controlword

The controlword object 0x6040 is used to request state changes on the CiA402 state machine for Axis 1. The control commands are coded by bits.

Bit	Description
0	Switch on
1	Enable voltage
2	Quick Stop
3	Enable Operation
4-6	Operation mode specific
7	Fault reset

These defines are available to transition between different states.

Define	Value	Description
CONTROLWORD_COMMAND_SHUTDOWN	0x0006	Shutdown command
CONTROLWORD_COMMAND_SWITCHON	0x0007	Switch on command
CONTROLWORD_COMMAND_SWITCHON_ENABLEOPERATION	0x000F	Switch on and enable command
CONTROLWORD_COMMAND_DISABLEVOLTAGE	0x0000	Disable voltage command
CONTROLWORD_COMMAND_QUICKSTOP	0x0002	Quickstop command
CONTROLWORD_COMMAND_DISABLEOPERATION	0x0007	Disable operation command
CONTROLWORD_COMMAND_ENABLEOPERATION	0x000F	Enable operation command
CONTROLWORD_COMMAND_FAULTRESET	0x0080	Fault reset command

Statusword

The statusword object 0x6041 is used to get the current state of the CiA402 state machine for Axis 1.

Bit	Description
0	Ready to switch on
1	Switched on
2	Operation enabled
3	Fault
4	Voltage enabled
5	Quick Stop
6	Switch on disabled

These defines are available to interpret the FSA states.

Define	Value	Description
STATUSWORD_STATE_NOTREADYTOSWITCHON	0x0000	Not ready to switch on
STATUSWORD_STATE_SWITCHEDONDISABLED	0x0040	Switched on but disabled
STATUSWORD_STATE_READYTOSWITCHON	0x0021	Ready to switch on
STATUSWORD_STATE_SWITCHEDON	0x0023	Switched on
STATUSWORD_STATE_OPERATIONENABLED	0x0027	Operation enabled
STATUSWORD_STATE_QUICKSTOPACTIVE	0x0007	Quickstop active
STATUSWORD_STATE_FAULTREACTIONACTIVE	0x000F	Fault reaction active
STATUSWORD_STATE_FAULT	0x0008	Fault state

Operation modes

These modes are recommended to be supported by the drives:

Operation mode	Code
Profile position mode	1
Velocity mode	2
Profile velocity mode	3
Torque profile mode	4
Homing mode	6
Interpolated position mode	7
Cyclic synchronous position mode	8
Cyclic synchronous velocity mode	9
Cyclic synchronous torque mode	10
Cyclic synchronous torque mode with commutation angle	11

The mode of operation object 0x6060 is used to request an operation mode for Axis 1.

The mode of operation display object 0x6061 is used to get the current operation mode for Axis 1.

Position control

The target position object 0x607A is used to request the target position for Axis 1.

The position actual value object 0x6064 gets the drive's current position for Axis 1.

Velocity control

The target velocity object 0x60FF is used to request the target velocity for Axis 1.

The velocity actual value object 0x606C gets the drive's current velocity for Axis 1.

Torque control

The target torque object 0x6071 is used to request the target torque for Axis 1.

The torque actual value object 0x6077 gets the drive's current torque for Axis 1.

Request object summary

These are the objects that must be written to the drive to command a specific operation.

Index	Description
0x6040	Controlword
0x6060	Operation mode
0x607A	Target position
0x60FF	Target velocity
0x6071	Target torque

Feedback object summary

These are the objects that must be read to get the current status of the drive.

Index	Description
0x6041	Statusword
0x6061	Operation mode display
0x6064	Position actual value
0x606C	Velocity actual value
0x6077	Torque actual value

Using CiA402 Example on TwinCAT

• The CiA402 demo simulates a three axis motor control application. For each axis, the objects described above are mapped as process data. Therefore, the first step is to scan the SubDevice and bring it to Operational state (OP).
• Scan the EtherCAT SubDevice.
• Select the NC-Configuration.

NC Configuration

• Activate 'Free Run' mode.
• Double click on Drive 1 (TI EtherCAT Toolkit CiA402 for AM64X.R5F) and navigate to Online tab. Make sure that the EtherCAT SubDevice is in OP state.

CiA402 runs in OP

• Open the Drive 1 (TI EtherCAT Toolkit CiA402 for AM64X.R5F), the CiA402 mapped objects can be accessed here.

CiA402 example process data

• Go to the Modes of operation object and set to Cyclic Synchronous Position (CSP) mode, 0x08.

Set value dialog

Online view

• Go to the Modes of operation display object. It will show that the Cyclic Synchronous Position (CSP) mode has been set.

Online view

• Go to the Target position object and set a desired position for the axis, for example 65536.

Set value dialog

Online view

• Go to the Position actual value object. As we did not enable the operation, the axis did not move.

Online view

• After power up, the CiA402 state machine is in Switch on disabled state. It should be changed to Operation enabled state in a sequence defined by the CiA402 state machine.
• Go to the Controlword object and set the Ready to switch on command (0x0006). Now the CiA402 state is transitioned to Ready to switch on state.
• Set the switched on command (0x0007). Now the CiA402 state is transitioned to switched on state.
• Set the enable operation command (0x000F). Now the CiA402 state is transitioned to Operation enabled state.

Set Value Dialog

Online view

• Go to the Statusword object. It will show that drive has been switched on and the operation has been enabled. The statusword value is 4663 (0x1237). The bits 0, 1 and 2 are set indicating that the SubDevice has progressed through a sequence of states, Ready to switch on ->Switched on -> Operation enabled, in compliance with the CiA402 state machine.

Online view

• Finally, go to the Position actual value object again. As the operation has been enabled, the axis reached the nearest target position based on the control logic. In this case, actual position is set to 65532.

Online view

• The written values as well as the values provided by the drive simulation can be seen in the object dictionary.

Object Dictionary

The rest of the axis and other operation modes can be configured following the same steps described above. Furthermore, there are more objects used for the drive application but this example covers only the basic configuration. Please refer to the ETG6010 document for a detailed description of the CiA402 directive.

Cyclic Synchronous Position (CSP) in TwinCAT NC Manual Mode

To test the Cyclic Synchronous Position (CSP) mode in TwinCAT NC:Online configurations, please follow the below steps.

• Scan the EtherCAT SubDevice.
• Select the NC-Configuration.

NC Configuration

• Activate 'Free Run' mode.
• Make sure that the EtherCAT SubDevice is in OP state.
• Double click on Drive 1 (TI EtherCAT Toolkit CiA402 for AM64X.R5F) and navigate to DC tab.
• Select Operation Mode as DC-Synchron mode.
• Click on Activate Configuration Icon and click OK.

Activate Configuration Icon

Activate Configuration

• Click OK to restart TwinCAT system in run mode.

Restart TwinCAT System in Run Mode

• Navigate to NC-Online tab.

TwinCAT NC:Online View

• Perform all settings as indicated on the screenshot below and click F5 button to run the simulation.

CSP settings in NC:Online

• After the simulation completes, observe the Actual position value. The Actual position value should be closer to the Target position value.
  

Actual Position

Cyclic Synchronous Velocity (CSV) in TwinCAT NC Manual Mode

Below are the steps to test the Cyclic Synchronous Velocity (CSV) mode in TwinCAT NC:Online configurations.

• Follow the steps mentioned in section Cyclic Synchronous Position (CSP) in TwinCAT NC Manual Mode until restarting the TwinCAT in run mode.
• Click on the RxPDO Modes of operation and force the mode to Cyclic Synchronous Velocity (CSV) mode by writting the value 0x09.

Force write to set CSV mode

CSV mode selected

• Perform all settings as indicated on the screenshot below and click F5 button to run the simulation.

CSV settings in NC:Online

• While the simulation is running observe that the Actual velocity is following the Target velocity.

Actual Velocity

• The simulation completes when the Actual position value has reached the set Target position value.
  

Actual Position

Extend to 6 Axis Motor Control Application

Below are the steps to be followed to extend the CiA402 example to 6-axis motor control application.

• Increase the number of axis to 6 in ecSlvCiA402.h file

#define AXES_NUMBER (6u)

• Create additional pointers to TxPDOs and RxPDOs in EC_SLV_APP_CIA_Application_t in ecSlvCiA402.h file.

// RxPDOs
EC_API_SLV_Pdo_t*               ptRxPdo1600;
EC_API_SLV_Pdo_t*               ptRxPdo1601;
EC_API_SLV_Pdo_t*               ptRxPdo1602;
 
// To extend to 6-axis
EC_API_SLV_Pdo_t*               ptRxPdo1603;
EC_API_SLV_Pdo_t*               ptRxPdo1604;
EC_API_SLV_Pdo_t*               ptRxPdo1605;
 
// TxPDOs
EC_API_SLV_Pdo_t*               ptTxPdo1A00;
EC_API_SLV_Pdo_t*               ptTxPdo1A01;
EC_API_SLV_Pdo_t*               ptTxPdo1A02;
 
// To extend to 6-axis
EC_API_SLV_Pdo_t*               ptTxPdo1A03;
EC_API_SLV_Pdo_t*               ptTxPdo1A04;
EC_API_SLV_Pdo_t*               ptTxPdo1A05;

• Adapt the additional axis indexes in ecSlvCiA402.c file

#define AXIS_ONE        0
#define AXIS_TWO        1
#define AXIS_THREE      2
#define AXIS_FOUR       3
#define AXIS_FIVE       4
#define AXIS_SIX        5

• In EC_SLV_APP_CIA_applicationInit(), create additional RxPDOs required.

error = EC_SLV_APP_populateRxPDO(pAppInstance_p, pAppInstance_p->ptRxPdo1603, AXIS_FOUR);
if (error != EC_API_eERR_NONE)
{
    OSAL_printf("Create RxPDO error code: 0x%08x\r\n", error);
    goto Exit;
}
 
error = EC_SLV_APP_populateRxPDO(pAppInstance_p, pAppInstance_p->ptRxPdo1604, AXIS_FIVE);
if (error != EC_API_eERR_NONE)
{
    OSAL_printf("Create RxPDO error code: 0x%08x\r\n", error);
    goto Exit;
}
 
error = EC_SLV_APP_populateRxPDO(pAppInstance_p, pAppInstance_p->ptRxPdo1605, AXIS_SIX);
if (error != EC_API_eERR_NONE)
{
    OSAL_printf("Create RxPDO error code: 0x%08x\r\n", error);
    goto Exit;
}

• In EC_SLV_APP_CIA_applicationInit(), create additional TxPDOs required.

error = EC_SLV_APP_populateTxPDO(pAppInstance_p, pAppInstance_p->ptTxPdo1A03, AXIS_FOUR);
if (error != EC_API_eERR_NONE)
{
    OSAL_printf("Create TxPDO error code: 0x%08x\r\n", error);
    goto Exit;
}
 
error = EC_SLV_APP_populateTxPDO(pAppInstance_p, pAppInstance_p->ptTxPdo1A04, AXIS_FIVE);
if (error != EC_API_eERR_NONE)
{
    OSAL_printf("Create TxPDO error code: 0x%08x\r\n", error);
    goto Exit;
}
 
error = EC_SLV_APP_populateTxPDO(pAppInstance_p, pAppInstance_p->ptTxPdo1A05, AXIS_SIX);
if (error != EC_API_eERR_NONE)
{
    OSAL_printf("Create TxPDO error code: 0x%08x\r\n", error);
    goto Exit;
}

• Increase the heap size in sysconfig file.

Each additional axis needs 27952 bytes of heap memory for creating the objects such as controlword, statusword, quick stop, shutdown, mode of operation, position actual value, velocity actual value and more. Also, RxPDOs/TxPDOs, CoE object dictionary consumes some memory. Hence it is required to increase the heap memory size. For 6-axis control, the minimum heap size required would be 229376 bytes.

Increase Heap Memory

Allocate System Memory from MSRAM

Similarly, the CiA402 example can be extended to 9-axis motor control application. For 9-axis control, the minimum heap size required would be 327680 bytes. Make sure that the size of MSRAM memory region is increased.

Create and Register Custom CiA402 State Machine

Below are the steps to be followed to register your custom CiA402 state machine.

Create Custom CiA402 State Machine

The first step is to create a custom CiA402 state machine to be registered later by following the instructions at Register Custom CiA402 State Machine.

The custom CiA402 state machine function should be created as per the below function prototype defined in ind_comms_sdk_am64x_2025_00_XX/source/industrial_comms/ethercat_subdevice/stack/inc/ecSlvApi_types.h file.

bool (*EC_API_SLV_CiA402_CBUsrApplSM_t)(void* pAppRunCtxt);

Create a custom CiA402 state machine function in ind_comms_sdk_am64x_2025_00_XX/examples/industrial_comms/ethercat_subdevice_demo/device_profiles/402_cia/ESL_cia402Demo.c file.

Below code snippet serves as a 'sample' custom CiA402 state machine function implementation in ind_comms_sdk_am64x_2025_00_XX/examples/industrial_comms/ethercat_subdevice_demo/device_profiles/402_cia/ESL_cia402Demo.c file for your reference.

⚠️ This 'sample' CiA402 state machine function is not implemented in ESL_cia402Demo.c file available in ind_comms_sdk_am64x_2025_00_XX. By default, the EtherCAT stack internal CiA402 state machine shall be active.

bool EC_SLV_APP_customCiA402StateMachine(void* pAppRunCtxt)
{
    static uint16_t controlWord;                   
    static uint16_t statusWord;                    
    static uint16_t CiA402SMState = 0;
    static uint16_t axisNo = 0;
    EC_API_SLV_EEsmState_t state = EC_API_SLV_eESM_uninit;
    uint16_t alErrorCode = 0;
 
    EC_SLV_APP_CIA_Application_t*  pApplicationInstance    = (EC_SLV_APP_CIA_Application_t*)pAppRunCtxt;
    EC_API_SLV_SHandle_t *pHandle = (EC_API_SLV_SHandle_t*)pApplicationInstance->ptEcSlvApi;
 
    EC_API_SLV_getState(pHandle, &state, &alErrorCode);
    EC_SLV_APP_getCiA402ObjectValue(pApplicationInstance,(pApplicationInstance->CiA402_axisData[axisNo].statusWordIndex).pSdo,sizeof(uint16_t),&statusWord);
 
    /*skip transition 1 and 2*/
    if ((CiA402SMState = STATE_NOT_READY_TO_SWITCH_ON) && (EC_API_SLV_eESM_op == state))
    {
        CiA402SMState = STATE_SWITCH_ON_DISABLED;
    }
 
    // Implement CiA402 State Machine States
    switch(CiA402SMState)
    {
        case STATE_NOT_READY_TO_SWITCH_ON:
        {
            // TODO: state implementation
            statusWord |= (STATUSWORD_STATE_NOTREADYTOSWITCHON);
 
            if(state == EC_API_SLV_eESM_op)
            {
                // Automatic transition
                CiA402SMState = STATE_SWITCH_ON_DISABLED; // Transition 1
            }
        } break;
 
        case STATE_SWITCH_ON_DISABLED:
        {
            // TODO: state implementation
            statusWord |= (STATUSWORD_STATE_SWITCHEDONDISABLED);
            EC_SLV_APP_getCiA402ObjectValue(pApplicationInstance,(pApplicationInstance->CiA402_axisData[axisNo].controlWordIndex).pSdo,sizeof(uint16_t),&controlWord);
            if(controlWord == CONTROLWORD_COMMAND_SHUTDOWN)
            {
                CiA402SMState = STATE_READY_TO_SWITCH_ON; // Transition 2
            }
        } break;
 
        case STATE_SWITCHED_ON:
        {
            // TODO: state implementation
        } break;
 
        case STATE_OPERATION_ENABLED:
        {
            // TODO: state implementation
        } break;
 
        case STATE_QUICK_STOP_ACTIVE:
        {
            // TODO: state implementation
        } break;
 
        default:
        {
            // TODO: state implementation
        } break;
        
    }
 
    EC_SLV_APP_setCiA402ObjectValue(pApplicationInstance,(pApplicationInstance->CiA402_axisData[axisNo].statusWordIndex).pSdo,sizeof(uint16_t),&statusWord);
 
    // Always return 'true' to disable the internal default CiA402 state machine.
    return true;
}

Register Custom CiA402 State Machine

Once the custom CiA402 state machine is created, you can register and activate it by following the below instructions.

Use the below API available in the ind_comms_sdk_am64x_2025_00_XX/source/industrial_comms/ethercat_subdevice/stack/inc/profiles/ecSlvApi_CiA402.h to register your custom CiA402 state machine function.

uint32_t EC_API_SLV_CiA402_registerStateMachine(EC_API_SLV_SHandle_t *pHandle, EC_API_SLV_CiA402_CBUsrApplSM_t cbFunc, void *pContext)

Call this API in EC_SLV_APP_populateCiA402Functions() in ind_comms_sdk_am64x_2025_00_XX/examples/industrial_comms/ethercat_subdevice_demo/device_profiles/402_cia/ecSubDeviceCiA402.c file, to register your custom CiA402 state machine function.

static EC_API_EError_t EC_SLV_APP_populateCiA402Functions(EC_SLV_APP_CIA_Application_t* pApplicationInstance_p)
{
    EC_API_EError_t error = EC_API_eERR_INVALID;
    EC_API_SLV_SHandle_t* ptSubDevice;
    ptSubDevice = pApplicationInstance_p->ptEcSlvApi;
    .
    .
 
    EC_API_SLV_CiA402_registerStateMachine(ptSubDevice, EC_SLV_APP_customCiA402StateMachine, pApplicationInstance_p);
    .
    .
    return error;
}

⚠️ When a custom CiA402 state machine is registered, the default EtherCAT stack internal CiA402 state machine shall be deactivated.

References

• https://www.can-cia.org/can-knowledge/canopen/cia402/
• https://www.ethercat.org/en/downloads/downloads_733ABB98E11545EA901D80D9A4CA7F80.htm