Changeset 884 in ntrip for trunk/BNS/bnseph.cpp

Timestamp:

May 8, 2008, 3:36:12 PM (16 years ago)

Author:

mervart

Message:

* empty log message *

File:

• trunk/BNS/bnseph.cpp (modified) (2 diffs)

trunk/BNS/bnseph.cpp

@@ -85 +85 @@
 void t_bnseph::readEph() {
 
-  gpsEph* ep = new gpsEph;
-
-  bool flagGlonass = false;
-
-  const int NUMLINES = 8;
-
-  for (int ii = 1; ii <= NUMLINES; ii++) {
-
+
+  t_eph* eph = 0;
+
+  QByteArray  line = _socket->readLine();
+  QTextStream in(line);
+  QString     prn;
+   
+  in >> prn;
+
+  int numlines = 0;    
+  if (prn.indexOf('R') != -1) {
+    eph = new t_ephGlo();
+    numlines = 4;
+  }
+  else {
+    eph = new t_ephGPS();
+    numlines = 8;
+  }
+
+  QStringList lines;
+  lines << line;
+
+  for (int ii = 2; ii <= numlines; ii++) {
     QByteArray line = _socket->readLine();
-
-    if (flagGlonass) {
-      if (ii == 4) {
-        delete ep;
-        return;
-      }
-      else {
-        continue;
-      }
-    }
-
-    QTextStream in(line);
+    lines << line;
+  }
+
+  eph->read(lines);
+
+  emit(newEph(eph));
+}
+
+// Read GPS Ephemeris
+////////////////////////////////////////////////////////////////////////////
+void t_ephGPS::read(const QStringList& lines) {
+
+  for (int ii = 1; ii <= lines.size(); ii++) {
+    QTextStream in(lines.at(ii-1).toAscii());
 
     if (ii == 1) {
-      in >> ep->prn;
-
-      if (ep->prn.indexOf('R') != -1) {
-        flagGlonass = true;
-        continue;
-      }
-
       double  year, month, day, hour, minute, second;
-      in >> year >> month >> day >> hour >> minute >> second
-         >> ep->clock_bias >> ep->clock_drift >> ep->clock_driftrate;
+      in >> _prn >> year >> month >> day >> hour >> minute >> second
+         >> _clock_bias >> _clock_drift >> _clock_driftrate;
       
       if (year < 100) year += 2000;
@@ -124 +134 @@
                          QTime(int(hour), int(minute), int(second)), Qt::UTC);
       int week;
-      GPSweekFromDateAndTime(dateTime, week, ep->TOC); 
-      ep->GPSweek = week;
+      GPSweekFromDateAndTime(dateTime, week, _TOC); 
+      _GPSweek = week;
     }
     else if (ii == 2) {
-      in >> ep->IODE >> ep->Crs >> ep->Delta_n >> ep->M0;
+      in >> _IODE >> _Crs >> _Delta_n >> _M0;
     }  
     else if (ii == 3) {
-      in >> ep->Cuc >> ep->e >> ep->Cus >> ep->sqrt_A;
+      in >> _Cuc >> _e >> _Cus >> _sqrt_A;
     }
     else if (ii == 4) {
-      in >> ep->TOE >> ep->Cic >> ep->OMEGA0 >> ep->Cis;
+      in >> _TOE >> _Cic >> _OMEGA0 >> _Cis;
     }  
     else if (ii == 5) {
-      in >> ep->i0 >> ep->Crc >> ep->omega >> ep->OMEGADOT;
+      in >> _i0 >> _Crc >> _omega >> _OMEGADOT;
     }
     else if (ii == 6) {
-      in >>  ep->IDOT;
+      in >>  _IDOT;
     }
     else if (ii == 7) {
       double hlp, health;
-      in >> hlp >> health >> ep->TGD >> ep->IODC;
+      in >> hlp >> health >> _TGD >> _IODC;
     }
     else if (ii == 8) {
-      in >> ep->TOW;
-    }
-  }
-
-  emit(newEph(ep));
-}
+      in >> _TOW;
+    }
+  }
+}
+
+// Compute GPS Satellite Position
+////////////////////////////////////////////////////////////////////////////
+void t_ephGPS::position(int GPSweek, double GPSweeks, ColumnVector& xc,
+                        ColumnVector& vv) const {
+
+  const static double secPerWeek = 7 * 86400.0;
+  const static double omegaEarth = 7292115.1467e-11;
+  const static double gmWGS      = 398.6005e12;
+
+  if (xc.Nrows() < 4) {
+    xc.ReSize(4);
+  }
+  xc = 0.0;
+
+  if (vv.Nrows() < 3) {
+    vv.ReSize(3);
+  }
+  vv = 0.0;
+
+  double a0 = _sqrt_A * _sqrt_A;
+  if (a0 == 0) {
+    return;
+  }
+
+  double n0 = sqrt(gmWGS/(a0*a0*a0));
+  double tk = GPSweeks - _TOE;
+  if (GPSweek != _GPSweek) {  
+    tk += (GPSweek - _GPSweek) * secPerWeek;
+  }
+  double n  = n0 + _Delta_n;
+  double M  = _M0 + n*tk;
+  double E  = M;
+  double E_last;
+  do {
+    E_last = E;
+    E = M + _e*sin(E);
+  } while ( fabs(E-E_last)*a0 > 0.001 );
+  double v      = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
+  double u0     = v + _omega;
+  double sin2u0 = sin(2*u0);
+  double cos2u0 = cos(2*u0);
+  double r      = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
+  double i      = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
+  double u      = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
+  double xp     = r*cos(u);
+  double yp     = r*sin(u);
+  double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk - 
+                   omegaEarth*_TOE;
+  
+  double sinom = sin(OM);
+  double cosom = cos(OM);
+  double sini  = sin(i);
+  double cosi  = cos(i);
+  xc(1) = xp*cosom - yp*cosi*sinom;
+  xc(2) = xp*sinom + yp*cosi*cosom;
+  xc(3) = yp*sini;                 
+  
+  double tc = GPSweeks - _TOC;
+  if (GPSweek != _GPSweek) {  
+    tc += (GPSweek - _GPSweek) * secPerWeek;
+  }
+  xc(4) = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc 
+          - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
+
+  // Velocity
+  // --------
+  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) 
+               * 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 
+                + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
+  double dotx  = dotr*cos(u) - r*sin(u)*dotu;
+  double doty  = dotr*sin(u) + r*cos(u)*dotu;
+
+  vv(1)  = cosom   *dotx  - cosi*sinom   *doty      // dX / dr
+           - xp*sinom*dotom - yp*cosi*cosom*dotom   // dX / dOMEGA
+                       + yp*sini*sinom*doti;        // dX / di
+
+  vv(2)  = sinom   *dotx  + cosi*cosom   *doty
+           + xp*cosom*dotom - yp*cosi*sinom*dotom
+                          - yp*sini*cosom*doti;
+
+  vv(3)  = sini    *doty  + yp*cosi      *doti;
+}
+
+// Compare Time
+////////////////////////////////////////////////////////////////////////////
+bool t_ephGPS::isNewerThan(const t_eph* ep) const {
+
+  const t_ephGPS* eph = dynamic_cast<const t_ephGPS*>(ep);
+  if (!eph) {
+    return false;
+  } 
+
+  if (_GPSweek >  eph->_GPSweek ||
+      (_GPSweek == eph->_GPSweek && _TOC > eph->_TOC)) {
+    return true;
+  }
+  else {
+    return false;
+  }
+}
+
+// 
+////////////////////////////////////////////////////////////////////////////
+void t_ephGlo::read(const QStringList& lines) {
+
+  for (int ii = 1; ii <= lines.size(); ii++) {
+    QTextStream in(lines.at(ii-1).toAscii());
+
+    if (ii == 1) {
+      double  year, month, day, hour, minute, second;
+      in >> _prn >> year >> month >> day >> hour >> minute >> second
+         >> _tau >> _gamma;
+      
+      if (year < 100) year += 2000;
+      
+      QDateTime dateTime(QDate(int(year), int(month), int(day)), 
+                         QTime(int(hour), int(minute), int(second)), Qt::UTC);
+      int week;
+      GPSweekFromDateAndTime(dateTime, week, _GPSTOW); 
+      _GPSweek = week;
+    }
+    else if (ii == 2) {
+      in >>_x_pos >> _x_velocity >> _x_acceleration >> _flags;
+    }
+    else if (ii == 3) {
+      in >>_y_pos >> _y_velocity >> _y_acceleration >> _frequency_number;
+    }
+    else if (ii == 4) {
+      in >>_z_pos >> _z_velocity >> _z_acceleration >> _E;
+    }
+  }
+}
+
+// 
+////////////////////////////////////////////////////////////////////////////
+bool t_ephGlo::isNewerThan(const t_eph* ep) const {
+  return false;
+}
+
+// 
+////////////////////////////////////////////////////////////////////////////
+int  t_ephGlo::IOD() const {
+  return 0;
+}
+
+// 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 
+  // ------------
+  const static double GM    = 398.60044e12;
+  const static double AE    = 6378136.0;
+  const static double OMEGA = 7292115.e-11;
+  const static 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;
+}
+
+// 
+////////////////////////////////////////////////////////////////////////////
+void t_ephGlo::position(int GPSweek, double GPSweeks, ColumnVector& xc,
+                        ColumnVector& vv) const {
+
+}
+