LXRT Examples

Below is a LXRT example

Output

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

Makefile

From the mailinglist:

you don't need to include rtai_lxrt.h. Just include rtai_lxrt_user.h and link with liblxrt.a

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 $@

Sourcecode: vgttime.h

/**
 * @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 */

Sourcecode: vgttime.c

/**
 * @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 */