ogr_main.c

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

#if 0
//  printf("   latitude    longitude          HAE      clock error (ppm)\n");
  printf( "lat: %4d:%02d:%05.2f lon: %4d:%02d:%05.2f HAE: %6.2f m clk err: %f ppm\n",
    cur_lat.deg, abs( cur_lat.min), fabs( cur_lat.sec),
    cur_long.deg, abs( cur_long.min), fabs( cur_long.sec),
    rec_pos_llh.hae, clock_offset);
#endif

  llh = Rcvr_Info.llh;

  printf( "lat: %9.5f lon: %10.5f HAE: %6.2f m clk err: %f ppm\n",
    llh.lat*RAD2DEG, llh.lon*RAD2DEG,
    llh.hae, Ctrl.clock_offset);

  printf( "speed: %.2f m/s   heading: %.2f deg        dump file: %s\n",
    Rcvr_Info.speed, Rcvr_Info.heading * RAD2DEG, (FpDmp ? DmpFileName : "-/-                  "));

//  dilution of precision
  printf( "GDOP: %4.1f  HDOP: %4.1f  VDOP: %4.1f\n",
    Dops.gdop, Dops.hdop, Dops.vdop);

//  GP2021 register access/miss flags
  printf( "accum_new: %03x  accum_missed: %04x  leading chn: %d\n", 
    RT_Ctrl->accum_new, RT_Ctrl->accum_missed, RT_Ctrl->leadch);

  printf( "tracking: %2d    almanach valid: %2d    gps week: %4d\n",
    Ctrl.nof_trk, bitsum32( Ctrl.almanac_read), Ctrl.gps_week % 1024);

//   summary over all 12 channels
  printf( " ch prn st frq  az  el doppler   cnt frm sfd ura pg sync miss CN0 parity\n");

//  'monosat' mode
  leadch = RT_Ctrl->leadch;
  n = Chan[leadch].prn; 
  printf( " %2d %2d %2d %3d %4.0f %3.0f %8ld %4d %3d %3d %3d %3d %5d %4.1f %d/%d\n",
        leadch, Chan[leadch].prn, Chan[leadch].state,
        Chan[leadch].n_freq,
        Xmit_Info[n].azimuth * RAD2DEG,
        Xmit_Info[n].elevation * RAD2DEG, 
        Chan[leadch].car_frq ? Chan[leadch].car_frq - CARRIER_REF : 0,
        Chan[leadch].bit_count,
        Chan[leadch].frame_count,                         
        Chan[leadch].sfid,
        GPS_Eph[n].ura,
        Chan[leadch].page5,
        Chan[leadch].missed,
        Chan[leadch].CN0,
        Parity_Passed[leadch],
        Parity_Failed[leadch]);

  printf( "\n");

  printf( " ch prn st frq carph codph chpph epoch ms    I     Q\n");
  for ( ch=0; ch<NOFCHN; ch++)
  {
    printf(" %2d %2d %2d %3d %5d %5d %5d %2d/%2d %4d %6d %6d\n",
        ch, Chan[ch].prn,
        Chan[ch].state,
        Chan[ch].n_freq,
        Chan[ch].carr_dco_phase - Chan[leadch].carr_dco_phase,
        Chan[ch].code_phase - Chan[leadch].code_phase,
        Chan[ch].code_dco_phase - Chan[leadch].code_dco_phase,
        Chan[ch].epoch >> 8,
        Chan[ch].epoch & 0xff,
        Chan[ch].missed,
        Chan[ch].i_prompt,
        Chan[ch].q_prompt);
  }

  return;
}

/*
 *  display info on screen
 */
void display( void)
{
  
  switch ( Ctrl.status)
  {
  case mode_riseset:
    display_riseset();
    break;
  case mode_monosat:
    display_monosat();
    break;
  default:
    display_navigate();
    break;
  }
}

/*
 *  sum over all bits in long
 */
static INT16 bitsum32( unsigned long x)
{
  int i, res = 0;

  for (i=0; i<32; i++)
    res += (x>>i) & 0x1;

  return res;
}


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

PARAMETERS None.

PURPOSE  Allocate the PRNs to channels for a cold start
         (no almanac information)

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
static void chan_cold_allocate( void)
{
  XMITINFO dummy;
  INT16 ch, i, alloc;

  RT_Ctrl->search_max_f = 50;
  dummy = satfind( 0);

  Ctrl.almanac_valid = 1;

//  reset_cntl( 0x1fff);

  for ( i=1; i<=32; i++)
  {
    if ( GPS_Alm[i].inc > 0.0 && 
         GPS_Alm[i].week != Ctrl.gps_week)
      Ctrl.almanac_valid = 0;
  }

  if ( Iono.al0 == 0.0 && Iono.b0 == 0.0)
    Ctrl.almanac_valid = 0;

// if no satellite is being tracked turn the channel off
  for ( ch=0; ch<NOFCHN; ch++)
  {
    if ( Chan[ch].CN0 < CN0_THRESH && RT_Ctrl->leadch < 0)
    {
      Chan[ch].state = off;
      Chan[ch].prn   = 0;
      Chan[ch].CN0   = 0.0;

      if ( RT_Ctrl->leadch >= 0)
        reset_monosat();

    }
  }

  for (i=0; i<NOFCHN; i++)
  {
    alloc = 0;
    for ( ch=0; ch<NOFCHN; ch++)
    {
      if ( Chan[ch].prn == Ctrl.cold_prn)
      {
//  satellite is already allocated a channel
        alloc = 1;
        break;
      }
    }

    if ( alloc == 0) // if not allocated find an empty channel
    {
      for ( ch=0; ch<NOFCHN; ch++)
      {
        if ( Chan[ch].state == off)
        {
          Chan[ch].car_corr = (INT32)
            (-Ctrl.clock_offset * L1FRQMHZ / CARFRQRES);
          Chan[ch].car_frq      = CARRIER_REF + Chan[ch].car_corr;
          Chan[ch].cod_frq      = CODE_REF;

          Chan[ch].prn          = Ctrl.cold_prn;

          Ctrl.cold_prn         = (Ctrl.cold_prn % 32) + 1;

          Chan[ch].state        = initialize;

          Msg[ch].subframe_full = 0;

          break;
        }
      }
    }
  }
  return;
}

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

PARAMETERS None.

PURPOSE  Allocate the PRNs to channels for a warm start
         (almanac information available)

WRITTEN BY
  Clifford Kelley

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

static void chan_warm_allocate( void)
{
  INT16 ch, prnn, alloc;

  RT_Ctrl->search_max_f  = 30;    // search from -30*200,-29*200,...,29*200,30*200Hz
  Ctrl.almanac_valid = 1;

  for ( prnn=1; prnn<=32; prnn++)
  {
    Xmit_Info[prnn] = satfind( prnn);
    if ( GPS_Alm[prnn].inc > 0.0 &&
         GPS_Alm[prnn].week != Ctrl.gps_week % 1024)
      Ctrl.almanac_valid = 0;
  }

  if ( Iono.al0 == 0.0 && Iono.b0 == 0.0)
    Ctrl.almanac_valid = 0;

  for (ch=0; ch<NOFCHN; ch++) // if the sat has dropped below mask angle
                              // and C/N0 is low turn the channel off
  {
    if (( Chan[ch].CN0 < CN0_THRESH && 
          Xmit_Info[Chan[ch].prn].elevation < Ctrl.mask_angle) ||
          GPS_Alm[Chan[ch].prn].ety == 0.0)
    {
      Chan[ch].state = off;
      Chan[ch].prn   = 0;
      Chan[ch].CN0   = 0.0;
      
      if ( RT_Ctrl->leadch >= 0)
        reset_monosat();

    }
  }

  for ( prnn=1; prnn<=32; prnn++)
  {
    if ( Xmit_Info[prnn].elevation > Ctrl.mask_angle && 
         GPS_Alm[prnn].health == 0 &&
         GPS_Alm[prnn].ety != 0.0)
    {
      alloc = 0;
      for (ch=0; ch<NOFCHN; ch++)
      {
        if ( Chan[ch].prn == prnn)
        {
          alloc = 1;     // satellite already allocated a channel
          break;
        }
      }

      if ( alloc == 0) // if not allocated find an empty channel
      {
        for ( ch=0; ch<NOFCHN; ch++)
        {
          if ( Chan[ch].state == off)
          {
            Chan[ch].car_corr = (INT32)
             ((-Xmit_Info[prnn].doppler -
              Ctrl.clock_offset * L1FRQMHZ) / CARFRQRES);

// calculate code correction
            RT_Ctrl->code_corr     = (INT32)
              (Ctrl.clock_offset * 24.0 +
              Xmit_Info[prnn].doppler / 134.0);

            Chan[ch].cod_frq      = CODE_REF + RT_Ctrl->code_corr;
            Chan[ch].car_frq      = CARRIER_REF + 
              Chan[ch].car_corr + RT_Ctrl->frq_step * Chan[ch].n_freq;

            Chan[ch].prn          = prnn;

            Chan[ch].state        = initialize;

            Msg[ch].subframe_full = 0;

            break;
          }  // --- if ( Chan[ch].state == off) ---
        }  // --- for ( ch=0; ch<NOFCHN; ch++) ---
      }
    }
  }  // --- for ( prnn=1; prnn<=32; prnn++) ---
  return;
}

/*
 *  switch back from trailing mode to normal navigation
 */
static void init_user_monosat( char key)
{
  char tmpfile[64];

  if ( key <= '9')
    RT_Ctrl->leadch = (INT16) (key - '0');
  else
    RT_Ctrl->leadch = (INT16) (key - 'a' + 10);

  RT_Ctrl->leadch = min( max( 0, RT_Ctrl->leadch), 11);

// transition to trailing mode at next TIC
//  RT_Ctrl->transition2monosat = 1;
  RT_Ctrl->trans = trans2monosat;

//  mode of operation is now mono-satellite mode
  Ctrl.status = mode_monosat;

  if ( FpOut1)
  {
    fclose( FpOut1);
    FpOut1 = NULL;
  }  

// 64 bytes max!
  sprintf( tmpfile, "../output/cor-%03d-%06ld.dat", 
    Ctrl.gps_week, RT_Ctrl->tow_tic_count/10);

  if (( FpOut1 = fopen( tmpfile, "wb")) == NULL)
  {
    printf( "ogr: error opening file %s.\n", tmpfile);
    user_exit( 0);
  }

  write_cor_file_header( FpOut1);

  return;
}

// -------------------------------------------------------------------------
//
//  switch back from trailing mode to normal navigation
//
static void reset_monosat( void)
{
  INT16 ch;

  Ctrl.status = mode_tracking;

  if ( FpOut1)
  {
    fclose( FpOut1);
    FpOut1 = NULL; 
  }

  for ( ch=0; ch<NOFCHN; ch++)
  {
    Chan[ch].state = off;
    Chan[ch].prn   = 0;
  }

  clrscr();

  RT_Ctrl->leadch = -1;

//  allocate channels again ...
//  chan_cold_allocate();
  chan_warm_allocate();

  return;
}

/*
 *  clean up before quit
 */
static void cleanup( void)
{

//  make rt-process stop
  RT_Ctrl->rt_polling = 0; 

//  free shared memore objects
  if ( Chan)
    rtai_free( nam2num( SHM_CHANNEL), (void*) Chan);

  if ( RT_Ctrl)
    rtai_free( nam2num( SHM_RT_CTRL), (void*) RT_Ctrl);

  if ( FpOut1)
  {
    fclose( FpOut1);
    FpOut1 = NULL;
  }  

  if ( FpDmp)
  {
    fclose( FpDmp);
    FpDmp = NULL;
  }  

  if ( FpCor)
  {
    fclose( FpCor);
    FpCor = NULL;
  }  

  close_keyboard();

  return;
}

//
//  exit from user space process
//
void user_exit( int code)
{
  cleanup();
  exit( code);
}

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

PARAMETERS None.

PURPOSE
  user land initialization

WRITTEN BY
  G. Beyerle

*******************************************************************************/
static void init_user( void)
{
  INT16 ch, i;

  static UINT16 monosat_cod_ofs[] =
  { 
//   11 channels, dither = early:
//   o  o  o  o  o                 O  o  o  o  o  o
//                  x  x  x  x  x                 X  x  x  x  x  x
    0, 205, 410, 615, 820, 1025, 2253, 2458, 2663, 2868, 3073, 3277
  };

  init_keyboard();

  putenv( tzstr);
  tzset();

//  init shared memory
  Chan = (CHANNEL*) rtai_malloc( nam2num( SHM_CHANNEL), 
         sizeof( CHANNEL)*NOFCHN);
  if ( !Chan)
  { 
    printf( "ogr: rtai_malloc() failed in main().\n");
    user_exit(-1);
  }

  printf( "ogr: shared memory 'Chan[]' allocated.\n");

  RT_Ctrl = (RT_CTRL*) rtai_malloc( nam2num( SHM_RT_CTRL), 
          sizeof( RT_CTRL));
  if ( !RT_Ctrl)
  { 
    printf( "ogr: rtai_malloc() failed in main().\n");
    user_exit(-1);
  }

  printf( "ogr: shared memory 'RT_Ctrl' allocated.\n");

  if ( RT_Ctrl->mod_loaded != 0x1234)
  {
    printf( "ogr: module ogrt_module.o is not loaded!\n");
    exit(-1);
  }

  for ( ch=0; ch<NOFCHN; ch++)
  {
    Chan[ch].state        = off;
//    Msg[ch].preamidx      = -1;
    Msg[ch].subframe_full = 0;
  }

  RT_Ctrl->leadch    = -1;
  RT_Ctrl->earlylate = -1;     // -1 : early, +1 : late

  for ( i=0; i<NOFCHN; i++)
    RT_Ctrl->monosat_cod_ofs[i] = monosat_cod_ofs[i];

  Ctrl.gps_week      = -1;     // will be set from decoded nav message
  Ctrl.cold_prn      = 1;
  Ctrl.nof_trk       = 0;
  Ctrl.nav_upd       = 1.0,
  Ctrl.clock_offset  = -0.6;

  return;
}



/*
 *  initialize rcvr clock
 */
static void  init_rcvr_clock( void)
{
  double    time_s;    // time in days
  struct tm *gmt;

// set up global variable 'thetime' so it can be taken over by this program
  time( &The_Time);  

  gmt = gmtime( &The_Time);

  gmt2julian( &time_s, &Ctrl.gps_week, gmt);

//  almanac_date = GPS_Alm[i].week * 7.0 + 2444244.5;
//// *** FIXME ***
//  if ( gps_week - GPS_Alm[i].week > 512)
//    almanac_date += 1024 * 7.0;
//  alm_time  = (time_s - almanac_date) * 86400.0;

//  time-of-week in tenth of seconds
  RT_Ctrl->tow_tic_count = (unsigned long) 
    ( (time_s - Ctrl.gps_week * 7.0 - 2444244.5) * 86400.0 * 10.0);

  return;
}

/*
 * dummy signal handler
 */
void dummy_hndl( int sig)
{
  return;
}

/*
 *  setup alarm clock: user prc is woken up twice per second
 */
void setup_timer( void)
{
//  check every 0.5 sec
  long interval = 500000;
  struct sigaction sigact;
  struct itimerval time;