void RT::OS::sleepTimestep(RT::OS::Task task) { xenomai_task_t *t = reinterpret_cast<xenomai_task_t *>(task); // Prevent significant early wake up from happening and drying the Linux system rt_task_sleep_until(t->next_t - t->wakeup_t); // Busy sleep until ready for the next cycle rt_timer_spin(rt_timer_ticks2ns(t->next_t - rt_timer_read())); // Update next interrupt time t->next_t += t->period; }
static inline void add_histogram (long *histogram, long addval) { /* bucketsize steps */ long inabs = rt_timer_ticks2ns(addval >= 0 ? addval : -addval) / bucketsize; histogram[inabs < histogram_size ? inabs : histogram_size-1]++; }
void latency (void *cookie) { int err, count, nsamples; RTIME expected, period; err = rt_timer_start(TM_ONESHOT); if (err) { fprintf(stderr,"latency: cannot start timer, code %d\n",err); return; } nsamples = ONE_BILLION / sampling_period; period = rt_timer_ns2ticks(sampling_period); expected = rt_timer_tsc(); err = rt_task_set_periodic(NULL,TM_NOW,sampling_period); if (err) { fprintf(stderr,"latency: failed to set periodic, code %d\n",err); return; } for (;;) { long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj; overrun = 0; test_loops++; for (count = sumj = 0; count < nsamples; count++) { unsigned long ov; expected += period; err = rt_task_wait_period(&ov); if (err) { if (err != -ETIMEDOUT) rt_task_delete(NULL); /* Timer stopped. */ overrun += ov; } dt = (long)(rt_timer_tsc() - expected); if (dt > maxj) maxj = dt; if (dt < minj) minj = dt; sumj += dt; if (!finished && (do_histogram || do_stats)) add_histogram(histogram_avg, dt); } if (!finished && (do_histogram || do_stats)) { add_histogram(histogram_max, maxj); add_histogram(histogram_min, minj); } minjitter = rt_timer_ticks2ns(minj); maxjitter = rt_timer_ticks2ns(maxj); avgjitter = rt_timer_ticks2ns(sumj / nsamples); rt_sem_v(&display_sem); } }
int main(void) { int ret; RT_TASK gyro_read_task; RT_TASK motor_cmd_task; RT_TASK mosq_task; if (uei_of_initialize() < 0) exit(1); if (uei_ethercat_initialize() < 0) exit(1); if (uei_framing_init() < 0) exit(1); printf("Initialized!\n"); atexit(uei_cleanup); // Create our Tasks ret = rt_task_create(&gyro_read_task, "read_serial", 0, T_HIPRIO, T_JOINABLE|T_FPU); if (ret) { perror("failed to create task"); exit(1); } ret = rt_task_create(&motor_cmd_task, "motor_cmds", 0, 30, T_JOINABLE); if (ret) { perror("failed to create motor task"); exit(1); } ret = rt_task_create(&mosq_task, "mosq_monitor", 0, 40, T_JOINABLE); if (ret) { perror("failed to create Mosquitto task"); exit(1); } ret = rt_task_start(&gyro_read_task, &gyro_read_routine, (void*)(&uei_handle)); if (ret) { perror("failed to start gyro_read_task"); exit(1); } ret = rt_task_start(&motor_cmd_task, &motor_cmd_routine, NULL); if (ret) { perror("failed to start motor_cmd_task"); exit(1); } ret = rt_task_start(&mosq_task, &uei_framing_loop, NULL); if (ret) { perror("failed to start frame handling routine"); exit(1); } while (!stop) { float yaw,pitch,roll; usleep(100000); // printf("%d\n", ethercat_get_current()); uei_gyro_get_vals(&roll,&yaw,&pitch); printf("%f\t--\t%f\t%f\t%f\n", rt_timer_ticks2ns(rt_timer_read()) / 1000000000.0, yaw, pitch,roll); fflush(stdout); } rt_task_join(&gyro_read_task); rt_task_join(&motor_cmd_task); rt_task_join(&mosq_task); }