ogrt_monosat.c

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

}

/* ------------------------------------------------------------------------- *
FUNCTION ch_track_monosat()
RETURNS  None.

PARAMETERS
  ch   : channel number

PURPOSE
  track signal in code and doppler space
  write inphase / quad data to user prg via FIFO

WRITTEN BY
  Clifford Kelley
* ------------------------------------------------------------------------- */

void ch_track_monosat( INT16 ch, INT16 accum_missed)
{
  INT16 bit;
  long  i_pd, q_pd, dfrq, 
        car_phs_res, cod_phs_res,
        prompt_mag, dith_mag;

  if ( ch != RT_Ctrl->leadch)
    return;

//  GP2021 has two tracking arms only: prompt & dither
  i_pd = Chan[ch].i_prompt + Chan[ch].i_dith;
  q_pd = Chan[ch].q_prompt + Chan[ch].q_dith;

  Chan[ch].i_pd_20ms  += i_pd;
  Chan[ch].qq_pd_20ms += (q_pd * q_pd);

/*  
 *  phase discriminator: atan( Q, I), we can't use atan2() here, since data bits
 *  are still modulated on the signal causing sign flips.
 */
  car_phs_res = fix_atan( q_pd, i_pd);

/* --- 2nd order PLL --- */
  dfrq = trk_car_c3 * car_phs_res - 
         trk_car_c1 * car_phs_res_prv[ch];

/* --- save residual phase for next iteration --- */
  car_phs_res_prv[ch] = car_phs_res;

/* --- fix point arithmetics (2^14 <-> 1.0) --- */
  Chan[ch].car_frq += dfrq >> 14;

/*
 *  update carrier frq (all channels simultaneously!)
 */
  all_carrier( Chan[ch].car_frq);
//  ch_carrier( ch, Chan[ch].car_frq);

#if 0
//  adjust carrier frqs of trailing channels every TIC event only
  rt_printk( "ch_track_monosat: delta_carrier[%d] = %ld\n", ch, delta_carrier[ch]);
  if ( accum_missed & 0x2000)
  {
    int i;
    for ( i=0; i<NOFCHN; i++)
    {
      rt_printk( "ch_track_monosat: (2) delta_carrier[i] = %ld\n", delta_carrier[i]);
      if ( i != ch)
        ch_carrier( i, Chan[ch].car_frq - delta_carrier[i]);
    }
  }
#endif


/*
 *  average over 20ms
 */
  Chan[ch].q_dith_20ms   += Chan[ch].q_dith;
  Chan[ch].q_prompt_20ms += Chan[ch].q_prompt;
  Chan[ch].i_dith_20ms   += Chan[ch].i_dith;
  Chan[ch].i_prompt_20ms += Chan[ch].i_prompt;

/*
 *  we've added 20 samples
 */
  if ( ( Chan[ch].ms_count % 20) == 0)
  {
    prompt_mag = rss( Chan[ch].i_prompt_20ms, Chan[ch].q_prompt_20ms);
    dith_mag   = rss( Chan[ch].i_dith_20ms,   Chan[ch].q_dith_20ms);

    cod_phs_res = RT_Ctrl->earlylate * ( prompt_mag - 2 * dith_mag);

/* --- 2nd order PLL --- */
    dfrq = trk_cod_c3 * cod_phs_res - 
           trk_cod_c1 * cod_phs_res_prv[ch];

/* --- save for next iteration --- */
    cod_phs_res_prv[ch] = cod_phs_res;

/* --- fix point arithmetics (2^14 <-> 1.0) --- */
    Chan[ch].cod_frq += dfrq >> 14;

//  update code frq
    all_code( Chan[ch].cod_frq);

#if 0
//  shift slave channels at TIC rate (0.1 s) only
      if ( accum_missed & 0x2000)
      {
        int i;
        for ( i=0; i<NOFCHN; i++)
        {
          if ( i != ch)
            ch_code( i, Chan[ch].cod_frq - delta_code[i]);
        }
      }
#endif

//    Chan[ch].sum += Chan[ch].prompt_mag + Chan[ch].dith_mag;
//
//    if ( Chan[ch].ms_count % 100 == 0)
//    {
//      Chan[ch].avg_20ms = Chan[ch].sum / 5;
//      Chan[ch].sum      = 0;
//    }

//  nav bit
    bit = BSIGN( Chan[ch].i_prompt_20ms + Chan[ch].i_dith_20ms);

/* 
 *  Find preamble if not yet found. If already found, just read it.
 */
    if ( !Chan[ch].tow_sync)
      find_preamble( ch, bit);
    else
      read_preamble( ch, bit);

/* 
 *  Variable 'Preamble_Found[]' is set in find_preamble()
 */
    if ( Preamble_Found[ch])
      writefifo_navbit( ch, bit);

    writefifo_corrdata( ch, ch);  // in monosat mode, ch == RT_Ctrl->leadch!

    Chan[ch].q_dith_20ms   = 0;
    Chan[ch].q_prompt_20ms = 0;
    Chan[ch].i_dith_20ms   = 0;
    Chan[ch].i_prompt_20ms = 0;

    Chan[ch].i_pd_20ms  = 0;
    Chan[ch].qq_pd_20ms = 0;

  }  // --- if ( msec_count[ch] ... ) ---

//  we count bits & frames only in tracking mode
  if ( Chan[ch].ms_count % 20 == 0)
    Chan[ch].bit_count = (Chan[ch].bit_count + 1) % 1500;

  if ( Chan[ch].ms_count == 0 && Chan[ch].sfid == 5)
    Chan[ch].frame_count += 1;

  return;
}

/* ------------------------------------------------------------------------- *
FUNCTION ch_follow_monosat_DUMP()

RETURNS
  none

PARAMETERS
  ch  : channel number
  idx : index in channel_list + 1 (1,...,11)

PURPOSE
  in 'monosat' mode 11 'slave' channels follow 1 'master' channel,
  add amplitudes at DUMP event
* ------------------------------------------------------------------------- */

void ch_follow_monosat_DUMP( INT16 ch)
{
  INT16 leadch;
  
/* 
 * don't touch the master channel!
 */
  if ( ch == RT_Ctrl->leadch)
    return;

  leadch = RT_Ctrl->leadch;

// code_phase : 0,...,2045; halfchips
// code_DCO_phase : 0,...,1023; 1/1024 halfchip
//  Ofs = (Chan[ch].code_phase - Chan[RT_Ctrl->leadch].code_phase)*1024 + 
//        (Chan[ch].code_DCO_phase - Chan[RT_Ctrl->leadch].code_DCO_phase);

//  keep track of I/Q
  Chan[ch].q_dith_20ms   += Chan[ch].q_dith;
  Chan[ch].q_prompt_20ms += Chan[ch].q_prompt;
  Chan[ch].i_dith_20ms   += Chan[ch].i_dith;
  Chan[ch].i_prompt_20ms += Chan[ch].i_prompt;

  if ( ( Chan[leadch].ms_count % 20) == 0)
  {
    writefifo_corrdata( ch, leadch);

    Chan[ch].q_dith_20ms   = 0;
    Chan[ch].q_prompt_20ms = 0;
    Chan[ch].i_dith_20ms   = 0;
    Chan[ch].i_prompt_20ms = 0;
  }

  return;
 
}

/* ------------------------------------------------------------------------- *
FUNCTION ch_follow_monosat_TIC()

RETURNS
  none

PARAMETERS
  ch  : channel number
  idx : index in channel_list + 1 (1,...,11)

PURPOSE
  in 'monosat' mode 11 'slave' channels follow 1 'master' channel,
  keep synchronization
  Correct code/carrier DCO frequencies at TICs only.
* ------------------------------------------------------------------------- */

void ch_follow_monosat_TIC( INT16 leadch)
{
  INT16 ch;
  long  delta;


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

// don't touch the master channel
    if ( ch == leadch)
      continue;

  //  first align in carrier space ...
    if ( Chan[ch].carr_dco_phase <= 1023 && Chan[leadch].carr_dco_phase <= 1023)
    {

      delta = Chan[ch].carr_dco_phase - Chan[leadch].carr_dco_phase;

  /* 
   * delta : -1023,...,1-,0,1,...,1023
   * map to interval -512,...,512 
   */
      delta = ((delta + 1024 + 512) % 1024) - 512;

  /*
   *  positive phase difference -> run slower
   *  negative phase difference -> run faster
   */

  /*
   * re-sync to master's carrier phase
   */
      if ( delta)
        ch_carrier( ch, Chan[leadch].car_frq - 10 * delta);

    }

  /* 
   * ... then align in code space
   *  Chan[ch].code_phase     = 0,...,2045   [halfchips]
   *  Chan[ch].code_dco_phase = 0,...,1023   [1/1024 halfchips]
   */

  /*
   *  ignore outlier values
   */
    if ( Chan[ch].code_phase <= 2047 && Chan[ch].code_dco_phase <= 1023 &&
         Chan[leadch].code_phase <= 2047 && Chan[leadch].code_dco_phase <= 1023)
    {
      delta = 
        (Chan[ch].code_phase     * 1024L + Chan[ch].code_dco_phase) - 
        (Chan[leadch].code_phase * 1024L + Chan[leadch].code_dco_phase);

  // delta : -2046*1024,...,1-,0,1,...,2046*1024
  //    delta = ( delta >  1023L * 1024L) ? delta - 2046L * 1024L : delta;
  //    delta = ( delta < -1023L * 1024L) ? delta + 2046L * 1024L : delta;

      delta = ((delta + 2046L * 1024L + 1023L * 1024L) % (2046L * 1024L)) - 1023L * 1024L;

  // add offsets to spread slave channels in code space
      delta += monosat_cod_ofs[ch];

  /*
   * update code and carrier NCOs to keep them in sync
   */
      if ( delta)
        ch_code( ch, Chan[leadch].cod_frq - delta);
    }  
  }  // --- for ( ch=0; ch<NOFCHN; ch++) ---  
  
  return;
}


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