sf-mx/PBSwissMX
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add sample c realtime code

Browse Source
This commit is contained in:
zamofing_t
2019-01-15 11:34:00 +01:00
parent 5aacb85c8d
commit 29b6af46fe
8 changed files with 362 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

125
python/MXMotion.py
View File

@@ -36,126 +36,53 @@ class MotionBase:
self.gather=gather self.gather=gather
self.verbose=verbose 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 '''setup the timing synchronization for the motion program
mode=0 : no sync at all mode=0 : no sync at all
mode=1 : synchronize start mode=1 : synchronize start
mode=2 : synchronize start and adapt motion speed mode=2 : synchronize start and adapt motion speed
this function generates the code blocks: this function generates the code blocks:
self.sync_wait and self.sync_run self.sync_wait and self.sync_run
sync_wait can be put in the program to force a timing sync sync_wait can be put in the program to force a timing sync
sync_run are the commands to run the whole program sync_run are the commands to run the whole program
''' '''
if mode == 0: if mode==0:
try: try:
del self.sync_prg del self.sync_prg
except AttributeError: except AttributeError:
pass pass
self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, crdId=crdId) 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 # code block to insert in the program
# - waits untis <flag> is false # - waits untis <flag> is false
# - waits raising edge of <flag> and the sets DesTimeBase=ServoPeriod # - waits raising edge of <flag> and the sets DesTimeBase=ServoPeriod
# flag ='PowerBrick[0].GpioData[0].0.1==1' # 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 = ''' prg = '''
//L0=Sys.PhaseCount
//send 1"sync0 %d:%d\\n",Sys.PhaseCount,Sys.PhaseCount-L0
Coord[1].Q[1]=1 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 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) '''.format(plcId=plcId, crdId=crdId, flag0=flag0, flag1=flag1)
self.sync_prg = prg self.sync_prg = prg
self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId) self.sync_run = '&{crdId}b{prgId}r'''.format(prgId=prgId, plcId=plcId, crdId=crdId)
if mode==2: elif mode==2:
#frequence jitter 50Hz Swissgrid: #frequence jitter 50Hz Swissgrid:
#https://www.swissgrid.ch/de/home/operation/grid-data/current-data.html# #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): def run(self):
'runs the code sync_run which has been generated with setup_sync()' 'runs the code sync_run which has been generated with setup_sync()'

17
python/shapepath.py
View File

@@ -616,7 +616,8 @@ class ShapePath(MotionBase):
hl += ax.plot(jitter*ts, 'b-',label='jitter') hl += ax.plot(jitter*ts, 'b-',label='jitter')
ax.xaxis.set_label_text('position idx') ax.xaxis.set_label_text('position idx')
ax.yaxis.set_label_text('jitter motion (ms)') 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() plt.show()
def bode_plot(self,xy=(0,1),mode=25,db=True): def bode_plot(self,xy=(0,1),mode=25,db=True):
@@ -724,7 +725,7 @@ if __name__=='__main__':
def run_test(args): def run_test(args):
args.host=None #args.host=None
if args.host is None: if args.host is None:
comm=gather=None comm=gather=None
else: else:
@@ -767,7 +768,7 @@ if __name__=='__main__':
#sp.plot_gather() #sp.plot_gather()
#return #return
xy=False 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]) #print(sp.points[:,0])
#sp.gen_swissmx_points(width=1000, ofs=(-500, 0)); #sp.gen_swissmx_points(width=1000, ofs=(-500, 0));
#sp.points=np.array([[0.,1.],[1.,0.],[0.,-1.],[-1.,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.]]) [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=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.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.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) exit(0)
# fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S') # fn='/home/zamofing_t/Documents/prj/SwissFEL/epics_ioc_modules/ESB_MX/python/data/'+time.strftime('%y-%m-%d-%H_%M_%S')

55
src/triggerSync/Makefile Normal file
View File

@@ -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

32
src/triggerSync/Readme.md Normal file
View File

@@ -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]
```

15
src/triggerSync/c-sample.layout Normal file
View File

@@ -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>

40
src/triggerSync/c-samples.cbp Normal file
View File

@@ -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>

15
src/triggerSync/c-samples.layout Normal file
View File

@@ -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>

167
src/triggerSync/sampleCode.c Normal file
View File

@@ -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;
}