linux/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 {
33  int32 ain[2];
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 {
48  int16 aout[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",
188  cyclecount, ec_DCtime, in_EBOX->counter, in_EBOX->stream[0], in_EBOX->stream[1],
189  in_EBOX->stream[2], in_EBOX->stream[3], in_EBOX->stream[4], in_EBOX->stream[5],
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 ht;
279  int i;
280  int pcounter = 0;
281  int64 cycletime;
282 
283  pthread_mutex_lock(&mutex);
284  gettimeofday(&tp, NULL);
285 
286  /* Convert from timeval to timespec */
287  ts.tv_sec = tp.tv_sec;
288  ht = (tp.tv_usec / 1000) + 1; /* round to nearest ms */
289  ts.tv_nsec = ht * 1000000;
290  cycletime = *(int*)ptr * 1000; /* cycletime in ns */
291  toff = 0;
292  dorun = 0;
293  while(1)
294  {
295  /* calculate next cycle start */
296  add_timespec(&ts, cycletime + toff);
297  /* wait to cycle start */
298  pthread_cond_timedwait(&cond, &mutex, &ts);
299  if (dorun>0)
300  {
301  gettimeofday(&tp, NULL);
302 
304 
306 
307  cyclecount++;
308 
309 
310  if((in_EBOX->counter != pcounter) && (streampos < (MAXSTREAM - 1)))
311  {
312  // check if we have timing problems in master
313  // if so, overwrite stream data so it shows up clearly in plots.
314  if(in_EBOX->counter > (pcounter + 1))
315  {
316  for(i = 0 ; i < 50 ; i++)
317  {
318  stream1[streampos] = 20000;
319  stream2[streampos++] = -20000;
320  }
321  }
322  else
323  {
324  for(i = 0 ; i < 50 ; i++)
325  {
326  stream1[streampos] = in_EBOX->stream[i * 2];
327  stream2[streampos++] = in_EBOX->stream[(i * 2) + 1];
328  }
329  }
330  pcounter = in_EBOX->counter;
331  }
332 
333  /* calulate toff to get linux time and DC synced */
334  ec_sync(ec_DCtime, cycletime, &toff);
335  }
336  }
337 }
338 
339 int main(int argc, char *argv[])
340 {
341  int ctime;
342  struct sched_param param;
343  int policy = SCHED_OTHER;
344 
345  printf("SOEM (Simple Open EtherCAT Master)\nE/BOX test\n");
346 
347  memset(&schedp, 0, sizeof(schedp));
348  /* do not set priority above 49, otherwise sockets are starved */
349  schedp.sched_priority = 30;
350  sched_setscheduler(0, SCHED_FIFO, &schedp);
351 
352  do
353  {
354  usleep(1000);
355  }
356  while (dorun);
357 
358  if (argc > 1)
359  {
360  dorun = 1;
361  if( argc > 2)
362  ctime = atoi(argv[2]);
363  else
364  ctime = 1000; // 1ms cycle time
365  /* create RT thread */
366  pthread_create( &thread1, NULL, (void *) &ecatthread, (void*) &ctime);
367  memset(&param, 0, sizeof(param));
368  /* give it higher priority */
369  param.sched_priority = 40;
370  pthread_setschedparam(thread1, policy, &param);
371 
372  /* start acyclic part */
373  eboxtest(argv[1]);
374  }
375  else
376  {
377  printf("Usage: ebox ifname [cycletime]\nifname = eth0 for example\ncycletime in us\n");
378  }
379 
380  schedp.sched_priority = 0;
381  sched_setscheduler(0, SCHED_OTHER, &schedp);
382 
383  printf("End program\n");
384 
385  return (0);
386 }
int dorun
void add_timespec(struct timespec *ts, int64 addtime)
pthread_t thread1
int ec_readstate(void)
int16 aout[2]
#define SYNC0TIME
int ec_send_processdata(void)
void ec_dcsync0(uint16 slave, boolean act, uint32 CyclTime, int32 CyclShift)
Definition: ethercatdc.c:434
#define EC_TIMEOUTSTATE
Definition: ethercattype.h:76
uint8 control
uint8_t uint8
Definition: osal.h:28
void ecatthread(void *ptr)
Headerfile for all ethercat headers.
int16 stream[100]
int ec_receive_processdata(int timeout)
uint32 ob
struct PACKED out_EBOX_streamt
uint16 state
Definition: ethercatmain.h:109
int ec_init(const char *ifname)
char IOmap[4096]
int16 stream1[MAXSTREAM]
int streampos
uint16_t uint16
Definition: osal.h:29
uint32 length
Definition: ethercatsoe.h:96
int64 ec_DCtime
Definition: ethercatmain.c:95
void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
struct PACKED in_EBOXt
#define EC_TIMEOUTRXM
Definition: ethercattype.h:74
pthread_cond_t cond
in_EBOX_streamt * in_EBOX
#define TRUE
Definition: osal.h:19
int ec_config_map(void *pIOmap)
struct PACKED in_EBOX_streamt
struct timeval tv t1 t2
uint8 ob3
int deltat
int32 ain[2]
struct PACKED out_EBOXt
pthread_mutex_t mutex
uint8 din
uint8 counter
uint8 dout
int ec_slavecount
Definition: ethercatmain.c:69
int tmax
int64_t int64
Definition: osal.h:31
#define FALSE
Definition: osal.h:22
struct sched_param schedp
int32_t int32
Definition: osal.h:27
void eboxtest(char *ifname)
#define MAXSTREAM
#define EC_TIMEOUTRET
Definition: ethercattype.h:64
int ec_writestate(uint16 slave)
int64 integral
int DCdiff
boolean ec_configdc(void)
Definition: ethercatdc.c:444
int16 ob2
int64 toff
uint32 cyclecount
int main(int argc, char *argv[])
uint32_t uint32
Definition: osal.h:30
int gettimeofday(struct timeval *tp, void *tzp)
Definition: rtk/osal.c:50
int16_t int16
Definition: osal.h:26
uint16 pwmout[2]
int32 enc[2]
char name[EC_SOE_MAXNAME]
Definition: ethercatsoe.h:42
int16 stream2[MAXSTREAM]
int os
void ec_close(void)
int output_cvs(char *fname, int length)
uint16 ec_statecheck(uint16 slave, uint16 reqstate, int timeout)
uint32 tsain
int ainc
int ec_SDOwrite(uint16 Slave, uint16 Index, uint8 SubIndex, boolean CA, int psize, void *p, int Timeout)
Definition: ethercatcoe.c:1367
#define NSEC_PER_SEC
out_EBOX_streamt * out_EBOX
uint8 status
int ec_config_init(uint8 usetable)


soem
Author(s): Arthur Ketels and M.J.G. van den Molengraft
autogenerated on Mon Feb 28 2022 23:46:57