source: ntrip/trunk/BNC/RTCM/RTCM2Decoder.cpp@ 1202

// Part of BNC, a utility for retrieving decoding and
// converting GNSS data streams from NTRIP broadcasters.
//
// Copyright (C) 2007
// German Federal Agency for Cartography and Geodesy (BKG)
// http://www.bkg.bund.de
// Czech Technical University Prague, Department of Geodesy
// http://www.fsv.cvut.cz
//
// Email: euref-ip@bkg.bund.de
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation, version 2.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

/* -------------------------------------------------------------------------
 * BKG NTRIP Client
 * -------------------------------------------------------------------------
 *
 * Class:      RTCM2Decoder
 *
 * Purpose:    RTCM2 Decoder
 *
 * Author:     L. Mervart
 *
 * Created:    24-Aug-2006
 *
 * Changes:    
 *
 * -----------------------------------------------------------------------*/

#include <math.h>
#include <sstream>
#include <iomanip>

#include "../bncutils.h"
#include "rtcm_utils.h"
#include "GPSDecoder.h"
#include "RTCM2Decoder.h"

using namespace std;
using namespace rtcm2;

// 
// Constructor
// 

RTCM2Decoder::RTCM2Decoder(const std::string& ID) {
  _ID = ID;
}

// 
// Destructor
// 

RTCM2Decoder::~RTCM2Decoder() {
  for (t_pairMap::iterator ii = _ephPair.begin(); ii != _ephPair.end(); ii++) {
    delete ii->second;
  }
}


//
t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz) {
  if ( !_msg03.validMsg ) {
    return failure;
  }
  
  xx = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
  yy = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
  zz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);

  return success;
}

//
t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz,
                              double& dx1, double& dy1, double& dz1,
                              double& dx2, double& dy2, double& dz2) {
  xx = _msg03.x;
  yy = _msg03.y;
  zz = _msg03.z;

  dx1 = (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
  dy1 = (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
  dz1 = (_msg22.validMsg ? _msg22.dL1[2] : 0.0);

  dx2 = (_msg22.validMsg ? _msg22.dL2[0] : 0.0);
  dy2 = (_msg22.validMsg ? _msg22.dL2[1] : 0.0);
  dz2 = (_msg22.validMsg ? _msg22.dL2[2] : 0.0);

  return success;
}


//
t_irc RTCM2Decoder::Decode(char* buffer, int bufLen) {

  _buffer.append(buffer, bufLen);
  int    refWeek;
  double refSecs;
  currentGPSWeeks(refWeek, refSecs);
  bool decoded = false;

  while(true) {
    _PP.getPacket(_buffer);
    if (!_PP.valid()) {
      if (decoded) {
        return success;
      } else {
        return failure;
      }
    }

    if ( _PP.ID()==18 || _PP.ID()==19 ) {   

      _ObsBlock.extract(_PP);

      if (_ObsBlock.valid()) {
        decoded = true;

        int    epochWeek;
        double epochSecs;
        _ObsBlock.resolveEpoch(refWeek, refSecs, epochWeek, epochSecs);
          
        for (int iSat=0; iSat < _ObsBlock.nSat; iSat++) {
          p_obs obs = new t_obs();
          _obsList.push_back(obs);
          if (_ObsBlock.PRN[iSat] > 100) {
            obs->_o.satNum      = _ObsBlock.PRN[iSat] % 100;
            obs->_o.satSys      = 'R';
          }                     
          else {                
            obs->_o.satNum      = _ObsBlock.PRN[iSat];
            obs->_o.satSys      = 'G';
          }                     
          obs->_o.GPSWeek       = epochWeek;
          obs->_o.GPSWeeks      = epochSecs;
          obs->_o.C1            = _ObsBlock.rng_C1[iSat];
          obs->_o.P1            = _ObsBlock.rng_P1[iSat];
          obs->_o.P2            = _ObsBlock.rng_P2[iSat];
          obs->_o.L1            = _ObsBlock.resolvedPhase_L1(iSat);
          obs->_o.L2            = _ObsBlock.resolvedPhase_L2(iSat);
          obs->_o.slip_cnt_L1   = _ObsBlock.slip_L1[iSat];
          obs->_o.slip_cnt_L2   = _ObsBlock.slip_L2[iSat];
          obs->_o.lock_timei_L1 = -1;
          obs->_o.lock_timei_L2 = -1;
        }
        _ObsBlock.clear();
      }
    }

    else if ( _PP.ID() == 20 || _PP.ID() == 21 ) {
      _msg2021.extract(_PP);

      if (_msg2021.valid()) {
        decoded = true;
        translateCorr2Obs();
      } 
    }

    else if ( _PP.ID() == 3 ) {
      _msg03.extract(_PP);
    }

    else if ( _PP.ID() == 22 ) {
      _msg22.extract(_PP);
    }
  }
  return success;
}



void RTCM2Decoder::storeEph(const gpsephemeris& gpseph) {
  t_ephGPS eph; eph.set(&gpseph);

  storeEph(eph);
}


void RTCM2Decoder::storeEph(const t_ephGPS& gpseph) {
  t_ephGPS* eph = new t_ephGPS(gpseph);

  string prn = eph->prn();

  t_pairMap::iterator ip = _ephPair.find(prn);
  if (ip == _ephPair.end() ) {
    ip = _ephPair.insert(pair<string, t_ephPair*>(prn, new t_ephPair)).first;
  }
  t_ephPair* pair = ip->second;

  if ( !pair->eph || eph->isNewerThan(pair->eph) ) {
    delete pair->oldEph;
    pair->oldEph = pair->eph;
    pair->eph    = eph;

    return;
  }

  delete eph;
}
  
  
void RTCM2Decoder::translateCorr2Obs() {

  if ( !_msg03.validMsg || !_msg2021.valid() ) {
    return;
  }

  double stax = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
  double stay = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
  double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);

  int    refWeek;
  double refSecs;
  currentGPSWeeks(refWeek, refSecs);

  // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1)
  // ----------------------------------------------------------------------------------
  double hoursec_est  = _msg2021.hoursec();              // estimated time of measurement
  double hoursec_rcv  = rint(hoursec_est * 1e2) / 1e2;   // receiver clock reading at hoursec_est  
  double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light;

  int    GPSWeek;
  double GPSWeeks;
  resolveEpoch(hoursec_est, refWeek, refSecs,
               GPSWeek, GPSWeeks);

  int    GPSWeek_rcv;
  double GPSWeeks_rcv;
  resolveEpoch(hoursec_rcv, refWeek, refSecs,
               GPSWeek_rcv, GPSWeeks_rcv);

  // Loop over all satellites
  // ------------------------
  for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin();
       icorr != _msg2021.data.end(); icorr++) {
    const RTCM2_2021::HiResCorr* corr = icorr->second;

    ostringstream oPRN; oPRN.fill('0');

    oPRN <<            (corr->PRN < 200 ? 'G'       : 'R')
         << setw(2) << (corr->PRN < 200 ? corr->PRN : corr->PRN - 200);

    string PRN(oPRN.str());

    t_pairMap::const_iterator ieph = _ephPair.find(PRN);
    const t_eph* eph0 = 0;
    const t_eph* eph1 = 0;

    if ( ieph != _ephPair.end() ) {
      eph0 = ieph->second->eph;
      eph1 = ieph->second->oldEph;
    }

    if ( !eph0 && !eph1 ) {
      continue;
    }

    double L1 = 0;
    double L2 = 0;
    double P1 = 0;
    double P2 = 0;
    string obsT = "";

    // new observation
    p_obs new_obs = 0;

    for (unsigned ii = 0; ii < 4; ii++) {
      int          IODcorr = 0;
      double       corrVal = 0;
      const t_eph* eph     = 0;
      double*      obsVal  = 0;

      switch (ii) {
      case 0: // --- L1 ---
        IODcorr = corr->IODp1;
        corrVal = corr->phase1 * LAMBDA_1;
        obsVal  = &L1;
        obsT    = "L1";
        break;
      case 1: // --- L2 ---
        IODcorr = corr->IODp2;
        corrVal = corr->phase2 * LAMBDA_2;
        obsVal  = &L2;
        obsT    = "L2";
        break;
      case 2: // --- P1 ---
        IODcorr = corr->IODr1;
        corrVal = corr->range1;
        obsVal  = &P1;
        obsT    = "P1";
        break;
      case 3: // --- P2 ---
        IODcorr = corr->IODr2;
        corrVal = corr->range2;
        obsVal  = &P2;
        obsT    = "P2";
        break;
      default:
        continue;
      }

      eph = 0;
      if      ( eph0 && eph0->IOD() == IODcorr ) 
        eph = eph0;
      else if ( eph1 && eph1->IOD() == IODcorr ) 
        eph = eph1;
      if ( eph && corr ) {
        int    GPSWeek_tot;
        double GPSWeeks_tot;
        double rho, xSat, ySat, zSat, clkSat;
        cmpRho(eph, stax, stay, staz, 
               GPSWeek, GPSWeeks,
               rho, GPSWeek_tot, GPSWeeks_tot,
               xSat, ySat, zSat, clkSat);

        *obsVal = rho - corrVal + rcv_clk_bias - clkSat;

        if ( *obsVal == 0 )  *obsVal = ZEROVALUE;

        // Allocate new memory
        // -------------------
        if ( !new_obs ) {
          new_obs = new t_obs();

          new_obs->_o.StatID[0] = '\x0';
          new_obs->_o.satSys    = (corr->PRN < 200 ? 'G'       : 'R');
          new_obs->_o.satNum    = (corr->PRN < 200 ? corr->PRN : corr->PRN - 200);
          
          new_obs->_o.GPSWeek   = GPSWeek_rcv;
          new_obs->_o.GPSWeeks  = GPSWeeks_rcv;
        }
        
        // Store estimated measurements
        // ----------------------------
        switch (ii) {
        case 0: // --- L1 ---
          new_obs->_o.L1 = *obsVal / LAMBDA_1;
          new_obs->_o.slip_cnt_L1   = corr->lock1;
          new_obs->_o.lock_timei_L1 = -1;
          break;
        case 1: // --- L2 ---
          new_obs->_o.L2 = *obsVal / LAMBDA_2;
          new_obs->_o.slip_cnt_L2   = corr->lock2;
          new_obs->_o.lock_timei_L2 = -1;
          break;
        case 2: // --- C1 / P1 ---
          if ( corr->Pind1 )
            new_obs->_o.P1 = *obsVal;
          else
            new_obs->_o.C1 = *obsVal;
          break;
        case 3: // --- C2 / P2 ---
          if ( corr->Pind2 )
            new_obs->_o.P2 = *obsVal;
          else
            new_obs->_o.C2 = *obsVal;
          break;
        default:
          continue;
        }
      }
    } // loop over frequencies
    
    if ( new_obs ) {
      _obsList.push_back( new_obs );
    }
  }
}