📄 gpsrcvr.cpp
字号:
printf("meas time %f error %f delta %f\n",m_time[1],m_error,delta_m_time);
cur_lat.deg=rec_pos_llh.lat*r_to_d;
cur_lat.min=(rec_pos_llh.lat*r_to_d-cur_lat.deg)*60;
cur_lat.sec=(rec_pos_llh.lat*r_to_d-cur_lat.deg-cur_lat.min/60.)*3600.;
cur_long.deg=rec_pos_llh.lon*r_to_d;
cur_long.min=(rec_pos_llh.lon*r_to_d-cur_long.deg)*60;
cur_long.sec=(rec_pos_llh.lon*r_to_d-cur_long.deg-cur_long.min/60.)*3600.;
printf(" latitude longitude HAE clock error (ppm)\n");
printf(" %4d:%2d:%5.2f %4d:%2d:%5.2f %10.2f %f\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 Heading TIC_dt\n");
printf(" %lf %lf %lf\n",speed,heading*r_to_d,TIC_dt);
printf(" \n");
printf("tracking %2d status %1d almanac valid %1d gps week %4d\n",
n_track,status,almanac_valid,gps_week%1024);
if (display_page==0)
{
printf(
" ch prn state n_freq az el doppler t_count n_frame sfid ura page missed CNo\n");
for (ch=0;ch<=11;ch++)
{
printf(
" %2d %2d %2d %3d %4.0f %3.0f %6.0f %4d %3d %3d %3d %3d%5d %4.1f\n",
ch,chan[ch].prn,chan[ch].state,chan[ch].n_freq,
xyz[chan[ch].prn].azimuth*57.3,xyz[chan[ch].prn].elevation*57.3,
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);
}
}
else if (display_page==1)
{
printf( " ch prn state TLM TOW Health Valid TOW_sync offset\n");
for (ch=0;ch<=11;ch++)
{
printf(" %2d %2d %2d %6ld %6ld %2d %2d %2d %d\n",
ch, chan[ch].prn, chan[ch].state, chan[ch].TLM, chan[ch].TOW,
gps_eph[chan[ch].prn].health,gps_eph[chan[ch].prn].valid,chan[ch].tow_sync,
chan[ch].offset);
}
}
else if (display_page==2)
{
printf(
" ch prn state n_freq az el tropo iono\n");
for (ch=0;ch<=11;ch++)
{
printf(
" %2d %2d %2d %3d %4.0f %3.0f %10.4lf %10.4lf\n",
ch, chan[ch].prn, chan[ch].state, chan[ch].n_freq,
xyz[chan[ch].prn].azimuth*57.3, xyz[chan[ch].prn].elevation*57.3,
tropo[ch]*c, iono[ch]*c);
}
}
else if (display_page==3)
{
printf(
" ch prn state Pseudorange delta Pseudorange\n");
for (ch=0;ch<=11;ch++)
{
printf(
" %2d %2d %2d %20.10lf %15.10lf\n",
ch, chan[ch].prn, chan[ch].state, Pr[ch], dPr[ch]);
}
}
printf(" GDOP=%6.3f HDOP=%6.3f VDOP=%6.3f TDOP=%6.3f\n",gdop,hdop,vdop,tdop);
}
/*******************************************************************************
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.
*******************************************************************************/
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,GPS_Interrupt);
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
}
/*******************************************************************************
FUNCTION Interrupt_Remove()
RETURNS None.
PARAMETERS None.
PURPOSE
This function removes the custom interrupt vector from the vector
table and restores the previous vector.
*******************************************************************************/
void Interrupt_Remove()
{
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);
}
/*******************************************************************************
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 ipart;
double tr_time[13],tr_avg;
static double t_cor[13];
unsigned int i,ms,chip,phase,tr_ch[13];
ecef rp_ecef;
eceft dm_gps_sat[13],dp_gps_sat[13];
tr_avg=0.0;
n=1;
for (ch=0;ch<=11;ch++)
{
ms=chan[ch].epoch & 0x1f;
chip=chan[ch].code_phase;
phase=chan[ch].code_dco_phase;
bit=chan[ch].epoch>>8;
// Use only satellites being tracked with valid ephemeris data
// (valid subframe 1,2,3), good health,high enough raw snr, and
// valid phase data
fprintf(kalm," ch=%d bit time= %ld bit= %d ms= %d chip= %d phase= %d\n",
ch,chan[ch].meas_bit_time,bit,ms,chip,phase);
if (chan[ch].meas_bit_time%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*.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 (out_debug) fprintf(stream,"%d\n",n_track);
for (i=1;i<=n_track;i++)
{
chan[tr_ch[i]].int_carr_phase=chan[tr_ch[i]].carrier_counter+
float(chan[tr_ch[i]].d_carr_phase)/1024.;
track_sat[i]=satpos_ephemeris(tr_time[i],chan[tr_ch[i]].prn);
dm_gps_sat[i]=satpos_ephemeris(tr_time[i]-TIC_dt,chan[tr_ch[i]].prn);
dp_gps_sat[i]=satpos_ephemeris(tr_time[i],chan[tr_ch[i]].prn);
if (out_debug) 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(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]);
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;
// meas_icp[i]= chan[tr_ch[i]].int_carr_phase/TIC_dt-1.405396826e6;
meas_dop[i]= chan[tr_ch[i]].int_carr_phase/TIC_dt-1.405396826e6;
}
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 quick 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)
{
if (fabs(clock_error)<5.0) clock_offset=clock_error;
if (almanac_valid==1) status=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;
dops(n_track);
if (out_pos==1) fprintf(stream,
"time = %20.10lf, lat= %lf,long= %lf,hae= %lf,gdop= %f,hdop= %f,vdop= %f\n",
m_time[1],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
// 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++)
{
Pr[tr_ch[i]]=(m_time[1]-(tr_time[i]-t_cor[i]))*c;
dPr[tr_ch[i]]=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",
Pr[tr_ch[i]],dPr[tr_ch[i]]);
}
}
// gotoxy(1,24);
// printf("nav_fix-> lat %lf lon %lf hae %lf",
// rec_pos_llh.lat*r_to_d,rec_pos_llh.lon*r_to_d,rec_pos_llh.hae);
}
/*******************************************************************************
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;
// gotoxy(1,24);
// printf("->chan_allocate");
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;
fprintf(out,"prn %d inc=%f alm_week=%d gps_week=%d\n",
prnn,gps_alm[prnn].inc,gps_alm[prnn].week,gps_week);
}
}
if (al0==0.0 && b0==0.0)almanac_valid=0;
for (ch=0;ch<=11;ch++) // if the sat has dropped below mask angle
// turn the channel off
{
if (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.00)
{
alloc=0;
for (ch=0;ch<=11;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<=11;ch++)
{
reset_cntl(0x1fff);
if (chan[ch].state==off)
{
// calculate carrier clock and doppler correction
chan[ch].carrier_corr=(-xyz[prnn].doppler-
clock_offset*1575.42)/42.57475e-3;
// calculate code clock and doppler correction
code_corr=clock_offset*24.+xyz[prnn].doppler/65.5;
chan[ch].code_freq=code_ref+code_corr;
ch_code(ch,chan[ch].code_freq); // 1.023 MHz chipping rate
ch_cntl(ch,prn_code[prnn]|0xa000); // select satellite
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+
d_freq*chan[ch].n_freq; // set carrier
ch_carrier(ch,chan[ch].carrier_freq); // select carrier
break;
}
}
}
}
}
// gotoxy(1,24);
// printf("chan_allocate->");
}
/*******************************************************************************
FUNCTION GPS_Interrupt()
RETURNS None.
PARAMETERS None.
PURPOSE
This function replaces the current IRQ0 Interrupt service routine with
our GPS function which will perform the acquisition - tracking functions
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void interrupt GPS_Interrupt(...)
{
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -