Changeset 2221 in ntrip

Timestamp:

Jan 12, 2010, 8:36:29 AM (16 years ago)

Author:

mervart

Message:

* empty log message *

Location:

trunk/BNC

Files:

• RTCM3/ephemeris.cpp (modified) (2 diffs)
• RTCM3/ephemeris.h (modified) (5 diffs)
• bncpppclient.cpp (modified) (1 diff)
• bncutils.cpp (modified) (1 diff)
• bncutils.h (modified) (1 diff)

trunk/BNC/RTCM3/ephemeris.cpp

@@ -5 +5 @@
 
 #include "ephemeris.h"
+#include "bncutils.h"
 #include "timeutils.h"
 
@@ -270 +271 @@
   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) {
+
+}

trunk/BNC/RTCM3/ephemeris.h

@@ -1 +1 @@
 #ifndef EPHEMERIS_H
 #define EPHEMERIS_H
+
+#include <newmat.h>
 
 #include <stdio.h>
@@ -19 +21 @@
 
   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 +51 @@
  public:
   t_ephGPS() { }
-  ~t_ephGPS() {}
+  virtual ~t_ephGPS() {}
   double TOC() const {return _TOC;}
 
@@ -55 +57 @@
 
   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<int>(_IODC); }
+  virtual int  IOD() const { return static_cast<int>(_IODC); }
 
-  void print(std::ostream& out) const;
+  virtual void print(std::ostream& out) const;
 
  private:
@@ -109 +111 @@
 };
 
+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

trunk/BNC/bncpppclient.cpp

@@ -158 +158 @@
   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;

trunk/BNC/bncutils.cpp

@@ -243 +243 @@
            + 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;
+}
+

trunk/BNC/bncutils.h

@@ -51 +51 @@
 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