📄 gpsfuncs.cpp
字号:
PURPOSE checks the parity of the 5 subframes of the nav message
WRITTEN BY
Clifford Kelley
*******************************************************************************/
void parity_check(void)
{
long pb1=0x3b1f3480L,pb2=0x1d8f9a40L,pb3=0x2ec7cd00L;
long pb4=0x1763e680L,pb5=0x2bb1f340L,pb6=0x0b7a89c0L;
int parity,m_parity;
char d29=0,d30=0,sfm,word;
int err_bit;
for (sfm=1;sfm<=5;sfm++)
{
p_error[sfm]=0;
for (word=1;word<=10;word++)
{
m_parity=int(sf[sfm][word] &0x3f);
//
// try new parity generation methodology
//
parity=exor(d29,sf[sfm][word] & pb1) << 1;
parity=(parity | exor(d30,sf[sfm][word] & pb2)) << 1;
parity=(parity | exor(d29,sf[sfm][word] & pb3)) << 1;
parity=(parity | exor(d30,sf[sfm][word] & pb4)) << 1;
parity=(parity | exor(0,sf[sfm][word] & pb5)) << 1;
parity= parity | exor(d29^d30,sf[sfm][word] & pb6);
err_bit=0;
if (parity != m_parity)
{
err_bit=1;
}
p_error[sfm]=(p_error[sfm] << 1) + err_bit;
if (d30==1) sf[sfm][word]=0x03fffffc0L & ~sf[sfm][word];
sf[sfm][word]=sf[sfm][word]>>6;
d29=(m_parity & 0x2) >>1;
d30=m_parity & 0x1;
}
}
}
/*******************************************************************************
FUNCTION exor(char bit, long parity)
RETURNS None.
PARAMETERS
bit char
parity long
PURPOSE
count the number of bits set in the parameter parity and
do an exclusive or with the parameter bit
WRITTEN BY
Clifford Kelley
*******************************************************************************/
int exor(char bit, long parity)
{
char i;
int result;
result=0;
for (i=7;i<=30;i++)
{
if (bit_test_l(parity,i)) result++;
}
result=result%2;
result=(bit ^ result) & 0x1;
return(result);
}
/*******************************************************************************
FUNCTION ecef_to_llh(ecef pos)
RETURNS position in llh structure
PARAMETERS
pos ecef
PURPOSE Convert a position in in cartesian ecef coordinates to
Geodetic WGS 84 coordinates
Based on equations found in Hoffman-Wellinhoff
WRITTEN BY
Clifford Kelley
*******************************************************************************/
llh ecef_to_llh(ecef pos)
{
double p,n,thet,esq,epsq;
llh result;
// gotoxy(1,24);
// printf("->ecef_to_llh");
p=sqrt(pos.x*pos.x+pos.y*pos.y);
thet=atan(pos.z*a/(p*b));
esq =1.0-b*b/(a*a);
epsq=a*a/(b*b)-1.0;
result.lat=atan((pos.z+epsq*b*pow(sin(thet),3))/(p-esq*a*pow(cos(thet),3)));
result.lon=atan2(pos.y,pos.x);
n=a*a/sqrt(a*a*cos(result.lat)*cos(result.lat) +
b*b*sin(result.lat)*sin(result.lat));
result.hae=p/cos(result.lat)-n;
// gotoxy(1,24);
// printf("ecef_to_llh->");
return(result);
}
/*******************************************************************************
FUNCTION llh_to_ecef(llh pos)
RETURNS position in ecef structure
PARAMETERS
pos llh
PURPOSE Convert a position in Geodetic WGS 84 coordinates to cartesian
ecef coordinates
Based on equations found in Hoffman-Wellinhoff
WRITTEN BY
Clifford Kelley
*******************************************************************************/
ecef llh_to_ecef(llh pos)
{
double n;
// gotoxy(1,24);
// printf("->llh_to_ecef");
ecef result;
n=a*a/sqrt(a*a*cos(pos.lat)*cos(pos.lat)+b*b*sin(pos.lat)*sin(pos.lat));
result.x=(n+pos.hae)*cos(pos.lat)*cos(pos.lon);
result.y=(n+pos.hae)*cos(pos.lat)*sin(pos.lon);
result.z=(b*b/(a*a)*n+pos.hae)*sin(pos.lat);
// gotoxy(1,24);
// printf("llh_to_ecef->");
return(result);
}
/*******************************************************************************
FUNCTION pos_vel_time(int nsl)
RETURNS None.
PARAMETERS
nsl int
PURPOSE
This routine processes the all-in-view pseudorange to arrive
at a receiver position
INPUTS:
pseudo_range[nsl] Vector of measured range from satellites to the receiver
sat_location[nsl][3] Array of satellite locations in ECEF when the signal
was sent
nsl number of satellites used
OUTPUTS:
RP[3] VECTOR OF RECEIVER POSITION IN ECEF (X,Y,Z)
CBIAS RECEIVER CLOCK BIAS FROM GPS TIME
VARIABLES USED:
C SPEED OF LIGHT IN VACUUM IN M/S
S[6][5] MATRIX USED FOR SOLVING FOR RECEIVER POSITION CORRECTION
B[5] RESULTING RECEIVER CLOCK BIAS & POSITION CORRECTIONS
X,Y,Z TEMPORARY RECEIVER POSITION
T TEMPORARY RECEIVER CLOCK BIAS
R[5] RANGE FROM RECEIVER TO SATELLITES
IF THE POSITION CANNOT BE DETERMINED THE RESULT OF RP
WILL BE (0,0,0) THE CENTER OF THE EARTH
WRITTEN BY
Clifford Kelley
*******************************************************************************/
pvt pos_vel_time( int nsl)
{
double dd[5][5],r,ms[5][13],pm[5][13],bm[13],br[5],correct_mag,x,y,z,t;
double a1,b1,c1,d1,e1,f1,g1,h1,i1,j1,k1,l1,m1,n1,o1,p1,denom,alpha;
int i,j,k,nits;
pvt result;
//
// USE ITERATIVE APPROACH TO SOLVING FOR THE POSITION OF
// THE RECEIVER
//
// gotoxy(1,24);
// printf("->pos_vel_time");
nits=0;
t=0.0;
x=rec_pos_xyz.x;
y=rec_pos_xyz.y;
z=rec_pos_xyz.z;
//printf("a %lf,%lf,%lf\n",x,y,z);
do
{
for (i=1;i<=nsl;i++)
{
//
// Compute range in ECI at the time of arrival at the receiver
//
alpha=(t-dt[i])*omegae;
r=sqrt(pow(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x,2)+
pow(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y,2)+
pow(track_sat[i].z-z,2));
bm[i]=r-(dt[i]-t)*c;
ms[1][i]=(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x)/r;
ms[2][i]=(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y)/r;
ms[3][i]=(track_sat[i].z-z)/r;
ms[4][i]=1.0;
}
a1=0.;b1=0.;c1=0.;d1=0.;
e1=0.;f1=0.;g1=0.;h1=0.;
i1=0.;j1=0.;k1=0.;l1=0.;
m1=0.;n1=0.;o1=0.;p1=0.;
for (k=1;k<=nsl;k++)
{
a1+=ms[1][k]*ms[1][k];
b1+=ms[1][k]*ms[2][k];
c1+=ms[1][k]*ms[3][k];
d1+=ms[1][k]*ms[4][k];
// e1+=ms[2][k]*ms[1][k]; for completeness, the matrix is symmetric
f1+=ms[2][k]*ms[2][k];
g1+=ms[2][k]*ms[3][k];
h1+=ms[2][k]*ms[4][k];
// i1+=ms[3][k]*ms[1][k];
// j1+=ms[3][k]*ms[2][k];
k1+=ms[3][k]*ms[3][k];
l1+=ms[3][k]*ms[4][k];
// m1+=ms[1][k];
// n1+=ms[2][k];
// o1+=ms[3][k];
p1+=ms[4][k];
}
o1=l1;m1=d1;n1=h1;e1=b1;i1=c1;j1=g1;
/*
SOLVE FOR THE MATRIX INVERSE
*/
denom=(k1*p1-l1*o1)*(a1*f1-b1*e1) + (l1*n1-j1*p1)*(a1*g1-c1*e1) +
(j1*o1-k1*n1)*(a1*h1-d1*e1) + (l1*m1-i1*p1)*(c1*f1-b1*g1) +
(i1*o1-k1*m1)*(d1*f1-b1*h1) + (i1*n1-j1*m1)*(c1*h1-d1*g1);
dd[1][1]=f1*(k1*p1-l1*o1)+g1*(l1*n1-j1*p1)+h1*(j1*o1-k1*n1);
dd[1][2]=e1*(l1*o1-k1*p1)+g1*(i1*p1-l1*m1)+h1*(k1*m1-i1*o1);
dd[1][3]=e1*(j1*p1-n1*l1)-i1*(f1*p1-n1*h1)+m1*(f1*l1-j1*h1);
dd[1][4]=e1*(n1*k1-j1*o1)+i1*(f1*o1-n1*g1)+m1*(j1*g1-f1*k1);
// dd[2][1]=b1*(l1*o1-k1*p1)+j1*(c1*p1-d1*o1)+n1*(d1*k1-c1*l1);
dd[2][1]=dd[1][2];
dd[2][2]=a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1);
dd[2][3]=a1*(l1*n1-j1*p1)+i1*(b1*p1-n1*d1)+m1*(j1*d1-b1*l1);
dd[2][4]=a1*(j1*o1-n1*k1)-i1*(b1*o1-n1*c1)+m1*(b1*k1-c1*j1);
// dd[3][1]=b1*(g1*p1-h1*o1)-f1*(c1*p1-o1*d1)+n1*(c1*h1-d1*g1);
dd[3][1]=dd[1][3];
// dd[3][2]=a1*(o1*h1-g1*p1)+e1*(c1*p1-o1*d1)+m1*(d1*g1-c1*h1);
dd[3][2]=dd[2][3];
dd[3][3]=a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1);
dd[3][4]=a1*(n1*g1-f1*o1)+e1*(b1*o1-c1*n1)+m1*(c1*f1-b1*g1);
// dd[4][1]=b1*(h1*k1-g1*l1)+f1*(c1*l1-d1*k1)+j1*(d1*g1-c1*h1);
dd[4][1]=dd[1][4];
// dd[4][2]=a1*(g1*l1-h1*k1)-e1*(c1*l1-d1*k1)+i1*(c1*h1-d1*g1);
dd[4][2]=dd[2][4];
// dd[4][3]=a1*(j1*h1-f1*l1)+e1*(b1*l1-d1*j1)+i1*(d1*f1-b1*h1);
dd[4][3]=dd[3][4];
dd[4][4]=a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1);
if ( denom<=0.0 )
{
result.x=1.0; // something went wrong
result.y=1.0; // set solution to near center of earth
result.z=1.0;
result.dt=0.0;
}
else
{
for (i=1;i<=4;i++)
{
for (j=1;j<=4;j++) dd[i][j]=dd[i][j]/denom;
}
for (i=1;i<=nsl;i++)
{
for (j=1;j<=4;j++)
{
pm[j][i]=0.0;
for (k=1;k<=4;k++)pm[j][i]+=dd[j][k]*ms[k][i];
}
}
for (i=1;i<=4;i++)
{
br[i]=0.0;
for (j=1;j<=nsl;j++)br[i]+=bm[j]*pm[i][j];
}
nits++;
x=x+br[1];
y=y+br[2];
z=z+br[3];
t=t-br[4]/c;
// printf("%lf,%lf,%lf,%20.10lf\n",x,y,z,t);
// printf("%lf,%lf,%lf,%lf\n",br[1],br[2],br[3],br[4]);
correct_mag=sqrt(br[1]*br[1]+br[2]*br[2]+br[3]*br[3]);
// printf("mag=%lf\n",correct_mag);
}
} while ( correct_mag > 0.01 && correct_mag < 1.e8 && nits < 10);
result.x=x;
result.y=y;
result.z=z;
result.dt=t;
//
// Now for Velocity
//
for (i=1;i<=nsl;i++)
{
alpha=(dt[i]-t)*omegae;
r=sqrt(pow(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x,2)+
pow(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y,2)+
pow(track_sat[i].z-z,2));
bm[i]=((track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-x)*d_sat[i].x+
(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-y)*d_sat[i].y+
(track_sat[i].z-z)*d_sat[i].z)/r-meas_dop[i]*lambda;
}
for (i=1;i<=4;i++)
{
br[i]=0.0;
for (j=1;j<=nsl;j++)br[i]+=bm[j]*pm[i][j];
}
result.xv=br[1]+y*omegae; // get rid of earth
result.yv=br[2]-x*omegae; // rotation rate
result.zv=br[3];
result.df=br[4]/c*1000000.0; // frequency error in parts per million (ppm)
// gotoxy(1,24);
// printf("pos_vel_time->");
return(result);
}
/*******************************************************************************
FUNCTION dops(int nsl)
RETURNS None.
PARAMETERS
nsl int
PURPOSE
This routine computes the dops
INPUTS:
sat_location[nsl][3] Array of satellite locations in ECEF when the signal
was sent
nsl number of satellites used
receiver position
OUTPUTS:
hdop = horizontal dilution of precision (rss of ndop & edop)
vdop = vertical dilution of precision
tdop = time dilution of precision
pdop = position dilution of precision (rss of vdop & hdop)
gdop = geometric dilution of precision (rss of pdop & tdop)
WRITTEN BY
Clifford Kelley
*******************************************************************************/
#ifdef VCPP
void dops( int nsl)
{
double r,xls,yls,zls;
// double det;
int i;
Matrix H(nsl,4),G(4,4);
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);
for (i=1;i<=nsl;i++)
{
//
// Compute line of sight vectors
//
xls=track_sat[i].x-rec_pos_xyz.x;
yls=track_sat[i].y-rec_pos_xyz.y;
zls=track_sat[i].z-rec_pos_xyz.z;
r=sqrt(xls*xls+yls*yls+zls*zls);
H(i,1)=(xls*receiver.north.x+yls*receiver.north.y+zls*receiver.north.z)/r;
H(i,2)=(xls*receiver.east.x+yls*receiver.east.y)/r;
H(i,3)=(xls*receiver.up.x+yls*receiver.up.y+zls*receiver.up.z)/r;
H(i,4)=1.0;
}
// G=(H.transpose()*H).inverse(); //for Alberto's library
G=(H.t()*H).i();
hdop=sqrt(G(1,1)+G(2,2));
vdop=sqrt(G(3,3));
tdop=sqrt(G(4,4));
pdop=sqrt(G(1,1)+G(2,2)+G(3,3));
gdop=sqrt(G(1,1)+G(2,2)+G(3,3)+G(4,4));
// gotoxy(1,24);
// printf("->dops");
}
#endif
#ifdef BCPP
void dops( int nsl)
{
double ddx,ddy,ddz,ddt,r,rxy,ms[5][13],x,y,z;
double a1,b1,c1,d1,e1,f1,g1,h1,i1,j1,k1,l1,m1,n1,o1,p1,denom;
double msx,msy,msz;
int i,k;
ecef north,east,up;
// gotoxy(1,24);
// printf("->dops");
x=rec_pos_xyz.x;
y=rec_pos_xyz.y;
z=rec_pos_xyz.z;
r=sqrt(x*x+y*y+z*z);
rxy=sqrt(x*x+y*y);
north.x=-x/rxy*z/r;
north.y=-y/rxy*z/r;
north.z= rxy/r;
east.x=-y/rxy;
east.y= x/rxy;
//east.z=0.0; just for completeness
up.x=x/r;
up.y=y/r;
up.z=z/r;
for (i=1;i<=nsl;i++)
{
//
// Compute line of sight vectors
//
r=sqrt(pow(track_sat[i].x-x,2)+
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -