void *CommandClock_task(void *args)
{
RT_TASK *mytask;
unsigned long command;
char R = 'R';
int ackn = 0;
RT_TASK *get = (RT_TASK *)0, *put = (RT_TASK *)0, *task;
if (!(mytask = rt_thread_init(nam2num("CLKTSK"), 1, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT TASK CommandClock_task\n");
exit(1);
}
printf("INIT TASK CommandClock_task %p.\n", mytask);
mlockall(MCL_CURRENT | MCL_FUTURE);
Clockstatus = stopped;
while (ackn != ('a' + 'b')) {
task = rt_receive((RT_TASK *)0, &command);
switch (command) {
case 'b':
get = task;
ackn += command;
break;
case 'a':
put = task;
ackn += command;
break;
}
}
rt_return(put, command);
rt_return(get, command);
while(1) {
switch (Clockstatus) {
case stopped:
rt_receive(put, &command);
if (command == 'R') {
Clockstatus = running;
}
break;
case running:
if (rt_receive_if(put, &command)) {
if (command == 'T') {
Clockstatus = stopped;
}
} else {
command = R;
}
break;
}
rt_send(get, command);
if (command == 'F') {
goto end;
}
}
end:
rt_task_delete(mytask);
printf("END TASK CommandClock_task %p.\n", mytask);
return 0;
}
static void Display_task(long t)
{
unsigned long command;
int ackn = 0;
RT_TASK *get = (RT_TASK *)0, *tput = (RT_TASK *)0, *hput = (RT_TASK *)0, *task;
while (ackn != ('g' + 'p' + 'P')) {
task = rt_receive((RT_TASK *)0, &command);
switch (command) {
case 'g':
get = task;
ackn += command;
break;
case 'p':
tput = task;
ackn += command;
break;
case 'P':
hput = task;
ackn += command;
break;
}
}
rt_return(get, command);
rt_return(tput, command);
rt_return(hput, command);
while(1) {
cpu_used[hard_cpu_id()]++;
task = rt_receive(0, &command);
if (task == tput || task == hput) {
rt_send(get, command);
}
}
}
static void CommandChrono_task(long t)
{
RTIME fiveSeconds = nano2count(FIVE_SECONDS);
unsigned long command;
unsigned int buffered = 0;
unsigned int C = 'C';
unsigned int R = 'R';
int ackn = 0;
RT_TASK *get = (RT_TASK *)0, *put = (RT_TASK *)0, *task;
Chronostatus = stoppedInitial;
while (ackn != ('c' + 'd')) {
task = rt_receive((RT_TASK *)0, &command);
switch (command) {
case 'd':
get = task;
ackn += command;
break;
case 'c':
put = task;
ackn += command;
break;
}
}
rt_return(put, command);
rt_return(get, command);
while(1) {
cpu_used[hard_cpu_id()]++;
switch (Chronostatus) {
case stoppedInitial:
if (buffered) {
command = buffered;
buffered = 0;
} else {
rt_receive(put, &command);
}
Chronostatus = running;
break;
case running:
if (rt_receive_if(put, &command)) {
if (command == 'E') {
Chronostatus = stoppedFinal;
}
} else {
command = C;
}
break;
case stoppedFinal:
Chronostatus = stoppedInitial;
if (rt_receive_timed(put, &command, fiveSeconds) > 0) {
buffered = command;
}
command = R;
break;
}
rt_send(get, command);
}
}
int main(void)
{
RT_TASK *spktsk, *plrtsk;
RTIME period;
MBX *mbx;
char data, temp;
unsigned int msg, i;
// ioperm(PORT_ADR, 1, 1);
iopl(3);
if (!(spktsk = rt_task_init_schmod(nam2num("SPKTSK"), 1, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT SPEAKER TASK\n");
exit(1);
}
mbx = rt_mbx_init(nam2num("SNDMBX"), 4000);
printf("\nSPEAKER TASK RUNNING\n");
rt_set_oneshot_mode();
start_rt_timer(0);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
period = nano2count(PERIOD);
rt_task_make_periodic(spktsk, rt_get_time() + 5*period, period);
for (i = 0; i < 100; i++) {
plrtsk = rt_receive(0, &msg);
rt_return(plrtsk, msg);
}
while(1) {
if (!rt_mbx_receive_if(mbx, &data, 1)) {
data = filter(data);
temp = inb(PORT_ADR);
temp &= 0xfd;
temp |= (data & 1) << 1;
outb(temp, PORT_ADR);
}
rt_task_wait_period();
if ((plrtsk = rt_receive_if(0, &msg))) {
rt_return(plrtsk, msg);
break;
}
}
rt_sleep(nano2count(100000000));
rt_make_soft_real_time();
rt_mbx_delete(mbx);
stop_rt_timer();
rt_task_delete(spktsk);
printf("\nSPEAKER TASK STOPS\n");
return 0;
}
void *Display_task(void *args)
{
RT_TASK *mytask;
unsigned long command;
int ackn = 0;
RT_TASK *get = (RT_TASK *)0, *tput = (RT_TASK *)0, *hput = (RT_TASK *)0, *task;
if (!(mytask = rt_thread_init(nam2num("DSPTSK"), 1, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT TASK Display_task\n");
exit(1);
}
printf("INIT TASK Display_task %p.\n", mytask);
mlockall(MCL_CURRENT | MCL_FUTURE);
while (ackn != ('g' + 'p' + 'P')) {
task = rt_receive((RT_TASK *)0, &command);
switch (command) {
case 'g':
get = task;
ackn += command;
break;
case 'p':
tput = task;
ackn += command;
break;
case 'P':
hput = task;
ackn += command;
break;
}
}
rt_return(get, command);
rt_return(tput, command);
rt_return(hput, command);
while(1) {
task = rt_receive(0, &command);
if (task == tput || task == hput) {
rt_send(get, command);
if (((char *)command)[1] == 101) {
goto end;
}
}
}
end:
rt_task_delete(mytask);
printf("END TASK Display_task %p.\n", mytask);
return 0;
}
static void driver(int t)
{
RT_TASK *thread[NTASKS];
int i, l;
unsigned int msg = 0;
RTIME now;
for (i = 1; i < NTASKS; i++) {
thread[0] = rt_receive(0, &msg);
thread[msg] = thread[0];
}
for (i = 1; i < NTASKS; i++) {
rt_return(thread[i], i);
}
now = rt_get_time();
rt_task_make_periodic(rt_whoami(), now + NTASKS*tick_period, tick_period);
msg = 0;
l = LOOPS;
while(l--) {
for (i = 1; i < NTASKS; i++) {
cpu_used[hard_cpu_id()]++;
if (i%2) {
rt_rpc(thread[i], msg, &msg);
} else {
rt_send(thread[i], msg);
msg = 1 - msg;
}
rt_task_wait_period();
}
}
for (i = 1; i < NTASKS; i++) {
rt_send(thread[i], END);
}
}
int main(int argc, char *argv[])
{
unsigned long msg;
int prio, bprio;
if (argc > 1) {
USE_RPC = atoi(argv[1]);
SNDBRCV = atoi(argv[2]);
HARDMAIN = atoi(argv[3]);
}
if (!(maintask = rt_task_init_schmod(0xcacca, 1, 0, 0, SCHED_FIFO, 0x1))) {
rt_printk("CANNOT INIT MAIN\n");
exit(1);
}
start_rt_timer(0);
rt_printk("MAIN INIT\n");
pthread_create(&thread, NULL, thread_fun, NULL);
mlockall(MCL_CURRENT | MCL_FUTURE);
if (HARDMAIN) {
rt_make_hard_real_time();
}
rt_get_priorities(maintask, &prio, &bprio);
rt_printk("TEST: %s, %s, %d\n", USE_RPC ? "WITH RPC" : "WITH SUSP/RESM", SNDBRCV ? "SEND BEFORE RECEIVE" : "RECEIVE BEFORE SEND", prio);
if (SNDBRCV) {
rt_sleep(nano2count(100000000));
}
rt_get_priorities(maintask, &prio, &bprio);
rt_printk("MAIN REC %d\n", prio);
if (USE_RPC) {
RT_TASK *task;
task = rt_receive(0, &msg);
rt_get_priorities(maintask, &prio, &bprio);
rt_printk("MAIN RET %d\n", prio);
rt_return(task, 0);
} else {
rt_receive(0, &msg);
rt_get_priorities(maintask, &prio, &bprio);
rt_printk("MAIN RES %d\n", prio);
rt_task_resume(funtask);
}
rt_get_priorities(maintask, &prio, &bprio);
rt_printk("MAIN DONE %d\n", prio);
stop_rt_timer();
rt_task_delete(maintask);
rt_printk("MAIN END\n");
return 0;
}
static void fun(int t)
{
unsigned int msg;
rt_rpc(&thread[0], t, &msg);
while(msg != END) {
cpu_used[hard_cpu_id()]++;
rt_receive(&thread[0], &msg);
rt_leds_set_mask(1,msg);
if (rt_isrpc(&thread[0])) {
rt_return(&thread[0], 1 - msg);
}
}
rt_leds_set_mask(1,0);
}
static void *intr_handler(void *args)
{
RT_TASK *mytask, *master;
RTIME period;
MBX *mbx;
char data = 'G';
char temp;
unsigned int msg;
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
// rt_allow_nonroot_hrt();
ioperm(PORT_ADR, 1, 1);
if (!(mytask = rt_task_init_schmod(nam2num("SOUND"), 1, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT SOUND TASK\n");
exit(1);
}
mbx = rt_get_adr(nam2num("SNDMBX"));
mlockall(MCL_CURRENT | MCL_FUTURE);
printf("\nINIT SOUND TASK %p\n", mytask);
rt_make_hard_real_time();
period = nano2count(PERIOD);
rt_mbx_send(mbx, &data, 1);
rt_task_make_periodic(mytask, rt_get_time() + 100*period, period);
while(1) {
if (!rt_mbx_receive_if(mbx, &data, 1)) {
data = filter(data);
temp = inb(PORT_ADR);
temp &= 0xfd;
temp |= (data & 1) << 1;
outb(temp, PORT_ADR);
}
rt_task_wait_period();
if ((master = rt_receive_if(0, &msg))) {
rt_return(master, msg);
break;
}
}
rt_make_soft_real_time();
rt_task_delete(mytask);
printf("\nEND SOUND TASK %p\n", mytask);
return 0;
}
static void pend_task (long t)
{
unsigned long msg;
while(1) {
switch (change) {
case 0:
rt_task_suspend(thread + t);
break;
case 1:
rt_sem_wait(&sem);
break;
case 2:
rt_return(rt_receive(NULL, &msg), 0);
break;
}
cpu_used[hard_cpu_id()]++;
}
}
int rt_Reply(pid_t pid, void *msg, size_t size)
{
RT_TASK *task, *retask;
MSGCB *cb;
unsigned int retlen;
if ((task = pid2rttask(pid))) {
if ((cb = (MSGCB *)task->msg)->cmd == SYNCMSG) {
if ((retlen = size <= cb->rbytes ? size : cb->rbytes)) {
memcpy(cb->rbuf, msg, retlen);
}
if (!(retask = rt_return(task, retlen))) {
return -EINVAL;
} else if (retask != task) {
return -ESRCH;
}
return 0;
}
return -EPERM;
}
return -ESRCH;
}
static void *thread_fun(void *arg)
{
int mytask_indx;
unsigned long msg;
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
mytask_indx = ((int *)arg)[0];
if (!(mytask[mytask_indx] = rt_thread_init(taskname(mytask_indx), 0, 0,
SCHED_FIFO, 0x1))) {
printf("CANNOT INIT TASK %u\n", taskname(mytask_indx));
exit(1);
}
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
hrt[mytask_indx] = 1;
while (!end) {
switch (change) {
case 0:
rt_task_suspend(mytask[mytask_indx]);
break;
case 1:
rt_sem_wait(sem);
break;
case 2:
rt_receive(NULL, &msg);
break;
case 3:
rt_return(rt_receive(NULL, &msg), 0);
break;
}
}
rt_make_soft_real_time();
rt_task_delete(mytask[mytask_indx]);
hrt[mytask_indx] = 0;
return (void*)0;
}
int main(int argc, char *argv[])
{
unsigned long sndnode;
long sndport, i, r;
RT_TASK *rcvtsk, *sndtsk;
struct sockaddr_in addr;
static MBX *sndmbx;
SERVER = atoi(argv[1]);
if (!(rcvtsk = rt_task_init_schmod(nam2num("RCVTSK"), 2, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT RECEIVER TASK\n");
exit(1);
}
mbx = rt_mbx_init(nam2num("MBX"), 100);
athread = rt_thread_create(async_fun, NULL, 10000);
sndnode = 0;
if (argc == 3 && strstr(argv[2], "SndNode=")) {
inet_aton(argv[2] + 8, &addr.sin_addr);
sndnode = addr.sin_addr.s_addr;
}
if (!sndnode) {
inet_aton("127.0.0.1", &addr.sin_addr);
sndnode = addr.sin_addr.s_addr;
}
rt_set_oneshot_mode();
start_rt_timer(0);
while ((sndport = rt_request_port_mbx(sndnode, mbx)) <= 0 && sndport != -EINVAL);
while (!(sndmbx = RT_get_adr(sndnode, sndport, "SNDMBX"))) {
rt_sleep(nano2count(100000000));
}
sndtsk = RT_get_adr(sndnode, sndport, "SNDTSK");
printf("\nRECEIVER TASK RUNNING\n");
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
while (!end) {
r = RT_mbx_receive(sndnode, -sndport, sndmbx, &i, sizeof(long));
rt_printk("RECEIVE %ld %ld\n", r, i);
if (SERVER) {
rt_sleep(nano2count(100000000));
} else {
while (!end && rt_waiting_return(sndnode, sndport)) {
rt_sleep(nano2count(1000000));
}
if (!end) {
unsigned long long retval;
long i1, i2;
int l1, l2;
if (rt_sync_net_rpc(sndnode, -sndport)) {
l1 = l2 = sizeof(long);
rt_get_net_rpc_ret(mbx, &retval, &i1, &l1, &i2, &l2, 0LL, MBX_RECEIVE);
rt_printk("RECEIVER ASYNC MSG: RETVAL = %d, MSG = %ld, LEN = %d.\n", (int)retval, i1, l1);
if (i1 < 0) {
end = 1;
break;
}
}
}
}
}
rt_mbx_delete(mbx);
rt_release_port(sndnode, sndport);
rt_make_soft_real_time();
rt_thread_join(athread);
rt_return(rt_receive(0, &i), i);
rt_task_delete(rcvtsk);
stop_rt_timer();
printf("\nRECEIVER TASK STOPS\n");
return 0;
}
int main(int argc, char *argv[])
{
int diff;
int sample;
long average;
int min_diff;
int max_diff;
int period;
int i;
RTIME t, svt;
RTIME expected, exectime[3];
MBX *mbx;
RT_TASK *task, *latchk;
struct sample { long long min; long long max; int index, ovrn; } samp;
double s = 0.0, sref;
long long max = -1000000000, min = 1000000000;
signal(SIGINT, endme);
signal(SIGKILL, endme);
signal(SIGTERM, endme);
if (!(mbx = rt_mbx_init(nam2num("LATMBX"), 20*sizeof(samp)))) {
printf("CANNOT CREATE MAILBOX\n");
exit(1);
}
if (!(task = rt_task_init_schmod(nam2num("LATCAL"), 0, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT MASTER LATENCY TASK\n");
exit(1);
}
printf("\n## RTAI latency calibration tool ##\n");
printf("# period = %i (ns) \n", PERIOD);
printf("# average time = %i (s)\n", (int)AVRGTIME);
printf("# use the FPU\n");
printf("#%sstart the timer\n", argc == 1 ? " " : " do not ");
printf("# timer_mode is %s\n", TIMER_MODE ? "periodic" : "oneshot");
printf("\n");
if (!(hard_timer_running = rt_is_hard_timer_running())) {
if (TIMER_MODE) {
rt_set_periodic_mode();
} else {
rt_set_oneshot_mode();
}
period = start_rt_timer(nano2count(PERIOD));
} else {
period = nano2count(PERIOD);
}
for(i = 0; i < MAXDIM; i++) {
a[i] = b[i] = 3.141592;
}
sref = dot(a, b, MAXDIM);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
expected = rt_get_time() + 200*period;
rt_task_make_periodic(task, expected, period);
svt = rt_get_cpu_time_ns();
i = 0;
samp.ovrn = 0;
while (!end) {
min_diff = 1000000000;
max_diff = -1000000000;
average = 0;
for (sample = 0; sample < SMPLSXAVRG && !end; sample++) {
expected += period;
if (!rt_task_wait_period()) {
if (TIMER_MODE) {
diff = (int) ((t = rt_get_cpu_time_ns()) - svt - PERIOD);
svt = t;
} else {
diff = (int) count2nano(rt_get_time() - expected);
}
} else {
samp.ovrn++;
diff = 0;
if (TIMER_MODE) {
svt = rt_get_cpu_time_ns();
}
}
outb(i = 1 - i, 0x378);
if (diff < min_diff) {
min_diff = diff;
}
if (diff > max_diff) {
max_diff = diff;
}
average += diff;
s = dot(a, b, MAXDIM);
if (fabs((s - sref)/sref) > 1.0e-15) {
printf("\nDOT PRODUCT RESULT = %20.16e %20.16e %20.16e\n", s, sref, fabs((s - sref)/sref));
return 0;
}
}
samp.min = min_diff;
samp.max = max_diff;
samp.index = average/SMPLSXAVRG;
#if SOLO
if (max < samp.max) max = samp.max;
if (min > samp.min) min = samp.min;
rt_printk("SOLO * min: %lld/%lld, max: %lld/%lld average: %d (%d) <Hit [RETURN] to stop> *\n", samp.min, min, samp.max, max, samp.index, samp.ovrn);
#else
rt_mbx_send_if(mbx, &samp, sizeof(samp));
if ((latchk = rt_get_adr(nam2num("LATCHK"))) && (rt_receive_if(latchk, &average) || end)) {
rt_return(latchk, average);
break;
}
#endif
}
while (rt_get_adr(nam2num("LATCHK"))) {
rt_sleep(nano2count(1000000));
}
rt_make_soft_real_time();
if (!hard_timer_running) {
stop_rt_timer();
}
rt_get_exectime(task, exectime);
if (exectime[1] && exectime[2]) {
printf("\n>>> S = %g, EXECTIME = %G\n", s, (double)exectime[0]/(double)(exectime[2] - exectime[1]));
}
rt_task_delete(task);
rt_mbx_delete(mbx);
return 0;
}
int main(int argc, char *argv[])
{
int diff;
int skip;
long average;
int min_diff;
int max_diff;
int period;
int i;
RTIME t, svt;
RTIME expected, exectime[3];
MBX *mbx;
RT_TASK *task, *latchk;
struct sample { long long min; long long max; int index, ovrn; } samp;
double s;
signal(SIGHUP, endme);
signal(SIGKILL, endme);
signal(SIGTERM, endme);
signal(SIGALRM, endme);
if (!(mbx = rt_mbx_init(nam2num("LATMBX"), 20*sizeof(samp)))) {
printf("CANNOT CREATE MAILBOX\n");
exit(1);
}
if (!(task = rt_task_init_schmod(nam2num("LATCAL"), 0, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT MASTER TASK\n");
exit(1);
}
printf("\n## RTAI latency calibration tool ##\n");
printf("# period = %i (ns) \n", PERIOD);
printf("# average time = %i (s)\n", (int)AVRGTIME);
printf("# use the FPU\n");
printf("#%sstart the timer\n", argc == 1 ? " " : " do not ");
printf("# timer_mode is %s\n", TIMER_MODE ? "periodic" : "oneshot");
printf("\n");
if (argc == 1) {
if (TIMER_MODE) {
rt_set_periodic_mode();
} else {
rt_set_oneshot_mode();
}
period = start_rt_timer(nano2count(PERIOD));
} else {
period = nano2count(PERIOD);
}
for(i = 0; i < MAXDIM; i++) {
a[i] = b[i] = 3.141592;
}
s = dot(a, b, MAXDIM);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
rt_task_make_periodic(task, expected = rt_get_tscnt() + 10*period, period);
svt = rt_get_cpu_time_ns();
samp.ovrn = i = 0;
while (!end) {
min_diff = 1000000000;
max_diff = -1000000000;
average = 0;
for (skip = 0; skip < SKIP && !end; skip++) {
expected += period;
if (!rt_task_wait_period()) {
if (TIMER_MODE) {
diff = (int) ((t = rt_get_cpu_time_ns()) - svt - PERIOD);
svt = t;
} else {
diff = (int) count2nano(rt_get_tscnt() - expected);
}
} else {
samp.ovrn++;
diff = 0;
if (TIMER_MODE) {
svt = rt_get_cpu_time_ns();
}
}
if (diff < min_diff) {
min_diff = diff;
}
if (diff > max_diff) {
max_diff = diff;
}
average += diff;
s = dot(a, b, MAXDIM);
}
samp.min = min_diff;
samp.max = max_diff;
samp.index = average/SKIP;
rt_mbx_send_if(mbx, &samp, sizeof(samp));
if ((latchk = rt_get_adr(nam2num("LATCHK"))) && (rt_receive_if(latchk, (unsigned long *)&average) || end)) {
rt_return(latchk, (unsigned long)average);
break;
}
}
while (rt_get_adr(nam2num("LATCHK"))) {
rt_sleep(nano2count(1000000));
}
rt_make_soft_real_time();
if (argc == 1) {
stop_rt_timer();
}
rt_get_exectime(task, exectime);
if (exectime[1] && exectime[2]) {
printf("\n>>> S = %g, EXECTIME = %G\n", s, (double)exectime[0]/(double)(exectime[2] - exectime[1]));
}
rt_task_delete(task);
rt_mbx_delete(mbx);
return 0;
}
int main(int argc, char *argv[])
{
int diff;
int skip;
int average;
int min_diff;
int max_diff;
int period;
int i;
RTIME expected, ts;
MBX *mbx;
RT_TASK *task;
struct sample { long long min; long long max; int index; double s; int ts; } samp;
double s;
if (!(mbx = rt_mbx_init(nam2num("LATMBX"), 20*sizeof(samp)))) {
printf("CANNOT CREATE MAILBOX\n");
exit(1);
}
if (!(task = rt_task_init_schmod(nam2num("LATCAL"), 0, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT MASTER TASK\n");
exit(1);
}
if (argc == 1) {
rt_set_oneshot_mode();
start_rt_timer(0);
}
for(i = 0; i < MAXDIM; i++) {
a[i] = b[i] = 3.141592;
}
vdot(a, b, MAXDIM);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
period = nano2count(PERIOD);
rt_task_make_periodic(task, expected = rt_get_time() + 5*period, period);
#ifdef OVERALL
min_diff = 1000000000;
max_diff = -1000000000;
#endif
while (1) {
#ifndef OVERALL
min_diff = 1000000000;
max_diff = -1000000000;
#endif
average = 0;
for (skip = 0; skip < SKIP; skip++) {
expected += period;
rt_task_wait_period();
diff = (int)count2nano(rt_get_time() - expected);
if (diff < min_diff) {
min_diff = diff;
}
if (diff > max_diff) {
max_diff = diff;
}
average += diff;
ts = rt_get_time();
s = vdot(a, b, MAXDIM);
ts = rt_get_time() - expected;
}
samp.min = min_diff;
samp.max = max_diff;
samp.index = average/SKIP;
samp.s = (double)s;
samp.ts = (int)count2nano(ts);
rt_mbx_send_if(mbx, &samp, sizeof(samp));
if (rt_receive_if(rt_get_adr(nam2num("LATCHK")), (unsigned int *)&average)) {
rt_return(rt_get_adr(nam2num("LATCHK")), (unsigned int)average);
break;
}
}
while (rt_get_adr(nam2num("LATCHK"))) {
rt_sleep(nano2count(1000000));
}
rt_make_soft_real_time();
if (argc == 1) {
stop_rt_timer();
}
rt_task_delete(task);
rt_mbx_delete(mbx);
return 0;
}
static void *rt_HostInterface(void *args)
{
RT_TASK *task;
unsigned int Request;
int Reply, len;
char nome[8];
if (!(rt_HostInterfaceTask = rt_task_init_schmod(nam2num(HostInterfaceTaskName), rt_HostInterfaceTaskPriority, 0, 0, SCHED_RR, 0xFF))) {
fprintf(stderr,"Cannot init rt_HostInterfaceTask.\n");
return (void *)1;
}
sem_post(&err_sem);
while (!endInterface) {
task = rt_receive(0, &Request);
num2nam(rt_get_name(task),nome);
if (endInterface) break;
switch (Request & 0xFF) {
case 'c': {
int i ,Idx, idx[2];
rtTargetParamInfo rtParam;
float samplingTime;
int NPAR,i1,i2;
mxp_t pardata;
double value;
strncpyz(rtParam.modelName, modelname, MAX_NAME_SIZE);
rtParam.dataType = 0;
NPAR = mxp_getnvars();
rt_return(task, (isRunning << 16) | ( NPAR & 0xFFFF ));
rt_receivex(task, &Request, 1, &len);
rt_returnx(task, &rtParam, sizeof(rtParam));
for (i = 0; i < NPAR; i++) {
sprintf(rtParam.blockName,"%s/%s",rtParam.modelName,"Tunable Parameters");
mxp_getvarbyidx(i, &pardata);
if ( pardata.ndim == 0 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
mxp_getparam(i,idx, &value);
rtParam.dataValue[0] = value;
rtParam.dataClass = rt_SCALAR;
rtParam.nRows = 1;
rtParam.nCols = 1;
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if ( pardata.ndim == 1 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
rtParam.dataClass = rt_VECTOR;
rtParam.nRows = 1;
rtParam.nCols = pardata.dim[0];
for (i1 = 0; i1 < pardata.dim[0] ; i1++){
idx[0] = i1;
mxp_getparam(i,idx, &value);
rtParam.dataValue[i1] = value;
}
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if ( pardata.ndim == 2 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
rtParam.dataClass = rt_MATRIX_ROW_MAJOR;
rtParam.nRows = pardata.dim[0];
rtParam.nCols = pardata.dim[1];
for (i1 = 0; i1 < pardata.dim[0] ; i1++){
for (i2 = 0; i2 < pardata.dim[1] ; i2++){
idx[0] = i1;
idx[1] = i2;
mxp_getparam(i,idx, &value);
rtParam.dataValue[i1*rtParam.nCols+i2] = value;
}
}
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if (pardata.ndim > 2){
fprintf(stderr,"MAX PARAMETER DIMESION = 2........\n");
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, &rtaiScope[Idx].ntraces, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, rtaiScope[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, &rtaiLogData[Idx].nrow, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, &rtaiLogData[Idx].ncol, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, rtaiLogData[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, &rtaiALogData[Idx].nrow, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, &rtaiALogData[Idx].ncol, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, rtaiALogData[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, &rtaiLed[Idx].nleds, sizeof(int));
rt_receivex(task, &Idx, sizeof(int), &len);
rt_returnx(task, rtaiLed[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, rtaiMeter[Idx].name, MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
while (1) {
rt_receivex(task, &Idx, sizeof(int), &len);
if (Idx < 0) {
rt_returnx(task, &Idx, sizeof(int));
break;
} else {
rt_returnx(task, "", MAX_NAME_SIZE);
rt_receivex(task, &Idx, sizeof(int), &len);
samplingTime = get_tsamp();
rt_returnx(task, &samplingTime, sizeof(float));
}
}
break;
}
case 's': {
rt_task_resume(rt_MainTask);
rt_return(task, 1);
break;
}
case 't': {
endex = 1;
rt_return(task, 0);
break;
}
case 'p': {
int index;
double param;
int mat_ind,Idx[2];
mxp_t pardata;
rt_return(task, isRunning);
rt_receivex(task, &index, sizeof(int), &len);
Reply = 0;
rt_returnx(task, &Reply, sizeof(int));
rt_receivex(task, ¶m, sizeof(double), &len);
Reply = 1;
rt_returnx(task, &Reply, sizeof(int));
rt_receivex(task, &mat_ind, sizeof(int), &len);
mxp_getvarbyidx(index, &pardata);
if ( pardata.ndim == 1 ) Idx[0] = mat_ind;
if ( pardata.ndim == 2 ){
Idx[0] = mat_ind/pardata.dim[1];
Idx[1] = mat_ind - Idx[0]*pardata.dim[1];
}
mxp_setparam(index, Idx, param);
rt_returnx(task, &Reply, sizeof(int));
break;
}
case 'g': {
int i, idx[2], i1,i2;
rtTargetParamInfo rtParam;
int NPAR;
mxp_t pardata;
double value;
strncpyz(rtParam.modelName, modelname, MAX_NAME_SIZE);
rtParam.dataType = 0;
NPAR = mxp_getnvars();
rt_return(task, isRunning);
for (i = 0; i < NPAR; i++) {
sprintf(rtParam.blockName,"%s/%s",rtParam.modelName,"Tunable Parameters");
mxp_getvarbyidx(i, &pardata);
if ( pardata.ndim == 0 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
mxp_getparam(i,idx, &value);
rtParam.dataValue[0] = value;
rtParam.dataClass = rt_SCALAR;
rtParam.nRows = 1;
rtParam.nCols = 1;
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if ( pardata.ndim == 1 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
rtParam.dataClass = rt_VECTOR;
rtParam.nRows = 1;
rtParam.nCols = pardata.dim[0];
for (i1 = 0; i1 < pardata.dim[0] ; i1++){
idx[0] = i1;
mxp_getparam(i,idx, &value);
rtParam.dataValue[i1] = value;
}
rt_returnx(task, &rtParam, sizeof(rtParam));
}
if ( pardata.ndim == 2 ){
rt_receivex(task,&Request,1,&len);
sprintf(rtParam.paramName, pardata.name);
rtParam.dataClass = rt_MATRIX_ROW_MAJOR;
rtParam.nRows = pardata.dim[0];
rtParam.nCols = pardata.dim[1];
for (i1 = 0; i1 < pardata.dim[0] ; i1++){
for (i2 = 0; i2 < pardata.dim[1] ; i2++){
idx[0] = i1;
idx[1] = i2;
mxp_getparam(i,idx, &value);
rtParam.dataValue[i1*rtParam.nCols+i2] = value;
}
}
rt_returnx(task, &rtParam, sizeof(rtParam));
}
}
break;
}
case 'd': {
int ParamCnt;
int Idx[2];
mxp_t pardata;
rt_return(task, isRunning);
rt_receivex(task, &ParamCnt, sizeof(int), &len);
Reply = 0;
rt_returnx(task, &Reply, sizeof(int));
{
struct {
int index;
int mat_ind;
double value;
} Params[ParamCnt];
int i;
rt_receivex(task, &Params, sizeof(Params), &len);
for (i = 0; i < ParamCnt; i++) {
mxp_getvarbyidx(Params[i].index, &pardata);
if ( pardata.ndim == 1 ) Idx[0] = Params[i].mat_ind;
if ( pardata.ndim == 2 ){
Idx[0] = Params[i].mat_ind/pardata.dim[1];
Idx[1] = Params[i].mat_ind - Idx[0]*pardata.dim[1];
}
mxp_setparam(Params[i].index, Idx, Params[i].value);
}
}
Reply = 1;
rt_returnx(task, &Reply, sizeof(int));
break;
}
case 'm': {
float time = SIM_TIME;
rt_return(task, isRunning);
rt_receivex(task, &Reply, sizeof(int), &len);
rt_returnx(task, &time, sizeof(float));
break;
}
case 'b': {
rt_return(task, (unsigned int)rt_BaseRateTask);
break;
}
default : {
break;
}
}
}
rt_task_delete(rt_HostInterfaceTask);
return 0;
}
static int rt_Main(int priority)
{
SEM *hard_timers_cnt;
char name[7];
RTIME rt_BaseTaskPeriod;
struct timespec err_timeout;
int i;
rt_allow_nonroot_hrt();
for (i = 0; i < MAX_NTARGETS; i++) {
sprintf(name,"MNT%d",i);
if (!rt_get_adr(nam2num(name))) break;
}
if (!(rt_MainTask = rt_task_init_schmod(nam2num(name), rt_MainTaskPriority, 0, 0, SCHED_RR, 0xFF))) {
fprintf(stderr,"Cannot init rt_MainTask.\n");
return 1;
}
sem_init(&err_sem, 0, 0);
printf("TARGET STARTS.\n");
pthread_create(&rt_HostInterfaceThread, NULL, rt_HostInterface, NULL);
err_timeout.tv_sec = (long int)(time(NULL)) + 1;
err_timeout.tv_nsec = 0;
if ((sem_timedwait(&err_sem, &err_timeout)) != 0) {
fprintf(stderr, "Target is terminated.\n");
goto finish;
}
pthread_create(&rt_BaseRateThread, NULL, rt_BaseRate, &priority);
err_timeout.tv_sec = (long int)(time(NULL)) + 1;
err_timeout.tv_nsec = 0;
if ((sem_timedwait(&err_sem, &err_timeout)) != 0) {
endInterface = 1;
rt_send(rt_HostInterfaceTask, 0);
pthread_join(rt_HostInterfaceThread, NULL);
fprintf(stderr, "Target is terminated.\n");
goto finish;
}
rt_BaseTaskPeriod = (RTIME) (1e9*get_tsamp());
if (InternTimer) {
WaitTimingEvent = (void *)rt_task_wait_period;
if (!(hard_timers_cnt = rt_get_adr(nam2num("HTMRCN")))) {
if (!ClockTick) {
rt_set_oneshot_mode();
start_rt_timer(0);
rt_BaseRateTick = nano2count(rt_BaseTaskPeriod);
}
else {
rt_set_periodic_mode();
rt_BaseRateTick = start_rt_timer(nano2count(rt_BaseTaskPeriod));
}
hard_timers_cnt = rt_sem_init(nam2num("HTMRCN"), 0);
}
else {
rt_BaseRateTick = nano2count(rt_BaseTaskPeriod);
rt_sem_signal(hard_timers_cnt);
}
}
else {
WaitTimingEvent = (void *)DummyWait;
SendTimingEvent = (void *)DummySend;
}
if (verbose) {
printf("Model : %s .\n", modelname);
printf("Executes on CPU map : %x.\n", CpuMap);
printf("Sampling time : %e (s).\n", get_tsamp());
}
{
int msg;
rt_receive(0, &msg);
}
if (WaitToStart) {
if (verbose) {
printf("Target is waiting to start ... ");
fflush(stdout);
}
rt_task_suspend(rt_MainTask);
}
if (verbose) {
printf("Target is running.\n");
}
rt_return(rt_BaseRateTask, 0);
isRunning = 1;
while (!endex && (!FinalTime || SIM_TIME < FinalTime)) {
msleep(POLL_PERIOD);
}
endBaseRate = 1;
if (!InternTimer) {
SendTimingEvent(TimingEventArg);
}
pthread_join(rt_BaseRateThread, NULL);
isRunning = 0;
endInterface = 1;
rt_send(rt_HostInterfaceTask, 0);
if (verbose) {
printf("Target has been stopped.\n");
}
pthread_join(rt_HostInterfaceThread, NULL);
if (InternTimer) {
if (!rt_sem_wait_if(hard_timers_cnt)) {
rt_sem_delete(hard_timers_cnt);
}
}
finish:
for (i=0 ; i<NSCOPE ; i++)
RT_named_mbx_delete(0, 0, rtaiScope[i].mbx);
for (i=0 ; i<NLOGS ; i++)
RT_named_mbx_delete(0, 0, rtaiLogData[i].mbx);
for (i=0 ; i<NLEDS ; i++)
RT_named_mbx_delete(0, 0, rtaiLed[i].mbx);
for (i=0 ; i<NMETERS ; i++)
RT_named_mbx_delete(0, 0, rtaiMeter[i].mbx);
for ( i=0 ; i<MAX_COMEDI_DEVICES ; i++ ){
if ( ComediDev[i] != NULL ){
comedi_close(ComediDev[i]);
}
}
for ( i=0 ; i<N_DATAIN ; i++){
free( ComediDataIn[i].comdev );
}
for ( i=0 ; i<N_DATAOUT ; i++){
free( ComediDataOut[i].comdev );
}
for ( i=0 ; i<N_DIOIN ; i++){
free( ComediDioIn[i].comdev );
}
for ( i=0 ; i<N_DIOOUT ; i++){
free( ComediDioOut[i].comdev );
}
SA_Output_To_File();
rt_task_delete(rt_MainTask);
printf("TARGET ENDS.\n");
return 0;
}
