Index: trunk/BNC/RTCM3/ephemeris.cpp =================================================================== --- trunk/BNC/RTCM3/ephemeris.cpp (revision 2220) +++ trunk/BNC/RTCM3/ephemeris.cpp (revision 2221) @@ -5,4 +5,5 @@ #include "ephemeris.h" +#include "bncutils.h" #include "timeutils.h" @@ -270,2 +271,110 @@ out << endl; } + +// Derivative of the state vector using a simple force model (static) +//////////////////////////////////////////////////////////////////////////// +ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) { + + // State vector components + // ----------------------- + ColumnVector rr = xv.rows(1,3); + ColumnVector vv = xv.rows(4,6); + + // Acceleration + // ------------ + static const double GM = 398.60044e12; + static const double AE = 6378136.0; + static const double OMEGA = 7292115.e-11; + static const double C20 = -1082.63e-6; + + double rho = rr.norm_Frobenius(); + double t1 = -GM/(rho*rho*rho); + double t2 = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho); + double t3 = OMEGA * OMEGA; + double t4 = 2.0 * OMEGA; + double z2 = rr(3) * rr(3); + + // Vector of derivatives + // --------------------- + ColumnVector va(6); + va(1) = vv(1); + 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); + va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1); + va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho)) ) * rr(3); + + return va; +} + +// Compute Glonass Satellite Position (virtual) +//////////////////////////////////////////////////////////////////////////// +void t_ephGlo::position(int GPSweek, double GPSweeks, + double* xc, double* vv) const { + + static const double secPerWeek = 7 * 86400.0; + static const double nominalStep = 10.0; + + memset(xc, 0, 4*sizeof(double)); + memset(vv, 0, 3*sizeof(double)); + + double dtPos = GPSweeks - _tt; + if (GPSweek != _GPSweek) { + dtPos += (GPSweek - _GPSweek) * secPerWeek; + } + + int nSteps = int(fabs(dtPos) / nominalStep) + 1; + double step = dtPos / nSteps; + + for (int ii = 1; ii <= nSteps; ii++) { + _xv = rungeKutta4(_tt, _xv, step, glo_deriv); + _tt += step; + } + + // Position and Velocity + // --------------------- + xc[0] = _xv(1); + xc[1] = _xv(2); + xc[2] = _xv(3); + + vv[0] = _xv(4); + vv[1] = _xv(5); + vv[2] = _xv(6); + + // Clock Correction + // ---------------- + double dtClk = GPSweeks - _GPSweeks; + if (GPSweek != _GPSweek) { + dtClk += (GPSweek - _GPSweek) * secPerWeek; + } + xc[3] = -_tau + _gamma * dtClk; +} + +// IOD of Glonass Ephemeris (virtual) +//////////////////////////////////////////////////////////////////////////// +int t_ephGlo::IOD() const { + + bool old = false; + + if (old) { // 5 LSBs of iod are equal to 5 LSBs of tb + unsigned int tb = int(fmod(_GPSweeks,86400.0)); //sec of day + const int shift = sizeof(tb) * 8 - 5; + unsigned int iod = tb << shift; + return (iod >> shift); + } + else { + return int(fmod(_tki, 3600)) / 30; + } +} + +// Print Glonass Ephemeris (virtual) +//////////////////////////////////////////////////////////////////////////// +void t_ephGlo::print(std::ostream& out) const { + +} + +// Set Glonass Ephemeris +//////////////////////////////////////////////////////////////////////////// +void t_ephGlo::set(const glonassephemeris* ee) { + +} Index: trunk/BNC/RTCM3/ephemeris.h =================================================================== --- trunk/BNC/RTCM3/ephemeris.h (revision 2220) +++ trunk/BNC/RTCM3/ephemeris.h (revision 2221) @@ -1,4 +1,6 @@ #ifndef EPHEMERIS_H #define EPHEMERIS_H + +#include #include @@ -19,9 +21,9 @@ virtual void position(int GPSweek, double GPSweeks, - double* xc, - double* vv) const = 0; + double* xc, + double* vv) const = 0; void position(int GPSweek, double GPSweeks, - double& xx, double& yy, double& zz, double& cc) const { + double& xx, double& yy, double& zz, double& cc) const { double tmp_xx[4]; double tmp_vv[4]; @@ -49,5 +51,5 @@ public: t_ephGPS() { } - ~t_ephGPS() {} + virtual ~t_ephGPS() {} double TOC() const {return _TOC;} @@ -55,27 +57,27 @@ void set(int prn, - int GPSWeek, - double toc, double toe, double tot, - double IODE, double IODC, - double clock_bias, double clock_drift, double clock_driftrate, - double OMEGA0, double OMEGADOT, - double i0, double IDOT, - double omega, - double M0, double Delta_n, - double sqrt_A, - double e, - double Crc, double Crs, - double Cic, double Cis, - double Cuc, double Cus, - double TGD, - int health); + int GPSWeek, + double toc, double toe, double tot, + double IODE, double IODC, + double clock_bias, double clock_drift, double clock_driftrate, + double OMEGA0, double OMEGADOT, + double i0, double IDOT, + double omega, + double M0, double Delta_n, + double sqrt_A, + double e, + double Crc, double Crs, + double Cic, double Cis, + double Cuc, double Cus, + double TGD, + int health); - void position(int GPSweek, double GPSweeks, - double* xc, - double* vv) const; + virtual void position(int GPSweek, double GPSweeks, + double* xc, + double* vv) const; - int IOD() const { return static_cast(_IODC); } + virtual int IOD() const { return static_cast(_IODC); } - void print(std::ostream& out) const; + virtual void print(std::ostream& out) const; private: @@ -109,3 +111,43 @@ }; +class t_ephGlo : public t_eph { + public: + t_ephGlo() { _gps_utc = 0.0; _xv.ReSize(6); } + + virtual ~t_ephGlo() {} + + virtual void position(int GPSweek, double GPSweeks, + double* xc, + double* vv) const; + + virtual int IOD() const; + + virtual void print(std::ostream& out) const; + + void set(const glonassephemeris* ee); + + private: + static ColumnVector glo_deriv(double /* tt */, const ColumnVector& xv); + + mutable double _tt; // time in seconds of GPSweek + mutable ColumnVector _xv; // status vector (position, velocity) at time _tt + + double _gps_utc; // GPS - UTC in seconds + double _E; // [days] + double _tau; // [s] + double _gamma; // + double _x_pos; // [km] + double _x_velocity; // [km/s] + double _x_acceleration; // [km/s^2] + double _y_pos; // [km] + double _y_velocity; // [km/s] + double _y_acceleration; // [km/s^2] + double _z_pos; // [km] + double _z_velocity; // [km/s] + double _z_acceleration; // [km/s^2] + double _health; // 0 = O.K. + double _frequency_number; // ICD-GLONASS data position + double _tki; // message frame time +}; + #endif Index: trunk/BNC/bncpppclient.cpp =================================================================== --- trunk/BNC/bncpppclient.cpp (revision 2220) +++ trunk/BNC/bncpppclient.cpp (revision 2221) @@ -158,4 +158,6 @@ QString prn = QString("%1%2").arg(obs->satSys).arg(obs->satNum, 2, 10, QChar('0')); + + cout << prn.toAscii().data() << " " << obs->slot << endl; _epoData->satData[prn] = satData; Index: trunk/BNC/bncutils.cpp =================================================================== --- trunk/BNC/bncutils.cpp (revision 2220) +++ trunk/BNC/bncutils.cpp (revision 2221) @@ -243,2 +243,24 @@ + sinPhi * xyz[2]; } + +// Fourth order Runge-Kutta numerical integrator for ODEs +//////////////////////////////////////////////////////////////////////////// +ColumnVector rungeKutta4( + double xi, // the initial x-value + const ColumnVector& yi, // vector of the initial y-values + double dx, // the step size for the integration + ColumnVector (*der)(double x, const ColumnVector& y) + // A pointer to a function that computes the + // derivative of a function at a point (x,y) + ) { + + ColumnVector k1 = der(xi , yi ) * dx; + ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0) * dx; + ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0) * dx; + ColumnVector k4 = der(xi+dx , yi+k3 ) * dx; + + ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0; + + return yf; +} + Index: trunk/BNC/bncutils.h =================================================================== --- trunk/BNC/bncutils.h (revision 2220) +++ trunk/BNC/bncutils.h (revision 2221) @@ -51,3 +51,5 @@ void xyz2neu(const double* Ell, const double* xyz, double* neu); +ColumnVector rungeKutta4(double xi, const ColumnVector& yi, double dx, + ColumnVector (*der)(double x, const ColumnVector& y)); #endif