gpsrcvr.c

A simple software GPS signal correlator

// ************** debug *******************
//  if ( ch==0)
//  {
//    printf( "trk: (%d) Ip=%4d, Qp=%4d, Id=%4d, Qd=%4d, r/lk:%.2f, d/lk:%.2f\n", 
//      chan[ch].t_count, 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].bit = ( chan[ch].i_prmt_sum + chan[ch].i_dith_sum > 0) ? 1 : 0;
    chan[ch].bit = ( chan[ch].bitsign > 0) ? 1 : 0;

// see if we can find the preamble
    pream( ch, chan[ch].bit);   

    chan[ch].message[chan[ch].t_count] = chan[ch].bit;   // message[] : array of 0 & 1
    chan[ch].t_count++;

    write_to_file_navbit( ch);

//    if ( chan[ch].t_count % 5 == 0)
//    {
//      chan[ch].avg = chan[ch].sum / 10;
//      chan[ch].sum = 0;
//    }
//
//    chan[ch].q_dith_20 = 0;
//    chan[ch].q_prmt_20 = 0;
//    chan[ch].i_dith_20 = 0;
//    chan[ch].i_prmt_20 = 0;
  } // --- if ( chan[ch].ms_count >= 19) ---

//  one data frame (1500 bits) completed
  if ( chan[ch].t_count == 1500)
  {
    chan[ch].n_frame++;
    chan[ch].t_count = 0;
  }

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

  chan[ch].old_i_sum = i_sum;

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

  return;
}

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

PARAMETERS

  ch   channel number (0-11)
  bit  the latest bit from the satellite

PURPOSE
  This function finds the preamble in the navigation message

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
void pream( char ch, char bit)
{
  unsigned int parity, parity1;     // int replaced by unsigned int (GB-020217)
  unsigned long TOWs, sum, pinv, dinv;
  static unsigned long pream = 0x22c00000L,    // 0x8b << 22
                       fifo0[13], 
                       fifo1[13];
  static unsigned long pb1 = 0xbb1f3480L, 
                       pb2 = 0x5d8f9a40L, 
                       pb3 = 0xaec7cd00L,
                       pb4 = 0x5763e680L, 
                       pb5 = 0x2bb1f340L, 
                       pb6 = 0xcb7a89c0L;

//
//  fifo0 = 0x1,0x3,0x7,...,0x1fffffff,
//
  if ( fifo1[ch] & 0x20000000L)  
    fifo0[ch] = (fifo0[ch] << 1) + 0x1;
  else 
    fifo0[ch] = fifo0[ch] << 1;
    
  fifo1[ch] = (fifo1[ch] << 1) + bit;

  sum = (pream^fifo0[ch]) & 0x3fc00000L;

  if ( sum == 0 || sum == 0x3fc00000L)
  {
//
//  preamble bit pattern found:
//  bit 22-29 are 0x8b or 0x74 (= 0x8b^0xff)
//
    dinv = 0;
    if ( sum == 0x3fc00000UL) 
      dinv = 0x3;
    parity = (xors( fifo0[ch] & pb1) << 5) + 
             (xors( fifo0[ch] & pb2) << 4) +
             (xors( fifo0[ch] & pb3) << 3) + 
             (xors( fifo0[ch] & pb4) << 2) +
             (xors( fifo0[ch] & pb5) << 1) +  
              xors( fifo0[ch] & pb6);
    pinv = 0;

    if ( fifo1[ch] & 0x40000000L) 
      pinv = 0xffffffffL;

    if ( parity == (fifo0[ch] & 0x3f))
    {

      parity1 = (xors( fifo1[ch] & pb1) << 5) + 
                (xors( fifo1[ch] & pb2) << 4) +
                (xors( fifo1[ch] & pb3) << 3) + 
                (xors( fifo1[ch] & pb4) << 2) +
                (xors( fifo1[ch] & pb5) << 1) + 
                 xors( fifo1[ch] & pb6);

      if ( parity1 == (fifo1[ch] & 0x3f) && ((fifo1[ch] & 0x3) == dinv ))
      {
      
//        printf("preamble found!\n");
//        getchar();
      
        chan[ch].sfid = (int)(((fifo1[ch]^pinv) & 0x700) >> 8);
        if ( chan[ch].sfid == 1)     // synchronize on subframe 0 if TOW matches
        {                            // clock within 300 seconds

          TOWs = ((fifo1[ch]^pinv) & 0x3fffffffL) >> 13;

          d_tow = clock_tow - TOWs * 6 + 5;

          if ( labs( d_tow) < 300)
          {
            chan[ch].offset = chan[ch].t_count - 59;
            ch_epoch_load( ch, (0x1f & ch_epoch_chk( ch)) | 0xa00);  ///// cwk
            if ( chan[ch].offset < 0) 
              chan[ch].offset += 1500;
            chan[ch].tr_bit_time = TOWs * 300 - 240;
            chan[ch].TOW         = TOWs;
            chan[ch].tow_sync    = 1;
            thetime              = thetime - d_tow;
            clock_tow            = TOWs * 6 - 5;
          }
        }

// allow resync on other subframes if TOW matches clock to 3 seconds
// this should improve the re-acquisition time
        if ( chan[ch].sfid > 1 && chan[ch].sfid < 6)
        {
          TOWs = ((fifo1[ch]^pinv) & 0x3fffffffL) >> 13;

          d_tow = clock_tow - TOWs * 6 + 5;

          if ( labs( d_tow) < 3)
          {
            chan[ch].offset = chan[ch].t_count - 59 - (chan[ch].sfid - 1) * 300;
            ch_epoch_load( ch, (0x1f & ch_epoch_chk( ch)) | 0xa00);  ///// cwk
            if ( chan[ch].offset < 0) 
              chan[ch].offset += 1500;
            chan[ch].tr_bit_time = TOWs * 300 - 240;
            chan[ch].tow_sync    = 1;
          }
        }
      }
    }
  }  // --- if ( sum == 0 || sum == 0x3fc00000L) ---

//  a 1500 bit frame of data is ready to be read
  if (( chan[ch].t_count-chan[ch].offset) % 1500 == 0) 
    chan[ch].frame_ready = 1;

  return;  
}

/*******************************************************************************
FUNCTION xors()
RETURNS  Integer

PARAMETERS
        pattern  a 32 bit data

PURPOSE
        check the pattern
WRITTEN BY
  Clifford Kelley

*******************************************************************************/
int xors( long pattern)
{
  int count = 0, i;
  
  pattern = pattern >> 6;
  for ( i=0; i<=25; i++)
  {
    count += (int)( pattern & 0x1);
    pattern = pattern >> 1;
  }
  count = count % 2;
  return (count);
}

/*******************************************************************************
FUNCTION sign()
RETURNS  Integer

PARAMETERS Long

PURPOSE
       This function returns +1 when parameter is positive
            0 when zero
           -1 when negative

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
inline int sign( long data)
{
  int result;
  
  if      ( data > 0 ) 
    result = 1;
  else if ( data == 0 ) 
    result = 0;
  else if ( data < 0 ) 
    result = -1;

  return(result);
}

/*******************************************************************************
FUNCTION bsign()
RETURNS  Integer

PARAMETERS long integer

PURPOSE

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
inline int bsign( long data)
{
  int result;
  
  if ( data > 0 ) 
    result = 1;
  else  
    result = 0;
  return (result);
}

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

PARAMETERS None.

PURPOSE

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
inline int  bit_test( int data, char bit_n)
{
  return (data & test[bit_n]);
}

/*******************************************************************************
FUNCTION near_int()
RETURNS  long

PARAMETERS double

PURPOSE
  This function finds the nearest integer of a double

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
long near_int( double input)
{
  long result;
  
  if ( input > 0.0 )
    result = (long) (input + 0.5);
  else 
    result = (long) (input - 0.5);

  return result;
}


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

PARAMETERS None.

PURPOSE  To convert velocity from ecef to local level axes

WRITTEN BY
  Clifford Kelley

*******************************************************************************/
void velocity( void)
{
  double  pt,pb,dxn,dyn,dzn,dn_mag,dxe,dye,de_mag,dxu,dyu,dzu;
  double  north,east,up;

  pt = (a*a-b*b) * sin( rp_llh.lat)*cos( rp_llh.lat);

//  printf("vor sqrt() *** 3 ***\n");
  pb = sqrt( a*a * cos( rp_llh.lat) * cos( rp_llh.lat) + 
             b*b * sin( rp_llh.lat) * sin( rp_llh.lat));
//  printf("nach sqrt() *** 3 ***\n");

  dxn =  a * a * pt / pow( pb, 3) * cos( rp_llh.lat) * cos( rp_llh.lon) - 
        (a * a / pb + rp_llh.hae) * sin( rp_llh.lat) * cos( rp_llh.lon);
  dyn =  a * a * pt / pow( pb, 3) * cos( rp_llh.lat) * sin( rp_llh.lon) - 
        (a * a / pb + rp_llh.hae) * sin( rp_llh.lat) * sin( rp_llh.lon);
  dzn =  b * b * pt / pow( pb, 3) * sin( rp_llh.lat) + 
        (b * b / pb + rp_llh.hae) * cos( rp_llh.lat);

//  printf("vor sqrt() *** 4 ***\n");
  dn_mag = sqrt( dxn * dxn + dyn * dyn + dzn * dzn);  // north magnitude
//  printf("nach sqrt() *** 4 ***\n");

  if ( dn_mag == 0.0) 
    north = 0.0;
  else 
    north = (rpvt.xv*dxn+rpvt.yv*dyn+rpvt.zv*dzn)/dn_mag;

  dxe = -(a*a/pb + rp_llh.hae) * cos( rp_llh.lat) * sin( rp_llh.lon);
  dye =  (a*a/pb + rp_llh.hae) * cos( rp_llh.lat) * cos( rp_llh.lon);

//  printf("vor sqrt() *** 5 ***\n");
  de_mag = sqrt(dxe*dxe+dye*dye);             // east magnitude
//  printf("nach sqrt() *** 5 ***\n");

  if ( de_mag == 0.0) 
    east = 0.0;
  else 
    east = (rpvt.xv * dxe + rpvt.yv * dye) / de_mag;

  dxu = cos( rp_llh.lat) * cos( rp_llh.lon);
  dyu = cos( rp_llh.lat) * sin( rp_llh.lon);
  dzu = sin( rp_llh.lat);

  up = rpvt.xv*dxu + rpvt.yv*dyu + rpvt.zv*dzu;   // up magnitude

//  printf("vor sqrt() *** 6 ***\n");
  speed   = sqrt( north*north + east*east);
//  printf("nach sqrt() *** 6 ***\n");

  heading = atan2( east, north);
  if ( out_vel && stream) 
    fprintf( stream, "vel north=%lf,east=%lf,up=%lf\n", north, east, up);

  return;
}

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

PARAMETERS None.
  float BandWidth   : natural loop frequency
  float DampRatio   : damping ratio
  float Gain        : loop gain
  float *par1 
  float *par2

PURPOSE  Calculate tracking loop parameters from bandwidth and gain
------------------------------------------------------------------ */
static void calc_track_par( float BandWidth, float DampRatio, float Gain,
  float *par1, float *par2)
{

// typical values
// loop gain
//   CodGain  = 50e-3;    50 if chips are used
//   CarGain  = 4*pi*100;  
// band width
//   CodBW    =  1;  
//   CarBW    = 20; 
// damping ratio
//   DampRatio = 0.707 : PLL is critically damped

  float wT, c1, c2;
  float tupd = UPDATETIME;

// wT is natural frequency times update time
  wT = 2.0 * BandWidth / (DampRatio + 1.0 / (4.0*DampRatio)) * tupd;

  c1 = 1.0 / Gain * 8.0 * DampRatio*wT / (4.0 + 4.0 * DampRatio * wT + wT*wT);
  c2 = 1.0 / Gain * 4.0 * wT*wT        / (4.0 + 4.0 * DampRatio * wT + wT*wT);

  *par1 = (c1 + c2) / tupd;
  *par2 = c1 / tupd;

//  printf("calc_track_par: Gain = %e \n", Gain);
//  printf("calc_track_par: wT = %e, BandWidth = %e \n", wT, BandWidth);
//  printf("calc_track_par: c1 = %e, c2 = %e, tupd = %e\n", c1, c2, tupd);
//  printf("calc_track_par: par1 = %e, par2 = %e, tupd = %e\n", *par1, *par2, tupd);
//  getchar();

  return;
}


static void init_tracking_par( void)
{

//  tracking loops in tracking state
  calc_track_par( bw_car_track, damp_car_track, gain_car_track, 
    &car_PLL_track_p1, &car_PLL_track_p2);
  calc_track_par( bw_cod_track, damp_cod_track, gain_cod_track, 
    &cod_PLL_track_p1, &cod_PLL_track_p2);

//  tracking loops in pul