source: ntrip/trunk/BNC/src/satObs.cpp

#include <iostream>
#include <iomanip>
#include <sstream>
#include <newmatio.h>

#include "satObs.h"

using namespace std;

// Constructor
////////////////////////////////////////////////////////////////////////////
t_clkCorr::t_clkCorr()
{
  _updateInt = 0;
  _iod = 0;
  _dClk = 0.0;
  _dotDClk = 0.0;
  _dotDotDClk = 0.0;
}

//
////////////////////////////////////////////////////////////////////////////
void t_clkCorr::writeEpoch(ostream* out, const QList<t_clkCorr>& corrList)
{
  if (!out || corrList.size() == 0) {
    return;
  }
  out->setf(ios::fixed);
  bncTime epoTime;
  QListIterator<t_clkCorr> it(corrList);
  while (it.hasNext()) {
    const t_clkCorr& corr = it.next();
    if (!epoTime.valid()) {
      epoTime = corr._time;
      *out << "> CLOCK " << epoTime.datestr(' ') << ' ' << epoTime.timestr(1, ' ') << " "
        << corr._updateInt << " "
        << corrList.size() << ' ' << corr._staID << endl;
    }
    *out << corr._prn.toString() << ' ' << setw(11) << corr._iod << ' '
      << setw(10) << setprecision(4) << corr._dClk * t_CST::c << ' '                // m
      << setw(10) << setprecision(4) << corr._dotDClk * t_CST::c * 1.e3 << ' '      // m/s  => mm/s
      << setw(10) << setprecision(4) << corr._dotDotDClk * t_CST::c * 1.e3 << endl; // m/s² => mm/s²
  }
  out->flush();
}

//
////////////////////////////////////////////////////////////////////////////
void t_clkCorr::readEpoch(const string& epoLine, istream& inStream, QList<t_clkCorr>& corrList)
{
  bncTime epoTime;
  unsigned int updateInt;
  int numCorr;
  string staID;
  if (t_corrSSR::readEpoLine(epoLine, epoTime, updateInt, numCorr, staID) != t_corrSSR::clkCorr) {
    return;
  }
  for (int ii = 0; ii < numCorr; ii++) {
    t_clkCorr corr;
    corr._time = epoTime;
    corr._updateInt = updateInt;
    corr._staID = staID;

    string line;
    getline(inStream, line);
    istringstream in(line.c_str());

    in >> corr._prn >> corr._iod >> corr._dClk >> corr._dotDClk >> corr._dotDotDClk;

    char sys = corr._prn.system();
    int num = corr._prn.number();
    int flag = t_corrSSR::getSsrNavTypeFlag(sys, num);
    corr._prn.setFlag(flag);

    corr._dClk /= (t_CST::c);
    corr._dotDClk /= (t_CST::c * 1.e3);
    corr._dotDotDClk /= (t_CST::c * 1.e3);

    corrList.push_back(corr);
  }
}

// Constructor
////////////////////////////////////////////////////////////////////////////
t_orbCorr::t_orbCorr()
{
  _updateInt = 0;
  _iod = 0;
  _system = 'R';
  _xr.ReSize(3);
  _xr = 0.0;
  _dotXr.ReSize(3);
  _dotXr = 0.0;
  _ssrFormat = ssrUnknown;
}

//
////////////////////////////////////////////////////////////////////////////
void t_orbCorr::writeEpoch(ostream* out, const QList<t_orbCorr>& corrList)
{
  if (!out || corrList.size() == 0) {
    return;
  }
  out->setf(ios::fixed);
  bncTime epoTime;
  QListIterator<t_orbCorr> it(corrList);
  while (it.hasNext()) {
    const t_orbCorr& corr = it.next();
    if (!epoTime.valid()) {
      epoTime = corr._time;
      *out << "> ORBIT " << epoTime.datestr(' ') << ' ' << epoTime.timestr(1, ' ') << " "
        << corr._updateInt << " "
        << corrList.size() << ' ' << corr._staID << endl;
      *out << " " << static_cast<int>(corr._ssrFormat) << endl;
    }
    *out << corr._prn.toString() << ' ' << setw(11) << corr._iod << ' '
      << setw(10) << setprecision(4) << corr._xr[0] << ' '
      << setw(10) << setprecision(4) << corr._xr[1] << ' '
      << setw(10) << setprecision(4) << corr._xr[2] << "    "
      << setw(10) << setprecision(4) << corr._dotXr[0] * 1.e3 << ' '   // m/s => mm/s
      << setw(10) << setprecision(4) << corr._dotXr[1] * 1.e3 << ' '   // m/s => mm/s
      << setw(10) << setprecision(4) << corr._dotXr[2] * 1.e3 << endl; // m/s => mm/s
  }
  out->flush();
}

//
////////////////////////////////////////////////////////////////////////////
void t_orbCorr::readEpoch(const string& epoLine, istream& inStream, QList<t_orbCorr>& corrList)
{
  bncTime epoTime;
  unsigned int updateInt;
  int numCorr;
  string staID;
  if (t_corrSSR::readEpoLine(epoLine, epoTime, updateInt, numCorr, staID) != t_corrSSR::orbCorr) {
    return;
  }

  e_ssrFormat ssrFormat = ssrUnknown;
  {
    string line;
    getline(inStream, line);
    istringstream in(line.c_str());
    int ssrFormatInt = 0;
    if (in >> ssrFormatInt) {
      ssrFormat = static_cast<e_ssrFormat>(ssrFormatInt);
    }
  }

  for (int ii = 0; ii < numCorr; ii++) {
    t_orbCorr corr;
    corr._time = epoTime;
    corr._updateInt = updateInt;
    corr._staID = staID;
    corr._ssrFormat = ssrFormat;

    string line;
    getline(inStream, line);
    istringstream in(line.c_str());

    in >> corr._prn >> corr._iod >> corr._xr[0] >> corr._xr[1] >> corr._xr[2] >> corr._dotXr[0] >> corr._dotXr[1] >> corr._dotXr[2];

    char sys = corr._prn.system();
    int num = corr._prn.number();
    int flag = t_corrSSR::getSsrNavTypeFlag(sys, num);
    corr._prn.setFlag(flag);

    corr._dotXr[0] /= 1.e3; // mm/s => m/s
    corr._dotXr[1] /= 1.e3; // mm/s => m/s
    corr._dotXr[2] /= 1.e3; // mm/s => m/s

    corrList.push_back(corr);
  }
}

// Constructor
////////////////////////////////////////////////////////////////////////////
t_URA::t_URA()
{
  _updateInt = 0;
  _iod = 0;
  _ura = 0.0;
}

//
////////////////////////////////////////////////////////////////////////////
void t_URA::writeEpoch(ostream* out, const QList<t_URA>& corrList)
{
  if (!out || corrList.size() == 0) {
    return;
  }
  out->setf(ios::fixed);
  bncTime epoTime;
  QListIterator<t_URA> it(corrList);
  while (it.hasNext()) {
    const t_URA& corr = it.next();
    if (!epoTime.valid()) {
      epoTime = corr._time;
      *out << "> URA " << epoTime.datestr(' ') << ' ' << epoTime.timestr(1, ' ') << " "
        << corr._updateInt << " "
        << corrList.size() << ' ' << corr._staID << endl;
    }
    *out << corr._prn.toString() << ' ' << setw(11) << corr._iod << ' '
      << setw(10) << setprecision(4) << corr._ura << endl;
  }
  out->flush();
}

//
////////////////////////////////////////////////////////////////////////////
void t_URA::readEpoch(const string& epoLine, istream& inStream, QList<t_URA>& corrList)
{
  bncTime epoTime;
  unsigned int updateInt;
  int numCorr;
  string staID;
  if (t_corrSSR::readEpoLine(epoLine, epoTime, updateInt, numCorr, staID) != t_corrSSR::URA) {
    return;
  }
  for (int ii = 0; ii < numCorr; ii++) {
    t_URA corr;
    corr._time = epoTime;
    corr._updateInt = updateInt;
    corr._staID = staID;

    string line;
    getline(inStream, line);
    istringstream in(line.c_str());

    in >> corr._prn >> corr._iod >> corr._ura;

    char sys = corr._prn.system();
    int num = corr._prn.number();
    int flag = t_corrSSR::getSsrNavTypeFlag(sys, num);
    corr._prn.setFlag(flag);

    corrList.push_back(corr);
  }
}

//
////////////////////////////////////////////////////////////////////////////
void t_satCodeBias::writeEpoch(ostream* out, const QList<t_satCodeBias>& biasList)
{
  if (!out || biasList.size() == 0) {
    return;
  }
  out->setf(ios::fixed);
  bncTime epoTime;
  QListIterator<t_satCodeBias> it(biasList);
  while (it.hasNext()) {
    const t_satCodeBias& satCodeBias = it.next();
    if (!epoTime.valid()) {
      epoTime = satCodeBias._time;
      *out << "> CODE_BIAS " << epoTime.datestr(' ') << ' ' << epoTime.timestr(1, ' ') << " "
        << satCodeBias._updateInt << " "
        << biasList.size() << ' ' << satCodeBias._staID << endl;
    }
    *out << satCodeBias._prn.toString() << "   " << setw(2) << satCodeBias._bias.size();
    for (unsigned ii = 0; ii < satCodeBias._bias.size(); ii++) {
      const t_frqCodeBias& frqCodeBias = satCodeBias._bias[ii];
      *out << "   " << frqCodeBias._rnxType2ch << ' '
        << setw(10) << setprecision(4) << frqCodeBias._value;
    }
    *out << endl;
  }
  out->flush();
}

//
////////////////////////////////////////////////////////////////////////////
void t_satCodeBias::readEpoch(const string& epoLine, istream& inStream, QList<t_satCodeBias>& biasList)
{
  bncTime epoTime;
  unsigned int updateInt;
  int numSat;
  string staID;
  if (t_corrSSR::readEpoLine(epoLine, epoTime, updateInt, numSat, staID) != t_corrSSR::codeBias) {
    return;
  }
  for (int ii = 0; ii < numSat; ii++) {
    t_satCodeBias satCodeBias;
    satCodeBias._time = epoTime;
    satCodeBias._updateInt = updateInt;
    satCodeBias._staID = staID;

    string line;
    getline(inStream, line);
    istringstream in(line.c_str());

    int numBias;
    in >> satCodeBias._prn >> numBias;

    char sys = satCodeBias._prn.system();
    int num = satCodeBias._prn.number();
    int flag = t_corrSSR::getSsrNavTypeFlag(sys, num);
    satCodeBias._prn.setFlag(flag);

    while (in.good()) {
      t_frqCodeBias frqCodeBias;
      in >> frqCodeBias._rnxType2ch >> frqCodeBias._value;
      if (!frqCodeBias._rnxType2ch.empty()) {
        satCodeBias._bias.push_back(frqCodeBias);
      }
    }

    biasList.push_back(satCodeBias);
  }
}

//
////////////////////////////////////////////////////////////////////////////
void t_satPhaseBias::writeEpoch(ostream* out, const QList<t_satPhaseBias>& biasList)
{
  if (!out || biasList.size() == 0) {
    return;
  }
  out->setf(ios::fixed);
  bncTime epoTime;
  QListIterator<t_satPhaseBias> it(biasList);
  while (it.hasNext()) {
    const t_satPhaseBias& satPhaseBias = it.next();
    if (!epoTime.valid()) {
      epoTime = satPhaseBias._time;
      *out << "> PHASE_BIAS " << epoTime.datestr(' ') << ' ' << epoTime.timestr(1, ' ') << " "
        << satPhaseBias._updateInt << " "
        << biasList.size() << ' ' << satPhaseBias._staID << endl;

      if (satPhaseBias._ssrFormat == e_ssrFormat::ssrRtcmNew) {
        *out << " " << static_cast<int>(satPhaseBias._ssrFormat) << "   "
          << satPhaseBias._satYawInfoInd << "   "
          << satPhaseBias._extPBPhaseInd << endl;
      }
      else {
        *out << " " << static_cast<int>(satPhaseBias._ssrFormat) << "   "
          << satPhaseBias._dispBiasConsistInd << "   "
          << satPhaseBias._MelbWuebConsistInd << endl;
      }
    }
    if (satPhaseBias._ssrFormat == e_ssrFormat::ssrRtcmNew) {
      *out << satPhaseBias._prn.toString() << ' '
        << setw(12) << setprecision(8) << satPhaseBias._yaw * 180.0 / M_PI << ' '
        << setw(12) << setprecision(8) << satPhaseBias._yawRate * 180.0 / M_PI << "   "
        << setw(2) << satPhaseBias._bias.size();
      for (unsigned ii = 0; ii < satPhaseBias._bias.size(); ii++) {
        const t_frqPhaseBias& frqPhaseBias = satPhaseBias._bias[ii];
        *out << "   " << frqPhaseBias._rnxType2ch << ' '
          << setw(10) << setprecision(4) << frqPhaseBias._value << ' '
          << setw(3) << frqPhaseBias._fixIndicator << ' '
          << setw(3) << frqPhaseBias._fixWideLaneIndicator << ' '
          << setw(3) << frqPhaseBias._jumpCounter;
      }
      *out << endl;
    }
    else {
      *out << satPhaseBias._prn.toString() << ' '
        << setw(12) << setprecision(8) << satPhaseBias._yaw * 180.0 / M_PI << ' '
        << setw(12) << setprecision(8) << satPhaseBias._yawRate * 180.0 / M_PI << "   "
        << setw(2) << satPhaseBias._bias.size();
      for (unsigned ii = 0; ii < satPhaseBias._bias.size(); ii++) {
        const t_frqPhaseBias& frqPhaseBias = satPhaseBias._bias[ii];
        *out << "   " << frqPhaseBias._rnxType2ch << ' '
          << setw(10) << setprecision(4) << frqPhaseBias._value << ' '
          << setw(3) << frqPhaseBias._fixIndicator << ' '
          << setw(3) << frqPhaseBias._fixWideLaneIndicator << ' '
          << setw(3) << frqPhaseBias._jumpCounter;
      }
      *out << endl;
    }
  }
  out->flush();
}

//
////////////////////////////////////////////////////////////////////////////
void t_satPhaseBias::readEpoch(const string& epoLine, istream& inStream, QList<t_satPhaseBias>& biasList)
{
  bncTime epoTime;
  unsigned int updateInt;
  int numSat;
  string staID;
  unsigned int metadata1;
  unsigned int metadata2;
  e_ssrFormat ssrFormat = ssrUnknown;
  if (t_corrSSR::readEpoLine(epoLine, epoTime, updateInt, numSat, staID) != t_corrSSR::phaseBias) {
    return;
  }
  for (int ii = 0; ii <= numSat; ii++) {
    t_satPhaseBias satPhaseBias;
    satPhaseBias._time = epoTime;
    satPhaseBias._updateInt = updateInt;
    satPhaseBias._staID = staID;

    string line;
    getline(inStream, line);
    istringstream in(line.c_str());

    if (ii == 0) {
      int ssrFormatInt = 0;
      in >> ssrFormatInt >> metadata1 >> metadata2;
      ssrFormat = static_cast<e_ssrFormat>(ssrFormatInt);
      continue;
    }
    satPhaseBias._ssrFormat = ssrFormat;
    switch (ssrFormat) {
    case e_ssrFormat::ssrRtcmNew:
      satPhaseBias._dispBiasConsistInd = metadata1;
      satPhaseBias._MelbWuebConsistInd = metadata2;
      break;
    case e_ssrFormat::ssrRtcmOld:
    case e_ssrFormat::ssrUnknown:
      satPhaseBias._satYawInfoInd = metadata1;
      satPhaseBias._extPBPhaseInd = metadata2;
      break;
    }
    int numBias;
    double yawDeg, yawDegRate;
    in >> satPhaseBias._prn >> yawDeg >> yawDegRate >> numBias;
    satPhaseBias._yaw = yawDeg * M_PI / 180.0;
    satPhaseBias._yawRate = yawDegRate * M_PI / 180.0;

    char sys = satPhaseBias._prn.system();
    int num = satPhaseBias._prn.number();
    int flag = t_corrSSR::getSsrNavTypeFlag(sys, num);
    satPhaseBias._prn.setFlag(flag);

    while (in.good()) {
      t_frqPhaseBias frqPhaseBias;
      in >> frqPhaseBias._rnxType2ch >> frqPhaseBias._value >> frqPhaseBias._fixIndicator >> frqPhaseBias._fixWideLaneIndicator >> frqPhaseBias._jumpCounter;
      if (!frqPhaseBias._rnxType2ch.empty()) {
        satPhaseBias._bias.push_back(frqPhaseBias);
      }
    }

    biasList.push_back(satPhaseBias);
  }
}

//
////////////////////////////////////////////////////////////////////////////
void t_vTec::write(ostream* out, const t_vTec& vTec)
{
  if (!out || vTec._layers.size() == 0) {
    return;
  }
  out->setf(ios::fixed);
  bncTime epoTime = vTec._time;
  *out << "> VTEC " << epoTime.datestr(' ') << ' ' << epoTime.timestr(1, ' ') << " "
    << vTec._updateInt << " "
    << vTec._layers.size() << ' ' << vTec._staID << endl;
  for (unsigned ii = 0; ii < vTec._layers.size(); ii++) {
    const t_vTecLayer& layer = vTec._layers[ii];
    *out << setw(2) << ii + 1 << ' '
      << setw(2) << layer._C.Nrows() - 1 << ' '
      << setw(2) << layer._C.Ncols() - 1 << ' '
      << setw(10) << setprecision(1) << layer._height << endl
      << setw(10) << setprecision(4) << layer._C
      << setw(10) << setprecision(4) << layer._S;
  }
  out->flush();
}

//
////////////////////////////////////////////////////////////////////////////
void t_vTec::read(const string& epoLine, istream& inStream, t_vTec& vTec)
{
  bncTime epoTime;
  unsigned int updateInt;
  int numLayers;
  string staID;
  if (t_corrSSR::readEpoLine(epoLine, epoTime, updateInt, numLayers, staID) != t_corrSSR::vTec) {
    return;
  }
  if (numLayers <= 0) {
    return;
  }
  vTec._time = epoTime;
  vTec._updateInt = updateInt;
  vTec._staID = staID;
  for (int ii = 0; ii < numLayers; ii++) {
    t_vTecLayer layer;

    string line;
    getline(inStream, line);
    istringstream in(line.c_str());

    int dummy, maxDeg, maxOrd;
    in >> dummy >> maxDeg >> maxOrd >> layer._height;

    layer._C.ReSize(maxDeg + 1, maxOrd + 1);
    layer._S.ReSize(maxDeg + 1, maxOrd + 1);

    for (int iDeg = 0; iDeg <= maxDeg; iDeg++) {
      for (int iOrd = 0; iOrd <= maxOrd; iOrd++) {
        inStream >> layer._C[iDeg][iOrd];
      }
    }
    for (int iDeg = 0; iDeg <= maxDeg; iDeg++) {
      for (int iOrd = 0; iOrd <= maxOrd; iOrd++) {
        inStream >> layer._S[iDeg][iOrd];
      }
    }

    vTec._layers.push_back(layer);
  }
}

//
////////////////////////////////////////////////////////////////////////////
t_corrSSR::e_type t_corrSSR::readEpoLine(const string& line, bncTime& epoTime,
  unsigned int& updateInt, int& numEntries,
  string& staID)
{

  istringstream inLine(line.c_str());

  char epoChar;
  string typeString;
  int year, month, day, hour, min;
  double sec;

  inLine >> epoChar >> typeString >> year >> month >> day >> hour >> min >> sec >> updateInt >> numEntries >> staID;

  if (epoChar == '>') {
    epoTime.set(year, month, day, hour, min, sec);
    if (typeString == "CLOCK") {
      return clkCorr;
    }
    else if (typeString == "ORBIT") {
      return orbCorr;
    }
    else if (typeString == "CODE_BIAS") {
      return codeBias;
    }
    else if (typeString == "PHASE_BIAS") {
      return phaseBias;
    }
    else if (typeString == "VTEC") {
      return vTec;
    }
    else if (typeString == "URA") {
      return URA;
    }
  }

  return unknown;
}

// Set NAV type to force NAVtype usage according SSR standard
/////////////////////////////////////////////////////////////
t_eph::e_type t_corrSSR::getSsrNavTypeFlag(char sys, int num)
{

  t_eph::e_type flag = t_eph::undefined;

  switch (sys) {
  case 'G':
  case 'J':
  case 'I': // not part of the SSR standard
    flag = t_eph::LNAV;
    break;
  case 'R':
    flag = t_eph::FDMA_M;
    break;
  case 'E':
    flag = t_eph::INAV;
    break;
  case 'C':
    if (num < 6) { // GEO
      flag = t_eph::D2;
    }
    else if (num > 58 && num < 63) { // GEO
      flag = t_eph::D2;
    }
    else {
      flag = t_eph::D1;
    }
    break;
  case 'S':
    flag = t_eph::SBASL1;
    break;
  }

  return flag;
}