gpsrcvr.c

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

*******************************************************************************/
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<NOFCHAN; ch++)
  {
    if ( chan[ch].state != off)
      printf(
"%2d %2d %1d %3d %4d %3.0f %3.0f %3.0f %3d %4d %2d %2d %2d %2d %4.1f %5d %5d %5.2f %4.2f\n",
        ch, chan[ch].prn, chan[ch].state, chan[ch].n_freq, chan[ch].codes, 
        xyz[chan[ch].prn].azimuth * r_to_d, 
        xyz[chan[ch].prn].elevation * r_to_d,
        xyz[chan[ch].prn].doppler, 
        chan[ch].t_count, chan[ch].n_frame, chan[ch].sfid,
        gps_eph[chan[ch].prn].ura, 
        chan[ch].page5, chan[ch].missed, chan[ch].CNo, 
        chan[ch].carrier_freq-0x1f7b1b9L, chan[ch].code_freq-0x16ea4a8L, 
        chan[ch].car_lock_det, chan[ch].cod_lock_det);
  }

// print first 40*8 = 320 bits of nav message
//  int i, j, tmpmessage[40];
//  for ( i=0; i<40; i++)
//  {
//    tmpmessage[i] = 0;
//    for (j=7; j>=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<NOFCHAN;ch++)
  {
//  Use only satellites being tracked with valid ephemeris data
//  (valid subframe 1,2,3) and high enough raw snr
    if ( chan[ch].state == track && chan[ch].CNo > 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<NOFCHAN; ch++) // if the sat has dropped below mask angle
                                // and CNo is low turn the channel off
  {
    if (( chan[ch].CNo < 30.0 && xyz[chan[ch].prn].elevation < mask_angle) ||
        gps_alm[chan[ch].prn].ety == 0.0)
    {
      chan[ch].state = off;
      chan[ch].prn   = 0;
    }
  }

  for ( prnn=1; prnn<=32; prnn++)
  {
    if ( xyz[prnn].elevation > mask_angle && 
         gps_alm[prnn].health == 0 && 
         gps_alm[prnn].ety != 0.0)
    {
      alloc = 0;

      for ( ch=0; ch<NOFCHAN; ch++)
      {
        if (chan[ch].prn == prnn)
        {
          alloc = 1;// satellite already allocated a channel
          break;
        }
      }

      if ( alloc == 0) // if not allocated find an empty channel
      {
        for ( ch=0; ch<NOFCHAN; ch++)
        {
          reset_cntl( 0x1fff);
          if ( chan[ch].state == off)
          {

            chan[ch].carrier_corr = (long)(( -xyz[prnn].doppler - clock_offset*1575.42) / 
              FRQUNIT);  // GB: cast inserted

// calculate code correction
            code_corr             = (long)( clock_offset*24. + xyz[prnn].doppler/134.); // GB: cast inserted
            chan[ch].code_freq    = code_ref + code_corr;
            ch_code( ch, chan[ch].code_freq);               // 1.023 MHz chipping rate

//  0xa000 : select late code in tracking arm
            ch_cntl( ch, prn_code[prnn] | 0xa000);
//            chan[ch].prn = prnn;
            if ( PRNChn[prnn])
              chan[ch].prn = PRNChn[prnn];
            else  
              chan[ch].prn = prnn;

//      ch_on(ch);
            chan[ch].state    = acquisition;   
            chan[ch].codes    = 0;
            chan[ch].n_freq   = search_min_f;
            chan[ch].del_freq = 1;
            chan[ch].carrier_freq = carrier_ref + 
              chan[ch].carrier_corr + delta_frq*chan[ch].n_freq;   // set carrier

//            printf("chan_allocate: carrier_ref = %x (%d), carrier_corr = %x (%d)\n",
//              carrier_ref, carrier_ref, chan[ch].carrier_corr, chan[ch].carrier_corr);
//            printf("delta_frq = %x (%d), chan[ch].n_freq = %x (%d)\n",
//              delta_frq, delta_frq, chan[ch].n_freq, chan[ch].n_freq);
//            getchar();

            ch_carrier( ch, chan[ch].carrier_freq);             // select carrier
            break;
          }
        }   // for (ch=0; ch<NOFCHAN; ch++)
      }   // if (alloc==0) 
    }   // if (xyz[prnn].elevation >  ...
  }   // 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++)
  {