352 lines
9.4 KiB
C
352 lines
9.4 KiB
C
//*-----------------------------------------------------------------------*
|
|
//| |
|
|
//| 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
|
|
|
|
//before start set: Gather.Enable=1
|
|
//to trigger a simulated start: Coord[1].Q[10]=1
|
|
|
|
//the simulated frame trigger comes at 40.2 ms period.
|
|
|
|
#include <stdio.h>
|
|
#include <gplib.h>
|
|
#include <math.h>
|
|
#include <signal.h>
|
|
#include <unistd.h>
|
|
#include <sys/mman.h>
|
|
#include <native/task.h>
|
|
#include <native/timer.h>
|
|
|
|
|
|
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 SIMFLAG0 (pshm->Coord[1].Q[10])
|
|
#define SIMFLAG1 (pshm->Coord[1].Q[11])
|
|
//#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)
|
|
|
|
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,loopPeriod/1.E6);
|
|
|
|
//Make the task periodic with a specified 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
|
|
unsigned int scStart,scLast,scCur,scDiff; //sc* are servo counts of the motion
|
|
float srvPer;
|
|
float mtAct,mtDes; //mt* motion time. Actual and desired. this is the accumulated time that controls motion speed
|
|
|
|
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;
|
|
|
|
rtStart=rt_timer_read();
|
|
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->Gather.Enable=2; //start gathering data
|
|
pshm->Coord[1].Q[0]=-1;
|
|
if(mode&4)
|
|
{
|
|
while(!SIMFLAG1)
|
|
rt_task_wait_period(NULL);
|
|
}
|
|
else
|
|
{
|
|
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&4)
|
|
{
|
|
while(SIMFLAG1)
|
|
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;
|
|
rtDiff=rtCur-rtLast;
|
|
scDiff=scCur-scLast;
|
|
|
|
mtAct+=scDiff*srvPer;
|
|
mtDes=i*mtPt2Pt;
|
|
srvPer=(mtPt2Pt+mtDes-mtAct)/scDiff;
|
|
pshm->Coord[1].DesTimeBase=srvPer;
|
|
pshm->Coord[1].Q[0]++;
|
|
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");
|
|
}
|
|
|
|
void trigsim_func(void *arg)
|
|
{
|
|
RT_TASK *curtask;
|
|
RT_TASK_INFO curtaskinfo;
|
|
int iret = 0;
|
|
curtask = rt_task_self();
|
|
rt_task_inquire(curtask, &curtaskinfo);
|
|
//Print the info
|
|
//printf("Starting task %s with period of %f ms ....\n", curtaskinfo.name,10*1E6/1E6);
|
|
//Make the task periodic with a specified loop period
|
|
//rt_task_set_periodic(NULL, TM_NOW, 10*1E6);
|
|
printf("Starting task %s \n", curtaskinfo.name);
|
|
|
|
int i,j=0;
|
|
RTIME rtStart,rtSlice;
|
|
//printf("sizeof RTTIME %d\n",sizeof(rtStart));
|
|
pshm = GetSharedMemPtr();
|
|
SIMFLAG0=0;
|
|
rtStart=rt_timer_read();
|
|
|
|
for(i=0,rtSlice=rtStart;;i++)
|
|
{
|
|
//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
|
|
if(mode&4)
|
|
{
|
|
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");
|
|
}
|
|
|
|
|
|
|
|
|
|
void trigsync_run()
|
|
{
|
|
RT_TASK trigsync_task;
|
|
RT_TASK trigsim_task;
|
|
const char* strSync="trigsync_task";
|
|
const char* strSim="trigsim_task";
|
|
//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");
|
|
|
|
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");
|
|
|
|
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");
|
|
|
|
}
|
|
|
|
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;
|
|
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);
|
|
|
|
trigsync_run();
|
|
|
|
CloseLibrary();
|
|
return !err;
|
|
|
|
if (initialized)
|
|
CloseLibrary();
|
|
|
|
return 0;
|
|
|
|
}
|