nav_fix.cpp

OpenSource GPS is software for x86 PCs that allows you to acquire, track and demodulate signals from

#include  <stdio.h>
#include  <stdlib.h>
#include  <conio.h>
#include  <math.h>
#include  <string.h>
#include  <io.h>
#include  <Dos.h>
#include  <time.h>
#include  "structs.h"
#include  "consts.h"
#include  "globals.h"
#include  "gpsfuncs.h"
#include  "gp2021.h"

void nav_fix(void);
void  velocity(void);

/*******************************************************************************
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

*******************************************************************************/
#ifdef BCPP
void  nav_fix(void)
{
	char   ch,n,bit;
	double tr_time[N_channels+1],tr_avg,ipart,clock_error;
	static double t_cor[N_channels+1];
	unsigned int    i,ms,chip,phase;
    int meas_bit_time_rem;
    long meas_bit_time_offset;
	 ecef   rp_ecef;
	 eceft  dm_gps_sat[N_channels+1],dp_gps_sat[N_channels+1];

	 tr_avg=0.0;
	 n=1;
	 for (ch=0;ch<N_channels;ch++)
	 {
       meas_bit_time_offset=0;
		 ms=chan[ch].epoch & 0x1f;
		 chip=chan[ch].code_phase;
		 phase=chan[ch].code_dco_phase;
		 bit=chan[ch].epoch>>8;
 	    chan[ch].int_carr_phase=chan[ch].carrier_counter+
 					                float(chan[ch].d_carr_phase)/1024.;
       if (out_debug)
       {
		    fprintf(debug," ch= %d,%d,%ld,%d,%d,%d,%d,%d,%d,%d,%d,%f,",
		    ch,chan[ch].prn,chan[ch].meas_bit_time,bit,ms,chip,phase,chan[ch].state,
          gps_eph[chan[ch].prn].valid,gps_eph[chan[ch].prn].health,
          chan[ch].tow_sync,chan[ch].CNo);
          if (ICP_CTL==0) fprintf(debug,"%ld\n",chan[ch].doppler);
          else            fprintf(debug,"%lf\n",chan[ch].int_carr_phase);
       }
//  Use only satellites being tracked with valid ephemeris data
//  (valid subframe 1,2,3), good health,high enough raw snr, and
//  valid phase data

       meas_bit_time_rem = chan[ch].meas_bit_time%50;
       if ( meas_bit_time_rem == bit+1 ) meas_bit_time_offset = -1;
       if ( meas_bit_time_rem == bit-1 ) meas_bit_time_offset = +1;
       if ( meas_bit_time_rem == 0 && bit == 49 ) meas_bit_time_offset = -1;
		 if ((chan[ch].meas_bit_time+meas_bit_time_offset)%50==bit &&
           chan[ch].state==track && chan[ch].CNo>33 &&
			  gps_eph[chan[ch].prn].valid==1 &&
			  gps_eph[chan[ch].prn].health==0 &&
			  chan[ch].tow_sync==1 && phase<1024 && chip<2046)
		 {
			 tr_time[n]= (chan[ch].meas_bit_time + meas_bit_time_offset)*.02 +
							 ms/1000.0+chip/2.046e6+phase/2.046e6/1024.;
			 tr_ch[n]=ch;
			 tr_avg+=tr_time[n];
			 n++;
		 }
	}
	n_track=n-1;
   TIC_dt=i_TIC_dt*175.0e-9; //each clock count is 175 ns
//   if (ICP_CTL==1)TIC_dt=TIC_dt/float(nav_tic); //use basic TIC interval for ICP
	if (out_debug) fprintf(debug,"n_track= %d\n",n_track);
	for (i=1;i<=n_track;i++)
	{
		 track_sat[i]=satpos_ephemeris(tr_time[i],chan[tr_ch[i]].prn);
      // process Carrier Tracking Loop or Integrated Carrier Phase
		 if(ICP_CTL==0)// satellite velocity
       {
          dm_gps_sat[i]=satpos_ephemeris(tr_time[i]-TIC_dt/2.0,chan[tr_ch[i]].prn); //for CTL
		    dp_gps_sat[i]=satpos_ephemeris(tr_time[i]+TIC_dt/2.0,chan[tr_ch[i]].prn);
		    d_sat[i].x=(dp_gps_sat[i].x-dm_gps_sat[i].x)/TIC_dt-track_sat[i].y*omegae;
		    d_sat[i].y=(dp_gps_sat[i].y-dm_gps_sat[i].y)/TIC_dt+track_sat[i].x*omegae;
		    d_sat[i].z=(dp_gps_sat[i].z-dm_gps_sat[i].z)/TIC_dt;
          meas_dop[i]=(chan[tr_ch[i]].doppler-33010105L)*42.574746268e-3;       }
       else
       {
		    dm_gps_sat[i]=satpos_ephemeris(tr_time[i]-TIC_dt/float(nav_tic),chan[tr_ch[i]].prn); //for ICP
		    dp_gps_sat[i]=track_sat[i];
		    d_sat[i].x=(dp_gps_sat[i].x-dm_gps_sat[i].x)/TIC_dt*float(nav_tic)-track_sat[i].y*omegae;
		    d_sat[i].y=(dp_gps_sat[i].y-dm_gps_sat[i].y)/TIC_dt*float(nav_tic)+track_sat[i].x*omegae;
          d_sat[i].z=(dp_gps_sat[i].z-dm_gps_sat[i].z)/TIC_dt*float(nav_tic);
          meas_dop[i]= chan[tr_ch[i]].int_carr_phase/TIC_dt*float(nav_tic)-1.405396826e6;
       }
		 t_cor[i]=track_sat[i].tb-
					 tropo_iono(tr_ch[i],track_sat[i].az,track_sat[i].el,tr_time[i]);
		 dt[i]=m_time[1]-(tr_time[i]-t_cor[i]);
	}
	if (n_track>=4)
	{
	 rpvt=pos_vel_time(n_track);
	 cbias=rpvt.dt;
	 clock_error=rpvt.df;
//	 m_time[1]=m_time[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< 18000 ) // a position reasonableness check
	 {
//
//  Translate velocity into North, East, Up coordinates
//
		velocity();
		if (sqrt(pow(receiver.vel.north,2.0)+pow(receiver.vel.east,2.0)+pow(receiver.vel.up,2.0))<514.0)
		{                                       // a velocity reasonableness check
			if (fabs(clock_error)<5.0) clock_offset=clock_error;
			status=navigating;                   // if we have a good fix we're navigating
			if (align_t==1)
			{
			  old_TIC_cntr=TIC_cntr;
			  delta_m_time= modf(m_time[1],&ipart);
			  if (nav_up<1.0)
			  {
				 delta_m_time=modf(delta_m_time/nav_up,&ipart);
				 if (delta_m_time>0.5) m_error=(delta_m_time-1.0)*nav_up;
				 else m_error=delta_m_time*nav_up;
			  }
			  else
			  {
				 if (delta_m_time>0.5) m_error=(delta_m_time-1.0)/nav_up;
				 else m_error=delta_m_time/nav_up;
			  }
			  TIC_cntr=(TIC_ref-m_error*TIC_ref/10)*(1.0-clock_offset*1.0e-6);
			  program_TIC(TIC_cntr);
			}
			rec_pos_llh.lon=rp_llh.lon;
			rec_pos_llh.lat=rp_llh.lat;
			rec_pos_llh.hae=rp_llh.hae;
			current_loc.lon=rp_llh.lon;
			current_loc.lat=rp_llh.lat;
			current_loc.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;
//
//  Calculate DOPS
//
			dops(n_track);
			if (out_pos==1) fprintf(stream,"time = %20.10lf, lat= %lf,long= %lf,hae= %lf ",
			m_time[1],rec_pos_llh.lat*r_to_d,rec_pos_llh.lon*r_to_d,rec_pos_llh.hae);
         if (out_vel==1) fprintf(stream,"vn= %lf, ve= %lf, vu= %lf ",
         receiver.vel.north,receiver.vel.east,receiver.vel.up);
         if (out_time==1) fprintf(stream," clock= %lf ",clock_offset);
         if (out_pos || out_vel || out_time) fprintf(stream,"hdop= %f,vdop= %f,tdop= %f\n",hdop,vdop,tdop);
//
//    Since we have a valid position/velocity narrow the doppler search window
//    to +-5 doppler bins
//
			search_max_f=5;
			m_time[0]=m_time[1];
  		}
	  }
	 }
	 else
	 {
			m_time[1]=m_time[1]+TIC_dt*(1.0+clock_offset/1.e6); // less than 4 sats
			rp_ecef.x=0.0;
			rp_ecef.y=0.0;
			rp_ecef.z=0.0;
			rpvt.xv=0.0;
			rpvt.yv=0.0;
			rpvt.zv=0.0;
	 }
	 if (out_kalman==1)   // Kalman filter output
	 {
			fprintf(kalm,
			"time %20.10lf, rpx %15.10lf, rpy %15.10lf, rpz %15.10lf, ",
			  m_time[1],rp_ecef.x,rp_ecef.y,rp_ecef.z);
			fprintf(kalm,"rvx %15.10lf, rvy %15.10lf, rvz %15.10lf, Nsats %d\n",
			  rpvt.xv,rpvt.yv,rpvt.zv,n_track);
	 }
	 for (i=1;i<=n_track;i++)
	 {
			chan[tr_ch[i]].Pr=(m_time[1]-(tr_time[i]-t_cor[i]))*c;
			chan[tr_ch[i]].dPr=meas_dop[i]*lambda;
			if (out_kalman==1)   // Kalman filter output
			{
				fprintf(kalm,"  PRN %2d,  px %20.10lf,  py %20.10lf, pz %20.10lf,  ",
				chan[tr_ch[i]].prn,track_sat[i].x,track_sat[i].y,track_sat[i].z);
				fprintf(kalm," vx %16.10lf, vy %16.10lf, vz %16.10lf,  ",
				d_sat[i].x,d_sat[i].y,d_sat[i].z);
				fprintf(kalm," Pr %20.10lf,  dPr %16.10lf\n",
				chan[tr_ch[i]].Pr,chan[tr_ch[i]].dPr);
			}
	 }