static void fun(long none)
{
volatile double s, a[MAXDIM], b[MAXDIM];
long msg;
RTIME period, wait_delay, sync_time, aim_time;
*worst_lat = -2000000000;
wait_delay = nano2count(WAIT_DELAY);
period = nano2count(PERIOD);
for(msg = 0; msg < MAXDIM; msg++) {
a[msg] = b[msg] = 3.141592;
}
rtai_cli();
aim_time = rt_get_time();
sync_time = aim_time + wait_delay;
aim_time += period;
while (!rt_receive_if(NULL, &msg)) {
WAIT_AIM_TIME();
sync_time = rt_get_time();
msg = abs((long)(sync_time - aim_time));
sync_time = aim_time + wait_delay;
aim_time += period;
if (msg > *worst_lat) {
*worst_lat = msg;
}
s = dot(a,b, MAXDIM);
rt_busy_sleep(WORKING_TIME);
rt_sleep_until(sync_time);
}
rtai_sti();
}
static void rt_timers_manager(int dummy)
{
static unsigned long cr0;
RTIME now;
struct rt_tasklet_struct *tmr, *timer;
unsigned long flags;
int priority, used_fpu;
while (1) {
rt_sleep_until((timers_list.next)->firing_time);
now = timers_manager.resume_time + tuned.timers_tol[0];
// find all the timers to be fired, in priority order
while (1) {
used_fpu = 0;
tmr = timer = &timers_list;
priority = RT_LOWEST_PRIORITY;
flags = rt_spin_lock_irqsave(&timers_lock);
while ((tmr = tmr->next)->firing_time <= now) {
if (tmr->priority < priority) {
priority = (timer = tmr)->priority;
}
}
timers_manager.priority = priority;
rt_spin_unlock_irqrestore(flags, &timers_lock);
if (timer == &timers_list) {
break;
}
if (!timer->period) {
flags = rt_spin_lock_irqsave(&timers_lock);
(timer->next)->prev = timer->prev;
(timer->prev)->next = timer->next;
timer->next = timer->prev = timer;
rt_spin_unlock_irqrestore(flags, &timers_lock);
} else {
set_timer_firing_time(timer, timer->firing_time + timer->period);
}
if (!timer->task) {
if (!used_fpu && timer->uses_fpu) {
used_fpu = 1;
save_cr0_and_clts(cr0);
save_fpenv(timers_manager.fpu_reg);
}
timer->handler(timer->data);
} else {
rt_task_resume(timer->task);
}
}
if (used_fpu) {
restore_fpenv(timers_manager.fpu_reg);
restore_cr0(cr0);
}
// set next timers_manager priority according to the highest priority timer
asgn_min_prio();
// if no more timers in timers_struct remove timers_manager from tasks list
}
}
static void rt_timers_manager(long cpuid)
{
RTIME now;
RT_TASK *timer_manager;
struct rtdm_timer_struct *tmr, *timer, *timerl;
spinlock_t *lock;
unsigned long flags, timer_tol;
int priority;
timer_manager = &timers_manager[LIST_CPUID];
timerl = &timers_list[LIST_CPUID];
lock = &timers_lock[LIST_CPUID];
timer_tol = tuned.timers_tol[LIST_CPUID];
while (1) {
rt_sleep_until((timerl->next)->firing_time);
now = rt_get_time() + timer_tol;
while (1) {
tmr = timer = timerl;
priority = RT_SCHED_LOWEST_PRIORITY;
flags = rt_spin_lock_irqsave(lock);
while ((tmr = tmr->next)->firing_time <= now) {
if (tmr->priority < priority) {
priority = (timer = tmr)->priority;
}
}
rt_spin_unlock_irqrestore(flags, lock);
if (timer == timerl) {
if (timer_manager->priority > TimersManagerPrio) {
timer_manager->priority = TimersManagerPrio;
}
break;
}
timer_manager->priority = priority;
flags = rt_spin_lock_irqsave(lock);
rem_timer(timer);
if (timer->period) {
timer->firing_time += timer->period;
enq_timer(timer);
}
rt_spin_unlock_irqrestore(flags, lock);
timer->handler(timer->data);
}
asgn_min_prio(LIST_CPUID);
}
}
static void rt_timers_manager(long cpuid)
{
RTIME now;
RT_TASK *timer_manager;
struct rt_tasklet_struct *tmr, *timer, *timerl;
spinlock_t *lock;
unsigned long flags, timer_tol;
int priority, used_fpu;
timer_manager = &timers_manager[LIST_CPUID];
timerl = &timers_list[LIST_CPUID];
lock = &timers_lock[LIST_CPUID];
timer_tol = tuned.timers_tol[LIST_CPUID];
while (1) {
int retval;
retval = rt_sleep_until((timerl->next)->firing_time);
// now = timer_manager->resume_time + timer_tol;
now = rt_get_time() + timer_tol;
// find all the timers to be fired, in priority order
while (1) {
used_fpu = 0;
tmr = timer = timerl;
priority = RT_SCHED_LOWEST_PRIORITY;
flags = rt_spin_lock_irqsave(lock);
while ((tmr = tmr->next)->firing_time <= now) {
if (tmr->priority < priority) {
priority = (timer = tmr)->priority;
}
}
rt_spin_unlock_irqrestore(flags, lock);
if (timer == timerl) {
if (timer_manager->priority > TimersManagerPrio) {
timer_manager->priority = TimersManagerPrio;
}
break;
}
timer_manager->priority = priority;
#if 1
flags = rt_spin_lock_irqsave(lock);
rem_timer(timer);
if (timer->period) {
timer->firing_time += timer->period;
enq_timer(timer);
}
rt_spin_unlock_irqrestore(flags, lock);
#else
if (!timer->period) {
flags = rt_spin_lock_irqsave(lock);
rem_timer(timer);
rt_spin_unlock_irqrestore(flags, lock);
} else {
set_timer_firing_time(timer, timer->firing_time + timer->period);
}
#endif
// if (retval != RTE_TMROVRN) {
tmr->overrun = 0;
if (!timer->task) {
if (!used_fpu && timer->uses_fpu) {
used_fpu = 1;
save_fpcr_and_enable_fpu(linux_cr0);
save_fpenv(timer_manager->fpu_reg);
}
timer->handler(timer->data);
} else {
rt_task_resume(timer->task);
}
// } else {
// tmr->overrun++;
// }
}
if (used_fpu) {
restore_fpenv(timer_manager->fpu_reg);
restore_fpcr(linux_cr0);
}
// set next timers_manager priority according to the highest priority timer
asgn_min_prio(LIST_CPUID);
// if no more timers in timers_struct remove timers_manager from tasks list
}
}
int main(int argc, char *argv[])
{
RTIME until;
RT_TASK *task;
comedi_insn insn[NCHAN];
unsigned int read_chan[NICHAN] = { 2, 3, 4, 5, 6 };
unsigned int write_chan[NOCHAN] = { 0, 1 };
#if !SINGLE_INSN
comedi_insnlist ilist = { NCHAN, insn };
#endif
#if SIN_FREQ
lsampl_t sinewave;
double omega = (2.0*M_PI*SIN_FREQ)/1.0E9;
double actualtime;
#endif
lsampl_t *hist;
lsampl_t data[NCHAN];
long i, k, n, retval;
int toggle;
FILE *fp;
struct sockaddr_in addr;
signal(SIGKILL, endme);
signal(SIGTERM, endme);
hist = malloc(SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
memset(hist, 0, SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
start_rt_timer(0);
task = rt_task_init_schmod(nam2num("MYTASK"), 1, 0, 0, SCHED_FIFO, 0xF);
daqnode = 0;
if (argc == 2 && strstr(argv[1], "RcvNode=")) {
inet_aton(argv[1] + 8, &addr.sin_addr);
daqnode = addr.sin_addr.s_addr;
}
if (!daqnode) {
inet_aton("127.0.0.1", &addr.sin_addr);
daqnode = addr.sin_addr.s_addr;
}
while ((daqport = rt_request_port(daqnode)) <= 0 && daqport != -EINVAL);
mlockall(MCL_CURRENT | MCL_FUTURE);
printf("COMEDI INSN%s TEST BEGINS: SAMPLING FREQ: %d, RUN TIME: %d, NODE: %x, PORT: %d.\n", SINGLE_INSN ? "" : "LIST", SAMP_FREQ, RUN_TIME, daqnode, daqport);
rt_make_hard_real_time();
if (init_board()) {;
printf("Board initialization failed.\n");
return 1;
}
for (i = 0; i < NICHAN; i++) {
BUILD_AREAD_INSN(insn[i], subdevai, data[i], 1, read_chan[i], AI_RANGE, AREF_GROUND);
}
for (i = 0; i < NOCHAN; i++) {
BUILD_AWRITE_INSN(insn[NICHAN + i], subdevao, data[NICHAN + i], 1, write_chan[i], AO_RANGE, AREF_GROUND);
}
until = rt_get_time();
for (toggle = n = k = 0; k < SAMP_FREQ*RUN_TIME && !end /*SAMP_FREQ*RUN_TIME && !end*/; k++) {
#if SIN_FREQ
actualtime = count2nano(rt_get_time());
sinewave = (int) (maxdatao/8*sin(omega*actualtime));
data[NICHAN] = sinewave+maxdatao/2;
data[NICHAN + 1] = -sinewave+maxdatao/2;
#else
data[NICHAN] = toggle*maxdatao/2;
data[NICHAN + 1] = (1 - toggle)*maxdatao/2;
toggle = 1 - toggle;
#endif
#if SINGLE_INSN
for (i = 0; i < NCHAN; i++) {
if ((retval = RT_comedi_do_insn(daqnode, daqport, dev, insn + i)) > 0) {
hist[n++] = data[i];
} else {
printf("Comedi insn failed # %ld out of %d instructions, retval %ld.\n", i, NCHAN, retval);
break;
}
}
#else
if ((retval = RT_comedi_do_insnlist(daqnode, daqport, dev, &ilist)) == NCHAN) {
for (i = 0; i < NCHAN; i++) {
hist[n++] = data[i];
}
} else {
printf("Comedi insnlist processed only %lu out of %d instructions.\n", retval, NCHAN);
break;
}
#endif
rt_sleep_until(until += nano2count(SAMP_TIME));
}
RT_comedi_cancel(daqnode, daqport, dev, subdevai);
RT_comedi_cancel(daqnode, daqport, dev, subdevao);
RT_comedi_data_write(daqnode, daqport, dev, subdevao, 0, 0, AREF_GROUND, 2048);
RT_comedi_data_write(daqnode, daqport, dev, subdevao, 1, 0, AREF_GROUND, 2048);
RT_comedi_close(daqnode, daqport, dev);
printf("COMEDI INSN%s ENDS.\n", SINGLE_INSN ? "" : "LIST");
fp = fopen("rec.dat", "w");
for (n = k = 0; k < SAMP_FREQ*RUN_TIME; k++) {
fprintf(fp, "# %ld: ", k);
for (i = 0; i < NCHAN; i++) {
fprintf(fp, "%d\t", hist[n++]);
}
fprintf(fp, "\n");
}
fclose(fp);
free(hist);
stop_rt_timer();
rt_make_soft_real_time();
rt_task_delete(task);
return 0;
}
int main(void)
{
RTIME until;
RT_TASK *task;
comedi_insn insn[NCHAN];
unsigned int read_chan[NICHAN] = { 0 };
unsigned int write_chan[NOCHAN] = { 0 };
#if !SINGLE_INSN
comedi_insnlist ilist = { NCHAN, insn };
#endif
#if SIN_FREQ
lsampl_t sinewave;
double omega = (2.0*M_PI*SIN_FREQ)/1.0E9;
double actualtime;
#endif
lsampl_t *hist;
lsampl_t data[NCHAN];
long i, k, n, retval;
int toggle;
FILE *fp;
double tms[2];
#if SINGLE_INSN
printf("single insn true\n");
#endif
#if SIN_FREQ
printf(" true\n");
#endif
signal(SIGKILL, endme);
signal(SIGTERM, endme);
hist = malloc(SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
memset(hist, 0, SAMP_FREQ*RUN_TIME*NCHAN*sizeof(lsampl_t) + 1000);
start_rt_timer(0);
task = rt_task_init_schmod(nam2num("MYTASK"), 1, 0, 0, SCHED_FIFO, 0xF);
printf("COMEDI INSN%s TEST BEGINS: SAMPLING FREQ: %d, RUN TIME: %d.\n", SINGLE_INSN ? "" : "LIST", SAMP_FREQ, RUN_TIME);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
if (init_board()) {;
printf("Board initialization failed.\n");
return 1;
}
for (i = 0; i < NICHAN; i++) {
BUILD_AREAD_INSN(insn[i], subdevai, data[i], 1, read_chan[i], AI_RANGE, AREF_GROUND);
}
for (i = 0; i < NOCHAN; i++) {
BUILD_AWRITE_INSN(insn[NICHAN + i], subdevao, data[NICHAN + i], 1, write_chan[i], AO_RANGE, AREF_GROUND);
}
printf("done building.\n"); fflush (stdout);
until = rt_get_time();
for (toggle = n = k = 0; k < SAMP_FREQ*RUN_TIME && !end; k++) {
#if SIN_FREQ
actualtime = count2nano(rt_get_time());
if(k<2) tms[k] = actualtime;
sinewave = (int) (maxdatao/8*sin(omega*actualtime));
data[NICHAN] = sinewave+maxdatao/2;
//data[NICHAN + 1] = -sinewave+maxdatao/2;
#else
data[NICHAN] = toggle*maxdatao/2;
data[NICHAN + 1] = (1 - toggle)*maxdatao/2;
toggle = 1 - toggle;
#endif
#if SINGLE_INSN
for (i = 0; i < NCHAN; i++) {
if ((retval = comedi_do_insn(dev, insn + i)) > 0) {
hist[n++] = data[i];
} else {
printf("Comedi insn failed # %ld out of %d instructions, retval %ld.\n", i, NCHAN, retval);
break;
}
}
#else
if ((retval = rt_comedi_do_insnlist(dev, &ilist)) == NCHAN) {
for (i = 0; i < NCHAN; i++) {
hist[n++] = data[i];
}
} else {
printf("Comedi insnlist processed only %lu out of %d instructions.\n", retval, NCHAN);
break;
}
#endif
rt_sleep_until(until += nano2count(SAMP_TIME));
}
comedi_cancel(dev, subdevai);
comedi_cancel(dev, subdevao);
comedi_data_write(dev, subdevao, 0, 0, AREF_GROUND, 2048);
comedi_data_write(dev, subdevao, 1, 0, AREF_GROUND, 2048);
comedi_close(dev);
printf("COMEDI INSN%s ENDS.\n", SINGLE_INSN ? "" : "LIST");
printf("t1: %g\n", tms[0]);
printf("t2: %g\n", tms[1]);
printf("tdiff: %g\n", tms[1]-tms[0]);
fp = fopen("rec.dat", "w");
for (n = k = 0; k < SAMP_FREQ*RUN_TIME; k++) {
fprintf(fp, "# %ld: ", k);
for (i = 0; i < NCHAN; i++) {
fprintf(fp, "%d\t", hist[n++]);
}
fprintf(fp, "\n");
}
fclose(fp);
free(hist);
stop_rt_timer();
rt_make_soft_real_time();
rt_task_delete(task);
return 0;
}
