Mirrors_Framework/SOEM
1
0
Fork 0
mirror of https://github.com/OpenEtherCATsociety/SOEM.git synced 2026-02-06 00:53:00 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge samples

Browse Source 
Merge simple_test, red_test and coetest samples into ec_sample for
easier maintenance. The ec_sample program demonstrates distributed
clock synchronization and slave mailbox cyclic handling in addition to
running a simple process data loop.

Change-Id: If0e9cd484e701902fddffc7165e0c83b70a06979
This commit is contained in:
Hans-Erik Floryd
2025-07-11 11:08:46 +02:00
parent f3c4d11754
commit f93f66f822
11 changed files with 118 additions and 674 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@@ -158,12 +158,10 @@ install(FILES
)
if(SOEM_BUILD_SAMPLES)
add_subdirectory(samples/coetest)
add_subdirectory(samples/ec_sample)
add_subdirectory(samples/eepromtool)
add_subdirectory(samples/firm_update)
add_subdirectory(samples/red_test)
add_subdirectory(samples/simple_ng)
add_subdirectory(samples/simple_test)
add_subdirectory(samples/slaveinfo)
if (${CMAKE_SYSTEM_NAME} STREQUAL Linux)

4
cmake/Linux.cmake
View File

@@ -18,14 +18,12 @@ target_include_directories(soem PUBLIC
foreach(target IN ITEMS
soem
coetest
ec_sample
eepromtool
eni_test
eoe_test
firm_update
red_test
simple_ng
simple_test
slaveinfo)
if (TARGET ${target})
target_compile_options(${target} PRIVATE

4
cmake/Windows.cmake
View File

@@ -31,14 +31,12 @@ target_link_libraries(soem PUBLIC
foreach(target IN ITEMS
soem
coetest
ec_sample
eepromtool
eni_test
eoe_test
firm_update
red_test
simple_ng
simple_test
slaveinfo)
if (TARGET ${target})
target_compile_options(${target} PRIVATE

4
cmake/rt-kernel.cmake
View File

@@ -28,14 +28,12 @@ target_include_directories(soem PUBLIC
foreach(target IN ITEMS
soem
coetest
ec_sample
eepromtool
eni_test
eoe_test
firm_update
red_test
simple_ng
simple_test
slaveinfo)
if (TARGET ${target})
target_compile_options(${target} PRIVATE

3
samples/coetest/CMakeLists.txt
View File

@@ -1,3 +0,0 @@
add_executable(coetest coetest.c)
target_link_libraries(coetest soem)
install(TARGETS coetest DESTINATION bin)

3
samples/ec_sample/CMakeLists.txt Normal file
View File

@@ -0,0 +1,3 @@
add_executable(ec_sample ec_sample.c)
target_link_libraries(ec_sample soem)
install(TARGETS ec_sample DESTINATION bin)

146
samples/coetest/coetest.c → samples/ec_sample/ec_sample.c
View File

@@ -1,26 +1,27 @@
/** \file
* \brief CoE example code for Simple Open EtherCAT master
*
* Usage : coetest [ifname1]
* ifname is NIC interface, f.e. eth0
*
* (c)Arthur Ketels 2010 - 2024
/*
* This software is dual-licensed under GPLv3 and a commercial
* license. See the file LICENSE.md distributed with this software for
* full license information.
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <inttypes.h>
#include "soem/soem.h"
#define EC_TIMEOUTMON 500
#define NSEC_PER_SEC 1000000000
static uint8 IOmap[4096];
static OSAL_THREAD_HANDLE threadrt, thread1;
static int expectedWKC;
static int wkc;
static int mappingdone, dorun, inOP, dowkccheck;
static int adapterisbound, conf_io_size, currentgroup;
static int currentgroup = 0;
static int cycle = 0;
static int64_t cycletime = 1000000;
static ecx_contextt ctx;
@@ -39,7 +40,7 @@ static float pgain = 0.01f;
static float igain = 0.00002f;
/* set linux sync point 500us later than DC sync, just as example */
static int64 syncoffset = 500000;
int64 timeerror;
static int64 timeerror;
/* PI calculation to get linux time synced to DC time */
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
@@ -56,11 +57,11 @@ void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
*offsettime = (int64)((timeerror * pgain) + (integral * igain));
}
/* RT EtherCAT thread */
/* Cyclic RT EtherCAT thread */
OSAL_THREAD_FUNC_RT ecatthread(void)
{
ec_timet ts;
int ht, wkc;
int ht;
static int64_t toff = 0;
dorun = 0;
@@ -80,11 +81,13 @@ OSAL_THREAD_FUNC_RT ecatthread(void)
osal_monotonic_sleep(&ts);
if (dorun > 0)
{
cycle++;
wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET);
if (wkc != expectedWKC)
dowkccheck++;
else
dowkccheck = 0;
if (ctx.slavelist[0].hasdc && (wkc > 0))
{
/* calculate toff to get linux time and DC synced */
@@ -96,6 +99,7 @@ OSAL_THREAD_FUNC_RT ecatthread(void)
}
}
/* Slave error handler */
OSAL_THREAD_FUNC ecatcheck(void)
{
int slaveix;
@@ -147,7 +151,6 @@ OSAL_THREAD_FUNC ecatcheck(void)
if (slave->Ibytes)
{
memset(slave->inputs, 0x00, slave->Ibytes);
printf("zero inputs %p %d\n\r", slave->inputs, slave->Ibytes);
}
printf("ERROR : slave %d lost\n", slaveix);
}
@@ -178,59 +181,127 @@ OSAL_THREAD_FUNC ecatcheck(void)
}
}
void ethercatstartup(char *ifname)
/* Transition network to operational state */
void ecatbringup(char *ifname)
{
printf("EtherCAT Startup\n");
int rv = ecx_init(&ctx, ifname);
if (rv)
{
adapterisbound = 1;
ecx_config_init(&ctx);
if (ctx.slavecount > 0)
{
ec_groupt *group = &ctx.grouplist[0];
conf_io_size = ecx_config_map_group(&ctx, IOmap, 0);
ecx_config_map_group(&ctx, IOmap, 0);
expectedWKC = (group->outputsWKC * 2) + group->inputsWKC;
printf("%d slaves found and configured.\n", ctx.slavecount);
printf("segments : %d : %d %d %d %d\n",
group->nsegments,
group->IOsegment[0],
group->IOsegment[1],
group->IOsegment[2],
group->IOsegment[3]);
/* Configure distributed clocks */
mappingdone = 1;
ecx_configdc(&ctx);
/* Add all CoE slaves to cyclic mailbox handler */
int sdoslave = -1;
for (int si = 1; si <= ctx.slavecount; si++)
{
ec_slavet *slave = &ctx.slavelist[si];
printf("Slave %d name:%s man:%8.8x id:%8.8x rev:%d ser:%d\n", si, slave->name, slave->eep_man, slave->eep_id, slave->eep_rev, slave->eep_ser);
if (slave->CoEdetails > 0)
{
ecx_slavembxcyclic(&ctx, si);
sdoslave = si;
printf(" Slave added to cyclic mailbox handler\n");
printf(" Slave %d added to cyclic mailbox handler\n", si);
}
}
/* Let network sync to clocks */
dorun = 1;
osal_usleep(1000000);
/* Go to operational state */
ctx.slavelist[0].state = EC_STATE_OPERATIONAL;
ecx_writestate(&ctx, 0);
ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
inOP = 1;
printf("EtherCAT OP\n");
uint32_t aval = 0;
int avals = sizeof(aval);
for (int i = 0; i < 100; i++)
if (ctx.slavelist[0].state != EC_STATE_OPERATIONAL)
{
printf("Cycle %d timeerror %" PRId64 "\n", i, timeerror);
if (sdoslave > 0)
ecx_readstate(&ctx);
for (int si = 1; si <= ctx.slavecount; si++)
{
aval = 0;
int rval = ecx_SDOread(&ctx, sdoslave, 0x1018, 0x02, FALSE, &avals, &aval, EC_TIMEOUTRXM);
printf(" Slave %d Rval %d Prodcode %8.8x\n", sdoslave, rval, aval);
ec_slavet *slave = &ctx.slavelist[si];
if (slave->state != EC_STATE_OPERATIONAL)
{
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
si,
slave->state,
slave->ALstatuscode,
ec_ALstatuscode2string(slave->ALstatuscode));
}
}
osal_usleep(100000);
}
inOP = 0;
printf("EtherCAT to safe-OP\n");
else
{
int size;
printf("EtherCAT OP\n");
inOP = TRUE;
/* acyclic loop 5000 x 20ms = 100s */
for (int i = 0; i < 5000; i++)
{
printf("Processdata cycle %5d , Wck %3d, DCtime %12" PRId64 ", dt %8" PRId64 ", O:",
cycle,
wkc,
ctx.DCtime,
timeerror);
size = group->Obytes < 8 ? group->Obytes : 8;
for (int j = 0; j < size; j++)
{
printf(" %2.2x", *(ctx.slavelist[0].outputs + j));
}
printf(" I:");
size = group->Ibytes < 8 ? group->Ibytes : 8;
for (int j = 0; j < size; j++)
{
printf(" %2.2x", *(ctx.slavelist[0].inputs + j));
}
printf("\r");
fflush(stdout);
/* Demonstrate SDO access from other threads */
uint32_t value = 0;
int size = sizeof(value);
if (sdoslave > 0)
{
value = 0;
int sdo_wkc = ecx_SDOread(&ctx, sdoslave, 0x1018, 0x02, FALSE, &size, &value, EC_TIMEOUTRXM);
(void)sdo_wkc;
}
osal_usleep(20000);
}
printf("\n");
dorun = 0;
inOP = FALSE;
}
/* Go to SAFE_OP */
printf("EtherCAT to SAFE_OP\n");
ctx.slavelist[0].state = EC_STATE_SAFE_OP;
ecx_writestate(&ctx, 0);
ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
/* Go to INIT state */
printf("EtherCAT to INIT\n");
ctx.slavelist[0].state = EC_STATE_INIT;
ecx_writestate(&ctx, 0);
@@ -241,22 +312,27 @@ void ethercatstartup(char *ifname)
int main(int argc, char *argv[])
{
printf("SOEM (Simple Open EtherCAT Master)\nCoEtest\n");
printf("SOEM (Simple Open EtherCAT Master)\nec_sample\n");
if (argc > 2)
cycletime = atoi(argv[2]) * 1000;
if (argc > 1)
{
/* create thread to handle slave error handling in OP */
/* create process data thread */
osal_thread_create_rt(&threadrt, 128000, &ecatthread, NULL);
/* create thread to handle slave error handling in OP */
osal_thread_create(&thread1, 128000, &ecatcheck, NULL);
/* start cyclic part */
ethercatstartup(argv[1]);
/* bringup network */
ecatbringup(argv[1]);
}
else
{
ec_adaptert *adapter = NULL;
ec_adaptert *head = NULL;
printf("Usage: coetest ifname1\nifname = eth0 for example\n");
printf("Usage: ec_sample ifname1 [cycletime]\n");
printf("ifname = eth0 for example\n");
printf("cycletime in us\n");
printf("\nAvailable adapters:\n");
head = adapter = ec_find_adapters();

3
samples/red_test/CMakeLists.txt
View File

@@ -1,3 +0,0 @@
add_executable(red_test red_test.c)
target_link_libraries(red_test soem)
install(TARGETS red_test DESTINATION bin)

356
samples/red_test/red_test.c
View File

@@ -1,356 +0,0 @@
/** \file
* \brief Example code for Simple Open EtherCAT master
*
* Usage : red_test [ifname1] [ifname2] [cycletime]
* ifname is NIC interface, f.e. eth0
* cycletime in us, f.e. 500
*
* This is a redundancy test.
*
* (c)Arthur Ketels 2008
*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <inttypes.h>
#include "soem/soem.h"
#define EC_TIMEOUTMON 500
#define NSEC_PER_SEC 1000000000
static uint8 IOmap[4096];
static OSAL_THREAD_HANDLE thread1;
static OSAL_THREAD_HANDLE thread2;
static int expectedWKC;
static boolean needlf;
static volatile int wkc;
static boolean inOP;
static uint8 currentgroup = 0;
static uint8 *digout = NULL;
static int dorun = 0;
static int64 toff, gl_delta;
static ecx_contextt ctx;
void redtest(char *ifname, char *ifname2)
{
int cnt, i, j, oloop, iloop;
printf("Starting Redundant test\n");
/* initialise SOEM, bind socket to ifname */
(void)ifname2;
// if (ecx_init_redundant(&ctx, ifname, ifname2))
if (ecx_init(&ctx, ifname))
{
printf("ecx_init on %s succeeded.\n", ifname);
/* find and auto-config slaves */
if (ecx_config_init(&ctx) > 0)
{
ec_groupt *group = &ctx.grouplist[0];
ecx_config_map_group(&ctx, IOmap, 0);
printf("%d slaves found and configured.\n", ctx.slavecount);
/* wait for all slaves to reach SAFE_OP state */
ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);
ecx_configdc(&ctx);
/* read indevidual slave state and store in ctx.slavelist[] */
ecx_readstate(&ctx);
for (cnt = 1; cnt <= ctx.slavecount; cnt++)
{
printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
cnt, ctx.slavelist[cnt].name, ctx.slavelist[cnt].Obits, ctx.slavelist[cnt].Ibits,
ctx.slavelist[cnt].state, (int)ctx.slavelist[cnt].pdelay, ctx.slavelist[cnt].hasdc);
printf(" Out:%p,%4d In:%p,%4d\n",
ctx.slavelist[cnt].outputs, ctx.slavelist[cnt].Obytes, ctx.slavelist[cnt].inputs, ctx.slavelist[cnt].Ibytes);
/* check for EL2004 or EL2008 */
if (!digout && ((ctx.slavelist[cnt].eep_id == 0x0af83052) || (ctx.slavelist[cnt].eep_id == 0x07d83052)))
{
digout = ctx.slavelist[cnt].outputs;
}
}
expectedWKC = (group->outputsWKC * 2) + group->inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
printf("Request operational state for all slaves\n");
ctx.slavelist[0].state = EC_STATE_OPERATIONAL;
/* request OP state for all slaves */
ecx_writestate(&ctx, 0);
/* activate cyclic process data */
dorun = 1;
/* wait for all slaves to reach OP state */
ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE);
oloop = group->Obytes;
if (oloop > 8) oloop = 8;
iloop = group->Ibytes;
if (iloop > 8) iloop = 8;
if (ctx.slavelist[0].state == EC_STATE_OPERATIONAL)
{
printf("Operational state reached for all slaves.\n");
inOP = TRUE;
/* acyclic loop 5000 x 20ms = 10s */
for (i = 1; i <= 5000; i++)
{
printf("Processdata cycle %5d , Wck %3d, DCtime %12" PRId64 ", dt %12" PRId64 ", O:",
dorun, wkc, ctx.DCtime, gl_delta);
for (j = 0; j < oloop; j++)
{
printf(" %2.2x", *(ctx.slavelist[0].outputs + j));
}
printf(" I:");
for (j = 0; j < iloop; j++)
{
printf(" %2.2x", *(ctx.slavelist[0].inputs + j));
}
printf("\r");
fflush(stdout);
osal_usleep(20000);
}
dorun = 0;
inOP = FALSE;
}
else
{
printf("Not all slaves reached operational state.\n");
ecx_readstate(&ctx);
for (i = 1; i <= ctx.slavecount; i++)
{
ec_slavet *slave = &ctx.slavelist[i];
if (slave->state != EC_STATE_OPERATIONAL)
{
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
i, slave->state, slave->ALstatuscode, ec_ALstatuscode2string(slave->ALstatuscode));
}
}
}
printf("Request safe operational state for all slaves\n");
ctx.slavelist[0].state = EC_STATE_SAFE_OP;
/* request SAFE_OP state for all slaves */
ecx_writestate(&ctx, 0);
}
else
{
printf("No slaves found!\n");
}
printf("End redundant test, close socket\n");
/* stop SOEM, close socket */
ecx_close(&ctx);
}
else
{
printf("No socket connection on %s\nExcecute as root\n", ifname);
}
}
/* add ns to ec_timet */
void add_time_ns(ec_timet *ts, int64 addtime)
{
ec_timet addts;
addts.tv_nsec = addtime % NSEC_PER_SEC;
addts.tv_sec = (addtime - addts.tv_nsec) / NSEC_PER_SEC;
osal_timespecadd(ts, &addts, ts);
}
/* PI calculation to get linux time synced to DC time */
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
{
static int64 integral = 0;
int64 delta;
/* set linux sync point 50us later than DC sync, just as example */
delta = (reftime - 50000) % cycletime;
if (delta > (cycletime / 2))
{
delta = delta - cycletime;
}
if (delta > 0)
{
integral++;
}
if (delta < 0)
{
integral--;
}
*offsettime = -(delta / 100) - (integral / 20);
gl_delta = delta;
}
/* RT EtherCAT thread */
OSAL_THREAD_FUNC_RT ecatthread(void *ptr)
{
ec_timet ts;
int ht;
int64 cycletime;
osal_get_monotonic_time(&ts);
ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
ts.tv_nsec = ht * 1000000;
if (ts.tv_nsec >= NSEC_PER_SEC)
{
ts.tv_sec++;
ts.tv_nsec -= NSEC_PER_SEC;
}
cycletime = *(int *)ptr * 1000; /* cycletime in ns */
toff = 0;
dorun = 0;
ecx_send_processdata(&ctx);
while (1)
{
/* calculate next cycle start */
add_time_ns(&ts, cycletime + toff);
/* wait to cycle start */
osal_monotonic_sleep(&ts);
if (dorun > 0)
{
wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET);
dorun++;
/* if we have some digital output, cycle */
if (digout) *digout = (uint8)((dorun / 16) & 0xff);
if (ctx.slavelist[0].hasdc)
{
/* calculate toff to get linux time and DC synced */
ec_sync(ctx.DCtime, cycletime, &toff);
}
ecx_send_processdata(&ctx);
}
}
}
OSAL_THREAD_FUNC ecatcheck(void *ptr)
{
int slaveix;
(void)ptr; /* Not used */
while (1)
{
if (inOP && ((wkc < expectedWKC) || ctx.grouplist[currentgroup].docheckstate))
{
if (needlf)
{
needlf = FALSE;
printf("\n");
}
/* one or more slaves are not responding */
ctx.grouplist[currentgroup].docheckstate = FALSE;
ecx_readstate(&ctx);
for (slaveix = 1; slaveix <= ctx.slavecount; slaveix++)
{
ec_slavet *slave = &ctx.slavelist[slaveix];
if ((slave->group == currentgroup) && (slave->state != EC_STATE_OPERATIONAL))
{
ctx.grouplist[currentgroup].docheckstate = TRUE;
if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
{
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slaveix);
slave->state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
ecx_writestate(&ctx, slaveix);
}
else if (slave->state == EC_STATE_SAFE_OP)
{
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slaveix);
slave->state = EC_STATE_OPERATIONAL;
if (slave->mbxhandlerstate == ECT_MBXH_LOST) slave->mbxhandlerstate = ECT_MBXH_CYCLIC;
ecx_writestate(&ctx, slaveix);
}
else if (slave->state > EC_STATE_NONE)
{
if (ecx_reconfig_slave(&ctx, slaveix, EC_TIMEOUTMON) >= EC_STATE_PRE_OP)
{
slave->islost = FALSE;
printf("MESSAGE : slave %d reconfigured\n", slaveix);
}
}
else if (!slave->islost)
{
/* re-check state */
ecx_statecheck(&ctx, slaveix, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
if (slave->state == EC_STATE_NONE)
{
slave->islost = TRUE;
slave->mbxhandlerstate = ECT_MBXH_LOST;
/* zero input data for this slave */
if (slave->Ibytes)
{
memset(slave->inputs, 0x00, slave->Ibytes);
printf("zero inputs %p %d\n\r", slave->inputs, slave->Ibytes);
}
printf("ERROR : slave %d lost\n", slaveix);
}
}
}
if (slave->islost)
{
if (slave->state <= EC_STATE_INIT)
{
if (ecx_recover_slave(&ctx, slaveix, EC_TIMEOUTMON))
{
slave->islost = FALSE;
printf("MESSAGE : slave %d recovered\n", slaveix);
}
}
else
{
slave->islost = FALSE;
printf("MESSAGE : slave %d found\n", slaveix);
}
}
}
if (!ctx.grouplist[currentgroup].docheckstate)
printf("OK : all slaves resumed OPERATIONAL.\n");
}
osal_usleep(10000);
}
}
int main(int argc, char *argv[])
{
int ctime;
printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n");
if (argc > 3)
{
dorun = 0;
ctime = atoi(argv[3]);
/* create RT thread */
osal_thread_create_rt(&thread1, 128000, &ecatthread, (void *)&ctime);
/* create thread to handle slave error handling in OP */
osal_thread_create(&thread2, 128000, &ecatcheck, NULL);
/* start acyclic part */
redtest(argv[1], argv[2]);
}
else
{
ec_adaptert *adapter = NULL;
ec_adaptert *head = NULL;
printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n");
printf("\nAvailable adapters:\n");
head = adapter = ec_find_adapters();
while (adapter != NULL)
{
printf(" - %s (%s)\n", adapter->name, adapter->desc);
adapter = adapter->next;
}
ec_free_adapters(head);
}
printf("End program\n");
return (0);
}

3
samples/simple_test/CMakeLists.txt
View File

@@ -1,3 +0,0 @@
add_executable(simple_test simple_test.c)
target_link_libraries(simple_test soem)
install(TARGETS simple_test DESTINATION bin)

262
samples/simple_test/simple_test.c
View File

@@ -1,262 +0,0 @@
/** \file
* \brief Example code for Simple Open EtherCAT master
*
* Usage : simple_test [ifname1]
* ifname is NIC interface, f.e. eth0
*
* This is a minimal test.
*
* (c)Arthur Ketels 2010 - 2011
*/
#include <stdio.h>
#include <string.h>
#include <inttypes.h>
#include "soem/soem.h"
#define EC_TIMEOUTMON 500
static uint8 IOmap[4096];
static OSAL_THREAD_HANDLE thread1;
static int expectedWKC;
static boolean needlf;
static volatile int wkc;
static boolean inOP;
static uint8 currentgroup = 0;
static ecx_contextt ctx;
void simpletest(char *ifname)
{
int i, j, oloop, iloop, chk;
needlf = FALSE;
inOP = FALSE;
printf("Starting simple test\n");
/* initialise SOEM, bind socket to ifname */
if (ecx_init(&ctx, ifname))
{
printf("ecx_init on %s succeeded.\n", ifname);
/* find and auto-config slaves */
if (ecx_config_init(&ctx) > 0)
{
ec_groupt *group = &ctx.grouplist[0];
ecx_config_map_group(&ctx, IOmap, 0);
ecx_configdc(&ctx);
printf("%d slaves found and configured.\n", ctx.slavecount);
/* wait for all slaves to reach SAFE_OP state */
ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
oloop = group->Obytes;
if (oloop > 8) oloop = 8;
iloop = group->Ibytes;
if (iloop > 8) iloop = 8;
printf("segments : %d : %d %d %d %d\n", group->nsegments, group->IOsegment[0], group->IOsegment[1], group->IOsegment[2], group->IOsegment[3]);
printf("Request operational state for all slaves\n");
expectedWKC = (group->outputsWKC * 2) + group->inputsWKC;
printf("Calculated workcounter %d\n", expectedWKC);
ctx.slavelist[0].state = EC_STATE_OPERATIONAL;
/* send one valid process data to make outputs in slaves happy*/
ecx_send_processdata(&ctx);
ecx_receive_processdata(&ctx, EC_TIMEOUTRET);
/* request OP state for all slaves */
ecx_writestate(&ctx, 0);
chk = 200;
/* wait for all slaves to reach OP state */
do
{
ecx_send_processdata(&ctx);
ecx_receive_processdata(&ctx, EC_TIMEOUTRET);
ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, 50000);
} while (chk-- && (ctx.slavelist[0].state != EC_STATE_OPERATIONAL));
if (ctx.slavelist[0].state == EC_STATE_OPERATIONAL)
{
printf("Operational state reached for all slaves.\n");
inOP = TRUE;
/* cyclic loop */
for (i = 1; i <= 10000; i++)
{
ecx_send_processdata(&ctx);
wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET);
if (wkc >= expectedWKC)
{
printf("Processdata cycle %4d, WKC %d , O:", i, wkc);
for (j = 0; j < oloop; j++)
{
printf(" %2.2x", *(ctx.slavelist[0].outputs + j));
}
printf(" I:");
for (j = 0; j < iloop; j++)
{
printf(" %2.2x", *(ctx.slavelist[0].inputs + j));
}
printf(" T:%" PRId64 "\r", ctx.DCtime);
needlf = TRUE;
}
osal_usleep(5000);
}
inOP = FALSE;
}
else
{
printf("Not all slaves reached operational state.\n");
ecx_readstate(&ctx);
for (i = 1; i <= ctx.slavecount; i++)
{
ec_slavet *slave = &ctx.slavelist[i];
if (slave->state != EC_STATE_OPERATIONAL)
{
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
i, slave->state, slave->ALstatuscode, ec_ALstatuscode2string(slave->ALstatuscode));
}
}
}
printf("\nRequest init state for all slaves\n");
ctx.slavelist[0].state = EC_STATE_INIT;
/* request INIT state for all slaves */
ecx_writestate(&ctx, 0);
}
else
{
printf("No slaves found!\n");
}
printf("End simple test, close socket\n");
/* stop SOEM, close socket */
ecx_close(&ctx);
}
else
{
printf("No socket connection on %s\nExecute as root\n", ifname);
}
}
OSAL_THREAD_FUNC ecatcheck(void *ptr)
{
int slaveix;
(void)ptr; /* Not used */
while (1)
{
if (inOP && ((wkc < expectedWKC) || ctx.grouplist[currentgroup].docheckstate))
{
printf("check wkc=%d/%d docheckstate=%d!\n", wkc, expectedWKC, ctx.grouplist[currentgroup].docheckstate);
if (needlf)
{
needlf = FALSE;
printf("\n");
}
/* one or more slaves are not responding */
ctx.grouplist[currentgroup].docheckstate = FALSE;
ecx_readstate(&ctx);
for (slaveix = 1; slaveix <= ctx.slavecount; slaveix++)
{
ec_slavet *slave = &ctx.slavelist[slaveix];
if ((slave->group == currentgroup) && (slave->state != EC_STATE_OPERATIONAL))
{
ctx.grouplist[currentgroup].docheckstate = TRUE;
if (slave->state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
{
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slaveix);
slave->state = (EC_STATE_SAFE_OP + EC_STATE_ACK);
ecx_writestate(&ctx, slaveix);
}
else if (slave->state == EC_STATE_SAFE_OP)
{
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slaveix);
slave->state = EC_STATE_OPERATIONAL;
if (slave->mbxhandlerstate == ECT_MBXH_LOST) slave->mbxhandlerstate = ECT_MBXH_CYCLIC;
ecx_writestate(&ctx, slaveix);
}
else if (slave->state > EC_STATE_NONE)
{
if (ecx_reconfig_slave(&ctx, slaveix, EC_TIMEOUTMON) >= EC_STATE_PRE_OP)
{
slave->islost = FALSE;
printf("MESSAGE : slave %d reconfigured\n", slaveix);
}
}
else if (!slave->islost)
{
/* re-check state */
ecx_statecheck(&ctx, slaveix, EC_STATE_OPERATIONAL, EC_TIMEOUTRET);
if (slave->state == EC_STATE_NONE)
{
slave->islost = TRUE;
slave->mbxhandlerstate = ECT_MBXH_LOST;
/* zero input data for this slave */
if (slave->Ibytes)
{
memset(slave->inputs, 0x00, slave->Ibytes);
printf("zero inputs %p %d\n\r", slave->inputs, slave->Ibytes);
}
printf("ERROR : slave %d lost\n", slaveix);
}
}
}
if (slave->islost)
{
if (slave->state <= EC_STATE_INIT)
{
if (ecx_recover_slave(&ctx, slaveix, EC_TIMEOUTMON))
{
slave->islost = FALSE;
printf("MESSAGE : slave %d recovered\n", slaveix);
}
}
else
{
slave->islost = FALSE;
printf("MESSAGE : slave %d found\n", slaveix);
}
}
}
if (!ctx.grouplist[currentgroup].docheckstate)
printf("OK : all slaves resumed OPERATIONAL.\n");
}
osal_usleep(10000);
}
}
int main(int argc, char *argv[])
{
printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
if (argc > 1)
{
/* create thread to handle slave error handling in OP */
osal_thread_create(&thread1, 128000, &ecatcheck, NULL);
/* start cyclic part */
simpletest(argv[1]);
}
else
{
ec_adaptert *adapter = NULL;
ec_adaptert *head = NULL;
printf("Usage: simple_test ifname1\nifname = eth0 for example\n");
printf("\nAvailable adapters:\n");
head = adapter = ec_find_adapters();
while (adapter != NULL)
{
printf(" - %s (%s)\n", adapter->name, adapter->desc);
adapter = adapter->next;
}
ec_free_adapters(head);
}
printf("End program\n");
return (0);
}