add sample c realtime code

python/MXMotion.py

@@ -36,7 +36,7 @@ class MotionBase:
     self.gather=gather
     self.verbose=verbose
 
-  def setup_sync(self, crdId=1, prgId=2, plcId=2, mode=0, **kwargs):
+  def setup_sync(self, crdId=1, prgId=2, plcId=2, encId=8, mode=0, **kwargs):
     '''setup the timing synchronization for the motion program
        mode=0 : no sync at all
        mode=1 : synchronize start
@@ -53,109 +53,36 @@ class MotionBase:
         pass
       self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, crdId=crdId)
 
-    elif mode in (1,2):
+    elif mode==1:
       # code block to insert in the program
       # - waits untis <flag> is false
       # - waits raising edge of <flag> and the sets DesTimeBase=ServoPeriod
-
       # flag ='PowerBrick[0].GpioData[0].0.1==1'
-      flag0='Gate3[1].Chan[0].UserFlag==0'
+      flag0='Gate3[1].Chan[0].UserFlag'
-      flag1='Gate3[1].Chan[1].UserFlag==0'
+      flag1='Gate3[1].Chan[1].UserFlag'
+      flag0='P0';flag1='P1'
       prg = '''
-  //L0=Sys.PhaseCount
-  //send 1"sync0 %d:%d\\n",Sys.PhaseCount,Sys.PhaseCount-L0
   Coord[1].Q[1]=1
-  while({flag0}){{}}
+  while({flag0}==0){{}}
-  //send 1"sync1 %d:%d\\n",Sys.PhaseCount,Sys.PhaseCount-L0
+  while({flag1}==0){{}}
-  //L1=Sys.PhaseCount
-  while({flag1}){{}}
   Coord[1].Q[1]=2
-  //PowerBrick[0].GpioData[0].16.8=255
-  //send 1"sync2 %d:%d\\n",Sys.PhaseCount,Sys.PhaseCount-L1
   '''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
       self.sync_prg = prg
       self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId)
-      if mode==2:
+    elif mode==2:
       #frequence jitter 50Hz Swissgrid:
       #https://www.swissgrid.ch/de/home/operation/grid-data/current-data.html#
-        notFlag1 = 'Gate3[1].Chan[1].UserFlag!=0'
+      flag0='Gate3[1].Chan[0].UserFlag'
-        try:
+      flag1='Gate3[1].Chan[1].UserFlag'
-          prop=kwargs['prop'] #proportional value to adapt speed
+      flag0='P0';flag1='P1'
-        except KeyError:
+      pt2pt_time=40
-          prop = .5E-4
+      prg='''
-        try:
+      Coord[1].Q[1]=1
-          maxJitter = kwargs['maxJitter']  # proportional value to adapt speed
+      Coord[1].DesTimeBase=0
-        except KeyError:
+      Coord[1].Q[1]=2
-          maxJitter=100
+      '''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
-        prg='''close all buffers
+      self.sync_prg=prg
-disable plc {plcId}
+      self.sync_run='&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId)
-open plc {plcId}
-Coord[1].DesTimeBase=Sys.ServoPeriod //reset to 100% timebase
-Coord[1].Q[0]=0 //set syncCnt counter to 0
-
-while({flag0}){{}} //wait Jungfrau start Trigger
-while({flag1}){{}} //ESx detector trigger
-
-
-{syncCnt}=0 //sync counter, sent when motion is on a crystal
-//{evnt} current PhaseCount event
-//{jitter} jitter: neg:motion lags trigger,pos:motion leads trigger
-//{syncEnvt} Prev syncEnvt PhaseCount
-{sync2sync}=0 // last syncEnvt-syncEnvt PhaseCount
-{dtb}=Sys.ServoPeriod //DesTimeBase
-//{tmp} temp variable
-
-while({syncCnt}>=0) //as long we are not at the last point
-{{
-  {syncCnt}=Coord[1].Q[0] //sync counter
-  while({flag1} && {syncCnt}==Coord[1].Q[0]){{}} //wait for event
-  {evnt}=Sys.PhaseCount   //a event happened
-  {jitter}=1000
-  //send 1"event\\n"
-  if({syncCnt}==Coord[1].Q[0])
-  {{//it was a trigger
-    //send 1"trigger %d %d %d\\n",{syncCnt},{evnt},{sync2sync}
-    while(Sys.PhaseCount<{evnt}+{maxJitter}) //wait half a trigger period for event
-    {{
-      if({syncCnt}!=Coord[1].Q[0])
-      {{//event syncCnt
-        {tmp}=Sys.PhaseCount
-        {sync2sync}={tmp}-{syncEnvt};{syncEnvt}={tmp}
-        {jitter}={evnt}-{tmp} //jitter: neg:motion lags trigger
-        break
-      }}
-    }}
-  }}
-  else
-  {{//it was a syncCnt
-    {sync2sync}={evnt}-{syncEnvt};{syncEnvt}={evnt}
-    //send 1"syncCnt %d %d %d\\n",{syncCnt},{evnt},{sync2sync}
-    while(Sys.PhaseCount<{evnt}+{maxJitter}) //wait half a trigger period for event
-    {{
-      if({notFlag1})
-      {{//event trigger
-        {jitter}=Sys.PhaseCount-{evnt} //jitter: pos:motion leads trigger
-        break
-      }}
-    }}
-  }}
-  if({jitter}!=1000 && {sync2sync}>0)
-  {{
-    {dtb}=Sys.ServoPeriod*(1-{prop}*{jitter})
-    //{dtb}={dtb}*(1-{jitter}/{sync2sync}*.5)
-    //send 1"syncCnt %d jitter %d sync2sync %d timebase: %f\\n",{syncCnt},{jitter},{sync2sync},{dtb}
-    Coord[1].DesTimeBase={dtb}
-  }}
-}}
-disable plc {plcId}
-close
-enable plc {plcId}
-'''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1, notFlag1=notFlag1, prop=prop, maxJitter=maxJitter,
-           syncCnt='L0', evnt='L1', jitter='L2', syncEnvt='L3', sync2sync='L4', dtb='L5', tmp='L6')
-        comm=self.comm
-        if comm is not None:
-          gpascii=comm.gpascii
-          gpascii.send_block(prg)
 
   def run(self):
     'runs the code sync_run which has been generated with setup_sync()'

python/shapepath.py

@@ -616,6 +616,7 @@ class ShapePath(MotionBase):
       hl += ax.plot(jitter*ts, 'b-',label='jitter')
       ax.xaxis.set_label_text('position idx')
       ax.yaxis.set_label_text('jitter motion (ms)')
+      if num>0:
         print('scaling of DesTimeBase: %f'%(float(idxInPos[num-1])/idxTrigger[num-1]))
     plt.show()
 
@@ -724,7 +725,7 @@ if __name__=='__main__':
 
 
   def run_test(args):
-    args.host=None
+    #args.host=None
     if args.host is None:
       comm=gather=None
     else:
@@ -767,7 +768,7 @@ if __name__=='__main__':
     #sp.plot_gather()
     #return
     xy=False
-    sp.gen_rand_points(n=14, scale=1000);sp.sort_points(xy=xy)
+    #sp.gen_rand_points(n=14, scale=1000);sp.sort_points(xy=xy)
     #print(sp.points[:,0])
     #sp.gen_swissmx_points(width=1000, ofs=(-500, 0));
     #sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,0.]])
@@ -779,9 +780,15 @@ if __name__=='__main__':
                         [0., 1.], [1., 0.], [0., -1.], [-1., 0.], [0., 1.]])
     #sp.gen_grid_points(w=10,h=10,pitch=100,rnd=0,ofs=(0,0));sp.sort_points(False);
     #sp.gen_grid_points(w=1,h=10,pitch=100,rnd=0,ofs=(0,0))
-    sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=0)
+    #sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=0)
-    sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=1)
+    #sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=1)
+    sp.setup_gather(acq_per=2)
+    sp.setup_sync(mode=2) #sync with timing system and PLC to sync speed (PROG)
+    sp.setup_coord_trf() # reset to shape path system
     sp.setup_motion(fnPrg=fn + '.prg', mode=3, pt2pt_time=40,scale=1)
+    sp.run()
+    sp.gather_upload(fnRec=fn+'.npz')
+    sp.plot_gather(mode=11)
     exit(0)
 
     # fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')

src/triggerSync/Makefile

@@ -0,0 +1,55 @@
+HOST=MOTTEST-CPPM-CRM0573
+
+
+XENOMAI_INC_DIR=/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/include
+XENOMAI_LIB_DIR=/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/lib
+
+INC=-I/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac -I/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac -I$(XENOMAI_INC_DIR)
+
+
+LIB=-L/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac -lppmac -L/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/usr/local/xenomai-2.6.2.1/lib -lnative -lxenomai -lpthread_rt -lpthread -lrt -ldl
+
+CC=/opt/eldk-4.2/usr/bin/ppc_4xxFP-g++
+LD=$(CC)
+
+all: sampleCode
+
+%.o: %.c
+	$(CC) -g $(INC) -c $^ -o $@
+
+sampleCode: sampleCode.o
+	$(LD) -g $(LIB) $^ -o $@
+	scp $@ root@$(HOST):/tmp
+	scp /opt/eldk-4.2/ppc_4xxFP/usr/bin/gdbserver root@$(HOST):/tmp
+
+clean:
+	rm -f sampleCode sampleCode *.o
+
+Debug: all
+cleanDebug: clean
+
+#export LD_LIBRARY_PATH=/opt/ppmac/libppmac/
+#/tmp/sampleCode
+#/tmp/gdbserver localhost:2000 /tmp/sampleCode
+
+# /opt/eldk-4.2/usr/bin/ppc_4xxFP-gdb
+# file sampleCode
+# b main
+# target remote MOTTEST-CPPM-CRM0573:2000
+# continue
+# step
+#
+
+#ddd --debugger /opt/eldk-4.2/usr/bin/ppc_4xxFP-gdb
+
+#scp /opt/eldk-4.2/PPMAC_rootfs-7-wheezy/usr/bin/gdbserver root@mottest-cppm-crm0573:/tmp   -> failed
+#scp /opt/eldk-4.2/PPMAC_rootfs/usr/bin/gdbserver root@mottest-cppm-crm0573:/tmp   -> works
+#scp /opt/eldk-4.2/ppc_4xxFP/usr/bin/gdbserver root@mottest-cppm-crm0573:/tmp      -> works
+
+#codeblocks config is in: /home/zamofing_t/.config/codeblocks/default.conf
+
+
+#rtpmacapi.h:struct SHM* GetSharedMemPtr(void);
+
+#Linux MOTTEST-CPPM-CRM0573 3.2.21-powerpmac-smp #213 SMP Fri Oct 2 14:29:46 PDT 2015 ppc
+

src/triggerSync/Readme.md

@@ -0,0 +1,32 @@
+Activate an user servoloop
+--------------------------
+```
+PBTools/pbtools/usr_servo_phase$ make
+PBTools/pbtools/usr_servo_phase/usrServoSample$ make
+scp userservo_util userphase_util usrServoSample/usralgo.ko root@MOTTEST-CPPM-CRM0573:/tmp
+
+rmmod usralgo
+insmod /tmp/usralgo.ko
+
+cat /proc/kallsyms | grep MyUserAlgoFunctionName (e.g. cat /proc/kallsyms | grep usr_servo_ctrl_2)
+	a10385ca r __kstrtab_usr_servo_ctrl_2	[usralgo]
+	a1038570 r __ksymtab_usr_servo_ctrl_2	[usralgo]
+	a103812c T usr_servo_ctrl_2	[usralgo]
+
+UserAlgo.ServoCtrlAddr[1] = $a103812c
+Motor[1].Ctrl =UserAlgo.ServoCtrlAddr[1].a
+
+.. but this can not be set directly in gpascii.
+
+root@:/opt/ppmac#
+LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -d 1
+rmmod usralgo
+insmod /tmp/usralgo.ko
+LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -l 1 usr_servo_ctrl_2
+LD_LIBRARY_PATH=/opt/ppmac/libppmac/ /tmp/userservo_util -e 1
+
+gpascii:
+Motor[1].Ctrl =UserAlgo.ServoCtrlAddr[1]
+
+
+```

src/triggerSync/c-sample.layout

@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_layout_file>
+	<FileVersion major="1" minor="0" />
+	<ActiveTarget name="Debug" />
+	<File name="sampleCode.c" open="1" top="1" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="337" topLine="0" />
+		</Cursor>
+	</File>
+	<File name="Makefile" open="1" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="735" topLine="0" />
+		</Cursor>
+	</File>
+</CodeBlocks_layout_file>

src/triggerSync/c-samples.cbp

@@ -0,0 +1,40 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_project_file>
+	<FileVersion major="1" minor="6" />
+	<Project>
+		<Option title="c-samples" />
+		<Option makefile_is_custom="1" />
+		<Option pch_mode="2" />
+		<Option compiler="deltatau_gcc" />
+		<Build>
+			<Target title="Debug">
+				<Option output="bin/c-samples" prefix_auto="1" extension_auto="1" />
+				<Option object_output="obj/" />
+				<Option type="1" />
+				<Option compiler="deltatau_gcc" />
+			</Target>
+		</Build>
+		<Unit filename="Makefile" />
+		<Unit filename="Readme.md" />
+		<Unit filename="sampleCode.c">
+			<Option compilerVar="CC" />
+		</Unit>
+		<Extensions>
+			<envvars />
+			<code_completion>
+				<search_path add="/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac" />
+				<search_path add="/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac" />
+				<search_path add="/opt/eldk-4.2/ppc_4xxFP/usr/include" />
+			</code_completion>
+			<lib_finder disable_auto="1" />
+			<debugger>
+				<search_path add="/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac" />
+				<search_path add="/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac" />
+				<search_path add="/opt/eldk-4.2/ppc_4xxFP/usr/include" />
+				<remote_debugging>
+					<options conn_type="0" serial_baud="115200" ip_address="MOTTEST-CPPM-CRM0573" ip_port="2000" additional_cmds_before="file sampleCode" />
+				</remote_debugging>
+			</debugger>
+		</Extensions>
+	</Project>
+</CodeBlocks_project_file>

src/triggerSync/c-samples.layout

@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
+<CodeBlocks_layout_file>
+	<FileVersion major="1" minor="0" />
+	<ActiveTarget name="Debug" />
+	<File name="Makefile" open="1" top="0" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="687" topLine="3" />
+		</Cursor>
+	</File>
+	<File name="sampleCode.c" open="1" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
+		<Cursor>
+			<Cursor1 position="835" topLine="9" />
+		</Cursor>
+	</File>
+</CodeBlocks_layout_file>

src/triggerSync/sampleCode.c

@@ -0,0 +1,167 @@
+#include <stdio.h>
+#include <gplib.h>
+#include <math.h>
+
+#include <stdio.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sys/mman.h>
+#include <native/task.h>
+#include <native/timer.h>
+#include <math.h>
+
+
+extern struct SHM *pshm;
+// >>>>>>> https://www.ashwinnarayan.com/post/xenomai-realtime-programming-part-2/
+
+
+#define CLOCK_RES 1e-9 //Clock resolution is 1 ns by default
+#define LOOP_PERIOD 1e7 //Expressed in ticks
+//RTIME period = 1000000000;
+RT_TASK loop_task;
+void loop_task_proc(void *arg)
+{
+  RT_TASK *curtask;
+  RT_TASK_INFO curtaskinfo;
+  int iret = 0;
+
+  RTIME tstart, now;
+
+  curtask = rt_task_self();
+  rt_task_inquire(curtask, &curtaskinfo);
+  int ctr = 0;
+
+  //Print the info
+  printf("Starting task %s with period of 10 ms ....\n", curtaskinfo.name);
+
+  //Make the task periodic with a specified loop period
+  rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD);
+
+  tstart = rt_timer_read();
+
+  //Start the task loop
+  while(1){
+    printf("Loop count: %d, Loop time: %.5f ms\n", ctr, (rt_timer_read() - tstart)/1000000.0);
+    ctr++;
+    rt_task_wait_period(NULL);
+  }
+}
+
+void loop_task_run()
+{
+  char str[20];
+  //Lock the memory to avoid memory swapping for this program
+  mlockall(MCL_CURRENT | MCL_FUTURE);
+  printf("Starting cyclic task...\n");
+  //Create the real time task
+  sprintf(str, "cyclic_task");
+  rt_task_create(&loop_task, str, 0, 50, 0);
+  //Since task starts in suspended mode, start task
+  rt_task_start(&loop_task, &loop_task_proc, 0);
+  //Wait for Ctrl-C
+  pause();
+}
+
+
+RT_TASK trigsync_task;
+void trigsync_func(void *arg)
+{
+  RT_TASK *curtask;
+  RT_TASK_INFO curtaskinfo;
+  int iret = 0;
+
+  RTIME tstart, now;
+
+  curtask = rt_task_self();
+  rt_task_inquire(curtask, &curtaskinfo);
+  int ctr = 0;
+
+  //Print the info
+  printf("Starting task %s with period of 10 ms ....\n", curtaskinfo.name);
+
+  //Make the task periodic with a specified loop period
+  rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD);
+
+  tstart = rt_timer_read();
+
+  //Start the task loop
+  while(1){
+    printf("Loop count: %d, Loop time: %.5f ms\n", ctr, (rt_timer_read() - tstart)/1000000.0);
+    ctr++;
+    rt_task_wait_period(NULL);
+  }
+  /*  float ang,pos;
+    int i;
+    float srvStart,srvCnt,diff,maxDiff,tmp;
+
+    srvStart=srvCnt=pshm->ServoCount;
+    for(maxDiff=0,i=0;i<10000000;i++)
+    {
+      tmp=pshm->ServoCount;
+      diff=tmp-srvCnt;
+      if(diff>maxDiff)
+        maxDiff=diff;
+      srvCnt=tmp;
+    }
+    printf("srvCnt %d diff %f maxDiff %f\n",(int)(srvCnt-srvStart),diff, maxDiff);*/
+}
+
+void trigsync_run()
+{
+  char str[20];
+  //Lock the memory to avoid memory swapping for this program
+  mlockall(MCL_CURRENT | MCL_FUTURE);
+  printf("Starting cyclic task...\n");
+  //Create the real time task
+  sprintf(str, "cyclic_task");
+  rt_task_create(&trigsync_task, str, 0, 50, 0);
+  //Since task starts in suspended mode, start task
+  rt_task_start(&trigsync_task, &trigsync_func, 0);
+  //Wait for Ctrl-C
+  pause();
+}
+
+
+int main(int argc, char *argv[])
+{
+    int err;
+    int initialized=0;
+    char s[256];
+    int i;
+
+    if ((err = InitLibrary()) != 0) {
+        abort();
+    }
+    initialized = 1;
+    pshm = GetSharedMemPtr();
+
+    printf("P:%g\n",pshm->P[1011]);
+    printf("pshm->MaxRtPlc:%d\n",pshm->MaxRtPlc);
+
+    for(i=0;i<8;i++) //MAX_MOTORS instead of 8
+      printf("pshm->Motor[%d].Ctrl:%p\n",i,pshm->Motor[i].Ctrl);
+
+    for(i=0;i<8;i++) //MAX_MOTORS instead of 8
+      printf("pshm->UserAlgo.ServoCtrlAddr[%d]:%d\n",i,pshm->UserAlgo.ServoCtrlAddr[i]);
+
+    for(i=0;i<MAX_PLC;i++) //MAX_MOTORS instead of 8
+      printf("pshm->UserAlgo.BgCplcPID[%d]:%d\n",i,pshm->UserAlgo.BgCplcPID[i]);
+
+    printf("pshm->UserAlgo.RtiCplcPID:%d\n",pshm->UserAlgo.RtiCplcPID);
+    printf("pshm->UserAlgo.RtiCplcMaxTime:%d\n",pshm->UserAlgo.RtiCplcMaxTime);
+    printf("pshm->UserAlgo.CaptCompFuncAddr:%p\n",pshm->UserAlgo.CaptCompFuncAddr);
+
+    trigsync_run();
+    //trigsync_func(0);
+    //loop_task_run();
+
+
+    CloseLibrary();
+    return !err;
+
+    if (initialized)
+        CloseLibrary();
+
+    return 0;
+
+}