gps_nav.cpp

gps 软件接收机,用 c++ 语言实现

  gpsWeekNumber_     = parseMsgDataU32(dataWord3,1,10);
  codesOnL2_         = parseMsgDataU32(dataWord3,11,12);
  svAccuracy_        = parseMsgDataU32(dataWord3,13,16);
  svHealthSummary_   = parseMsgDataU32(dataWord3,17,17);
  svHealth_          = parseMsgDataU32(dataWord3,18,22);
  IODC_              = parseMsgDataU32(dataWord3,23,24,dataWord8,1,8);
  pCodeOffOnL2_      = parseMsgDataU32(dataWord4,1,1);
  T_GD_              = parseMsgDataSF64(-31,dataWord7,17,24);
  t_oc_              = parseMsgDataU32(dataWord8,9,24) << 4;
  a_f2_              = parseMsgDataSF64(-55,dataWord9,1,8);
  a_f1_              = parseMsgDataSF64(-43,dataWord9,9,24); 
  a_f0_              = parseMsgDataSF64(-31,dataWord10,1,22);
                  
  return true;
}
//---------------------------------------------------------------------------
std::string
SubFrame1::debugDump()
{
  std::stringstream result;
  result << SubFrameBase::debugDump();
  
  M_DUMP_VALUE(gpsWeekNumber_);
  M_DUMP_VALUE(codesOnL2_);
  M_DUMP_VALUE(svAccuracy_);
  M_DUMP_VALUE(svHealthSummary_);
  M_DUMP_VALUE(svHealth_);
  M_DUMP_VALUE(IODC_);
  M_DUMP_VALUE(pCodeOffOnL2_);
  M_DUMP_VALUE(T_GD_);
  M_DUMP_VALUE(t_oc_);
  M_DUMP_VALUE(a_f2_);
  M_DUMP_VALUE(a_f1_);
  M_DUMP_VALUE(a_f0_);

  return result.str();
}
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
//SubFrame2::
//---------------------------------------------------------------------------
bool
SubFrame2::parse(
  SubFrameRaw const&       inSubFrame)
{
  if(!SubFrameBase::parse(inSubFrame))
    return false;
    
  if(2 != subFrameId_)
    return false;
    
  u32 dataWord3;
  if(!checkWordParity(inSubFrame.words_[2],dataWord3))
    return false; 
  u32 dataWord4;
  if(!checkWordParity(inSubFrame.words_[3],dataWord4))
    return false; 
  u32 dataWord5;
  if(!checkWordParity(inSubFrame.words_[4],dataWord5))
    return false; 
  u32 dataWord6;
  if(!checkWordParity(inSubFrame.words_[5],dataWord6))
    return false; 
  u32 dataWord7;
  if(!checkWordParity(inSubFrame.words_[6],dataWord7))
    return false; 
  u32 dataWord8;
  if(!checkWordParity(inSubFrame.words_[7],dataWord8))
    return false; 
  u32 dataWord9;
  if(!checkWordParity(inSubFrame.words_[8],dataWord9))
    return false; 
  u32 dataWord10;
  if(!checkWordParity(inSubFrame.words_[9],dataWord10))
    return false; 

//sub-frame data
  sf2IODE_           = parseMsgDataU32(dataWord3,1,8);
  C_rs_              = parseMsgDataSF64( -5,dataWord3,9,24);
  deltan_            = parseMsgDataSF64(-43,dataWord4,1,16);
  M_0_               = parseMsgDataSF64(-31,dataWord4,17,24,dataWord5,1,24);
  C_uc_              = parseMsgDataSF64(-29,dataWord6,1,16);
  e_                 = parseMsgDataUF64(-33,dataWord6,17,24,dataWord7,1,24);
  C_us_              = parseMsgDataSF64(-29,dataWord8,1,16);
  sqrtA_             = parseMsgDataUF64(-19,dataWord8,17,24,dataWord9,1,24);
  t_oe_              = parseMsgDataU32(dataWord10,1,16) << 4;
  fitIntervalFlag_   = parseMsgDataU32(dataWord10,17,17);
  AODO_              = parseMsgDataU32(dataWord10,18,22);
  
  return true;
}
//---------------------------------------------------------------------------
std::string
SubFrame2::debugDump()
{
  std::stringstream result;
  result << SubFrameBase::debugDump();
  
  M_DUMP_VALUE(sf2IODE_);
  M_DUMP_VALUE(C_rs_);
  M_DUMP_VALUE(deltan_);
  M_DUMP_VALUE(M_0_);
  M_DUMP_VALUE(C_uc_);
  M_DUMP_VALUE(e_);
  M_DUMP_VALUE(C_us_);
  M_DUMP_VALUE(sqrtA_);
  M_DUMP_VALUE(t_oe_);
  M_DUMP_VALUE(fitIntervalFlag_);
  M_DUMP_VALUE(AODO_);

  return result.str();
}
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
//SubFrame3::
//---------------------------------------------------------------------------
bool
SubFrame3::parse(
  SubFrameRaw const&       inSubFrame)
{
  if(!SubFrameBase::parse(inSubFrame))
    return false;
    
  if(3 != subFrameId_)
    return false;
    
  u32 dataWord3;
  if(!checkWordParity(inSubFrame.words_[2],dataWord3))
    return false; 
  u32 dataWord4;
  if(!checkWordParity(inSubFrame.words_[3],dataWord4))
    return false; 
  u32 dataWord5;
  if(!checkWordParity(inSubFrame.words_[4],dataWord5))
    return false; 
  u32 dataWord6;
  if(!checkWordParity(inSubFrame.words_[5],dataWord6))
    return false; 
  u32 dataWord7;
  if(!checkWordParity(inSubFrame.words_[6],dataWord7))
    return false; 
  u32 dataWord8;
  if(!checkWordParity(inSubFrame.words_[7],dataWord8))
    return false; 
  u32 dataWord9;
  if(!checkWordParity(inSubFrame.words_[8],dataWord9))
    return false; 
  u32 dataWord10;
  if(!checkWordParity(inSubFrame.words_[9],dataWord10))
    return false; 

//sub-frame data
  C_ic_            = parseMsgDataSF64(-29,dataWord3,1,16);
  omega_0_         = parseMsgDataSF64(-31,dataWord3,17,24,dataWord4,1,24);
  C_is_            = parseMsgDataSF64(-29,dataWord5,1,16);
  i_0_             = parseMsgDataSF64(-31,dataWord5,17,24,dataWord6,1,24);
  C_rc_            = parseMsgDataSF64( -5,dataWord7,1,16);
  w_               = parseMsgDataSF64(-31,dataWord7,17,24,dataWord8,1,24);
  dot_omega_       = parseMsgDataSF64(-43,dataWord9,1,24);
  sf3IODE_         = parseMsgDataU32(dataWord10,1,8);
  IDOT_            = parseMsgDataSF64(-43,dataWord10,9,22);

  return true;
}
//---------------------------------------------------------------------------
std::string
SubFrame3::debugDump()
{
  std::stringstream result;
  result << SubFrameBase::debugDump();
  
  M_DUMP_VALUE(C_ic_);
  M_DUMP_VALUE(omega_0_);
  M_DUMP_VALUE(C_is_);
  M_DUMP_VALUE(i_0_);
  M_DUMP_VALUE(C_rc_);
  M_DUMP_VALUE(w_);
  M_DUMP_VALUE(dot_omega_);
  M_DUMP_VALUE(sf3IODE_);
  M_DUMP_VALUE(IDOT_);

  return result.str();
}
//---------------------------------------------------------------------------

//---------------------------------------------------------------------------
//Ephemeris::
//---------------------------------------------------------------------------
Ephemeris::Ephemeris()
: ephemerisValid_(false)
{
}
//---------------------------------------------------------------------------
void
Ephemeris::reset()
{
  ephemerisValid_ = false;
}
//---------------------------------------------------------------------------
bool
Ephemeris::ephemerisValid()
const
{
  return ephemerisValid_;
}
//---------------------------------------------------------------------------
void
Ephemeris::processSubFrameRaw(
  SubFrameRaw const&       inSubFrameRaw)
{
  s32 id;
  s32 page;
  if(SubFrameIDGet(inSubFrameRaw,id,page))
  {
    switch(id)
    {
      case 1:                    
        preliminarySF1_.parse(inSubFrameRaw);
        preliminarySubFramesProcess();
        break;
      case 2:
        preliminarySF2_.parse(inSubFrameRaw);
        preliminarySubFramesProcess();
        break;
      case 3:
        preliminarySF3_.parse(inSubFrameRaw);
        preliminarySubFramesProcess();
        break;
    }
  }
}  
//---------------------------------------------------------------------------
void
Ephemeris::debugLoadCAMsg(
  c8 const* const          inFileName)
{
  std::fstream f(inFileName,std::ios::in);
  
  M_KK_ASSERT(f.is_open());
  
  SubFrameRaw sfraw;
  u32 word = 0;
  u32 bitIndex = 0;
  for(;;)
  {
    c8 c;
    f.get(c);
    if(!f.good()) break;

    switch(c)
    {
      case '0':
      case 0:
        ++bitIndex;
        word <<= 1;
        break;
      case '1':
      case 1:
        ++bitIndex;
        word <<= 1;
        word |= 1;
        break;
      default:
        break;
    }
    
    if(0 == (bitIndex % kMsgBitsPerWord))
    { //add word to buffer
      sfraw.words_[bitIndex / kMsgBitsPerWord - 1] = word;      
    }
    
    if(0 == (bitIndex % (kMsgBitsPerWord * kMsgWordsPerSubFrame)))
    { //process subframe
      processSubFrameRaw(sfraw);
      bitIndex = 0;
    }
  }
}
//---------------------------------------------------------------------------
void
Ephemeris::calculateDeltaSVTimeAndEkFromGPSTime(
  f64 const                inT_GPS,
  f64&                     ouDeltaT_sv,
  f64&                     ouE_k)
const
{
  f64 const t = inT_GPS;
 
  f64 const F = -4.442807633e-10; //-2 * sqrt(k_mu) / (k_c * k_c);
  
  f64 const A = sqrtA_ * sqrtA_;
  
  f64 const n_0 = std::sqrt(k_mu / (A * A * A));
  
  f64 const n = n_0 + deltan_ * k_semi_circle_radians;
  
  f64 const t_k = limitTimeToValidRange(t - t_oe_);
  
  f64 const M_k = M_0_ * k_semi_circle_radians + n * t_k;
  
  //E_k is calculated iteratively
  //GPS standard says that 10 iteration is enough 
  f64 E_k = M_k;
  for(;;) 
  {
    f64 const E_k_last = E_k;
    E_k = M_k + e_ * std::sin(E_k);
    if(std::fabs(static_cast<f64>(E_k_last) - static_cast<f64>(E_k)) < 1e-14) break;
//debug    std::cerr 
//debug      << "E_k Iter: " << static_cast<f64>(E_k_last) - static_cast<f64>(E_k)
//debug      << " " << E_k
//debug      << "\n";
  }
//debug  std::cerr << "E_k Done\n";
  
  f64 const delta_t_r = F * e_ * sqrtA_ * std::sin(E_k);
  
  f64 const dt = limitTimeToValidRange(t - t_oc_); 

  f64 const delta_t_sv = a_f0_ + a_f1_ * dt + a_f2_ * dt * dt + delta_t_r;

  f64 const delta_t_sv_l1 = delta_t_sv - T_GD_;

  ouDeltaT_sv = delta_t_sv_l1; 
  ouE_k = E_k;
}
//---------------------------------------------------------------------------
FourDPos
Ephemeris::findPos(
  f64 const                inTtr_SV)
const
{
  FourDPos pos;