www.pudn.com > opengpssim.zip > correlator.c
/* ************************************************************************
* *
* 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
#include
#include
#include
#ifdef __TURBOC__
# include
#endif
#include
#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;
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> 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 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