From 29b6af46fe3b1883ce05f421cbe9dd1edaa03401 Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Tue, 15 Jan 2019 11:34:00 +0100 Subject: [PATCH] add sample c realtime code --- python/MXMotion.py | 125 +++++------------------ python/shapepath.py | 17 +++- src/triggerSync/Makefile | 55 ++++++++++ src/triggerSync/Readme.md | 32 ++++++ src/triggerSync/c-sample.layout | 15 +++ src/triggerSync/c-samples.cbp | 40 ++++++++ src/triggerSync/c-samples.layout | 15 +++ src/triggerSync/sampleCode.c | 167 +++++++++++++++++++++++++++++++ 8 files changed, 362 insertions(+), 104 deletions(-) create mode 100644 src/triggerSync/Makefile create mode 100644 src/triggerSync/Readme.md create mode 100644 src/triggerSync/c-sample.layout create mode 100644 src/triggerSync/c-samples.cbp create mode 100644 src/triggerSync/c-samples.layout create mode 100644 src/triggerSync/sampleCode.c diff --git a/python/MXMotion.py b/python/MXMotion.py index 534474b..00085b6 100644 --- a/python/MXMotion.py +++ b/python/MXMotion.py @@ -36,126 +36,53 @@ 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 mode=2 : synchronize start and adapt motion speed - this function generates the code blocks: - self.sync_wait and self.sync_run - sync_wait can be put in the program to force a timing sync - sync_run are the commands to run the whole program + this function generates the code blocks: + self.sync_wait and self.sync_run + sync_wait can be put in the program to force a timing sync + sync_run are the commands to run the whole program ''' - if mode == 0: + if mode==0: try: del self.sync_prg except AttributeError: 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 is false # - waits raising edge of and the sets DesTimeBase=ServoPeriod - # flag ='PowerBrick[0].GpioData[0].0.1==1' - flag0='Gate3[1].Chan[0].UserFlag==0' - flag1='Gate3[1].Chan[1].UserFlag==0' + flag0='Gate3[1].Chan[0].UserFlag' + 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}){{}} - //send 1"sync1 %d:%d\\n",Sys.PhaseCount,Sys.PhaseCount-L0 - //L1=Sys.PhaseCount - while({flag1}){{}} + while({flag0}==0){{}} + while({flag1}==0){{}} 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: - #frequence jitter 50Hz Swissgrid: - #https://www.swissgrid.ch/de/home/operation/grid-data/current-data.html# - notFlag1 = 'Gate3[1].Chan[1].UserFlag!=0' - try: - prop=kwargs['prop'] #proportional value to adapt speed - except KeyError: - prop = .5E-4 - try: - maxJitter = kwargs['maxJitter'] # proportional value to adapt speed - except KeyError: - maxJitter=100 - prg='''close all buffers -disable plc {plcId} -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) + elif mode==2: + #frequence jitter 50Hz Swissgrid: + #https://www.swissgrid.ch/de/home/operation/grid-data/current-data.html# + flag0='Gate3[1].Chan[0].UserFlag' + flag1='Gate3[1].Chan[1].UserFlag' + flag0='P0';flag1='P1' + pt2pt_time=40 + prg=''' + Coord[1].Q[1]=1 + Coord[1].DesTimeBase=0 + Coord[1].Q[1]=2 + '''.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) def run(self): 'runs the code sync_run which has been generated with setup_sync()' diff --git a/python/shapepath.py b/python/shapepath.py index 3ef9da5..8b0bd42 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -616,7 +616,8 @@ 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)') - print('scaling of DesTimeBase: %f'%(float(idxInPos[num-1])/idxTrigger[num-1])) + if num>0: + print('scaling of DesTimeBase: %f'%(float(idxInPos[num-1])/idxTrigger[num-1])) plt.show() def bode_plot(self,xy=(0,1),mode=25,db=True): @@ -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=1) + #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_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') diff --git a/src/triggerSync/Makefile b/src/triggerSync/Makefile new file mode 100644 index 0000000..8b9e971 --- /dev/null +++ b/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 + diff --git a/src/triggerSync/Readme.md b/src/triggerSync/Readme.md new file mode 100644 index 0000000..7c8dcd7 --- /dev/null +++ b/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] + + +``` diff --git a/src/triggerSync/c-sample.layout b/src/triggerSync/c-sample.layout new file mode 100644 index 0000000..7475a03 --- /dev/null +++ b/src/triggerSync/c-sample.layout @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/triggerSync/c-samples.cbp b/src/triggerSync/c-samples.cbp new file mode 100644 index 0000000..adc923a --- /dev/null +++ b/src/triggerSync/c-samples.cbp @@ -0,0 +1,40 @@ + + + + + + diff --git a/src/triggerSync/c-samples.layout b/src/triggerSync/c-samples.layout new file mode 100644 index 0000000..fe4d7ab --- /dev/null +++ b/src/triggerSync/c-samples.layout @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/triggerSync/sampleCode.c b/src/triggerSync/sampleCode.c new file mode 100644 index 0000000..818bd5c --- /dev/null +++ b/src/triggerSync/sampleCode.c @@ -0,0 +1,167 @@ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + + +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;iUserAlgo.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; + +}