📄 gpsfuncs.cpp
字号:
PURPOSE
THIS FUNCTION DETERMINES THE SATELLITES TO SEARCH FOR
WHEN ALMANAC DATA IS AVAILABLE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
satvis satfind(char i)
{
float tdot,az;
float satang,alm_time,almanac_date;
double range1,range2,xls,yls,zls,xaz,yaz;
long jd_yr;
ecef gpspos1,gpspos2,north,east,up;
satvis result;
int jd_m;
struct tm *gmt;
double time_s;
/*
INITIALIZE ALL THE CONSTANTS
*/
// gotoxy(1,24);
// printf("->satfind");
putenv(tzstr);
tzset();
// thetime=time(NULL);
gmt=gmtime(&thetime);
// set up the correct time
if (gmt->tm_mon <= 1)
{
jd_yr =365.25*(gmt->tm_year-1.+1900.);
jd_m =30.6001*(gmt->tm_mon+14.);
}
else
{
jd_yr=365.25*(gmt->tm_year+1900.);
jd_m =30.6001*(gmt->tm_mon+2.);
}
time_s=gmt->tm_min/1440.+gmt->tm_sec/86400.+1720981.5+gmt->tm_hour/24.
+jd_yr+jd_m+gmt->tm_mday;
gps_week=int((time_s-2444244.5)/7.);
almanac_date=gps_alm[i].week*7.0+2444244.5;
if (gps_week-gps_alm[i].week>512) almanac_date+=1024*7.0;
alm_time=(time_s-almanac_date)*86400.;
clock_tow=(time_s-gps_week*7.-2444244.5)*86400.;
/*
CALCULATE THE POSITION OF THE SATELLITES
*/
if (gps_alm[i].inc > 0.0 && i>0)
{
gpspos1=satpos_almanac(alm_time,i);
gpspos2=satpos_almanac(alm_time+1.0,i);
/*
CALCULATE THE POSITION OF THE RECEIVER
*/
rec_pos_xyz=llh_to_ecef(current_loc);
north.x=-cos(current_loc.lon)*sin(current_loc.lat);
north.y=-sin(current_loc.lon)*sin(current_loc.lat);
north.z= cos(current_loc.lat);
east.x=-sin(current_loc.lon);
east.y= cos(current_loc.lon);
// east.z=0.0;
up.x=cos(current_loc.lon)*cos(current_loc.lat);
up.y=sin(current_loc.lon)*cos(current_loc.lat);
up.z=sin(current_loc.lat);
/*
DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
*/
xls =gpspos1.x-rec_pos_xyz.x;
yls =gpspos1.y-rec_pos_xyz.y;
zls =gpspos1.z-rec_pos_xyz.z;
range1=sqrt(xls*xls+yls*yls+zls*zls);
tdot=(up.x*xls+up.y*yls+up.z*zls)/range1;
xls =xls/range1;
yls =yls/range1;
zls =zls/range1;
range2=sqrt(pow(gpspos2.x-rec_pos_xyz.x-rpvt.xv,2)+
pow(gpspos2.y-rec_pos_xyz.y-rpvt.yv,2)+
pow(gpspos2.z-rec_pos_xyz.z-rpvt.zv,2));
if ( tdot >= 1.00 ) satang=pi/2.0;
else if ( tdot <= -1.00 ) satang=-pi/2.0;
else satang=asin(tdot);
xaz=east.x*xls+east.y*yls;
yaz=north.x*xls+north.y*yls+north.z*zls;
if (xaz !=0.0 || yaz !=0.0) az=atan2(xaz,yaz);
else az=0.0;
result.x=gpspos1.x;
result.y=gpspos1.y;
result.z=gpspos1.z;
result.elevation=satang;
result.azimuth =az;
result.doppler =(range1-range2)/lambda; // changed to lambda
}
// gotoxy(1,24);
// printf("satfind->");
return(result);
}
/*******************************************************************************
FUNCTION satpos_almanac(float time, char n)
RETURNS None.
PARAMETERS
time float time of week
n char satellite prn
PURPOSE
THIS SUBROUTINE CALCULATES THE SATELLITE POSITION
BASED ON ALMANAC DATA
R - RADIUS OF SATELLITE AT TIME T
SLAT - SATELLITE LATITUDE
SLONG- SATELLITE LONGITUDE
T - TIME FROM START OF WEEKLY EPOCH
ETY - ORBITAL ECCENTRICITY
TOA - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
INC - ORBITAL INCLINATION
RRA - RATE OF RIGHT ASCENSION
SQA - SQUARE ROOT OF SEMIMAJOR AXIS
LAN - LONGITUDE OF NODE AT WEEKLY EPOCH
AOP - ARGUMENT OF PERIGEE
MA - MEAN ANOMALY AT TOA
WRITTEN BY
Clifford Kelley
******************************************************************************/
ecef satpos_almanac( float time, char n)
{
double ei,ea,diff,r,ta,la,aol,xp,yp,d_toa;
ecef result;
/*
MA IS THE ANGLE FROM PERIGEE AT TOA
*/
// gotoxy(1,24);
// printf("->satpos_almanac");
d_toa = time - gps_alm[ n ].toa;
if (d_toa>302400.0) d_toa=d_toa-604800.0;
else if (d_toa<-302400.0)d_toa=d_toa+604800.0;
ei = gps_alm[ n ].ma + d_toa * gps_alm[ n ].w;
ea = ei;
do
{
diff=(ei-(ea-gps_alm[n].ety*sin(ea)))/(1.-gps_alm[n].ety*cos(ea));
ea=ea+diff;
} while (fabs(diff) > 1.0e-6);
/*
EA IS THE ECCENTRIC ANOMALY
*/
if (gps_alm[n].ety != 0.0 )
ta=atan2(sqrt(1.-pow(gps_alm[n].ety,2))*sin(ea),cos(ea)-gps_alm[n].ety);
else
ta=ea;
/*
TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
*/
r=pow(gps_alm[n].sqa,2)*(1.-gps_alm[n].ety*cos(ea));
/*
R IS THE RADIUS OF SATELLITE ORBIT AT TIME T
*/
aol=ta+gps_alm[n].aop;
/*
AOL IS THE ARGUMENT OF LATITUDE
LA IS THE LONGITUDE OF THE ASCENDING NODE
*/
la=gps_alm[n].lan+(gps_alm[n].rra-omegae)*d_toa-gps_alm[n].toa*omegae;
xp=r*cos(aol);
yp=r*sin(aol);
result.x=xp*cos(la)-yp*cos(gps_alm[n].inc)*sin(la);
result.y=xp*sin(la)+yp*cos(gps_alm[n].inc)*cos(la);
result.z=yp*sin(gps_alm[n].inc);
// gotoxy(1,24);
// printf("satpos_almanac->");
return(result);
}
/*******************************************************************************
FUNCTION satpos_ephemeris(double t, char n)
RETURNS None.
PARAMETERS
t double time of week
n char satellite prn
PURPOSE
THIS SUBROUTINE CALCULATES THE SATELLITE POSITION
BASED ON BROADCAST EPHEMERIS DATA
R - RADIUS OF SATELLITE AT TIME T
Crc - RADIUS COSINE CORRECTION TERM
Crs - RADIUS SINE CORRECTION TERM
SLAT - SATELLITE LATITUDE
SLONG- SATELLITE LONGITUDE
TOE - TIME OF EPHEMERIS FROM START OF WEEKLY EPOCH
ETY - ORBITAL INITIAL ECCENTRICITY
TOA - TIME OF APPLICABILITY FROM START OF WEEKLY EPOCH
INC - ORBITAL INCLINATION
IDOT - RATE OF INCLINATION ANGLE
CUC - ARGUMENT OF LATITUDE COSINE CORRECTION TERM
CUS - ARGUMENT OF LATITUDE SINE CORRECTION TERM
CIC - INCLINATION COSINE CORRECTION TERM
CIS - INCLINATION SINE CORRECTION TERM
RRA - RATE OF RIGHT ASCENSION
SQA - SQUARE ROOT OF SEMIMAJOR AXIS
LAN - LONGITUDE OF NODE AT WEEKLY EPOCH
AOP - ARGUMENT OF PERIGEE
MA - MEAN ANOMALY AT TOA
DN - MEAN MOTION DIFFERENCE
WRITTEN BY
Clifford Kelley
*******************************************************************************/
eceft satpos_ephemeris(double t,char n)
{
double ei,ea,diff,ta,aol,delr,delal,delinc,r,inc;
double la,xp,yp,bclk,tc,d_toc,d_toe;
double xls,yls,zls,range1,tdot,satang,xaz,yaz;
double az;
ecef north,east,up;
eceft result;
//
// MA IS THE ANGLE FROM PERIGEE AT TOA
//
// gotoxy(1,24);
// printf("->satpos_ephemeris");
d_toc=t-gps_eph[n].toc;
if (d_toc>302400.0) d_toc=d_toc-604800.0;
else if (d_toc<-302400.0)d_toc=d_toc+604800.0;
bclk=gps_eph[n].af0+gps_eph[n].af1*d_toc+gps_eph[n].af2*d_toc*d_toc
-gps_eph[n].tgd;
tc=t-bclk;
d_toe=tc-gps_eph[n].toe;
if (d_toe>302400.0) d_toe=d_toe-604800.0;
else if (d_toe<-302400.0)d_toe=d_toe+604800.0;
ei=gps_eph[n].ma+d_toe*(gps_eph[n].wm+gps_eph[n].dn);
ea=ei;
do
{
diff=(ei-(ea-gps_eph[n].ety*sin(ea)))/(1.0E0-gps_eph[n].ety*cos(ea));
ea=ea+diff;
} while (fabs(diff) > 1.0e-12 );
bclk=bclk-4.442807633E-10*gps_eph[n].ety*gps_eph[n].sqra*sin(ea);
result.tb=bclk;
//
// ea is the eccentric anomaly
//
ta=atan2(sqrt(1.00-pow(gps_eph[n].ety,2))*sin(ea),cos(ea)-gps_eph[n].ety);
//
// TA IS THE TRUE ANOMALY (ANGLE FROM PERIGEE)
//
aol=ta+gps_eph[n].w;
//
// AOL IS THE ARGUMENT OF LATITUDE OF THE SATELLITE
//
// calculate the second harmonic perturbations of the orbit
//
delr =gps_eph[n].crc*cos(2.0*aol)+gps_eph[n].crs*sin(2.0*aol);
delal =gps_eph[n].cuc*cos(2.0*aol)+gps_eph[n].cus*sin(2.0*aol);
delinc=gps_eph[n].cic*cos(2.0*aol)+gps_eph[n].cis*sin(2.0*aol);
//
// R IS THE RADIUS OF SATELLITE ORBIT AT TIME T
//
r=pow(gps_eph[n].sqra,2)*(1.00-gps_eph[n].ety*cos(ea))+delr;
aol=aol+delal;
inc=gps_eph[n].inc0+delinc+gps_eph[n].idot*d_toe;
// WRITE(6,*)T-TOE(N)
//
// LA IS THE CORRECTED LONGITUDE OF THE ASCENDING NODE
//
la=gps_eph[n].w0+(gps_eph[n].omegadot-omegae)*d_toe-
omegae*gps_eph[n].toe;
xp=r*cos(aol);
yp=r*sin(aol);
result.x=xp*cos(la)-yp*cos(inc)*sin(la);
result.y=xp*sin(la)+yp*cos(inc)*cos(la);
result.z=yp*sin(inc);
result.az=0.0;
result.el=0.0;
if (rec_pos_xyz.x != 0.0 || rec_pos_xyz.y != 0.0 || rec_pos_xyz.z != 0.0)
{
/*
CALCULATE THE POSITION OF THE RECEIVER
*/
north.x=-cos(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
north.y=-sin(rec_pos_llh.lon)*sin(rec_pos_llh.lat);
north.z= cos(rec_pos_llh.lat);
east.x=-sin(rec_pos_llh.lon);
east.y= cos(rec_pos_llh.lon);
east.z=0.0;
up.x=cos(rec_pos_llh.lon)*cos(rec_pos_llh.lat);
up.y=sin(rec_pos_llh.lon)*cos(rec_pos_llh.lat);
up.z=sin(rec_pos_llh.lat);
/*
DETERMINE IF A CLEAR LINE OF SIGHT EXISTS
*/
xls =result.x-rec_pos_xyz.x;
yls =result.y-rec_pos_xyz.y;
zls =result.z-rec_pos_xyz.z;
range1=sqrt(xls*xls+yls*yls+zls*zls);
tdot=(up.x*xls+up.y*yls+up.z*zls)/range1;
if ( tdot >= 1.00 ) satang=pi/2.0;
else if ( tdot <= -1.00 ) satang=-pi/2.0;
else satang=asin(tdot);
xaz=east.x*xls+east.y*yls;
yaz=north.x*xls+north.y*yls+north.z*zls;
if (xaz !=0.0 || yaz !=0.0) az=atan2(xaz,yaz);
else az=0.0;
result.el=satang;
result.az=az;
/* if (satang<(mask_angle-.035))
{
fprintf(kalm,"prn %d LOS problem time=%lf\n",n,t);
fprintf(kalm,"lat=%lf long=%lf hae=%lf\n",
rec_pos_llh.lat,rec_pos_llh.lon,rec_pos_llh.hae);
fprintf(kalm,"rcvr x=%lf y=%lf z=%lf\n",rec_pos_xyz.x,rec_pos_xyz.y,rec_pos_xyz.z);
fprintf(kalm,"sat x=%lf y=%lf z=%lf\n",result.x,result.y,result.z);
write_Kalm_ephemeris(n);
}*/
}
// gotoxy(1,24);
// printf("satpos_ephemeris->");
return(result);
}
/*******************************************************************************
FUNCTION read_initial_data(void)
RETURNS None.
PARAMETERS None.
PURPOSE
To read in all of the receiver initialization files
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void read_initial_data(void)
{
int id;
satvis dummy;
for (id=1;id<=32;id++) gps_alm[id].inc=0.0;
/*
READ THE INPUT DATA FILE(s)
*/
status=cold_start; // for initialization, if we have enough data we
read_ion_utc(); // will switch to warm or hot start
thetime=time(NULL);
read_almanac();
dummy=satfind(0);
}
/*******************************************************************************
FUNCTION receiver_loc(void)
RETURNS None.
PARAMETERS None.
PURPOSE
To read in the last location of the receiver from file "curloc.dat"
to help in warm and hot starts
WRITTEN BY
Clifford Kelley
*******************************************************************************/
llh receiver_loc(void)
{
float latitude,longitude,height;
char text[10];
llh result;
result.lat=0.0;
result.lon=0.0;
result.hae=0.0;
/*
READ THE CURRENT LOCATION DATA FILE
*/
if ((in = fopen("CURLOC.DAT", "rt")) == NULL)
{
printf("Cannot open curloc.dat file.\n");
status=cold_start;
}
else
{
fscanf(in,"%10s",text);
fscanf(in,"%f",&latitude);
fscanf(in,"%10s",text);
fscanf(in,"%f",&longitude);
fscanf(in,"%10s",text);
fscanf(in,"%f",&height);
result.lat=latitude/57.296;
result.lon=longitude/57.296;
result.hae=height;
}
fclose(in);
return(result);
}
/*******************************************************************************
FUNCTION navmess()
RETURNS None.
PARAMETERS None.
PURPOSE
This function assembles and decodes the 1500 bit nav message
into almanac and ephemeris messages
WRITTEN BY
Clifford Kelley
3-2-2002 Made corrections suggested by Georg Beyerle GFZ
*******************************************************************************/
void navmess(char prn,char ch)
{
int i,j,k;
unsigned long isqra,ie;
long iaf0,iomegadot,iomega0;
char itgd,iaf2;
// int icapl2;
int iweek,iura,ihealth,iodc,iaf1;
unsigned int itoe,itoc;
// int fif;
int iode,icrs,idn,icuc,icus,icic,iomegad;
int icis,icrc,idoe,idot;
unsigned int iae,iatoa;
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -