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--;
}
}
}
int main(void)
{
int pid;
char ch;
char k = 'k';
RT_TASK *mytask;
MBX *Keyboard;
menu();
pid = fork();
if (!pid) {
execl("./screen", "./screen", NULL);
}
sleep(1);
if (!(mytask = rt_task_init(nam2num("KBRTSK"), 10, 0, 0))) {
printf("CANNOT INIT KEYBOARD TASK\n");
exit(1);
}
if (!(Keyboard = rt_get_adr(nam2num("KEYBRD")))) {
printf("CANNOT FIND KEYBOARD MAILBOX\n");
exit(1);
}
if (rt_mbx_send(Keyboard, &k, 1) > 0 ) {
fprintf(stderr, "Can't send initial command to RT-task\n");
exit(1);
}
do {
ch = get_key();
if (ch == 'p' || ch == 'P') {
menu();
}
if (ch != 'f' && rt_mbx_send_if(Keyboard, &ch, 1) > 0 ) {
fprintf(stderr, "Can't send command to RT-task\n");
}
} while (ch != 'f');
ch = 'r';
rt_mbx_send(Keyboard, &ch, 1);
ch = 'c';
rt_mbx_send(Keyboard, &ch, 1);
ch = 'f';
rt_mbx_send(Keyboard, &ch, 1);
rt_task_resume(rt_get_adr(nam2num("MASTER")));
while (rt_get_adr(nam2num("MASTER"))) {
rt_sleep(nano2count(1000000));
}
kill(pid, SIGINT);
rt_task_delete(mytask);
stop_rt_timer();
exit(0);
}
/***
* rt_mark_stack_mgr
*
*/
void rt_mark_stack_mgr(struct rtnet_device *rtdev)
{
struct rtnet_msg msg;
if (rtdev) {
msg.msg_type=Rx_PACKET;
msg.rtdev=rtdev;
rt_mbx_send_if(msg.rtdev->stack_mbx, &msg, sizeof (struct rtnet_msg));
}
}
static inline void send_to_mbx(FILE *fs, struct LOGMSG msg)
{
RT_TASK *buddy;
msg.msg[MAX_MSG_SIZE - 1] = 0;
if (++msg.nch >= (MAX_MSG_SIZE - 1)) {
msg.nch = MAX_MSG_SIZE - 1;
}
if (!(buddy = rt_buddy()) || (buddy && !rt_is_hard_real_time(buddy))) {
fprintf(stderr, msg.msg);
} else {
msg.fs = fs;
rt_mbx_send_if(logmbx, &msg, sizeof(int) + sizeof(FILE *) + msg.nch);
}
}
/****
* Realtime txqueue wakeup manager.
*
* Waits blocking for a wakeup message in its mailbox and calls the rtdevice
* specific wakeup function (if one is registered) after a message arrived.
* After processing the realtime txqueue, a wakeup message is sent to the
* proxy wakeup manager.
*/
void rt_wakeup_manager_task (int mgr_id) {
struct rtnet_msg msg;
struct rtnet_mgr *mgr = (struct rtnet_mgr *)mgr_id;
rt_printk("RTnet: Realtime txqueue wakeup manager started. (%p)\n", rt_whoami());
while(1) {
rt_mbx_receive(&(mgr->mbx), &msg, sizeof(struct rtnet_msg));
if ((msg.msg_type==WAKEUP) && (msg.rtdev)) {
if (msg.rtdev->rt_wakeup_xmit) {
msg.rtdev->rt_wakeup_xmit(msg.rtdev);
}
}
// The next call should depend on the not yet specified returncode of rt_wakeup_xmit().
rt_mbx_send_if(&(proxy_wakeup_manager.mbx), &msg, sizeof (struct rtnet_msg));
}
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
float data;
#ifndef MATLAB_MEX_FILE
MBX *mbx = (MBX *)ssGetPWorkValue(S,0);
data = (float)*uPtrs[0];
if (data < -1.0) data = -1.0;
if (data > 1.0) data = 1.0;
#ifdef KRTAI
mbx_rt_mbx_send_if(mbx, &data, sizeof(data));
#else
rt_mbx_send_if(mbx, &data, sizeof(data));
#endif
#endif
}
static void receiver(long nothing)
{
fd_set rxfds;
RTIME timeout = 0;
int ready = 0;
socklen_t fromlen = sizeof(rx_addr);
rt_printk("RtnetTest: Receiver task initialised\n");
while(!end) {
FD_ZERO(&rxfds);
FD_SET(sock, &rxfds);
ready = 0;
ready = rt_dev_select(sock + 1, &rxfds, NULL, NULL, timeout);
if (ready > 0 && FD_ISSET(sock, &rxfds)) {
rlen = rt_dev_recvfrom(sock, buffer_in, sizeof(buffer_in), 0, (struct sockaddr*) &rx_addr, &fromlen);
if (rlen > 0) {
rx_samp.cnt++;
// rx_samp.tx = simple_strtoll(buffer_in, NULL, 10);
sscanf(buffer_in, "%lld", &rx_samp.tx);
rx_samp.rx = rt_get_real_time_ns();
rt_mbx_send_if(mbx, &rx_samp, sizeof(rx_samp));
memset(buffer_in, 0, sizeof(buffer_in));
}
} else if (EPERM == -ready) {
rt_printk("RtnetTest: Failed to rt_dev_select on socket\n");
end = 1;
break;
} else if (EINTR == -ready) {
rt_printk("RtnetTest: rt_dev_select was interrupted\n");
end = 1;
break;
}
}
return;
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
int_T *dim = ssGetInputPortDimensions(S,0);
struct {
float u[dim[0]*dim[1]];
} data;
int i;
#ifndef MATLAB_MEX_FILE
MBX *mbx = (MBX *)ssGetPWork(S)[0];
for (i = 0; i < (dim[0]*dim[1]); i++) {
data.u[i] = (float)*uPtrs[i];
}
#ifdef KRTAI
mbx_rt_mbx_send_if(mbx, &data, sizeof(data));
#else
rt_mbx_send_if(mbx, &data, sizeof(data));
#endif
#endif
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
InputRealPtrsType uPtrs = ssGetInputPortRealSignalPtrs(S,0);
unsigned int led_mask = 0;
int i;
#ifndef MATLAB_MEX_FILE
MBX *mbx = (MBX *)ssGetPWorkValue(S,0);
for (i = 0; i < NUM_LEDS; i++) {
if (*uPtrs[i] > 0.) {
led_mask += (1 << i);
} else {
led_mask += (0 << i);
}
}
#ifdef KRTAI
mbx_rt_mbx_send_if(mbx, &led_mask, sizeof(led_mask));
#else
rt_mbx_send_if(mbx, &led_mask, sizeof(led_mask));
#endif
#endif
}
int main(void)
{
RT_TASK *rtask;
MBX *mbx;
int maxisr = 0, maxsched = 0, ovrun = 0, semcnt;
struct { int isr, sched; } latency;
int timer_period, timer_freq, h_timer_freq;
int count, perns, pervar, maxpervar = 0;
RTIME tp, t;
rt_allow_nonroot_hrt();
pthread_create(&thread, NULL, endt, NULL);
iopl(3);
if (!(rtask = rt_task_init_schmod(nam2num("RTASK"), 0, 0, 0, SCHED_FIFO, ALLOWED_CPUS))) {
printf("CANNOT CREATE REAL TIME TASK\n");
return 1;
}
if (!(mbx = rt_named_mbx_init("LATMBX", 100*sizeof(latency)))) {
printf("CANNOT CREATE MAILBOX\n");
rt_task_delete(rtask);
return 1;
}
rt_make_hard_real_time();
timer_period = 1;
timer_freq = 2;
tmr_get_setup(timer, &timer_period, &timer_freq);
h_timer_freq = timer_freq/2;
perns = timer_period*1000000000LL/timer_freq;
tmr_start(timer);
#ifdef USE_EXT_WAIT
printf("Wait_on_timer returns timer count and cpu time.\n");
#else
printf("Wait_on_timer does just that.\n");
#endif
#ifdef DISABLE_INTR
printf("Wait_on_timer and times getting with interrupt disabled.\n");
#else
printf("Wait_on_timer and times getting with interrupt enabled.\n");
#endif
printf("Timer setup completed, running.\n\n");
mlockall(MCL_CURRENT | MCL_FUTURE);
rt_make_hard_real_time();
while (wait_on_timer(timer) > 0);
tp = rt_get_cpu_time_ns();
while (!end) {
CLI();
CHECK_FLAGS();
#ifdef USE_EXT_WAIT
if ((semcnt = wait_on_timer_ext(timer, &count, &t)) > 0) {
rt_printk("OVRN: %d, TOT: %d\n", semcnt, ++ovrun);
}
#else
if ((semcnt = wait_on_timer(timer)) > 0) {
rt_printk("OVRN: %d, TOT: %d\n", semcnt, ++ovrun);
}
count = tmr_get_count(timer, &t);
#endif
CHECK_FLAGS();
STI();
if ((latency.sched = ((timer_period - count)*1000000 + h_timer_freq)/timer_freq) > maxsched) {
maxsched = latency.sched;
}
if ((latency.isr = ((timer_period - tmr_get_isr_count(timer))*1000000 + h_timer_freq)/timer_freq) > maxisr) {
maxisr = latency.isr;
}
pervar = abs((int)(t - tp));
tp = t;
if (pervar > maxpervar) {
maxpervar = pervar;
}
rt_mbx_send_if(mbx, &latency, sizeof(latency));
// rt_printk("MXI %d, MXS %d\n", maxisr, maxsched);
}
tmr_stop(timer);
rt_make_soft_real_time();
rt_task_delete(rtask);
rt_mbx_delete(mbx);
printf("*** MAX LATENCIES: ISR %d (us), SCHED %d (us) ***\n", maxisr, maxsched);
printf("*** MAX PERIOD VARIATION %d (us) ***\n", (maxpervar - perns + 500)/1000);
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;
}
void* TrackerSim::_thread(void *arg)
{
RT_TASK *rt_task;
RTIME interval = nano2count(_read_interval_ms * 1E6);
MBX *mbx_tracker;
int val;
rt_allow_nonroot_hrt();
mlockall(MCL_CURRENT | MCL_FUTURE);
// create task
rt_task = rt_task_init_schmod(nam2num("TSKTRA"), 2, 0, 0, SCHED_FIFO, rttools::cpu_id(0));
if (!rt_task)
{
ERROR("Cannot init TRACKER task");
pthread_exit(NULL);
}
mbx_tracker = rttools::get_mbx(MBX_TRACKER_NAME, MBX_TRACKER_BLOCK * sizeof(trackerdata_t));
if (!mbx_tracker)
{
ERROR("Cannot init TRACKER mailbox");
rt_task_delete(rt_task);
pthread_exit(NULL);
}
while (_running)
{
// simulate the current position
switch (_sim_type)
{
case constant:
sim_constant();
break;
case calculated:
sim_calculated_az();
//sim_calculated_el();
break;
case from_file:
sim_from_file();
break;
case cipic_angles:
sim_cipic_angles();
break;
}
_data.timestamp = (unsigned long) rt_get_time_ns();
// send message (non-blocking)
val = rt_mbx_send_if(mbx_tracker, &_data, sizeof(trackerdata_t));
if (-EINVAL == val)
{
ERROR("Mailbox is invalid");
break;
}
rt_sleep(interval);
}
rt_task_delete(rt_task);
rttools::del_mbx(MBX_TRACKER_NAME);
return arg;
}
