int main(void)
{
RT_TASK *mytask;
int smbx, rmbx[NTASKS];
int i, bthread, mthread[NTASKS];
char msg[] = "let's end the game";
mytask = rt_thread_init(nam2num("MAIN"), 2, 0, SCHED_FIFO, 0xF);
barrier = rt_sem_init(rt_get_name(0), NTASKS + 1);
smbx = rt_msgget(nam2num("SMSG"), 0x666 | IPC_CREAT);
for (i = 0; i < NTASKS; i++) {
char mname[6] = "RMBX";
mname[4] = i + '0';
mname[5] = 0;
rmbx[i] = rt_msgget(nam2num(mname), 0x666 | IPC_CREAT);
}
rt_set_oneshot_mode();
start_rt_timer(0);
bthread = rt_thread_create(bfun, 0, 0x8000);
for (i = 0; i < NTASKS; i++) {
mthread[i] = rt_thread_create(mfun, (void *)i, 0x8000);
}
printf("IF NOTHING HAPPENS IS OK, TYPE ENTER TO FINISH.\n");
getchar();
for (i = 0; i < NTASKS; i++) {
end = i;
rt_msgsnd_nu(rmbx[i], 1, msg, sizeof(msg), 0);
rt_thread_join(mthread[i]);
}
end = NTASKS;
rt_msgsnd_nu(smbx, 1, msg, sizeof(msg), 0);
rt_thread_join(bthread);
for (i = 0; i < NTASKS; i++) {
rt_msgctl(rmbx[i], IPC_RMID, NULL);
printf("TASK %d, LOOPS: %d.\n", i, cnt[i]);
}
rt_msgctl(smbx, IPC_RMID, NULL);
stop_rt_timer();
rt_sem_delete(barrier);
rt_task_delete(mytask);
printf("MAIN TASK ENDS.\n");
return 0;
}
void Display_PutTimes(MenageHmsh_tChain11 chain)
{
static MenageHmsh_tChain11 times;
static RT_TASK *ackn = 0;
unsigned int put = 'P';
unsigned long msg;
if (ackn != rt_get_adr(nam2num("DSPTSK"))) {
ackn = rt_rpc(rt_get_adr(nam2num("DSPTSK")), put, &msg);
}
times = chain;
times.chain[0] = 't';
rt_send_if(rt_get_adr(nam2num("DSPTSK")), (unsigned long)times.chain);
}
void Display_PutHour(MenageHmsh_tChain11 chain)
{
static MenageHmsh_tChain11 hours;
static RT_TASK *ackn = 0;
unsigned int put = 'p';
unsigned long msg;
if (ackn != rt_get_adr(nam2num("DSPTSK"))) {
ackn = rt_rpc(rt_get_adr(nam2num("DSPTSK")), put, &msg);
}
hours = chain;
hours.chain[0] = 'h';
rt_send_if(rt_get_adr(nam2num("DSPTSK")), (unsigned long)hours.chain);
}
int main(void)
{
pthread_t thread;
unsigned int player, cnt;
unsigned long msg;
RT_TASK *mytask;
MBX *mbx;
char data[BUFSIZE];
signal(SIGINT, endme);
rt_allow_nonroot_hrt();
if ((player = open("../../../share/linux.au", O_RDONLY)) < 0) {
printf("ERROR OPENING SOUND FILE (linux.au)\n");
exit(1);
}
if (!(mytask = rt_task_init(nam2num("SNDMAS"), 2, 0, 0))) {
printf("CANNOT INIT MASTER TASK\n");
exit(1);
}
mlockall(MCL_CURRENT | MCL_FUTURE);
printf("\nINIT MASTER TASK %p\n\n(CTRL-C TO END EVERYTHING)\n", mytask);
mbx = rt_typed_named_mbx_init("SNDMBX", 2000, FIFO_Q);
rt_set_oneshot_mode();
start_rt_timer(0);
thread = rt_thread_create(intr_handler, NULL, 10000);
rt_mbx_receive(mbx, &data, 1);
while (!end) {
lseek(player, 0, SEEK_SET);
while(!end && (cnt = read(player, &data, BUFSIZE)) > 0) {
rt_mbx_send(mbx, data, cnt);
}
}
rt_rpc(rt_get_adr(nam2num("SOUND")), msg, &msg);
while (rt_get_adr(nam2num("SOUND"))) {
rt_sleep(nano2count(1000000));
}
rt_task_delete(mytask);
rt_mbx_delete(mbx);
stop_rt_timer();
close(player);
printf("\nEND MASTER TASK %p\n", mytask);
rt_thread_join(thread);
return 0;
}
int main(void)
{
RT_TASK *mytask;
MBX *Keyboard, *Screen;
unsigned int i;
char d = 'd';
char chain[12];
char displine[40] = "CLOCK-> 00:00:00 CHRONO-> 00:00:00";
signal(SIGINT, endme);
if (!(mytask = rt_task_init_schmod(nam2num("SCRTSK"), 20, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT SCREEN TASK\n");
exit(1);
}
if (!(Keyboard = rt_get_adr(nam2num("KEYBRD")))) {
printf("CANNOT FIND KEYBOARD MAILBOX\n");
exit(1);
}
if (rt_mbx_send(Keyboard, &d, 1) > 0 ) {
fprintf(stderr, "Can't send initial command to RT-task\n");
exit(1);
}
if (!(Screen = rt_get_adr(nam2num("SCREEN")))) {
printf("CANNOT FIND SCREEN MAILBOX\n");
exit(1);
}
sleep(1);
printf("%s\n", displine);
while(1) {
if (rt_mbx_receive(Screen, chain, 12)) break;
if (chain[0] == 't') {
for (i = 0; i < 11; i++) {
displine[i+27] = chain[i+1];
}
} else if (chain[0] == 'h') {
for (i = 0; i < 8; i++) {
displine[i+8] = chain[i+1];
}
}
printf("%s\n", displine);
}
rt_task_delete(mytask);
return 0;
}
void struct_Odometry::iniSHM(int shm_in, int shm_out, char* SHM_name)
{
if (shm_in == 1)
{
dataIN = (odometry_t*)rtai_malloc (nam2num(SHM_name), sizeof(struct odometry_t)) ;
dataIN->pose.pose.position.x = 0.0;
dataIN->pose.pose.position.y = 0.0;
dataIN->pose.pose.position.z = 0.0;
dataIN->pose.pose.orientation.x = 0.0;
dataIN->pose.pose.orientation.y = 0.0;
dataIN->pose.pose.orientation.z = 0.0;
dataIN->pose.pose.orientation.w = 0.0;
dataIN->twist.twist.angular.x = 0.0;
dataIN->twist.twist.angular.y = 0.0;
dataIN->twist.twist.angular.z = 0.0;
dataIN->twist.twist.linear.x = 0.0;
dataIN->twist.twist.linear.y = 0.0;
dataIN->twist.twist.linear.z = 0.0;
}
if (shm_out == 1)
{
dataOUT = (odometry_t*)rtai_malloc (nam2num(SHM_name), sizeof(struct odometry_t)) ;
dataOUT->pose.pose.position.x = 0.0;
dataOUT->pose.pose.position.y = 0.0;
dataOUT->pose.pose.position.z = 0.0;
dataOUT->pose.pose.orientation.x = 0.0;
dataOUT->pose.pose.orientation.y = 0.0;
dataOUT->pose.pose.orientation.z = 0.0;
dataOUT->pose.pose.orientation.w = 0.0;
dataOUT->twist.twist.angular.x = 0.0;
dataOUT->twist.twist.angular.y = 0.0;
dataOUT->twist.twist.angular.z = 0.0;
dataOUT->twist.twist.linear.x = 0.0;
dataOUT->twist.twist.linear.y = 0.0;
dataOUT->twist.twist.linear.z = 0.0;
}
}
int main(int argc, char *argv[])
{
unsigned long rcvnode;
RT_TASK *sndtsk, *rcvtsk;
MBX *mbx;
long rcvport, i;
struct sockaddr_in addr;
thread = rt_thread_create(endme, NULL, 2000);
if (!(sndtsk = rt_task_init_schmod(nam2num("SNDTSK"), 1, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT SENDER TASK\n");
exit(1);
}
rcvnode = 0;
if (argc == 2 && strstr(argv[1], "RcvNode=")) {
inet_aton(argv[1] + 8, &addr.sin_addr);
rcvnode = addr.sin_addr.s_addr;
}
if (!rcvnode) {
inet_aton("127.0.0.1", &addr.sin_addr);
rcvnode = addr.sin_addr.s_addr;
}
mbx = rt_mbx_init(nam2num("SNDMBX"), 500);
while ((rcvport = rt_request_port(rcvnode)) <= 0 && rcvport != -EINVAL);
printf("\nSENDER TASK RUNNING\n");
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
for (i = 0; i < MAXLOOPS && !end; i++) {
rt_mbx_send(mbx, &i, sizeof(long));
rt_printk("SENT %ld\n", i);
RT_sleep(rcvnode, rcvport, 200000000);
}
i = -1;
rt_mbx_send(mbx, &i, sizeof(long));
rt_make_soft_real_time();
while (!(rcvtsk = RT_get_adr(rcvnode, rcvport, "RCVTSK"))) {
RT_sleep(rcvnode, rcvport, 100000000);
}
RT_rpc(rcvnode, rcvport, rcvtsk, i, &i);
rt_release_port(rcvnode, rcvport);
rt_mbx_delete(mbx);
rt_task_delete(sndtsk);
printf("\nSENDER TASK STOPS\n");
return 0;
}
int main(void)
{
int i, srq, count = 0, nextcount = 0, repeat;
rt_thread_init(nam2num("MNTSK"), 100, 0, SCHED_FIFO, 0x1);
rt_printk("\nTESTING THE SCHEDULER WITH SRQs [%d LOOPs].\n", LOOPS);
repeat = 1000000000LL/((long long)DELAY*(long long)PRINT_FREQ);
srq = rtai_open_srq(0xcacca);
start_rt_timer(0);
rt_grow_and_lock_stack(100000);
#ifdef MAKE_HARD
MAKE_HARD();
#endif
for (i = 0; i < LOOPS; i++) {
rtai_srq(srq, (unsigned long)nano2count(DELAY));
if (++count > nextcount) {
nextcount += repeat;
rt_printk(">>> %d.\n", nextcount);
}
}
rt_make_soft_real_time();
stop_rt_timer();
rt_task_delete(NULL);
rt_printk("END SCHEDULER TEST WITH SRQs.\n\n");
return 0;
}
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;
}
int main(void)
{
RT_TASK *spktsk, *plrtsk;
RTIME period;
char buf[BUFSIZE], data, temp;
unsigned int msg, i, len;
printf("\n\nGENERIC RECEIVE\n");
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);
}
rt_set_oneshot_mode();
start_rt_timer(0);
mlockall(MCL_CURRENT | MCL_FUTURE);
printf("\nSPEAKER TASK RUNNING\n");
rt_make_hard_real_time();
period = nano2count(PERIOD);
rt_task_make_periodic(spktsk, rt_get_time() + 5*period, period);
rt_returnx(rt_receivex(0, &msg, sizeof(int), &len), &msg, 1);
for (i = 0; i < 100; i++) {
rt_returnx(rt_receivex(0, &msg, sizeof(int), &len), &msg, 1);
}
len = 0;
while(1) {
if (len) {
data = filter(buf[i++]);
temp = inb(PORT_ADR);
temp &= 0xfd;
temp |= (data & 1) << 1;
outb(temp, PORT_ADR);
len--;
} else {
if (rt_evdrpx(0, buf, BUFSIZE, &i)) {
// rt_printk("EVDRP %d\n", i);
}
if ((plrtsk = rt_receivex_if(0, buf, BUFSIZE, &len))) {
rt_returnx(plrtsk, &len, sizeof(int));
if (len == sizeof(int) && ((int *)buf)[0] == 0xFFFFFFFF) {
break;
}
i = 0;
}
}
rt_task_wait_period();
}
rt_sleep(nano2count(100000000));
rt_make_soft_real_time();
stop_rt_timer();
rt_task_delete(spktsk);
printf("\nSPEAKER TASK STOPS\n");
return 0;
}
void *urt_sys_shmem_attach(const char *name, int *error)
{
void *mem = rt_shm_alloc(nam2num(name), 0, USE_VMALLOC);
if (mem == NULL && error)
*error = ENOENT;
return mem;
}
int main(void)
{
unsigned long hrttsk_name = nam2num("HRTTSK");
struct sched_param mysched;
mysched.sched_priority = sched_get_priority_max(SCHED_FIFO);
if( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 ) {
puts("ERROR IN SETTING THE SCHEDULER");
perror("errno");
exit(0);
}
if (!(hrttsk = rt_task_init(hrttsk_name, 1, 0, 0))) {
printf("CANNOT INIT TESTB MASTER TASK\n");
exit(1);
}
rt_set_usp_flags_mask(FORCE_SOFT);
rt_task_suspend(hrttsk);
printf("BACKGROUND REAL TIME TASK IS HARD .....\n");
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
while(rt_is_hard_real_time(hrttsk)) {
rt_task_wait_period();
}
printf("..... BACKGROUND REAL TIME TASK IS SOFT NOW, YOU CAN KILL IT BY HAND\n");
rt_task_delete(hrttsk);
while(1) { sleep(3); printf("BACKGROUND PROCESS STILL RUNNING\n"); }
}
int signal_func(void *param)
{
RT_TASK *rt_signal;
//TODO: RTIME sampling;
int value = 0;
int rval = 0;
char name[8];
t_info *t = param;
snprintf(name, 8, "S_%c", t->id);
printf("%s\n", name);
rt_signal = rt_thread_init(nam2num(name), 0, 0, SCHED_FIFO, CPUMAP);
if (!rt_signal) {
printf("Could not init real time signal %c\n", t->id);
rval = -ENODEV;
goto exit;
}
rt_task_make_periodic(rt_signal, rt_get_time() + t->period +
t->delay, t->period);
while (!finish) {
value = !value;
printf("[%Ld] signal %c now in %s.\n",
rt_get_time_ns(),
t->id,
value ? "up" : "down");
rt_task_wait_period();
}
rt_task_delete(rt_signal);
exit:
pthread_exit(NULL);
return rval;
}
int main(void)
{
RT_TASK *task;
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
if (!(task = rt_thread_init(nam2num("SWITCH"), 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT SWITCH TASK SIGNAL\n");
exit(1);
}
start_rt_timer(0);
rt_request_signal(SWITCH_SIGNAL, switch_handler);
rt_task_signal_handler(task, (void *)SWITCH_SIGNAL);
rt_make_hard_real_time();
while (!end) {
rt_sleep(nano2count(PERIOD));
}
rt_task_signal_handler(task, NULL);
rt_release_signal(SWITCH_SIGNAL, task);
rt_make_soft_real_time();
stop_rt_timer();
rt_task_delete(task);
return 0;
}
void *signalSan(void *arg)
{
RT_TASK *Task_3;
unsigned long Task_3_name = nam2num("TSK_3") + i++;
time_t aclock;
time_t clockNow;
int timeEx, begin_3;
int count = 0;
struct tm *newtime;
Task_3 = rt_task_init(Task_3_name, 0, 0, 0);
// if(!(Task_3 = rt_task_init_schmod(Task_3_name,3,0,0,SCHED_FIFO,1))) {
// printf("CANNOT INIT HANDLER TASK > Task 3 <\n");
// exit(1);
// }
rt_allow_nonroot_hrt();
rt_make_hard_real_time();
rt_task_make_periodic(Task_3, rt_get_time(), sampling * 30);
rt_change_prio(Task_3, 3);
begin_3 = begin;
while (count < 20) {
rt_sem_wait(rMutex);
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf(" Signal 3 =======> %s", asctime(newtime));
sleep(1);
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf(" Signal 3 after Sleep =======> %s", asctime(newtime));
timeEx = 3600 * newtime->tm_hour + 60 * newtime->tm_min + newtime->tm_sec;
if( (timeEx - begin_3) > 15 )
printf(" Time Failure of the Signal 3\n");
else printf(" Time Correct of the Signal 3\n");
begin_3 = timeEx + (15 - (timeEx-begin)%15);
rt_sem_signal(rMutex);
rt_task_wait_period();
count++;
}
rt_make_soft_real_time();
rt_task_delete(Task_3);
return 0;
}
void *init_task_1(void *arg)
{
unsigned long Task_1_name = nam2num("TASK_1");
//TODO:RAWLINSON
if(!(Task_1 = rt_task_init_schmod(Task_1_name,1,0,0,SCHED_FIFO, CPU_ALLOWED))) {
printf("[ERRO] Não foi possível criar a tarefa 1.\n");
exit(1);
}
rt_allow_nonroot_hrt();
rt_make_hard_real_time();
rt_task_make_periodic(Task_1, rt_get_time() + sampling * 5, sampling * 10);
while (1) {
time(&aclock); // Pega tempo em segundos.
newtime = localtime(&aclock);
printf("[TASK 1] =======> %s", asctime(newtime));
rt_task_wait_period();
}
}
void *ClockChrono_Write(void *args)
{
RT_TASK *mytask;
Display_tDest receiver;
MenageHmsh_tChain11 chain;
mqd_t Screen;
struct mq_attr sc_attrs = { MAX_MSGS, 12, 0, 0 };
if (!(mytask = rt_thread_init(nam2num("WRITE"), 1, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT TASK ClockChronoWrite\n");
exit(1);
}
Screen = mq_open("SCREEN", O_WRONLY | O_CREAT | O_NONBLOCK, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP, &sc_attrs);
printf("INIT TASK ClockChronoWrite %p.\n", mytask);
mlockall(MCL_CURRENT | MCL_FUTURE);
while(1) {
Display_Get(&chain, &receiver);
if (chain.chain[1] == 'e') {
goto end;
}
if (!hide && !Pause) {
mq_send(Screen, chain.chain, 12, 0);
}
}
end:
mq_close(Screen);
rt_task_delete(mytask);
printf("END TASK ClockChronoWrite %p.\n", mytask);
return 0;
}
int main(int argc, char *argv[])
{
int fifo, period, skip, average = 0;
RT_TASK *task;
RTIME expected;
if ((fifo = open("/dev/rtf0", O_WRONLY)) < 0) {
printf("Error opening FIFO0 in UCAL\n");
exit(1);
}
if (!(task = rt_task_init_schmod(nam2num("UCAL"), 0, 0, 0, SCHED_FIFO, 0xF))) {
printf("Cannot init UCAL\n");
exit(1);
}
rt_set_oneshot_mode();
period = start_rt_timer(nano2count(atoi(argv[1])));
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
expected = rt_get_time() + 100*period;
rt_task_make_periodic(task, expected, period);
for (skip = 0; skip < atoi(argv[2]); skip++) {
expected += period;
rt_task_wait_period();
average += (int)count2nano(rt_get_time() - expected);
}
rt_make_soft_real_time();
stop_rt_timer();
rt_task_delete(task);
write(fifo, &average, sizeof(average));
close(fifo);
exit(0);
}
static void *logMsg_fun(void *args)
{
RT_TASK *buddy;
struct { int nch; FILE *fs; } type;
char msg[MAX_MSG_SIZE];
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
if (!(buddy = rt_task_init_schmod(nam2num("LOGSRV"), 1, 0, 0, SCHED_FIFO, 0xFFFFFFFF))) {
printf("CANNOT INIT LOG MSG SERVER BUDDY TASK %p.\n", buddy);
return (void *)1;
}
rt_send(*(RT_TASK **)args, (unsigned int)buddy);
while (1) {
if (!rt_mbx_receive(logmbx, &type, sizeof(type)) && !rt_mbx_receive(logmbx, msg, type.nch)) {
if (type.fs) {
fprintf(type.fs, msg);
} else {
fprintf(stderr, msg);
fprintf(logfile, msg);
}
} else {
while(!rt_mbx_receive_wp(logmbx, msg, MAX_MSG_SIZE));
}
}
return (void *)0;
}
int main(int argc, char *argv[])
{
RT_TASK *task;
struct sockaddr_in addr;
int i, srvport;
if (!(task = rt_task_init(nam2num("TSKCOD"), 0, 0, 0))) {
printf("CANNOT INIT TASK CODE\n");
exit(1);
}
comnode = 0;
if (argc == 2 && strstr(argv[1], "ComNode=")) {
inet_aton(argv[1] + 8, &addr.sin_addr);
comnode = addr.sin_addr.s_addr;
}
if (!comnode) {
inet_aton("127.0.0.1", &addr.sin_addr);
comnode = addr.sin_addr.s_addr;
}
init_module();
for (i = 0; i < NUM_TASKS; i++) {
rt_thread_join(thread[i]);
}
while ((srvport = rt_request_port(comnode)) <= 0) {
msleep(100);
}
RT_sem_signal(comnode, srvport, end_sem);
rt_release_port(comnode, srvport);
rt_task_delete(task);
exit(0);
}
//----------------------------------------------------------------------------
// real time task
void writing_task_proc(void *arg)
{
#ifdef RTAI
writing_task = rt_task_init_schmod(nam2num("WRTSK"),2, 0, 0,
SCHED_FIFO, 0xF);
rt_make_hard_real_time();
#endif
while (1) {
std::list<TPCANMsg>::iterator iter;
for (iter = List->begin(); iter != List->end(); iter++) {
// test for standard frames only
if ((nExtended == CAN_INIT_TYPE_EX) ||
!(iter->MSGTYPE & MSGTYPE_EXTENDED)) {
// send the message
if ((errno = CAN_Write(h, &(*iter))))
shutdownnow = 1;
}
if (shutdownnow == 1)
break;
}
if (shutdownnow == 1)
break;
}
#ifdef RTAI
rt_make_soft_real_time();
rt_task_delete(writing_task);
#endif
}
void *urt_global_mem_get(const char *name, size_t size, int *error)
{
void *mem = rt_shm_alloc(nam2num(name), size, USE_VMALLOC);
if (mem == NULL && error)
*error = ENOMEM;
return mem;
}
static void *async_fun(void *args)
{
RT_TASK *asynctsk;
if (!(asynctsk = rt_task_init_schmod(nam2num("ASYTSK"), 2, 0, 0, SCHED_FIFO, 0xF))) {
printf("CANNOT INIT ASYNC TASK\n");
exit(1);
}
mlockall(MCL_CURRENT | MCL_FUTURE);
while (!end) {
if (SERVER) {
unsigned long long retval;
long i1, i2;
int l1, l2;
l1 = l2 = sizeof(long);
rt_get_net_rpc_ret(mbx, &retval, &i1, &l1, &i2, &l2, 0LL, MBX_RECEIVE);
printf("SERVER ASYNC MSG: RETVAL = %llu, MSG = %ld, LEN = %d.\n", retval, i1, l1);
if (i1 < 0) {
end = 1;
break;
}
} else {
rt_sleep(nano2count(100000000));
}
}
rt_task_delete(asynctsk);
printf("\nASYNC SERVER TASK STOPS\n");
return (void *)0;
}
static void *fast_fun(void *arg)
{
int jit, period;
RTIME expected;
if (!(Fast_Task = rt_thread_init(nam2num("FSTSK"), 2, 0, SCHED_FIFO, CPUMAP))) {
printf("CANNOT INIT FAST TASK\n");
exit(1);
}
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
rt_sem_wait_barrier(barrier);
period = nano2count(FASTMUL*TICK_TIME);
expected = start + 6*nano2count(TICK_TIME);
rt_task_make_periodic(Fast_Task, expected, period);
while (!end) {
jit = abs(count2nano(rt_get_time() - expected));
if (jit > fastjit) {
fastjit = jit;
}
rt_busy_sleep((FASTMUL*TICK_TIME*USEDFRAC)/100);
expected += period;
END("FE\n");
rt_task_wait_period();
BEGIN("FB\n");
}
rt_sem_wait_barrier(barrier);
rt_make_soft_real_time();
rt_thread_delete(Fast_Task);
return 0;
}
static void mfun(int t)
{
char tname[6] = "MFUN", mname[6] = "RMBX";
RT_TASK *mytask;
int msg[MAXSIZ + 1], mtype, i;
void *smbx, *rmbx;
randu();
tname[4] = mname[4] = t + '0';
tname[5] = mname[5] = 0;
mytask = rt_thread_init(nam2num(tname), t + 1, 0, SCHED_FIFO, 0xF);
smbx = rt_msgq_init(nam2num("SMSG"), 0, 0);
rmbx = rt_msgq_init(nam2num(mname), 0, 0);
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
msg[0] = t;
rt_sem_wait_barrier(barrier);
while (end < t) {
msg[MAXSIZ] = 0;
for (i = 1; i < MAXSIZ; i++) {
msg[MAXSIZ] += (msg[i] = MAXSIZ*randu());
}
if (rt_msg_send(smbx, msg, sizeof(msg), 1)) {
rt_printk("SEND FAILED, TASK: %d\n", t);
goto prem;
}
msg[0] = msg[1] = 0;
if (rt_msg_receive(rmbx, msg, sizeof(msg), &mtype) < 0) {
rt_printk("RECEIVE FAILED, TASK: %d\n", t);
goto prem;
}
if (msg[0] != t || msg[1] != 0xFFFFFFFF || mtype != RETPRIO) {
rt_printk("WRONG REPLY TO TASK: %d.\n", t);
goto prem;
}
cnt[t]++;
// rt_printk("TASK: %d, OK (%d).\n", t, cnt[t]);
rt_sleep(nano2count(SLEEP_TIME));
}
prem:
rt_msgq_delete(rmbx);
rt_msgq_delete(smbx);
rt_make_soft_real_time();
rt_task_delete(mytask);
printf("TASK %d ENDS.\n", t);
}
int main(void)
{
RT_TASK *receiving_task;
RT_TASK *agentask;
int i, *shm;
unsigned int msg, chksum;
struct sched_param mysched;
mysched.sched_priority = 99;
if( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 ) {
puts(" ERROR IN SETTING THE SCHEDULER UP");
perror( "errno" );
exit( 0 );
}
mlockall(MCL_CURRENT | MCL_FUTURE);
receiving_task = rt_task_init(nam2num("RTSK"), 0, 0, 0);
agentask = rt_get_adr(nam2num("ATSK"));
shm = rtai_malloc(nam2num("MEM"), 1);
while(1) {
printf("RECEIVING TASK RPCS TO AGENT TASK %x\n", 0xaaaaaaaa);
rt_rpc(agentask, 0xaaaaaaaa, &msg);
printf("AGENT TASK RETURNED %x\n", msg);
if (msg != 0xeeeeeeee) {
chksum = 0;
for (i = 1; i <= shm[0]; i++) {
chksum += shm[i];
}
printf("RECEIVING TASK: CHECKSUM = %x\n", chksum);
if (chksum != shm[shm[0] + 1]) {
printf("RECEIVING TASK: WRONG SHMEM CHECKSUM\n");
}
printf("RECEIVING TASK SENDS TO AGENT TASK %x\n", 0xaaaaaaaa);
rt_send(agentask, 0xaaaaaaaa);
} else {
printf("RECEIVING TASK DELETES ITSELF\n");
rt_task_delete(receiving_task);
printf("END RECEIVING TASK\n");
exit(1);
}
}
return 0;
}
int main (void)
{
int i,j;
M3EcSystemShm * sys;
RT_TASK *task;
pthread_t ptsys;
int cntr=0;
signal(SIGINT, endme);
sys = rtai_malloc (nam2num(SHMNAM_M3MKMD),1);
int ns=sys->slaves_active;
InitStats();
printf("Found %d active M3 EtherCAT slaves\n",ns);
if (ns==0)
{
printf("No slaves available. Exiting...\n");
return 0;
}
rt_allow_nonroot_hrt();
if (!(task = rt_task_init_schmod(nam2num("M3MAIN"), RT_TASK_PRIORITY, 0, 0, SCHED_FIFO, 0xF)))
{
rt_shm_free(nam2num(SHMNAM_M3MKMD));
printf("Cannot init the RTAI task %s\n","M3MAIN");
return 0;
}
hst=rt_thread_create((void*)rt_system_thread, sys, 10000);
usleep(100000); //Let start up
if (!sys_thread_active)
{
rt_task_delete(task);
rt_shm_free(nam2num(SHMNAM_M3MKMD));
printf("Startup of thread failed.\n",0);
return 0;
}
while(!end)
{
//SysEcShmPrettyPrint(sys);
usleep(250000);
}
printf("Removing RT thread...\n",0);
sys_thread_end=1;
rt_thread_join(hst);
if (sys_thread_active)printf("Real-time thread did not shutdown correctly\n");
rt_task_delete(task);
rt_shm_free(nam2num(SHMNAM_M3MKMD));
return 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;
}
INTERNAL_QUAL int rtos_task_create(RTOS_TASK* task,
int priority,
const char* name,
int sched_type,
size_t stack_size,
void * (*start_routine)(void *),
ThreadInterface* obj)
{
char taskName[7];
if ( strlen(name) == 0 )
name = "Thread";
strncpy(taskName, name, 7);
unsigned long task_num = nam2num( taskName );
if ( rt_get_adr(nam2num( taskName )) != 0 ) {
unsigned long nname = nam2num( taskName );
while ( rt_get_adr( nname ) != 0 ) // check for existing 'NAME'
++nname;
num2nam( nname, taskName); // set taskName to new name
taskName[6] = 0;
task_num = nname;
}
// Set and truncate name
task->name = strcpy( (char*)malloc( (strlen(name)+1)*sizeof(char) ), name);
// name, priority, stack_size, msg_size, policy, cpus_allowed ( 1111 = 4 first cpus)
// Set priority
task->priority = priority;
// Set rtai task struct to zero
task->rtaitask = 0;
RTAI_Thread* rt = (RTAI_Thread*)malloc( sizeof(RTAI_Thread) );
rt->priority = priority;
rt->data = obj;
rt->wrapper = start_routine;
rt->task = task;
rt->tnum = task_num;
int retv = pthread_create(&(task->thread), 0,
rtai_thread_wrapper, rt);
// poll for thread creation to be done.
int timeout = 0;
while ( task->rtaitask == 0 && ++timeout < 20)
usleep(100000);
return timeout < 20 ? retv : -1;
}
int init_module(void)
{
start_rt_timer(0);
worst_lat = rt_shm_alloc(nam2num("WSTLAT"), sizeof(RTIME), USE_VMALLOC);
task = rt_named_task_init_cpuid("LOOPER", fun, 0, 100000, 0, 1, 0, 1);
rt_task_resume(task);
return 0;
}
