linux/red_test/red_test.c
Go to the documentation of this file.
1 
13 #include <stdio.h>
14 #include <stdlib.h>
15 #include <sys/time.h>
16 #include <unistd.h>
17 #include <sched.h>
18 #include <string.h>
19 #include <sys/time.h>
20 #include <time.h>
21 #include <pthread.h>
22 #include <math.h>
23 
24 #include "ethercat.h"
25 
26 #define NSEC_PER_SEC 1000000000
27 #define EC_TIMEOUTMON 500
28 
29 struct sched_param schedp;
30 char IOmap[4096];
31 pthread_t thread1, thread2;
32 struct timeval tv, t1, t2;
33 int dorun = 0;
34 int deltat, tmax = 0;
36 int DCdiff;
37 int os;
42 boolean needlf;
43 volatile int wkc;
44 boolean inOP;
46 
47 
48 void redtest(char *ifname, char *ifname2)
49 {
50  int cnt, i, j, oloop, iloop;
51 
52  printf("Starting Redundant test\n");
53 
54  /* initialise SOEM, bind socket to ifname */
55 // if (ec_init_redundant(ifname, ifname2))
56  if (ec_init(ifname))
57  {
58  printf("ec_init on %s succeeded.\n",ifname);
59  /* find and auto-config slaves */
60  if ( ec_config(FALSE, &IOmap) > 0 )
61  {
62  printf("%d slaves found and configured.\n",ec_slavecount);
63  /* wait for all slaves to reach SAFE_OP state */
65 
66  /* configure DC options for every DC capable slave found in the list */
67  ec_configdc();
68 
69  /* read indevidual slave state and store in ec_slave[] */
70  ec_readstate();
71  for(cnt = 1; cnt <= ec_slavecount ; cnt++)
72  {
73  printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
74  cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
75  ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
76  printf(" Out:%8.8x,%4d In:%8.8x,%4d\n",
77  (int)ec_slave[cnt].outputs, ec_slave[cnt].Obytes, (int)ec_slave[cnt].inputs, ec_slave[cnt].Ibytes);
78  /* check for EL2004 or EL2008 */
79  if( !digout && ((ec_slave[cnt].eep_id == 0x0af83052) || (ec_slave[cnt].eep_id == 0x07d83052)))
80  {
81  digout = ec_slave[cnt].outputs;
82  }
83  }
84  expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
85  printf("Calculated workcounter %d\n", expectedWKC);
86 
87  printf("Request operational state for all slaves\n");
89  /* request OP state for all slaves */
90  ec_writestate(0);
91  /* activate cyclic process data */
92  dorun = 1;
93  /* wait for all slaves to reach OP state */
95  oloop = ec_slave[0].Obytes;
96  if ((oloop == 0) && (ec_slave[0].Obits > 0)) oloop = 1;
97  if (oloop > 8) oloop = 8;
98  iloop = ec_slave[0].Ibytes;
99  if ((iloop == 0) && (ec_slave[0].Ibits > 0)) iloop = 1;
100  if (iloop > 8) iloop = 8;
101  if (ec_slave[0].state == EC_STATE_OPERATIONAL )
102  {
103  printf("Operational state reached for all slaves.\n");
104  inOP = TRUE;
105  /* acyclic loop 5000 x 20ms = 10s */
106  for(i = 1; i <= 5000; i++)
107  {
108  printf("Processdata cycle %5d , Wck %3d, DCtime %12lld, dt %12lld, O:",
110  for(j = 0 ; j < oloop; j++)
111  {
112  printf(" %2.2x", *(ec_slave[0].outputs + j));
113  }
114  printf(" I:");
115  for(j = 0 ; j < iloop; j++)
116  {
117  printf(" %2.2x", *(ec_slave[0].inputs + j));
118  }
119  printf("\r");
120  fflush(stdout);
121  osal_usleep(20000);
122  }
123  dorun = 0;
124  inOP = FALSE;
125  }
126  else
127  {
128  printf("Not all slaves reached operational state.\n");
129  ec_readstate();
130  for(i = 1; i<=ec_slavecount ; i++)
131  {
132  if(ec_slave[i].state != EC_STATE_OPERATIONAL)
133  {
134  printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
135  i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
136  }
137  }
138  }
139  printf("Request safe operational state for all slaves\n");
141  /* request SAFE_OP state for all slaves */
142  ec_writestate(0);
143  }
144  else
145  {
146  printf("No slaves found!\n");
147  }
148  printf("End redundant test, close socket\n");
149  /* stop SOEM, close socket */
150  ec_close();
151  }
152  else
153  {
154  printf("No socket connection on %s\nExcecute as root\n",ifname);
155  }
156 }
157 
158 /* add ns to timespec */
159 void add_timespec(struct timespec *ts, int64 addtime)
160 {
161  int64 sec, nsec;
162 
163  nsec = addtime % NSEC_PER_SEC;
164  sec = (addtime - nsec) / NSEC_PER_SEC;
165  ts->tv_sec += sec;
166  ts->tv_nsec += nsec;
167  if ( ts->tv_nsec > NSEC_PER_SEC )
168  {
169  nsec = ts->tv_nsec % NSEC_PER_SEC;
170  ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
171  ts->tv_nsec = nsec;
172  }
173 }
174 
175 /* PI calculation to get linux time synced to DC time */
176 void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime)
177 {
178  static int64 integral = 0;
179  int64 delta;
180  /* set linux sync point 50us later than DC sync, just as example */
181  delta = (reftime - 50000) % cycletime;
182  if(delta> (cycletime / 2)) { delta= delta - cycletime; }
183  if(delta>0){ integral++; }
184  if(delta<0){ integral--; }
185  *offsettime = -(delta / 100) - (integral / 20);
186  gl_delta = delta;
187 }
188 
189 /* RT EtherCAT thread */
191 {
192  struct timespec ts, tleft;
193  int ht;
194  int64 cycletime;
195 
196  clock_gettime(CLOCK_MONOTONIC, &ts);
197  ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
198  ts.tv_nsec = ht * 1000000;
199  cycletime = *(int*)ptr * 1000; /* cycletime in ns */
200  toff = 0;
201  dorun = 0;
203  while(1)
204  {
205  /* calculate next cycle start */
206  add_timespec(&ts, cycletime + toff);
207  /* wait to cycle start */
208  clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, &tleft);
209  if (dorun>0)
210  {
212 
213  dorun++;
214  /* if we have some digital output, cycle */
215  if( digout ) *digout = (uint8) ((dorun / 16) & 0xff);
216 
217  if (ec_slave[0].hasdc)
218  {
219  /* calulate toff to get linux time and DC synced */
220  ec_sync(ec_DCtime, cycletime, &toff);
221  }
223  }
224  }
225 }
226 
228 {
229  int slave;
230 
231  while(1)
232  {
233  if( inOP && ((wkc < expectedWKC) || ec_group[currentgroup].docheckstate))
234  {
235  if (needlf)
236  {
237  needlf = FALSE;
238  printf("\n");
239  }
240  /* one ore more slaves are not responding */
242  ec_readstate();
243  for (slave = 1; slave <= ec_slavecount; slave++)
244  {
245  if ((ec_slave[slave].group == currentgroup) && (ec_slave[slave].state != EC_STATE_OPERATIONAL))
246  {
248  if (ec_slave[slave].state == (EC_STATE_SAFE_OP + EC_STATE_ERROR))
249  {
250  printf("ERROR : slave %d is in SAFE_OP + ERROR, attempting ack.\n", slave);
253  }
254  else if(ec_slave[slave].state == EC_STATE_SAFE_OP)
255  {
256  printf("WARNING : slave %d is in SAFE_OP, change to OPERATIONAL.\n", slave);
259  }
260  else if(ec_slave[slave].state > EC_STATE_NONE)
261  {
263  {
265  printf("MESSAGE : slave %d reconfigured\n",slave);
266  }
267  }
268  else if(!ec_slave[slave].islost)
269  {
270  /* re-check state */
272  if (ec_slave[slave].state == EC_STATE_NONE)
273  {
275  printf("ERROR : slave %d lost\n",slave);
276  }
277  }
278  }
279  if (ec_slave[slave].islost)
280  {
281  if(ec_slave[slave].state == EC_STATE_NONE)
282  {
284  {
286  printf("MESSAGE : slave %d recovered\n",slave);
287  }
288  }
289  else
290  {
292  printf("MESSAGE : slave %d found\n",slave);
293  }
294  }
295  }
296  if(!ec_group[currentgroup].docheckstate)
297  printf("OK : all slaves resumed OPERATIONAL.\n");
298  }
299  osal_usleep(10000);
300  }
301 }
302 
303 #define stack64k (64 * 1024)
304 
305 int main(int argc, char *argv[])
306 {
307  int ctime;
308 
309  printf("SOEM (Simple Open EtherCAT Master)\nRedundancy test\n");
310 
311  if (argc > 3)
312  {
313  dorun = 0;
314  ctime = atoi(argv[3]);
315 
316  /* create RT thread */
317  osal_thread_create_rt(&thread1, stack64k * 2, &ecatthread, (void*) &ctime);
318 
319  /* create thread to handle slave error handling in OP */
321 
322  /* start acyclic part */
323  redtest(argv[1],argv[2]);
324  }
325  else
326  {
327  printf("Usage: red_test ifname1 ifname2 cycletime\nifname = eth0 for example\ncycletime in us\n");
328  }
329 
330  printf("End program\n");
331 
332  return (0);
333 }
deltat
int deltat
Definition: linux/red_test/red_test.c:34
osal_thread_create_rt
int osal_thread_create_rt(void *thandle, int stacksize, void *func, void *param)
Definition: linux/osal.c:119
ec_configdc
boolean ec_configdc(void)
Definition: ethercatdc.c:444
os
int os
Definition: linux/red_test/red_test.c:37
slave
int slave
Definition: aliastool.c:44
schedp
struct sched_param schedp
Definition: linux/red_test/red_test.c:29
ec_sync
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
Definition: linux/red_test/red_test.c:176
digout
uint8 * digout
Definition: linux/red_test/red_test.c:40
ec_slave::islost
boolean islost
Definition: ethercatmain.h:229
ec_readstate
int ec_readstate(void)
Definition: ethercatmain.c:2138
integral
int64 integral
Definition: linux/ebox/ebox.c:77
ec_close
void ec_close(void)
Definition: ethercatmain.c:2048
add_timespec
void add_timespec(struct timespec *ts, int64 addtime)
Definition: linux/red_test/red_test.c:159
thread1
pthread_t thread1
Definition: linux/red_test/red_test.c:31
ec_slave::outputs
uint8 * outputs
Definition: ethercatmain.h:131
ec_send_processdata
int ec_send_processdata(void)
Definition: ethercatmain.c:2383
ethercat.h
Headerfile for all ethercat headers.
ec_init
int ec_init(const char *ifname)
Definition: ethercatmain.c:2029
ec_group
Definition: ethercatmain.h:239
ob
uint8 ob
Definition: linux/red_test/red_test.c:38
int64
int64_t int64
Definition: osal.h:31
IOmap
char IOmap[4096]
Definition: linux/red_test/red_test.c:30
ec_slave::Ibytes
uint32 Ibytes
Definition: ethercatmain.h:137
needlf
boolean needlf
Definition: linux/red_test/red_test.c:42
ec_group::outputsWKC
uint16 outputsWKC
Definition: ethercatmain.h:266
dorun
int dorun
Definition: linux/red_test/red_test.c:33
main
int main(int argc, char *argv[])
Definition: linux/red_test/red_test.c:305
ec_config
int ec_config(uint8 usetable, void *pIOmap)
Definition: ethercatconfig.c:1665
tmax
int tmax
Definition: linux/red_test/red_test.c:34
t2
struct timeval tv t1 t2
Definition: linux/red_test/red_test.c:32
ec_DCtime
int64 ec_DCtime
Definition: ethercatmain.c:95
uint8
uint8_t uint8
Definition: osal.h:28
EC_STATE_SAFE_OP
@ EC_STATE_SAFE_OP
Definition: ethercattype.h:166
OSAL_THREAD_FUNC_RT
#define OSAL_THREAD_FUNC_RT
Definition: intime/osal_defs.h:37
j
int j
Definition: linux/firm_update/firm_update.c:31
ec_slave::state
uint16 state
Definition: ethercatmain.h:109
expectedWKC
int expectedWKC
Definition: linux/red_test/red_test.c:41
EC_TIMEOUTRET
#define EC_TIMEOUTRET
Definition: ethercattype.h:64
ec_recover_slave
int ec_recover_slave(uint16 slave, int timeout)
Definition: ethercatconfig.c:1700
wkc
volatile int wkc
Definition: linux/red_test/red_test.c:43
ec_group::docheckstate
boolean docheckstate
Definition: ethercatmain.h:270
redtest
void redtest(char *ifname, char *ifname2)
Definition: linux/red_test/red_test.c:48
ec_reconfig_slave
int ec_reconfig_slave(uint16 slave, int timeout)
Definition: ethercatconfig.c:1712
uint16
uint16_t uint16
Definition: osal.h:29
toff
int64 toff
Definition: linux/red_test/red_test.c:35
OSAL_THREAD_FUNC
#define OSAL_THREAD_FUNC
Definition: intime/osal_defs.h:36
inOP
boolean inOP
Definition: linux/red_test/red_test.c:44
NSEC_PER_SEC
#define NSEC_PER_SEC
Definition: linux/red_test/red_test.c:26
ecatthread
OSAL_THREAD_FUNC_RT ecatthread(void *ptr)
Definition: linux/red_test/red_test.c:190
osal_usleep
int osal_usleep(uint32 usec)
Definition: erika/osal.c:22
TRUE
#define TRUE
Definition: osal.h:19
currentgroup
uint8 currentgroup
Definition: linux/red_test/red_test.c:45
ec_writestate
int ec_writestate(uint16 slave)
Definition: ethercatmain.c:2149
gl_delta
int64 gl_delta
Definition: linux/red_test/red_test.c:35
ec_slave
Definition: ethercatmain.h:106
stack64k
#define stack64k
Definition: linux/red_test/red_test.c:303
FALSE
#define FALSE
Definition: osal.h:22
EC_STATE_ERROR
@ EC_STATE_ERROR
Definition: ethercattype.h:171
EC_TIMEOUTSTATE
#define EC_TIMEOUTSTATE
Definition: ethercattype.h:76
ec_slavecount
int ec_slavecount
Definition: ethercatmain.c:69
DCdiff
int DCdiff
Definition: linux/red_test/red_test.c:36
ecatcheck
OSAL_THREAD_FUNC ecatcheck(void *ptr)
Definition: linux/red_test/red_test.c:227
ec_statecheck
uint16 ec_statecheck(uint16 slave, uint16 reqstate, int timeout)
Definition: ethercatmain.c:2162
ec_receive_processdata
int ec_receive_processdata(int timeout)
Definition: ethercatmain.c:2393
ec_ALstatuscode2string
char * ec_ALstatuscode2string(uint16 ALstatuscode)
Definition: ethercatprint.c:245
EC_TIMEOUTMON
#define EC_TIMEOUTMON
Definition: linux/red_test/red_test.c:27
ec_slave::Obytes
uint32 Obytes
Definition: ethercatmain.h:129
thread2
pthread_t thread2
Definition: linux/red_test/red_test.c:31
EC_STATE_NONE
@ EC_STATE_NONE
Definition: ethercattype.h:158
osal_thread_create
int osal_thread_create(void *thandle, int stacksize, void *func, void *param)
Definition: linux/osal.c:102
EC_STATE_OPERATIONAL
@ EC_STATE_OPERATIONAL
Definition: ethercattype.h:168
EC_STATE_ACK
@ EC_STATE_ACK
Definition: ethercattype.h:170
ob2
uint16 ob2
Definition: linux/red_test/red_test.c:39


soem
Author(s): Arthur Ketels and M.J.G. van den Molengraft
autogenerated on Wed Mar 2 2022 01:01:49