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