pfavr@sorgenfri:~/VGTcontroller/src$ ./vgttime | head 0 1046176506.208491 -0.017991 1 1046176506.208631 -0.017131 2 1046176506.208669 -0.016169 19 1046176506.209501 -0.000001 20 1046176506.210499 +0.000001 21 1046176506.211494 +0.000006 22 1046176506.212494 +0.000006 23 1046176506.213494 +0.000006 24 1046176506.214493 +0.000007 25 1046176506.215493 +0.000007
COPTS=-O2 -Wall -DRTAI -I/home/pfavr/rtai-24.1.9/lxrt/ -I/home/pfavr/rtai-24.1.9/include vgttime: vgttime.c $(CC) $(COPTS) -DTEST $< -lpthread -o $@
/** * @file vgttime.h * @author Peter Favrholdt * $Date: 2003/02/05 16:01:52 $ * \b Project: VGT-controller * * @brief Time utility functions (header file). */ #ifndef VGTTIME_H #define VGTTIME_H /** * the time_mode determines the way the get_time() and wait_period() * works. */ typedef enum { HARD, SOFT, SIM } time_mode_t; void set_time_mode(time_mode_t new_mode); double time_get_time(); double time_get_laptime(); double time_wait_period(double period); #endif /* VGTTIME_H */
/** * @file vgttime.c * @author Peter Favrholdt * $Date: 2003/02/23 21:28:28 $ * \b Project: VGT-controller * * @brief Time utility functions. * * This module provides a unified interface to normal Linux and RTAI * time functions, e.g. sleep() and rt_task_wait_period(). * * The behaviour differs greatly depending on if this library is * compiled with RTAI defined or not: * * # with RTAI the functions provides hard realtime. * # without RTAI the functions tries to perform as well as possible, * e.g. time_wait_period() will return approximately at the right time * (but never earlier than expected). To better suit simulation * purposes, time_get_time() will *not* return the real time, but * instead an internally updated time variable. This means that * equidistant samples can be obtained even though the time returned * does not track real time. * * Four modes are available: * * -HARD get_time() returns the current CPU timer * wait_period(P) waits precisely P before returning * -SOFT get_time() returns the current CPU timer * wait_period(P) waits at least P before returning * -SIM get_time() returns simulated time (with no jitter) * wait_period(P) waits at maximum P before returning (trying to * track the real time, updates simulated time) * -WARP get_time() returns simulated time (with no jitter) * wait_period(P) returns immediately (updates simulated time) * (Actually, WARP equals SIM mode when warp_factor is 1.0) * * Instead of ony two functions with four modes, we could have these functions: * * get_real_timer() * get_sim_timer() * wait_equidistant_period() - make periodic * wait_at_least_period() * increase_simulated_time() * * But then we need to call the appropriate functions according to mode. * */ /* TODO: * test for helper thread coming up * wait-period when changing period could use the previous time (no skip) * init could be called from wait-period if necessary? * finish should complement the init function * make it so that more than one task can use it (how is that done? only one helper thread?) * measure-jitter function? * laptime? */ #include <sys/time.h> #include <unistd.h> #include <pthread.h> #include <stdio.h> #include <stdlib.h> #include <sys/mman.h> // mlockall() #include <sched.h> #include <math.h> #include "vgttime.h" #ifdef RTAI #define KEEP_STATIC_INLINE #include <rtai_lxrt_user.h> #include <rtai_lxrt.h> #include <rtai_fifos_lxrt.h> /** * Mailbox to use for stdout */ MBX* mbxstdout; /** * Thread to use for stdout thread */ pthread_t t; int quit=0; /** * Copies stdout mailbox to stdout * * @param pmbx a void pointer (due to pthread_create) which is really * a MBX pointer. * */ void* time_stdout_thread(void*pmbx) { char buf[1024]; RT_TASK *stdtsk; struct sched_param mysched; mysched.sched_priority = 99; if( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 ) { puts(" ERROR IN SETTING THE SCHEDULER UP"); perror( "errno" ); exit( 0 ); } if(!(stdtsk = rt_task_init(nam2num("STDTSK"), 1 /* priority */, 0 /* stack_size */, 0 /* max_msg_size */))) { puts("CANNOT INIT STDOUT TASK\n"); exit(3); } mlockall(MCL_CURRENT | MCL_FUTURE); rt_task_use_fpu(stdtsk,1); // n�dvendig? rt_linux_use_fpu(1); // n�dvendig? while(!quit) { rt_mbx_receive(pmbx,buf,1024); buf[1024]=0; printf("%s",buf); } return 0; } #endif /* RTAI */ /** * Simulated time (internal state) */ double vgttime=0.0; /** * Time offset (internal state) */ double vgttime_offset=0.0; #ifdef RTAI /** * Number of ticks in one period (internal state) */ int ticks_per_second=0; #ifdef TEST unsigned long hrttsk_name; RT_TASK *hrttsk; #else extern unsigned long hrttsk_name; // from vgtserver.c extern RT_TASK *hrttsk; // from vgtserver.c #endif /* TEST */ #endif /* RTAI */ /** * time mode variable (internal state) */ time_mode_t time_mode=SIM; /** * warp factor (only used when in SIM mode) * if set to 0.0, the program will run as fast as CPU ressources allow. * if set to 1.0, the internal time variable will track the current time * if set to 2.0, the internal time variable will count 0.5 seconds every second. * if set to 0.1, the program will run at ten times normal speed. */ double time_warp_factor=1.0; /** * sets the time mode and initializes internal stuff */ void time_init(time_mode_t new_mode, double warp_factor) { struct sched_param mysched; time_mode=new_mode; time_warp_factor=warp_factor; #ifdef RTAI // rtai stuff goes here // allow hard realtime from non-root users rt_allow_nonroot_hrt(); // change to fifo scheduler and set high priority // (required according to RTAI programmers manual p. 132) mysched.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1; if( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 ) { puts("ERROR IN SETTING THE SCHEDULER"); perror("errno"); exit(1); } hrttsk_name = nam2num("HRTTSK"); if(!(hrttsk = rt_task_init(hrttsk_name, 1 /* priority */, 0 /* stack_size */, 0 /* max_msg_size */))) { puts("CANNOT INIT MASTER TASK\n"); exit(3); } // lock all memory in RAM mlockall(MCL_CURRENT | MCL_FUTURE); // use oneshot (pentium) timer rt_set_oneshot_mode(); // calc no of ticks equal to one second ticks_per_second = (int)nano2count((RTIME)(1000000000)); // printf("Ticks per second: %d\n",ticks_per_second); rt_task_use_fpu(hrttsk,1); // necessary? rt_linux_use_fpu(1); // necessary? // make a mailbox for stdout use if((mbxstdout=rt_mbx_init(nam2num("STDOUT"),2048/*65504*/))==0) { puts("Couldn't create stdout mailbox.\n"); exit(4); } pthread_create(&t,NULL,time_stdout_thread,(void*)mbxstdout); rt_make_hard_real_time(); start_rt_timer(0); usleep(200000); // to allow the stdout-thread to initialize itself #endif /* RTAI */ } /** * reads the clock and returns the seconds since EPOCH (01-01-1970 * 00:00:00). Of course the internal machine clock has to be correct. */ double time_get_clock_internal() { struct timeval tv; struct timezone tz; gettimeofday(&tv, &tz); return tv.tv_sec+(1.0*tv.tv_usec)/1000000; } /** * checks the internal time variable and initializes it if necessary. * @return current time (either internal time variable or real clock) */ double time_get_time() { #ifdef RTAI vgttime=1.0*rt_get_time()/ticks_per_second; if(vgttime_offset==0.0) { vgttime_offset=time_get_clock_internal() - vgttime; } #else if(vgttime_offset==0.0) { vgttime_offset=time_get_clock_internal(); } if(time_mode==SOFT) vgttime=time_get_clock_internal()-vgttime_offset; #endif /* RTAI */ // printf("offset: %lf, vgttime:%lf\n",vgttime_offset,vgttime); return vgttime+vgttime_offset; } double vgttimeperiod=-1.0; /** * @brief waits for a specified period and returns the current time. * * If called multiple times with the same argument, the time returned * will be equidistant. If RTAI is supported, the function will be * hard realtime and the time returned will be from the cpu-timer. If * RTAI is not supported, the function may not track the real time */ double time_wait_period(double period) { #ifdef RTAI if(vgttimeperiod!=period) { if(vgttimeperiod!=-1.0) { // need to suspend before make_periodic // rt_task_suspend(hrttsk); } // make task periodic with the given period rt_task_make_periodic(hrttsk, rt_get_time() /*+(RTIME)(period*ticks_per_second+.5)*/, (RTIME)(period*ticks_per_second/*+.5*/)); vgttimeperiod=period; } // wait until next tick rt_task_wait_period(); #else if(vgttimeperiod!=period) { vgttimeperiod=period; } if(time_mode==SIM) { vgttime += period; // increase the internal time variable // compensate for time already spent (cpu and io) // i.e. calculate the waiting period, compensating for warp speed. period = time_warp_factor*vgttime + vgttime_offset - time_get_clock_internal(); } if(period>0.0) usleep(1000000.0*period); // wait if necessary #endif /* RTAI */ return time_get_time(); } #ifdef TEST int main() { int i; double s,t,d,dt,dmax; char buf[1024]; dmax=0.0; dt=.001; time_init(SIM,1.0); t=time_wait_period(dt); for(i=0;i<100000;i++) { s=time_wait_period(dt); d=t+dt*(i+1)-s; if((i>500) && (fabs(d)>dmax)) dmax=fabs(d); snprintf(buf,1024,"%3d\t%f\t%+f\n",i,s,d); rt_mbx_send_if(mbxstdout,buf,1024); } /* t+=dt*100000; dt=.0033; for(i=0;i<100000;i++) { s=time_wait_period(dt); d=t+dt*(i+1)-s; if((i>500) && (fabs(d)>dmax)) dmax=fabs(d); snprintf(buf,1024,"%3d\t%f\t%+f\n",i,s,d); rt_mbx_send_if(mbxstdout,buf,1024); } */ quit=1; usleep(200000); printf("dmax: %f\n",dmax); return 0; } #endif /* TEST */