diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index bc42aeecf..7a13109c1 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -262,141 +262,6 @@ signed long int Gps_Navigation_Message::read_navigation_signed(std::bitset half_week) - { - corrTime = time - 2 * half_week; - } - else if (time < -half_week) - { - corrTime = time + 2 * half_week; - } - return corrTime; -} - - - - -// 20.3.3.3.3.1 User Algorithm for SV Clock Correction. -double Gps_Navigation_Message::sv_clock_correction(double transmitTime) -{ - double dt; - dt = check_t(transmitTime - d_Toc); - d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr; - double correctedTime = transmitTime - d_satClkCorr; - return correctedTime; -} - - - - - -void Gps_Navigation_Message::satellitePosition(double transmitTime) -{ - double tk; - double a; - double n; - double n0; - double M; - double E; - double E_old; - double dE; - double nu; - double phi; - double u; - double r; - double i; - double Omega; - - // Find satellite's position ---------------------------------------------- - - // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; - - // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe); - - // Computed mean motion - n0 = sqrt(GM / (a * a * a)); - - // Corrected mean motion - n = n0 + d_Delta_n; - - // Mean anomaly - M = d_M_0 + n * tk; - - // Reduce mean anomaly to between 0 and 2pi - M = fmod((M + 2 * GPS_PI), (2 * GPS_PI)); - - // Initial guess of eccentric anomaly - E = M; - - // --- Iteratively compute eccentric anomaly ---------------------------- - for (int ii = 1; ii < 20; ii++) - { - E_old = E; - E = M + d_e_eccentricity * sin(E); - dE = fmod(E - E_old, 2 * GPS_PI); - if (fabs(dE) < 1e-12) - { - //Necessary precision is reached, exit from the loop - break; - } - } - - // Compute relativistic correction term - d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E); - - // Compute the true anomaly - double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); - double tmp_X = cos(E) - d_e_eccentricity; - nu = atan2(tmp_Y, tmp_X); - - // Compute angle phi (argument of Latitude) - phi = nu + d_OMEGA; - - // Reduce phi to between 0 and 2*pi rad - phi = fmod((phi), (2 * GPS_PI)); - - // Correct argument of latitude - u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi); - - // Correct radius - r = a * (1 - d_e_eccentricity * cos(E)) + d_Crc * cos(2 * phi) + d_Crs * sin(2 * phi); - - // Correct inclination - i = d_i_0 + d_IDOT * tk + d_Cic * cos(2 * phi) + d_Cis * sin(2 * phi); - - // Compute the angle between the ascending node and the Greenwich meridian - Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT) * tk - OMEGA_EARTH_DOT * d_Toe; - - // Reduce to between 0 and 2*pi rad - Omega = fmod((Omega + 2 * GPS_PI), (2 * GPS_PI)); - - // --- Compute satellite coordinates in Earth-fixed coordinates - d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); - d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); - d_satpos_Z = sin(u) * r * sin(i); - - // Satellite's velocity. Can be useful for Vector Tracking loops - double Omega_dot = d_OMEGA_DOT - OMEGA_EARTH_DOT; - d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); - d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); - d_satvel_Z = d_satpos_Y * sin(i); -} - - - - - int Gps_Navigation_Message::subframe_decoder(char *subframe) { int subframe_ID = 0; diff --git a/src/core/system_parameters/gps_navigation_message.h b/src/core/system_parameters/gps_navigation_message.h index aaded71a1..53482c905 100644 --- a/src/core/system_parameters/gps_navigation_message.h +++ b/src/core/system_parameters/gps_navigation_message.h @@ -58,15 +58,6 @@ private: signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); bool read_navigation_bool(std::bitset bits, const std::vector> parameter); void print_gps_word_bytes(unsigned int GPS_word); - /* - * Accounts for the beginning or end of week crossover - * - * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) - * \param[in] - time in seconds - * \param[out] - corrected time, in seconds - */ - double check_t(double time); - public: bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check //broadcast orbit 1 @@ -212,19 +203,6 @@ public: */ int subframe_decoder(char *subframe); - /*! - * \brief Computes the position of the satellite - * - * Implementation of Table 20-IV (IS-GPS-200E) - */ - void satellitePosition(double transmitTime); - - /*! - * \brief Sets (\a d_satClkCorr) according to the User Algorithm for SV Clock Correction - * and returns the corrected clock (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_correction(double transmitTime); - /*! * \brief Computes the Coordinated Universal Time (UTC) and * returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4)