Index: trunk/BNS/bnseph.cpp =================================================================== --- trunk/BNS/bnseph.cpp (revision 839) +++ trunk/BNS/bnseph.cpp (revision 884) @@ -85,37 +85,47 @@ 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,31 +134,224 @@ 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(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 { + +} +