-
Notifications
You must be signed in to change notification settings - Fork 668
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Added example with DC-sync #382
Open
stex2005
wants to merge
5
commits into
OpenEtherCATsociety:master
Choose a base branch
from
stex2005:master
base: master
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
Changes from all commits
Commits
Show all changes
5 commits
Select commit
Hold shift + click to select a range
File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,4 @@ | ||
set(SOURCES dcsync_test.c) | ||
add_executable(dcsync_test ${SOURCES}) | ||
target_link_libraries(dcsync_test soem) | ||
install(TARGETS dcsync_test DESTINATION bin) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,362 @@ | ||
/** \file | ||
* \brief Example code for Simple Open EtherCAT master with Distributed Clock Sync | ||
* | ||
* Usage : dcsync_test [ifname1] [cycletime] | ||
* ifname is NIC interface, f.e. eth0 | ||
* cycletime in us, f.e. 1000 | ||
* | ||
* This is a script based on redundancy test to run EtherCAT master with DC Sync. | ||
* | ||
* (c)Arthur Ketels 2008 | ||
* | ||
* modified by Stefano Dalla Gasperina 2020 | ||
*/ | ||
|
||
#include <stdio.h> | ||
#include <stdlib.h> | ||
#include <sys/time.h> | ||
#include <unistd.h> | ||
#include <sched.h> | ||
#include <string.h> | ||
#include <sys/time.h> | ||
#include <time.h> | ||
#include <pthread.h> | ||
#include <math.h> | ||
|
||
#include "ethercat.h" | ||
|
||
#define NSEC_PER_SEC 1000000000 | ||
#define EC_TIMEOUTMON 500 | ||
|
||
struct sched_param schedp; | ||
char IOmap[4096]; | ||
pthread_t thread1, thread2; | ||
struct timeval tv, t1, t2; | ||
int dorun = 0; | ||
int deltat, tmax = 0; | ||
int64 toff, gl_delta; | ||
int DCdiff; | ||
int os; | ||
uint8 ob; | ||
uint16 ob2; | ||
uint8 *digout = 0; | ||
int expectedWKC; | ||
boolean needlf; | ||
volatile int wkc; | ||
boolean inOP; | ||
uint8 currentgroup = 0; | ||
|
||
/* Slave Distributed Clock Configuration */ | ||
boolean dcsync_enable = TRUE; | ||
|
||
static int slave_dc_config(uint16 slave) | ||
{ | ||
// ec_dcsync0(slave, active, cycletime, calc and copy time) | ||
ec_dcsync0(slave, dcsync_enable, 1000000U, 1000U); | ||
printf("ec_dcsync0 called on slave %u\n",slave); | ||
return 0; | ||
} | ||
|
||
|
||
void redtest(char *ifname) | ||
{ | ||
int cnt, i, oloop, iloop; | ||
int j; | ||
|
||
printf("Starting DC-sync test\n"); | ||
|
||
/* initialise SOEM, bind socket to ifname */ | ||
if (ec_init(ifname)) | ||
{ | ||
printf("ec_init on %s succeeded.\n",ifname); | ||
/* find and auto-config slaves */ | ||
if (ec_config_init(FALSE) > 0) // == ec_config_init + ec_config_map | ||
{ | ||
printf("%d slaves found and configured.\n",ec_slavecount); | ||
|
||
// PO2SOconfig is for registering a hook function that will be called when the slave does the transition | ||
// between Pre-OP and Safe-OP. | ||
if ((ec_slavecount >= 1)) { | ||
for (cnt = 1; cnt <= ec_slavecount; cnt++) { | ||
printf("Found %s at position %d\n", ec_slave[cnt].name, cnt); | ||
ec_slave[cnt].PO2SOconfig = &slave_dc_config; | ||
} | ||
} | ||
|
||
/* Locate DC slaves, measure propagation delays. */ | ||
ec_configdc(); | ||
|
||
ec_config_map(&IOmap); | ||
|
||
/* wait for all slaves to reach SAFE_OP state */ | ||
ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE); | ||
|
||
/* read indevidual slave state and store in ec_slave[] */ | ||
ec_readstate(); | ||
for(cnt = 1; cnt <= ec_slavecount ; cnt++) | ||
{ | ||
/* BEGIN USER CODE */ | ||
|
||
printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n", | ||
cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits, | ||
ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc); | ||
|
||
/* END USER CODE */ | ||
} | ||
expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC; | ||
printf("Calculated workcounter %d\n", expectedWKC); | ||
|
||
printf("Request operational state for all slaves\n"); | ||
ec_slave[0].state = EC_STATE_OPERATIONAL; | ||
/* request OP state for all slaves */ | ||
ec_writestate(0); | ||
/* activate cyclic process data */ | ||
dorun = 1; | ||
/* wait for all slaves to reach OP state */ | ||
ec_statecheck(0, EC_STATE_OPERATIONAL, 5 * EC_TIMEOUTSTATE); | ||
oloop = ec_slave[0].Obytes; | ||
if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1; | ||
if (oloop > 8) oloop = 8; | ||
iloop = ec_slave[0].Ibytes; | ||
if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1; | ||
if (iloop > 8) iloop = 8; | ||
if (ec_slave[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++) | ||
{ | ||
/* BEGIN USER CODE */ | ||
|
||
printf("Processdata cycle %5d , Wck %3d, DCtime %12ld, dt %12ld, O:", | ||
dorun, wkc , ec_DCtime, gl_delta); | ||
for(j = 0 ; j < oloop; j++) | ||
{ | ||
printf(" %2.2x", *(ec_slave[0].outputs + j)); | ||
} | ||
printf(" I:"); | ||
for(j = 0 ; j < iloop; j++) | ||
{ | ||
printf(" %2.2x", *(ec_slave[0].inputs + j)); | ||
} | ||
printf("\r"); | ||
fflush(stdout); | ||
osal_usleep(20000); | ||
|
||
/* END USER CODE */ | ||
|
||
} | ||
dorun = 0; | ||
inOP = FALSE; | ||
} | ||
else | ||
{ | ||
printf("Not all slaves reached operational state.\n"); | ||
ec_readstate(); | ||
for(i = 1; i<=ec_slavecount ; i++) | ||
{ | ||
if(ec_slave[i].state != EC_STATE_OPERATIONAL) | ||
{ | ||
printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n", | ||
i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode)); | ||
} | ||
} | ||
} | ||
printf("Request safe operational state for all slaves\n"); | ||
ec_slave[0].state = EC_STATE_SAFE_OP; | ||
/* request SAFE_OP state for all slaves */ | ||
ec_writestate(0); | ||
} | ||
else | ||
{ | ||
printf("No slaves found!\n"); | ||
} | ||
printf("End DC-sync test, close socket\n"); | ||
/* stop SOEM, close socket */ | ||
ec_close(); | ||
} | ||
else | ||
{ | ||
printf("No socket connection on %s\nExcecute as root\n",ifname); | ||
} | ||
} | ||
|
||
/* add ns to timespec */ | ||
void add_timespec(struct timespec *ts, int64 addtime) | ||
{ | ||
int64 sec, nsec; | ||
|
||
nsec = addtime % NSEC_PER_SEC; | ||
sec = (addtime - nsec) / NSEC_PER_SEC; | ||
ts->tv_sec += sec; | ||
ts->tv_nsec += nsec; | ||
if ( ts->tv_nsec > NSEC_PER_SEC ) | ||
{ | ||
nsec = ts->tv_nsec % NSEC_PER_SEC; | ||
ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC; | ||
ts->tv_nsec = nsec; | ||
} | ||
} | ||
|
||
/* 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) | ||
{ | ||
struct timespec ts, tleft; | ||
int ht; | ||
int64 cycletime; | ||
|
||
clock_gettime(CLOCK_MONOTONIC, &ts); | ||
ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */ | ||
ts.tv_nsec = ht * 1000000; | ||
cycletime = *(int*)ptr * 1000; /* cycletime in ns */ | ||
toff = 0; | ||
dorun = 0; | ||
ec_send_processdata(); | ||
while(1) | ||
{ | ||
/* calculate next cycle start */ | ||
add_timespec(&ts, cycletime + toff); | ||
/* wait to cycle start */ | ||
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft); | ||
if (dorun>0) | ||
{ | ||
/* BEGIN USER CODE */ | ||
wkc = ec_receive_processdata(EC_TIMEOUTRET); | ||
|
||
dorun++; | ||
/* if we have some digital output, cycle */ | ||
if( digout ) *digout = (uint8) ((dorun / 16) & 0xff); | ||
|
||
if (ec_slave[0].hasdc) | ||
{ | ||
/* calulate toff to get linux time and DC synced */ | ||
ec_sync(ec_DCtime, cycletime, &toff); | ||
} | ||
ec_send_processdata(); | ||
/* END USER CODE */ | ||
} | ||
} | ||
} | ||
|
||
OSAL_THREAD_FUNC ecatcheck() | ||
{ | ||
int slave; | ||
|
||
while(1) | ||
{ | ||
if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate)) | ||
{ | ||
if (needlf) | ||
{ | ||
needlf = FALSE; | ||
printf("\n"); | ||
} | ||
/* one ore more slaves are not responding */ | ||
ec_group[currentgroup].docheckstate = FALSE; | ||
ec_readstate(); | ||
for (slave = 1; slave <= ec_slavecount; slave++) | ||
{ | ||
if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL)) | ||
{ | ||
ec_group[currentgroup].docheckstate = TRUE; | ||
if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR)) | ||
{ | ||
printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave); | ||
ec_slave[slave].state = (EC_STATE_SAFE_OP + EC_STATE_ACK); | ||
ec_writestate(slave); | ||
} | ||
else if(ec_slave[slave].state == EC_STATE_SAFE_OP) | ||
{ | ||
printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave); | ||
ec_slave[slave].state = EC_STATE_OPERATIONAL; | ||
ec_writestate(slave); | ||
} | ||
else if(ec_slave[slave].state > EC_STATE_NONE) | ||
{ | ||
if (ec_reconfig_slave(slave, EC_TIMEOUTMON)) | ||
{ | ||
ec_slave[slave].islost = FALSE; | ||
printf("MESSAGE : slave %d reconfigured\n",slave); | ||
} | ||
} | ||
else if(!ec_slave[slave].islost) | ||
{ | ||
/* re-check state */ | ||
ec_statecheck(slave, EC_STATE_OPERATIONAL, EC_TIMEOUTRET); | ||
if (ec_slave[slave].state == EC_STATE_NONE) | ||
{ | ||
ec_slave[slave].islost = TRUE; | ||
printf("ERROR : slave %d lost\n",slave); | ||
} | ||
} | ||
} | ||
if (ec_slave[slave].islost) | ||
{ | ||
if(ec_slave[slave].state == EC_STATE_NONE) | ||
{ | ||
if (ec_recover_slave(slave, EC_TIMEOUTMON)) | ||
{ | ||
ec_slave[slave].islost = FALSE; | ||
printf("MESSAGE : slave %d recovered\n",slave); | ||
} | ||
} | ||
else | ||
{ | ||
ec_slave[slave].islost = FALSE; | ||
printf("MESSAGE : slave %d found\n",slave); | ||
} | ||
} | ||
} | ||
if(!ec_group[currentgroup].docheckstate) | ||
printf("OK : all slaves resumed OPERATIONAL.\n"); | ||
} | ||
osal_usleep(10000); | ||
} | ||
} | ||
|
||
#define stack64k (64 * 1024) | ||
|
||
int main(int argc, char *argv[]) | ||
{ | ||
int ctime; | ||
|
||
printf("SOEM (Simple Open EtherCAT Master)\nDC-sync test\n"); | ||
|
||
if (argc > 2) | ||
{ | ||
dorun = 0; | ||
ctime = atoi(argv[2]); | ||
|
||
/* create RT thread */ | ||
osal_thread_create_rt(&thread1, stack64k * 2, &ecatthread, (void*) &ctime); | ||
|
||
/* create thread to handle slave error handling in OP */ | ||
osal_thread_create(&thread2, stack64k * 4, &ecatcheck, NULL); | ||
|
||
/* start acyclic part */ | ||
redtest(argv[1]); | ||
} | ||
else | ||
{ | ||
printf("Usage: dcsync_test ifname cycletime\nifname = eth0 for example\ncycletime in us\n"); | ||
} | ||
|
||
printf("End program\n"); | ||
|
||
return (0); | ||
} |
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This can be deleted to pass checks