gpsrcvr.c

C写的用软件无线电实现的GPS模拟程序

  a combination FLL and PLL.
  It will attempt to track for xxx ms, the last xxx ms of data will be
  gathered to determine if we have both code and carrier lock.
  If so we will transition to state 'tracking'.
  Determine start of data bit transition (bit synchronization) using 
  histogram method.
      
REFERENCES
  Tsui, Fundamentals of GPS receivers: A software approach, Wiley
  Krunvieda et al., A complete IF software GPS receiver: a tutorial about
    the detail, Data Fusion Corporation
  Kaplan, Understanding GPS: Principles and Applications

WRITTEN BY
  Clifford Kelley
  Modifications by G. Beyerle
  
TO DO
  02-05-01: Replace floating point with integer arithmetic

NOTES
  02-03-20:  

*******************************************************************************/
static void ch_pull_in( char ch)
{
  int i, idx, bitsign;
  long Ip, Qp, Id, Qd;
  long ddf, ddcar, q_sum, i_sum;
  double dsc_car_phs_old, 
         dsc_car_phs, 
         car_frq_ofs,
         dsc_cod_phs_old, 
         dsc_cod_phs, 
         cod_frq_ofs;
  double sum_IQd, sum_IQp; 
  double Ip2, Qp2, IQp2;
  double curphs;

  if ( chan[ch].ch_time < pull_in_time)
  {

//
//  if current phase deviated more than 90 deg from last value
//  we probably have a nav bit flip
//
    curphs = atan2( chan[ch].bitsign*chan[ch].q_prmt, 
                    chan[ch].bitsign*chan[ch].i_prmt) / (2*M_PI);
    if ( fabs( chan[ch].dsc_car_phs - curphs) > 0.25)
    {
      chan[ch].bitsign *= -1;
      chan[ch].databithist[chan[ch].ms_count] += 1;
    }  

//    printf( "chan[ch].dsc_car_phs = %e, curphs = %e\n", 
//      chan[ch].dsc_car_phs, curphs);

    bitsign = chan[ch].bitsign;
    
    Ip = bitsign * chan[ch].i_prmt;
    Qp = bitsign * chan[ch].q_prmt;
    Id = bitsign * chan[ch].i_dith;
    Qd = bitsign * chan[ch].q_dith;

//
//  for efficiency we implement summation over NOFCOHINT values by 
//  keeping track of individual additions in arrays 'chan[ch].i/q_prmt/dith_sav[]'
//

// --- add latest value ...
    chan[ch].i_prmt_sum   += Ip;
    chan[ch].q_prmt_sum   += Qp;
    chan[ch].i_dith_sum   += Id;
    chan[ch].q_dith_sum   += Qd;
    chan[ch].iq2_prmt_sum += Ip * Ip + Qp * Qp;

// ... subtract first value ...
    idx = (chan[ch].iq_sav_idx + 1) % NOFCOHINT;
    chan[ch].iq_sav_idx = idx;
    chan[ch].i_prmt_sum   -= chan[ch].i_prmt_sav[idx];
    chan[ch].q_prmt_sum   -= chan[ch].q_prmt_sav[idx];
    chan[ch].i_dith_sum   -= chan[ch].i_dith_sav[idx];
    chan[ch].q_dith_sum   -= chan[ch].q_dith_sav[idx];
    chan[ch].iq2_prmt_sum -= chan[ch].iq2_prmt_sav[idx];

// ... and replace by latest value in "store array"
    chan[ch].i_prmt_sav[idx]   = Ip;
    chan[ch].q_prmt_sav[idx]   = Qp;
    chan[ch].i_dith_sav[idx]   = Id;
    chan[ch].q_dith_sav[idx]   = Qd;
    chan[ch].iq2_prmt_sav[idx] = Ip * Ip + Qp * Qp;

// --- carrier tracking loop ---
    dsc_car_phs_old      = chan[ch].dsc_car_phs;

//    printf( "chan[%d].q_prmt_sum = %d\n", ch, chan[ch].q_prmt_sum);
//    printf( "chan[%d].i_prmt_sum = %d\n", ch, chan[ch].i_prmt_sum);

    dsc_car_phs          = atan2( chan[ch].q_prmt_sum, chan[ch].i_prmt_sum) / (2*M_PI);  // optimal carrier phase discriminator
    chan[ch].dsc_car_phs = dsc_car_phs;

    car_frq_ofs = car_PLL_pullin_p1 * dsc_car_phs - car_PLL_pullin_p2 * dsc_car_phs_old;

//    if (ch==0)
//    {
//      printf( "car_PLL_pullin_p1    = %e\n", car_PLL_pullin_p1);
//      printf( "car_PLL_pullin_p2    = %e\n", car_PLL_pullin_p2);
//      printf( "dsc_car_phs          = %e\n", dsc_car_phs);
//      printf( "dsc_car_phs_old      = %e\n", dsc_car_phs_old);
//      printf( "chan[ch].dsc_car_phs = %e\n", chan[ch].dsc_car_phs);
//      printf( "car_frq_ofs          = %e\n", car_frq_ofs);
//    }  
//    getchar();

    car_frq_ofs /= FRQUNIT;   // INCR counter runs in units of 42 mHz

  //  printf( "d codfrq = %.6f Hz, d carfrq = %.6f Hz\n", 
  //    (chan[ch].code_freq    - 24028328) * 42.57475e-3, 
  //    (chan[ch].carrier_freq - 33010105) * 42.57475e-3);

// --- update INCR register ---
    chan[ch].carrier_freq += (long)( car_frq_ofs);
    ch_carrier( ch, chan[ch].carrier_freq);

// --- code tracking loop ---
//    printf("vor sqrt() *** 1 ***\n");
//    printf("iDith = %e \n", (double)( chan[ch].i_dith_sum * chan[ch].i_dith_sum));
//    printf("qDith = %e \n", (double)( chan[ch].q_dith_sum * chan[ch].q_dith_sum));
//    printf("iPrmt = %e \n", (double)( chan[ch].i_prmt_sum * chan[ch].i_prmt_sum));
//    printf("qPrmt = %e \n", (double)( chan[ch].q_prmt_sum * chan[ch].q_prmt_sum));

    sum_IQd = sqrt( (double)( chan[ch].i_dith_sum) * (double)( chan[ch].i_dith_sum) + 
                    (double)( chan[ch].q_dith_sum) * (double)( chan[ch].q_dith_sum));
    sum_IQp = sqrt( (double)( chan[ch].i_prmt_sum) * (double)( chan[ch].i_prmt_sum) + 
                    (double)( chan[ch].q_prmt_sum) * (double)( chan[ch].q_prmt_sum));

//    printf("nach sqrt() *** 1 ***\n");
                  
    dsc_cod_phs_old = chan[ch].dsc_cod_phs;

    if ( sum_IQp)
      dsc_cod_phs = (sum_IQd / sum_IQp - 0.5); 
    else  
      dsc_cod_phs = 0.0; 

    chan[ch].dsc_cod_phs = dsc_cod_phs;

    cod_frq_ofs = cod_PLL_pullin_p1 * dsc_cod_phs - cod_PLL_pullin_p2 * dsc_cod_phs_old;

    cod_frq_ofs /= 2*FRQUNIT;   // INCR counter runs in units of 85 mHz

// --- update INCR register ---
    chan[ch].code_freq += (long)( cod_frq_ofs);
    ch_code( ch, chan[ch].code_freq);

// --- carrier and code lock detector ---
    Ip2 = (double)( chan[ch].i_prmt_sum) * (double)( chan[ch].i_prmt_sum);
    Qp2 = (double)( chan[ch].q_prmt_sum) * (double)( chan[ch].q_prmt_sum);

// --- carrier lock detector ---
    chan[ch].car_lock_det = (Ip2 - Qp2) / (Ip2 + Qp2);

//    if (chan[ch].car_lock_det > 1.0)
//    {
//      printf("** r/lk = %e, Ip2 = %e, Qp2 = %e\n", chan[ch].car_lock_det, Ip2, Qp2);
//      getchar();
//    }

// --- code lock detector ---
    chan[ch].cod_lock_det = (Ip2 + Qp2) / chan[ch].iq2_prmt_sum / NOFCOHINT;

// ************** debug *******************
//    if ( ch==0)
//    {
//      extern unsigned long Sample_Counter_Start[], Sample_Counter_End[];
//
//      printf( "pll: (%d-%d) Ip=%4d, Qp=%4d, Id=%4d, Qd=%4d, r/lk:%.2f, d/lk:%.2f", 
//        Sample_Counter_Start[0], Sample_Counter_End[0], Ip, Qp, Id, Qd, 
//        chan[ch].car_lock_det, chan[ch].cod_lock_det);
//      printf( "pll: Ip=%4d, Qp=%4d, Id=%4d, Qd=%4d, r/lk:%.2f, d/lk:%.2f\n", 
//        Ip, Qp, Id, Qd, chan[ch].car_lock_det, chan[ch].cod_lock_det);
//      getchar();  
//    }  
// ************** debug *******************

    if ( chan[ch].ms_count >= 19)
    {
      chan[ch].tr_bit_time++;

      chan[ch].prompt_mag = rss( chan[ch].i_prmt_sum, chan[ch].q_prmt_sum);
      chan[ch].dith_mag   = rss( chan[ch].i_dith_sum, chan[ch].q_dith_sum);
      chan[ch].sum        += chan[ch].prompt_mag + chan[ch].dith_mag;

      chan[ch].t_count++;

      if ( chan[ch].t_count % 5 == 0)
      {
        chan[ch].avg = chan[ch].sum / 10;
        chan[ch].sum = 0;
      }
    }

    if ( chan[ch].ch_time > pull_in_time - phase_test) 
    {
      chan[ch].th_rms += (long)( dsc_car_phs * dsc_car_phs * 268435456);   // (2^14)^2
//      printf( "pull_in: chan[ch].th_rms = %d\n", chan[ch].th_rms); 
//      getchar();   
    }  


//    if ( chan[ch].cod_lock_det > cod_lock_det_thresh)
//    {
//      i_sum = chan[ch].i_dith + chan[ch].i_prmt;
//
//// --- sign change? if yes mark in histogram ---
//      if ( sign( chan[ch].old_i_sum) == -sign( i_sum))
//        chan[ch].databithist[chan[ch].ms_count] += 1;
//    }

    chan[ch].old_i_sum = i_sum;
    chan[ch].ch_time++;

  } // --- if ( chan[ch].ch_time < pull_in_time) ---
  else
  {

    if ( chan[ch].cod_lock_det < cod_lock_det_thresh)
    {

// --- go back to square one and start with acq ---
      chan[ch].state = acquisition;
    }  
    else
    {

// --- analyze data bit histogram (bit synchronization)
      int ret;    
      ret = analyze_databit_histogram( ch);

//      printf("pll: ret = %d\n", ret);
//      getchar();

      if ( ret >= 0)
      {
//        chan[ch].ms_count  = (20-ret+chan[ch].ms_count) % 20;  // reset msec counter
        chan[ch].ms_count  = (chan[ch].ms_count-ret+20) % 20;  // reset msec counter
//      chan[ch].databit_start = ret;
// --- transition to tracking state ---
        chan[ch].state     = track;
        chan[ch].t_count   = 0;
//      chan[ch].avg       = chan[ch].avg * 20;
//      chan[ch].sum       = 0;
      }  
      else
        chan[ch].state = acquisition;
    }  
  }  // --- if ( chan[ch].ch_time == pull_in_time) ---
  
// --- if back to acq, reset frq ---
  if ( chan[ch].state == acquisition)
  {
    chan[ch].codes     = 0;
    chan[ch].code_freq = code_ref + code_corr;
    ch_code( ch, chan[ch].code_freq);        // 1.023 MHz chipping rate
  }

  chan[ch].ms_count = (++chan[ch].ms_count) % NOFMSECPERDATABIT;

  return;
}

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

PARAMETERS None.

PURPOSE
  Track signal with a combination FLL and PLL.
  Identify navigation data frame (frame synchronization).

REFERENCES
  Kaplan, Understanding GPS: Principles and Applications

WRITTEN BY
  Clifford Kelley
  Modifications by G. Beyerle

TO DO
  02-05-01: Replace floating point with integer arithmetic

NOTES
  02-03-20: 

*******************************************************************************/
static void ch_track( char ch)
{
  int i, idx, bitsign;
  long Ip, Qp, Id, Qd;
  long ddf, ddcar, q_sum, i_sum;
  float  dsc_car_phs_old, 
         dsc_car_phs, 
         car_frq_ofs,
         dsc_cod_phs_old, 
         dsc_cod_phs, 
         cod_frq_ofs;
  float sum_IQd, sum_IQp; 
  float Ip2, Qp2, IQp2, curphs;


//
//  if current phase deviated more than 90 deg from last value
//  we probably have a nav bit flip
//
  curphs = atan2( chan[ch].bitsign*chan[ch].q_prmt, 
                  chan[ch].bitsign*chan[ch].i_prmt) / (2*M_PI);

//  if ( fabs( chan[ch].dsc_car_phs - curphs) > 0.25 &&
//       chan[ch].ms_count == 0)
  if ( fabs( chan[ch].dsc_car_phs - curphs) > 0.25)
  {
//    printf("trk: chan[%d].ms_count = %d\n", ch, chan[ch].ms_count);
//    getchar();
    chan[ch].bitsign *= -1;

//    printf("trk: chan[%d].ms_count = %d\n", ch, chan[ch].ms_count); 
//    getchar();
//    chan[ch].databithist[chan[ch].ms_count] += 1;
  }  

//
//  implement summation over NOFCOHINT values by keeping track
//  of individual additions
//
  bitsign = chan[ch].bitsign;

  Ip = bitsign * chan[ch].i_prmt;
  Qp = bitsign * chan[ch].q_prmt;
  Id = bitsign * chan[ch].i_dith;
  Qd = bitsign * chan[ch].q_dith;

  if ( OutputIQ)
    write_to_file_IQ( chan[ch].i_prmt, chan[ch].q_prmt, chan[ch].prn);  // with nav msg
//    write_to_file_IQ( Ip, Qp, chan[ch].prn);  // nav msg wiped off

// add latest value ...
  chan[ch].i_prmt_sum   += Ip;
  chan[ch].q_prmt_sum   += Qp;
  chan[ch].i_dith_sum   += Id;
  chan[ch].q_dith_sum   += Qd;
  chan[ch].iq2_prmt_sum += Ip * Ip + Qp * Qp;

// ... subtract first value ...
  idx = (chan[ch].iq_sav_idx + 1) % NOFCOHINT;
  chan[ch].iq_sav_idx = idx;
  chan[ch].i_prmt_sum   -= chan[ch].i_prmt_sav[idx];
  chan[ch].q_prmt_sum   -= chan[ch].q_prmt_sav[idx];
  chan[ch].i_dith_sum   -= chan[ch].i_dith_sav[idx];
  chan[ch].q_dith_sum   -= chan[ch].q_dith_sav[idx];
  chan[ch].iq2_prmt_sum -= chan[ch].iq2_prmt_sav[idx];

// ... and replace by latest value in "store array"
  chan[ch].i_prmt_sav[idx] = Ip;
  chan[ch].q_prmt_sav[idx] = Qp;
  chan[ch].i_dith_sav[idx] = Id;
  chan[ch].q_dith_sav[idx] = Qd;
  chan[ch].iq2_prmt_sav[idx] = Ip * Ip + Qp * Qp;

//  carrier tracking loop
  dsc_car_phs_old      = chan[ch].dsc_car_phs;
  dsc_car_phs          = atan2( chan[ch].q_prmt_sum, chan[ch].i_prmt_sum) / (2*M_PI);  // optimal carrier phase discriminator
  chan[ch].dsc_car_phs = dsc_car_phs;

  car_frq_ofs = car_PLL_track_p1 * dsc_car_phs - car_PLL_track_p2 * dsc_car_phs_old;

  car_frq_ofs /= FRQUNIT;   // INCR counter runs in units of 42.57 mHz

//  update INCR register
  chan[ch].carrier_freq += (long)( car_frq_ofs);
  ch_carrier( ch, chan[ch].carrier_freq);

  if ( OutputCarFrq)
    write_to_file_carfrq( (long)( car_frq_ofs), chan[ch].prn);

//  code tracking loop
  sum_IQd = sqrt( (double)( chan[ch].i_dith_sum) * (double)( chan[ch].i_dith_sum) + 
                  (double)( chan[ch].q_dith_sum) * (double)( chan[ch].q_dith_sum));
  sum_IQp = sqrt( (double)( chan[ch].i_prmt_sum) * (double)( chan[ch].i_prmt_sum) + 
                  (double)( chan[ch].q_prmt_sum) * (double)( chan[ch].q_prmt_sum));
                  
  dsc_cod_phs_old = chan[ch].dsc_cod_phs;

  if ( sum_IQp)
    dsc_cod_phs = (sum_IQd / sum_IQp - 0.5); 
  else  
    dsc_cod_phs = 0.0; 

  chan[ch].dsc_cod_phs = dsc_cod_phs;

  cod_frq_ofs = cod_PLL_track_p1 * dsc_cod_phs - cod_PLL_track_p2 * dsc_cod_phs_old;

//  cod_frq_ofs *= NOFCHIPS;

  cod_frq_ofs /= 2*FRQUNIT;   // INCR counter runs in units of 85 mHz

//  update INCR register
  chan[ch].code_freq += (long)( cod_frq_ofs);
  ch_code( ch, chan[ch].code_freq);

//  carrier and code lock detector
  Ip2 = (double)( chan[ch].i_prmt_sum) * (double)( chan[ch].i_prmt_sum);
  Qp2 = (double)( chan[ch].q_prmt_sum) * (double)( chan[ch].q_prmt_sum);

  chan[ch].car_lock_det = (Ip2 - Qp2) / (Ip2 + Qp2);
  
  chan[ch].cod_lock_det = (Ip2 + Qp2) / chan[ch].iq2_prmt_sum / NOFCOHINT;

  if ( chan[ch].cod_lock_det < cod_lock_det_thresh)
  {
    chan[ch].state = acquisition;
//    printf("trk: chan[%d].cod_lock_det = %e, cod_lock_det_thresh = %e\n", 
//      ch, chan[ch].cod_lock_det, cod_lock_det_thresh);
//    getchar();
  }

// ----------------------
//  printf( "Ip=%5d, Qp=%5d, Id=%5d, Qd=%5d, r/lk:%.2f, d/lk:%.2f\n", 
//    Ip, Qp, Id, Qd, chan[ch].car_lock_det, chan[ch].cod_lock_det);
//  getchar();    
// ----------------------

//  printf( "d codfrq = %.6f Hz, d carfrq = %.6f Hz\n", 
//    (chan[ch].code_freq    - 24028328) * 42.57475e-3, 
//    (chan[ch].carrier_freq - 33010105) * 42.57475e-3);
//  printf( "DrPhs = %e, DdPhs = %e, drFrq = %e, ddFrq = %e\n", 
//    dsc_car_phs, dsc_cod_phs, car_frq_ofs, cod_frq_ofs);
//  LastSmplIdx = BUFFERPTR;
//  if ( abs( chan[ch].i_prmtpt) < abs( chan[ch].q_prmtpt))
//    getchar();