Increase configure time for special slave like Elmo drive, add xmc480…

…0 DC example

soem/ethercattype.h

@@ -89,7 +89,9 @@ extern "C"
 /** timeout value in us for tx frame to return to rx */
 #define EC_TIMEOUTRET     	2000
 /** timeout value in us for safe data transfer, max. triple retry */
-#define EC_TIMEOUTRET3     (EC_TIMEOUTRET * 10)
+/** EC_TIMEOUTRET3 is used mostly for ecx_config_init, scan slave, sdo configuration,...*/
+/** default is 10 times of EC_TIMEOUTRET (20ms), some slaves like Elmo drives need longer timer for configuration, changed to 200ms*/
+#define EC_TIMEOUTRET3     (EC_TIMEOUTRET * 100)
 /** timeout value in us for return "safe" variant (f.e. wireless) */
 #define EC_TIMEOUTSAFE     20000
 /** timeout value in us for EEPROM access */

test/xmc4800_dc/Makefile

@@ -0,0 +1,75 @@
+#
+# 'make depend' uses makedepend to automatically generate dependencies 
+#               (dependencies are added to end of Makefile)
+# 'make'        build executable file 'mycc'
+# 'make clean'  removes all .o and executable files
+#
+
+# define the C compiler to use
+CC = gcc
+
+# define any compile-time flags
+skin = alchemy
+CFLAGS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --cflags)
+LDLIBS := $(shell /usr/xenomai/bin/xeno-config --skin=$(skin) --ldflags)
+
+#SOEM EtherCAT
+CFLAGS += -I../../soem 
+CFLAGS += -I../../osal
+CFLAGS += -I../../oshw
+CFLAGS += -O3 -Wall -g
+
+
+# define any libraries to link into executable:
+#   if I want to link in libraries (libx.so or libx.a) I use the -llibname 
+#   option, something like (this will link in libmylib.so and libm.so:
+
+
+#SOEM EtherCAT
+LDLIBS += -L../../lib
+LDLIBS +=-lsoem
+LDLIBS +=-losal
+LDLIBS +=-loshw
+LDLIBS +=-lwiznet_drv
+
+# define the C source files
+SRCS = main.c ecat_dc.c
+
+# define the C object files 
+#
+# This uses Suffix Replacement within a macro:
+#   $(name:string1=string2)
+#         For each word in 'name' replace 'string1' with 'string2'
+# Below we are replacing the suffix .c of all words in the macro SRCS
+# with the .o suffix
+#
+OBJS = $(SRCS:.c=.o)
+
+# define the executable file 
+MAIN = xmc4800_dc
+
+#
+# The following part of the makefile is generic; it can be used to 
+# build any executable just by changing the definitions above and by
+# deleting dependencies appended to the file from 'make depend'
+#
+
+.PHONY: depend clean
+
+all:    $(MAIN)
+
+$(MAIN): $(OBJS)
+	$(CC) $(CFLAGS) -o $(MAIN) $(OBJS) $(LDLIBS)
+
+# this is a suffix replacement rule for building .o's from .c's
+# it uses automatic variables $<: the name of the prerequisite of
+# the rule(a .c file) and $@: the name of the target of the rule (a .o file) 
+# (see the gnu make manual section about automatic variables)
+.c.o:
+	$(CC) $(CFLAGS) $(LDLIBS) -c $<  -o $@
+
+clean:
+	$(RM) *.o *~ $(MAIN)
+
+
+# DO NOT DELETE THIS LINE -- make depend needs it

test/xmc4800_dc/ecat_dc.c

@@ -0,0 +1,128 @@
+
+#include <stdlib.h>
+#include <signal.h>
+#include "ecat_dc.h"
+/* 
+ * SOEM EtherCAT exmaple
+ * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 
+ */
+
+//convert float, double to fixed point
+void double_to_fixed(double f_input, int32_t *pValue, int32_t *pBase)
+{
+	if (f_input<1.0)
+	{
+		(*pBase)=15;
+		(*pValue)=(int32_t) (32768.0*f_input);
+	}
+	else if (f_input<2.0)
+	{
+		(*pBase)=14;
+		(*pValue)=(int32_t) (16384.0*f_input);
+	}
+	else if (f_input<4.0)
+	{
+		(*pBase)=13;
+		(*pValue)=(int32_t) (8192.0*f_input);
+	}
+	else if (f_input<8.0)
+	{
+		(*pBase)=12;
+		(*pValue)=(int32_t) (4096.0*f_input);
+	}
+	else if (f_input<16.0)
+	{
+		(*pBase)=11;
+		(*pValue)=(int32_t) (2048.0*f_input);
+	}
+	else if (f_input<32.0)
+	{
+		(*pBase)=10;
+		(*pValue)=(int32_t) (1024.0*f_input);
+	}
+	else if (f_input<64.0)
+	{
+		(*pBase)=9;
+		(*pValue)=(int32_t) (512.0*f_input);
+	}
+	else if (f_input<128.0)
+	{
+		(*pBase)=8;
+		(*pValue)=(int32_t) (256.0*f_input);
+	}
+	else if (f_input<256.0)
+	{
+		(*pBase)=7;
+		(*pValue)=(int32_t) (128.0*f_input);
+	}
+	else if (f_input<512.0)
+	{
+		(*pBase)=6;
+		(*pValue)=(int32_t) (64.0*f_input);
+	}
+	else if (f_input<1024.0)
+	{
+		(*pBase)=5;
+		(*pValue)=(int32_t) (32.0*f_input);
+	}
+	else if (f_input<2048.0)
+	{
+		(*pBase)=4;
+		(*pValue)=(int32_t) (16.0*f_input);
+	}
+	else if (f_input<4096.0)
+	{
+		(*pBase)=3;
+		(*pValue)=(int32_t) (8.0*f_input);
+	}
+	else if (f_input<81928.0)
+	{
+		(*pBase)=2;
+		(*pValue)=(int32_t) (4.0*f_input);
+	}
+	else if (f_input<16384.0)
+	{
+		(*pBase)=1;
+		(*pValue)=(int32_t) (2.0*f_input);
+	}
+	else if (f_input<32768.0)
+	{
+		(*pBase)=0;
+		(*pValue)=(int32_t) (1.0*f_input);
+	}
+}
+
+
+
+#define PI_SAT_VAL	50000
+
+int32_t  _pPart=0, _iPart=0;
+int32_t   _sync_err=0, _sync_pre_err=0;
+
+double Kp=0.1, Ki=0.0005;
+//double Kp=0.1, Ki=0.001;
+
+//PI compensation
+long long dc_pi_sync(long long  reftime, long long  cycletime, int32_t shift_time)
+{
+	long long  adj_time=0;
+	int32_t iKp=0, iKp_Base, iKi=0, iKi_Base;
+	double_to_fixed(Kp, &iKp, &iKp_Base);
+	double_to_fixed(Ki, &iKi, &iKi_Base);
+
+	_sync_err = (reftime - shift_time) % cycletime;
+	if(_sync_err> (cycletime /2)) { _sync_err= _sync_err - cycletime; }
+
+	_pPart=_sync_err*iKp;
+	_iPart+=(_sync_err+_sync_pre_err)*iKi;
+	_sync_pre_err=_sync_err;
+
+	adj_time = -(_pPart>>iKp_Base) - (_iPart>>iKi_Base);
+
+	if (adj_time>PI_SAT_VAL) adj_time=PI_SAT_VAL;
+	if (adj_time<-PI_SAT_VAL) adj_time=-PI_SAT_VAL;
+
+	return adj_time;
+
+}
+

test/xmc4800_dc/ecat_dc.h

@@ -0,0 +1,21 @@
+/* 
+ * SOEM EtherCAT exmaple
+ * Ported to raspberry pi by Ho Tam - thanhtam.h[at]gmail.com 
+ */
+
+#ifndef _ECAT_DC_H_
+#define _ECAT_DC_H_
+
+#include <stdio.h>
+#include <signal.h>
+#include <string.h>
+#include <sys/time.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <math.h>
+#include <stdint.h>
+
+//PI compensation
+long long dc_pi_sync(long long  reftime, long long  cycletime, int32_t shift_time);
+#endif //_ECAT_DC_H_
+