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

work on sync process

Browse Source
This commit is contained in:
zamofing_t
2019-01-21 21:25:50 +00:00
parent 069e7795e5
commit 7c2a2c5fe6
4 changed files with 116 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/triggerSync/Makefile
View File

@@ -1,5 +1,5 @@
HOST=MOTTEST-CPPM-CRM0573
#HOST=MOTTEST-CPPM-CRM0573
HOST=SAR-CPPM-EXPMX1
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

188
src/triggerSync/triggerSync.c
View File

@@ -1,6 +1,32 @@
// >>>>>>> https://www.ashwinnarayan.com/post/xenomai-realtime-programming-part-2/
// >>>>>>> https://xenomai.org/documentation/xenomai-2.4/html/api/group__task.html
//mode:
// bit0: sync mode
// bit1: simulate mode
// bit2: verbose
// 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
#include <stdio.h>
#include <gplib.h>
#include <math.h>
@@ -12,16 +38,15 @@
extern struct SHM *pshm;
static char mode=0;
static float mtPt2Pt=40.f; //motion point to point time
#define CLOCK_RES 1e-9 //Clock resolution is 1 ns by default
#define LOOP_PERIOD 1e4 //Expressed in ticks
//RTIME period = 10000000;
//#define FLAG0 (gate3_1->Chan[0].Status&0x8000000)
//#define FLAG1 (gate3_1->Chan[1].Status&0x8000000)
#define FLAG0 (pshm->P[0])
#define FLAG1 (pshm->P[1])
#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)
RT_TASK trigsync_task;
void trigsync_func(void *arg)
@@ -29,16 +54,17 @@ void trigsync_func(void *arg)
RT_TASK *curtask;
RT_TASK_INFO curtaskinfo;
int iret = 0;
int loopPeriod=1e5;//Expressed in ticks
curtask = rt_task_self();
rt_task_inquire(curtask, &curtaskinfo);
//Print the info
printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,LOOP_PERIOD/1000000.f);
printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,loopPeriod/1.E6);
//Make the task periodic with a specified loop period
rt_task_set_periodic(NULL, TM_NOW, LOOP_PERIOD);
rt_task_set_periodic(NULL, TM_NOW, loopPeriod);
int i;
RTIME rtStart,rtLast,rtCur,rtDiff; //rt* is real time of the rt-process
@@ -52,35 +78,58 @@ void trigsync_func(void *arg)
struct GateArray3* gate3_1=GetGate3MemPtr(1);
pshm = GetSharedMemPtr();
pshm->Coord[1].Q[0]=-2;
srvPer=pshm->ServoPeriod;
if(pshm->Gather.Enable==1)
pshm->Gather.Enable=2; //start record at flag0' trigger
usleep(1000000); //wait 1 sec(record before motion)
rtStart=rt_timer_read();
printf("Wait for 'arm' event:\n");
pshm->Coord[1].Q[0]=-10;
while(!FLAG0)
rt_task_wait_period(NULL);
printf("Wait for 'arm' event...\n");
if(mode&2)
{
while(!SIMFLAG0)
rt_task_wait_period(NULL);
}
else
{
while(!FLAG0)
rt_task_wait_period(NULL);
}
printf("Flag 0: %.5f ms\n", (rt_timer_read() - rtStart)/1E6);
pshm->Coord[1].Q[0]=-9;
while(!FLAG1)
rt_task_wait_period(NULL);
pshm->Coord[1].Q[0]=-1;
if(mode&2)
{
while(!SIMFLAG1)
rt_task_wait_period(NULL);
}
else
{
while(!FLAG1)
rt_task_wait_period(NULL);
}
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]=1;
pshm->Coord[1].Q[0]=0;
pshm->Coord[1].DesTimeBase=srvPer; //start motion at default speed
for(i=1;pshm->Gather.Enable;i++)
{
while(FLAG1)
rt_task_wait_period(NULL);
if(mode&2)
{
while(SIMFLAG1)
rt_task_wait_period(NULL);
while(!FLAG1)
rt_task_wait_period(NULL);
while(!SIMFLAG1)
rt_task_wait_period(NULL);
}
else
{
while(FLAG1)
rt_task_wait_period(NULL);
while(!FLAG1)
rt_task_wait_period(NULL);
}
//FEL shot arrived
rtCur = rt_timer_read();
scCur=pshm->ServoCount;
@@ -92,25 +141,11 @@ void trigsync_func(void *arg)
srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff;
pshm->Coord[1].DesTimeBase=srvPer;
pshm->Coord[1].Q[0]++;
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&4)
printf("Trigger count: %d, rtDiff: %.3f ms scDiff %d mtAct %.3f mtDes %.3f srvPer %.3f\n", i, rtDiff/1E6,scDiff, mtAct,mtDes,srvPer);
rtLast=rtCur;
scLast=scCur;
}
/* 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);*/
}
RT_TASK trigsim_task;
@@ -131,7 +166,7 @@ void trigsim_func(void *arg)
RTIME rtStart,rtSlice;
printf("sizeof RTTIME %d\n",sizeof(rtStart));
pshm = GetSharedMemPtr();
pshm->Coord[1].Q[10]=0;
rtStart=rt_timer_read();
for(i=0,rtSlice=rtStart;;i++)
@@ -139,34 +174,28 @@ void trigsim_func(void *arg)
//rtSlice=rtStart+i*40*1E6; // a slice is 40 ms
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->P[1]=1;
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->P[1]=0;
pshm->Coord[1].Q[11]=0;
//if(pshm->Coord[1].Q[0]==-10)
if(pshm->Coord[1].DesTimeBase==0)
//if(pshm->Coord[1].DesTimeBase==0)
if(pshm->Coord[1].Q[10]==1)
{
rt_task_sleep_until(rtSlice+10*1E6); //in ns
pshm->P[0]=1;
pshm->Coord[1].Q[11]=1;
rt_task_sleep_until(rtSlice+20*1E6); //in ns
pshm->P[0]=0;
pshm->Coord[1].Q[11]=0;
pshm->Coord[1].Q[10]=0;
}
if(pshm->Coord[1].Q[0]==-1)
if(pshm->Coord[1].Q[10]=2)
break;
putchar('.');fflush(stdout);
if(mode&4)
putchar('.');fflush(stdout);
}
if(pshm->Gather.Enable)
{
//pshm->Gather.Enable=0;
}
pshm->P[1]=1;
rt_task_sleep(1*1E6); //in ns
pshm->P[1]=0;
printf("trigsim_func done:\n");
printf("trigsim_func done\n");
}
@@ -176,33 +205,38 @@ void trigsync_run()
{
const char* strSync="trigsync_task";
const char* strSim="trigsim_task";
pshm->P[0]=0;
pshm->P[1]=0;
pshm->Coord[1].Q[0]=0;
//if(!pshm->Gather.Enable)
// pshm->Gather.Enable=1;
//mode:
// bit0: sync mode
// bit1: simulate mode
//Lock the memory to avoid memory swapping for this program
mlockall(MCL_CURRENT | MCL_FUTURE);
printf("Starting rt task...\n");
//Create the real time task
rt_task_create(&trigsync_task, strSync, 0, 50, T_JOINABLE);
//Since task starts in suspended mode, start task
rt_task_start(&trigsync_task, &trigsync_func, 0);
rt_task_create(&trigsim_task, strSim, 0, 50, T_JOINABLE);
rt_task_start(&trigsim_task, &trigsim_func, 0);
if(mode&1)
{
rt_task_create(&trigsync_task, strSync, 0, 50, T_JOINABLE); //Create the real time task
rt_task_start(&trigsync_task, &trigsync_func, 0); //Since task starts in suspended mode, start task
}
if(mode&2)
{
rt_task_create(&trigsim_task, strSim, 0, 50, T_JOINABLE);
rt_task_start(&trigsim_task, &trigsim_func, 0);
}
printf("Wait end of rt_task\n");
rt_task_join(&trigsync_task);
printf("trigsync_task done\n");
pshm->Coord[1].Q[0]=-1;
rt_task_join(&trigsim_task);
printf("trigsim_task done\n");
if(mode&1)
{
rt_task_join(&trigsync_task);
printf("trigsync_task done\n");
}
if(mode&2)
{
rt_task_join(&trigsim_task);
printf("trigsim_task done\n");
}
printf("rt_task ended\n");
}