www.pudn.com > opengpssim.zip > gpsrcvr.c
/* ************************************************************************
* *
* GPS Simulation *
* *
* -------------------------------------------------------------------- *
* *
* Module: gpsrcvr.c *
* *
* Version: 0.1 *
* *
* Date: 17.02.02 *
* *
* Author: G. Beyerle *
* *
* -------------------------------------------------------------------- *
* *
* Copyright (c) 1996-2001 Clifford Kelley. All Rights Reserved. *
* 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 *
* *
* -------------------------------------------------------------------- *
* *
* GPS receiver *
* *
************************************************************************ */
/* ******************************* changes ********************************
dd.mm.yy -
************************************************************************ */
/***********************************************************************
GPS RECEIVER (GPSRCVR) Ver. 1.02
12 Channel All-in-View GPS Receiver Program based on Mitel GP2021
chipset
Clifford Kelley cwkelley@earthlink.net
Copyright (c) 1996-2001 Clifford Kelley. All Rights Reserved.
This LICENSE must be included with the GPSRCVR code.
***********************************************************************/
/*
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
CONDITIONS
1. Redistributions of GPSRCVR source code must retain the above copyright
notice, this list of conditions, and the following disclaimer.
2. Redistributions in binary form must contain the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
3. All modifications to the source code must be clearly marked as
such. Binary redistributions based on modified source code must be
clearly marked as modified versions in the documentation and/or other
materials provided with the distribution.
4. Notice must be given of the location of the availability of the
unmodified current source code, e.g.,
http://www.Kelley.com/
or
ftp://ftp.Kelley.com
in the documentation and/or other materials provided with the
distribution.
5. All advertising and published materials mentioning features or use
of this software must display the following acknowledgment: "This
product includes software developed by Clifford Kelley and other
contributors."
6. The name of Clifford Kelley may not be used to endorse or promote
products derived from this software without specific prior written
permission.
DISCLAIMER
This software is provided by Clifford Kelley and contributors "as is" and
any expressed or implied warranties, including, but not limited to, the
implied warranties of merchantability and fitness for a particular
purpose are disclaimed. In no event shall Clifford Kelley or
contributors be liable for any direct, indirect, incidental, special,
exemplary, or consequential damages (including, but not limited to,
procurement of substitute goods or services; loss of use, data, or
profits; or business interruption) however caused and on any theory of
liability, whether in contract, strict liability, or tort (including
negligence or otherwise) arising in any way out of the use of this
software, even if advised of the possibility of such damage.
*/
//
// Additional structure definitions
//
typedef struct velocity
{
double east, north, up,
clock_err,
x, y, z;
} VELOCITY;
typedef struct hms
{
int deg,
min;
float sec;
} HMS;
HMS cur_lat, cur_long;
int a_missed, n_chan, chmax = 11;
unsigned test[16]=
{
0x0001, 0x0002, 0x0004, 0x0008,
0x0010, 0x0020, 0x0040, 0x0080,
0x0100, 0x0200, 0x0400, 0x0800,
0x1000, 0x2000, 0x4000, 0x8000
};
int prn_code[37] =
{
0,
0x3f6, 0x3ec, 0x3d8, 0x3b0, 0x04b, 0x096, 0x2cb, 0x196, 0x32c,
0x3ba, 0x374, 0x1d0, 0x3a0, 0x340, 0x280, 0x100, 0x113, 0x226,
0x04c, 0x098, 0x130, 0x260, 0x267, 0x338, 0x270, 0x0e0, 0x1c0,
0x380, 0x22b, 0x056, 0x0ac, 0x158, 0x058, 0x18b, 0x316, 0x058
};
//
// include files
//
#include
#include
#include
#include
#include
#include
#ifdef __TURBOC__
# include
#endif
#include "ogsdefine.h"
#include "ogsstructs.h"
#include "ogsextern.h"
#include "ogsprototypes.h"
#include "ogslibrary.h"
//#include "gp2021.cpp"
//#include "gpsfuncs.cpp"
#define IRQLEVEL 0 // IRQ Line
#define FRQUNIT 42.57475e-3
// Function definitions
// These are arrays for debugging
// they can be written into while running and dumped to a
// file at the end of the rum
// int far i_prmtpta[6][1500],far q_prmtpta[6][1500];
// int far i_dithera[6][1500],far q_dithera[6][1500];
// long far car_freq[6][1500],far chip_freq[6][1500];
//
// definitions with default values which can be overridden in
// file 'rcvr_par.dat'
//
static int nav_tic,
search_max_f,
search_min_f = 0,
cold_prn = 1,
pull_in_time = 1000, // pull in time in msec
phase_test = 500,
confirm_m = 10,
n_of_m_thresh = 8,
key,
tic_count = 0,
hms_count = 0,
nav_count,
min_flag,
nav_flag,
sec_flag,
n_track;
static unsigned int interr_int = 512;
static long rms = 312, // noise RMS
acq_thresh = 650,
code_corr,
time_on = 0,
delta_frq = 4698, // carrier frq search interval in units of 42.6 mHz (200 Hz)
d_tow,
trk_div = 19643;
static float clock_offset = -0.6,
cod_lock_det_thresh = 0.5;
static float nav_up = 1.0;
// pull_code_k = 111,
// pull_code_d = 7,
// pull_carr_k = -12,
// pull_carr_d = 28,
// trk_code_k = 55,
// trk_code_d = 3,
// trk_carr_k = -9,
// trk_carr_d = 21,
// 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
static float bw_car_track = 25,
damp_car_track = 0.707,
gain_car_track = 4*M_PI*0.1,
bw_cod_track = 1,
damp_cod_track = 0.707,
gain_cod_track = 50,
bw_car_pullin = 25,
damp_car_pullin = 0.707,
gain_car_pullin = 4*M_PI*0.1,
bw_cod_pullin = 1,
damp_cod_pullin = 0.707,
gain_cod_pullin = 50;
static double speed, heading;
static ECEF rec_pos_ecef;
static PVT rpvt;
// write messages to log file
static int out_pos = 1,
out_vel = 1,
out_time = 1,
out_kalman = 1,
out_debug = 1;
typedef struct
{
} CONTROLPARAMETER;
CONTROLPARAMETER Par;
static float car_PLL_track_p1,
car_PLL_track_p2,
cod_PLL_track_p1,
cod_PLL_track_p2,
car_PLL_pullin_p1,
car_PLL_pullin_p2,
cod_PLL_pullin_p1,
cod_PLL_pullin_p2;
/* ------------------------------- globals ---------------------------------- */
time_t thetime;
/* ------------------------------ prototypes -------------------------------- */
static void ch_confirm( char ch);
static void ch_pull_in( char ch);
static void ch_track( char ch);
static void ch_acq( char ch);
static void read_rcvr_par( void);
void write_to_file_carfrq( long car_frq, int prn);
void write_to_file_IQ( long Ip, long Qp, int prn);
void write_to_file_navbit( int ch);
/* ------------------------------ functions --------------------------------- */
/*******************************************************************************
FUNCTION do_the_tracking()
RETURNS None.
PARAMETERS None.
PURPOSE
This function replaces the current IRQ0 Interrupt service routine with
our GPS function.
WRITTEN BY
Clifford Kelley
*******************************************************************************/
static int do_the_tracking( void)
{
int astat, mstat, res = 0;
unsigned int add;
char ch;
// acquire data and correlate, returns from software_correlator(),
// if there is something to process
if ( !run_software_correlator())
{
printf( "Simulation finished.\n");
// exit(1);
res = -1;
return (res);
}
to_gps( 0x80, 0); // tell GP2021 to latch the correlators
a_missed = from_gps( 0x83); // missed dump event?
astat = from_gps( 0x82); // get info on what channels have data ready
for ( ch=0; ch<=chmax; ch++)
{
if ( astat & test[ch])
{
add = 0x84 + ( ch << 2); // 4 byte per channel
chan[ch].i_dith = from_gps( add); // inphase dither
add++;
chan[ch].q_dith = from_gps( add); // quadrature dither
add++;
chan[ch].i_prmt = from_gps( add); // inphase prompt
add++;
chan[ch].q_prmt = from_gps( add); // quadrature prompt
ch_accum_reset( ch);
if ( a_missed & test[ch])
{
chan[ch].missed++;
ch_accum_reset( ch);
}
}
}
for ( ch=0; ch<=chmax; ch++)
{
if ( astat & test[ch])
{
switch( chan[ch].state)
{
case acquisition:
ch_acq( ch);
break;
case confirm:
ch_confirm( ch);
break;
case pull_in:
ch_pull_in( ch);
break;
case track:
ch_track( ch);
break;
}
}
}
mstat = a_missed & 0x2000; // has a tic occured?
if ( mstat)
{
tic_count = (++tic_count) % 10;
if ( tic_count == 0)
sec_flag = 1;
hms_count = (++hms_count) % 600;
if ( hms_count == 0)
min_flag = 1;
nav_count = (++nav_count) % nav_tic;
if ( nav_count == 0)
nav_flag = 1;
if ( nav_count == 0)
{
add = 1;
for ( ch=0; ch<=chmax; ch++)
{
chan[ch].code_phase = from_gps( add); // get carrier
add++; // and code data
chan[ch].carr_cycle_l = from_gps( add); // for computing
add++; // pseudorange and
chan[ch].carr_dco_phase = from_gps( add); // delta-pseudorange
add++;
chan[ch].epoch = from_gps( add);
add++;
chan[ch].code_dco_phase = from_gps( add);
add++;
chan[ch].carr_cycle_h = from_gps( add);
add++;
add++;
add++;
chan[ch].meas_bit_time = chan[ch].tr_bit_time;
chan[ch].doppler = chan[ch].carrier_freq;
}
} // if ( nav_count == 0)
} // if ( mstat)
#if 0
// reset the interrupt
outportb( 0x20, 0x20);
#endif
return (res);
}
/*******************************************************************************
FUNCTION display()
RETURNS None.
PARAMETERS None.
PURPOSE
This function displays the current status of the receiver on the
computer screen. It is called when there is nothing else to do
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void display( void)
{
char ch;
char *buf;
gotoxy( 1, 1);
// printf( "%s", ctime( &thetime));
// printf( "TOW %6ld\n", clock_tow);
buf = ctime( &thetime);
buf[strlen(buf)-1] = '\0'; // remove trailing '\n'
printf( "%s TOW %6ld\n", buf, clock_tow);
cur_lat.deg = (int)( rec_pos_llh.lat*r_to_d); // GB: int() inserted
cur_lat.min = (int)( (rec_pos_llh.lat*r_to_d-cur_lat.deg)*60); // GB: int() inserted
cur_lat.sec = (rec_pos_llh.lat*r_to_d - cur_lat.deg - cur_lat.min/60.)*3600.;
cur_long.deg = (int)( rec_pos_llh.lon * r_to_d); // GB: int() inserted
cur_long.min = (int)( (rec_pos_llh.lon * r_to_d - cur_long.deg) * 60); // GB: int() inserted
cur_long.sec = (rec_pos_llh.lon * r_to_d - cur_long.deg-cur_long.min/60.) * 3600.;
printf( "lat %3d:%2d:%5.2f lon %4d:%2d:%5.2f HAE %8.2f clk err %5.2f (ppm)\n", cur_lat.deg,
abs( cur_lat.min), fabs( cur_lat.sec), cur_long.deg, abs( cur_long.min),
fabs( cur_long.sec), rec_pos_llh.hae, clock_offset);
printf( "speed %lf heading %lf", speed, heading * r_to_d);
printf( " GDOP=%4.1f HDOP=%4.1f VDOP=%4.1f\n", gdop, hdop, vdop);
printf( "tracking %2d almanac valid %1d gps week %4d\n",
n_track, almanac_valid, gps_week % 1024);
printf( "\n");
printf( "ch prn st frq cod az el dpl bit frm sfid ura pg mis CNo cFrq dFrq r/lk d/lk\n");
for ( ch=0; ch=0; j--)
// tmpmessage[i] |= (!!chan[0].message[i*8+(7-j)] << j);
// printf( "%02x", tmpmessage[i]);
// }
// gotoxy( 1, 20);
// for ( i=0; i<80; i++)
// printf( "%01x", chan[0].message[i]);
// gotoxy( 1, 21);
// for ( i=80; i<160; i++)
// printf( "%01x", chan[0].message[i]);
// gotoxy( 1, 22);
// for ( i=160; i<240; i++)
// printf( "%01x", chan[0].message[i]);
// gotoxy( 1, 23);
// for ( i=240; i<320; i++)
// printf( "%01x", chan[0].message[i]);
return;
}
/*******************************************************************************
FUNCTION Interrupt_Install()
RETURNS None.
PARAMETERS None.
PURPOSE
This function replaces the current IRQ0 Interrupt service routine with
our own custom function. The old vector is stored in a global variable
and will be reinstalled at the end of program execution. IRQ0 is
enabled by altering the interrupt mask stored by the 8259 interrupt
handler.
*******************************************************************************/
#if 1
void Interrupt_Install()
{
return;
}
#else
void Interrupt_Install()
{
unsigned char int_mask,i_high,i_low;
i_high=interr_int>>8;
i_low=interr_int&0xff;
Old_Interrupt = getvect(8 + IRQLEVEL);
disable();
setvect( 8 + IRQLEVEL, do_the_tracking);
int_mask = inportb(0x21); // get hardware interrupt mask
int_mask = int_mask & ~(1 << IRQLEVEL);
outportb( 0x21, int_mask); // send new mask to 8259
enable();
// modify the timer to divide by interr_int
outportb( 0x43, 0x34);
outportb( 0x40, i_low);
outportb( 0x40, i_high);
outportb( 0x20, 0x20); // Clear PIC
}
#endif
/*******************************************************************************
FUNCTION Interrupt_Remove()
RETURNS None.
PARAMETERS None.
PURPOSE
This function removes the custom interrupt vector from the vector
table and restores the previous vector.
*******************************************************************************/
#if 1
void Interrupt_Remove( void)
{
return;
}
#else
void Interrupt_Remove( void)
{
unsigned char int_mask;
outportb(0x20,0x20); // clear interrupt and allow next one
int_mask = inportb( 0x21); // get hardware interrupt mask
int_mask = int_mask | (1 << IRQLEVEL);
disable();
outportb( 0x21, int_mask); // send new mask to 8259
setvect(8 + IRQLEVEL,Old_Interrupt);
enable(); // allow hardware interrupts
outportb( 0x20, 0x20); // clear interrupt and allow next one
outportb( 0x43, 0x34); // reset clock
outportb( 0x40, 0xff);
outportb( 0x40, 0xff);
}
#endif
/*******************************************************************************
FUNCTION nav_fix()
RETURNS None.
PARAMETERS None.
PURPOSE
This function determines the pseudorange and doppler to each
satellite and calls pos_vel_time to determine the position and
velocity of the receiver
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void nav_fix(void)
{
char ch,n,bit;
float clock_error;
double tr_time[13],t_cor[13],dtime[13],tr_avg,tmax;
static double time[3];
int i,ms,chip,phase,tr_ch[13];
ECEF rp_ecef;
ECEFT dm_gps_sat[13],dp_gps_sat[13];
n=1;
tr_avg=0.0;
for (ch=0;ch 30 &&
gps_eph[chan[ch].prn].valid == 1 &&
gps_eph[chan[ch].prn].health == 0 &&
chan[ch].tow_sync == 1)
{
ms = chan[ch].epoch & 0x1f;
chip = chan[ch].code_phase;
phase = chan[ch].code_dco_phase;
bit = (chan[ch].epoch-ms) / 256;
tr_time[n] = (chan[ch].meas_bit_time - chan[ch].meas_bit_time%50+bit) * 0.02 +
ms/1000.0 + chip/2.046e6 + phase/2.046e6/1024.;
tr_ch[n] = ch;
tr_avg += tr_time[n];
if ( out_debug && stream)
fprintf( stream,
"ch= %d,epoch= %d,ms= %d,chip= %d,phase= %d,n= %d,bit* time= %ld,t= %20.10lf\n",
ch, (chan[ch].epoch-ms)/256, ms, chip, phase, n, chan[ch].meas_bit_time%50,
tr_time[n]);
n++;
}
}
n_track = n-1;
if ( n_track >= 4)
{
tmax = -10.0;
for ( i=1; i<=n_track; i++)
{
if (tr_time[i]>tmax)
tmax = tr_time[i];
}
tmax = tmax+0.067;
if (out_kalman)
fprintf( out, "%d\n", n_track);
if (out_debug && stream)
fprintf( stream, "%d\n", n_track);
for ( i=1; i<=n_track; i++)
{
track_sat[i] = satpos_ephemeris( tr_time[i],chan[tr_ch[i]].prn);
dm_gps_sat[i] = satpos_ephemeris( tr_time[i]-0.5,chan[tr_ch[i]].prn);
dp_gps_sat[i] = satpos_ephemeris( tr_time[i]+0.5,chan[tr_ch[i]].prn);
if ( out_debug && stream)
fprintf( stream,"i=%d,az=%20.10lf,el=%20.10lf\n",
i,track_sat[i].az,track_sat[i].el);
t_cor[i] = track_sat[i].tb +
tropo_iono(track_sat[i].az,track_sat[i].el,tr_time[i]);
dt[i] = tmax-(tr_time[i]-t_cor[i]);
// if (dt[i]>=1) printf("dt[%d]=%20.10lf\n",i,dt[i]);
if ( out_debug && stream)
{
fprintf( stream,"i=% d,tb= %20.10lf,tropo_iono= %20.10lf, dt= %20.10lf\n",
i,track_sat[i].tb,tropo_iono(track_sat[i].az,track_sat[i].el,tr_time[i]),dt[i]);
fprintf( stream, "%20.10lf,%20.10lf,%20.10lf,%20.10lf\n",
track_sat[i].x,track_sat[i].y,track_sat[i].z,dt[i]);
}
d_sat[i].x = dp_gps_sat[i].x-dm_gps_sat[i].x-track_sat[i].y*omegae;
d_sat[i].y = dp_gps_sat[i].y-dm_gps_sat[i].y+track_sat[i].x*omegae;
d_sat[i].z = dp_gps_sat[i].z-dm_gps_sat[i].z;
if ( out_kalman)
{
fprintf(out,"%20.10lf,%20.10lf,%20.10lf,%20.10lf\n",
track_sat[i].x,track_sat[i].y,track_sat[i].z,dt[i]);
fprintf(out,"%20.10lf,%20.10lf,%20.10lf,",
d_sat[i].x,d_sat[i].y,d_sat[i].z);
}
meas_dop[i] = (chan[tr_ch[i]].doppler - 33010105L) * 42.574746268e-3;
if ( out_kalman)
fprintf( out, "%lf\n", meas_dop[i]);
}
rpvt = pos_vel_time(n_track);
dops( n_track);
cbias = rpvt.dt;
clock_error = rpvt.df;
time[1] = tr_time[1]+dtime[1]-cbias;
rp_ecef.x = rpvt.x;
rp_ecef.y = rpvt.y;
rp_ecef.z = rpvt.z;
rp_llh = ecef_to_llh(rp_ecef);
if (rp_llh.hae > -2000.0 && rp_llh.hae < 1.e6) // a quick reasonableness check
{
if (fabs(clock_error) < 5.0)
clock_offset = clock_error;
if ( almanac_valid == 1)
status = navigating;
rec_pos_llh.lon = rp_llh.lon;
rec_pos_llh.lat = rp_llh.lat;
rec_pos_llh.hae = rp_llh.hae;
rec_pos_xyz.x = rp_ecef.x;
rec_pos_xyz.y = rp_ecef.y;
rec_pos_xyz.z = rp_ecef.z;
if ( out_pos && stream)
fprintf( stream, "lat= %lf,long= %lf,hae= %lf,gdop= %f,hdop= %f,vdop= %f\n",
rec_pos_llh.lat*r_to_d,rec_pos_llh.lon*r_to_d,rec_pos_llh.hae,gdop,hdop,vdop);
//
// Since we have a valid position/velocity narrow the doppler search window
//
search_max_f=5;
//
// Translate velocity into North, East, Up coordinates
//
velocity();
if (out_kalman)
{
fprintf( out, " %20.10lf,%20.10lf,%20.10lf,%20.10lf\n",
rp_ecef.x, rp_ecef.y, rp_ecef.z, cbias);
fprintf( out, " %20.10lf,%20.10lf,%20.10lf,%10.10lf\n\n",
rpvt.xv, rpvt.yv, rpvt.zv, clock_error);
}
time[0]=time[1];
}
}
}
/*******************************************************************************
FUNCTION chan_allocate()
RETURNS None.
PARAMETERS None.
PURPOSE
This function allocates the channels with PRN numbers
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void chan_allocate()
{
char ch,prnn,alloc;
search_max_f=30;
almanac_valid=1;
for ( prnn=1; prnn<=32; prnn++)
{
xyz[prnn] = satfind( prnn);
if ( gps_alm[prnn].inc > 0.0 && gps_alm[prnn].week != gps_week % 1024)
almanac_valid = 0;
}
if ( Iono.al0 == 0.0 && Iono.b0 == 0.0)
almanac_valid = 0;
for ( ch=0; ch mask_angle &&
gps_alm[prnn].health == 0 &&
gps_alm[prnn].ety != 0.0)
{
alloc = 0;
for ( ch=0; ch ...
} // for (prnn=1;prnn<=32;prnn++)
return;
}
/*******************************************************************************
FUNCTION cold_allocate()
RETURNS None.
PARAMETERS None.
PURPOSE To allocate the PRNs to channels for a cold start
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void cold_allocate( void)
{
SATVIS dummy;
char ch,i,alloc;
// delta_frq=4698; // search 200 Hz intervals
search_max_f = 50;
dummy = satfind( 0);
almanac_valid = 1;
reset_cntl( 0x1fff);
for ( i=1; i<=32; i++)
{
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 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
a combination FLL and PLL.
It will attempt to track for xxx ms, the last xxx ms of data will be
gathered to determine if we have both code and carrier lock.
If so we will transition to state 'tracking'.
Determine start of data bit transition (bit synchronization) using
histogram method.
REFERENCES
Tsui, Fundamentals of GPS receivers: A software approach, Wiley
Krunvieda et al., A complete IF software GPS receiver: a tutorial about
the detail, Data Fusion Corporation
Kaplan, Understanding GPS: Principles and Applications
WRITTEN BY
Clifford Kelley
Modifications by G. Beyerle
TO DO
02-05-01: Replace floating point with integer arithmetic
NOTES
02-03-20:
*******************************************************************************/
static void ch_pull_in( char ch)
{
int i, idx, bitsign;
long Ip, Qp, Id, Qd;
long ddf, ddcar, q_sum, i_sum;
double dsc_car_phs_old,
dsc_car_phs,
car_frq_ofs,
dsc_cod_phs_old,
dsc_cod_phs,
cod_frq_ofs;
double sum_IQd, sum_IQp;
double Ip2, Qp2, IQp2;
double curphs;
if ( chan[ch].ch_time < pull_in_time)
{
//
// if current phase deviated more than 90 deg from last value
// we probably have a nav bit flip
//
curphs = atan2( chan[ch].bitsign*chan[ch].q_prmt,
chan[ch].bitsign*chan[ch].i_prmt) / (2*M_PI);
if ( fabs( chan[ch].dsc_car_phs - curphs) > 0.25)
{
chan[ch].bitsign *= -1;
chan[ch].databithist[chan[ch].ms_count] += 1;
}
// printf( "chan[ch].dsc_car_phs = %e, curphs = %e\n",
// chan[ch].dsc_car_phs, curphs);
bitsign = chan[ch].bitsign;
Ip = bitsign * chan[ch].i_prmt;
Qp = bitsign * chan[ch].q_prmt;
Id = bitsign * chan[ch].i_dith;
Qd = bitsign * chan[ch].q_dith;
//
// for efficiency we implement summation over NOFCOHINT values by
// keeping track of individual additions in arrays 'chan[ch].i/q_prmt/dith_sav[]'
//
// --- add latest value ...
chan[ch].i_prmt_sum += Ip;
chan[ch].q_prmt_sum += Qp;
chan[ch].i_dith_sum += Id;
chan[ch].q_dith_sum += Qd;
chan[ch].iq2_prmt_sum += Ip * Ip + Qp * Qp;
// ... subtract first value ...
idx = (chan[ch].iq_sav_idx + 1) % NOFCOHINT;
chan[ch].iq_sav_idx = idx;
chan[ch].i_prmt_sum -= chan[ch].i_prmt_sav[idx];
chan[ch].q_prmt_sum -= chan[ch].q_prmt_sav[idx];
chan[ch].i_dith_sum -= chan[ch].i_dith_sav[idx];
chan[ch].q_dith_sum -= chan[ch].q_dith_sav[idx];
chan[ch].iq2_prmt_sum -= chan[ch].iq2_prmt_sav[idx];
// ... and replace by latest value in "store array"
chan[ch].i_prmt_sav[idx] = Ip;
chan[ch].q_prmt_sav[idx] = Qp;
chan[ch].i_dith_sav[idx] = Id;
chan[ch].q_dith_sav[idx] = Qd;
chan[ch].iq2_prmt_sav[idx] = Ip * Ip + Qp * Qp;
// --- carrier tracking loop ---
dsc_car_phs_old = chan[ch].dsc_car_phs;
// printf( "chan[%d].q_prmt_sum = %d\n", ch, chan[ch].q_prmt_sum);
// printf( "chan[%d].i_prmt_sum = %d\n", ch, chan[ch].i_prmt_sum);
dsc_car_phs = atan2( chan[ch].q_prmt_sum, chan[ch].i_prmt_sum) / (2*M_PI); // optimal carrier phase discriminator
chan[ch].dsc_car_phs = dsc_car_phs;
car_frq_ofs = car_PLL_pullin_p1 * dsc_car_phs - car_PLL_pullin_p2 * dsc_car_phs_old;
// if (ch==0)
// {
// printf( "car_PLL_pullin_p1 = %e\n", car_PLL_pullin_p1);
// printf( "car_PLL_pullin_p2 = %e\n", car_PLL_pullin_p2);
// printf( "dsc_car_phs = %e\n", dsc_car_phs);
// printf( "dsc_car_phs_old = %e\n", dsc_car_phs_old);
// printf( "chan[ch].dsc_car_phs = %e\n", chan[ch].dsc_car_phs);
// printf( "car_frq_ofs = %e\n", car_frq_ofs);
// }
// getchar();
car_frq_ofs /= FRQUNIT; // INCR counter runs in units of 42 mHz
// printf( "d codfrq = %.6f Hz, d carfrq = %.6f Hz\n",
// (chan[ch].code_freq - 24028328) * 42.57475e-3,
// (chan[ch].carrier_freq - 33010105) * 42.57475e-3);
// --- update INCR register ---
chan[ch].carrier_freq += (long)( car_frq_ofs);
ch_carrier( ch, chan[ch].carrier_freq);
// --- code tracking loop ---
// printf("vor sqrt() *** 1 ***\n");
// printf("iDith = %e \n", (double)( chan[ch].i_dith_sum * chan[ch].i_dith_sum));
// printf("qDith = %e \n", (double)( chan[ch].q_dith_sum * chan[ch].q_dith_sum));
// printf("iPrmt = %e \n", (double)( chan[ch].i_prmt_sum * chan[ch].i_prmt_sum));
// printf("qPrmt = %e \n", (double)( chan[ch].q_prmt_sum * chan[ch].q_prmt_sum));
sum_IQd = sqrt( (double)( chan[ch].i_dith_sum) * (double)( chan[ch].i_dith_sum) +
(double)( chan[ch].q_dith_sum) * (double)( chan[ch].q_dith_sum));
sum_IQp = sqrt( (double)( chan[ch].i_prmt_sum) * (double)( chan[ch].i_prmt_sum) +
(double)( chan[ch].q_prmt_sum) * (double)( chan[ch].q_prmt_sum));
// printf("nach sqrt() *** 1 ***\n");
dsc_cod_phs_old = chan[ch].dsc_cod_phs;
if ( sum_IQp)
dsc_cod_phs = (sum_IQd / sum_IQp - 0.5);
else
dsc_cod_phs = 0.0;
chan[ch].dsc_cod_phs = dsc_cod_phs;
cod_frq_ofs = cod_PLL_pullin_p1 * dsc_cod_phs - cod_PLL_pullin_p2 * dsc_cod_phs_old;
cod_frq_ofs /= 2*FRQUNIT; // INCR counter runs in units of 85 mHz
// --- update INCR register ---
chan[ch].code_freq += (long)( cod_frq_ofs);
ch_code( ch, chan[ch].code_freq);
// --- carrier and code lock detector ---
Ip2 = (double)( chan[ch].i_prmt_sum) * (double)( chan[ch].i_prmt_sum);
Qp2 = (double)( chan[ch].q_prmt_sum) * (double)( chan[ch].q_prmt_sum);
// --- carrier lock detector ---
chan[ch].car_lock_det = (Ip2 - Qp2) / (Ip2 + Qp2);
// if (chan[ch].car_lock_det > 1.0)
// {
// printf("** r/lk = %e, Ip2 = %e, Qp2 = %e\n", chan[ch].car_lock_det, Ip2, Qp2);
// getchar();
// }
// --- code lock detector ---
chan[ch].cod_lock_det = (Ip2 + Qp2) / chan[ch].iq2_prmt_sum / NOFCOHINT;
// ************** debug *******************
// if ( ch==0)
// {
// extern unsigned long Sample_Counter_Start[], Sample_Counter_End[];
//
// printf( "pll: (%d-%d) Ip=%4d, Qp=%4d, Id=%4d, Qd=%4d, r/lk:%.2f, d/lk:%.2f",
// Sample_Counter_Start[0], Sample_Counter_End[0], Ip, Qp, Id, Qd,
// chan[ch].car_lock_det, chan[ch].cod_lock_det);
// printf( "pll: Ip=%4d, Qp=%4d, Id=%4d, Qd=%4d, r/lk:%.2f, d/lk:%.2f\n",
// 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].t_count++;
if ( chan[ch].t_count % 5 == 0)
{
chan[ch].avg = chan[ch].sum / 10;
chan[ch].sum = 0;
}
}
if ( chan[ch].ch_time > pull_in_time - phase_test)
{
chan[ch].th_rms += (long)( dsc_car_phs * dsc_car_phs * 268435456); // (2^14)^2
// printf( "pull_in: chan[ch].th_rms = %d\n", chan[ch].th_rms);
// getchar();
}
// if ( chan[ch].cod_lock_det > cod_lock_det_thresh)
// {
// i_sum = chan[ch].i_dith + chan[ch].i_prmt;
//
//// --- sign change? if yes mark in histogram ---
// if ( sign( chan[ch].old_i_sum) == -sign( i_sum))
// chan[ch].databithist[chan[ch].ms_count] += 1;
// }
chan[ch].old_i_sum = i_sum;
chan[ch].ch_time++;
} // --- if ( chan[ch].ch_time < pull_in_time) ---
else
{
if ( chan[ch].cod_lock_det < cod_lock_det_thresh)
{
// --- go back to square one and start with acq ---
chan[ch].state = acquisition;
}
else
{
// --- analyze data bit histogram (bit synchronization)
int ret;
ret = analyze_databit_histogram( ch);
// printf("pll: ret = %d\n", ret);
// getchar();
if ( ret >= 0)
{
// chan[ch].ms_count = (20-ret+chan[ch].ms_count) % 20; // reset msec counter
chan[ch].ms_count = (chan[ch].ms_count-ret+20) % 20; // reset msec counter
// chan[ch].databit_start = ret;
// --- transition to tracking state ---
chan[ch].state = track;
chan[ch].t_count = 0;
// chan[ch].avg = chan[ch].avg * 20;
// chan[ch].sum = 0;
}
else
chan[ch].state = acquisition;
}
} // --- if ( chan[ch].ch_time == pull_in_time) ---
// --- if back to acq, reset frq ---
if ( chan[ch].state == acquisition)
{
chan[ch].codes = 0;
chan[ch].code_freq = code_ref + code_corr;
ch_code( ch, chan[ch].code_freq); // 1.023 MHz chipping rate
}
chan[ch].ms_count = (++chan[ch].ms_count) % NOFMSECPERDATABIT;
return;
}
/*******************************************************************************
FUNCTION ch_track()
RETURNS None.
PARAMETERS None.
PURPOSE
Track signal with a combination FLL and PLL.
Identify navigation data frame (frame synchronization).
REFERENCES
Kaplan, Understanding GPS: Principles and Applications
WRITTEN BY
Clifford Kelley
Modifications by G. Beyerle
TO DO
02-05-01: Replace floating point with integer arithmetic
NOTES
02-03-20:
*******************************************************************************/
static void ch_track( char ch)
{
int i, idx, bitsign;
long Ip, Qp, Id, Qd;
long ddf, ddcar, q_sum, i_sum;
float dsc_car_phs_old,
dsc_car_phs,
car_frq_ofs,
dsc_cod_phs_old,
dsc_cod_phs,
cod_frq_ofs;
float sum_IQd, sum_IQp;
float Ip2, Qp2, IQp2, curphs;
//
// if current phase deviated more than 90 deg from last value
// we probably have a nav bit flip
//
curphs = atan2( chan[ch].bitsign*chan[ch].q_prmt,
chan[ch].bitsign*chan[ch].i_prmt) / (2*M_PI);
// if ( fabs( chan[ch].dsc_car_phs - curphs) > 0.25 &&
// chan[ch].ms_count == 0)
if ( fabs( chan[ch].dsc_car_phs - curphs) > 0.25)
{
// printf("trk: chan[%d].ms_count = %d\n", ch, chan[ch].ms_count);
// getchar();
chan[ch].bitsign *= -1;
// printf("trk: chan[%d].ms_count = %d\n", ch, chan[ch].ms_count);
// getchar();
// chan[ch].databithist[chan[ch].ms_count] += 1;
}
//
// implement summation over NOFCOHINT values by keeping track
// of individual additions
//
bitsign = chan[ch].bitsign;
Ip = bitsign * chan[ch].i_prmt;
Qp = bitsign * chan[ch].q_prmt;
Id = bitsign * chan[ch].i_dith;
Qd = bitsign * chan[ch].q_dith;
if ( OutputIQ)
write_to_file_IQ( chan[ch].i_prmt, chan[ch].q_prmt, chan[ch].prn); // with nav msg
// write_to_file_IQ( Ip, Qp, chan[ch].prn); // nav msg wiped off
// add latest value ...
chan[ch].i_prmt_sum += Ip;
chan[ch].q_prmt_sum += Qp;
chan[ch].i_dith_sum += Id;
chan[ch].q_dith_sum += Qd;
chan[ch].iq2_prmt_sum += Ip * Ip + Qp * Qp;
// ... subtract first value ...
idx = (chan[ch].iq_sav_idx + 1) % NOFCOHINT;
chan[ch].iq_sav_idx = idx;
chan[ch].i_prmt_sum -= chan[ch].i_prmt_sav[idx];
chan[ch].q_prmt_sum -= chan[ch].q_prmt_sav[idx];
chan[ch].i_dith_sum -= chan[ch].i_dith_sav[idx];
chan[ch].q_dith_sum -= chan[ch].q_dith_sav[idx];
chan[ch].iq2_prmt_sum -= chan[ch].iq2_prmt_sav[idx];
// ... and replace by latest value in "store array"
chan[ch].i_prmt_sav[idx] = Ip;
chan[ch].q_prmt_sav[idx] = Qp;
chan[ch].i_dith_sav[idx] = Id;
chan[ch].q_dith_sav[idx] = Qd;
chan[ch].iq2_prmt_sav[idx] = Ip * Ip + Qp * Qp;
// carrier tracking loop
dsc_car_phs_old = chan[ch].dsc_car_phs;
dsc_car_phs = atan2( chan[ch].q_prmt_sum, chan[ch].i_prmt_sum) / (2*M_PI); // optimal carrier phase discriminator
chan[ch].dsc_car_phs = dsc_car_phs;
car_frq_ofs = car_PLL_track_p1 * dsc_car_phs - car_PLL_track_p2 * dsc_car_phs_old;
car_frq_ofs /= FRQUNIT; // INCR counter runs in units of 42.57 mHz
// update INCR register
chan[ch].carrier_freq += (long)( car_frq_ofs);
ch_carrier( ch, chan[ch].carrier_freq);
if ( OutputCarFrq)
write_to_file_carfrq( (long)( car_frq_ofs), chan[ch].prn);
// code tracking loop
sum_IQd = sqrt( (double)( chan[ch].i_dith_sum) * (double)( chan[ch].i_dith_sum) +
(double)( chan[ch].q_dith_sum) * (double)( chan[ch].q_dith_sum));
sum_IQp = sqrt( (double)( chan[ch].i_prmt_sum) * (double)( chan[ch].i_prmt_sum) +
(double)( chan[ch].q_prmt_sum) * (double)( chan[ch].q_prmt_sum));
dsc_cod_phs_old = chan[ch].dsc_cod_phs;
if ( sum_IQp)
dsc_cod_phs = (sum_IQd / sum_IQp - 0.5);
else
dsc_cod_phs = 0.0;
chan[ch].dsc_cod_phs = dsc_cod_phs;
cod_frq_ofs = cod_PLL_track_p1 * dsc_cod_phs - cod_PLL_track_p2 * dsc_cod_phs_old;
// cod_frq_ofs *= NOFCHIPS;
cod_frq_ofs /= 2*FRQUNIT; // INCR counter runs in units of 85 mHz
// update INCR register
chan[ch].code_freq += (long)( cod_frq_ofs);
ch_code( ch, chan[ch].code_freq);
// carrier and code lock detector
Ip2 = (double)( chan[ch].i_prmt_sum) * (double)( chan[ch].i_prmt_sum);
Qp2 = (double)( chan[ch].q_prmt_sum) * (double)( chan[ch].q_prmt_sum);
chan[ch].car_lock_det = (Ip2 - Qp2) / (Ip2 + Qp2);
chan[ch].cod_lock_det = (Ip2 + Qp2) / chan[ch].iq2_prmt_sum / NOFCOHINT;
if ( chan[ch].cod_lock_det < cod_lock_det_thresh)
{
chan[ch].state = acquisition;
// printf("trk: chan[%d].cod_lock_det = %e, cod_lock_det_thresh = %e\n",
// ch, chan[ch].cod_lock_det, cod_lock_det_thresh);
// getchar();
}
// ----------------------
// printf( "Ip=%5d, Qp=%5d, Id=%5d, Qd=%5d, r/lk:%.2f, d/lk:%.2f\n",
// Ip, Qp, Id, Qd, chan[ch].car_lock_det, chan[ch].cod_lock_det);
// getchar();
// ----------------------
// printf( "d codfrq = %.6f Hz, d carfrq = %.6f Hz\n",
// (chan[ch].code_freq - 24028328) * 42.57475e-3,
// (chan[ch].carrier_freq - 33010105) * 42.57475e-3);
// printf( "DrPhs = %e, DdPhs = %e, drFrq = %e, ddFrq = %e\n",
// dsc_car_phs, dsc_cod_phs, car_frq_ofs, cod_frq_ofs);
// LastSmplIdx = BUFFERPTR;
// if ( abs( chan[ch].i_prmtpt) < abs( chan[ch].q_prmtpt))
// getchar();
// ************** 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 pull-in state
calc_track_par( bw_car_pullin, damp_car_pullin, gain_car_pullin,
&car_PLL_pullin_p1, &car_PLL_pullin_p2);
calc_track_par( bw_cod_pullin, damp_cod_pullin, gain_cod_pullin,
&cod_PLL_pullin_p1, &cod_PLL_pullin_p2);
return;
}
/*******************************************************************************
FUNCTION main()
RETURNS None.
PARAMETERS None.
PURPOSE
This is the main program to control the GPS receiver
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void main_gpsrcvr( void)
{
int i = 0, j, ret;
char ch;
// self_test();
io_config( 0x301);
test_control( 0);
system_setup( 0);
reset_cntl( 0x1fff);
ch_status = 1;
// read receiver parameter data from file 'rcvr_par.dat'
read_rcvr_par();
read_initial_data();
// for ( ch=0; ch :
if ( token)
{
ret = read_key_value_pair_long( token,
"acquisition threshold", &acq_thresh, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"clock offset [ppm]", &clock_offset, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"code lock threshold", &cod_lock_det_thresh, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"cold prn", &cold_prn, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"confirm_m", &confirm_m, sep);
if ( !ret)
ret = read_key_value_pair_uint( token,
"interrupt vector", &interr_int, sep);
if ( !ret)
{
ret = read_key_value_pair_float( token,
"mask angle [deg]", &mask_angle, sep);
if ( ret) mask_angle /= r_to_d;
}
if ( !ret)
ret = read_key_value_pair_float( token,
"nav update [s]", &nav_up, sep);
if ( !ret)
ret = read_key_value_pair_long( token,
"noise rms", &rms, sep);
if ( !ret)
{
ret = read_key_value_pair_int( token,
"number active channels", &NofChanActive, sep);
if ( ret)
{
NofChanActive = min( NofChanActive, 12);
NofChanActive = max( NofChanActive, 1);
}
}
if ( !ret)
ret = read_key_value_pair_int( token,
"phase test [ms]", &phase_test, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"pull-in time [ms]", &pull_in_time, sep);
if ( !ret)
{
ret = read_key_value_pair_long( token,
"search frequency step [Hz]", &delta_frq, sep);
if ( ret) delta_frq = (long)( delta_frq / FRQUNIT);
}
if ( !ret)
ret = read_key_value_pair_long( token,
"trk_div", &trk_div, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"n of m threshold", &n_of_m_thresh, sep);
// pull-in parameters
if ( !ret)
ret = read_key_value_pair_float( token,
"carrier loop bandwidth (pull-in) [Hz]", &bw_car_pullin, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"code loop bandwidth (pull-in) [Hz]", &bw_cod_pullin, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"carrier loop gain (pull-in)", &gain_car_pullin, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"code loop gain (pull-in)", &gain_cod_pullin, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"carrier loop damping ratio (pull-in)", &damp_car_pullin, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"code loop damping ratio (pull-in)", &damp_cod_pullin, sep);
// tracking parameters
if ( !ret)
ret = read_key_value_pair_float( token,
"carrier loop bandwidth (track) [Hz]", &bw_car_track, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"code loop bandwidth (track) [Hz]", &bw_cod_track, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"carrier loop gain (track)", &gain_car_track, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"code loop gain (track)", &gain_cod_track, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"carrier loop damping ratio (track)", &damp_car_track, sep);
if ( !ret)
ret = read_key_value_pair_float( token,
"code loop damping ratio (track)", &damp_cod_track, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"kalman_file", &out_kalman, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"debug_file", &out_debug, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"search_min_f", &search_min_f, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 0", PRNChn+1, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 1", PRNChn+2, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 2", PRNChn+3, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 3", PRNChn+4, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 4", PRNChn+5, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 5", PRNChn+6, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 6", PRNChn+7, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 7", PRNChn+8, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 8", PRNChn+9, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 9", PRNChn+10, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 10", PRNChn+11, sep);
if ( !ret)
ret = read_key_value_pair_int( token,
"PRN Channel 11", PRNChn+12, sep);
// unkown keyword
if ( !ret)
{
token = strtok( NULL, sep);
if ( token)
{
printf( "unkown keyword >%s< in file %s\n", buf, tmpstr);
exit( -1);
}
}
}
else
// --- token is NULL ---
loop = 0;
} // --- while ( !feof( in) && loop) ---
}
fclose( in);
printf( "Finished reading parameter file %s\n", tmpstr);
printf( "... press ENTER\n");
if ( tmpstr)
{
free( tmpstr);
tmpstr = NULL;
}
return;
}
/* ========================= End of File ========================= */