win32/ebox/ebox.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 
28 typedef struct PACKED
29 {
30  uint8 status;
31  uint8 counter;
32  uint8 din;
33  int32 ain[2];
34  uint32 tsain;
35  int32 enc[2];
36 } in_EBOXt;
37 
38 typedef struct PACKED
39 {
40  uint8 counter;
41  int16 stream[100];
43 
44 typedef struct PACKED
45 {
46  uint8 control;
47  uint8 dout;
48  int16 aout[2];
49  uint16 pwmout[2];
50 } out_EBOXt;
51 
52 typedef struct PACKED
53 {
54  uint8 control;
56 
57 // total samples to capture
58 #define MAXSTREAM 200000
59 // sample interval in ns, here 8us -> 125kHz
60 // maximum data rate for E/BOX v1.0.1 is around 150kHz
61 #define SYNC0TIME 8000
62 
63 struct sched_param schedp;
64 char IOmap[4096];
65 pthread_t thread1;
66 struct timeval tv,t1,t2;
67 int dorun = 0;
68 int deltat, tmax=0;
70 int DCdiff;
71 int os;
75 pthread_cond_t cond = PTHREAD_COND_INITIALIZER;
76 pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
81 double ain[2];
82 int ainc;
86 
87 int output_cvs(char *fname, int length)
88 {
89  FILE *fp;
90 
91  int i;
92 
93  fp = fopen(fname, "w");
94  if(fp == NULL)
95  return 0;
96  for (i = 0; i < length; i++)
97  {
98  fprintf(fp, "%d %d %d\n", i, stream1[i], stream2[i]);
99  }
100  fclose(fp);
101 
102  return 1;
103 }
104 
105 void eboxtest(char *ifname)
106 {
107  int cnt, i;
108 
109  printf("Starting E/BOX test\n");
110 
111  /* initialise SOEM, bind socket to ifname */
112  if (ec_init(ifname))
113  {
114  printf("ec_init on %s succeeded.\n",ifname);
115  /* find and auto-config slaves */
116  if ( ec_config_init(FALSE) > 0 )
117  {
118  printf("%d slaves found and configured.\n",ec_slavecount);
119 
120  // check if first slave is an E/BOX
121  if (( ec_slavecount >= 1 ) &&
122  (strcmp(ec_slave[1].name,"E/BOX") == 0))
123  {
124  // reprogram PDO mapping to set slave in stream mode
125  // this can only be done in pre-OP state
126  os=sizeof(ob2); ob2 = 0x1601;
127  ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
128  os=sizeof(ob2); ob2 = 0x1a01;
129  ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
130  }
131 
133 
134  ec_configdc();
135 
136  /* wait for all slaves to reach SAFE_OP state */
138 
139  /* configure DC options for every DC capable slave found in the list */
140  printf("DC capable : %d\n",ec_configdc());
141 
142  /* check configuration */
143  if (( ec_slavecount >= 1 ) &&
144  (strcmp(ec_slave[1].name,"E/BOX") == 0)
145  )
146  {
147  printf("E/BOX found.\n");
148 
149  /* connect struct pointers to slave I/O pointers */
150  in_EBOX = (in_EBOX_streamt*) ec_slave[1].inputs;
151  out_EBOX = (out_EBOX_streamt*) ec_slave[1].outputs;
152 
153  /* read indevidual slave state and store in ec_slave[] */
154  ec_readstate();
155  for(cnt = 1; cnt <= ec_slavecount ; cnt++)
156  {
157  printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
158  cnt, ec_slave[cnt].name, ec_slave[cnt].Obits, ec_slave[cnt].Ibits,
159  ec_slave[cnt].state, (int)ec_slave[cnt].pdelay, ec_slave[cnt].hasdc);
160  }
161  printf("Request operational state for all slaves\n");
162 
163  /* send one processdata cycle to init SM in slaves */
166 
168  /* request OP state for all slaves */
169  ec_writestate(0);
170  /* wait for all slaves to reach OP state */
172  if (ec_slave[0].state == EC_STATE_OPERATIONAL )
173  {
174  printf("Operational state reached for all slaves.\n");
175  ain[0] = 0;
176  ain[1] = 0;
177  ainc = 0;
178  dorun = 1;
179  usleep(100000); // wait for linux to sync on DC
180  ec_dcsync0(1, TRUE, SYNC0TIME, 0); // SYNC0 on slave 1
181  /* acyclic loop 20ms */
182  for(i = 1; i <= 200; i++)
183  {
184  /* read DC difference register for slave 2 */
185  // ec_FPRD(ec_slave[1].configadr, ECT_REG_DCSYSDIFF, sizeof(DCdiff), &DCdiff, EC_TIMEOUTRET);
186  // if(DCdiff<0) { DCdiff = - (int32)((uint32)DCdiff & 0x7ffffff); }
187  printf("PD cycle %5d DCtime %12lld Cnt:%3d Data: %6d %6d %6d %6d %6d %6d %6d %6d \n",
190  in_EBOX->stream[98], in_EBOX->stream[99]);
191  usleep(20000);
192  }
193  dorun = 0;
194  // printf("\nCnt %d : Ain0 = %f Ain2 = %f\n", ainc, ain[0] / ainc, ain[1] / ainc);
195  }
196  else
197  {
198  printf("Not all slaves reached operational state.\n");
199  }
200  }
201  else
202  {
203  printf("E/BOX not found in slave configuration.\n");
204  }
205  ec_dcsync0(1, FALSE, 8000, 0); // SYNC0 off
206  printf("Request safe operational state for all slaves\n");
208  /* request SAFE_OP state for all slaves */
209  ec_writestate(0);
210  /* wait for all slaves to reach state */
213  /* request SAFE_OP state for all slaves */
214  ec_writestate(0);
215  /* wait for all slaves to reach state */
217  if (( ec_slavecount >= 1 ) &&
218  (strcmp(ec_slave[1].name,"E/BOX") == 0))
219  {
220  // restore PDO to standard mode
221  // this can only be done is pre-op state
222  os=sizeof(ob2); ob2 = 0x1600;
223  ec_SDOwrite(1,0x1c12,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
224  os=sizeof(ob2); ob2 = 0x1a00;
225  ec_SDOwrite(1,0x1c13,01,FALSE,os,&ob2,EC_TIMEOUTRXM);
226  }
227  printf("Streampos %d\n", streampos);
228  output_cvs("stream.txt", streampos);
229  }
230  else
231  {
232  printf("No slaves found!\n");
233  }
234  printf("End E/BOX, close socket\n");
235  /* stop SOEM, close socket */
236  ec_close();
237  }
238  else
239  {
240  printf("No socket connection on %s\nExcecute as root\n",ifname);
241  }
242 }
243 
244 /* add ns to timespec */
245 void add_timespec(struct timespec *ts, int64 addtime)
246 {
247  int64 sec, nsec;
248 
249  nsec = addtime % NSEC_PER_SEC;
250  sec = (addtime - nsec) / NSEC_PER_SEC;
251  ts->tv_sec += sec;
252  ts->tv_nsec += nsec;
253  if ( ts->tv_nsec > NSEC_PER_SEC )
254  {
255  nsec = ts->tv_nsec % NSEC_PER_SEC;
256  ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
257  ts->tv_nsec = nsec;
258  }
259 }
260 
261 /* PI calculation to get linux time synced to DC time */
262 void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime)
263 {
264  int64 delta;
265  /* set linux sync point 50us later than DC sync, just as example */
266  delta = (reftime - 50000) % cycletime;
267  if(delta> (cycletime /2)) { delta= delta - cycletime; }
268  if(delta>0){ integral++; }
269  if(delta<0){ integral--; }
270  *offsettime = -(delta / 100) - (integral /20);
271 }
272 
273 /* RT EtherCAT thread */
274 void ecatthread( void *ptr )
275 {
276  struct timespec ts;
277  struct timeval tp;
278  int rc;
279  int ht;
280  int i;
281  int pcounter = 0;
282  int64 cycletime;
283 
284  rc = pthread_mutex_lock(&mutex);
285  rc = gettimeofday(&tp, NULL);
286 
287  /* Convert from timeval to timespec */
288  ts.tv_sec = tp.tv_sec;
289  ht = (tp.tv_usec / 1000) + 1; /* round to nearest ms */
290  ts.tv_nsec = ht * 1000000;
291  cycletime = *(int*)ptr * 1000; /* cycletime in ns */
292  toff = 0;
293  dorun = 0;
294  while(1)
295  {
296  /* calculate next cycle start */
297  add_timespec(&ts, cycletime + toff);
298  /* wait to cycle start */
299  rc = pthread_cond_timedwait(&cond, &mutex, &ts);
300  if (dorun>0)
301  {
302  rc = gettimeofday(&tp, NULL);
303 
305 
307 
308  cyclecount++;
309 
310 
311  if((in_EBOX->counter != pcounter) && (streampos < (MAXSTREAM - 1)))
312  {
313  // check if we have timing problems in master
314  // if so, overwrite stream data so it shows up clearly in plots.
315  if(in_EBOX->counter > (pcounter + 1))
316  {
317  for(i = 0 ; i < 50 ; i++)
318  {
319  stream1[streampos] = 20000;
320  stream2[streampos++] = -20000;
321  }
322  }
323  else
324  {
325  for(i = 0 ; i < 50 ; i++)
326  {
327  stream1[streampos] = in_EBOX->stream[i * 2];
328  stream2[streampos++] = in_EBOX->stream[(i * 2) + 1];
329  }
330  }
331  pcounter = in_EBOX->counter;
332  }
333 
334  /* calulate toff to get linux time and DC synced */
335  ec_sync(ec_DCtime, cycletime, &toff);
336  }
337  }
338 }
339 
340 int main(int argc, char *argv[])
341 {
342  int iret1;
343  int ctime;
344  struct sched_param param;
345  int policy = SCHED_OTHER;
346 
347  printf("SOEM (Simple Open EtherCAT Master)\nE/BOX test\n");
348 
349  memset(&schedp, 0, sizeof(schedp));
350  /* do not set priority above 49, otherwise sockets are starved */
351  schedp.sched_priority = 30;
352  sched_setscheduler(0, SCHED_FIFO, &schedp);
353 
354  do
355  {
356  usleep(1000);
357  }
358  while (dorun);
359 
360  if (argc > 1)
361  {
362  dorun = 1;
363  if( argc > 2)
364  ctime = atoi(argv[2]);
365  else
366  ctime = 1000; // 1ms cycle time
367  /* create RT thread */
368  iret1 = pthread_create( &thread1, NULL, (void *) &ecatthread, (void*) &ctime);
369  memset(&param, 0, sizeof(param));
370  /* give it higher priority */
371  param.sched_priority = 40;
372  iret1 = pthread_setschedparam(thread1, policy, &param);
373 
374  /* start acyclic part */
375  eboxtest(argv[1]);
376  }
377  else
378  {
379  printf("Usage: ebox ifname [cycletime]\nifname = eth0 for example\ncycletime in us\n");
380  }
381 
382  schedp.sched_priority = 0;
383  sched_setscheduler(0, SCHED_OTHER, &schedp);
384 
385  printf("End program\n");
386 
387  return (0);
388 }
ec_configdc
boolean ec_configdc(void)
Definition: ethercatdc.c:444
os
int os
Definition: win32/ebox/ebox.c:71
ecatthread
void ecatthread(void *ptr)
Definition: win32/ebox/ebox.c:274
ec_readstate
int ec_readstate(void)
Definition: ethercatmain.c:2138
ec_sync
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
Definition: win32/ebox/ebox.c:262
in_EBOXt
struct PACKED in_EBOXt
in_EBOX
in_EBOX_streamt * in_EBOX
Definition: win32/ebox/ebox.c:79
ec_close
void ec_close(void)
Definition: ethercatmain.c:2048
ec_send_processdata
int ec_send_processdata(void)
Definition: ethercatmain.c:2383
IOmap
char IOmap[4096]
Definition: win32/ebox/ebox.c:64
ob
uint32 ob
Definition: win32/ebox/ebox.c:72
uint32
uint32_t uint32
Definition: osal.h:30
ethercat.h
Headerfile for all ethercat headers.
ob3
uint8 ob3
Definition: win32/ebox/ebox.c:74
ec_init
int ec_init(const char *ifname)
Definition: ethercatmain.c:2029
EC_TIMEOUTRXM
#define EC_TIMEOUTRXM
Definition: ethercattype.h:74
int16
int16_t int16
Definition: osal.h:26
main
int main(int argc, char *argv[])
Definition: win32/ebox/ebox.c:340
int64
int64_t int64
Definition: osal.h:31
stream1
int16 stream1[MAXSTREAM]
Definition: win32/ebox/ebox.c:84
int32
int32_t int32
Definition: osal.h:27
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
streampos
int streampos
Definition: win32/ebox/ebox.c:83
PACKED::enc
int32 enc[2]
Definition: linux/ebox/ebox.c:35
MAXSTREAM
#define MAXSTREAM
Definition: win32/ebox/ebox.c:58
PACKED::pwmout
uint16 pwmout[2]
Definition: linux/ebox/ebox.c:49
output_cvs
int output_cvs(char *fname, int length)
Definition: win32/ebox/ebox.c:87
ec_slave::state
uint16 state
Definition: ethercatmain.h:109
add_timespec
void add_timespec(struct timespec *ts, int64 addtime)
Definition: win32/ebox/ebox.c:245
PACKED::aout
int16 aout[2]
Definition: linux/ebox/ebox.c:48
ec_config_init
int ec_config_init(uint8 usetable)
Definition: ethercatconfig.c:1606
EC_TIMEOUTRET
#define EC_TIMEOUTRET
Definition: ethercattype.h:64
PACKED
Definition: ethercatcoe.c:24
PACKED::tsain
uint32 tsain
Definition: linux/ebox/ebox.c:34
deltat
int deltat
Definition: win32/ebox/ebox.c:68
EC_STATE_PRE_OP
@ EC_STATE_PRE_OP
Definition: ethercattype.h:162
NSEC_PER_SEC
#define NSEC_PER_SEC
Definition: win32/ebox/ebox.c:26
integral
int64 integral
Definition: win32/ebox/ebox.c:77
PACKED::control
uint8 control
Definition: linux/ebox/ebox.c:46
out_EBOX
out_EBOX_streamt * out_EBOX
Definition: win32/ebox/ebox.c:80
uint16
uint16_t uint16
Definition: osal.h:29
schedp
struct sched_param schedp
Definition: win32/ebox/ebox.c:63
tmax
int tmax
Definition: win32/ebox/ebox.c:68
mutex
pthread_mutex_t mutex
Definition: win32/ebox/ebox.c:76
dorun
int dorun
Definition: win32/ebox/ebox.c:67
PACKED::counter
uint8 counter
Definition: linux/ebox/ebox.c:31
ec_SDOwrite
int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, boolean CA, int psize, void *p, int Timeout)
Definition: ethercatcoe.c:1367
out_EBOX_streamt
struct PACKED out_EBOX_streamt
ainc
int ainc
Definition: win32/ebox/ebox.c:82
TRUE
#define TRUE
Definition: osal.h:19
ec_writestate
int ec_writestate(uint16 slave)
Definition: ethercatmain.c:2149
out_EBOXt
struct PACKED out_EBOXt
ec_slave
Definition: ethercatmain.h:106
FALSE
#define FALSE
Definition: osal.h:22
PACKED::ain
int32 ain[2]
Definition: linux/ebox/ebox.c:33
cond
pthread_cond_t cond
Definition: win32/ebox/ebox.c:75
EC_TIMEOUTSTATE
#define EC_TIMEOUTSTATE
Definition: ethercattype.h:76
t2
struct timeval tv t1 t2
Definition: win32/ebox/ebox.c:66
PACKED::dout
uint8 dout
Definition: linux/ebox/ebox.c:47
cyclecount
uint32 cyclecount
Definition: win32/ebox/ebox.c:78
ob2
int16 ob2
Definition: win32/ebox/ebox.c:73
stream2
int16 stream2[MAXSTREAM]
Definition: win32/ebox/ebox.c:85
ec_slavecount
int ec_slavecount
Definition: ethercatmain.c:69
toff
int64 toff
Definition: win32/ebox/ebox.c:69
thread1
pthread_t thread1
Definition: win32/ebox/ebox.c:65
PACKED::status
uint8 status
Definition: linux/ebox/ebox.c:30
DCdiff
int DCdiff
Definition: win32/ebox/ebox.c:70
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
SYNC0TIME
#define SYNC0TIME
Definition: win32/ebox/ebox.c:61
gettimeofday
int gettimeofday(struct timeval *tp, void *tzp)
Definition: rtk/osal.c:50
ain
double ain[2]
Definition: win32/ebox/ebox.c:81
PACKED::stream
int16 stream[100]
Definition: linux/ebox/ebox.c:41
ec_dcsync0
void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
Definition: ethercatdc.c:434
ec_config_map
int ec_config_map(void *pIOmap)
Definition: ethercatconfig.c:1643
eboxtest
void eboxtest(char *ifname)
Definition: win32/ebox/ebox.c:105
PACKED::din
uint8 din
Definition: linux/ebox/ebox.c:32
EC_STATE_OPERATIONAL
@ EC_STATE_OPERATIONAL
Definition: ethercattype.h:168
in_EBOX_streamt
struct PACKED in_EBOX_streamt


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