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

first running sync with rt-C-code

Browse Source
This commit is contained in:
zamofing_t
2019-01-22 14:41:16 +00:00
parent 7c2a2c5fe6
commit 66e2d038ba
5 changed files with 217 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

216
src/triggerSync/triggerSync.c
View File

@@ -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 <stdio.h>
#include <gplib.h>
@@ -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;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);
//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;

2
src/triggerSync/triggerSync.cbp
View File

@@ -25,12 +25,14 @@
<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" />
<search_path add="/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/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" />
<search_path add="/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/include" />
<remote_debugging>
<options conn_type="0" serial_baud="115200" ip_address="MOTTEST-CPPM-CRM0573" ip_port="2000" additional_cmds_before="file triggerSync" />
</remote_debugging>

8
src/triggerSync/triggerSync.layout
View File

@@ -2,14 +2,14 @@
<CodeBlocks_layout_file>
<FileVersion major="1" minor="0" />
<ActiveTarget name="Debug" />
<File name="triggerSync.c" open="1" top="0" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="triggerSync.c" open="1" top="0" tabpos="1" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="952" topLine="9" />
<Cursor1 position="5484" topLine="200" />
</Cursor>
</File>
<File name="Makefile" open="1" top="1" tabpos="3" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<File name="Makefile" open="1" top="1" tabpos="2" split="0" active="1" splitpos="0" zoom_1="0" zoom_2="0">
<Cursor>
<Cursor1 position="692" topLine="1" />
<Cursor1 position="122" topLine="0" />
</Cursor>
</File>
</CodeBlocks_layout_file>