Changeset 884 in ntrip


Ignore:
Timestamp:
May 8, 2008, 3:36:12 PM (17 years ago)
Author:
mervart
Message:

* empty log message *

Location:
trunk/BNS
Files:
2 deleted
6 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNS/bns.cpp

    r874 r884  
    3939  _bnseph = new t_bnseph(parent);
    4040
    41   connect(_bnseph, SIGNAL(newEph(gpsEph*)), this, SLOT(slotNewEph(gpsEph*)));
     41  connect(_bnseph, SIGNAL(newEph(t_eph*)), this, SLOT(slotNewEph(t_eph*)));
    4242  connect(_bnseph, SIGNAL(newMessage(QByteArray)),
    4343          this, SLOT(slotMessage(const QByteArray)));
    … …  
    229229//
    230230////////////////////////////////////////////////////////////////////////////
    231 void t_bns::slotNewEph(gpsEph* ep) {
     231void t_bns::slotNewEph(t_eph* ep) {
    232232
    233233  QMutexLocker locker(&_mutex);
    234234
    235235  t_ephPair* pair;
    236   if ( !_ephList.contains(ep->prn) ) {
     236  if ( !_ephList.contains(ep->prn()) ) {
    237237    pair = new t_ephPair();
    238     _ephList.insert(ep->prn, pair);
    239   }
    240   else {
    241     pair = _ephList[ep->prn];
     238    _ephList.insert(ep->prn(), pair);
     239  }
     240  else {
     241    pair = _ephList[ep->prn()];
    242242  }
    243243
    … …  
    246246  }
    247247  else {
    248     if (ep->GPSweek >  pair->eph->GPSweek ||
    249         (ep->GPSweek == pair->eph->GPSweek && ep->TOC > pair->eph->TOC)) {
     248    if (ep->isNewerThan(pair->eph)) {
    250249      delete pair->oldEph;
    251250      pair->oldEph = pair->eph;
    … …  
    328327        QString      prn;
    329328        ColumnVector xx(4);
    330         gpsEph*      ep = 0;
     329        t_eph*       ep = 0;
    331330
    332331        if (oldEph == 0) {
    … …  
    344343          if ( _ephList.contains(prn) ) {
    345344            t_ephPair* pair = _ephList[prn];
    346             prn = pair->eph->prn;
     345            prn = pair->eph->prn();
    347346            xx  = pair->xx;
    348347            ep  = pair->oldEph;
    … …  
    379378//
    380379////////////////////////////////////////////////////////////////////////////
    381 void t_bns::processSatellite(gpsEph* ep, int GPSweek, double GPSweeks,
     380void t_bns::processSatellite(t_eph* ep, int GPSweek, double GPSweeks,
    382381                             const QString& prn, const ColumnVector& xx,
    383382                             struct ClockOrbit::SatData* sd) {
    … …  
    386385  ColumnVector vv(3);
    387386
    388   satellitePosition(GPSweek, GPSweeks, ep, xB(1), xB(2), xB(3), xB(4),
    389                     vv(1), vv(2), vv(3));
     387  ep->position(GPSweek, GPSweeks, xB, vv);
    390388
    391389  ColumnVector dx   = xx.Rows(1,3) - xB.Rows(1,3);
    … …  
    397395  if (sd) {
    398396    sd->ID                    = prn.mid(1).toInt();
    399     sd->IOD                   = int(ep->IODE);
     397    sd->IOD                   = ep->IOD();
    400398    sd->Clock.DeltaA0         = dClk;
    401399    sd->Orbit.DeltaRadial     = rsw(1);
    … …  
    406404  if (_outStream) {
    407405    QString line;
    408     line.sprintf("%d %.1f %s   %3d %3d   %8.3f   %8.3f %8.3f %8.3f\n",
    409                  GPSweek, GPSweeks, ep->prn.toAscii().data(),
    410                  int(ep->IODC), int(ep->IODE), dClk, rsw(1), rsw(2), rsw(3));
    411      *_outStream << line;
     406    line.sprintf("%d %.1f %s   %3d   %8.3f   %8.3f %8.3f %8.3f\n",
     407                 GPSweek, GPSweeks, ep->prn().toAscii().data(),
     408                 ep->IOD(), dClk, rsw(1), rsw(2), rsw(3));
     409    *_outStream << line;
    412410    _outStream->flush();
    413411  }

  • trunk/BNS/bns.h

    r874 r884  
    2828
    2929  ColumnVector xx;
    30   gpsEph* eph;
    31   gpsEph* oldEph;
     30  t_eph* eph;
     31  t_eph* oldEph;
    3232};
    3333
    … …  
    4545 
    4646 private slots:
    47   void slotNewEph(gpsEph* ep);
     47  void slotNewEph(t_eph* ep);
    4848  void slotNewConnection();
    4949  void slotMessage(const QByteArray msg);
    … …  
    5555  void openCaster();
    5656  void readEpoch();
    57   void processSatellite(gpsEph* ep, int GPSweek, double GPSweeks,
     57  void processSatellite(t_eph* ep, int GPSweek, double GPSweeks,
    5858                        const QString& prn, const ColumnVector& xx,
    5959                        struct ClockOrbit::SatData* sd);

  • trunk/BNS/bnseph.cpp

    r839 r884  
    8585void t_bnseph::readEph() {
    8686
    87   gpsEph* ep = new gpsEph;
    88 
    89   bool flagGlonass = false;
    90 
    91   const int NUMLINES = 8;
    92 
    93   for (int ii = 1; ii <= NUMLINES; ii++) {
    94 
     87
     88  t_eph* eph = 0;
     89
     90  QByteArray  line = _socket->readLine();
     91  QTextStream in(line);
     92  QString     prn;
     93   
     94  in >> prn;
     95
     96  int numlines = 0;   
     97  if (prn.indexOf('R') != -1) {
     98    eph = new t_ephGlo();
     99    numlines = 4;
     100  }
     101  else {
     102    eph = new t_ephGPS();
     103    numlines = 8;
     104  }
     105
     106  QStringList lines;
     107  lines << line;
     108
     109  for (int ii = 2; ii <= numlines; ii++) {
    95110    QByteArray line = _socket->readLine();
    96 
    97     if (flagGlonass) {
    98       if (ii == 4) {
    99         delete ep;
    100         return;
    101       }
    102       else {
    103         continue;
    104       }
    105     }
    106 
    107     QTextStream in(line);
     111    lines << line;
     112  }
     113
     114  eph->read(lines);
     115
     116  emit(newEph(eph));
     117}
     118
     119// Read GPS Ephemeris
     120////////////////////////////////////////////////////////////////////////////
     121void t_ephGPS::read(const QStringList& lines) {
     122
     123  for (int ii = 1; ii <= lines.size(); ii++) {
     124    QTextStream in(lines.at(ii-1).toAscii());
    108125
    109126    if (ii == 1) {
    110       in >> ep->prn;
    111 
    112       if (ep->prn.indexOf('R') != -1) {
    113         flagGlonass = true;
    114         continue;
    115       }
    116 
    117127      double  year, month, day, hour, minute, second;
    118       in >> year >> month >> day >> hour >> minute >> second
    119          >> ep->clock_bias >> ep->clock_drift >> ep->clock_driftrate;
     128      in >> _prn >> year >> month >> day >> hour >> minute >> second
     129         >> _clock_bias >> _clock_drift >> _clock_driftrate;
    120130     
    121131      if (year < 100) year += 2000;
    … …  
    124134                         QTime(int(hour), int(minute), int(second)), Qt::UTC);
    125135      int week;
    126       GPSweekFromDateAndTime(dateTime, week, ep->TOC);
    127       ep->GPSweek = week;
     136      GPSweekFromDateAndTime(dateTime, week, _TOC);
     137      _GPSweek = week;
    128138    }
    129139    else if (ii == 2) {
    130       in >> ep->IODE >> ep->Crs >> ep->Delta_n >> ep->M0;
     140      in >> _IODE >> _Crs >> _Delta_n >> _M0;
    131141    } 
    132142    else if (ii == 3) {
    133       in >> ep->Cuc >> ep->e >> ep->Cus >> ep->sqrt_A;
     143      in >> _Cuc >> _e >> _Cus >> _sqrt_A;
    134144    }
    135145    else if (ii == 4) {
    136       in >> ep->TOE >> ep->Cic >> ep->OMEGA0 >> ep->Cis;
     146      in >> _TOE >> _Cic >> _OMEGA0 >> _Cis;
    137147    } 
    138148    else if (ii == 5) {
    139       in >> ep->i0 >> ep->Crc >> ep->omega >> ep->OMEGADOT;
     149      in >> _i0 >> _Crc >> _omega >> _OMEGADOT;
    140150    }
    141151    else if (ii == 6) {
    142       in >>  ep->IDOT;
     152      in >>  _IDOT;
    143153    }
    144154    else if (ii == 7) {
    145155      double hlp, health;
    146       in >> hlp >> health >> ep->TGD >> ep->IODC;
     156      in >> hlp >> health >> _TGD >> _IODC;
    147157    }
    148158    else if (ii == 8) {
    149       in >> ep->TOW;
    150     }
    151   }
    152 
    153   emit(newEph(ep));
    154 }
     159      in >> _TOW;
     160    }
     161  }
     162}
     163
     164// Compute GPS Satellite Position
     165////////////////////////////////////////////////////////////////////////////
     166void t_ephGPS::position(int GPSweek, double GPSweeks, ColumnVector& xc,
     167                        ColumnVector& vv) const {
     168
     169  const static double secPerWeek = 7 * 86400.0;
     170  const static double omegaEarth = 7292115.1467e-11;
     171  const static double gmWGS      = 398.6005e12;
     172
     173  if (xc.Nrows() < 4) {
     174    xc.ReSize(4);
     175  }
     176  xc = 0.0;
     177
     178  if (vv.Nrows() < 3) {
     179    vv.ReSize(3);
     180  }
     181  vv = 0.0;
     182
     183  double a0 = _sqrt_A * _sqrt_A;
     184  if (a0 == 0) {
     185    return;
     186  }
     187
     188  double n0 = sqrt(gmWGS/(a0*a0*a0));
     189  double tk = GPSweeks - _TOE;
     190  if (GPSweek != _GPSweek) { 
     191    tk += (GPSweek - _GPSweek) * secPerWeek;
     192  }
     193  double n  = n0 + _Delta_n;
     194  double M  = _M0 + n*tk;
     195  double E  = M;
     196  double E_last;
     197  do {
     198    E_last = E;
     199    E = M + _e*sin(E);
     200  } while ( fabs(E-E_last)*a0 > 0.001 );
     201  double v      = 2.0*atan( sqrt( (1.0 + _e)/(1.0 - _e) )*tan( E/2 ) );
     202  double u0     = v + _omega;
     203  double sin2u0 = sin(2*u0);
     204  double cos2u0 = cos(2*u0);
     205  double r      = a0*(1 - _e*cos(E)) + _Crc*cos2u0 + _Crs*sin2u0;
     206  double i      = _i0 + _IDOT*tk + _Cic*cos2u0 + _Cis*sin2u0;
     207  double u      = u0 + _Cuc*cos2u0 + _Cus*sin2u0;
     208  double xp     = r*cos(u);
     209  double yp     = r*sin(u);
     210  double OM     = _OMEGA0 + (_OMEGADOT - omegaEarth)*tk -
     211                   omegaEarth*_TOE;
     212 
     213  double sinom = sin(OM);
     214  double cosom = cos(OM);
     215  double sini  = sin(i);
     216  double cosi  = cos(i);
     217  xc(1) = xp*cosom - yp*cosi*sinom;
     218  xc(2) = xp*sinom + yp*cosi*cosom;
     219  xc(3) = yp*sini;                 
     220 
     221  double tc = GPSweeks - _TOC;
     222  if (GPSweek != _GPSweek) { 
     223    tc += (GPSweek - _GPSweek) * secPerWeek;
     224  }
     225  xc(4) = _clock_bias + _clock_drift*tc + _clock_driftrate*tc*tc
     226          - 4.442807633e-10 * _e * sqrt(a0) *sin(E);
     227
     228  // Velocity
     229  // --------
     230  double tanv2 = tan(v/2);
     231  double dEdM  = 1 / (1 - _e*cos(E));
     232  double dotv  = sqrt((1.0 + _e)/(1.0 - _e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
     233               * dEdM * n;
     234  double dotu  = dotv + (-_Cuc*sin2u0 + _Cus*cos2u0)*2*dotv;
     235  double dotom = _OMEGADOT - omegaEarth;
     236  double doti  = _IDOT + (-_Cic*sin2u0 + _Cis*cos2u0)*2*dotv;
     237  double dotr  = a0 * _e*sin(E) * dEdM * n
     238                + (-_Crc*sin2u0 + _Crs*cos2u0)*2*dotv;
     239  double dotx  = dotr*cos(u) - r*sin(u)*dotu;
     240  double doty  = dotr*sin(u) + r*cos(u)*dotu;
     241
     242  vv(1)  = cosom   *dotx  - cosi*sinom   *doty      // dX / dr
     243           - xp*sinom*dotom - yp*cosi*cosom*dotom   // dX / dOMEGA
     244                       + yp*sini*sinom*doti;        // dX / di
     245
     246  vv(2)  = sinom   *dotx  + cosi*cosom   *doty
     247           + xp*cosom*dotom - yp*cosi*sinom*dotom
     248                          - yp*sini*cosom*doti;
     249
     250  vv(3)  = sini    *doty  + yp*cosi      *doti;
     251}
     252
     253// Compare Time
     254////////////////////////////////////////////////////////////////////////////
     255bool t_ephGPS::isNewerThan(const t_eph* ep) const {
     256
     257  const t_ephGPS* eph = dynamic_cast<const t_ephGPS*>(ep);
     258  if (!eph) {
     259    return false;
     260  }
     261
     262  if (_GPSweek >  eph->_GPSweek ||
     263      (_GPSweek == eph->_GPSweek && _TOC > eph->_TOC)) {
     264    return true;
     265  }
     266  else {
     267    return false;
     268  }
     269}
     270
     271//
     272////////////////////////////////////////////////////////////////////////////
     273void t_ephGlo::read(const QStringList& lines) {
     274
     275  for (int ii = 1; ii <= lines.size(); ii++) {
     276    QTextStream in(lines.at(ii-1).toAscii());
     277
     278    if (ii == 1) {
     279      double  year, month, day, hour, minute, second;
     280      in >> _prn >> year >> month >> day >> hour >> minute >> second
     281         >> _tau >> _gamma;
     282     
     283      if (year < 100) year += 2000;
     284     
     285      QDateTime dateTime(QDate(int(year), int(month), int(day)),
     286                         QTime(int(hour), int(minute), int(second)), Qt::UTC);
     287      int week;
     288      GPSweekFromDateAndTime(dateTime, week, _GPSTOW);
     289      _GPSweek = week;
     290    }
     291    else if (ii == 2) {
     292      in >>_x_pos >> _x_velocity >> _x_acceleration >> _flags;
     293    }
     294    else if (ii == 3) {
     295      in >>_y_pos >> _y_velocity >> _y_acceleration >> _frequency_number;
     296    }
     297    else if (ii == 4) {
     298      in >>_z_pos >> _z_velocity >> _z_acceleration >> _E;
     299    }
     300  }
     301}
     302
     303//
     304////////////////////////////////////////////////////////////////////////////
     305bool t_ephGlo::isNewerThan(const t_eph* ep) const {
     306  return false;
     307}
     308
     309//
     310////////////////////////////////////////////////////////////////////////////
     311int  t_ephGlo::IOD() const {
     312  return 0;
     313}
     314
     315// Derivative of the state vector using a simple force model (static)
     316////////////////////////////////////////////////////////////////////////////
     317ColumnVector t_ephGlo::glo_deriv(double /* tt */, const ColumnVector& xv) {
     318
     319  // State vector components
     320  // -----------------------
     321  ColumnVector rr = xv.rows(1,3);
     322  ColumnVector vv = xv.rows(4,6);
     323
     324  // Acceleration
     325  // ------------
     326  const static double GM    = 398.60044e12;
     327  const static double AE    = 6378136.0;
     328  const static double OMEGA = 7292115.e-11;
     329  const static double C20   = -1082.63e-6;
     330
     331  double rho = rr.norm_Frobenius();
     332  double t1  = -GM/(rho*rho*rho);
     333  double t2  = 3.0/2.0 * C20 * (GM*AE*AE) / (rho*rho*rho*rho*rho);
     334  double t3  = OMEGA * OMEGA;
     335  double t4  = 2.0 * OMEGA;
     336  double z2  = rr(3) * rr(3);
     337
     338  // Vector of derivatives
     339  // ---------------------
     340  ColumnVector va(6);
     341  va(1) = vv(1);
     342  va(2) = vv(2);
     343  va(3) = vv(3);
     344  va(4) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(1) + t4*vv(2);
     345  va(5) = (t1 + t2*(1.0-5.0*z2/(rho*rho)) + t3) * rr(2) - t4*vv(1);
     346  va(6) = (t1 + t2*(3.0-5.0*z2/(rho*rho))     ) * rr(3);
     347
     348  return va;
     349}
     350
     351//
     352////////////////////////////////////////////////////////////////////////////
     353void t_ephGlo::position(int GPSweek, double GPSweeks, ColumnVector& xc,
     354                        ColumnVector& vv) const {
     355
     356}
     357

  • trunk/BNS/bnseph.h

    r838 r884  
    22#define BNSEPH_H
    33
     4#include <newmat.h>
     5
     6#include <QtCore>
    47#include <QThread>
    58#include <QtNetwork>
    69
    7 class gpsEph {
     10
     11class t_eph {
    812 public:
    9   QString prn;
    10   double GPSweek;         
    11   double TOW;              //  [s]   
    12   double TOC;              //  [s]   
    13   double TOE;              //  [s]   
    14   double IODE;             
    15   double IODC;             
     13  virtual ~t_eph() {};
     14  virtual void position(int GPSweek, double GPSweeks, ColumnVector& xc,
     15                        ColumnVector& vv) const = 0;
     16  virtual void read(const QStringList& lines) = 0;
     17  virtual bool isNewerThan(const t_eph* ep) const = 0;
     18  virtual int  IOD() const = 0;
     19  QString prn() const {return _prn;}
     20 protected: 
     21  QString _prn;
     22};
    1623
    17   double clock_bias;       //  [s]   
    18   double clock_drift;      //  [s/s] 
    19   double clock_driftrate;  //  [s/s^2]
     24class t_ephGlo : public t_eph {
     25 public:
     26  t_ephGlo() {
     27    _xv.ReSize(6);
     28  };
     29  virtual ~t_ephGlo() {};
     30  virtual void read(const QStringList& lines);
     31  virtual void position(int GPSweek, double GPSweeks, ColumnVector& xc,
     32                        ColumnVector& vv) const;
     33  virtual bool isNewerThan(const t_eph* ep) const;
     34  virtual int  IOD() const;
     35 private:
     36  static ColumnVector glo_deriv(double /* tt */, const ColumnVector& xv);
     37  ColumnVector _xv;
    2038
    21   double Crs;              //  [m]   
    22   double Delta_n;          //  [rad/s]
    23   double M0;               //  [rad] 
    24   double Cuc;              //  [rad] 
    25   double e;                //         
    26   double Cus;              //  [rad] 
    27   double sqrt_A;           //  [m^0.5]
    28   double Cic;              //  [rad] 
    29   double OMEGA0;           //  [rad] 
    30   double Cis;              //  [rad] 
    31   double i0;               //  [rad] 
    32   double Crc;              //  [m]   
    33   double omega;            //  [rad] 
    34   double OMEGADOT;         //  [rad/s]
    35   double IDOT;             //  [rad/s]
     39  double _GPSweek;
     40  double _GPSTOW;
     41  double _E;                  /* [days]   */
     42  double _tau;                /* [s]      */
     43  double _gamma;              /*          */
     44  double _x_pos;              /* [km]     */
     45  double _x_velocity;         /* [km/s]   */
     46  double _x_acceleration;     /* [km/s^2] */
     47  double _y_pos;              /* [km]     */
     48  double _y_velocity;         /* [km/s]   */
     49  double _y_acceleration;     /* [km/s^2] */
     50  double _z_pos;              /* [km]     */
     51  double _z_velocity;         /* [km/s]   */
     52  double _z_acceleration;     /* [km/s^2] */
     53  int    _flags;              /* GLOEPHF_xxx */
     54  int    _frequency_number;   /* ICD-GLONASS data position */
     55};
    3656
    37   double TGD;              //  [s]   
     57
     58class t_ephGPS : public t_eph {
     59 public:
     60  t_ephGPS() {};
     61  virtual ~t_ephGPS() {};
     62  virtual void read(const QStringList& lines);
     63  virtual void position(int GPSweek, double GPSweeks, ColumnVector& xc,
     64                        ColumnVector& vv) const;
     65  virtual bool isNewerThan(const t_eph* ep) const;
     66  virtual int  IOD() const {return int(_IODE);}
     67
     68 private:
     69  double  _GPSweek;         
     70  double  _TOW;              //  [s]   
     71  double  _TOC;              //  [s]   
     72  double  _TOE;              //  [s]   
     73  double  _IODE;             
     74  double  _IODC;             
     75
     76  double  _clock_bias;       //  [s]   
     77  double  _clock_drift;      //  [s/s] 
     78  double  _clock_driftrate;  //  [s/s^2]
     79
     80  double  _Crs;              //  [m]   
     81  double  _Delta_n;          //  [rad/s]
     82  double  _M0;               //  [rad] 
     83  double  _Cuc;              //  [rad] 
     84  double  _e;                //         
     85  double  _Cus;              //  [rad] 
     86  double  _sqrt_A;           //  [m^0.5]
     87  double  _Cic;              //  [rad] 
     88  double  _OMEGA0;           //  [rad] 
     89  double  _Cis;              //  [rad] 
     90  double  _i0;               //  [rad] 
     91  double  _Crc;              //  [m]   
     92  double  _omega;            //  [rad] 
     93  double  _OMEGADOT;         //  [rad/s]
     94  double  _IDOT;             //  [rad/s]
     95
     96  double  _TGD;              //  [s]   
    3897};
    3998
    … …  
    46105
    47106 signals:
    48   void newEph(gpsEph* eph);
     107  void newEph(t_eph* eph);
    49108  void newMessage(const QByteArray msg);
    50109  void error(const QByteArray msg);

  • trunk/BNS/bnsutils.cpp

    r861 r884  
    108108}
    109109
    110 // Satellite Position computed using broadcast ephemeris
    111 ////////////////////////////////////////////////////////////////////////////
    112 void satellitePosition(int GPSweek, double GPSweeks, const gpsEph* ep,
    113                        double& X, double& Y, double& Z, double& dt,
    114                        double& vX, double& vY, double& vZ) {
    115 
    116   const static double secPerWeek = 7 * 86400.0;
    117   const static double omegaEarth = 7292115.1467e-11;
    118   const static double gmWGS      = 398.6005e12;
    119 
    120   X = Y = Z = dt = 0.0;
    121 
    122   double a0 = ep->sqrt_A * ep->sqrt_A;
    123   if (a0 == 0) {
    124     return;
    125   }
    126 
    127   double n0 = sqrt(gmWGS/(a0*a0*a0));
    128   double tk = GPSweeks - ep->TOE;
    129   if (GPSweek != ep->GPSweek) { 
    130     tk += (GPSweek - ep->GPSweek) * secPerWeek;
    131   }
    132   double n  = n0 + ep->Delta_n;
    133   double M  = ep->M0 + n*tk;
    134   double E  = M;
    135   double E_last;
    136   do {
    137     E_last = E;
    138     E = M + ep->e*sin(E);
    139   } while ( fabs(E-E_last)*a0 > 0.001 );
    140   double v      = 2.0*atan( sqrt( (1.0 + ep->e)/(1.0 - ep->e) )*tan( E/2 ) );
    141   double u0     = v + ep->omega;
    142   double sin2u0 = sin(2*u0);
    143   double cos2u0 = cos(2*u0);
    144   double r      = a0*(1 - ep->e*cos(E)) + ep->Crc*cos2u0 + ep->Crs*sin2u0;
    145   double i      = ep->i0 + ep->IDOT*tk + ep->Cic*cos2u0 + ep->Cis*sin2u0;
    146   double u      = u0 + ep->Cuc*cos2u0 + ep->Cus*sin2u0;
    147   double xp     = r*cos(u);
    148   double yp     = r*sin(u);
    149   double OM     = ep->OMEGA0 + (ep->OMEGADOT - omegaEarth)*tk -
    150                    omegaEarth*ep->TOE;
    151  
    152   double sinom = sin(OM);
    153   double cosom = cos(OM);
    154   double sini  = sin(i);
    155   double cosi  = cos(i);
    156   X = xp*cosom - yp*cosi*sinom;
    157   Y = xp*sinom + yp*cosi*cosom;
    158   Z = yp*sini;                 
    159  
    160   double tc = GPSweeks - ep->TOC;
    161   if (GPSweek != ep->GPSweek) { 
    162     tc += (GPSweek - ep->GPSweek) * secPerWeek;
    163   }
    164   dt = ep->clock_bias + ep->clock_drift*tc + ep->clock_driftrate*tc*tc
    165        - 4.442807633e-10 * ep->e * sqrt(a0) *sin(E);
    166 
    167   // Velocity
    168   // --------
    169   double tanv2 = tan(v/2);
    170   double dEdM  = 1 / (1 - ep->e*cos(E));
    171   double dotv  = sqrt((1.0 + ep->e)/(1.0 - ep->e)) / cos(E/2)/cos(E/2) / (1 + tanv2*tanv2)
    172                * dEdM * n;
    173   double dotu  = dotv + (-ep->Cuc*sin2u0 + ep->Cus*cos2u0)*2*dotv;
    174   double dotom = ep->OMEGADOT - omegaEarth;
    175   double doti  = ep->IDOT + (-ep->Cic*sin2u0 + ep->Cis*cos2u0)*2*dotv;
    176   double dotr  = a0 * ep->e*sin(E) * dEdM * n
    177                 + (-ep->Crc*sin2u0 + ep->Crs*cos2u0)*2*dotv;
    178   double dotx  = dotr*cos(u) - r*sin(u)*dotu;
    179   double doty  = dotr*sin(u) + r*cos(u)*dotu;
    180 
    181   vX  = cosom   *dotx  - cosi*sinom   *doty      // dX / dr
    182       - xp*sinom*dotom - yp*cosi*cosom*dotom     // dX / dOMEGA
    183                        + yp*sini*sinom*doti;     // dX / di
    184 
    185   vY  = sinom   *dotx  + cosi*cosom   *doty
    186       + xp*cosom*dotom - yp*cosi*sinom*dotom
    187                        - yp*sini*cosom*doti;
    188 
    189   vZ  = sini    *doty  + yp*cosi      *doti;
    190 }
    191 
    192110// Transformation xyz --> radial, along track, out-of-plane
    193111////////////////////////////////////////////////////////////////////////////
    … …  
    202120  rsw(3) = DotProduct(xyz, cross) / cross.norm_Frobenius();
    203121}
     122
     123// Fourth order Runge-Kutta numerical integrator for ODEs
     124////////////////////////////////////////////////////////////////////////////
     125ColumnVector rungeKutta4(
     126  double xi,              // the initial x-value
     127  const ColumnVector& yi, // vector of the initial y-values
     128  double dx,              // the step size for the integration
     129  ColumnVector (*der)(double x, const ColumnVector& y)
     130                          // A pointer to a function that computes the
     131                          // derivative of a function at a point (x,y)
     132                         ) {
     133
     134  ColumnVector k1 = der(xi       , yi       ) * dx;
     135  ColumnVector k2 = der(xi+dx/2.0, yi+k1/2.0) * dx;
     136  ColumnVector k3 = der(xi+dx/2.0, yi+k2/2.0) * dx;
     137  ColumnVector k4 = der(xi+dx    , yi+k3    ) * dx;
     138
     139  ColumnVector yf = yi + k1/6.0 + k2/3.0 + k3/3.0 + k4/6.0;
     140 
     141  return yf;
     142}

  • trunk/BNS/bnsutils.h

    r860 r884  
    2323void currentGPSWeeks(int& week, double& sec);
    2424
    25 void satellitePosition(int GPSweek, double GPSweeks, const gpsEph* ep,
    26                        double& X, double& Y, double& Z, double&,
    27                        double& vX, double& vY, double& vZ);
    28 
    2925void XYZ_to_RSW(const ColumnVector& rr, const ColumnVector& vv,
    3026                const ColumnVector& xyz, ColumnVector& rsw);
     27
     28ColumnVector rungeKutta4(double xi, const ColumnVector& yi, double dx,
     29                         ColumnVector (*der)(double x, const ColumnVector& y));
     30
    3131#endif
Note: See TracChangeset for help on using the changeset viewer.