*******************************************************************************/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 1void Interrupt_Install(){  return;}#elsevoid 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 1void Interrupt_Remove( void){  return;}#elsevoid 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 receiverWRITTEN 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 numbersWRITTEN 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 startWRITTEN 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++)  {