correlator.c

OpenGPSSim是一个gps模拟程序

/* ************************************************************************ 
   *                                                                      *
   *                          GPS Simulation                              *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *    Module:   correlator.cpp                                          *
   *                                                                      *
   *   Version:   0.1                                                     *
   *                                                                      *
   *      Date:   02.03.02                                                *
   *                                                                      *
   *    Author:   G. Beyerle                                              *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * Copyright (C) 2002-2006  Georg Beyerle                               *
   *                                                                      *
   * This program is free software; you can redistribute it and/or modify *
   * it under the terms of the GNU General Public License as published by *
   * the Free Software Foundation; either version 2 of the License, or    *
   * (at your option) any later version.                                  *
   *                                                                      *
   * This program is distributed in the hope that it will be useful,      *
   * but WITHOUT ANY WARRANTY; without even the implied warranty of       *
   * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the        *
   * GNU General Public License for more details.                         *
   *                                                                      *
   * You should have received a copy of the GNU General Public License    *
   * along with this program; if not, write to the Free Software          *
   * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.            *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   * The files 'gpsfuncs.cpp', 'gpsrcvr.cpp' and 'gp2021.cpp' are modi-   *
   * fied versions of the files with the same name from Clifford Kelley's * 
   * OpenSourceGPS distribution. The unmodified files can be obtained     *
   * from http://www.home.earthlink.net/~cwkelley                         *
   *                                                                      *
   * -------------------------------------------------------------------- *
   *                                                                      *
   *             Software correlator (based on GP2021 chip)               *
   *                                                                      *
   ************************************************************************ */

/* ******************************* changes ********************************

   dd.mm.yy -

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

/* ******************************** to do *********************************

   02-06-02 : check chan[ch].prn <-> Regs[].prn

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

/* -------------------------------- define -------------------------------- */

#undef OGSDEBUG

/* ------------------------------- includes ------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#ifdef __TURBOC__
# include <conio.h>
#endif
#include <time.h>

#include "ogsdefine.h"
#include "ogsstructs.h"
#include "ogsextern.h"
#include "ogsprototypes.h"
#include "ogslibrary.h"

/* ------------------------------- defines -------------------------------- */

/* ------------------------------- globals -------------------------------- */

static int Carrier_DCO_I[] = {-1,1,2, 2, 1,-1,-2,-2};
static int Carrier_DCO_Q[] = { 2,2,1,-1,-2,-2,-1, 1};

//  somewhat better resolution ...
//int Carrier_DCO_I[] =  {25,71,106,126,126,106, 71, 25,-25,-71,-106,-126,-126,-106,-71,-25}  // 16-phase 256 level
//int Carrier_DCO_Q[] =  {126,106, 71, 25,-25,-71,-106,-126,-126,-106,-71,-25,25,71,106,126}  // 16-phase 256 level

typedef struct
{
   int    active,
          inhibited; 
   int    satcntl,
          sourcesel,
          preset_update,  // hi: preset, lo: update
          track_sel;      // 0:early, 1:late, 2:dither, 3:early-minus-late
   int    prn;            // satellite id
   long   i_prompt,       // accumulation registers
          i_track,
          q_prompt,
          q_track;
   unsigned int samples_per_sum;
   unsigned long sample_counter;
   long   carrier_cycle_counter;  // # of positive going zero crossings of i_lo
   long   code_phase_counter,
          code_slew_counter;  // slew code_DCO by XXX half chips, 0,...,2047
   long   carrier_dco_incr,   // 26 bit, frq res. is 42.57475 mHz
          carrier_dco_phase,  // carrier cycle fraction / phase in 2pi/1024 increments
          code_dco_phase,
          code_dco_incr;
   int    epoch_counter,
          epoch_1ms,          // 0,...,19; 1 msec (1 C/A) code epoch
          epoch_20ms;         // 0,...,49; 20 msec (20 C/A, 1 data bit) epoch
} REGS;

static REGS Regs[NOFCHAN];

typedef struct cntlregs
{
  int test_control,
      system_setup,
      reset_control;
} CNTLREGS;

CNTLREGS CntlRegs;

typedef struct accum_stat_a
{
  int accum_int, 
      new_accum_data; 
} ACCUM_STAT_A;

ACCUM_STAT_A Accum_Status_A;

typedef struct accum_stat_b
{
  int tic,
      meas_int,
      missed_accum; 
} ACCUM_STAT_B;

ACCUM_STAT_B Accum_Status_B;

typedef struct accum_stat_c
{
  int early_late; 
} ACCUM_STAT_C;

ACCUM_STAT_C Accum_Status_C;

typedef struct meas_stat_a
{
  int tic,
      meas_int,
      missed_meas_data;
} MEAS_STAT_A;

MEAS_STAT_A Meas_Status_A;

static long Carrier_Cycle[NOFCHAN],
            Code_Cycle[NOFCHAN];

static int HalfChip_Counter[NOFCHAN];

static long Tmp_I_Prompt[NOFCHAN],
            Tmp_I_Track[NOFCHAN],
            Tmp_Q_Prompt[NOFCHAN],
            Tmp_Q_Track[NOFCHAN];

// ------------ debugging -------------
//unsigned int  Samples_Per_Sum[NOFCHAN];
//unsigned long Sample_Counter_Start[NOFCHAN];
//unsigned long Sample_Counter_End[NOFCHAN];
// ------------ debugging -------------

static double Carrier_DCO_Cycle[NOFCHAN];

extern char *Buffer;      // data input buffer
extern long BufEnd;       // last position in data input buffer 

/* -------------------------- prototypes (global) ------------------------- */

void process_data( int);
void update_readbuffer( void);

/* ------------------------------ procedures ------------------------------ */

//
//  interface to software GP2021
//

int g2load_to_prn( int g2_load)
{
  int k, ret = 0;
  int g2_load_list[] = {
    0x3f6,0x3ec,0x3d8,0x3b0, 0x4b, 0x96,0x2cb,0x196,0x32c,0x3ba,
    0x374,0x1d0,0x3a0,0x340,0x280,0x100,0x113,0x226, 0x4c, 0x98,
    0x130,0x260,0x267,0x338,0x270, 0xe0,0x1c0,0x380,0x22b, 0x56,
    0xac, 0x158};

  for ( k=0; k<32; k++)
  {
    if ( g2_load == g2_load_list[k])
    {
      ret = k+1;
      break;
    }
  }

  if (!ret)
  {
    printf( "g2load_to_prn: unknown G2_LOAD 0x%x\n", g2_load);
    exit(-1);
  }
  return (ret);
}

void ch_carrier( char ch, long freq)
{
  Regs[ch].carrier_dco_incr = freq;
//  gotoxy(1,24);
//  printf("ch_carrier: carrier_dco_incr = %x (nominal: 0x1f7b1b9)", freq);
//  getchar();
  return;
}

void ch_code( char ch, long freq)
{
  Regs[ch].code_dco_incr = freq;
  return;
}

//
//  write to GP2021 registers
//
void to_gps( int adr, int data)
{
  int n, mask;
  long ldata;
  int g2_load;

  switch( adr)
  {
  case 0x0:  case 0x8:  case 0x10: case 0x18: case 0x20: case 0x28: 
  case 0x30: case 0x38: case 0x40: case 0x48: case 0x50: case 0x58:
    n = (adr - 0x0) / 8;
    Regs[n].satcntl   = data;
    Regs[n].sourcesel = (data & (0x1 << 10)) != 0;
    Regs[n].preset_update = (data & (0x1 << 12)) != 0;   // hi: preset, lo: update
    Regs[n].track_sel     = (data & (0x3 << 13)) >> 13;  // 0:early, 1:late, 2:dither, 3:early-minus-late
    g2_load = data & 0x3ff;
    Regs[n].prn = g2load_to_prn( g2_load);
//    printf( "to_gps: Regs[%d].track_sel = %d\n", n, Regs[n].track_sel);
//    getchar();
    break;

// write only if test mode is selected
//  case 0x1, 0x9, 0x11, 0x19, 0x21, 0x29, 0x31, 0x39, 0x41, 0x49, 0x51, 0x59:
//    n = (adr - 0x1) / 8;
//    Regs[n].code_phase_counter = data;
//    break;
  case 0x3:  case 0xb:  case 0x13: case 0x1b: case 0x23: case 0x2b: 
  case 0x33: case 0x3b: case 0x43: case 0x4b: case 0x53: case 0x5b:
    n = (adr - 0x3) / 8;
    ldata = data;
    Regs[n].carrier_dco_incr |= ((ldata << 16) & 0xf0000);
    break;

  case 0x4:  case 0xc:  case 0x14: case 0x1c: case 0x24: case 0x2c: 
  case 0x34: case 0x3c: case 0x44: case 0x4c: case 0x54: case 0x5c:
    n = (adr - 0x4) / 8;
    Regs[n].carrier_dco_incr |= data;
    break;

  case 0x5:  case 0xd:  case 0x15: case 0x1d: case 0x25: case 0x2d: 
  case 0x35: case 0x3d: case 0x45: case 0x4d: case 0x55: case 0x5d:
    n = (adr - 0x5) / 8;
    ldata = data;
    Regs[n].code_dco_incr |= ((ldata << 16) & 0xf0000);
    break;

  case 0x6:  case 0xe:  case 0x16: case 0x1e: case 0x26: case 0x2e: 
  case 0x36: case 0x3e: case 0x46: case 0x4e: case 0x56: case 0x5e:
    n = (adr - 0x6) / 8;
    Regs[n].code_dco_incr |= data;
    break;

  case 0x7: case 0xf: case 0x17: case 0x1f: case 0x27: case 0x2f: 
  case 0x37: case 0x3f: case 0x47: case 0x4f: case 0x57: case 0x5f:
    n = (adr - 0x7) / 8;
    Regs[n].epoch_1ms  = data & 0x1f;         // 0,...,19
    Regs[n].epoch_20ms = (data >> 8) & 0x3f;  // 0,...,49
    break;

  case 0x7c:
    CntlRegs.test_control = data;
    printf( "to_gps: write %d (0x%x) to test_control\n", data, data);
    break;

  case 0x7e:
    CntlRegs.system_setup = data;
    printf( "to_gps: write %d (0x%x) to system_setup\n", data, data);
    break;

  case 0x7f:
    CntlRegs.reset_control = data;
//    printf( "to_gps: write %d (0x%x) to reset_control\n", data, data);
    break;

  case 0x80:
// latch status of status flags into ACCUM_STATUS_x registers
//    printf( "to_gps: write %d (0x%x) to STATUS (0x80)\n", data, data);
//    getchar();
    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:
// code slew counter
    n = (adr - 0x84) / 4;
//    printf( "to_gps: write %d (0x%x) to CODE_SLEW_COUNTER (0x%x), channel %d\n", data, data, adr, n);
    Regs[n].code_slew_counter = data;
    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:
// accumulator reset
    n = (adr - 0x85) / 4;
//    printf( "to_gps: write %d (0x%x) to ACCUM_RESET (0x%x), channel %d\n", data, data, adr, n);
    mask = ~(0x1 << n);
    Accum_Status_A.new_accum_data &= mask;  // clear bit for that channel
    Accum_Status_B.missed_accum   &= mask;  
    Accum_Status_C.early_late     &= mask; 
    break;

  case 0xf0:
//    printf( "to_gps: write %d (0x%x) to IO_CONFIG (0xf0)\n", data, data);
    break;

  default:
    printf( "to_gps: attempt to write data %d (0x%x) to address %d (0x%x)\n", 
            data, data, adr, adr);
    exit(-1);
  }  
  return;
}

//
//  read from GP2021 registers
//
int from_gps( int adr)
{
  int n, ret;

  switch( adr)
  {
  case 0x0:  case 0x8:  case 0x10: case 0x18: case 0x20: case 0x28: 
  case 0x30: case 0x38: case 0x40: case 0x48: case 0x50: case 0x58:
    n = (adr - 0x0) / 8;
    ret = Regs[n].code_slew_counter;
    break;

  case 0x1:  case 0x9:  case 0x11: case 0x19: case 0x21: case 0x29: 
  case 0x31: case 0x39: case 0x41: case 0x49: case 0x51: case 0x59:
    n = (adr - 0x1) / 8;
    ret = Regs[n].code_phase_counter;
    break;

  case 0x2:  case 0xa:  case 0x12: case 0x1a: case 0x22: case 0x2a: 
  case 0x32: case 0x3a: case 0x42: case 0x4a: case 0x52: case 0x5a:
    n = (adr - 0x2) / 8;
    ret = Regs[n].carrier_cycle_counter & 0xffff;
    break;

  case 0x3:  case 0xb:  case 0x13: case 0x1b: case 0x23: case 0x2b: 
  case 0x33: case 0x3b: case 0x43: case 0x4b: case 0x53: case 0x5b:
    n = (adr - 0x3) / 8;
    ret = Regs[n].carrier_dco_phase;
    break;

  case 0x4:  case 0xc:  case 0x14: case 0x1c: case 0x24: case 0x2c: 
  case 0x34: case 0x3c: case 0x44: case 0x4c: case 0x54: case 0x5c:
    n = (adr - 0x4) / 8;
    ret = Regs[n].epoch_counter;
    break;

  case 0x5:  case 0xd:  case 0x15: case 0x1d: case 0x25: case 0x2d: 
  case 0x35: case 0x3d: case 0x45: case 0x4d: case 0x55: case 0x5d:
    n = (adr - 0x5) / 8;
    ret = Regs[n].code_dco_phase;
    break;

  case 0x6:  case 0xe:  case 0x16: case 0x1e: case 0x26: case 0x2e: 
  case 0x36: case 0x3e: case 0x46: case 0x4e: case 0x56: case 0x5e:
    n = (adr - 0x6) / 8;
    ret = (Regs[n].carrier_cycle_counter >> 16) & 0xf;
    break;

//  case 0x7: case 0xf: case 0x17: case 0x1f: case 0x27: case 0x2f: 
//  case 0x37: case 0x3f: case 0x47: case 0x4f: case 0x57: case 0x5f:
//    n = (adr - 0x7) / 8;
//    break;
//
  case 0x80:
    ret = Accum_Status_C.early_late;
    break;

  case 0x81:
    ret = (Meas_Status_A.tic << 13) | 
          (Meas_Status_A.meas_int << 12) | Meas_Status_A.missed_meas_data;
    break;

  case 0x82:
    ret = (Accum_Status_A.accum_int << 15) | Accum_Status_A.new_accum_data;
    break;

  case 0x83:
    ret = (Accum_Status_B.tic << 13) | 
          (Accum_Status_B.meas_int << 12) | Accum_Status_B.missed_accum;