INSTINCT Code Coverage Report

Directory:	src/
File:	Navigation/GNSS/Satellite/Ephemeris/IRNSSEphemeris.cpp
Date:	2025-02-07 16:54:41

	Exec	Total	Coverage
Lines:	85	136	62.5%
Functions:	3	6	50.0%
Branches:	39	76	51.3%

  
      Line
      Branch
      Exec
      Source
    
      // This file is part of INSTINCT, the INS Toolkit for Integrated
    
      // Navigation Concepts and Training by the Institute of Navigation of
    
      // the University of Stuttgart, Germany.
    
      //
    
      // This Source Code Form is subject to the terms of the Mozilla Public
    
      // License, v. 2.0. If a copy of the MPL was not distributed with this
    
      // file, You can obtain one at https://mozilla.org/MPL/2.0/.
    
      #include "IRNSSEphemeris.hpp"
    
      #include "Navigation/Constants.hpp"
    
      #include "Navigation/GNSS/Functions.hpp"
    
      #include "util/Logger.hpp"
    
      namespace NAV
    
      {
    
      ✗
      IRNSSEphemeris::IRNSSEphemeris(const InsTime& toc, const InsTime& toe,
    
                                     const size_t& IODEC,
    
                                     const std::array<double, 3>& a,
    
                                     const double& sqrt_A, const double& e, const double& i_0, const double& Omega_0, const double& omega, const double& M_0,
    
                                     const double& delta_n, const double& Omega_dot, const double& i_dot, const double& Cus, const double& Cuc,
    
                                     const double& Cis, const double& Cic, const double& Crs, const double& Crc,
    
                                     const double& svAccuracy, uint8_t svHealth,
    
      ✗
                                     const double& T_GD)
    
          : SatNavData(SatNavData::IRNSSEphemeris, toc),
    
      ✗
            toc(toc),
    
      ✗
            toe(toe),
    
      ✗
            IODEC(IODEC),
    
      ✗
            a(a),
    
      ✗
            sqrt_A(sqrt_A),
    
      ✗
            e(e),
    
      ✗
            i_0(i_0),
    
      ✗
            Omega_0(Omega_0),
    
      ✗
            omega(omega),
    
      ✗
            M_0(M_0),
    
      ✗
            delta_n(delta_n),
    
      ✗
            Omega_dot(Omega_dot),
    
      ✗
            i_dot(i_dot),
    
      ✗
            Cus(Cus),
    
      ✗
            Cuc(Cuc),
    
      ✗
            Cis(Cis),
    
      ✗
            Cic(Cic),
    
      ✗
            Crs(Crs),
    
      ✗
            Crc(Crc),
    
      ✗
            svAccuracy(svAccuracy),
    
      ✗
            svHealth(svHealth),
    
      ✗
            T_GD(T_GD) {}
    
      #ifdef TESTING
    
      2
      IRNSSEphemeris::IRNSSEphemeris(int32_t year, int32_t month, int32_t day, int32_t hour, int32_t minute, double second, double svClockBias, double svClockDrift, double svClockDriftRate,
    
                                     double IODEC, double Crs, double delta_n, double M_0,
    
                                     double Cuc, double e, double Cus, double sqrt_A,
    
                                     double Toe, double Cic, double Omega_0, double Cis,
    
                                     double i_0, double Crc, double omega, double Omega_dot,
    
                                     double i_dot, double /*spare1*/, double IRNWeek, double /*spare2*/,
    
                                     double svAccuracy, double svHealth, double T_GD, double /*spare3*/,
    
      2
                                     double /*TransmissionTimeOfMessage*/, double /*spare4*/, double /*spare5*/, double /*spare6*/)
    
        2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.

      2
          : SatNavData(SatNavData::IRNSSEphemeris, InsTime(year, month, day, hour, minute, second, SatelliteSystem(IRNSS).getTimeSystem())),
    
      2
            toc(refTime),
    
        2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.

      2
            toe(InsTime(0, static_cast<int32_t>(IRNWeek), Toe, SatelliteSystem(IRNSS).getTimeSystem())),
    
      2
            IODEC(static_cast<size_t>(IODEC)),
    
      2
            a({ svClockBias, svClockDrift, svClockDriftRate }),
    
      2
            sqrt_A(sqrt_A),
    
      2
            e(e),
    
      2
            i_0(i_0),
    
      2
            Omega_0(Omega_0),
    
      2
            omega(omega),
    
      2
            M_0(M_0),
    
      2
            delta_n(delta_n),
    
      2
            Omega_dot(Omega_dot),
    
      2
            i_dot(i_dot),
    
      2
            Cus(Cus),
    
      2
            Cuc(Cuc),
    
      2
            Cis(Cis),
    
      2
            Cic(Cic),
    
      2
            Crs(Crs),
    
      2
            Crc(Crc),
    
      2
            svAccuracy(svAccuracy),
    
      2
            svHealth(static_cast<uint8_t>(svHealth)),
    
        1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.

      4
            T_GD(T_GD)
    
      2
      {}
    
      #endif
    
      50
      Clock::Corrections IRNSSEphemeris::calcClockCorrections(const InsTime& recvTime, double dist, const Frequency& freq) const
    
      {
    
          LOG_DATA("Calc Sat Clock corrections at receiver time {}", recvTime.toGPSweekTow());
    
          // Earth gravitational constant [m³/s²]
    
      50
          const auto mu = InsConst::IRNSS::MU;
    
          // Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²)
    
      50
          const auto F = InsConst::IRNSS::F;
    
          LOG_DATA("    toe {} (Time of ephemeris)", toe.toGPSweekTow());
    
      50
          const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
    
          LOG_DATA("    A {} [m] (Semi-major axis)", A);
    
      50
          auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
    
          LOG_DATA("    n_0 {} [rad/s] (Computed mean motion)", n_0);
    
      50
          auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
    
          LOG_DATA("    n {} [rad/s] (Corrected mean motion)", n);
    
          // Time at transmission
    
        2/4✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.

      50
          InsTime transTime0 = recvTime - std::chrono::duration<double>(dist / InsConst::C);
    
      50
          InsTime transTime = transTime0;
    
          LOG_DATA("    Iterating Time at transmission");
    
      50
          double dt_sv = 0.0;
    
      50
          double clkDrift = 0.0;
    
        2/2✓ Branch 0 taken 100 times.
✓ Branch 1 taken 50 times.

      150
          for (size_t i = 0; i < 2; i++)
    
          {
    
              LOG_DATA("      transTime {} (Time at transmission)", transTime.toGPSweekTow());
    
              // [s]
    
        1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.

      100
              auto t_minus_toc = static_cast<double>((transTime - toc).count());
    
              LOG_DATA("      transTime - toc {} [s]", t_minus_toc);
    
              // Time difference from ephemeris reference epoch [s]
    
        1/2✓ Branch 1 taken 100 times.
✗ Branch 2 not taken.

      100
              double t_k = static_cast<double>((transTime - toe).count());
    
              LOG_DATA("      transTime - toe {} [s] (t_k = Time difference from ephemeris reference epoch)", t_k);
    
              // Mean anomaly [rad]
    
      100
              auto M_k = M_0 + n * t_k;
    
              LOG_DATA("      M_k {} [s] (Mean anomaly)", M_k);
    
              // Eccentric anomaly [rad]
    
      100
              double E_k = M_k;
    
      100
              double E_k_old = 0.0;
    
        5/6✓ Branch 1 taken 470 times.
✓ Branch 2 taken 100 times.
✓ Branch 3 taken 470 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 470 times.
✓ Branch 6 taken 100 times.

      570
              for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
    
              {
    
      470
                  E_k_old = E_k; // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration:
    
      470
                  E_k = M_k + e * sin(E_k);
    
              }
    
              // Relativistic correction term [s]
    
      100
              double dt_r = F * e * sqrt_A * std::sin(E_k);
    
              LOG_DATA("      dt_r {} [s] (Relativistic correction term)", dt_r);
    
              // SV PRN code phase time offset [s]
    
      100
              dt_sv = a[0] + a[1] * t_minus_toc + a[2] * std::pow(t_minus_toc, 2) + dt_r;
    
              // See /cite IRNSS-SIS-ICD-1.1 IRNSS ICD, ch. 6.2.1.5, p.31
    
        1/2✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.

      100
              dt_sv -= ratioFreqSquared(S01, freq, -128, -128) * T_GD;
    
              LOG_DATA("      dt_sv {} [s] (SV PRN code phase time offset)", dt_sv);
    
              // Groves ch. 9.3.1, eq. 9.78, p. 391
    
      100
              clkDrift = a[1] + a[2] / 2.0 * t_minus_toc;
    
              // Correct transmit time for the satellite clock bias
    
        2/4✓ Branch 2 taken 100 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 100 times.
✗ Branch 6 not taken.

      100
              transTime = transTime0 - std::chrono::duration<double>(dt_sv);
    
          }
    
          LOG_DATA("      transTime {} (Time at transmission)", transTime.toGPSweekTow());
    
      50
          return { .transmitTime = transTime, .bias = dt_sv, .drift = clkDrift };
    
      }
    
      50
      Orbit::PosVelAccel IRNSSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const
    
      {
    
        2/4✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.

      50
          Eigen::Vector3d e_pos = Eigen::Vector3d::Zero();
    
        2/4✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.

      50
          Eigen::Vector3d e_vel = Eigen::Vector3d::Zero();
    
        2/4✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.

      50
          Eigen::Vector3d e_accel = Eigen::Vector3d::Zero();
    
          LOG_DATA("Calc Sat Position at transmit time {}", transTime.toGPSweekTow());
    
          // Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user)
    
      50
          const auto mu = InsConst::GPS::MU;
    
          // Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate)
    
      50
          const auto Omega_e_dot = InsConst::GPS::omega_ie;
    
          LOG_DATA("    toe {} (Time of ephemeris)", toe.toGPSweekTow());
    
      50
          const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
    
          LOG_DATA("    A {} [m] (Semi-major axis)", A);
    
      50
          auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
    
          LOG_DATA("    n_0 {} [rad/s] (Computed mean motion)", n_0);
    
      50
          auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
    
          LOG_DATA("    n {} [rad/s] (Corrected mean motion)", n);
    
          // Eccentric anomaly [rad]
    
      50
          double E_k = 0.0;
    
          // Time difference from ephemeris reference epoch [s]
    
        1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.

      50
          double t_k = static_cast<double>((transTime - toe).count());
    
          LOG_DATA("    t_k {} [s] (Time difference from ephemeris reference epoch)", t_k);
    
          // Mean anomaly [rad]
    
      50
          auto M_k = M_0 + n * t_k;
    
          LOG_DATA("    M_k {} [s] (Mean anomaly)", M_k);
    
      50
          E_k = M_k; // Initial Value [rad]
    
      50
          double E_k_old = 0.0;
    
          LOG_DATA("    Iterating E_k");
    
          LOG_DATA("      E_k {} [rad] (Eccentric anomaly)", E_k);
    
        5/6✓ Branch 1 taken 150 times.
✓ Branch 2 taken 50 times.
✓ Branch 3 taken 150 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 150 times.
✓ Branch 6 taken 50 times.

      200
          for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
    
          {
    
      150
              E_k_old = E_k;                                                         // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration:
    
      150
              E_k = E_k + (M_k - E_k + e * std::sin(E_k)) / (1 - e * std::cos(E_k)); // – Refined Value, minimum of three iterations, (j=1,2,3)
    
              LOG_DATA("      E_k {} [rad] (Eccentric anomaly)", E_k);               // – Final Value (radians)
    
          }
    
          // auto v_k = 2.0 * std::atan(std::sqrt((1.0 + e) / (1.0 - e)) * std::tan(E_k / 2.0)); // True Anomaly (unambiguous quadrant) [rad] (GPS ICD algorithm)
    
          // auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k) / (1 - e * std::cos(E_k)), (std::cos(E_k) - e) / (1 - e * std::cos(E_k))); // True Anomaly [rad] (GALILEO ICD algorithm)
    
      50
          auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k), (std::cos(E_k) - e)); // True Anomaly [rad] // simplified, since the denominators cancel out
    
          LOG_DATA("    v_k {} [rad] (True Anomaly (unambiguous quadrant))", v_k);
    
      50
          auto Phi_k = v_k + omega; // Argument of Latitude [rad]
    
          LOG_DATA("    Phi_k {} [rad] (Argument of Latitude)", Phi_k);
    
          // Second Harmonic Perturbations
    
      50
          auto delta_u_k = Cus * std::sin(2 * Phi_k) + Cuc * std::cos(2 * Phi_k); // Argument of Latitude Correction [rad]
    
          LOG_DATA("    delta_u_k {} [rad] (Argument of Latitude Correction)", delta_u_k);
    
      50
          auto delta_r_k = Crs * std::sin(2 * Phi_k) + Crc * std::cos(2 * Phi_k); // Radius Correction [m]
    
          LOG_DATA("    delta_r_k {} [m] (Radius Correction)", delta_r_k);
    
      50
          auto delta_i_k = Cis * std::sin(2 * Phi_k) + Cic * std::cos(2 * Phi_k); // Inclination Correction [rad]
    
          LOG_DATA("    delta_i_k {} [rad] (Inclination Correction)", delta_i_k);
    
      50
          auto u_k = Phi_k + delta_u_k; // Corrected Argument of Latitude [rad]
    
          LOG_DATA("    u_k {} [rad] (Corrected Argument of Latitude)", u_k);
    
      50
          auto r_k = A * (1 - e * std::cos(E_k)) + delta_r_k; // Corrected Radius [m]
    
          LOG_DATA("    r_k {} [m] (Corrected Radius)", r_k);
    
      50
          auto i_k = i_0 + delta_i_k + i_dot * t_k; // Corrected Inclination [rad]
    
          LOG_DATA("    i_k {} [rad] (Corrected Inclination)", i_k);
    
      50
          auto x_k_op = r_k * std::cos(u_k); // Position in orbital plane [m]
    
          LOG_DATA("    x_k_op {} [m] (Position in orbital plane)", x_k_op);
    
      50
          auto y_k_op = r_k * std::sin(u_k); // Position in orbital plane [m]
    
          LOG_DATA("    y_k_op {} [m] (Position in orbital plane)", y_k_op);
    
          // Corrected longitude of ascending node [rad]
    
        1/2✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.

      50
          auto Omega_k = Omega_0 + (Omega_dot - Omega_e_dot) * t_k - Omega_e_dot * static_cast<double>(toe.toGPSweekTow(IRNSST).tow);
    
          LOG_DATA("    Omega_k {} [rad] (Corrected longitude of ascending node)", Omega_k);
    
          // Earth-fixed x coordinates [m]
    
      50
          auto x_k = x_k_op * std::cos(Omega_k) - y_k_op * std::cos(i_k) * std::sin(Omega_k);
    
          LOG_DATA("    x_k {} [m] (Earth-fixed x coordinates)", x_k);
    
          // Earth-fixed y coordinates [m]
    
      50
          auto y_k = x_k_op * std::sin(Omega_k) + y_k_op * std::cos(i_k) * std::cos(Omega_k);
    
          LOG_DATA("    y_k {} [m] (Earth-fixed y coordinates)", y_k);
    
          // Earth-fixed z coordinates [m]
    
      50
          auto z_k = y_k_op * std::sin(i_k);
    
          LOG_DATA("    z_k {} [m] (Earth-fixed z coordinates)", z_k);
    
        1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.

      50
          e_pos = Eigen::Vector3d{ x_k, y_k, z_k };
    
        3/6✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 50 times.

      50
          if (calc & Calc_Velocity || calc & Calc_Acceleration)
    
          {
    
              // Eccentric Anomaly Rate [rad/s]
    
      ✗
              auto E_k_dot = n / (1 - e * std::cos(E_k));
    
              // True Anomaly Rate [rad/s]
    
      ✗
              auto v_k_dot = E_k_dot * std::sqrt(1 - e * e) / (1 - e * std::cos(E_k));
    
              // Corrected Inclination Angle Rate [rad/s]
    
      ✗
              auto i_k_dot = i_dot + 2 * v_k_dot * (Cis * std::cos(2 * Phi_k) - Cic * std::sin(2 * Phi_k));
    
              // Corrected Argument of Latitude Rate [rad/s]
    
      ✗
              auto u_k_dot = v_k_dot + 2 * v_k_dot * (Cus * std::cos(2 * Phi_k) - Cuc * std::sin(2 * Phi_k));
    
              // Corrected Radius Rate [m/s]
    
      ✗
              auto r_k_dot = e * A * E_k_dot * std::sin(E_k) + 2 * v_k_dot * (Crs * std::cos(2 * Phi_k) - Crc * std::sin(2 * Phi_k));
    
              // Longitude of Ascending Node Rate [rad/s]
    
      ✗
              auto Omega_k_dot = Omega_dot - Omega_e_dot;
    
              // In-plane x velocity [m/s]
    
      ✗
              auto vx_k_op = r_k_dot * std::cos(u_k) - r_k * u_k_dot * std::sin(u_k);
    
              // In-plane y velocity [m/s]
    
      ✗
              auto vy_k_op = r_k_dot * std::sin(u_k) + r_k * u_k_dot * std::cos(u_k);
    
              // Earth-Fixed x velocity [m/s]
    
      ✗
              auto vx_k = -x_k_op * Omega_k_dot * std::sin(Omega_k) + vx_k_op * std::cos(Omega_k) - vy_k_op * std::sin(Omega_k) * std::cos(i_k)
    
      ✗
                          - y_k_op * (Omega_k_dot * std::cos(Omega_k) * std::cos(i_k) - i_k_dot * std::sin(Omega_k) * std::sin(i_k));
    
              // Earth-Fixed y velocity [m/s]
    
      ✗
              auto vy_k = x_k_op * Omega_k_dot * std::cos(Omega_k) + vx_k_op * std::sin(Omega_k) + vy_k_op * std::cos(Omega_k) * std::cos(i_k)
    
      ✗
                          - y_k_op * (Omega_k_dot * std::sin(Omega_k) * std::cos(i_k) + i_k_dot * std::cos(Omega_k) * std::sin(i_k));
    
              // Earth-Fixed z velocity [m/s]
    
      ✗
              auto vz_k = vy_k_op * std::sin(i_k) + y_k_op * i_k_dot * std::cos(i_k);
    
      ✗
              if (calc & Calc_Velocity)
    
              {
    
      ✗
                  e_vel = Eigen::Vector3d{ vx_k, vy_k, vz_k };
    
              }
    
      ✗
              if (calc & Calc_Acceleration)
    
              {
    
                  // Oblate Earth acceleration Factor [m/s^2]
    
      ✗
                  auto F = -(3.0 / 2.0) * InsConst::GPS::J2 * (mu / std::pow(r_k, 2)) * std::pow(InsConst::GPS::R_E / r_k, 2);
    
                  // Earth-Fixed x acceleration [m/s^2]
    
      ✗
                  auto ax_k = -mu * (x_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (x_k / r_k))
    
      ✗
                              + 2 * vy_k * Omega_e_dot + x_k * std::pow(Omega_e_dot, 2);
    
                  // Earth-Fixed y acceleration [m/s^2]
    
      ✗
                  auto ay_k = -mu * (y_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (y_k / r_k))
    
      ✗
                              + 2 * vx_k * Omega_e_dot + y_k * std::pow(Omega_e_dot, 2);
    
                  // Earth-Fixed z acceleration [m/s^2]
    
      ✗
                  auto az_k = -mu * (z_k / std::pow(r_k, 3)) + F * ((3.0 - 5.0 * std::pow(z_k / r_k, 2)) * (z_k / r_k));
    
      ✗
                  e_accel = Eigen::Vector3d{ ax_k, ay_k, az_k };
    
              }
    
          }
    
          return { .e_pos = e_pos,
    
                   .e_vel = e_vel,
    
        3/6✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.

      100
                   .e_accel = e_accel };
    
      }
    
      ✗
      bool IRNSSEphemeris::isHealthy() const // TODO Parse Signal Id as a parameter and differentiate depending on the bitset
    
      {
    
      ✗
          return svHealth.none();
    
      }
    
      ✗
      double IRNSSEphemeris::calcSatellitePositionVariance() const
    
      {
    
          // Getting the index and value again will discretize the URA values
    
      ✗
          return std::pow(gpsUraIdx2Val(gpsUraVal2Idx(svAccuracy)), 2);
    
      }
    
      } // namespace NAV

Line	Branch	Exec	Source
1			// This file is part of INSTINCT, the INS Toolkit for Integrated
2			// Navigation Concepts and Training by the Institute of Navigation of
3			// the University of Stuttgart, Germany.
4			//
5			// This Source Code Form is subject to the terms of the Mozilla Public
6			// License, v. 2.0. If a copy of the MPL was not distributed with this
7			// file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9			#include "IRNSSEphemeris.hpp"
10
11			#include "Navigation/Constants.hpp"
12			#include "Navigation/GNSS/Functions.hpp"
13
14			#include "util/Logger.hpp"
15
16			namespace NAV
17			{
18
19		✗	IRNSSEphemeris::IRNSSEphemeris(const InsTime& toc, const InsTime& toe,
20			const size_t& IODEC,
21			const std::array<double, 3>& a,
22			const double& sqrt_A, const double& e, const double& i_0, const double& Omega_0, const double& omega, const double& M_0,
23			const double& delta_n, const double& Omega_dot, const double& i_dot, const double& Cus, const double& Cuc,
24			const double& Cis, const double& Cic, const double& Crs, const double& Crc,
25			const double& svAccuracy, uint8_t svHealth,
26		✗	const double& T_GD)
27			: SatNavData(SatNavData::IRNSSEphemeris, toc),
28		✗	toc(toc),
29		✗	toe(toe),
30		✗	IODEC(IODEC),
31		✗	a(a),
32		✗	sqrt_A(sqrt_A),
33		✗	e(e),
34		✗	i_0(i_0),
35		✗	Omega_0(Omega_0),
36		✗	omega(omega),
37		✗	M_0(M_0),
38		✗	delta_n(delta_n),
39		✗	Omega_dot(Omega_dot),
40		✗	i_dot(i_dot),
41		✗	Cus(Cus),
42		✗	Cuc(Cuc),
43		✗	Cis(Cis),
44		✗	Cic(Cic),
45		✗	Crs(Crs),
46		✗	Crc(Crc),
47		✗	svAccuracy(svAccuracy),
48		✗	svHealth(svHealth),
49		✗	T_GD(T_GD) {}
50
51			#ifdef TESTING
52
53		2	IRNSSEphemeris::IRNSSEphemeris(int32_t year, int32_t month, int32_t day, int32_t hour, int32_t minute, double second, double svClockBias, double svClockDrift, double svClockDriftRate,
54			double IODEC, double Crs, double delta_n, double M_0,
55			double Cuc, double e, double Cus, double sqrt_A,
56			double Toe, double Cic, double Omega_0, double Cis,
57			double i_0, double Crc, double omega, double Omega_dot,
58			double i_dot, double /spare1/, double IRNWeek, double /spare2/,
59			double svAccuracy, double svHealth, double T_GD, double /spare3/,
60		2	double /TransmissionTimeOfMessage/, double /spare4/, double /spare5/, double /spare6/)
61	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	2	: SatNavData(SatNavData::IRNSSEphemeris, InsTime(year, month, day, hour, minute, second, SatelliteSystem(IRNSS).getTimeSystem())),
62		2	toc(refTime),
63	2/4 ✓ Branch 1 taken 2 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 2 times. ✗ Branch 5 not taken.	2	toe(InsTime(0, static_cast<int32_t>(IRNWeek), Toe, SatelliteSystem(IRNSS).getTimeSystem())),
64		2	IODEC(static_cast<size_t>(IODEC)),
65		2	a({ svClockBias, svClockDrift, svClockDriftRate }),
66		2	sqrt_A(sqrt_A),
67		2	e(e),
68		2	i_0(i_0),
69		2	Omega_0(Omega_0),
70		2	omega(omega),
71		2	M_0(M_0),
72		2	delta_n(delta_n),
73		2	Omega_dot(Omega_dot),
74		2	i_dot(i_dot),
75		2	Cus(Cus),
76		2	Cuc(Cuc),
77		2	Cis(Cis),
78		2	Cic(Cic),
79		2	Crs(Crs),
80		2	Crc(Crc),
81		2	svAccuracy(svAccuracy),
82		2	svHealth(static_cast<uint8_t>(svHealth)),
83	1/2 ✓ Branch 2 taken 2 times. ✗ Branch 3 not taken.	4	T_GD(T_GD)
84		2	{}
85
86			#endif
87
88		50	Clock::Corrections IRNSSEphemeris::calcClockCorrections(const InsTime& recvTime, double dist, const Frequency& freq) const
89			{
90			LOG_DATA("Calc Sat Clock corrections at receiver time {}", recvTime.toGPSweekTow());
91			// Earth gravitational constant [m³/s²]
92		50	const auto mu = InsConst::IRNSS::MU;
93			// Relativistic constant F for clock corrections [s/√m] (-2*√µ/c²)
94		50	const auto F = InsConst::IRNSS::F;
95
96			LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow());
97
98		50	const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
99			LOG_DATA(" A {} [m] (Semi-major axis)", A);
100		50	auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
101			LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0);
102		50	auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
103			LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n);
104
105			// Time at transmission
106	2/4 ✓ Branch 2 taken 50 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 50 times. ✗ Branch 6 not taken.	50	InsTime transTime0 = recvTime - std::chrono::duration<double>(dist / InsConst::C);
107
108		50	InsTime transTime = transTime0;
109			LOG_DATA(" Iterating Time at transmission");
110		50	double dt_sv = 0.0;
111		50	double clkDrift = 0.0;
112
113	2/2 ✓ Branch 0 taken 100 times. ✓ Branch 1 taken 50 times.	150	for (size_t i = 0; i < 2; i++)
114			{
115			LOG_DATA(" transTime {} (Time at transmission)", transTime.toGPSweekTow());
116
117			// [s]
118	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	100	auto t_minus_toc = static_cast<double>((transTime - toc).count());
119			LOG_DATA(" transTime - toc {} [s]", t_minus_toc);
120
121			// Time difference from ephemeris reference epoch [s]
122	1/2 ✓ Branch 1 taken 100 times. ✗ Branch 2 not taken.	100	double t_k = static_cast<double>((transTime - toe).count());
123			LOG_DATA(" transTime - toe {} [s] (t_k = Time difference from ephemeris reference epoch)", t_k);
124
125			// Mean anomaly [rad]
126		100	auto M_k = M_0 + n * t_k;
127			LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k);
128
129			// Eccentric anomaly [rad]
130		100	double E_k = M_k;
131		100	double E_k_old = 0.0;
132
133	5/6 ✓ Branch 1 taken 470 times. ✓ Branch 2 taken 100 times. ✓ Branch 3 taken 470 times. ✗ Branch 4 not taken. ✓ Branch 5 taken 470 times. ✓ Branch 6 taken 100 times.	570	for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
134			{
135		470	E_k_old = E_k; // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration:
136		470	E_k = M_k + e * sin(E_k);
137			}
138
139			// Relativistic correction term [s]
140		100	double dt_r = F * e * sqrt_A * std::sin(E_k);
141			LOG_DATA(" dt_r {} [s] (Relativistic correction term)", dt_r);
142
143			// SV PRN code phase time offset [s]
144		100	dt_sv = a[0] + a[1] * t_minus_toc + a[2] * std::pow(t_minus_toc, 2) + dt_r;
145
146			// See /cite IRNSS-SIS-ICD-1.1 IRNSS ICD, ch. 6.2.1.5, p.31
147	1/2 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken.	100	dt_sv -= ratioFreqSquared(S01, freq, -128, -128) * T_GD;
148
149			LOG_DATA(" dt_sv {} [s] (SV PRN code phase time offset)", dt_sv);
150
151			// Groves ch. 9.3.1, eq. 9.78, p. 391
152		100	clkDrift = a[1] + a[2] / 2.0 * t_minus_toc;
153
154			// Correct transmit time for the satellite clock bias
155	2/4 ✓ Branch 2 taken 100 times. ✗ Branch 3 not taken. ✓ Branch 5 taken 100 times. ✗ Branch 6 not taken.	100	transTime = transTime0 - std::chrono::duration<double>(dt_sv);
156			}
157			LOG_DATA(" transTime {} (Time at transmission)", transTime.toGPSweekTow());
158
159		50	return { .transmitTime = transTime, .bias = dt_sv, .drift = clkDrift };
160			}
161
162		50	Orbit::PosVelAccel IRNSSEphemeris::calcSatelliteData(const InsTime& transTime, Orbit::Calc calc) const
163			{
164	2/4 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 50 times. ✗ Branch 5 not taken.	50	Eigen::Vector3d e_pos = Eigen::Vector3d::Zero();
165	2/4 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 50 times. ✗ Branch 5 not taken.	50	Eigen::Vector3d e_vel = Eigen::Vector3d::Zero();
166	2/4 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 50 times. ✗ Branch 5 not taken.	50	Eigen::Vector3d e_accel = Eigen::Vector3d::Zero();
167
168			LOG_DATA("Calc Sat Position at transmit time {}", transTime.toGPSweekTow());
169			// Earth gravitational constant [m³/s²] (WGS 84 value of the earth's gravitational constant for GPS user)
170		50	const auto mu = InsConst::GPS::MU;
171			// Earth angular velocity [rad/s] (WGS 84 value of the earth's rotation rate)
172		50	const auto Omega_e_dot = InsConst::GPS::omega_ie;
173
174			LOG_DATA(" toe {} (Time of ephemeris)", toe.toGPSweekTow());
175
176		50	const auto A = sqrt_A * sqrt_A; // Semi-major axis [m]
177			LOG_DATA(" A {} [m] (Semi-major axis)", A);
178		50	auto n_0 = std::sqrt(mu / std::pow(A, 3)); // Computed mean motion [rad/s]
179			LOG_DATA(" n_0 {} [rad/s] (Computed mean motion)", n_0);
180		50	auto n = n_0 + delta_n; // Corrected mean motion [rad/s]
181			LOG_DATA(" n {} [rad/s] (Corrected mean motion)", n);
182
183			// Eccentric anomaly [rad]
184		50	double E_k = 0.0;
185
186			// Time difference from ephemeris reference epoch [s]
187	1/2 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken.	50	double t_k = static_cast<double>((transTime - toe).count());
188			LOG_DATA(" t_k {} [s] (Time difference from ephemeris reference epoch)", t_k);
189
190			// Mean anomaly [rad]
191		50	auto M_k = M_0 + n * t_k;
192			LOG_DATA(" M_k {} [s] (Mean anomaly)", M_k);
193
194		50	E_k = M_k; // Initial Value [rad]
195		50	double E_k_old = 0.0;
196			LOG_DATA(" Iterating E_k");
197			LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k);
198	5/6 ✓ Branch 1 taken 150 times. ✓ Branch 2 taken 50 times. ✓ Branch 3 taken 150 times. ✗ Branch 4 not taken. ✓ Branch 5 taken 150 times. ✓ Branch 6 taken 50 times.	200	for (size_t i = 0; std::abs(E_k - E_k_old) > 1e-13 && i < 10; i++)
199			{
200		150	E_k_old = E_k; // Kepler’s equation ( Mk = E_k − e sin E_k ) may be solved for Eccentric anomaly (E_k) by iteration:
201		150	E_k = E_k + (M_k - E_k + e * std::sin(E_k)) / (1 - e * std::cos(E_k)); // – Refined Value, minimum of three iterations, (j=1,2,3)
202			LOG_DATA(" E_k {} [rad] (Eccentric anomaly)", E_k); // – Final Value (radians)
203			}
204
205			// auto v_k = 2.0 * std::atan(std::sqrt((1.0 + e) / (1.0 - e)) * std::tan(E_k / 2.0)); // True Anomaly (unambiguous quadrant) [rad] (GPS ICD algorithm)
206			// auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k) / (1 - e * std::cos(E_k)), (std::cos(E_k) - e) / (1 - e * std::cos(E_k))); // True Anomaly [rad] (GALILEO ICD algorithm)
207		50	auto v_k = std::atan2(std::sqrt(1 - e * e) * std::sin(E_k), (std::cos(E_k) - e)); // True Anomaly [rad] // simplified, since the denominators cancel out
208			LOG_DATA(" v_k {} [rad] (True Anomaly (unambiguous quadrant))", v_k);
209		50	auto Phi_k = v_k + omega; // Argument of Latitude [rad]
210			LOG_DATA(" Phi_k {} [rad] (Argument of Latitude)", Phi_k);
211
212			// Second Harmonic Perturbations
213		50	auto delta_u_k = Cus * std::sin(2 * Phi_k) + Cuc * std::cos(2 * Phi_k); // Argument of Latitude Correction [rad]
214			LOG_DATA(" delta_u_k {} [rad] (Argument of Latitude Correction)", delta_u_k);
215		50	auto delta_r_k = Crs * std::sin(2 * Phi_k) + Crc * std::cos(2 * Phi_k); // Radius Correction [m]
216			LOG_DATA(" delta_r_k {} [m] (Radius Correction)", delta_r_k);
217		50	auto delta_i_k = Cis * std::sin(2 * Phi_k) + Cic * std::cos(2 * Phi_k); // Inclination Correction [rad]
218			LOG_DATA(" delta_i_k {} [rad] (Inclination Correction)", delta_i_k);
219
220		50	auto u_k = Phi_k + delta_u_k; // Corrected Argument of Latitude [rad]
221			LOG_DATA(" u_k {} [rad] (Corrected Argument of Latitude)", u_k);
222		50	auto r_k = A * (1 - e * std::cos(E_k)) + delta_r_k; // Corrected Radius [m]
223			LOG_DATA(" r_k {} [m] (Corrected Radius)", r_k);
224		50	auto i_k = i_0 + delta_i_k + i_dot * t_k; // Corrected Inclination [rad]
225			LOG_DATA(" i_k {} [rad] (Corrected Inclination)", i_k);
226
227		50	auto x_k_op = r_k * std::cos(u_k); // Position in orbital plane [m]
228			LOG_DATA(" x_k_op {} [m] (Position in orbital plane)", x_k_op);
229		50	auto y_k_op = r_k * std::sin(u_k); // Position in orbital plane [m]
230			LOG_DATA(" y_k_op {} [m] (Position in orbital plane)", y_k_op);
231
232			// Corrected longitude of ascending node [rad]
233	1/2 ✓ Branch 2 taken 50 times. ✗ Branch 3 not taken.	50	auto Omega_k = Omega_0 + (Omega_dot - Omega_e_dot) * t_k - Omega_e_dot * static_cast<double>(toe.toGPSweekTow(IRNSST).tow);
234			LOG_DATA(" Omega_k {} [rad] (Corrected longitude of ascending node)", Omega_k);
235
236			// Earth-fixed x coordinates [m]
237		50	auto x_k = x_k_op * std::cos(Omega_k) - y_k_op * std::cos(i_k) * std::sin(Omega_k);
238			LOG_DATA(" x_k {} [m] (Earth-fixed x coordinates)", x_k);
239			// Earth-fixed y coordinates [m]
240		50	auto y_k = x_k_op * std::sin(Omega_k) + y_k_op * std::cos(i_k) * std::cos(Omega_k);
241			LOG_DATA(" y_k {} [m] (Earth-fixed y coordinates)", y_k);
242			// Earth-fixed z coordinates [m]
243		50	auto z_k = y_k_op * std::sin(i_k);
244			LOG_DATA(" z_k {} [m] (Earth-fixed z coordinates)", z_k);
245
246	1/2 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken.	50	e_pos = Eigen::Vector3d{ x_k, y_k, z_k };
247
248	3/6 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken. ✗ Branch 4 not taken. ✓ Branch 5 taken 50 times. ✗ Branch 6 not taken. ✓ Branch 7 taken 50 times.	50	if (calc & Calc_Velocity \|\| calc & Calc_Acceleration)
249			{
250			// Eccentric Anomaly Rate [rad/s]
251		✗	auto E_k_dot = n / (1 - e * std::cos(E_k));
252			// True Anomaly Rate [rad/s]
253		✗	auto v_k_dot = E_k_dot * std::sqrt(1 - e * e) / (1 - e * std::cos(E_k));
254			// Corrected Inclination Angle Rate [rad/s]
255		✗	auto i_k_dot = i_dot + 2 * v_k_dot * (Cis * std::cos(2 * Phi_k) - Cic * std::sin(2 * Phi_k));
256			// Corrected Argument of Latitude Rate [rad/s]
257		✗	auto u_k_dot = v_k_dot + 2 * v_k_dot * (Cus * std::cos(2 * Phi_k) - Cuc * std::sin(2 * Phi_k));
258			// Corrected Radius Rate [m/s]
259		✗	auto r_k_dot = e * A * E_k_dot * std::sin(E_k) + 2 * v_k_dot * (Crs * std::cos(2 * Phi_k) - Crc * std::sin(2 * Phi_k));
260			// Longitude of Ascending Node Rate [rad/s]
261		✗	auto Omega_k_dot = Omega_dot - Omega_e_dot;
262			// In-plane x velocity [m/s]
263		✗	auto vx_k_op = r_k_dot * std::cos(u_k) - r_k * u_k_dot * std::sin(u_k);
264			// In-plane y velocity [m/s]
265		✗	auto vy_k_op = r_k_dot * std::sin(u_k) + r_k * u_k_dot * std::cos(u_k);
266			// Earth-Fixed x velocity [m/s]
267		✗	auto vx_k = -x_k_op * Omega_k_dot * std::sin(Omega_k) + vx_k_op * std::cos(Omega_k) - vy_k_op * std::sin(Omega_k) * std::cos(i_k)
268		✗	- y_k_op * (Omega_k_dot * std::cos(Omega_k) * std::cos(i_k) - i_k_dot * std::sin(Omega_k) * std::sin(i_k));
269			// Earth-Fixed y velocity [m/s]
270		✗	auto vy_k = x_k_op * Omega_k_dot * std::cos(Omega_k) + vx_k_op * std::sin(Omega_k) + vy_k_op * std::cos(Omega_k) * std::cos(i_k)
271		✗	- y_k_op * (Omega_k_dot * std::sin(Omega_k) * std::cos(i_k) + i_k_dot * std::cos(Omega_k) * std::sin(i_k));
272			// Earth-Fixed z velocity [m/s]
273		✗	auto vz_k = vy_k_op * std::sin(i_k) + y_k_op * i_k_dot * std::cos(i_k);
274
275		✗	if (calc & Calc_Velocity)
276			{
277		✗	e_vel = Eigen::Vector3d{ vx_k, vy_k, vz_k };
278			}
279
280		✗	if (calc & Calc_Acceleration)
281			{
282			// Oblate Earth acceleration Factor [m/s^2]
283		✗	auto F = -(3.0 / 2.0) * InsConst::GPS::J2 * (mu / std::pow(r_k, 2)) * std::pow(InsConst::GPS::R_E / r_k, 2);
284			// Earth-Fixed x acceleration [m/s^2]
285		✗	auto ax_k = -mu * (x_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (x_k / r_k))
286		✗	+ 2 * vy_k * Omega_e_dot + x_k * std::pow(Omega_e_dot, 2);
287			// Earth-Fixed y acceleration [m/s^2]
288		✗	auto ay_k = -mu * (y_k / std::pow(r_k, 3)) + F * ((1.0 - 5.0 * std::pow(z_k / r_k, 2)) * (y_k / r_k))
289		✗	+ 2 * vx_k * Omega_e_dot + y_k * std::pow(Omega_e_dot, 2);
290			// Earth-Fixed z acceleration [m/s^2]
291		✗	auto az_k = -mu * (z_k / std::pow(r_k, 3)) + F * ((3.0 - 5.0 * std::pow(z_k / r_k, 2)) * (z_k / r_k));
292
293		✗	e_accel = Eigen::Vector3d{ ax_k, ay_k, az_k };
294			}
295			}
296
297			return { .e_pos = e_pos,
298			.e_vel = e_vel,
299	3/6 ✓ Branch 1 taken 50 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 50 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 50 times. ✗ Branch 8 not taken.	100	.e_accel = e_accel };
300			}
301
302		✗	bool IRNSSEphemeris::isHealthy() const // TODO Parse Signal Id as a parameter and differentiate depending on the bitset
303			{
304		✗	return svHealth.none();
305			}
306
307		✗	double IRNSSEphemeris::calcSatellitePositionVariance() const
308			{
309			// Getting the index and value again will discretize the URA values
310		✗	return std::pow(gpsUraIdx2Val(gpsUraVal2Idx(svAccuracy)), 2);
311			}
312
313			} // namespace NAV
314