/*
Thread that runs f2().
*/
void* function2(void* arg){
//wait so that all tasks activate at the same time
while(rt_task_wait_period() == -1);
while(1){
f2(2,3);
struct timespec response_time;
//get execution end time
clock_gettime(CLOCK_MONOTONIC, &response_time);
//calculate response time
struct timespec activation_time;
rt_give_last_activation(&activation_time);
response_time = timespec_diference(response_time, activation_time);
//check if it's the worse response time so far
struct timespec temp = timespec_diference(worse_response_time[1], response_time);
if(temp.tv_sec < 0 || temp.tv_nsec < 0){
worse_response_time[1] = response_time;
}
rt_task_wait_period();
}
}
void send_msg(void *arg)
{
int ret;
struct msghdr msg;
struct iovec iov[2];
unsigned short msgsize = size;
while(1) {
iov[0].iov_base = &msgsize;
iov[0].iov_len = sizeof(msgsize);
iov[1].iov_base = buffer_out;
iov[1].iov_len = size;
memset(&msg, 0, sizeof(msg));
msg.msg_name = &dest_addr;
msg.msg_namelen = sizeof(dest_addr);
msg.msg_iov = iov;
msg.msg_iovlen = 2;
printk("Sending message of %d+2 bytes\n", size);
ret = rt_dev_sendmsg(sock, &msg, 0);
if (ret != (int)(sizeof(msgsize) + size))
printk(" rt_dev_sendmsg() = %d!\n", ret);
#ifdef CONFIG_XENO_2_0x /* imported via rtnet_config.h */
rt_task_wait_period(); /* old signature */
#else /* Xenomai 2.1 and later */
rt_task_wait_period(NULL);
#endif /* CONFIG_XENO_2_0x */
}
}
static void sender(long nothing)
{
int diff = 0, warmup = 1000000000/WORKCYCLE;
RTIME t, tb;
struct sample { unsigned long cnt; RTIME tx, rx; } samp = { 0, 0, 0 };
rt_printk("RtnetTest: Transmitter task initialised\n");
tb = t = rt_get_real_time_ns();
while(!end) {
slen = sprintf(buffer_out, "%lld", t);
slen = rt_dev_sendto(sock, buffer_out, slen, 0, (struct sockaddr*)&tx_addr, sizeof(tx_addr));
if (slen < 0) {
rt_printk("RtnetTest: Packet send failed! Errno %d\n", -slen);
return;
}
rt_task_wait_period();
t = rt_get_real_time_ns();
if (!warmup) {
diff = abs((int)(t - tb - WORKCYCLE));
samp.cnt++;
tb = t;
if (diff > samp.rx) samp.rx = diff;
rt_mbx_send_if(mbx, &samp, sizeof(samp));
} else {
tb = rt_get_real_time_ns();
warmup--;
}
}
}
void subscribe_proc(void *arg)
{
RTROSSubscriber* rtsub = (RTROSSubscriber*)arg;
rt_task_set_periodic(NULL, TM_NOW, 1e6); // 1e6 -> 1ms
int i;
while(1)
{
rt_task_wait_period(NULL); //wait for next cycle
rt_dynamixel_msgs::JointSetConstPtr rcvMsg = rtsub->subSetter.poll();
if(rcvMsg) // if message recieved ( if not rcvMsg == NULL )
{
// Data set
// ROS_INFO("Sub ")
for(i=0;i<4;i++)
{
dxlDevice[i].mutex_acquire();
}
for(i=0;i< (int)rcvMsg->id.size();i++)
{
if(check_vaild_dxl_from_id(rcvMsg->id[i]))
{
dxl_from_id(rcvMsg->id[i]).aim_radian = rcvMsg->angle[i];
}
}
for(i=0;i<4;i++)
{
dxlDevice[i].mutex_release();
}
}
}
}
void xmit_msg(void *arg)
{
int ret;
struct msghdr msg;
struct iovec iov[2];
unsigned short msgsize = size;
rt_task_set_periodic(NULL, TM_NOW, CYCLE);
while (1) {
iov[0].iov_base = &msgsize;
iov[0].iov_len = sizeof(msgsize);
iov[1].iov_base = buffer_out;
iov[1].iov_len = size;
memset(&msg, 0, sizeof(msg));
msg.msg_name = &dest_addr;
msg.msg_namelen = sizeof(dest_addr);
msg.msg_iov = iov;
msg.msg_iovlen = 2;
rt_printf("Sending message of %d+2 bytes\n", size);
ret = sendmsg_rt(sock, &msg, 0);
if (ret != (int)(sizeof(msgsize) + size))
rt_printf(" sendmsg_rt() = %d!\n", ret);
rt_task_wait_period();
}
}
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);
}
void testtask(void *cookie){
int count = 0;
int ret;
unsigned long overrun;
ret = rt_task_set_periodic(NULL, TM_NOW, rt_timer_ns2ticks(task_period_ns));
if (ret) {
printf("error while set periodic, code %d\n",ret);
return;
}
while(!end){
ret = rt_task_set_mode(0, T_CONFORMING, NULL);
//ret = rt_task_set_mode( 0,T_RRB,0 );
if (ret) {
printf("error while rt_task_set_mode, code %d\n",ret);
return;
}
ret = rt_task_wait_period(&overrun);
if (ret) {
printf("error while rt_task_wait_period, code %d\n",ret);
return;
}
count++;
printf("message from testtask: count=%d\n", count);
fflush(NULL);
}
}
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);
}
}
void sun_task(void* arg) {
sun_t* this = (sun_t*) arg;
rt_task_set_periodic(NULL, TM_NOW, SUN_PERIOD);
char buffer[9];
while (1) {
rt_task_wait_period(NULL);
sun_update_simulated_time(this);
if (this->current_simulated_time == 1200) {
sprintf(buffer, "23:59:59");
} else {
int seg_tot, h, r, min, seg;
seg_tot = (this->current_simulated_time * 86400) / 1200;
h = seg_tot / 3600;
r = seg_tot % 3600;
min = r / 60;
seg = r % 60;
sprintf(buffer, "%02d:%02d:%02d", h, min, seg);
}
FILE *fp;
if ((fp = fopen("/var/www/HoraVirtual.txt.new", "w+")) < 0) {
rt_printf("Error en archivo destino");
}
fprintf(fp, "%s", buffer);
fclose(fp);
system("mv /var/www/HoraVirtual.txt.new /var/www/HoraVirtual.txt");
}
}
void accelerometer(void *arg)
{
RTIME now;
int acceptable;
/*
* Arguments: &task (NULL=self), start time, period
*/
rt_task_set_periodic(NULL, TM_NOW, 33333333);
while (isRunning) {
rt_task_wait_period(NULL);
acceptable = 0;
//acceptable = rand() % (800 - 0 + 1) + 0; // acceleration between 0-50
acceptable = rand_lim(20);
rt_mutex_acquire(&mutex_acc,TM_INFINITE);
if(acceptable > acc_sample)
acc_sample = acc_sample + 1;
else if(acceptable < acc_sample)
acc_sample = acc_sample - 1;
rt_mutex_release(&mutex_acc);
}
}
void task(void *arg)
{
int err = 0;
rt_printf("Task started. This is computer 1\n");
if(run == 0) err = rt_task_set_periodic(NULL, TM_NOW, PERIOD);
if(err != 0) rt_printf("scheduling task filed with err %d: %s\n", err), strerror(-err);
outb(inb(0x378) | 0x01, 0x378); //set D0 HIGH
while(run<NUMRUNS){
RTIME s = rt_timer_read();
//set D0 LOW and HIGH again
outb(inb(0x378) & 0xfe, 0x378);
outb(inb(0x378) | 0x01, 0x378);
//wait for respons:
rt_intr_wait(&keypress, TM_INFINITE);
diffs[run] = rt_timer_read() - s;
run++;
rt_task_wait_period(NULL);
}
rt_printf("Done listening, saving to file\n");
write_RTIMES(FILEO, NUMRUNS, diffs);
rt_printf("Done\n");
}
void verifier_batterie(void *arg) {
DBattery *batterie = d_new_battery();
int status;
int vbat;
rt_printf("tbattery : Attente du sémaphore semVerifierBatterie\n");
rt_sem_p(&semVerifierBatterie, TM_INFINITE);
rt_printf("tbattery : Debut de l'execution periodique à 1s\n");
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
while (1) {
while (cptCommErr < MAX_ECHECS) {
/* Attente de l'activation périodique */
rt_task_wait_period(NULL);
rt_printf("tbattery : Activation périodique\n");
status = robot->get_vbat(robot, &vbat);
if (status == STATUS_OK) {
cptCommErr = 0;
if (vbat == BATTERY_OFF || vbat == BATTERY_LOW || vbat == BATTERY_OK)
batterie->set_level(batterie, vbat);
} else {
cptCommErr++;
rt_printf("tbattery : Erreur de communication avec le robot (%d)\n", cptCommErr);
}
}
comm_err_handler(status);
rt_sem_p(&semVerifierBatterie, TM_INFINITE);
}
}
void recharger_watchdog(void *arg) {
int status;
rt_printf("twatchdog : Attente du sémaphore semRechargerWatchdog\n");
rt_sem_p(&semRechargerWatchdog, TM_INFINITE);
rt_printf("twatchdog : Debut de l'execution periodique à 1s\n");
rt_task_set_periodic(NULL, TM_NOW, 1000000000);
while (1) {
while (cptCommErr < MAX_ECHECS) {
/* Attente de l'activation périodique */
rt_task_wait_period(NULL);
rt_printf("twatchdog : Activation périodique\n");
status = robot->reload_wdt(robot);
if (status == STATUS_OK) {
cptCommErr = 0;
} else {
cptCommErr++;
rt_printf("twatchdog : Erreur de communication avec le robot (%d)\n", cptCommErr);
}
}
comm_err_handler(status);
rt_sem_p(&semRechargerWatchdog, TM_INFINITE);
}
}
int main(void)
{
RT_TASK *task;
RTIME now;
int cnt=0;
// make main thread LXRT soft realtime
task = rt_task_init_schmod(nam2num("MYTASK"), 9, 0, 0, SCHED_FIFO, 0xF);
mlockall(MCL_CURRENT | MCL_FUTURE);
// start realtime timer and scheduler
//rt_set_oneshot_mode();
rt_set_periodic_mode();
start_rt_timer(0);
now = rt_get_time() + 10*PERIOD;
rt_task_make_periodic(task, now, PERIOD);
printf("Init mutex and cond.\n");
mutex = rt_sem_init(nam2num("MUTEX"), 1);
if (mutex==0)
printf("Error init mutex\n");
cond = rt_cond_init(nam2num("CONDITION"));
if (cond==0)
printf("Error init cond\n");
thread0 = rt_thread_create(fun0, NULL, 10000);
//thread1 = rt_thread_create(fun1, NULL, 20000);
//rt_sleep(PERIOD);
while (cnt < THRESHOLD) {
rt_task_wait_period();
rt_printk("main: Hello World %d!\n",cnt);
rt_sem_wait(mutex); //now the mutex should have value 0
if (instance_cnt==0) {
rt_sem_signal(mutex); //now the mutex should have vaule 1
rt_printk("worker thread busy!\n");
} else {
instance_cnt++;
rt_cond_signal(cond);
rt_sem_signal(mutex); //now the mutex should have vaule 1
rt_printk("signaling worker thread to start!\n");
}
cnt++;
}
// wait for end of program
printf("TYPE <ENTER> TO TERMINATE\n");
getchar();
// cleanup
stop_rt_timer();
return 0;
}
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;
}
void sensor_fusion(void *arg)
{
RTIME now;
int i,sum, print,k, count;
/*
* Arguments: &task (NULL=self), start time, period
*/
rt_task_set_periodic(NULL, TM_NOW, 16666666);
count =0;
while (isRunning) {
count++;
rt_task_wait_period(NULL);
rt_mutex_acquire(&mutex_acc,TM_INFINITE);
rt_mutex_acquire(&mutex_gyro,TM_INFINITE);
k=gyroIndex;
sum=0; print = 0;
for (i=0; i<5; i++){
sum = sum + gyro_sample[k];
k--;
}
print = ((sum/5) + acc_sample)/2;
rt_mutex_release(&mutex_gyro);
rt_mutex_release(&mutex_acc);
printlog[logIndex] = print;
logIndex++;
if(count == 50)
isRunning = 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 *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();
}
}
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 event(void *cookie)
{
int err;
err = rt_task_set_periodic(NULL,
TM_NOW,
rt_timer_ns2ticks(sampling_period));
if (err) {
warning("failed to enter periodic timing (%s)\n",
symerror(err));
return;
}
for (;;) {
err = rt_task_wait_period(NULL);
if (err) {
if (err != -ETIMEDOUT)
exit(EXIT_FAILURE);
late++;
}
switch_count++;
err = rt_sem_broadcast(&switch_sem);
switch_tsc = rt_timer_tsc();
if (err) {
if (err != -EIDRM && err != -EINVAL)
warning("failed to broadcast semaphore (%s)\n",
symerror(err));
break;
}
}
}
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"); }
}
void event(void *cookie)
{
int err;
err = rt_task_set_periodic(NULL,
TM_NOW,
rt_timer_ns2ticks(sampling_period));
if (err) {
fprintf(stderr,"switch: failed to set periodic, code %d\n", err);
return;
}
for (;;) {
err = rt_task_wait_period(NULL);
if (err) {
if (err != -ETIMEDOUT) {
/* Timer stopped. */
rt_task_delete(NULL);
}
}
switch_count++;
switch_tsc = rt_timer_tsc();
rt_sem_v(&switch_sem);
}
}
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 trainCtrl_periodic() {
unsigned long long buffer = 0;
unsigned long long tramaIdle = 0xFFFDFE007FC00000ULL;
char bufferBit = 0xFF;
rt_task_set_periodic(NULL, TM_NOW, TASK_TRAINCTRL_PERIOD);
while (1) {
rt_task_wait_period(NULL);
if (bufferBit == 0xFF) {
if (buffer == 0) {
if (tramaPendiente == 1) {
buffer = bufferSetVel;
tramaIdle = buffer;
//mvprintw(16, 3,"%llu",buffer);
tramaPendiente = 0;
} else {
buffer = tramaIdle;
}
}
if ((buffer & 0x8000000000000000ULL) == 0x8000000000000000ULL) {
bufferBit = 0xFD;
} else {
bufferBit = 0xF3;
}
buffer = buffer << 1;
}
digitalWrite(LINE_OUT, (bufferBit & 0x01));
bufferBit = bufferBit >> 1;
bufferBit = bufferBit | 0x80;
}
}
void fonction_periodique (void * arg)
{
int err;
unsigned long depassements;
// rend le thread periodique
// a partir de TM_NOW
// periode en nanosecondes
rt_task_set_periodic(rt_task_self(), TM_NOW, 1000000000);
// printf pour xenomai
rt_printf("[%lld] Timer programmé...\n", rt_timer_read());
// rt_task_wait_period
// attend le reveil
// depassement : permet de savoir si on a manqué des reveils
while ((err = rt_task_wait_period(& depassements)) == 0) {
rt_printf("[%lld]", rt_timer_read());
if (depassements != 0)
rt_printf(" Depassements : %lu", depassements);
rt_printf("\n");
}
fprintf(stderr, "rt_task_wait_period(): %s\n",strerror(-err));
exit(EXIT_FAILURE);
}
void Odo(void *arg){
double rtick_prev = 0;
double ltick_prev = 0;
double dtick_r = 0;
double dtick_l = 0;
char r_mesg[40], l_mesg[40];
int r_tick, l_tick;
double Dr, Dc, Dl, x, y, theta, x_dt, y_dt, theta_dt, x_new, y_new, theta_new;
Dr = Dc = Dl = 0;
x = y = theta = 0;
x_new=y_new=theta_new = 0;
x_dt = y_dt = theta_dt = 0;
rt_task_set_periodic(NULL, TM_NOW, period);
while(1){
rt_task_wait_period(NULL);
rt_queue_read(&rqueue, r_mesg, sizeof(r_mesg), TM_INFINITE);
rt_queue_read(&lqueue, l_mesg, sizeof(l_mesg), TM_INFINITE);
r_tick = atoi(r_mesg);
l_tick = atoi(l_mesg);
//rt_printf("int data: %d \n", l_tick);
//rt_printf("data r : %d \n", r_tick);
dtick_r = r_tick - rtick_prev;
dtick_l = l_tick - ltick_prev;
Dr = m_per_tick*dtick_r;
Dl = m_per_tick*dtick_l;
Dc = (Dr+Dl)/2;
x_dt = Dc*cos(theta);
y_dt = Dc*sin(theta);
theta_dt = (Dr-Dl)/L;
theta_new = theta + theta_dt;
x_new = x + x_dt;
y_new = y + y_dt;
rtick_prev = r_tick;
ltick_prev = l_tick;
x = x_new;
y = y_new;
theta = theta_new;
rt_printf("Robot pose (x, y, theta) is: %lf, %lf, %lf\n", x, y, theta);
}
}
INTERNAL_QUAL int rtos_task_wait_period( RTOS_TASK* mytask )
{
if (mytask->rtaitask == 0)
return -1;
// only in RTAI 3.2, this returns overrun or not.
// so do not use retval for compatibility reasons.
rt_task_wait_period();
return 0;
}
void fun(int t)
{
unsigned int loops = LOOPS;
while(loops--) {
cpu_used[hard_cpu_id()]++;
outb(t, LPT);
rt_task_wait_period();
}
}
static void intr_handler(long t)
{
char wakeup;
while(1) {
cpu_used[hard_cpu_id()]++;
rtf_put(CMDF, &wakeup, sizeof(wakeup));
rt_task_wait_period();
}
}
void *thread_fun(void *arg)
{
RTIME start_time, period;
RTIME t0, t;
SEM *sem;
RT_TASK *mytask;
unsigned long mytask_name;
int mytask_indx, jit, maxj, maxjp, count;
mytask_indx = *((int *)arg);
mytask_name = taskname(mytask_indx);
cpus_allowed = 1 - cpus_allowed;
if (!(mytask = rt_task_init_schmod(mytask_name, 1, 0, 0, SCHED_FIFO, 1 << cpus_allowed))) {
printf("CANNOT INIT TASK %lu\n", mytask_name);
exit(1);
}
printf("THREAD INIT: index = %d, name = %lu, address = %p.\n", mytask_indx, mytask_name, mytask);
mlockall(MCL_CURRENT | MCL_FUTURE);
if (!(mytask_indx%2)) {
rt_make_hard_real_time();
}
rt_receive(0, (unsigned long *)((void *)&sem));
period = nano2count(PERIOD);
start_time = rt_get_time() + nano2count(10000000);
rt_task_make_periodic(mytask, start_time + (mytask_indx + 1)*period, ntasks*period);
// start of task body
{
count = maxj = 0;
t0 = rt_get_cpu_time_ns();
while(count++ < LOOPS) {
rt_task_wait_period();
t = rt_get_cpu_time_ns();
if ((jit = t - t0 - ntasks*(RTIME)PERIOD) < 0) {
jit = -jit;
}
if (count > 1 && jit > maxj) {
maxj = jit;
}
t0 = t;
// rtai_print_to_screen("THREAD: index = %d, count %d\n", mytask_indx, count);
}
maxjp = (maxj + 499)/1000;
}
// end of task body
rt_sem_signal(sem);
if (!(mytask_indx%2)) {
rt_make_soft_real_time();
}
rt_task_delete(mytask);
printf("THREAD %lu ENDS, LOOPS: %d MAX JIT: %d (us)\n", mytask_name, count, maxjp);
return 0;
}
