ogr_inout.c

The OpenGPSRec receiver software runs on the real time operating system RTAI-Linux. It compiles with

  fprintf( out, "dtls:                       %f\n", Utc.dtls);
  fprintf( out, "tot:                        %f\n", Utc.tot);
  fprintf( out, "WNt:                        %f\n", Utc.WNt);
  fprintf( out, "WNlsf:                      %f\n", Utc.WNlsf);
  fprintf( out, "DN:                         %f\n", Utc.DN);
  fprintf( out, "dtlsf:                      %f\n", Utc.dtlsf);

  fclose( out);
}

/*******************************************************************************
FUNCTION read_location()
RETURNS  None.

PARAMETERS None.

PURPOSE

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

LLH read_location( void)
{
  double latitude, 
         longitude, 
         height;
  char   text[256], 
         tmpstr[] = LOC_FILE;
  LLH    res;
  FILE   *in;

  res.lat = 0.0;
  res.lon = 0.0;
  res.hae = 0.0;

//
//   READ THE CURRENT LOCATION DATA FILE
//
  if ((in = fopen( tmpstr, "rt")) == NULL)
  {
    printf( "ogr: error opening file %s. Revert to cold start.\n", tmpstr);
    Ctrl.status = mode_coldstart;
  }
  else
  {
    fscanf( in, "%10s", text);
    fscanf( in, "%le", &latitude);
    fscanf( in, "%10s", text);
    fscanf( in, "%le", &longitude);
    fscanf( in, "%10s", text);
    fscanf( in, "%le", &height);
    fclose( in);

    res.lat = latitude * DEG2RAD;
    res.lon = longitude * DEG2RAD;
    res.hae = height;
  }

  return (res);
}

/*
 *  write current location
 */
void write_location( void)
{
  FILE *out = NULL;

  if ( Ctrl.status == mode_navigate &&
       fabs( Rcvr_Info.llh.lat) <= M_PI/2 &&
       fabs( Rcvr_Info.llh.lon) <= M_PI)
  {
    char fname[] = LOC_FILE;

    if ( (out = fopen( fname, "w")) == NULL)
    {
      printf( "ogr: error opening file '%s'.\n", 
        fname);
      user_exit(-1);
    }

    fprintf( out, "latitude  %f\n", Rcvr_Info.llh.lat * RAD2DEG);
    fprintf( out, "longitude %f\n", Rcvr_Info.llh.lon * RAD2DEG);
    fprintf( out, "hae       %f\n", Rcvr_Info.llh.hae);

    fclose( out);
  }
  return;
}

/*******************************************************************************
FUNCTION read_initial_data()
RETURNS  None.

PARAMETERS None.

PURPOSE

WRITTEN BY
  Clifford Kelley

*******************************************************************************/

void read_initial_data(void)
{
  INT16    id;
  XMITINFO dummy;

  for (id=1; id<=32; id++)
    GPS_Alm[id].inc = 0.0;

/*
 *     READ THE INPUT DATA FILE(s)
 */
  Ctrl.status = mode_coldstart;       // for initialization, if we have data we will switch to warm or hot start

  read_ion_utc();
  read_almanac();

  dummy = satfind( 0);

  read_ephemeris();

  The_Time = time( NULL);

  return;
}

/*
 *  help screen
 */
void helpscreen( void)
{
  clrscr();
  printf( "OpenGPSRec  (c) 2001-2003  C. Kelley, G. Beyerle\n");
  printf( "\n");
  printf( "q or x      : exit program\n");
  printf( "m           : track rising/setting sats\n");
  printf( "n           : navigation mode\n");
  printf( "0,...,9,a,b : monosat mode: all channels track one sat\n");
  printf( "d           : dump nav and pseudorange data to file\n");
  printf( "o           : dump elevation & azimuth data (ASCII)\n");
  printf( "\n");
  printf( "h           : show help screen\n");
  printf( "ESC         : quit help screen\n");

  return;
}

/*
 *  clear screen
 */
void clrscr( void)
{
  printf( "\x1B[2J\x1B[0d\x1B[0G");
  return;
}

/* 
 *  move cursor to col x and row y
 */
void gotoxy( int x, int y)
{
  printf( "\x1B[%dd \x1B[%dG", y, x);
  return;
}

/*
 *  write header info to correlation data dump file
 */
void write_cor_file_header( FILE *Fp)
{
  INT16 tmp, nof = 0;

//  data file format (increment if you change something!)
  tmp = 6;                              // file format version
  nof += fwrite( &tmp, sizeof( INT16), 1, Fp);   // 2 bytes

//  header size in bytes
  tmp = MSG_HEADERSIZE;
  nof += fwrite( &tmp, sizeof( INT16), 1, Fp);   // 4 bytes

//  data body size in bytes
  tmp = MSG_DATASIZE;
  nof += fwrite( &tmp, sizeof( INT16), 1, Fp);   // 6 bytes

//  GPS week
  nof += fwrite( &Ctrl.gps_week, sizeof( INT16), 1, Fp); // 8 bytes

//  which channel leads the gang?
  nof += fwrite( &RT_Ctrl->leadch, sizeof( INT16), 1, Fp);  // 70 bytes

//  time of week
  nof += fwrite( &Chan[RT_Ctrl->leadch].tow_HOW, sizeof( long), 1, Fp); // 12 bytes 

//  TIC
  nof += fwrite( &RT_Ctrl->tow_tic_count, sizeof( long), 1, Fp);  // 16 bytes

// +1 late arm, -1 : early arm
  nof += fwrite( &RT_Ctrl->earlylate, sizeof( INT16), 1, Fp);  // 72 bytes

//  PRN of leading channel
  nof += fwrite( &Chan[RT_Ctrl->leadch].prn, sizeof( INT16), 1, Fp); // 18 bytes

//  code phase offset (monosat mode)
  nof += fwrite( RT_Ctrl->monosat_cod_ofs, sizeof( INT16), NOFCHN, Fp); // 44 bytes

  return;
}

/*
 * read from FIFOS and write to file (optional)
 */
void read_fifos( void)
{
  unsigned char len, msg_id;
  int nof;

/*
 *  check if data from FIFOs have arrived?
 */
  do
  {

    nof = read( RFd0, &msg_id, 1);
      
//  if FIFO empty : nof = -1
    if ( nof <= 0)
      break;

//  second byte contains length of message
    nof = read( RFd0, &len, 1);
      
    if ( nof <= 0)
      ogr_error( "ogr: read incomplete message (id = %d)\n", msg_id);

    switch ( msg_id)
    {
    case MSG_NAVBIT:
      read_msg_navbit( RFd0, (int)len);
      break;

    case MSG_PSIRANGE:
      read_msg_psirange( RFd0, (int)len);
      break;

    default:
      ogr_error( "ogr: unkown message id %d\n", msg_id);
      break;
    }
  } while( 1);

/*
 *  copy correlation data to file 
 *
 *    INT16 : ch
 *    long  : Chan[ch].TOW            time of week [sec]
 *    INT16 : Chan[ch].t_count        nav bit counter [0,...,1499]
 *    long  : Chan[ch].i_prompt_20ms
 *    long  : Chan[ch].i_dith_20ms
 *    long  : Chan[ch].q_prompt_20ms
 *    long  : Chan[ch].q_dith_20ms
 *    INI16 : Chan[ch].carr_cycle_l
 *    INI16 : Chan[ch].carr_cycle_h
 *    INI16 : Chan[ch].carr_dco_phase
 *    INI16 : Chan[ch].code_phase
 *    INI16 : Chan[ch].code_dco_phase
 *    ------
 *    12 * 34 Bytes / 20 msec = 19.9 kB/sec = 70.04 MB/h
 */

  if ( FpOut1)
  {
    UINT16 Tmp16;
    do
    {
      nof = read( RFd1, &Tmp16, sizeof( INT16));
      if ( nof > 0)
        fwrite( &Tmp16, sizeof( INT16), 1, FpOut1);
    } while( nof > 0);
    fflush( FpOut1);   // flush buffer
  }

  return;
}

/*
 *  open FIFOs for nav bit and IQ data transfer
 */
void open_fifos( void)
{
  char tmpstr[64];

//  NAV bits
  sprintf( tmpstr, "/dev/rtf%d", FIFO_NAVBIT);

  if (( RFd0 = open( tmpstr, O_RDONLY|O_NONBLOCK)) < 0) 
    ogr_error( "ogr: error opening FIFO_NAVBIT.\n");

//  I/Q in 'trailing' mode
  sprintf( tmpstr, "/dev/rtf%d", FIFO_IQDUMP);

  if (( RFd1 = open( tmpstr, O_RDONLY|O_NONBLOCK)) < 0) 
    ogr_error( "ogr: error opening FIFO_IQDUMP.\n");

  return;
}

/*
 *
 */
void read_msg_navbit( int fd, int len)
{
  char buf[MSG_MAX_LEN];
  int i, nof;
  INT16 ch, bit;
  
  nof = read( fd, buf, len);

  if ( nof != len)
    ogr_error( "incomplete message found (read %d, expected %d bytes)\n",
      nof, len);

  ch  = (INT16) buf[0];
  bit = (INT16) buf[1];

//  bit = [0|1] : nav bit; bit = [2|3] : start of TLM
  if ( bit & 0x2)
  {
    Msg[ch].idx = 0;
//  bits 29 and 30 of word 10 (and word 2) are always 0!
//  initialize to zero
    for ( i=1; i<=10; i++)
      Msg[ch].subframe_buffer[i] = 0; 
  }

// wait until start of TLM
  if ( Msg[ch].idx >= 0)
  {
    int word, shift;

    word  = Msg[ch].idx / 30 + 1; // index into subframe_parity: 1,...,10
    shift = (299 - Msg[ch].idx) % 30;

#if 0
    if ( 1)
    {
      gotoxy( 1, 22+ch);
      printf( "(%d)", ch);
      printf( " word = %d / shift = %d / bit = %d", word, shift, bit);
      printf( "\n");
    }
#endif
    
    Msg[ch].subframe_buffer[word] |= ((bit & 0x1) << shift); 

// copy last two parity bits to bit 31 & 30 of next word
    if ( shift < 2 && word < 10)
      Msg[ch].subframe_buffer[word+1] |= ((bit & 0x1) << (shift + 30)); 

    Msg[ch].idx = (Msg[ch].idx+1) % 300;
    
    if ( Msg[ch].idx == 0)
    {
      Msg[ch].subframe_full = 1;

//  save frame for later processing  
//  (*** FIXME: if buffer is not processed within 6 seconds we loose data ***)
      for ( i=1; i<=10; i++)
      {
        Msg[ch].subframe_parity[i] = Msg[ch].subframe_buffer[i];
      }
#if 0
    if ( 1)
    {
      int j;
      gotoxy( 1, 22+ch);
      printf( "(%d)", ch);
      for(j=1;j<=10;j++)
        printf( " %08lx", Msg[ch].subframe_parity[j]);
      printf( "\n");
    }
#endif

    }
  }  

  return;
}

/*
 *
 */
void read_msg_psirange( int fd, int len)
{
  int nof, ch;
  char buf[MSG_MAX_LEN];

  nof = read( fd, buf, len);

  if ( nof != len)
    ogr_error( "incomplete FIFO message (read %d, expected %d bytes)\n",
      nof, len);

  memcpy( &ch, buf, sizeof( INT16));

  memcpy( NavData.epoch + ch, buf+2,  sizeof( INT16));
  memcpy( NavData.code_phase + ch, buf+4, sizeof( INT16));
  memcpy( NavData.code_dco_phase + ch, buf+6, sizeof( INT16));
  memcpy( NavData.carr_dco_phase + ch, buf+8, sizeof( INT16));
  memcpy( NavData.carr_cycle_lo + ch, buf+10, sizeof( INT16));
  memcpy( NavData.carr_cycle_hi + ch, buf+12, sizeof( INT16));
  memcpy( NavData.tow_tic_count + ch, buf+14, sizeof( long));
  memcpy( NavData.tow_1s + ch, buf+18, sizeof( long));

//  did we read all 12 channels? if yes then try and find navigation solution
  if ( ch == 11)
    NavData.perform_navfix = 1;

  return;
}

/* ---------------------------------------------------------------------- */

/*
 * from kbhit.c, WROX book "Beginning LINUX Programming" (www.wrox.com)
 */
#include <termios.h>
#include <term.h>
#include <curses.h>
#include <unistd.h>

static struct termios   initial_settings, 
                        new_settings;
static int              peek_character = -1;

void init_keyboard( void)
{
  tcgetattr( 0, &initial_settings);
  new_settings = initial_settings;
  new_settings.c_lflag &= ~ICANON;
  new_settings.c_lflag &= ~ECHO;
  new_settings.c_lflag &= ~ISIG;
  new_settings.c_cc[VMIN] = 1;
  new_settings.c_cc[VTIME] = 0;
  tcsetattr( 0, TCSANOW, &new_settings);
  return;
}

void close_keyboard( void)
{
  tcsetattr(0, TCSANOW, &initial_settings);
  return;
}

int kbhit( void)
{
  char ch;
  int nread;

  if ( peek_character != -1)
    return 1;
  new_settings.c_cc[VMIN]=0;
  tcsetattr( 0, TCSANOW, &new_settings);
  nread = read(0,&ch,1);
  new_settings.c_cc[VMIN]=1;
  tcsetattr( 0, TCSANOW, &new_settings);

  if ( nread == 1) 
  {
    peek_character = ch;
    return (1);
  }
  return (0);
}

int readch( void)
{
  char ch;

  if ( peek_character != -1) 
  {
    ch = peek_character;
    peek_character = -1;
    return ch;
  }
  read( 0, &ch, 1);
  return ch;
}

/* ------------------------------- end of file ---------------------------- */