gpsrcvr.c

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

    if ( gps_alm[i].inc>0.0 && gps_alm[i].week!=gps_week) 
      almanac_valid=0;
  }

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

  for (ch=0; ch<=chmax; ch++) // if no satellite is being tracked
                              // turn the channel off
  {
    if ( chan[ch].CNo < 25.0 )
    {
      chan[ch].state = off;
      chan[ch].prn   = 0;
    }
  }

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

    if (alloc == 0) // if not allocated find an empty channel
    {
      for ( ch=0; ch<=chmax; ch++)
      {
        if ( chan[ch].state == off)
        {
          chan[ch].carrier_corr = (long)( -clock_offset * 1575.42 / FRQUNIT);  // GB: cast inserted

//          printf("cold_allocate: carrier_ref = %x (%d), chan[ch].carrier_corr = %x (%d)\n", 
//           carrier_ref, carrier_ref, chan[ch].carrier_corr, chan[ch].carrier_corr);
//           getchar();

          chan[ch].carrier_freq = carrier_ref + chan[ch].carrier_corr;       // set carrier
          ch_carrier( ch, chan[ch].carrier_freq);                            // select carrier
          chan[ch].code_freq = code_ref;
          ch_code( ch, chan[ch].code_freq);           // 1.023 MHz chipping rate
//  0xa000 : select late code in tracking arm
          ch_cntl( ch, prn_code[cold_prn] | 0xa000);  

//          chan[ch].prn      = cold_prn;
          if ( PRNChn[cold_prn])
            chan[ch].prn      = PRNChn[cold_prn];
          else  
            chan[ch].prn      = cold_prn;

          ch_on( ch);
          chan[ch].state    = acquisition;
          chan[ch].codes    = 0;
          chan[ch].n_freq   = search_min_f;
          chan[ch].del_freq = 1;
          cold_prn          = cold_prn % 32 + 1;
          break;
        } 
      }  // for ( ch=0; ch<=chmax; ch++)
    }  // if (alloc==0)
  }  // for (i=0; i<=chmax; i++)
}

/*******************************************************************************
FUNCTION rss( long a, long b)
RETURNS  long integer

PARAMETERS
      a  long integer
      b  long integer

PURPOSE
  This function finds the fixed point magnitude of a 2 dimensional vector

WRITTEN BY
  Clifford Kelley

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

inline long rss( long a, long b)
{
  long result, c, d;

// --- G.B. 02/03/26 ---

//
//  sqrt(a^2+b^2) ~ |a*(1+(b/a)^2/2)| = |a+b^2/(2*a)|  if |b| < |a|
//                ~ |b*(1+(a/b)^2/2)| = |b+a^2/(2*b)|  if |a| < |b|
//
  c = labs( a);
  d = labs( b);

  if ( c == 0)
    result = d;
  else if ( d == 0)
    result = c;
  else if ( d <= c)
    result = labs( c + d*d / (2*c));
  else if ( d > c)
    result = labs( d + c*c / (2*d));

// --- G.B. 02/03/26 ---

//  
//  if ( c == 0 && d == 0) 
//    result = 0;
//  else
//  {
//    if ( c > d) 
//      result = ( d >> 1) + c;
//    else     
//      result = ( c >> 1) + d;
//  }

  return (result);
}

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

PARAMETERS None.

PURPOSE

WRITTEN BY
  Clifford Kelley

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

static void ch_acq( char ch)
{
  long prompt_mag, dith_mag;

//  if ( ch == 0)
//    printf( "*** ch_acq ***\n");


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

  if ( abs( chan[ch].n_freq) <= search_max_f)        // search frequencies
  {
    prompt_mag = rss( chan[ch].i_prmt, chan[ch].q_prmt);
    dith_mag   = rss( chan[ch].i_dith, chan[ch].q_dith);

//  acq_thresh = 650 (default)

    if (( dith_mag > acq_thresh) && ( prompt_mag > acq_thresh))
    {
// <<< **** >>>
//      ch_code_slew( ch, 2044);         // slew back 1 chip so we can
// <<< **** >>>

      chan[ch].state     = confirm;    // confirm the signal

      chan[ch].i_confirm = 0;
      chan[ch].n_thresh  = 0;

//      printf("ch_acq: acq->conf, prompt_mag: %d, dith_mag: %d, acq_thresh: %d\n",
//        prompt_mag, dith_mag, acq_thresh);
//      getchar();
    }
    else
    {
// <<< **** >>>
//      ch_code_slew( ch, 2);            // slew forward 1 chip
//      chan[ch].codes += 2;
      ch_code_slew( ch, 1);            // slew forward 1 chip
      chan[ch].codes += 1;
// <<< **** >>>

//      printf("ch_acq: below thresh - ch_code_slew -> 2, prompt_mag: %d, dith_mag: %d, acq_thresh: %d\n",
//        prompt_mag, dith_mag, acq_thresh);
//      getchar();
    }

    if ( chan[ch].codes == 2044)
    {
//  stepped thru complete code period, now step to next frequency    
      chan[ch].n_freq      += chan[ch].del_freq;

//  step forward and backwards with increasing widths :  0,1,-1,2,-2, ...
      chan[ch].del_freq     = -(chan[ch].del_freq + sign( chan[ch].del_freq));
      chan[ch].carrier_freq = carrier_ref + chan[ch].carrier_corr + 
        delta_frq * chan[ch].n_freq;   // set carrier

//      printf("ch_acq: carrier_ref = %x (%d), chan[ch].carrier_corr = %x (%d) \n",
//        carrier_ref, carrier_ref, chan[ch].carrier_corr, chan[ch].carrier_corr);
//      printf("ch_acq: delta_frq = %x (%d), chan[ch].n_freq = %x (%d)", 
//        delta_frq, delta_frq, chan[ch].n_freq, chan[ch].n_freq);
//      getchar();

      ch_carrier( ch, chan[ch].carrier_freq);           // select carrier

      chan[ch].codes        = 0;
    }
  }
  else
  {
    chan[ch].n_freq       = search_min_f;               // keep looping
    chan[ch].del_freq     = 1;

    chan[ch].carrier_freq = carrier_ref + chan[ch].carrier_corr + 
      delta_frq * chan[ch].n_freq;                         // set carrier frq

//    printf("ch_acq: carrier_ref = %x (%d), chan[ch].carrier_corr = %x (%d) \n",
//     carrier_ref, carrier_ref, chan[ch].carrier_corr, chan[ch].carrier_corr);
//    printf("ch_acq: delta_frq = %x (%d), chan[ch].n_freq = %x (%d)", 
//     delta_frq, delta_frq, chan[ch].n_freq, chan[ch].n_freq);
//     getchar();

    ch_carrier( ch, chan[ch].carrier_freq);             // select carrier
    chan[ch].codes = 0;
  }
}

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

PARAMETERS None.

PURPOSE

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
static void ch_confirm( char ch)
{
  long prompt_mag, dith_mag;

//  if ( ch == 0)
//    printf( "*** ch_confirm ***\n");

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

  prompt_mag = rss( chan[ch].i_prmt, chan[ch].q_prmt);
  dith_mag   = rss( chan[ch].i_dith, chan[ch].q_dith);

  if (( prompt_mag > acq_thresh) || (dith_mag > acq_thresh)) 
  {
    chan[ch].n_thresh++;
//    printf( "ch_confirm: above thresh, prompt: %d, dith: %d, acq_thresh: %d\n", 
//      prompt_mag, dith_mag, acq_thresh);
  }
//  else
//  {
//    printf( "ch_confirm: below thresh, prompt: %d, dith: %d, acq_thresh: %d\n", 
//      prompt_mag, dith_mag, acq_thresh);
//    printf( "ch_confirm: chan[%d].i_prmtpt =  %d, chan[%d].q_prmtpt = %d\n", 
//      ch, chan[ch].i_prmtpt, ch, chan[ch].q_prmtpt);
//    getchar();  
//  }    

//
// channel need to be confirmed 'confirm_m' (10) times
// before being promoted to state 'pull_in'
//
//  printf( "ch_confirm: chan[ch].i_confirm: %d, confirm_m: %d\n",
//    chan[ch].i_confirm, confirm_m);

  if ( chan[ch].i_confirm == confirm_m)
  {
//    printf( "ch_confirm: chan[ch].n_thresh: %d, n_of_m_thresh: %d\n",
//      chan[ch].n_thresh, n_of_m_thresh);
//    getchar();
    if ( chan[ch].n_thresh >= n_of_m_thresh)
    {
      chan[ch].state   = pull_in;
      chan[ch].ch_time = 0;
      chan[ch].sum     = 0;
      chan[ch].th_rms  = 0;
    }
    else 
      chan[ch].state = acquisition;   // fall back to acquisition state
  }
  chan[ch].i_confirm++;
  return;
}

/*******************************************************************************
FUNCTION fix_atan2( long y, long x)
RETURNS  long integer

PARAMETERS 
      x  in phase fixed point value
      y  quadrature fixed point value

PURPOSE
      This function computes the fixed point arctangent represented by
      x and y in the parameter list
      1 radian = 16384 = 2^14
      based on the power series  f - f^3*2/9

WRITTEN BY
  Clifford Kelley
  Fixed for y==x  added special code for x==0
*******************************************************************************/

// pi/2 * 16384
#define SCALED_PI_ON_2  25736L
// pi * 16384
#define SCALED_PI       51472L
// pi * 16384
#define SCALED_2PI     102944L

inline long fix_atan2( long y, long x)
{
  long result, n, n3;
  
  if ((x == 0) && (y == 0))
    return (0); // invalid case

  if ( x > 0 && x >= labs( y))
  {
     n  = (y << 14) / x;
     n3 = ((((n*n) >> 14) * n) >> 13) / 9;
     result = n-n3;
  }
  else if( x <= 0 && -x >= labs( y))
  {
     n  = (y << 14) / x;
     n3 = ((((n*n) >> 14)*n) >> 13)/9;
     if ( y > 0) 
       result = n - n3 + SCALED_PI;
     else
       result = n - n3 - SCALED_PI;
  }
  else if( y > 0 &&  y > labs( x))
  {
     n  = (x << 14) / y;
     n3 = ((((n*n) >> 14) * n) >> 13) / 9;
     result = SCALED_PI_ON_2 - n + n3;
  }
  else if( y < 0 && -y > labs( x))
  {
     n  = (x << 14) / y;
     n3 = ((((n*n) >> 14) * n) >> 13) / 9;
     result = -n + n3 - SCALED_PI_ON_2;
  }
  return (result);
}

/*******************************************************************************
FUNCTION analyze_databit_histogram()

RETURNS  
  Start of bit transition (number between 0,...,19) 
  or -1 (too noise, back to acq)

PARAMETERS 
  ch : channel number

PURPOSE
  Find start of bit transition within 20 x 1 msec segments (bit 
  synchronization) using histogram method.
  
REFERENCES
  Krunvieda et al., A complete IF software GPS receiver: a tutorial about
    the detail, Data Fusion Corporation

WRITTEN BY
  G. Beyerle
  
TO DO

NOTES

*******************************************************************************/
static int analyze_databit_histogram( int ch)
{
  int i, maxhistidx, ret = -1; 
  unsigned int maxhist = 0, nofentry = 0;
  
//  printf( "bithist (%d):", ch);

  for ( i=0; i<NOFMSECPERDATABIT; i++)
  {
//    printf( " %d", chan[ch].databithist[i]);
    
    nofentry += chan[ch].databithist[i];

    if ( maxhist < chan[ch].databithist[i])
    {
      maxhist = chan[ch].databithist[i];
      maxhistidx = i;
    }

// --- clear array ---
    chan[ch].databithist[i] = 0;
  }  

//  printf( "\n");

// --- more than 50% of events in one bin -> synchronized! ---
  if ( 2 * maxhist > nofentry)
    ret = maxhistidx;

  return (ret);
}

/*******************************************************************************
FUNCTION ch_pull_in()

RETURNS  
  none

PARAMETERS 
  ch : channel number

PURPOSE
  Pull in carrier and code frequency by trying to track the signal with