gpsfunc.cpp

基于USB接口的GPS应用程序

 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));