Ignore:
Timestamp:
Mar 29, 2011, 7:15:07 PM (13 years ago)
Author:
mervart
Message:
 
File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC/upload/bncrtnetdecoder.cpp

    r3179 r3182  
    4343#include "bncutils.h"
    4444#include "bncsettings.h"
    45 #include "bncclockrinex.h"
    46 #include "bncsp3.h"
    4745
    4846using namespace std;
    … …  
    5149////////////////////////////////////////////////////////////////////////
    5250bncRtnetDecoder::bncRtnetDecoder() {
     51
    5352  bncSettings settings;
     53
    5454  _year = 0;
    55   _append = Qt::CheckState(settings.value("rnxAppend").toInt()) == Qt::Checked;
    5655
    57   // RINEX writer
    58   // ------------
    59   if ( settings.value("rnxPath").toString().isEmpty() ) {
    60     _rnx = 0;
    61   }
    62   else {
    63     QString prep  = "BNC";
    64     QString ext   = ".clk";
    65     QString path  = settings.value("rnxPath").toString();
    66     QString intr  = settings.value("rnxIntr").toString();
    67     int     sampl = settings.value("rnxSampl").toInt();
    68     _rnx = new bncClockRinex(prep, ext, path, intr, sampl);
    69   }
     56  // List of upload casters
     57  // ----------------------
    7058
    71   // SP3 writer
    72   // ----------
    73   if ( settings.value("sp3Path").toString().isEmpty() ) {
    74     _sp3 = 0;
    75   }
    76   else {
    77     QString prep  = "BNC";
    78     QString ext   = ".sp3";
    79     QString path  = settings.value("sp3Path").toString();
    80     QString intr  = settings.value("sp3Intr").toString();
    81     int     sampl = settings.value("sp3Sampl").toInt();
    82     _sp3 = new bncSP3(prep, ext, path, intr, sampl);
    83   }
    8459}
    8560
    … …  
    8762////////////////////////////////////////////////////////////////////////
    8863bncRtnetDecoder::~bncRtnetDecoder() {
    89   delete _rnx;
    90   delete _sp3;
    9164}
    9265
    … …  
    127100  // -----------------------------------
    128101  if (lines.size() > 0) {
    129 
    130     QStringList prns;
    131 
    132102    for (int ic = 0; ic < _caster.size(); ic++) {
    133       _caster.at(ic)->open();
    134 
    135       struct ClockOrbit co;
    136       memset(&co, 0, sizeof(co));
    137       co.GPSEpochTime      = (int)_GPSweeks;
    138       co.GLONASSEpochTime  = (int)fmod(_GPSweeks, 86400.0)
    139                            + 3 * 3600 - gnumleap(_year, _month, _day);
    140       co.ClockDataSupplied = 1;
    141       co.OrbitDataSupplied = 1;
    142       co.SatRefDatum       = DATUM_ITRF;
    143      
    144       struct Bias bias;
    145       memset(&bias, 0, sizeof(bias));
    146       bias.GPSEpochTime      = (int)_GPSweeks;
    147       bias.GLONASSEpochTime  = (int)fmod(_GPSweeks, 86400.0)
    148                              + 3 * 3600 - gnumleap(_year, _month, _day);
    149 
    150       for (int ii = 0; ii < lines.size(); ii++) {
    151 
    152         QString      prn;
    153         ColumnVector xx(14); xx = 0.0;
    154         t_ephPair*   pair = 0;
    155      
    156         if (ic == 0) {
    157           QTextStream in(lines[ii].toAscii());
    158           in >> prn;
    159           prns << prn;
    160           if ( _eph.contains(prn) ) {
    161             in >> xx(1) >> xx(2) >> xx(3) >> xx(4) >> xx(5)
    162                >> xx(6) >> xx(7) >> xx(8) >> xx(9) >> xx(10)
    163                >> xx(11) >> xx(12) >> xx(13) >> xx(14);
    164             xx(1) *= 1e3;          // x-crd
    165             xx(2) *= 1e3;          // y-crd
    166             xx(3) *= 1e3;          // z-crd
    167             xx(4) *= 1e-6;         // clk
    168             xx(5) *= 1e-6;         // rel. corr.
    169                                    // xx(6), xx(7), xx(8) ... PhaseCent - CoM
    170                                    // xx(9)  ... P1-C1 DCB in meters
    171                                    // xx(10) ... P1-P2 DCB in meters
    172                                    // xx(11) ... dT
    173             xx(12) *= 1e3;         // x-crd at time + dT
    174             xx(13) *= 1e3;         // y-crd at time + dT
    175             xx(14) *= 1e3;         // z-crd at time + dT
    176 
    177             pair     = _eph[prn];
    178             pair->xx = xx;
    179           }
    180         }
    181         else {
    182           prn = prns[ii];
    183           if ( _eph.contains(prn) ) {
    184             pair = _eph[prn];
    185             xx   = pair->xx;
    186           }
    187         }
    188 
    189         // Use old ephemeris if the new one is too recent
    190         // ----------------------------------------------
    191         t_eph* ep = 0;
    192         if (pair) {
    193           ep = pair->last;
    194           if (pair->prev && ep &&
    195               ep->receptDateTime().secsTo(QDateTime::currentDateTime()) < 60) {
    196             ep = pair->prev;
    197           }
    198         }
    199 
    200         if (ep != 0) {
    201           struct ClockOrbit::SatData* sd = 0;
    202           if      (prn[0] == 'G') {
    203             sd = co.Sat + co.NumberOfGPSSat;
    204             ++co.NumberOfGPSSat;
    205           }
    206           else if (prn[0] == 'R') {
    207             sd = co.Sat + CLOCKORBIT_NUMGPS + co.NumberOfGLONASSSat;
    208             ++co.NumberOfGLONASSSat;
    209           }
    210           if (sd) {
    211             QString outLine;
    212             processSatellite(ic, _caster.at(ic)->crdTrafo(),
    213                              _caster.at(ic)->CoM(), ep,
    214                              _GPSweek, _GPSweeks, prn, xx, sd, outLine);
    215             _caster.at(ic)->printAscii(outLine);
    216           }
    217 
    218           struct Bias::BiasSat* biasSat = 0;
    219           if      (prn[0] == 'G') {
    220             biasSat = bias.Sat + bias.NumberOfGPSSat;
    221             ++bias.NumberOfGPSSat;
    222           }
    223           else if (prn[0] == 'R') {
    224             biasSat = bias.Sat + CLOCKORBIT_NUMGPS + bias.NumberOfGLONASSSat;
    225             ++bias.NumberOfGLONASSSat;
    226           }
    227 
    228           // Coefficient of Ionosphere-Free LC
    229           // ---------------------------------
    230           const static double a_L1_GPS =  2.54572778;
    231           const static double a_L2_GPS = -1.54572778;
    232           const static double a_L1_Glo =  2.53125000;
    233           const static double a_L2_Glo = -1.53125000;
    234 
    235           if (biasSat) {
    236             biasSat->ID = prn.mid(1).toInt();
    237             biasSat->NumberOfCodeBiases = 3;
    238             if      (prn[0] == 'G') {
    239               biasSat->Biases[0].Type = CODETYPEGPS_L1_Z;
    240               biasSat->Biases[0].Bias = - a_L2_GPS * xx(10);
    241               biasSat->Biases[1].Type = CODETYPEGPS_L1_CA;
    242               biasSat->Biases[1].Bias = - a_L2_GPS * xx(10) + xx(9);
    243               biasSat->Biases[2].Type = CODETYPEGPS_L2_Z;
    244               biasSat->Biases[2].Bias = a_L1_GPS * xx(10);
    245             }
    246             else if (prn[0] == 'R') {
    247               biasSat->Biases[0].Type = CODETYPEGLONASS_L1_P;
    248               biasSat->Biases[0].Bias = - a_L2_Glo * xx(10);
    249               biasSat->Biases[1].Type = CODETYPEGLONASS_L1_CA;
    250               biasSat->Biases[1].Bias = - a_L2_Glo * xx(10) + xx(9);
    251               biasSat->Biases[2].Type = CODETYPEGLONASS_L2_P;
    252               biasSat->Biases[2].Bias = a_L1_Glo * xx(10);
    253             }
    254           }
    255         }
    256       }
    257      
    258       if ( _caster.at(ic)->usedSocket() &&
    259            (co.NumberOfGPSSat > 0 || co.NumberOfGLONASSSat > 0) ) {
    260         char obuffer[CLOCKORBIT_BUFFERSIZE];
    261 
    262         int len = MakeClockOrbit(&co, COTYPE_AUTO, 0, obuffer, sizeof(obuffer));
    263         if (len > 0) {
    264           _caster.at(ic)->write(obuffer, len);
    265         }
    266       }
    267 
    268       if ( _caster.at(ic)->usedSocket() &&
    269            (bias.NumberOfGPSSat > 0 || bias.NumberOfGLONASSSat > 0) ) {
    270         char obuffer[CLOCKORBIT_BUFFERSIZE];
    271         int len = MakeBias(&bias, BTYPE_AUTO, 0, obuffer, sizeof(obuffer));
    272         if (len > 0) {
    273           _caster.at(ic)->write(obuffer, len);
    274         }
    275       }
     103      _caster.at(ic)->uploadClockOrbitBias(lines, _eph, _year, _month, _day,
     104                                           _GPSweek, _GPSweeks);
    276105    }
    277106  }
    … …  
    280109}
    281110
    282 //
    283 ////////////////////////////////////////////////////////////////////////////
    284 void bncRtnetDecoder::processSatellite(int iCaster, const QString trafo,
    285                                        bool CoM, t_eph* ep, int GPSweek,
    286                                        double GPSweeks, const QString& prn,
    287                                        const ColumnVector& xx,
    288                                        struct ClockOrbit::SatData* sd,
    289                                        QString& outLine) {
    290 
    291   const double secPerWeek = 7.0 * 86400.0;
    292 
    293   ColumnVector rsw(3);
    294   ColumnVector rsw2(3);
    295   double dClk;
    296 
    297   for (int ii = 1; ii <= 2; ++ii) {
    298 
    299     int    GPSweek12  = GPSweek;
    300     double GPSweeks12 = GPSweeks;
    301     if (ii == 2) {
    302       GPSweeks12 += xx(11);
    303       if (GPSweeks12 > secPerWeek) {
    304         GPSweek12  += 1;
    305         GPSweeks12 -= secPerWeek;
    306       }
    307     }
    308 
    309     ColumnVector xB(4);
    310     ColumnVector vv(3);
    311 
    312     ep->position(GPSweek12, GPSweeks12, xB.data(), vv.data());
    313    
    314     ColumnVector xyz;
    315     if (ii == 1) {
    316       xyz = xx.Rows(1,3);
    317     }
    318     else {
    319       xyz = xx.Rows(12,14);
    320     }
    321    
    322     // Correction Center of Mass -> Antenna Phase Center
    323     // -------------------------------------------------
    324     if (! CoM) {
    325       xyz(1) += xx(6);
    326       xyz(2) += xx(7);
    327       xyz(3) += xx(8);
    328     }
    329    
    330     if (trafo != "IGS05") {
    331       crdTrafo(GPSweek12, xyz, trafo);
    332     }
    333    
    334     ColumnVector dx = xB.Rows(1,3) - xyz ;
    335    
    336     if (ii == 1) {
    337       XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw);
    338       dClk = (xx(4) + xx(5) - xB(4)) * 299792458.0;
    339     }
    340     else {
    341       XYZ_to_RSW(xB.Rows(1,3), vv, dx, rsw2);
    342     }
    343   }
    344 
    345   if (sd) {
    346     sd->ID                    = prn.mid(1).toInt();
    347     sd->IOD                   = ep->IOD();
    348     sd->Clock.DeltaA0         = dClk;
    349     sd->Orbit.DeltaRadial     = rsw(1);
    350     sd->Orbit.DeltaAlongTrack = rsw(2);
    351     sd->Orbit.DeltaCrossTrack = rsw(3);
    352     sd->Orbit.DotDeltaRadial     = (rsw2(1) - rsw(1)) / xx(11);
    353     sd->Orbit.DotDeltaAlongTrack = (rsw2(2) - rsw(2)) / xx(11);
    354     sd->Orbit.DotDeltaCrossTrack = (rsw2(3) - rsw(3)) / xx(11);
    355   }
    356 
    357   outLine.sprintf("%d %.1f %s  %3d  %10.3f  %8.3f %8.3f %8.3f\n",
    358                   GPSweek, GPSweeks, ep->prn().c_str(),
    359                   ep->IOD(), dClk, rsw(1), rsw(2), rsw(3));
    360 
    361   if (iCaster == 0) {
    362     if (_rnx) {
    363       _rnx->write(GPSweek, GPSweeks, prn, xx);
    364     }
    365     if (_sp3) {
    366       _sp3->write(GPSweek, GPSweeks, prn, xx, _append);
    367     }
    368   }
    369 }
    370 
    371 // Transform Coordinates
    372 ////////////////////////////////////////////////////////////////////////////
    373 void bncRtnetDecoder::crdTrafo(int GPSWeek, ColumnVector& xyz,
    374                                const QString& trafo) {
    375 
    376   bncSettings settings;
    377 
    378   if (trafo == "ETRF2000") {
    379     _dx  =    0.0541;
    380     _dy  =    0.0502;
    381     _dz  =   -0.0538;
    382     _dxr =   -0.0002;
    383     _dyr =    0.0001;
    384     _dzr =   -0.0018;
    385     _ox  =  0.000891;
    386     _oy  =  0.005390;
    387     _oz  = -0.008712;
    388     _oxr =  0.000081;
    389     _oyr =  0.000490;
    390     _ozr = -0.000792;
    391     _sc  =      0.40;
    392     _scr =      0.08;
    393     _t0  =    2000.0;
    394   }
    395   else if (trafo == "NAD83") {
    396     _dx  =    0.9963;
    397     _dy  =   -1.9024;
    398     _dz  =   -0.5210;
    399     _dxr =    0.0005;
    400     _dyr =   -0.0006;
    401     _dzr =   -0.0013;
    402     _ox  =  0.025915;
    403     _oy  =  0.009426;
    404     _oz  =  0.011599;
    405     _oxr =  0.000067;
    406     _oyr = -0.000757;
    407     _ozr = -0.000051;
    408     _sc  =      0.78;
    409     _scr =     -0.10;
    410     _t0  =    1997.0;
    411   }
    412   else if (trafo == "GDA94") {
    413     _dx  =   -0.07973;
    414     _dy  =   -0.00686;
    415     _dz  =    0.03803;
    416     _dxr =    0.00225;
    417     _dyr =   -0.00062;
    418     _dzr =   -0.00056;
    419     _ox  =  0.0000351;
    420     _oy  = -0.0021211;
    421     _oz  = -0.0021411;
    422     _oxr = -0.0014707;
    423     _oyr = -0.0011443;
    424     _ozr = -0.0011701;
    425     _sc  =      6.636;
    426     _scr =      0.294;
    427     _t0  =     1994.0;
    428   }
    429   else if (trafo == "SIRGAS2000") {
    430     _dx  =   -0.0051;
    431     _dy  =   -0.0065;
    432     _dz  =   -0.0099;
    433     _dxr =    0.0000;
    434     _dyr =    0.0000;
    435     _dzr =    0.0000;
    436     _ox  =  0.000150;
    437     _oy  =  0.000020;
    438     _oz  =  0.000021;
    439     _oxr =  0.000000;
    440     _oyr =  0.000000;
    441     _ozr =  0.000000;
    442     _sc  =     0.000;
    443     _scr =     0.000;
    444     _t0  =    0000.0;
    445   }
    446   else if (trafo == "SIRGAS95") {
    447     _dx  =    0.0077;
    448     _dy  =    0.0058;
    449     _dz  =   -0.0138;
    450     _dxr =    0.0000;
    451     _dyr =    0.0000;
    452     _dzr =    0.0000;
    453     _ox  =  0.000000;
    454     _oy  =  0.000000;
    455     _oz  = -0.000030;
    456     _oxr =  0.000000;
    457     _oyr =  0.000000;
    458     _ozr =  0.000000;
    459     _sc  =     1.570;
    460     _scr =     0.000;
    461     _t0  =    0000.0;
    462   }
    463   else if (trafo == "Custom") {
    464     _dx  = settings.value("trafo_dx").toDouble();
    465     _dy  = settings.value("trafo_dy").toDouble();
    466     _dz  = settings.value("trafo_dz").toDouble();
    467     _dxr = settings.value("trafo_dxr").toDouble();
    468     _dyr = settings.value("trafo_dyr").toDouble();
    469     _dzr = settings.value("trafo_dzr").toDouble();
    470     _ox  = settings.value("trafo_ox").toDouble();
    471     _oy  = settings.value("trafo_oy").toDouble();
    472     _oz  = settings.value("trafo_oz").toDouble();
    473     _oxr = settings.value("trafo_oxr").toDouble();
    474     _oyr = settings.value("trafo_oyr").toDouble();
    475     _ozr = settings.value("trafo_ozr").toDouble();
    476     _sc  = settings.value("trafo_sc").toDouble();
    477     _scr = settings.value("trafo_scr").toDouble();
    478     _t0  = settings.value("trafo_t0").toDouble();
    479   }
    480 
    481   // Current epoch minus 2000.0 in years
    482   // ------------------------------------
    483   double dt = (GPSWeek - (1042.0+6.0/7.0)) / 365.2422 * 7.0 + 2000.0 - _t0;
    484 
    485   ColumnVector dx(3);
    486 
    487   dx(1) = _dx + dt * _dxr;
    488   dx(2) = _dy + dt * _dyr;
    489   dx(3) = _dz + dt * _dzr;
    490 
    491   static const double arcSec = 180.0 * 3600.0 / M_PI;
    492 
    493   double ox = (_ox + dt * _oxr) / arcSec;
    494   double oy = (_oy + dt * _oyr) / arcSec;
    495   double oz = (_oz + dt * _ozr) / arcSec;
    496 
    497   double sc = 1.0 + _sc * 1e-9 + dt * _scr * 1e-9;
    498 
    499   Matrix rMat(3,3);
    500   rMat(1,1) = 1.0;
    501   rMat(1,2) = -oz;
    502   rMat(1,3) =  oy;
    503   rMat(2,1) =  oz;
    504   rMat(2,2) = 1.0;
    505   rMat(2,3) = -ox;
    506   rMat(3,1) = -oy;
    507   rMat(3,2) =  ox;
    508   rMat(3,3) = 1.0;
    509 
    510   xyz = sc * rMat * xyz + dx;
    511 }
Note: See TracChangeset for help on using the changeset viewer.