ogrt_multisat.c

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

  if ( prompt_mag != 0 || dith_mag != 0)
  {
    Cod_Phs[ch] = RT_Ctrl->earlylate * pull_code_k * 
      ((prompt_mag - 2 * dith_mag) << 14) /
       (prompt_mag + 2 * dith_mag);

    dcod_phs = pull_code_d * 
      (Cod_Phs[ch] - Cod_Phs_Prv[ch]);

//  don't close the loop for 2 ms to avoid transients
    if ( Pull_In_Count[ch] > 2 )
    {
      Chan[ch].cod_frq += (Cod_Phs[ch] + dcod_phs) >> 14;
//      Chan[ch].cod_frq += ((pul_cod_c1 + pul_cod_c2) * Cod_Phs[ch] + 
//                          - pul_cod_c1 * Cod_Phs_Prv[ch]) >> 14;
//      rt_printk("ch_pull_in(): ch_code -> Chan[%d].cod_frq\n", ch, Chan[ch].cod_frq);
      ch_code( ch, Chan[ch].cod_frq);
    }
  }

  Cod_Phs_Prv[ch] = Cod_Phs[ch];

//  rt_printk( "ch_pull_in(): Cod_Phs[%d]=%ld \n", ch, Cod_Phs[ch]);

  if ( i_sum != 0 || q_sum != 0)
// orig:     theta = fix_atan2( i_sum, -q_sum);           *** CHECKME ***
//    theta = fix_atan2( q_sum, i_sum);
    theta = -fix_atan2( q_sum, i_sum);
  else
    theta = Theta_Prv[ch];

  theta_dot     = theta - Theta_Prv[ch];
  Theta_Prv[ch] = theta;

  if ( theta > 0)
    theta_e = theta - 25736;
  else if ( theta <= 0)
    theta_e = theta + 25736;

  if (Pull_In_Count[ch] > pull_in_count_max - phase_test)
    Th_Rms[ch] += (theta_e * theta_e) >> 14;

//  rt_printk( "ch_pull_in(): Th_Rms[%d]=%ld \n", ch, Th_Rms[ch]);

  if ( rt_labs( theta_dot) < 32768L)
  {
    if ( i_sum != 0 || q_sum != 0)
    {
      wdot_gain      = Pull_In_Count[ch] / 499;
      wdot_gain      *= wdot_gain;
      wdot_gain      *= wdot_gain;

      Car_Phs[ch] = pull_carr_k * (theta_dot * 5 /
        (1 + wdot_gain) + theta_e);

      dcar_phs = pull_carr_d * (Car_Phs[ch] - Car_Phs_Prv[ch]);

// don't close the loop for 5 ms to avoid transients
      if ( Pull_In_Count[ch] > 5 )
      {
        Chan[ch].car_frq += (Car_Phs[ch] + dcar_phs) >> 14;
//        rt_printk( "ch_pull_in(): Chan[%d].car_frq=%ld \n", ch, Chan[ch].car_frq);
        ch_carrier( ch, Chan[ch].car_frq);
      }
    }    // --- if ( i_sum != 0 || q_sum !=0) ---
  }      // ---if ( rt_labs( theta_dot) < 32768L ) ---


//  after 'pull_in_count_max' ms decide if we can transition to state 'tracking'
  if ( Pull_In_Count[ch] >= pull_in_count_max)
  {
//    if ( pull_in_count_max == 0)
//      pull_in_count_max = 1500;
  
    Avg_20ms[ch] = Sum[ch] / pull_in_count_max / 2;
    Chan[ch].avg_20ms = Avg_20ms[ch];
    Th_Rms[ch]   = fix_sqrt( Th_Rms[ch] / phase_test);

    if ( Avg_20ms[ch] > 14 * rms / 10 && 
         Th_Rms[ch] < 12000 ) 
    {
//  sufficient signal transition to tracking mode?
      Avg_20ms[ch]      *= 20;
      Chan[ch].avg_20ms = Avg_20ms[ch];

      Chan[ch].state         = align;
      Sum[ch]           = 0;

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

      Align_Hist_Count[ch]   = 0;
      Repeat_Align[ch]       = 0;
    }
    else
    {
//  insufficient signal ... back to state 'acquisition'
      Chan[ch].state     = acquisition;
      Cod_Slew[ch]   = 0;
      Chan[ch].cod_frq   = CODE_REF + code_corr;
//      rt_printk( "ch_pull_in(): Chan[%d].cod_frq=%ld \n", ch, Chan[ch].cod_frq);
      ch_code( ch, Chan[ch].cod_frq);        // 1.023 MHz chipping rate
    }
  }

  Car_Phs_Prv[ch] = Car_Phs[ch];

  Pull_In_Count[ch] += 1;

  return;
}

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

PARAMETERS
  ch   : channel number

PURPOSE
  determine nav bit boundaries
  transition to tracking at nav bit boundary

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

static UINT16 Align_Time_Period = 500;

void ch_align( INT16 ch)
{
  INT16 i;
  long  i_sum, q_sum, dfrq, 
        car_phs_res, cod_phs_res;
  static long i_sum_prv[NOFCHN];

  static UINT16 Hist_Nav[NOFCHN][CHIPSPERBIT],
                Hist_Idx[NOFCHN];

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

  if ( Align_Hist_Count[ch] < Align_Time_Period)
  {

    if ( Align_Hist_Count[ch] == 0)
    {
      for (i=0; i<20; i++)
        Hist_Nav[ch][i] = 0;
      Hist_Idx[ch] = 0;
    }

    Align_Hist_Count[ch] += 1;

//  increment histogram array if sign changes (CHIPSPERBIT = 20)
    if ( i_sum * i_sum_prv[ch] < 0L)
      Hist_Nav[ch][Hist_Idx[ch]] += 1;
    Hist_Idx[ch] = (Hist_Idx[ch]+1) % CHIPSPERBIT;

    if ( Align_Hist_Count[ch] >= Align_Time_Period)
    {
      Chan[ch].ms_count = ( Chan[ch].ms_count + 20 - 
        find_max_element( Hist_Nav[ch], CHIPSPERBIT)) % 20;
    }
  }
  else if ( (Chan[ch].ms_count + 1) % 20 == 0)
  {
    if ( Repeat_Align[ch] < 2)
    {
//      rt_printk( "return to align ...\n"); 
      Align_Hist_Count[ch]   = 0;
      Repeat_Align[ch]      += 1;
    }
    else
    {
      Chan[ch].state         = track;

      Sum[ch]           = 0;
      Chan[ch].tow_sync      = 0;

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


/*  
 *  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_sum, i_sum);

/* --- 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
 */
  ch_carrier( ch, Chan[ch].car_frq);

/*
 *  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[ch] = rss( Chan[ch].i_prompt_20ms, Chan[ch].q_prompt_20ms);
    Dith_Mag[ch]   = rss( Chan[ch].i_dith_20ms,   Chan[ch].q_dith_20ms);

/*
 * code tracking loop, update every 20 msec
 */
    cod_phs_res = RT_Ctrl->earlylate *  
      ( Prompt_Mag[ch] - 2 * Dith_Mag[ch]);

/* --- 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
    ch_code( ch, Chan[ch].cod_frq);


    Sum[ch] += Prompt_Mag[ch] + Dith_Mag[ch];

    if ( Chan[ch].ms_count % 100 == 0)
    {
      Avg_20ms[ch] = Sum[ch] / 5;
      Chan[ch].avg_20ms = Avg_20ms[ch];

      Sum[ch]     = 0;
    }

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

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

/*
 *  save for next round
 */
  Car_Phs_Prv[ch] = Car_Phs[ch];
  Cod_Phs_Prv[ch] = Cod_Phs[ch];

  i_sum_prv[ch]      = i_sum;

  return;
}

/* ------------------------------------------------------------------------- *
FUNCTION ch_track()
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( INT16 ch)
{
  INT16 bit;
  long  i_sum, q_sum, dfrq, 
        car_phs_res, cod_phs_res;
//  signed char chr, buf[MSG_MAX_LEN];
//  unsigned char len;

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

//  first thing to do is to adjust the carrier and code PLL (time critical!)

/*
 *   phase discriminator: Q * sign(I)
 */

#if 0
    Car_Phs[ch] = -trk_carr_k * ( q_sum << 14) * 
      sign( i_sum) / rss( i_sum, q_sum);

    Car_Phs[ch] = -trk_carr_k * fix_atan( q_sum, i_sum);
    dcar_phs = trk_carr_d * 
      ( Car_Phs[ch] - Car_Phs_Prv[ch]);
    Chan[ch].car_frq += (Car_Phs[ch] + dcar_phs) >> 14;
    Car_Phs_Prv[ch] = Car_Phs[ch];
#endif

/*  
 *  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_sum, i_sum);

/* --- 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
 */
  ch_carrier( ch, Chan[ch].car_frq);

/*
 *  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[ch] = rss( Chan[ch].i_prompt_20ms, Chan[ch].q_prompt_20ms);
    Dith_Mag[ch]   = rss( Chan[ch].i_dith_20ms,   Chan[ch].q_dith_20ms);

/*
 * code tracking loop, update every 20 msec
 */

#if 0
      Cod_Phs[ch] = RT_Ctrl->earlylate * trk_code_k * 
        ( Prompt_Mag[ch] - 2 * Dith_Mag[ch]);

      dcod_phs = trk_code_d * 
        ( Cod_Phs[ch] - Cod_Phs_Prv[ch]);

      Cod_Phs_Prv[ch] = Cod_Phs[ch];

#ifdef CARRIER_AIDING
//      ddf=((Prompt_Mag[ch]-Dith_Mag[ch])*2048)/
//        (Prompt_Mag[ch]+Dith_Mag[ch])*trk_code_k;
//      chan[ch].dfreq+=ddf;
//      chan[ch].code_freq =(chan[ch].dfreq/trk_code_d+ddf)/256+
//        (carrier_ref-chan[ch].carrier_freq)/cc_scale + code_ref;
      Chan[ch].cod_frq += (Cod_Phs[ch] + dcod_phs) / trk_div + 
        (Car_Phs[ch] + dcar_phs) / CARRIER_AIDING_SCALE;
#else
      Chan[ch].cod_frq += (Cod_Phs[ch] + dcod_phs) / trk_div;
#endif

#endif

    cod_phs_res = RT_Ctrl->earlylate *  
      ( Prompt_Mag[ch] - 2 * Dith_Mag[ch]);

/* --- 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
    ch_code( ch, Chan[ch].cod_frq);

    Sum[ch] += Prompt_Mag[ch] + Dith_Mag[ch];

    if ( Chan[ch].ms_count % 100 == 0)
    {
      Avg_20ms[ch] = Sum[ch] / 5;
      Chan[ch].avg_20ms = Avg_20ms[ch];

      Sum[ch]      = 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);

    Chan[ch].q_dith_20ms   = 0;
    Chan[ch].q_prompt_20ms = 0;
    Chan[ch].i_dith_20ms   = 0;
    Chan[ch].i_prompt_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;
}

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