?? gpsfunc.cpp
字號:
inc = EphPtr->inc0 + delinc + EphPtr->idot*d_toe;
// LA IS THE CORRECTED LONGITUDE OF THE ASCENDING NODE
la = EphPtr->w0 + (EphPtr->omegadot - OmegaDotEarth)*d_toe
- OmegaDotEarth*EphPtr->toe;
xp = r*cos(aol);
yp = r*sin(aol);
Pos[0] = xp*cos(la) - yp*cos(inc)*sin(la);
Pos[1] = xp*sin(la) + yp*cos(inc)*cos(la);
Pos[2] = yp*sin(inc);
}
//--------------------------------------------------------------------
// calculate sat position using almanac data and time of week
void GPS_Func::SatPosECEFAlminfo(AlmInfo *AlmPtr, double Time, int WeekNo, double *Pos)
{
double d_toa, ei,ea,diff, m_m, ta ,r, aol, la, omegae,xp,yp;
double satPos[3];
// omegae = 7.2921151467E-5;
d_toa = Time - AlmPtr->Toa;
if( d_toa > 302400.0)
d_toa = d_toa - 604800.0;
else if( d_toa < -302400.0)
d_toa = d_toa + 604800.0;
// mean motion is
m_m = 19964981.84/pow(AlmPtr->SqrtA,3);
// recursively calc eccentric anomaly E
ei = AlmPtr->M0 + d_toa*m_m;
ea = ei;
do{
diff = (ei - (ea - AlmPtr->E*sin(ea)))/(1. - AlmPtr->E*cos(ea));
ea = ea + diff;
} while( fabs(diff) > 1.0e-6);
// Now ea is eccentric anomaly E
//ta is the true anomaly
if( AlmPtr->E != 0.0)
ta = atan2(sqrt(1. - AlmPtr->E * AlmPtr->E) * sin(ea),cos(ea)-AlmPtr->E);
else
ta = ea;
//r is the radius of satellite orbit at time Time
r = AlmPtr->SqrtA*AlmPtr->SqrtA*(1.0 - AlmPtr->E*cos(ea));
// aol is the argument of latitude
aol = ta + AlmPtr->Omega;
//
la = AlmPtr->Omega0 + (AlmPtr->OmegaDot-OmegaDotEarth)*d_toa
- AlmPtr->Toa*OmegaDotEarth;
xp= r*cos(aol);
yp= r*sin(aol);
Pos[0] = xp*cos(la) - yp*cos(AlmPtr->DeltaI)*sin(la);
Pos[1] = xp*sin(la) + yp*cos(AlmPtr->DeltaI)*cos(la);
Pos[2] = yp*sin(AlmPtr->DeltaI);
AlmPtr->ECEF_Pos[0] = Pos[0];
AlmPtr->ECEF_Pos[1] = Pos[1];
AlmPtr->ECEF_Pos[2] = Pos[2];
ConvertToLongLatAlt( satPos, Pos);
}
/*++
Routine Description:
calculate sat's position using ephemeris and almanac for comparition;
just for debugging use;
Argument:
prn: prn number of satellite;
Time: time of gps
Rreturn:
none;
--*/
void GPS_Func::GetSatPos( int prn , double Time)
{
double posalm[3], poseph[3];
SatPosECEFAlminfo(&Almanac[prn], Time, 0, posalm);
SatPosECEFEphinfo(&Ephemeris[prn], Time, 0, poseph);
//#ifdef GPS_FUNC_DBG
// debugfile << "TOW: " << Time << endl;
// debugfile.setf(10);
// debugfile << "Pos from Alm :" << posalm[0] << ","
// << posalm[1] << ","
// << posalm[2] << endl
// << "Pos from Eph :" << poseph[0] << ","
// << poseph[1] << ","
// << poseph[2] << endl;
//
//#endif
}
//--------------------------------------------------------------------
void GPS_Func::SatElevAndAzimuth( int SatId, double UsrPos[3],
double Time,int Week )
{
double Dx, Dy, Dz; /* Distance user to sat in ECEF */
double SatPos[3],SatPosPlus[3],SatSpeed[3]; /* Position of sat */
double UserToSatRange; /* Distance user to sat */
double DirCos[3]; /* Director cosines from user to sat */
double EnuCos[3];
/* Calculate the sat pos from almanac, current time and week number */
SatPosECEFAlminfo( &Almanac[SatId], Time, Week, SatPos );
SatPosECEFAlminfo( &Almanac[SatId], Time+1., Week, SatPosPlus );
for(int k =0; k<3; k++)
SatSpeed[k] = SatPosPlus[k] - SatPos[k];
/* Distance from user to sat */
Dx = SatPos[0] - UsrPos[0];
Dy = SatPos[1] - UsrPos[1];
Dz = SatPos[2] - UsrPos[2];
UserToSatRange = sqrt((Dx * Dx) + (Dy * Dy) + (Dz * Dz));
Almanac[SatId].Dopp= (SatSpeed[0]*Dx + SatSpeed[1]*Dy+SatSpeed[2]*Dz)*5.2514/UserToSatRange;
/* Director cosines */
DirCos[0] = Dx / UserToSatRange;
DirCos[1] = Dy / UserToSatRange;
DirCos[2] = Dz / UserToSatRange;
/* Transformation to local tangent plane */
EnuCos[0] = (PlaneET1 * DirCos[0]) + (PlaneET2 * DirCos[1]);
EnuCos[1] = (PlaneNT1 * DirCos[0]) + (PlaneNT2 * DirCos[1])
+ (PlaneCE * DirCos[2]);
EnuCos[2] = (PlaneUTx * DirCos[0]) + (PlaneUTy * DirCos[1])
+ (PlaneUTz * DirCos[2]);
/* Azimuth and elevation of sat as seen by user */
Almanac[SatId].Ele = asin(EnuCos[2]) * (180.0 / PI);
Almanac[SatId].Azm = atan2(EnuCos[0], EnuCos[1]) * (180.0 / PI);
}
//--------------------------------------------------------------------
/*++
Routine Description:
user alamanac file to calculate sat's elevation and azimuth roughly;
Argument:
time: time in second ;
Rreturn:
none;
--*/
void GPS_Func::process_allsat(double Time, int Week)
{
double MyECEFPos[3];
// first init ele, azm and doppler to invalid value
for( int i=0; i< SatMax+1; i++)
{
Almanac[i].Ele = -1;
Almanac[i].Azm = -1;
Almanac[i].Dopp = 0;
}
ConvertToECEF(MyECEFPos, UserPos );
for( int i=0; i<SatMax+1; i++)
{
if( Almanac[i].SatId != 0)
{ // this is a valid almanac data
SatElevAndAzimuth(i,MyECEFPos,Time,Week);
}
}
}
//--------------------------------------------------------------------
/*++
Routine Description:
process GPS message, using global variable allch_nagmsg to transfer msgs
Argument:
none;
Rreturn:
none;
--*/
void GPS_Func::processMsg( void )
{
int ch_cnt;
if( newmsg_rdy )
{
for(int i=0; i<CH_NUM; i++)
if( allch_navmsg[i].newdata ) // new frame is here
{
ch_prn[i] = allch_navmsg[i].prn; // set prn num for this channel
readOneChMsg( i );
allch_navmsg[i].newdata = false; // reset new flag
}
newmsg_rdy = false;
}
ch_cnt = allchtrtime.ch_count;
//#ifdef GPS_FUNC_DBG
// debugfile << " CH count is :" << ch_cnt << endl;
// debugfile.precision(16);
// for( int i=0; i<ch_cnt; i++)
// debugfile << allchtrtime.ch_transtime[i].prn << " "
// << allchtrtime.ch_transtime[i].tr_time << endl;
//#endif
if( ch_cnt >= 4)
{
resolve_PVT();
}
}
//--------------------------------------------------------------------
/*++
Routine Description:
P.V.T. resolution;
Argument:
none;
Rreturn:
none;
--*/
void GPS_Func::resolve_PVT(void)
{
long double min_trtime, max_trtime,m_time;
int prn;
sat_tr_pos sat_pvtpara[CH_NUM];
double tmp_pos[3];
// first check if all trans_time make sense?
min_trtime = max_trtime = allchtrtime.ch_transtime[0].tr_time;
for( int i=1; i< allchtrtime.ch_count; i++)
{
if( allchtrtime.ch_transtime[i].tr_time > max_trtime)
max_trtime = allchtrtime.ch_transtime[i].tr_time;
if( allchtrtime.ch_transtime[i].tr_time < min_trtime)
min_trtime = allchtrtime.ch_transtime[i].tr_time;
}
if( fabs( max_trtime - min_trtime ) > 0.1 ) // doesn't make sense
return;
// if any channel is not ready , quit
for( int i=0; i< allchtrtime.ch_count; i++)
{
prn = allchtrtime.ch_transtime[i].prn;
if( ! Ephemeris[prn].valid)
return;
}
m_time = (long double)( max_trtime + 0.08); // guess
for( int i=0; i< allchtrtime.ch_count; i++)
{
prn = allchtrtime.ch_transtime[i].prn;
SatPosECEFEphinfo(&Ephemeris[prn],
allchtrtime.ch_transtime[i].tr_time,
0,
&sat_pvtpara[i].pos[0]);
SatPosECEFEphinfo(&Ephemeris[prn],
allchtrtime.ch_transtime[i].tr_time - 1.,
0,
&tmp_pos[0]);
// calculate velocity of satellite
sat_pvtpara[i].vel[0] = sat_pvtpara[i].pos[0] - tmp_pos[0]-
sat_pvtpara[i].pos[1]* OmegaDotEarth;
sat_pvtpara[i].vel[1] = sat_pvtpara[i].pos[1] - tmp_pos[1]+
sat_pvtpara[i].pos[0]* OmegaDotEarth;
sat_pvtpara[i].vel[2] = sat_pvtpara[i].pos[2] - tmp_pos[2];
sat_pvtpara[i].delta_time = m_time - allchtrtime.ch_transtime[i].tr_time + Ephemeris[prn].bclk;
sat_pvtpara[i].doppler_freq = allchtrtime.ch_transtime[i].dopp_f;
#ifdef GPS_FUNC_DBG
debugfile.precision(16);
debugfile << "Prn:" <<prn << " "
<< " x: " << sat_pvtpara[i].pos[0]
<< " y: " << sat_pvtpara[i].pos[1]
<< " z: " << sat_pvtpara[i].pos[2]
<< " vx:" <<sat_pvtpara[i].vel[0]
<< " vy:" <<sat_pvtpara[i].vel[1]
<< " vz:" <<sat_pvtpara[i].vel[2]
<< " tr:" << allchtrtime.ch_transtime[i].tr_time << endl;
#endif
}
// next we will use recursion to get current location
int nits, i, j, k, nsl;
double t,x,y,z;
double dd[4][4],r,ms[4][6],pm[4][6],bm[6],br[4],correct_mag;
double a1,b1,c1,d1,e1,f1,g1,h1,i1,j1,k1,l1,m1,n1,o1,p1,denom,alpha;
double Dx, Dy, Dz; /* Distance user to sat in ECEF */
double UserToSatRange; /* Distance user to sat */
double DirCos[3]; /* Director cosines from user to sat */
double EnuCos[3];
nsl = allchtrtime.ch_count;
nits=0;
t=0.0;
x=ECEFuserpos[0];
y=ECEFuserpos[1];
z=ECEFuserpos[2];
do
{
for(i=0;i<nsl;i++)
{
// Compute range in ECI at the time of arrival at the receiver
alpha = (t-sat_pvtpara[i].delta_time)*OmegaDotEarth;
r = sqrt(pow(sat_pvtpara[i].pos[0]*cos(alpha)-sat_pvtpara[i].pos[1]*sin(alpha)-x,2)+
pow(sat_pvtpara[i].pos[1]*cos(alpha)+sat_pvtpara[i].pos[0]*sin(alpha)-y,2)+
pow(sat_pvtpara[i].pos[2]-z,2));
bm[i]=r-(sat_pvtpara[i].delta_time - t)*SpeedOfLight;
ms[0][i]=(sat_pvtpara[i].pos[0]*cos(alpha)-sat_pvtpara[i].pos[1]*sin(alpha)-x)/r;
ms[1][i]=(sat_pvtpara[i].pos[1]*cos(alpha)+sat_pvtpara[i].pos[0]*sin(alpha)-y)/r;
ms[2][i]=(sat_pvtpara[i].pos[2]-z)/r;
ms[3][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=0;k<nsl;k++)
{
a1+=ms[0][k]*ms[0][k];
b1+=ms[0][k]*ms[1][k];
c1+=ms[0][k]*ms[2][k];
d1+=ms[0][k]*ms[3][k];
// e1+=ms[2][k]*ms[1][k]; for completeness, the matrix is symmetric
f1+=ms[1][k]*ms[1][k];
g1+=ms[1][k]*ms[2][k];
h1+=ms[1][k]*ms[3][k];
// i1+=ms[3][k]*ms[1][k];
// j1+=ms[3][k]*ms[2][k];
k1+=ms[2][k]*ms[2][k];
l1+=ms[2][k]*ms[3][k];
// m1+=ms[1][k];
// n1+=ms[2][k];
// o1+=ms[3][k];
p1+=ms[3][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[0][0]=f1*(k1*p1-l1*o1)+g1*(l1*n1-j1*p1)+h1*(j1*o1-k1*n1);
dd[0][1]=e1*(l1*o1-k1*p1)+g1*(i1*p1-l1*m1)+h1*(k1*m1-i1*o1);
dd[0][2]=e1*(j1*p1-n1*l1)-i1*(f1*p1-n1*h1)+m1*(f1*l1-j1*h1);
dd[0][3]=e1*(n1*k1-j1*o1)+i1*(f1*o1-n1*g1)+m1*(j1*g1-f1*k1);
dd[1][0]=dd[0][1];
dd[1][1]=a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1);
dd[1][2]=a1*(l1*n1-j1*p1)+i1*(b1*p1-n1*d1)+m1*(j1*d1-b1*l1);
dd[1][3]=a1*(j1*o1-n1*k1)-i1*(b1*o1-n1*c1)+m1*(b1*k1-c1*j1);
dd[2][0]=dd[0][2];
dd[2][1]=dd[1][2];
dd[2][2]=a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1);
dd[2][3]=a1*(n1*g1-f1*o1)+e1*(b1*o1-c1*n1)+m1*(c1*f1-b1*g1);
dd[3][0]=dd[0][3];
dd[3][1]=dd[1][3];
dd[3][2]=dd[2][3];
dd[3][3]=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=0;i<4;i++)
{
for (j=0;j<4;j++)
dd[i][j]=dd[i][j]/denom;
}
for (i=0;i<nsl;i++)
{
for (j=0;j<4;j++)
{
pm[j][i]=0.0;
for (k=0;k<4;k++)
pm[j][i]+=dd[j][k]*ms[k][i];
}
}
for (i=0;i<4;i++)
{
br[i]=0.0;
for (j=0;j<nsl;j++)
br[i]+=bm[j]*pm[i][j];
}
nits++;
x=x+br[0];
y=y+br[1];
z=z+br[2];
t=t-br[3]/SpeedOfLight;
correct_mag=sqrt(br[1]*br[1]+br[2]*br[2]+br[0]*br[0]);
}
} while ( correct_mag > 0.01 && correct_mag < 1.e8 && nits < 10);
// set position
ECEFuserpos[0] = x;
ECEFuserpos[1] = y;
ECEFuserpos[2] = z;
ConvertToLongLatAlt(NEDuserpos, ECEFuserpos);
SetUserPos( NEDuserpos );
AccurateTime = m_time - t;
// for velocity
for (i=0;i<nsl;i++)
{
alpha=( AccurateTime - allchtrtime.ch_transtime[i].tr_time)*OmegaDotEarth;
r=sqrt(pow(sat_pvtpara[i].pos[0]*cos(alpha)-sat_pvtpara[i].pos[1]*sin(alpha)-x,2)+
pow(sat_pvtpara[i].pos[1]*cos(alpha)+sat_pvtpara[i].pos[0]*sin(alpha)-y,2)+
pow(sat_pvtpara[i].pos[2]-z,2));
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -