nav_fix.cpp

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

}
#endif

#ifdef VCPP

void  nav_fix(void)    // Using a Kalman Filter
{
	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>3 || init==0)
	{
	 rpvt=Kalman_filter(n_track);
	 cbias=rpvt.dt/c;
	 clock_error=rpvt.df/c*1000000.0;// frequency error in parts per million (ppm)
//	 m_time[1]=m_time[1]-cbias;
	 rp_ecef.x=rpvt.x;
	 rp_ecef.y=rpvt.y;
	 rp_ecef.z=rpvt.z;
	 if (out_kalman==1)
	 {   
	    fprintf(kalm,"m_time %20.12lf, n_track %d\n",m_time[1],n_track);
	    for (i=1;i<=n_track;i++)
	    {
	      fprintf(kalm,"Pr  %20.12lf, px %20.12lf, py %20.12lf, pz %20.12lf\n",
	      dt[i],track_sat[i].x,track_sat[i].y,track_sat[i].z);  
	    }
	    for (i=1;i<=n_track;i++)
	    {
	      fprintf(kalm,"dPr  %20.12lf, vx %20.12lf, vy %20.12lf, vz %20.12lf\n",
	      meas_dop[i],d_sat[i].x,d_sat[i].y,d_sat[i].z);  
	    }                
	  }
	  for (i=1;i<=n_track;i++)
	  {
          chan[tr_ch[i]].Pr=rpvt.dt+dt[i]*c;
		  chan[tr_ch[i]].dPr=rpvt.df+meas_dop[i]*lambda;
	  }                
	 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
//
			if (n_track>3) 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[1]=m_time[1]+TIC_dt; // update measurement time
}
#endif                             
/*******************************************************************************
FUNCTION velocity(void)
RETURNS  None.

PARAMETERS None.

PURPOSE  To convert velocity from ecef to local level (WGS-84) axes

WRITTEN BY
	Clifford Kelley

*******************************************************************************/
void velocity(void)
{
    receiver.north.x=-cos(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
    receiver.north.y=-sin(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
    receiver.north.z= cos(rec_pos_llh.lat);
    receiver.east.x=-sin(rec_pos_llh.lon);
    receiver.east.y= cos(rec_pos_llh.lon);
//  receiver.east.z=0.0;
    receiver.up.x=cos(rec_pos_llh.lon)*cos(rec_pos_llh.lat);
    receiver.up.y=sin(rec_pos_llh.lon)*cos(rec_pos_llh.lat);
    receiver.up.z=sin(rec_pos_llh.lat);

	 receiver.vel.north = rpvt.xv*receiver.north.x + rpvt.yv*receiver.north.y +
                         rpvt.zv*receiver.north.z;
	 receiver.vel.east  = rpvt.xv*receiver.east.x + rpvt.yv*receiver.east.y;
	 receiver.vel.up    = rpvt.xv*receiver.up.x + rpvt.yv*receiver.up.y +
                         rpvt.zv*receiver.up.z;

	 speed=sqrt(receiver.vel.north*receiver.vel.north+receiver.vel.east*receiver.vel.east);
	 if (speed==0.0) heading=0.0;
	 else heading=atan2(receiver.vel.east,receiver.vel.north);

}