void cleanup(void) { time_t actual_duration; long gmaxj, gminj, gavgj; if (test_mode == USER_TASK) { rt_sem_delete(&display_sem); gavgjitter /= (test_loops > 1 ? test_loops : 2) - 1; gminj = rt_timer_tsc2ns(gminjitter); gmaxj = rt_timer_tsc2ns(gmaxjitter); gavgj = rt_timer_tsc2ns(gavgjitter); } else { struct rttst_overall_bench_res overall; overall.histogram_min = histogram_min; overall.histogram_max = histogram_max; overall.histogram_avg = histogram_avg; rt_dev_ioctl(benchdev, RTTST_RTIOC_TMBENCH_STOP, &overall); gminj = overall.result.min; gmaxj = overall.result.max; gavgj = overall.result.avg; goverrun = overall.result.overruns; } if (benchdev >= 0) rt_dev_close(benchdev); if (need_histo()) dump_hist_stats(); time(&test_end); actual_duration = test_end - test_start - WARMUP_TIME; if (!test_duration) test_duration = actual_duration; printf ("---|-----------|-----------|-----------|--------|------|-------------------------\n" "RTS|%11.3f|%11.3f|%11.3f|%8ld|%6u| %.2ld:%.2ld:%.2ld/%.2d:%.2d:%.2d\n", (double)gminj / 1000, (double)gavgj / 1000, (double)gmaxj / 1000, goverrun, max_relaxed, actual_duration / 3600, (actual_duration / 60) % 60, actual_duration % 60, test_duration / 3600, (test_duration / 60) % 60, test_duration % 60); if (max_relaxed > 0) printf( "Warning! some latency maxima may have been due to involuntary mode switches.\n" "Please contact [email protected]\n"); if (histogram_avg) free(histogram_avg); if (histogram_max) free(histogram_max); if (histogram_min) free(histogram_min); exit(0); }
static int gpio_interrupt(rtdm_irq_t *irq_handle) { RTIME temp_time; static SRTIME curr_time; static SRTIME prev_time; static SRTIME diff_time; static int freq; temp_time = rt_timer_tsc(); curr_time = rt_timer_tsc2ns(temp_time); diff_time = curr_time - prev_time; prev_time = curr_time; //Get frequency freq = (uint32_t)div_u64(100000000000, diff_time); //rtdm_printk("F: %u \n", freq); set_next_tooth(curr_time); //freq = 100000000000 / diff_time; //Diff_time is in ns, freq = times*1000 return RTDM_IRQ_HANDLED; }
static inline void add_histogram(long *histogram, long addval) { /* bucketsize steps */ long inabs = rt_timer_tsc2ns(addval >= 0 ? addval : -addval) / bucketsize; histogram[inabs < histogram_size ? inabs : histogram_size - 1]++; }
void cleanup_upon_sig(int sig __attribute__((unused))) { time_t actual_duration; long gmaxj, gminj, gavgj; if (finished) return; finished = 1; if (!hard_timer_running) { rt_timer_stop(); } rt_sem_delete(&display_sem); if (do_histogram || do_stats) dump_hist_stats(); time(&test_end); actual_duration = test_end - test_start - WARMUP_TIME; if (!test_duration) test_duration = actual_duration; gavgjitter /= (test_loops > 1 ? test_loops : 2)-1; gminj = rt_timer_tsc2ns(gminjitter); gmaxj = rt_timer_tsc2ns(gmaxjitter); gavgj = rt_timer_tsc2ns(gavgjitter); printf("---|------------|------------|------------|--------|-------------------------\n" "RTS|%12ld|%12ld|%12ld|%8ld| %.2ld:%.2ld:%.2ld/%.2d:%.2d:%.2d\n", gminj, gavgj, gmaxj, goverrun, actual_duration / 3600, (actual_duration / 60) % 60, actual_duration % 60, test_duration / 3600, (test_duration / 60) % 60, test_duration % 60); if (histogram_avg) free(histogram_avg); if (histogram_max) free(histogram_max); if (histogram_min) free(histogram_min); exit(0); }
void check_sleep_inner(const char *fn, const char *prefix, unsigned long long start) { unsigned long long diff = rt_timer_tsc2ns(rt_timer_tsc() - start); if (diff < 10 * NS_PER_MS) { fprintf(stderr, "%s waited %Ld.%03u us\n", prefix, diff / 1000, (unsigned)(diff % 1000)); exit(EXIT_FAILURE); } }
static void check_sleep_inner(const char *fn, int line, const char *msg, unsigned long long start) { unsigned long long diff = rt_timer_tsc2ns(rt_timer_tsc() - start); if (diff < 300 * NS_PER_MS) { fprintf(stderr, "FAILED %s:%d: %s waited only %Ld.%06u ms\n", fn, line, msg, diff / 1000000, (unsigned)(diff % 1000000)); exit(EXIT_FAILURE); } }
void latency (void *cookie) { int err, count, nsamples, warmup = 1; RTIME expected_tsc, period_tsc, start_ticks; RT_TIMER_INFO timer_info; RT_QUEUE q; rt_queue_create(&q, "queue", 0, 100, 0); if (!(hard_timer_running = rt_is_hard_timer_running())) { err = rt_timer_start(TM_ONESHOT); if (err) { fprintf(stderr,"latency: cannot start timer, code %d\n",err); return; } } err = rt_timer_inquire(&timer_info); if (err) { fprintf(stderr,"latency: rt_timer_inquire, code %d\n",err); return; } nsamples = ONE_BILLION / period_ns / 1; period_tsc = rt_timer_ns2tsc(period_ns); /* start time: one millisecond from now. */ start_ticks = timer_info.date + rt_timer_ns2ticks(1000000); expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000); err = rt_task_set_periodic(NULL,start_ticks,period_ns); if (err) { fprintf(stderr,"latency: failed to set periodic, code %d\n",err); return; } for (;;) { long minj = TEN_MILLION, maxj = -TEN_MILLION, dt, sumj; long overrun = 0; test_loops++; for (count = sumj = 0; count < nsamples; count++) { expected_tsc += period_tsc; err = rt_task_wait_period(NULL); if (err) { if (err != -ETIMEDOUT) { rt_queue_delete(&q); rt_task_delete(NULL); /* Timer stopped. */ } overrun++; } dt = (long)(rt_timer_tsc() - expected_tsc); if (dt > maxj) maxj = dt; if (dt < minj) minj = dt; sumj += dt; if (!(finished || warmup) && (do_histogram || do_stats)) add_histogram(histogram_avg, dt); } if(!warmup) { if (!finished && (do_histogram || do_stats)) { add_histogram(histogram_max, maxj); add_histogram(histogram_min, minj); } minjitter = minj; if(minj < gminjitter) gminjitter = minj; maxjitter = maxj; if(maxj > gmaxjitter) gmaxjitter = maxj; avgjitter = sumj / nsamples; gavgjitter += avgjitter; goverrun += overrun; rt_sem_v(&display_sem); struct smpl_t { long minjitter, avgjitter, maxjitter, overrun; } *smpl; smpl = rt_queue_alloc(&q, sizeof(struct smpl_t)); #if 1 smpl->minjitter = rt_timer_tsc2ns(minj); smpl->maxjitter = rt_timer_tsc2ns(maxj); smpl->avgjitter = rt_timer_tsc2ns(sumj / nsamples); smpl->overrun = goverrun; rt_queue_send(&q, smpl, sizeof(struct smpl_t), TM_NONBLOCK); #endif } if(warmup && test_loops == WARMUP_TIME) { test_loops = 0; warmup = 0; } } }
void display (void *cookie) { int err, n = 0; time_t start; err = rt_sem_create(&display_sem,"dispsem",0,S_FIFO); if (err) { fprintf(stderr,"latency: cannot create semaphore: %s\n",strerror(-err)); return; } time(&start); if (quiet) fprintf(stderr, "running quietly for %d seconds\n", test_duration); for (;;) { long minj, gminj, maxj, gmaxj, avgj; err = rt_sem_p(&display_sem,TM_INFINITE); if (err) { if (err != -EIDRM) fprintf(stderr,"latency: failed to pend on semaphore, code %d\n",err); rt_task_delete(NULL); } /* convert jitters to nanoseconds. */ minj = rt_timer_tsc2ns(minjitter); gminj = rt_timer_tsc2ns(gminjitter); avgj = rt_timer_tsc2ns(avgjitter); maxj = rt_timer_tsc2ns(maxjitter); gmaxj = rt_timer_tsc2ns(gmaxjitter); if (!quiet) { if (data_lines && (n++ % data_lines)==0) { time_t now, dt; time(&now); dt = now - start - WARMUP_TIME; printf("RTT| %.2ld:%.2ld:%.2ld\n", dt / 3600,(dt / 60) % 60,dt % 60); printf("RTH|%12s|%12s|%12s|%8s|%12s|%12s\n", "-----lat min","-----lat avg","-----lat max","-overrun", "----lat best","---lat worst"); } printf("RTD|%12ld|%12ld|%12ld|%8ld|%12ld|%12ld\n", minj, avgj, maxj, goverrun, gminj, gmaxj); } } }
static inline void add_histogram(long addval) { /* usec steps */ long inabs = rt_timer_tsc2ns(addval >= 0 ? addval : -addval) / 1000; histogram[inabs < HISTOGRAM_CELLS ? inabs : HISTOGRAM_CELLS - 1]++; }
void worker(void *cookie) { long long minj = 10000000, maxj = -10000000, dt, sumj = 0; unsigned long long count = 0; int err, n; err = rt_sem_create(&switch_sem, "dispsem", 0, S_FIFO); if (err) { fprintf(stderr,"switch: cannot create semaphore: %s\n", strerror(-err)); return; } for (n=0; n<nsamples; n++) { err = rt_sem_p(&switch_sem, TM_INFINITE); if (err) { if (err != -EIDRM) fprintf(stderr,"switch: failed to pend on semaphore, code %d\n", err); rt_task_delete(NULL); } if (++count != switch_count) { count = switch_count; lost++; continue; } // First few switches are slow. // Probably due to the Linux <-> RT context migration at task startup. if (count < ignore) continue; dt = (long) (rt_timer_tsc() - switch_tsc); if (dt > maxj) maxj = dt; if (dt < minj) minj = dt; sumj += dt; if (do_histogram) add_histogram(dt); } rt_sem_delete(&switch_sem); minjitter = minj; maxjitter = maxj; avgjitter = sumj / n; printf("RTH|%12s|%12s|%12s|%12s\n", "lat min", "lat avg", "lat max", "lost"); printf("RTD|%12.3f|%12.3f|%12.3f|%12lld\n", rt_timer_tsc2ns(minjitter) / 1000.0, rt_timer_tsc2ns(avgjitter) / 1000.0, rt_timer_tsc2ns(maxjitter) / 1000.0, lost); if (do_histogram) dump_histogram(); exit(0); }
static void worker(void *cookie) { long long minj = 10000000, maxj = -10000000, dt, sumj = 0; unsigned long long count = 0; int err, n; err = rt_sem_create(&switch_sem, "dispsem", 0, S_FIFO); if (err) { warning("failed to create semaphore (%s)\n", symerror(err)); return; } for (n = 0; n < nsamples; n++) { err = rt_sem_p(&switch_sem, TM_INFINITE); if (err) { if (err != -EIDRM && err != -EINVAL) warning("failed to pend on semaphore (%s)\n", symerror(err)); exit(EXIT_FAILURE); } dt = (long) (rt_timer_tsc() - switch_tsc); if (switch_count - count > 1) { lost += switch_count - count; count = switch_count; continue; } if (++count < warmup) continue; if (dt > maxj) maxj = dt; if (dt < minj) minj = dt; sumj += dt; if (do_histogram) add_histogram(dt); } rt_sem_delete(&switch_sem); minjitter = minj; maxjitter = maxj; avgjitter = sumj / n; printf("RTH|%12s|%12s|%12s|%12s\n", "lat min", "lat avg", "lat max", "lost"); printf("RTD|%12.3f|%12.3f|%12.3f|%12lld\n", rt_timer_tsc2ns(minjitter) / 1000.0, rt_timer_tsc2ns(avgjitter) / 1000.0, rt_timer_tsc2ns(maxjitter) / 1000.0, lost); if (late) printf("LATE: %d\n", late); if (do_histogram) dump_histogram(); exit(0); }
long long RT::OS::getTime(void) { return rt_timer_tsc2ns(rt_timer_tsc()); }
int __po_hi_get_time (__po_hi_time_t* mytime) { #if defined (POSIX) || defined (RTEMS_POSIX) || defined (XENO_POSIX) struct timespec ts; #ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time clock_serv_t cclock; mach_timespec_t mts; host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); clock_get_time(cclock, &mts); mach_port_deallocate(mach_task_self(), cclock); ts.tv_sec = mts.tv_sec; ts.tv_nsec = mts.tv_nsec; #else if (clock_gettime (CLOCK_REALTIME, &ts)!=0) { return (__PO_HI_ERROR_CLOCK); } #endif mytime->sec = ts.tv_sec; mytime->nsec = ts.tv_nsec; return (__PO_HI_SUCCESS); #elif defined (_WIN32) SYSTEMTIME st; FILETIME ft; LARGE_INTEGER ularge; GetSystemTime(&st); SystemTimeToFileTime(&st,&ft); ularge.LowPart=ft.dwLowDateTime; ularge.HighPart=ft.dwHighDateTime; mytime->sec = __po_hi_windows_tick_to_unix_seconds (ularge.QuadPart); mytime->nsec = ularge.QuadPart % 10000000; mytime->nsec *= 100; return (__PO_HI_SUCCESS); #elif defined (RTEMS_PURE) rtems_time_of_day current_time; if (rtems_clock_get (RTEMS_CLOCK_GET_TOD, ¤t_time) != RTEMS_SUCCESSFUL) { __DEBUGMSG ("Error when trying to get the clock on RTEMS\n"); } mytime->sec = _TOD_To_seconds (¤t_time); mytime->nsec = current_time.ticks * rtems_configuration_get_microseconds_per_tick() * 1000; return (__PO_HI_SUCCESS); #elif defined (XENO_NATIVE) mytime->sec = rt_timer_tsc2ns (rt_timer_read ()) / 1000000000; mytime->nsec = rt_timer_tsc2ns (rt_timer_read ()) - (mytime->sec * 1000000000); return (__PO_HI_SUCCESS); #else return (__PO_HI_UNAVAILABLE); #endif }
void latency(void *cookie) { int err, count, nsamples, warmup = 1; RTIME expected_tsc, period_tsc, start_ticks, fault_threshold; RT_TIMER_INFO timer_info; unsigned old_relaxed = 0; err = rt_timer_inquire(&timer_info); if (err) { fprintf(stderr, "latency: rt_timer_inquire, code %d\n", err); return; } fault_threshold = rt_timer_ns2tsc(CONFIG_XENO_DEFAULT_PERIOD); nsamples = ONE_BILLION / period_ns / 1000; period_tsc = rt_timer_ns2tsc(period_ns); /* start time: one millisecond from now. */ start_ticks = timer_info.date + rt_timer_ns2ticks(1000000); expected_tsc = timer_info.tsc + rt_timer_ns2tsc(1000000); err = rt_task_set_periodic(NULL, start_ticks, rt_timer_ns2ticks(period_ns)); if (err) { fprintf(stderr, "latency: failed to set periodic, code %d\n", err); return; } for (;;) { long minj = TEN_MILLION, maxj = -TEN_MILLION, dt; long overrun = 0; long long sumj; test_loops++; for (count = sumj = 0; count < nsamples; count++) { unsigned new_relaxed; unsigned long ov; expected_tsc += period_tsc; err = rt_task_wait_period(&ov); dt = (long)(rt_timer_tsc() - expected_tsc); new_relaxed = sampling_relaxed; if (dt > maxj) { if (new_relaxed != old_relaxed && dt > fault_threshold) max_relaxed += new_relaxed - old_relaxed; maxj = dt; } old_relaxed = new_relaxed; if (dt < minj) minj = dt; sumj += dt; if (err) { if (err != -ETIMEDOUT) { fprintf(stderr, "latency: wait period failed, code %d\n", err); exit(EXIT_FAILURE); /* Timer stopped. */ } overrun += ov; expected_tsc += period_tsc * ov; } if (freeze_max && (dt > gmaxjitter) && !(finished || warmup)) { xntrace_user_freeze(rt_timer_tsc2ns(dt), 0); gmaxjitter = dt; } if (!(finished || warmup) && need_histo()) add_histogram(histogram_avg, dt); } if (!warmup) { if (!finished && need_histo()) { add_histogram(histogram_max, maxj); add_histogram(histogram_min, minj); } minjitter = minj; if (minj < gminjitter) gminjitter = minj; maxjitter = maxj; if (maxj > gmaxjitter) gmaxjitter = maxj; avgjitter = sumj / nsamples; gavgjitter += avgjitter; goverrun += overrun; rt_sem_v(&display_sem); } if (warmup && test_loops == WARMUP_TIME) { test_loops = 0; warmup = 0; } } }
void display(void *cookie) { int err, n = 0; time_t start; char sem_name[16]; if (test_mode == USER_TASK) { snprintf(sem_name, sizeof(sem_name), "dispsem-%d", getpid()); err = rt_sem_create(&display_sem, sem_name, 0, S_FIFO); if (err) { fprintf(stderr, "latency: cannot create semaphore: %s\n", strerror(-err)); return; } } else { struct rttst_tmbench_config config; if (test_mode == KERNEL_TASK) config.mode = RTTST_TMBENCH_TASK; else config.mode = RTTST_TMBENCH_HANDLER; config.period = period_ns; config.priority = priority; config.warmup_loops = WARMUP_TIME; config.histogram_size = need_histo() ? histogram_size : 0; config.histogram_bucketsize = bucketsize; config.freeze_max = freeze_max; err = rt_dev_ioctl(benchdev, RTTST_RTIOC_TMBENCH_START, &config); if (err) { fprintf(stderr, "latency: failed to start in-kernel timer benchmark, code %d\n", err); return; } } time(&start); if (WARMUP_TIME) printf("warming up...\n"); if (quiet) fprintf(stderr, "running quietly for %d seconds\n", test_duration); for (;;) { long minj, gminj, maxj, gmaxj, avgj; if (test_mode == USER_TASK) { err = rt_sem_p(&display_sem, TM_INFINITE); if (err) { if (err != -EIDRM) fprintf(stderr, "latency: failed to pend on semaphore, code %d\n", err); return; } /* convert jitters to nanoseconds. */ minj = rt_timer_tsc2ns(minjitter); gminj = rt_timer_tsc2ns(gminjitter); avgj = rt_timer_tsc2ns(avgjitter); maxj = rt_timer_tsc2ns(maxjitter); gmaxj = rt_timer_tsc2ns(gmaxjitter); } else { struct rttst_interm_bench_res result; err = rt_dev_ioctl(benchdev, RTTST_RTIOC_INTERM_BENCH_RES, &result); if (err) { if (err != -EIDRM) fprintf(stderr, "latency: failed to call RTTST_RTIOC_INTERM_BENCH_RES, code %d\n", err); return; } minj = result.last.min; gminj = result.overall.min; avgj = result.last.avg; maxj = result.last.max; gmaxj = result.overall.max; goverrun = result.overall.overruns; } if (!quiet) { if (data_lines && (n++ % data_lines) == 0) { time_t now, dt; time(&now); dt = now - start - WARMUP_TIME; printf ("RTT| %.2ld:%.2ld:%.2ld (%s, %Ld us period, " "priority %d)\n", dt / 3600, (dt / 60) % 60, dt % 60, test_mode_names[test_mode], period_ns / 1000, priority); printf("RTH|%11s|%11s|%11s|%8s|%6s|%11s|%11s\n", "----lat min", "----lat avg", "----lat max", "-overrun", "---msw", "---lat best", "--lat worst"); } printf("RTD|%11.3f|%11.3f|%11.3f|%8ld|%6u|%11.3f|%11.3f\n", (double)minj / 1000, (double)avgj / 1000, (double)maxj / 1000, goverrun, max_relaxed, (double)gminj / 1000, (double)gmaxj / 1000); } } }
void motor_cmd_routine(void *m_arg) { int ret; RT_TIMER_INFO timer_info; long long task_period; unsigned long overruns = 0; int16_t req_current = 0; int sync_ref_counter=0; float cos_el; float sin_el; float v_req_az; float V_REQ_AZ = 0; float P_term_az, error_az; float p_az = 20.0; float i_az = 1.0; static float az_integral = 0.0; float I_term_az, INTEGRAL_CUTOFF=0.5; printf("Starting Motor Commanding task\n"); rt_timer_inquire(&timer_info); if (timer_info.period == TM_ONESHOT) { // When using an aperiodic timer, task period is specified in ns task_period = rt_timer_ns2ticks(1000000000ll / 100); } else { // When using a periodic timer, task period is specified in number of timer periods task_period = (1000000000ll / 100) / timer_info.period; } ret = rt_task_set_periodic(NULL, TM_NOW, task_period); if (ret) { printf("error while set periodic, code %d\n", ret); return; } // Make sure we are in primary mode before entering the timer loop rt_task_set_mode(0, T_PRIMARY, NULL); while (!stop) { unsigned long ov; // Wait for next time period ret = rt_task_wait_period(&ov); if (ret && ret != -ETIMEDOUT) { printf("error while rt_task_wait_period, code %d (%s)\n", ret, strerror(-ret)); break; } overruns = overruns + ov; ecrt_master_receive(master); ecrt_domain_process(domain); // write application time to master ecrt_master_application_time(master, rt_timer_tsc2ns(rt_timer_tsc())); if (sync_ref_counter) { sync_ref_counter--; } else { sync_ref_counter = 1; // sync every cycle ecrt_master_sync_reference_clock(master); } ecrt_master_sync_slave_clocks(master); /*******************************************************************\ * Card0: Drive the Azimuth Motor (Reaction Wheel) * \*******************************************************************/ /* Read sin and cos of the inner frame elevation, calculated by mcp */ // cos_el = 1.0; //( COS_EL*0.000030517578125 ) - 1.0; // sin_el = 0.0; //( SIN_EL*0.000030517578125 ) - 1.0; // // v_req_az = 0.0; //(float)(V_REQ_AZ-32768.0)*0.0016276041666666666666666666666667; // = vreq/614.4 // // //roll, yaw contributions to az both -'ve (?) // error_az = (gy_ifroll*sin_el + gy_ifyaw*cos_el) + v_req_az; // // P_term_az = p_az*error_az; // // if( (p_az == 0.0) || (i_az == 0.0) ) { // az_integral = 0.0; // } else { // az_integral = (1.0 - INTEGRAL_CUTOFF)*az_integral + INTEGRAL_CUTOFF*error_az; // } // // I_term_az = az_integral * p_az * i_az; // if (I_term_az > 100.0) { // I_term_az = 100.0; // az_integral = az_integral *0.9; // } // if (I_term_az < -100.0) { // I_term_az = -100.0; // az_integral = az_integral * 0.9; // } // if (P_term_az > 1.0 || P_term_az < -1.0) printf("error_az: %f\tI: %f\tP: %f\n", error_az, I_term_az, P_term_az); // req_current = 0.5 *(-(P_term_az + I_term_az) ) ; req_current = 100; if (req_current > 200) printf("Error! Requested current is %d\n", req_current); else { EC_WRITE_S16(rx_controller_state.current_val, req_current); } ecrt_domain_queue(domain); ecrt_master_send(master); } //switch to secondary mode ret = rt_task_set_mode(T_PRIMARY, 0, NULL); if (ret) { printf("error while rt_task_set_mode, code %d\n", ret); return; } }