correlator.c

OpenGPSSim是一个gps模拟程序

    break;

  case 0x84: case 0x88: case 0x8c: case 0x90: case 0x94: case 0x98:  
  case 0x9c: case 0xa0: case 0xa4: case 0xa8: case 0xac: case 0xb0:
    n = (adr - 0x84) / 4;
    ret = Regs[n].i_track;
//    if ( n == 0)
//      printf("ch = %d, Id = %d\n", n, ret);
    break;

  case 0x85: case 0x89: case 0x8d: case 0x91: case 0x95: case 0x99: 
  case 0x9d: case 0xa1: case 0xa5: case 0xa9: case 0xad: case 0xb1:
    n = (adr - 0x85) / 4;
    ret = Regs[n].q_track;
//    if ( n == 0)
//      printf("ch = %d, Qd = %d\n", n, ret);
    break;

  case 0x86: case 0x8a: case 0x8e: case 0x92: case 0x96: case 0x9a: 
  case 0x9e: case 0xa2: case 0xa6: case 0xaa: case 0xae: case 0xb2:
    n = (adr - 0x86) / 4;
    ret = Regs[n].i_prompt;
//    if ( n == 0)
//      printf("ch = %d, Ip = %d\n", n, ret);
    break;

  case 0x87: case 0x8b: case 0x8f: case 0x93: case 0x97: case 0x9b: 
  case 0x9f: case 0xa3: case 0xa7: case 0xab: case 0xaf: case 0xb3:
    n = (adr - 0x87) / 4;
    ret = Regs[n].q_prompt;

    Accum_Status_A.new_accum_data &= ~(0x1 << n);  // q_prompt should be read last, clears status bit
    break;

  default:
    printf( "from_gps: attempt to read from address %d (%x)\n", adr, adr);
    exit(-1);
  }  

  return ret;
}

//
// init software correlator
//
void init_software_correlator( void)
{
  int i;
  long l;
  FILE *Fp;

//  activate channels
  for ( i=0; i<NofChanActive; i++)
    Regs[i].active = 1;
    
  for ( i=0; i<NOFCHAN; i++)
  {
    Regs[i].carrier_dco_incr = 0x1f7b1b9L;      // 33010105 * 42.57475 mHz = 1405396.968 Hz
//    Carrier_DCO_Frq[i]       = 1.405396825e6;

    Regs[i].code_dco_incr    = 0x16ea4a8L;      // 24028328 * 42.57475 mHz = 1023000.058 Hz
//    Code_DCO_Frq[i]          = 1.022999968e6 / 1023.0;   // ~1 kHz
  }

  return;
}


//
//  output:
//
//    GPS signal (-3,-1,1,3)
//
int get_raw_signal( int prn)
{
//  int code, carrier;
//  static double time = 0.0;
//  static int navidx = 0,
//             codeidx = 0;
  int sgn;

//
// 'carrier_counter'
//
// counter of carrier cycles, 2^31 corresponds to 1/1.405 MHz =  0.712 usec
// (1.405 MHz is down-converted carrier frq, sampling frq is 5.714 MHz;)
// counter is incremented by 2^31 * 1.405 MHz / 5.714 MHz = 528038944; 
// frq resolution: 1.405 MHz / 528038944 = 2.66 mHz
//
//   3         2         1         0
//  10987654321098765432109876543210 
//  ABBBCCCCCCCCCCCCCCCCCCCCCCCCCCCC
//
// 'code_counter'
//
// counter of code cycles, 2^22 corresponds to 1/1.023 MHz =  0.977 usec
// counter is incremented by 2^22 * 1.023 MHz / 5.714 MHz = 750923
// frq resolution: 1.023 MHz / 750923 = 1.36 Hz
//
//   3         2         1         0
//  10987654321098765432109876543210 
//  ABBBCCCCCCCCCCCCCCCCCCCCCCCCCCCC

  static int nidx = 0;           // 0,...,1499       1500 navigation message bits
//  static int ridx = 0; 
  int didx,                      // 0,...,1022       1023 C/A code chips
      cidx;                      // 0,...,7          8 steps per carrier cycle
  int tmpnav = 1;
  static int epoch_counter = 0;  // 0,...,19  1 msec epochs
//             chip_counter = 0;   // 0,...,1022  
  static unsigned long carrier_counter       = 0,
                       carrier_counter_step  = 528038944L,
                       code_counter          = 0,
                       code_counter_step     = 750923L;

//
//  C/A signal: navigation data, C/A code and carrier
//
  didx = code_counter >> 22;           // bits 22-31
  cidx = carrier_counter >> 28;        // bits 28-31

//  printf("cidx = %d, didx = %d\n", cidx, didx);
//  getchar();

//  assert( didx >= 0 && didx < 1023);

//  sgn = nav[nidx] * CACode[prn][didx] * Carrier_DCO_I[cidx];

//
//  values: -2,-1,1,2
//
  sgn = tmpnav * CACode[prn][didx] * Carrier_DCO_I[cidx];

//
//  Noise: Gaussian white noise, integer, zero mean, stddev 1000
//
//  sgn += Noise[ridx++] / 50;           // stddev reduced 20
//  ridx = ridx % NOISELEN;
//  sgn /= 4;                            // stddev reduced  5

  if (sgn > 6) sgn = 6;
  if (sgn < -6) sgn = -6;

//
//  increment by one time step
//
  carrier_counter += carrier_counter_step;
  code_counter    += code_counter_step;

  if ( carrier_counter & (0x1L << 31))   // if bit 31 set -> one cycle is completed
    carrier_counter &= 0x7fffffff;   // clear bit 31
  
//  code sequence finished?
  if ( (code_counter >> 22) == 1023)
  {
    code_counter &= 0x3fffff;        // clear bits 22-31
    epoch_counter += 1;   // 1 msec  
  }

//  message bit corresponds to 20 msec
  if ( epoch_counter == 20)
  {
    nidx += 1;
    epoch_counter = 0;
  }

  return sgn;
}

#ifdef OGSDEBUG
int dbg_buf1[6000],
    dbg_buf2[6000],
    dbg_buf3[6000],
    dbg_buf4[6000];
#endif

extern FILE *FpIn;

//
// run software correlator
//
int run_software_correlator( void)
{
  int ch, j, loop = 1, res = 1,
      sgn, i_lo, q_lo,
      ca_prompt, ca_earlylate,
      didx, didxlate, didxearly, cidx;

  static long bufpos = 0L;
  static int sgnidx = -1, SavBuf;

  static int dump[NOFCHAN] = {0,0,0,0,0,0,0,0,0,0,0,0};

//
//  loop 'til next dump
//  time step is sampling frequency (5.714 MHz <-> 175 nsec)
//
  while ( loop)
  {

    if ( sgnidx < 0)
    {
      SavBuf = Buffer[bufpos++];
      sgnidx = 3;
    }  

    if ( bufpos >= BufEnd)
    {
      printf("fill data buffer...                                             \n");
      update_readbuffer();
      bufpos = 0L;
    }  

    if ( !FpIn)
      return (0);

// extract 2 bits from 32 bit long and map from [0,1,2,3] to [-3,-1,1,3]
    sgn = ((SavBuf >> 2*sgnidx) & 0x3) * 2 - 3;
    sgnidx -= 1;

//    dbg_nof_smpl += 1;

#ifdef OGSDEBUG
    dbg_buf1[dbg_bufidx] = sgn;
#endif

//    BUFFERPTR += 1;   // global variable - debug only
    
//    if ( bufpos >= BufferLen)
//      return (0);

//
//  loop over all 12 channels
//
    for ( ch=0; ch<NOFCHAN; ch++)
    {

//  is channel switched on?
      if ( Regs[ch].active)
      {
//        prn = Regs[ch].prn;   // 1,...,32

//
//  increment carrier and code cycle counter
//
        Carrier_Cycle[ch] += Regs[ch].carrier_dco_incr;
        Code_Cycle[ch]    += Regs[ch].code_dco_incr;

//
//  carrier DCO: 2^27 = 0x8000000 corresponds to 1 carrier cycle (1.4 MHz)
//
        if ( Carrier_Cycle[ch] & (0x1 << 27))
        {
          Carrier_Cycle[ch] &= ~(0x1f << 27);   // clear top 5 bits
          Regs[ch].carrier_cycle_counter += 1; 
        }

//
//  code DCO: 2^26 = 0x4000000 corresponds to 1/2 chip (2*1.023 MHz)
//
        if ( Code_Cycle[ch] & (0x1 << 26))
        {
          Code_Cycle[ch] &= ~(0x3f << 26);   // clear top 6 bits
          HalfChip_Counter[ch] += 1;
        }

        if ( HalfChip_Counter[ch] == 2046)
        {
          dump[ch] = 1;                   // 1 code period finished -> dump!
          HalfChip_Counter[ch] = 0;

#ifdef OGSDEBUG
          FILE* dbgfp = fopen( "c:/tmp/testdata.dat","w");
          if ( !dbgfp)
          {
            printf( "ru_vrt_cr: error opening data file c:/tmp/testdata.dat\n");
            exit(-1);
          }
          fwrite( &dbg_bufidx, sizeof( int), 1, dbgfp);
          fwrite( dbg_buf1, sizeof( int), dbg_bufidx, dbgfp);
          fwrite( &dbg_bufidx, sizeof( int), 1, dbgfp);
          fwrite( dbg_buf2, sizeof( int), dbg_bufidx, dbgfp);
          fwrite( &dbg_bufidx, sizeof( int), 1, dbgfp);
          fwrite( dbg_buf3, sizeof( int), dbg_bufidx, dbgfp);
          fwrite( &dbg_bufidx, sizeof( int), 1, dbgfp);
          fwrite( dbg_buf4, sizeof( int), dbg_bufidx, dbgfp);
          fclose( dbgfp);
          printf( "file closed!\n"); 

          dbg_bufidx = 0;

          getchar();
#endif

        }  // --- if ( HalfChip_Counter[ch] == 2046) ---

//
//  index in sin/cos look-up table (8-phase)
//
        cidx = (Carrier_Cycle[ch] >> 24) & 0x7;

        didx      = HalfChip_Counter[ch];             // now 0,...,2046     
        didxearly = didx + 2046 + 1;                  // 1/2 chip (avoid neg numbers)
        didxlate  = didx + 2046 - 1;

        didx      = (didx % 2046) / 2;
        didxearly = (didxearly % 2046) / 2;
        didxlate  = (didxlate % 2046) / 2;

// code
//        ca_prompt = CACode[prn][didx];   // i=0,...,1022
        ca_prompt = CACode[chan[ch].prn][didx];   // i=0,...,1022

#ifdef OGSDEBUG
        dbg_buf2[dbg_bufidx] = ca_prompt;
#endif

//
//  code transition from index 1022 to 0 -> DUMP event
//  actually DUMP event occurs only if Regs[ch].code_slew_counter != 2047;
//  we ignore that
//

        if ( Accum_Status_C.early_late & (0x1 << ch))
        {
//          ca_earlylate = CACode[prn][didxlate];
          ca_earlylate = CACode[chan[ch].prn][didxlate];
          printf( "late track!\n");
          getchar();
        }  
        else
        {
//          ca_earlylate = CACode[prn][didxearly];
          ca_earlylate = CACode[chan[ch].prn][didxearly];
//          printf( "early track!\n");
//          getchar();
        }  

//
//  dump event!
//
        if ( dump[ch])
        {
          dump[ch] = 0;

//  adjust code cycle
//          Code_DCO_Cycle[ch] += Regs[ch].code_slew_counter / 2046;  // 1 cycle = 2046 half chips
          HalfChip_Counter[ch] += Regs[ch].code_slew_counter;

// value valid just for one DUMP period
          Regs[ch].code_slew_counter = 0;                          

// dump sum to accumulator registers ...
          Regs[ch].i_prompt = Tmp_I_Prompt[ch];
          Regs[ch].i_track  = Tmp_I_Track[ch];
          Regs[ch].q_prompt = Tmp_Q_Prompt[ch];
          Regs[ch].q_track  = Tmp_Q_Track[ch];

// --------- debugging -----
//          Regs[ch].samples_per_sum = Samples_Per_Sum[ch];
//          Sample_Counter_Start[ch] = Sample_Counter_End[ch]+1;
//          Sample_Counter_End[ch]   += Samples_Per_Sum[ch];
//          Regs[ch].sample_counter  = Sample_Counter_End[ch];
// --------- debugging -----

// ... and clear
          Tmp_I_Prompt[ch] = 0;
          Tmp_I_Track[ch]  = 0;
          Tmp_Q_Prompt[ch] = 0;
          Tmp_Q_Track[ch]  = 0;

// --------- debugging -----
//          Samples_Per_Sum[ch] = 0;
// --------- debugging -----

// new accumulator data available
          Accum_Status_A.new_accum_data |= (0x1 << ch);
          Accum_Status_A.accum_int = 1;
          loop = 0;
        }  // --- if ( dump[ch]) ---

        Regs[ch].carrier_dco_phase = (long)( Carrier_DCO_Cycle[ch] * 1024);  // resolution is 1/1024 cycles

        i_lo = Carrier_DCO_I[cidx];
        q_lo = Carrier_DCO_Q[cidx];

#ifdef OGSDEBUG
        dbg_buf3[dbg_bufidx] = i_lo;
        dbg_buf4[dbg_bufidx] = q_lo;
        dbg_bufidx += 1;
#endif

//  values
//    sgn          : +/-1, +/-3
//    i_lo,q_lo    : +/-1, +/-2
//    ca_prompt    : +/-1
//    ca_earlylate : +/-1 or +/-2, 0
//    product      : +/-1, +/-2, +/-3, +/-6

        Tmp_I_Prompt[ch] += sgn * i_lo * ca_prompt;
        Tmp_I_Track[ch]  += sgn * i_lo * ca_earlylate;
        Tmp_Q_Prompt[ch] += sgn * q_lo * ca_prompt;
        Tmp_Q_Track[ch]  += sgn * q_lo * ca_earlylate;
        
// --- debugging ---
//        Samples_Per_Sum[ch] += 1;
// --- debugging ---
        
      }  // --- if ( Regs[ch].active) ---
    }  // --- for ( ch=0; ch<NOFCHAN; ch++) ---
  }  // --- while ( loop) ---

  return (res);
}

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