AlmOrbit.cpp

Go to the documentation of this file.

//==============================================================================
//
//  This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
//
//  The GNSSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 3.0 of the License, or
//  any later version.
//
//  The GNSSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GNSSTk; if not, write to the Free Software Foundation,
//  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin.
//  Copyright 2004-2022, The Board of Regents of The University of Texas System
//
//==============================================================================
 
//==============================================================================
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin, under contract to an agency or agencies
//  within the U.S. Department of Defense. The U.S. Government retains all
//  rights to use, duplicate, distribute, disclose, or release this software.
//
//  Pursuant to DoD Directive 523024
//
//  DISTRIBUTION STATEMENT A: This software has been approved for public
//                            release, distribution is unlimited.
//
//==============================================================================
 
#include "GNSSconstants.hpp"
#include "GPSEllipsoid.hpp"
#include "AlmOrbit.hpp"
#include "GPSWeekSecond.hpp"
#include <cmath>
 
namespace gnsstk
{
   AlmOrbit :: AlmOrbit() noexcept
   {
      ecc = i_offset = OMEGAdot = Ahalf = OMEGA0 = w = M0 = AF0 = AF1 = 0.0;
 
      Toa = xmit_time = 0;
 
      week = SV_health = 0;
      PRN = 0;
   }
 
   AlmOrbit :: AlmOrbit(short prn, double aEcc, double ai_offset,
                        double aOMEGAdot, double aAhalf, double aOMEGA0,
                        double aw, double aM0, double aAF0, double aAF1,
                        long aToa, long axmit_time, short aweek,
                        short aSV_health)
         : PRN(prn), ecc(aEcc), i_offset(ai_offset), OMEGAdot(aOMEGAdot), Ahalf(aAhalf),
           OMEGA0(aOMEGA0), w(aw), M0(aM0), AF0(aAF0), AF1(aAF1), Toa(aToa),
           xmit_time(axmit_time), week(aweek), SV_health(aSV_health)
   {
   }
 
   Xvt AlmOrbit :: svXvt(const CommonTime& t) const
   {
      Xvt sv;
      GPSEllipsoid ell;
 
      double elapt;                 /* elapsed time since Toa */
      double A;                     /* semi-major axis */
      double n;                       /* mean motion */
      double meana;                 /* mean anomoly */
      double ea;                    /* eccentric anomoly */
      short loop;                   /* counter */
      double f,g,delea,q,gsta,gcta; /* temp. variables */
      double dtc;                   /* corrected time */
      double ta;                    /* true anomoly */
      double sinea,cosea,sinu,cosu;
      double alat;                  /* arguement of latitude */
      double ualat;                 /* corrected arguement of latitude */
      double r;                     /* radius */
      double i;                     /* inclination */
      double anlon;                 /* corrected longitue of ascending node */
      double xip,yip,can,san,cinc,sinc,xef,yef,zef,dek,dlk,div,domk,duv,
         drv,dxp,dyp,vxef,vyef,vzef;
      double sqrtgm = ::sqrt(ell.gm());
 
/*   Compute time since Almanac epoch (Toa) including week change */
      elapt = t - getToaTime();
 
         /* compute mean motion from semi-major axis */
      A = Ahalf * Ahalf;
      n = sqrtgm / (Ahalf * A);
 
         /* compute the mean anomaly */
      meana = M0 + elapt * n;
      meana = ::fmod(meana, 2.0 * PI);
 
         /* compute eccentric anomaly by iteration */
 
      ea = meana + ecc * ::sin(meana);
      loop = 1;
 
      do {
         f = meana - (ea - ecc * ::sin(ea));
         g = 1.0 - ecc * ::cos(ea);
         delea = f / g;
         ea += delea;
         loop++;
      }  while ( ::fabs(delea) > 1.0e-11 && (loop <= 20));
 
         /* compute clock corrections (no relativistic correction computed) */
      dtc = AF0 + elapt * AF1;
      sv.clkbias = dtc;
 
         /* compute the true anomaly */
      q = ::sqrt (1.0e0 - ecc * ecc);
      sinea = ::sin(ea);
      cosea = ::cos(ea);
      gsta = q * sinea;
      gcta = cosea  - ecc;
      ta = ::atan2(gsta,gcta);
      g = 1.0 - ecc * cosea;
 
         /* compute argument of latitude for orbit */
      alat = ta + w;
 
         /* compute correction terms ( no pertubation ) */
      ualat = alat;
      r = A * (1.0 - ecc * cosea);
      i = i_offset + 0.3e0 * PI;
 
         /* compute corrected longitude of ascending node */
      anlon = OMEGA0 +
         (OMEGAdot - ell.angVelocity()) * elapt -
         ell.angVelocity() * (double)Toa;
 
         /* compute positions in orbital plane */
      cosu = ::cos(ualat);
      sinu = ::sin(ualat);
      xip = r * cosu;
      yip = r * sinu;
 
         /* compute earch fixed coordinates (in meters) */
      can = ::cos (anlon);
      san = ::sin (anlon);
      cinc = ::cos(i);
      sinc = ::sin(i);
 
      xef = xip * can - yip * cinc * san;
      yef = xip * san + yip * cinc * can;
      zef =             yip * sinc;
 
      sv.x[0] = xef;
      sv.x[1] = yef;
      sv.x[2] = zef;
 
         /* compute velocity of rotation coordinates & velocity of sat. */
      dek = n / g;
      dlk = n * q / (g*g);
      div = 0.0e0;
      domk = OMEGAdot - ell.angVelocity();
      duv = dlk;
      drv = A * ecc * dek * sinea;
 
      dxp = drv * cosu - r * sinu * duv;
      dyp = drv * sinu + r * cosu * duv;
 
      vxef = dxp * can - xip * san * domk - dyp * cinc * san
         + yip * (sinc * san * div - cinc * can * domk);
      vyef = dxp * san + xip * can * domk + dyp * cinc * can
         - yip * (sinc * can * div + cinc * san * domk);
      vzef = dyp * sinc + yip * cinc * div;
 
      sv.v[0] = vxef;
      sv.v[1] = vyef;
      sv.v[2] = vzef;
      sv.health = SV_health == 0 ? Xvt::Healthy : Xvt::Unhealthy;
 
      return sv;
   }
 
   CommonTime AlmOrbit::getTransmitTime() const noexcept
   {
      return GPSWeekSecond(getFullWeek(), xmit_time);
   }
 
   short AlmOrbit::getFullWeek() const noexcept
   {
         // return value of the transmit week for the given PRN
      short xmit_week = week;
      double sow_diff = (double)(Toa - xmit_time);
      if (sow_diff < -HALFWEEK)
         xmit_week--;
      else if (sow_diff > HALFWEEK)
         xmit_week++;
 
      return xmit_week;
   }
 
   CommonTime AlmOrbit::getToaTime() const noexcept
   {
      return GPSWeekSecond(week, Toa);
   }
 
   void AlmOrbit::dump(std::ostream& s, int verbosity) const
   {
      using std::endl;
      using std::setw;
 
      s << std::setprecision(4);
      s.setf(std::ios::scientific);
      switch (verbosity)
      {
         case 0:
            s << PRN       << ", "
              << Toa       << ", "
              << week       << ", "
              << std::hex
              << SV_health << ", "
              << std::dec
              << AF0       << ", "
              << AF1       << ", "
              << ecc       << ", "
              << w         << ", "
              << Ahalf     << ", "
              << M0        << ", "
              << OMEGA0    << ", "
              << OMEGAdot  << ", "
              << i_offset
              << endl;
            break;
 
         case 1:
            s << "PRN:" << PRN
              << " Toa:" << Toa
              << " H:" << SV_health
              << " AFO:" << AF0
              << " AF1:" << AF1
              << " Ecc:" << ecc
              << endl
              << "   w:" << w
              << " Ahalf:" << Ahalf
              << " M0:" << M0
              << endl
              << "   OMEGA0:" << OMEGA0
              << " OMEGAdot:" << OMEGAdot
              << " Ioff:" << i_offset
              << endl;
            break;
 
         default:
            s << "PRN:                   " << PRN << endl
              << "Toa:                   " << Toa << endl
              << "xmit_time:             " << xmit_time << endl
              << "week:                  " << week << endl
              << "SV_health:             " << SV_health << endl
              << "AFO:                   " << setw(12) << AF0  << " sec" << endl
              << "AF1:                   " << setw(12) << AF1  << " sec/sec" << endl
              << "Sqrt A:                " << setw(12) << Ahalf  << " sqrt meters" << endl
              << "Eccentricity:          " << setw(12) << ecc    << endl
              << "Arg of perigee:        " << setw(12) << w      << " rad" << endl
              << "Mean anomaly at epoch: " << setw(12) << M0     << " rad" << endl
              << "Right ascension:       " << setw(12) << OMEGA0 << " rad    " << setw(16) << OMEGAdot << " rad/sec" << endl
              << "Inclination offset:    " << setw(12) << i_offset << " rad    " << endl;
      }
   }
 
   std::ostream& operator<<(std::ostream& s, const AlmOrbit& ao)
   {
      ao.dump(s);
      return s;
   }
 
} // namespace