From 66e2d038baab505a520635d7648f3003202c25ce Mon Sep 17 00:00:00 2001 From: Thierry Zamofing Date: Tue, 22 Jan 2019 14:41:16 +0000 Subject: [PATCH] first running sync with rt-C-code --- python/MXMotion.py | 91 ++++++------ python/shapepath.py | 36 +++-- src/triggerSync/triggerSync.c | 216 +++++++++++++++++++---------- src/triggerSync/triggerSync.cbp | 2 + src/triggerSync/triggerSync.layout | 8 +- 5 files changed, 217 insertions(+), 136 deletions(-) diff --git a/python/MXMotion.py b/python/MXMotion.py index 89004bb..e8ef979 100644 --- a/python/MXMotion.py +++ b/python/MXMotion.py @@ -45,6 +45,11 @@ class MotionBase: 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 + flag=0 : real start and frame trigger + flag=1 : simulated start and real frame trigger + flag=2 : real start and simulated frame trigger + flag=3 : simulated start and frame trigger + pt2pt_time : time point to point (needed sor sync code) ''' if mode==0: try: @@ -53,15 +58,15 @@ class MotionBase: pass self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, crdId=crdId) - 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' - flag1='Gate3[1].Chan[1].UserFlag' - flag0='P0';flag1='P1' - prg = ''' + elif mode in(1,2): + #frequence jitter 50Hz Swissgrid: + #https://www.swissgrid.ch/de/home/operation/grid-data/current-data.html# + flag=kwargs.get('flag',0); + pt2pt_time=kwargs.get('pt2pt_time',40); + flag0='Coord[1].Q[10]' if flag&1 else 'Gate3[1].Chan[0].UserFlag' + flag1='Coord[1].Q[11]' if flag&2 else 'Gate3[1].Chan[1].UserFlag' + if mode==1: + prg = ''' Coord[1].Q[1]=-2 Coord[1].TimeBaseSlew=1 //1E-4 is default Coord[1].DesTimeBase=0 @@ -71,36 +76,41 @@ class MotionBase: while({flag1}==0){{}} Coord[1].DesTimeBase=Sys.ServoPeriod '''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1) + else: + prg = ''' + //Gather.Enable=2 is done in the sync program + Coord[1].TimeBaseSlew=1 //1E-4 is default + Coord[1].DesTimeBase=0 + Coord[1].Q[1]=-1 + '''.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) - 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' - prg=''' - //Gather.Enable=2 is done in the sync program - Coord[1].TimeBaseSlew=1 //1E-4 is default - Coord[1].DesTimeBase=0 - Coord[1].Q[1]=-1 - '''.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) #download and start triggerSync code comm=self.comm - sftp=comm.sftp - dst='/tmp/triggerSync' - src=os.path.abspath(os.path.join(os.path.dirname(__file__), '../src/triggerSync/triggerSync')) - sftp.put(src, dst) - sftp.chmod(dst, 0o755) - self.cmdSync=cmd='LD_LIBRARY_PATH=/opt/ppmac/libppmac/ '+dst - #self.syncShell=comm.shell_channel(cmd) - self.syncChan=sc=comm._client.exec_command(cmd) - #ch=sc[1].channel - #ch.settimeout(1) - print ('starting '+cmd) + trigMode=0; + if mode==2:trigMode+=1 #synchronize + trigMode+=flag*2 #simulated or real triggers + trigMode+=8 # verbose + if mode==2 or trigMode&4: + sftp = comm.sftp + dst = '/tmp/triggerSync' + src = os.path.abspath(os.path.join(os.path.dirname(__file__), '../src/triggerSync/triggerSync')) + sftp.put(src, dst) + sftp.chmod(dst, 0o755) + cmd = 'LD_LIBRARY_PATH=/opt/ppmac/libppmac/ ' + dst + cmd+=' %g %d'%(pt2pt_time,trigMode) + self.cmdSync = cmd + print ('starting '+cmd) + self.syncShell=comm.shell_channel(cmd) + #self.syncChan=sc=comm._client.exec_command(cmd) + #ch=sc[1].channel + #ch.settimeout(1) + #sc[1].channel.setblocking(False) + #sc[1].read(100) + if flag&1: + print('set Coord[1].Q[10]=1 to start motion') def run(self): 'runs the code sync_run which has been generated with setup_sync()' @@ -112,17 +122,4 @@ class MotionBase: raise 'Need to call setup sync before' gpascii.send_block(cmd) - dst='/tmp/triggerSync' - src=os.path.abspath(os.path.join(os.path.dirname(__file__), '../src/triggerSync/triggerSync')) - cmd='LD_LIBRARY_PATH=/opt/ppmac/libppmac/ '+dst - # self.syncShell=comm.shell_channel(cmd) - - try: - cmd=self.cmdSync - except AttributeError: pass - else: - import time - time.sleep(1) - self.syncChan=sc=comm._client.exec_command(cmd) - diff --git a/python/shapepath.py b/python/shapepath.py index 7b84fc9..eb84aa2 100755 --- a/python/shapepath.py +++ b/python/shapepath.py @@ -286,7 +286,7 @@ class ShapePath(MotionBase): self.ptsCorr=ptsCorr print(ptsCorr) - def setup_gather(self,acq_per=1): + def setup_gather(self,acq_per=1,sim=False): ''' setup the channels to gather kwargs: @@ -295,9 +295,10 @@ class ShapePath(MotionBase): comm=self.comm gt=self.gather gt.set_phasemode(False) - address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Gate3[1].Chan[1].UserFlag") - address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Sys.P[1]") - #address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Coord[1].Q[1]") + if sim: + address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Coord[1].Q[11]") + else: + address=("Motor[1].ActPos","Motor[2].ActPos","Motor[1].DesPos","Motor[2].DesPos","Gate3[1].Chan[1].UserFlag") gt.set_address(*address) gt.set_property(MaxSamples=1000000, Period=acq_per) @@ -492,9 +493,6 @@ class ShapePath(MotionBase): else: print(syncShell.sync()) del self.syncShell - syncShell.close() - - pts=self.points ofsy=-rec[0,2]+pts[0,1] @@ -832,17 +830,37 @@ if __name__=='__main__': # [0., 1.]]) #sp.points*=100 #sp.gen_spiral_points(rStart=100,rInc=10,numSeg=4,numCir=60, ofs=(0, 0)) - sp.gen_spiral_points(rStart=100,rInc=10,numSeg=8,numCir=12, ofs=(0, 0)) + sp.gen_spiral_points(rStart=100,rInc=20,numSeg=8,numCir=32, ofs=(0, 0)) #sp.gen_closed_shifted() #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) + + #works: direct start + #sp.setup_gather(acq_per=1) + #sp.setup_sync(mode=0) + + #works: simulated start and frame trigger no sync + #sp.setup_gather(acq_per=1,sim=True) + #sp.setup_sync(mode=1,flag=3,pt2pt_time=40) + + #works: simulated start and frame trigger with sync + #sp.setup_gather(acq_per=1,sim=True) + #sp.setup_sync(mode=2,flag=3,pt2pt_time=40) + + #works: simulated start real frame trigger no sync + #sp.setup_gather(acq_per=1) + #sp.setup_sync(mode=1,flag=1,pt2pt_time=40) + + #works: simulated start real frame trigger with sync sp.setup_gather(acq_per=1) - sp.setup_sync(mode=1) #sync with timing system and PLC to sync speed (PROG) + sp.setup_sync(mode=2,flag=1,pt2pt_time=40) + sp.setup_coord_trf() # reset to shape path system sp.setup_motion(fnPrg=fn + '.prg', mode=3, pt2pt_time=40,scale=1,dwell=10) + #sp.setup_motion(fnPrg=fn + '.prg', mode=1, pt2pt_time=40,scale=0,dwell=10) sp.run() sp.gather_upload(fnRec=fn+'.npz') sp.plot_gather(mode=11) diff --git a/src/triggerSync/triggerSync.c b/src/triggerSync/triggerSync.c index 0733b95..0a434b7 100644 --- a/src/triggerSync/triggerSync.c +++ b/src/triggerSync/triggerSync.c @@ -1,31 +1,24 @@ +//*-----------------------------------------------------------------------* +//| | +//| Copyright (c) 2019 by Paul Scherrer Institute (http://www.psi.ch) | +//| | +//| Author Thierry Zamofing (thierry.zamofing@psi.ch) | +//*-----------------------------------------------------------------------* + // >>>>>>> https://www.ashwinnarayan.com/post/xenomai-realtime-programming-part-2/ // >>>>>>> https://xenomai.org/documentation/xenomai-2.4/html/api/group__task.html +// Usage -> read the usage function +//samples: root@:/opt/ppmac# +// /tmp/triggerSync 40 11 trigger all 40 ms, simulated start, use real frame triggers, verbose +// /tmp/triggerSync 40 14 trigger all 40 ms, simulated start and frame triggers, no sync, verbose +// /tmp/triggerSync 40 15 trigger all 40 ms, simulated start and frame triggers, with sync, verbose +// /tmp/triggerSync 40 7 trigger all 40 ms, simulated start and frame triggers, with sync, minimal verbose -//mode: -// bit0: sync mode -// bit1: simulate mode -// bit2: verbose +//before start set: Gather.Enable=1 +//to trigger a simulated start: Coord[1].Q[10]=1 -// 0: useless -// 1: synchronize real triggers -// 2: simulate triggers (no sync) -// 3: synchronize simulate triggers - -//in simulate mode: -//set pshm->Coord[1].Q[10]=1 to simulate a Jungfrau aquire start -//set pshm->Coord[1].Q[10]=2 to stop simulate trigger generation -//Coord[1].Q[11] is the simulated frame trigger - -//in synchronize mode -//Coord[1].Q[0]=-2 : trigsync_func start, Wait for 'arm' trigger -//Coord[1].Q[0]=-1 : got 'arm' trigger, wait frame trigger -//Coord[1].Q[0]= 0 : got frame trigger 0 -//Coord[1].Q[0] is incremented at each trigger -//sync task ends when Gather.Enable==0 - - -//Gather.Enable=1 +//the simulated frame trigger comes at 40.2 ms period. #include #include @@ -45,10 +38,12 @@ static float mtPt2Pt=40.f; //motion point to point time #define SIMFLAG0 (pshm->Coord[1].Q[10]) #define SIMFLAG1 (pshm->Coord[1].Q[11]) -#define FLAG0 (gate3_1->Chan[0].Status&0x8000000) -#define FLAG1 (gate3_1->Chan[1].Status&0x8000000) +//#define SIMFLAG0 (pshm->P[10]) +//#define SIMFLAG1 (pshm->P[11]) +//Power PMAC Software Reference Manual.pdf page 760 +#define FLAG0 (gate3_1->Chan[0].Status&0x800) +#define FLAG1 (gate3_1->Chan[1].Status&0x800) -RT_TASK trigsync_task; void trigsync_func(void *arg) { RT_TASK *curtask; @@ -72,12 +67,15 @@ void trigsync_func(void *arg) float srvPer; float mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed - float mtPt2Pt=40.f; //motion point to point time - unsigned int maxDiff; struct GateArray3* gate3_1=GetGate3MemPtr(1); pshm = GetSharedMemPtr(); + if(!pshm->Gather.Enable) + { + printf("ending trigsync_func because Gather.Enable==0\n"); + return; + } pshm->Coord[1].Q[0]=-2; srvPer=pshm->ServoPeriod; @@ -94,8 +92,10 @@ void trigsync_func(void *arg) rt_task_wait_period(NULL); } printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1E6); + + pshm->Gather.Enable=2; //start gathering data pshm->Coord[1].Q[0]=-1; - if(mode&2) + if(mode&4) { while(!SIMFLAG1) rt_task_wait_period(NULL); @@ -105,16 +105,24 @@ void trigsync_func(void *arg) while(!FLAG1) rt_task_wait_period(NULL); } +//puts("waitFlag1"); +//gate3_1->Chan[1].Status f057f77f +//gate3_1->Chan[1].Status f057ff7f +//printf("gate3_1->Chan[1].Status %x\n",gate3_1->Chan[1].Status); + printf("Flag 1: %.5f ms\n", (rt_timer_read() - rtStart)/1E6); rtStart=rtLast=rt_timer_read(); scStart=scLast=pshm->ServoCount; mtAct=0.f; pshm->Coord[1].Q[0]=0; pshm->Coord[1].DesTimeBase=srvPer; //start motion at default speed + if(mode&8) + printf("Start: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.6f\n", i, rtLast/1E6,scLast, mtAct,mtDes,srvPer); + for(i=1;pshm->Gather.Enable;i++) { - if(mode&2) + if(mode&4) { while(SIMFLAG1) rt_task_wait_period(NULL); @@ -141,14 +149,14 @@ void trigsync_func(void *arg) srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff; pshm->Coord[1].DesTimeBase=srvPer; pshm->Coord[1].Q[0]++; - if(mode&4) - printf("Trigger count: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.3f\n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer); + if(mode&8) + printf("Trigger count: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.6f\n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer); rtLast=rtCur; scLast=scCur; } + printf("trigsync_func done\n"); } -RT_TASK trigsim_task; void trigsim_func(void *arg) { RT_TASK *curtask; @@ -162,11 +170,11 @@ void trigsim_func(void *arg) //rt_task_set_periodic(NULL, TM_NOW, 10*1E6); printf("Starting task %s \n", curtaskinfo.name); - int i; + int i,j=0; RTIME rtStart,rtSlice; - printf("sizeof RTTIME %d\n",sizeof(rtStart)); + //printf("sizeof RTTIME %d\n",sizeof(rtStart)); pshm = GetSharedMemPtr(); - pshm->Coord[1].Q[10]=0; + SIMFLAG0=0; rtStart=rt_timer_read(); for(i=0,rtSlice=rtStart;;i++) @@ -175,25 +183,39 @@ void trigsim_func(void *arg) rtSlice=rtStart+i*40.2*1E6; // a slice is 40 ms //rtSlice+=(rand()%(int)(1*1E6)); //0.1ms jitter rt_task_sleep_until(rtSlice); //in ns - pshm->Coord[1].Q[11]=1; - //while(rt_timer_read()-rtStart<1E6*40*i) - // rt_task_wait_period(NULL); - rt_task_sleep_until(rtSlice+5*1E6); //in ns - pshm->Coord[1].Q[11]=0; - - //if(pshm->Coord[1].DesTimeBase==0) - if(pshm->Coord[1].Q[10]==1) - { - rt_task_sleep_until(rtSlice+10*1E6); //in ns - pshm->Coord[1].Q[11]=1; - rt_task_sleep_until(rtSlice+20*1E6); //in ns - pshm->Coord[1].Q[11]=0; - pshm->Coord[1].Q[10]=0; - } - if(pshm->Coord[1].Q[10]=2) - break; if(mode&4) - putchar('.');fflush(stdout); + { + SIMFLAG1=1; + //while(rt_timer_read()-rtStart<1E6*40*i) + // rt_task_wait_period(NULL); + rt_task_sleep_until(rtSlice+5*1E6); //in ns + SIMFLAG1=0; + if(mode&8) + {putchar('.');fflush(stdout);} + } + //if(pshm->Coord[1].DesTimeBase==0) + if(SIMFLAG0==1) + { + if(j) + { + SIMFLAG0=0;j=0; + } + else if(mode&8) + {putchar('x');fflush(stdout);} + j++; + } + if(!pshm->Gather.Enable) + { + if((mode&4) && j<10) + { + putchar('o'); + j++; + continue; + } + break; + } + //if(pshm->Coord[1].Q[10]==2) + // break; } printf("trigsim_func done\n"); } @@ -203,6 +225,8 @@ void trigsim_func(void *arg) void trigsync_run() { + RT_TASK trigsync_task; + RT_TASK trigsim_task; const char* strSync="trigsync_task"; const char* strSim="trigsim_task"; //mode: @@ -241,40 +265,80 @@ void trigsync_run() } +int usage(const char* cmd) +{ + const char* s="\n\ +mode:\n\ + bit0:1: sync mode\n\ + bit1:2: simulate start trigger\n\ + bit2:4: simulate frame trigger\n\ + bit3:8: verbose\n\ +\n\ +simulate start trigger:\n\ +set pshm->Coord[1].Q[10]=1 to simulate a Jungfrau aquire start\n\ +\n\ +simulate frame trigger\n\ +is output to pshm->Coord[1].Q[11]\n\ + 1: synchronize real frame and start triggers\n\ + 3: synchronize real frame and simulated start triggers\n\ + 6: simulated frame and start triggers (no sync)\n\ + 7: synchronize simulated frame and start triggers\n\ +\n\ +in simulate mode:\n\ +set pshm->Coord[1].Q[10]=1 to simulate a Jungfrau aquire start\n\ +set pshm->Coord[1].Q[10]=2 to stop simulate trigger generation\n\ +Coord[1].Q[11] is the simulated frame trigger\n\ +\n\ +in synchronize mode\n\ +Coord[1].Q[0]=-2 : trigsync_func start, Wait for 'arm' trigger\n\ +Coord[1].Q[0]=-1 : got 'arm' trigger, wait frame trigger\n\ +Coord[1].Q[0]= 0 : got frame trigger 0\n\ +Coord[1].Q[0] is incremented at each trigger\n\ +sync task ends when Gather.Enable==0\n\ +"; + printf("usage:\n%s pt2ptTime mode\n",cmd); + puts(s); + return -1; +} int main(int argc, char *argv[]) { int err; int initialized=0; - char s[256]; int i; + char *s; + if(argc!=3) + return usage(argv[0]); + mtPt2Pt= strtof(argv[1], &s); + if (argv[1]==s) + return usage(argv[0]); + mode= (int)strtol(argv[2], &s, 10); + if (argv[2]==s) + return usage(argv[0]); 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); + //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; diff --git a/src/triggerSync/triggerSync.cbp b/src/triggerSync/triggerSync.cbp index adb156c..3307b17 100644 --- a/src/triggerSync/triggerSync.cbp +++ b/src/triggerSync/triggerSync.cbp @@ -25,12 +25,14 @@ + + diff --git a/src/triggerSync/triggerSync.layout b/src/triggerSync/triggerSync.layout index f8c0742..9eeaede 100644 --- a/src/triggerSync/triggerSync.layout +++ b/src/triggerSync/triggerSync.layout @@ -2,14 +2,14 @@ - + - + - + - +