ogr_main.c

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

//  make sure SIGALRM doesn't kill user prc
  sigact.sa_flags = 0; 
  sigemptyset( &sigact.sa_mask);
  sigaddset( &sigact.sa_mask, SIGALRM);
  sigact.sa_handler = dummy_hndl;
  sigaction( SIGALRM, &sigact, 0);

  time.it_interval.tv_sec  = 0;   
  time.it_interval.tv_usec = interval;   
  time.it_value.tv_sec     = 0;   
  time.it_value.tv_usec    = interval;

  setitimer( ITIMER_REAL, &time, NULL);
  return;
}

/*
 *  code and carrier PLL parameter
 */
void calc_pll_parameter( void)
{
  double c1, c2, zeta, wT, tUpd, gain;  

//  C1 = 1/Gain * 8 * Zeta*wT / (4 + 4*Zeta*wT + wT^2);
//  C2 = 1/Gain * 4 * wT^2    / (4 + 4*Zeta*wT + wT^2);

//  code PLL (pull-in), 20 ms update period
  tUpd = 0.02;

  gain = Ctrl.pul_cod_loopgain;
  zeta = Ctrl.pul_cod_dampratio;
  wT   = 2.0 * Ctrl.pul_cod_bandwidth / 
         (zeta + 1.0 / (4.0 * zeta)) * tUpd;
  c1 = 1.0 / gain * 8.0 * zeta * wT / 
       (4.0 + 4.0 * zeta * wT + wT * wT),
  c2 = 1.0 / gain * 4.0 * wT * wT / 
       (4.0 + 4.0  * zeta * wT + wT * wT);

// code DCO in units of CODFRQRES Hz
// in real-time module we use fix point arithmetic!
  RT_Ctrl->pul_cod_c1 = (long) (c1 / CODFRQRES);
  RT_Ctrl->pul_cod_c3 = (long) ((c1+c2) / CODFRQRES);

#if 0
  printf( "code/pull-in: Ctrl.pul_cod_loopgain = %e\n", Ctrl.pul_cod_loopgain);
  printf( "code/pull-in: zeta = %e\n", zeta);
  printf( "code/pull-in: wT = %e\n", wT);
#endif

#if 1
  printf( "code/pull-in: c1 = %e, c2 = %e\n", c1, c2);
  printf( "code/pull-in (long): c1 = %ld, c3 = %ld\n", 
    RT_Ctrl->pul_cod_c1, RT_Ctrl->pul_cod_c3);
#endif

//  carrier PLL (pull-in), 1 ms update period
  tUpd = 0.001;

  gain = Ctrl.pul_car_loopgain;
  zeta = Ctrl.pul_car_dampratio;
  wT   = 2.0 * Ctrl.pul_car_bandwidth / 
         (zeta + 1.0 / (4.0 * zeta)) * tUpd;
  c1 = 1.0 / gain * 8.0 * zeta * wT / 
       (4.0 + 4.0 * zeta * wT + wT * wT),
  c2 = 1.0 / gain * 4.0 * wT * wT / 
       (4.0 + 4.0  * zeta * wT + wT * wT);

// carrier DCO in units of 42 mHz
  RT_Ctrl->pul_car_c1 = (long) (c1 / CARFRQRES);
  RT_Ctrl->pul_car_c3 = (long) ((c1+c2) / CARFRQRES);

#if 1
  printf( "carrier/pull-in: c1 = %e, c2 = %e\n", c1, c2);
  printf( "carrier/pull-in (long): c1 = %ld, c3 = %ld\n", 
    RT_Ctrl->pul_car_c1, RT_Ctrl->pul_car_c3);

//  getchar();
#endif

//  code PLL (tracking), 20 ms update period
  tUpd = 0.02;

  gain = Ctrl.trk_cod_loopgain;
  zeta = Ctrl.trk_cod_dampratio;
  wT   = 2.0 * Ctrl.trk_cod_bandwidth / 
         (zeta + 1.0 / (4.0 * zeta)) * tUpd;
  c1 = 1.0 / gain * 8.0 * zeta * wT / 
       (4.0 + 4.0 * zeta * wT + wT * wT),
  c2 = 1.0 / gain * 4.0 * wT * wT / 
       (4.0 + 4.0  * zeta * wT + wT * wT);

// code DCO in units of CODFRQRES Hz
// in real-time module we use fix point arithmetic!
  RT_Ctrl->trk_cod_c1 = (long) (c1 / CODFRQRES);
  RT_Ctrl->trk_cod_c3 = (long) ((c1+c2) / CODFRQRES);

#if 1
  printf( "code/tracking: c1 = %e, c2 = %e\n", c1, c2);
  printf( "code/tracking (long): c1 = %ld, c3 = %ld\n", 
    RT_Ctrl->trk_cod_c1, RT_Ctrl->trk_cod_c3);
//  getchar();
#endif

//  carrier PLL (tracking), 1 ms update period
  tUpd = 0.001;

  gain = Ctrl.trk_car_loopgain;
  zeta = Ctrl.trk_car_dampratio;

  wT   = 2.0 * Ctrl.trk_car_bandwidth / 
         (zeta + 1.0 / (4.0 * zeta)) * tUpd;
  c1 = 1.0 / gain * 8.0 * zeta * wT / 
       (4.0 + 4.0 * zeta * wT + wT * wT),
  c2 = 1.0 / gain * 4.0 * wT * wT / 
       (4.0 + 4.0  * zeta * wT + wT * wT);

// carrier DCO in units of 42 mHz
  RT_Ctrl->trk_car_c1 = (long) (c1 / CARFRQRES);
  RT_Ctrl->trk_car_c3 = (long) ((c1+c2) / CARFRQRES);

#if 1
  printf( "carrier/tracking: c1 = %e, c2 = %e\n", c1, c2);
  printf( "carrier/tracking (long): c1 = %ld, c3 = %ld\n", 
    RT_Ctrl->trk_car_c1, RT_Ctrl->trk_car_c3);

  printf( "press RETURN to continue ...\n");
  getchar();
#endif

  return;
}


/*
 *
 *
 *
 */
void process_key_pressed( INT16 key, INT16 *show_help_scr, int *main_loop)
{

  switch ( key)
  {
    case '0':
    case '1':
    case '2':
    case '3':
    case '4':
    case '5':
    case '6':
    case '7':
    case '8':
    case '9':
    case 'a':
    case 'b':
      init_user_monosat( key);
      clrscr();
      break;

    case 'd':
      if ( FpDmpASCII == 0)
      {
        if ( !FpDmp)
        {
          time_t thetime;

          open_dump_file();
          time ( &thetime);
          write_info_dump_file( Rcvr_Info.llh.lat*RAD2DEG, 
            Rcvr_Info.llh.lon*RAD2DEG, thetime);
        }
        else
          close_dump_file();
      }   
      break;
      
    case 'o':
      
      if ( FpDmp == 0)
      {
        if ( !FpDmpASCII)
          open_dump_file_ascii();
        else
          close_dump_file_ascii();
      }    
      break;

    case 'm':
//  transition to meteo mode
//          Ctrl->leadch = -1;
//          reset_monosat();
      break;

    case 'h':
      *show_help_scr = 1;
      helpscreen();
      break;

    case 'n':
      reset_monosat();
      break;

// shift slave C/A PLL w.r.t. master PLL by +/-0.5 chip 
    case '+':
      RT_Ctrl->monosat_slew = 1;
      break;

    case '-':
      RT_Ctrl->monosat_slew = -1;
      break;

    case ESC:
      *show_help_scr = 0;
      clrscr();
      break;

    case 'x':
    case 'q':
      *main_loop = 0;
      break;

    default:
      if ( key)
      {
        *show_help_scr = 1;
        helpscreen();
      }
      break;
  }
  return;
}      

/*
 *
 */
static void ogr_looping( void)
{
  INT16 ch, key, dsp_flag = 0,
        show_help_scr = 0;
  int main_loop = 1;
  double i2;

  if ( Ctrl.status != mode_coldstart) 
    chan_warm_allocate();
  else
    chan_cold_allocate();

// indicate to real/time-module that Chan[] is initialized
  RT_Ctrl->magic = MAGIC_NUMBER;

  setup_timer();

// main loop, runs until key == 'q' or Ctrl->okay == 0
  do
  {

/*
 * 'user_watch_flag' is incremented in RT module; RT module
 * exits if 'user_watch_flag' reaches some threshold value 
 * (i.e. if user program stops working).
 */
    RT_Ctrl->user_watch_flag = 0;

// has RT prg stopped? If yes, we quit as well
    if ( !RT_Ctrl->rt_polling)
      main_loop = 0;

// something new in the FIFO?
    read_fifos();

//  once every second
    if ( RT_Ctrl->sec_flag == 1)
    {
      RT_Ctrl->sec_flag = 0;

//  update global time variable
      The_Time++;

      dsp_flag += 1;

      for ( ch=0; ch<NOFCHN; ch++)
      {

// --- decode subframe if there is one ready ---
        decode_subframe( ch);

// --- estimate carrier to noise density ratio ---
        
#if 0
        i2 = pow( (double) Chan[ch].i_pd_20ms, 2.0);
//        i2 = pow( (double) Chan[ch].i_pd_1s, 2.0);

        if ( Chan[ch].qq_pd_20ms > 0.0 && i2 > 0.0)
        {
//          Chan[ch].CN0 = 10.0 * log10( i2 / Chan[ch].qq_pd_20ms / 0.04);
//          Chan[ch].CN0 = 10.0 * log10( i2 / Chan[ch].qq_pd_1s / 2.0);



//          gotoxy(1,24);
//          printf( "ch=%d, i2=%e, qq=%ld \n", ch, i2, Chan[ch].qq_pd_1s);

        }  
        else  
          Chan[ch].CN0 = 0.0;
#endif

        Chan[ch].CN0 = 10.0 * log10( Chan[ch].avg_20ms / 1395.0 *
          Chan[ch].avg_20ms / 1395.0 * 25.0 * 1.7777 + 1.0);


        if ( Chan[ch].state == track || Chan[ch].state == track_monosat)
        {

//  estimate  C/N0
//          Chan[ch].CN0 = 10.0 * log10( Chan[ch].avg_20ms / 1395.0 *
//            Chan[ch].avg_20ms / 1395.0 * 25.0 * 1.7777 + 1.0);

//  if C/N0 drops below 25 go back to acquisition
          if ( Chan[ch].CN0 < CN0_THRESH)
          {
            Chan[ch].CN0   = 0.0;
            Chan[ch].car_corr = (INT32)
              ((-Xmit_Info[Chan[ch].prn].doppler -
              Ctrl.clock_offset * L1FRQMHZ) / CARFRQRES);

//  calculate code doppler correction
            RT_Ctrl->code_corr    = (INT32)
              (Ctrl.clock_offset * 24.0 +
              Xmit_Info[Chan[ch].prn].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;   // set carrier

            Chan[ch].state        = initialize;

            Msg[ch].subframe_full = 0;

//  if in monosat mode reset to multisat mode
            if ( RT_Ctrl->leadch >= 0)
              reset_monosat();

          }
        }  // --- if ( Chan[ch].state == track) ---
      }  // --- for ( ch=0; ch<NOFCHN; ch++) ---
    }  // --- if ( sec_flag == 1) ---

//  once per nav update period (1 second)
    if ( NavData.perform_navfix)
    {
      NavData.perform_navfix = 0;
      nav_fix();
    }

// once every minute
    if ( RT_Ctrl->min_flag == 1)
    {
      RT_Ctrl->min_flag = 0;

// set system clock to receiver time
      stime( &The_Time);

      if ( RT_Ctrl->leadch < 0)
      {
        if ( Ctrl.status == mode_coldstart) 
          chan_cold_allocate();
        else
          chan_warm_allocate();
      }
    }

// display info
    if ( dsp_flag)
    {
      dsp_flag = 0;

      if ( !show_help_scr)
        display();

      if ( kbhit())
        key = readch();
      else
        key = '\0';

      process_key_pressed( key, &show_help_scr, &main_loop);

    }  // --- if ( dsp_flag) ---
    
/*  there should be enough time for a brief nap ... */
    pause();
        
  } while ( main_loop);  /* Stay in loop until 'q' or 'x' key is pressed. */

  return;
}

#if 0
/*
 *
 */
void check_options( void)
{
  if ( !RT_Ctrl->base_address)
  {
    fprintf( stderr, "base address not set, use option -b\n ");
    exit(-1);
  }
  
  return;
}
#endif

/* ------------------------------------------------------------------------------
 *   getOptions
 *  
 * We use this function whenever we get input from argc and argv -  it will
 * parse the input options correctly.
 *  
 */
static void get_options( int argc, char **argv) 
{
  int opt;
//  char *tmp;

  while ((opt = getopt( argc, argv, "b:h")) != -1) 
  {
    switch ( opt) 
    {
#if 0
    case 'b' : 
//      sscanf( optarg, "%x", &Base_Address);
//      Base_Address = atoi( optarg);
      if ( optarg)
      {
        if ( tolower( optarg[1]) == 'x')
          RT_Ctrl->base_address = strtoul( optarg+2, &tmp, 16);
        else  
          RT_Ctrl->base_address = strtoul( optarg, &tmp, 16);
        printf( "ogr: base adress is 0x%x\n", RT_Ctrl->base_address);
      }  
      break;
#endif
    case 'h' : 
    case '?' : 
      helpscreen();
      exit(0);
      break;
    default :
      break;
    }
  }

  return;
}


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

PARAMETERS None.

PURPOSE
  This is the main program to control the GPS receiver

WRITTEN BY
  Clifford Kelley

CHANGES
  02.04.03 - dont update current.loc for the moment

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

int main( int argc, char **argv)
{
  if ( argc <= 2) 
  {
    helpscreen();
    return (0);
  }

/* 
 * initialize shared memory before processing options, since in 
 * get_options() we write to structure RT_Ctrl
 */
 
  init_user();

  get_options( argc, argv);

//  check_options();


//  check_module_loaded();

// read receiver tracking parameters from file 'rcvr_par.dat'
  read_rcvr_par( RT_Ctrl, &Ctrl);

  calc_pll_parameter();

  read_initial_data();

// read receiver location from file ...
  Rcvr_Info.llh = read_location();

// ... and convert to cart co-ordinates
  Rcvr_Info.pos = llh_to_ecef( Rcvr_Info.llh);

  RT_Ctrl->nav_tic   = (UINT16)( Ctrl.nav_upd * 10);   // in multiples of 100 msec (TIC)
  RT_Ctrl->code_corr = (INT32)( Ctrl.clock_offset * 24.0);

  init_rcvr_clock();

  clrscr();

// open FIFOs
  open_fifos();

// ... now receive and process until someone hits 'quit'
  ogr_looping();

//  update the Almanac Data file
  write_almanac();

//  update the Ephemeris Data file
  write_ephemeris();

//  update the ionospheric model and UTC parameters
  write_ion_utc();

//  Update the curloc file for the next run
  write_location();

  cleanup();

  return (0);
}

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