gpsfunc.cpp

基于USB接口的GPS应用程序

	 bm[i]=((sat_pvtpara[i].pos[0]*cos(alpha)-sat_pvtpara[i].pos[1]*sin(alpha)-x)*sat_pvtpara[i].vel[0]+
			  (sat_pvtpara[i].pos[1]*cos(alpha)+sat_pvtpara[i].pos[0]*sin(alpha)-y)*sat_pvtpara[i].vel[1]+
			  (sat_pvtpara[i].pos[2]-z)*sat_pvtpara[i].vel[2])/r -
                           sat_pvtpara[i].doppler_freq*lambda;
  }

  for ( i=0;i<4;i++)
  {
	  br[i]=0.0;
	  for (j=0;j<nsl;j++)br[i]+=bm[j]*pm[i][j];
  }

 ECEFvel[0] = br[0] + y*OmegaDotEarth;
 ECEFvel[1] = br[1] - x*OmegaDotEarth;
 ECEFvel[2] = br[2];
 clk_bias = br[3]/SpeedOfLight*1000000.0;

// then calculate matrix G for dops
 Matrix H(nsl,4),G(4,4);

 EnterCriticalSection(&GpsCriticalSection);
  for( int i=0; i< allchtrtime.ch_count; i++)
 {
  allch_navresult[i].prn = allchtrtime.ch_transtime[i].prn;
  allch_navresult[i].valid = true;
  // calculate pseudorange
  allch_navresult[i].pseudorange =( AccurateTime -
                                 allchtrtime.ch_transtime[i].tr_time)*
                                 SpeedOfLight;
  // calculate elevation and azimuth


  /* Distance from user to sat */
  Dx = sat_pvtpara[i].pos[0] - UserPos[0];
  Dy = sat_pvtpara[i].pos[1] - UserPos[1];
  Dz = sat_pvtpara[i].pos[2] - UserPos[2];

  UserToSatRange = sqrt((Dx * Dx) + (Dy * Dy) + (Dz * Dz));

  H(i+1,1)= Dx/UserToSatRange;
  H(i+1,2)= Dy/UserToSatRange;
  H(i+1,3)= Dz/UserToSatRange;
  H(i+1,4)= 1.0;


   /* 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 */
  allch_navresult[i].ele = asin(EnuCos[2]) * (180.0 / PI);
  allch_navresult[i].azi = atan2(EnuCos[0], EnuCos[1]) * (180.0 / PI);

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

 LeaveCriticalSection(&GpsCriticalSection);



 #ifdef GPS_FUNC_DBG
 debugfile << " Pos of User:"
 << x << " " << y << " " << z <<" " << t<< " "
 <<nits << " " << correct_mag<<endl;
 #endif
}

//--------------------------------------------------------------------
/*++
 Routine Description:
   Process one channel's GPS message, idx is the channel's index
    using global variable allch_nagmsg to transfer msgs

 Argument:
         idx :  indicate which channel to read
 Rreturn:
         none;

--*/

void GPS_Func::readOneChMsg(int idx)
{
 int cur_idx;
 cur_idx = (allch_navmsg[idx].index + 1)%302;

 // pre_bit[0-1] are used to check paritys
 prev_bit[0] = (allch_navmsg[idx].msg[ cur_idx ])>>29;
 prev_bit[1] = (allch_navmsg[idx].msg[ (++cur_idx)%302 ])>>29;

 // re-organize the message bits to form message words
 for( int wi = 0; wi < 10; wi++ )
 {
  nav_msg[idx][wi] = 0;
  for( int j=0; j<30; j++)
  {
   if( allch_navmsg[idx].msg[(++cur_idx)%302] & 0x20000000 )
    nav_msg[idx][wi] = nav_msg[idx][wi]*2 + 0x1;
   else
    nav_msg[idx][wi] = nav_msg[idx][wi]*2 ;
  }
 }
  // till now , we have read all data for this channel in this time

 parity_check(idx);

 if( p_error[idx] == 0) // only when parity check passed
 {
  int frmidx = nav_msg[idx][1];
  frmidx = (frmidx>>2) & 0x07;
  if( frmidx >=1 && frmidx<=5 )
  {
//#ifdef GPS_FUNC_DBG
//   debugfile << hex << "CH #" << idx << ": ";
//   for( int k=0;k<10; k++)
//    debugfile <<  nav_msg[idx][k]<< " ";
//   debugfile << ". Parity: " << p_error[idx] << " frmidx:" << frmidx << endl;
//#endif
   navmsgparity[idx].par[frmidx-1] = 0;
   memcpy((void*)&navmsgparity[idx].msg[frmidx-1][0],(void*)&nav_msg[idx][0],10*sizeof(unsigned int));
   if( frmidx == 5) // we get all 5 subframes
    read_allsubframe(idx);
  }
 }
}

//--------------------------------------------------------------------
/*++
 Routine Description:
    Check parity for one subframe
     For details algorithmm, refer to ICD_GPS_200C
 Argument:
         ch_i :  indicate which channel to read
 Rreturn:
         none;

--*/
void GPS_Func::parity_check(int ch_i)
{
 char b_1, b_2, b_3, b_4, b_5, b_6;
 int parity, m_parity;
 p_error[ch_i] = 0x0;

 for( int word=0; word<10; word++)
 {
       m_parity = nav_msg[ch_i][word] & 0x3f;
       b_1=exor(prev_bit[0],nav_msg[ch_i][word] & pb1) *32;
       b_2=exor(prev_bit[1],nav_msg[ch_i][word] & pb2) *16;
       b_3=exor(prev_bit[0],nav_msg[ch_i][word] & pb3) *8;
       b_4=exor(prev_bit[1],nav_msg[ch_i][word] & pb4) *4;
       b_5=exor(0,nav_msg[ch_i][word] & pb5) *2;
       b_6=exor(prev_bit[0]^prev_bit[1],nav_msg[ch_i][word] & pb6);
       parity=b_1+b_2+b_3+b_4+b_5+b_6;
       if( parity != m_parity ) // parity check failed
        p_error[ch_i] = p_error[ch_i]*2 +1;
       else
        p_error[ch_i] = p_error[ch_i]*2;

       if( prev_bit[1] )
        nav_msg[ch_i][word] = nav_msg[ch_i][word]^0x3fffffc0;
       nav_msg[ch_i][word] = nav_msg[ch_i][word]>>6;
       prev_bit[0]=(m_parity & 0x2) >>1;
       prev_bit[1]= m_parity & 0x1;
 }

}
//--------------------------------------------------------------------
/*++
 Routine Description:
   Read nav msg for one channel,
    get ephemeris data for this channel.
 Argument:
         idx :  indicate which channel to read
 Rreturn:
         none;

--*/
void GPS_Func::read_allsubframe(int idx)
{
 int iodc,iode,idoe, iweek, iura, ihealth, iaf0, im0, inc0, iomega0,iw;
 int iomegadot, idot, prn;
 char itgd,iaf2;
 unsigned int itoc, ie,isqra, itoe;
 short int iaf1,icrs, idn, icuc,icus,icic,icis,icrc;
 unsigned int *sf;
 double r_sqra,r_inc0,r_ety;

 sf = (unsigned int*)&navmsgparity[idx].msg[0][0];
 prn = ch_prn[idx];

 if( navmsgparity[idx].par[0] == 0 &&
     navmsgparity[idx].par[1] == 0 &&
     navmsgparity[idx].par[2] == 0) // if frm1-3 is correct
     {
      iodc=int(((sf[0*10+2] & 0x3) <<8 ) | ((sf[0*10+7] & 0xFF0000) >>16));
      iode=int(sf[1*10+2]  >> 16);
      idoe=int(sf[2*10+9] >> 16);

      if( (iode == idoe) &&
          ( (iode!= Ephemeris[ prn ].iode) || (iodc!=Ephemeris[ prn ].iodc) ))
      {

       // subframe 1

       iweek= int(sf[0*10+2] >> 14);
       iura=int(( sf[0*10+2] & 0xF00 ) >> 8);
       ihealth=int(( sf[0*10+2] & 0xFC ) >> 2);
       itgd=sf[0*10+6] & 0xFF;
       itoc=sf[0*10+7] & 0xFFFF;
       iaf2=sf[0*10+8] >> 16;
       iaf1=sf[0*10+8] & 0xFFFF;
       iaf0=sf[0*10+9] >> 2;
       if( iaf0 & 0x200000) iaf0=iaf0 | 0xFFC00000; // sign extension

       //   subframe 2
       icrs=sf[1*10+2] & 0xFFFF;
       idn =(sf[1*10+3]>>8) & 0xFFFF;
       im0=((sf[1*10+3] & 0xFF) << 24) | sf[1*10+4];
       icuc=(sf[1*10+5]>>8) & 0xFFFF;
       ie=((sf[1*10+5] & 0xFF) << 24) | sf[1*10+6];
       icus=(sf[1*10+7]>>8) & 0xFFFF;
       isqra=(((sf[1*10+7] & 0xFF) << 24) | sf[1*10+8]);
       itoe=int(sf[1*10+9] >> 8);

       // subframe 3
       icic=(sf[2*10+2]>>8) & 0xFFFF;
       icis=(sf[2*10+4]>>8) & 0xFFFF;
       inc0=((sf[2*10+4] & 0xFF) << 24) | sf[2*10+5];
       iomega0=((sf[2*10+2] & 0xFF) << 24) | sf[2*10+3];
       icrc=(sf[2*10+6]>>8) & 0xFFFF;
       iw=((sf[2*10+6] & 0xFF) << 24) | sf[2*10+7];
       iomegadot=sf[2*10+8];
       if (iomegadot&0x800000)
        iomegadot=iomegadot | 0xFF000000;  // sign extension
       idot=((sf[2*10+9] & 0xFFFC) >> 2);
       if (idot&0x2000)
        idot=idot | 0xFFFFC000; // sign extension

       r_sqra=isqra*c_2m19;
       r_inc0=inc0*c_2m31*PI;
       r_ety=ie*c_2m33;

       if (   (r_inc0<1.05 && r_inc0>0.873)&&
              (r_sqra>5100.0 && r_sqra<5200.0)&&
              (r_ety <.05 && r_ety>0.0)  )
        {
          Ephemeris[prn].valid=1;
          eph_valid[prn] = true;  //set this global variable for gpsnav thread use

          Ephemeris[prn].iode=iode;
          Ephemeris[prn].iodc=iodc;
	  Ephemeris[prn].week=iweek;
	  Ephemeris[prn].ura=iura;
	  Ephemeris[prn].health=ihealth;
	  Ephemeris[prn].tgd=itgd*c_2m31;
	  Ephemeris[prn].toc=itoc*16.0;
	  Ephemeris[prn].af2=iaf2*c_2m55;
	  Ephemeris[prn].af1=iaf1*c_2m43;
	  Ephemeris[prn].af0=iaf0*c_2m31;
	  Ephemeris[prn].crs=icrs*c_2m5;
	  Ephemeris[prn].dn=idn*c_2m43*PI;
	  Ephemeris[prn].ma=im0*c_2m31*PI;
	  Ephemeris[prn].cuc=icuc*c_2m29;
	  Ephemeris[prn].ety=r_ety;
	  Ephemeris[prn].cus=icus*c_2m29;
	  Ephemeris[prn].sqra=r_sqra;
	  Ephemeris[prn].wm=19964981.84/pow(r_sqra,3);
	  Ephemeris[prn].toe=itoe*c_2p4;
	  Ephemeris[prn].cic=icic*c_2m29;
	  Ephemeris[prn].cis=icis*c_2m29;
	  Ephemeris[prn].inc0=r_inc0;
	  Ephemeris[prn].w0=iomega0*c_2m31*PI;
	  Ephemeris[prn].crc=icrc*c_2m5;
	  Ephemeris[prn].w=iw*c_2m31*PI;
	  Ephemeris[prn].omegadot=iomegadot*c_2m43*PI;
	  Ephemeris[prn].idot=idot*c_2m43*PI;
        } // end of  if (   (r_inc0<1.05 && r_inc0>0.873)&&
      } // end of if ( (iode == idoe) &&

     } // end of if( navmsgparity[idx].par[0] == 0 &&


  // finally init parity to invalid values back
  for( int j=0; j<5; j++)
  {
   navmsgparity[idx].par[j] = 0x1;  // init parity values for 5 frm of all channels
   memset((void*)&navmsgparity[idx].msg[j][0],0,10*sizeof(unsigned int));
  }
}
//--------------------------------------------------------------------
/*++
 Routine Description:
   Get result of processing ,
    such as user's pos, user's velocity, accurate time, etc.
 Argument:
         nav_pt:  pointer for processing result storing;
 Rreturn:
         none;

--*/

void GPS_Func::get_navinfo( PT_navinfo_disp nav_pt)
{
 memcpy(nav_pt->ECEFpos, ECEFuserpos, 3*sizeof(double));
 memcpy(nav_pt->NEDpos, NEDuserpos, 3*sizeof(double));
 memcpy(nav_pt->ECEFvel, ECEFvel, 3*sizeof(double));
 nav_pt->clk_bias = clk_bias;
 nav_pt->rcv_time = AccurateTime;
 nav_pt->GDOP = gdop;
 nav_pt->TDOP = tdop;
 nav_pt->PDOP = pdop;
 nav_pt->HDOP = hdop;
 nav_pt->VDOP = vdop;
}

//--------------------------------------------------------------------
/*++
 Routine Description:
  Set prn allocation according to sat's elevation.
 Argument:
         pn_num:  prn allocation buffer pointer;
 Rreturn:
         none;
--*/

void GPS_Func::SelectPrn(int* pn_num)
{
 int prn_idx[SatMax+1],tmp_id, sat_count, flag ;
 double ele[SatMax+1], tmp_ele;

 sat_count = 0;
 memset(prn_idx, 0, sizeof(prn_idx));

 for(int i=0 ; i<  SatMax+1; i++)
 {
  if( Almanac[i].SatId != 0 && Almanac[i].Ele > 0.) // a valid satellite
  {
   prn_idx[sat_count] = Almanac[i].SatId;
   ele[sat_count] = Almanac[i].Ele;
   sat_count++;
  }
 }
 // bubble sort to find the 6 biggest elevations
 do{
  flag = 0;
  for( int i=1; i< sat_count; i++)
   if( ele[i] > ele[i-1] )
   {   // swap i and i-1
    tmp_id = prn_idx[i];
    tmp_ele = ele[i];

    prn_idx[i] = prn_idx[i-1];
    ele[i] = ele[i-1];

    prn_idx[i-1] = tmp_id;
    ele[i-1] = tmp_ele;

    flag = 1; // set flat to indicate we need further addjustment
   }
 }while( flag );

 for( int idx = 0; idx < 6 ; idx++)
  pn_num[idx] = prn_idx[idx];

}

//--------------------------------------------------------------------
/*++
 Routine Description:
  calculate position of sats through almanac data
   and set prn allocation accordingly.
 Argument:
         pn:  prn allocation buffer pointer;
         gt:  struct tm* pointer, pointing to time of setting;
         AlmFile: FILE pointer of almanac file;
         Pos: roughly guessed user position;
 Rreturn:
         0;

--*/

int GPS_Func::CalcPrnSetting( int *pn, struct tm* gt, FILE* AlmFile, double *Pos )
{
 long double jd;
 int gpsweek, timeweek;

 SetUserPos(Pos);

 jd = CalcJulianDay(gt);
 gpsweek = CalcGpsWeek( jd );

 timeweek = CalcToW(jd);

 ReadAlm(AlmFile);
 process_allsat(timeweek, gpsweek);
 SelectPrn(pn);

 return 0; // should return some more meaningful value?
}
//--------------------------------------------------------------------
/*++
 Thread related functions...
 --*/

void GPS_Func::CreateGpsThrd(void)
{
 gpsthrd = new GpsThrd(true,this);
 thrdtermed = false;
 gpsthrd->Resume();
}
void GPS_Func::TerminateThrd(void)
{
 if( gpsthrd )
  gpsthrd->Terminate();
}
// function to set the terminate status of sampling thread
void GPS_Func::SetThrdStatus(bool stat)
{
 thrdtermed = stat;
}
bool GPS_Func::NavThrdIsTermed()
{
 return (thrdtermed);
}