gpsrcvr.c

C写的用软件无线电实现的GPS模拟程序

/* ************************************************************************ 
   *                                                                      *
   *                          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 [e-mail]
  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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include <assert.h>

#ifdef __TURBOC__
# include <conio.h>
#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