Index: branches/BNC_2.11.0/src/RTCM3/ephemeris.cpp =================================================================== --- branches/BNC_2.11.0/src/RTCM3/ephemeris.cpp (revision 6202) +++ branches/BNC_2.11.0/src/RTCM3/ephemeris.cpp (revision 6214) @@ -88,5 +88,5 @@ // Compute GPS Satellite Position (virtual) //////////////////////////////////////////////////////////////////////////// -void t_ephGPS::position(int GPSweek, double GPSweeks, +t_irc t_ephGPS::position(int GPSweek, double GPSweeks, double* xc, double* vv) const { @@ -101,5 +101,5 @@ double a0 = _sqrt_A * _sqrt_A; if (a0 == 0) { - return; + return failure; } @@ -126,7 +126,7 @@ double xp = r*cos(u); double yp = r*sin(u); - double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - + double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - omegaEarth*_TOEsec; - + double sinom = sin(OM); double cosom = cos(OM); @@ -135,6 +135,6 @@ xc[0] = xp*cosom - yp*cosi*sinom; xc[1] = xp*sinom + yp*cosi*cosom; - xc[2] = yp*sini; - + xc[2] = yp*sini; + double tc = tt - _TOC; xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc; @@ -144,10 +144,10 @@ double tanv2 = tan(v/2); double dEdM = 1 / (1 - _e*cos(E)); - double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) + double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) * dEdM * n; double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv; double dotom = _OMEGADOT - omegaEarth; double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv; - double dotr = a0 * _e*sin(E) * dEdM * n + double dotr = a0 * _e*sin(E) * dEdM * n + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv; double dotx = dotr*cos(u) - r*sin(u)*dotu; @@ -167,4 +167,6 @@ // ----------------------- xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c; + + return success; } @@ -274,5 +276,5 @@ /static_cast(1<<13)) GPSADDBITSFLOAT(8, _TGD, 1.0/static_cast(1<<30)/static_cast(1<<1)) - GPSADDBITS(6, _health) + GPSADDBITS(6, _health) GPSADDBITS(1, _L2PFlag) GPSADDBITS(1, 0) /* GPS fit interval */ @@ -299,5 +301,5 @@ ColumnVector vv = xv.rows(4,6); - // Acceleration + // Acceleration // ------------ static const double gmWGS = 398.60044e12; @@ -319,6 +321,6 @@ va(2) = vv(2); va(3) = vv(3); - va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; - va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1]; + va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2) + acc[0]; + va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1) + acc[1]; va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3) + acc[2]; @@ -328,5 +330,5 @@ // Compute Glonass Satellite Position (virtual) //////////////////////////////////////////////////////////////////////////// -void t_ephGlo::position(int GPSweek, double GPSweeks, +t_irc t_ephGlo::position(int GPSweek, double GPSweeks, double* xc, double* vv) const { @@ -337,4 +339,8 @@ double dtPos = bncTime(GPSweek, GPSweeks) - _tt; + + if (fabs(dtPos) > 24*3600.0) { + return failure; + } int nSteps = int(fabs(dtPos) / nominalStep) + 1; @@ -345,5 +351,5 @@ acc[1] = _y_acceleration * 1.e3; acc[2] = _z_acceleration * 1.e3; - for (int ii = 1; ii <= nSteps; ii++) { + for (int ii = 1; ii <= nSteps; ii++) { _xv = rungeKutta4(_tt.gpssec(), _xv, step, acc, glo_deriv); _tt = _tt + step; @@ -364,4 +370,6 @@ double dtClk = bncTime(GPSweek, GPSweeks) - _TOC; xc[3] = -_tau + _gamma * dtClk; + + return success; } @@ -382,8 +390,8 @@ int ww = ee->GPSWeek; - int tow = ee->GPSTOW; + int tow = ee->GPSTOW; updatetime(&ww, &tow, ee->tb*1000, 0); // Moscow -> GPS - // Check the day once more + // Check the day once more // ----------------------- bool timeChanged = false; @@ -418,16 +426,16 @@ if (false && timeChanged && BNC_CORE->mode() == t_bncCore::batchPostProcessing) { bncTime newHTime(ww, (double) tow); - cout << "GLONASS " << ee->almanac_number << " Time Changed at " - << currentTime.datestr() << " " << currentTime.timestr() + cout << "GLONASS " << ee->almanac_number << " Time Changed at " + << currentTime.datestr() << " " << currentTime.timestr() << endl - << "old: " << hTime.datestr() << " " << hTime.timestr() + << "old: " << hTime.datestr() << " " << hTime.timestr() << endl - << "new: " << newHTime.datestr() << " " << newHTime.timestr() + << "new: " << newHTime.datestr() << " " << newHTime.timestr() << endl - << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb + << "eph: " << ee->GPSWeek << " " << ee->GPSTOW << " " << ee->tb << endl - << "ww, tow (old): " << ww_old << " " << tow_old + << "ww, tow (old): " << ww_old << " " << tow_old << endl - << "ww, tow (new): " << ww << " " << tow + << "ww, tow (new): " << ww << " " << tow << endl << endl; } @@ -444,11 +452,11 @@ _gamma = ee->gamma; _x_pos = ee->x_pos; - _x_velocity = ee->x_velocity; + _x_velocity = ee->x_velocity; _x_acceleration = ee->x_acceleration; - _y_pos = ee->y_pos; - _y_velocity = ee->y_velocity; + _y_pos = ee->y_pos; + _y_velocity = ee->y_velocity; _y_acceleration = ee->y_acceleration; - _z_pos = ee->z_pos; - _z_velocity = ee->z_velocity; + _z_pos = ee->z_pos; + _z_velocity = ee->z_velocity; _z_acceleration = ee->z_acceleration; _health = 0; @@ -460,10 +468,10 @@ _tt = _TOC; - _xv(1) = _x_pos * 1.e3; - _xv(2) = _y_pos * 1.e3; - _xv(3) = _z_pos * 1.e3; - _xv(4) = _x_velocity * 1.e3; - _xv(5) = _y_velocity * 1.e3; - _xv(6) = _z_velocity * 1.e3; + _xv(1) = _x_pos * 1.e3; + _xv(2) = _y_pos * 1.e3; + _xv(3) = _z_pos * 1.e3; + _xv(4) = _x_velocity * 1.e3; + _xv(5) = _y_velocity * 1.e3; + _xv(6) = _z_velocity * 1.e3; _ok = true; @@ -513,5 +521,5 @@ GLONASSADDBITS(6, (static_cast(_tki)/60)%60) GLONASSADDBITS(1, (static_cast(_tki)/30)%30) - GLONASSADDBITS(1, _health) + GLONASSADDBITS(1, _health) GLONASSADDBITS(1, 0) unsigned long long timeofday = (static_cast(_tt.gpssec()+3*60*60-_gps_utc)%86400); @@ -581,5 +589,5 @@ _TOEsec = _TOC.gpssec(); - //// _TOEsec = ee->TOE; //// TODO: + //// _TOEsec = ee->TOE; //// TODO: _Cic = ee->Cic; _OMEGA0 = ee->OMEGA0; @@ -609,5 +617,5 @@ // Compute Galileo Satellite Position (virtual) //////////////////////////////////////////////////////////////////////////// -void t_ephGal::position(int GPSweek, double GPSweeks, +t_irc t_ephGal::position(int GPSweek, double GPSweeks, double* xc, double* vv) const { @@ -621,5 +629,5 @@ double a0 = _sqrt_A * _sqrt_A; if (a0 == 0) { - return; + return failure; } @@ -646,7 +654,7 @@ double xp = r*cos(u); double yp = r*sin(u); - double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - + double OM = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - omegaEarth*_TOEsec; - + double sinom = sin(OM); double cosom = cos(OM); @@ -655,6 +663,6 @@ xc[0] = xp*cosom - yp*cosi*sinom; xc[1] = xp*sinom + yp*cosi*cosom; - xc[2] = yp*sini; - + xc[2] = yp*sini; + double tc = tt - _TOC; xc[3] = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc; @@ -664,10 +672,10 @@ double tanv2 = tan(v/2); double dEdM = 1 / (1 - _e*cos(E)); - double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) + double dotv = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2) * dEdM * n; double dotu = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv; double dotom = _OMEGADOT - omegaEarth; double doti = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv; - double dotr = a0 * _e*sin(E) * dEdM * n + double dotr = a0 * _e*sin(E) * dEdM * n + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv; double dotx = dotr*cos(u) - r*sin(u)*dotu; @@ -688,4 +696,6 @@ // xc(4) -= 4.442807633e-10 * _e * sqrt(a0) *sin(E); xc[3] -= 2.0 * (xc[0]*vv[0] + xc[1]*vv[1] + xc[2]*vv[2]) / t_CST::c / t_CST::c; + + return success; } @@ -806,5 +816,5 @@ int year, month, day, hour, min; double sec; - + in >> _prn >> year >> month >> day >> hour >> min >> sec; @@ -812,5 +822,5 @@ _prn = QString("G%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0')); } - + if (year < 80) { year += 2000; @@ -926,5 +936,5 @@ int year, month, day, hour, min; double sec; - + in >> _prn >> year >> month >> day >> hour >> min >> sec; @@ -932,5 +942,5 @@ _prn = QString("R%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0')); } - + if (year < 80) { year += 2000; @@ -985,11 +995,11 @@ // ------------------------ _tt = _TOC; - _xv.ReSize(6); - _xv(1) = _x_pos * 1.e3; - _xv(2) = _y_pos * 1.e3; - _xv(3) = _z_pos * 1.e3; - _xv(4) = _x_velocity * 1.e3; - _xv(5) = _y_velocity * 1.e3; - _xv(6) = _z_velocity * 1.e3; + _xv.ReSize(6); + _xv(1) = _x_pos * 1.e3; + _xv(2) = _y_pos * 1.e3; + _xv(3) = _z_pos * 1.e3; + _xv(4) = _x_velocity * 1.e3; + _xv(5) = _y_velocity * 1.e3; + _xv(6) = _z_velocity * 1.e3; _ok = true; @@ -1028,5 +1038,5 @@ int year, month, day, hour, min; double sec; - + in >> _prn >> year >> month >> day >> hour >> min >> sec; @@ -1034,5 +1044,5 @@ _prn = QString("E%1").arg(_prn.toInt(), 2, 10, QLatin1Char('0')); } - + if (year < 80) { year += 2000; @@ -1113,5 +1123,5 @@ } -// +// ////////////////////////////////////////////////////////////////////////////// QString t_eph::rinexDateStr(const bncTime& tt, const QString& prn, @@ -1119,10 +1129,10 @@ QString datStr; - + unsigned year, month, day, hour, min; double sec; tt.civil_date(year, month, day); tt.civil_time(hour, min, sec); - + QTextStream out(&datStr); @@ -1155,7 +1165,7 @@ QString rnxStr = rinexDateStr(_TOC, _prn, version); - + QTextStream out(&rnxStr); - + out << QString("%1%2%3\n") .arg(_clock_bias, 19, 'e', 12) Index: branches/BNC_2.11.0/src/RTCM3/ephemeris.h =================================================================== --- branches/BNC_2.11.0/src/RTCM3/ephemeris.h (revision 6202) +++ branches/BNC_2.11.0/src/RTCM3/ephemeris.h (revision 6214) @@ -7,4 +7,5 @@ #include #include "bnctime.h" +#include "bncconst.h" extern "C" { #include "rtcm3torinex.h" @@ -25,5 +26,5 @@ virtual e_type type() const = 0; virtual QString toString(double version) const = 0; - virtual void position(int GPSweek, double GPSweeks, + virtual t_irc position(int GPSweek, double GPSweeks, double* xc, double* vv) const = 0; virtual int IOD() const = 0; @@ -38,14 +39,17 @@ const QDateTime& receptDateTime() const {return _receptDateTime;} - void position(int GPSweek, double GPSweeks, + t_irc position(int GPSweek, double GPSweeks, double& xx, double& yy, double& zz, double& cc) const { double tmp_xx[4]; double tmp_vv[4]; - position(GPSweek, GPSweeks, tmp_xx, tmp_vv); - + if (position(GPSweek, GPSweeks, tmp_xx, tmp_vv) != success) { + return failure; + } xx = tmp_xx[0]; yy = tmp_xx[1]; zz = tmp_xx[2]; cc = tmp_xx[3]; + + return success; } @@ -53,5 +57,5 @@ double version); - protected: + protected: QString _prn; bncTime _TOC; @@ -73,5 +77,5 @@ void set(const gpsephemeris* ee); - virtual void position(int GPSweek, double GPSweeks, + virtual t_irc position(int GPSweek, double GPSweeks, double* xc, double* vv) const; @@ -84,30 +88,30 @@ private: - double _clock_bias; // [s] - double _clock_drift; // [s/s] + double _clock_bias; // [s] + double _clock_drift; // [s/s] double _clock_driftrate; // [s/s^2] - double _IODE; - double _Crs; // [m] + double _IODE; + double _Crs; // [m] double _Delta_n; // [rad/s] - double _M0; // [rad] - - double _Cuc; // [rad] - double _e; // - double _Cus; // [rad] + double _M0; // [rad] + + double _Cuc; // [rad] + double _e; // + double _Cus; // [rad] double _sqrt_A; // [m^0.5] - double _TOEsec; // [s] - double _Cic; // [rad] - double _OMEGA0; // [rad] - double _Cis; // [rad] - - double _i0; // [rad] - double _Crc; // [m] - double _omega; // [rad] + double _TOEsec; // [s] + double _Cic; // [rad] + double _OMEGA0; // [rad] + double _Cis; // [rad] + + double _i0; // [rad] + double _Crc; // [m] + double _omega; // [rad] double _OMEGADOT; // [rad/s] double _IDOT; // [rad/s] - double _L2Codes; // Codes on L2 channel + double _L2Codes; // Codes on L2 channel double _TOEweek; double _L2PFlag; // L2 P data flag @@ -115,8 +119,8 @@ double _ura; // SV accuracy double _health; // SV health - double _TGD; // [s] - double _IODC; - - double _TOT; // Transmisstion time + double _TGD; // [s] + double _IODC; + + double _TOT; // Transmisstion time double _fitInterval; // Fit interval }; @@ -133,5 +137,5 @@ virtual QString toString(double version) const; - virtual void position(int GPSweek, double GPSweeks, + virtual t_irc position(int GPSweek, double GPSweeks, double* xc, double* vv) const; @@ -149,26 +153,26 @@ double* acc); - mutable bncTime _tt; // time + mutable bncTime _tt; // time mutable ColumnVector _xv; // status vector (position, velocity) at time _tt double _gps_utc; - double _tau; // [s] - double _gamma; // + double _tau; // [s] + double _gamma; // double _tki; // message frame time - double _x_pos; // [km] - double _x_velocity; // [km/s] - double _x_acceleration; // [km/s^2] - double _health; // 0 = O.K. - - double _y_pos; // [km] - double _y_velocity; // [km/s] - double _y_acceleration; // [km/s^2] - double _frequency_number; // ICD-GLONASS data position - - double _z_pos; // [km] - double _z_velocity; // [km/s] - double _z_acceleration; // [km/s^2] - double _E; // Age of Information [days] + double _x_pos; // [km] + double _x_velocity; // [km/s] + double _x_acceleration; // [km/s^2] + double _health; // 0 = O.K. + + double _y_pos; // [km] + double _y_velocity; // [km/s] + double _y_acceleration; // [km/s^2] + double _frequency_number; // ICD-GLONASS data position + + double _z_pos; // [km] + double _z_velocity; // [km/s] + double _z_acceleration; // [km/s^2] + double _E; // Age of Information [days] }; @@ -185,5 +189,5 @@ void set(const galileoephemeris* ee); - virtual void position(int GPSweek, double GPSweeks, + virtual t_irc position(int GPSweek, double GPSweeks, double* xc, double* vv) const; @@ -194,26 +198,26 @@ private: - double _clock_bias; // [s] - double _clock_drift; // [s/s] + double _clock_bias; // [s] + double _clock_drift; // [s/s] double _clock_driftrate; // [s/s^2] - double _IODnav; - double _Crs; // [m] + double _IODnav; + double _Crs; // [m] double _Delta_n; // [rad/s] - double _M0; // [rad] - - double _Cuc; // [rad] - double _e; // - double _Cus; // [rad] + double _M0; // [rad] + + double _Cuc; // [rad] + double _e; // + double _Cus; // [rad] double _sqrt_A; // [m^0.5] - double _TOEsec; // [s] - double _Cic; // [rad] - double _OMEGA0; // [rad] - double _Cis; // [rad] - - double _i0; // [rad] - double _Crc; // [m] - double _omega; // [rad] + double _TOEsec; // [s] + double _Cic; // [rad] + double _OMEGA0; // [rad] + double _Cis; // [rad] + + double _i0; // [rad] + double _Crc; // [m] + double _omega; // [rad] double _OMEGADOT; // [rad/s] @@ -226,6 +230,6 @@ double _E5aHS; // E5a Health Status double _E5bHS; // E5a Health Status - double _BGD_1_5A; // group delay [s] - double _BGD_1_5B; // group delay [s] + double _BGD_1_5A; // group delay [s] + double _BGD_1_5B; // group delay [s] double _TOT; // [s]