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; }