diff --git a/src/core/system_parameters/beidou_dnav_ephemeris.cc b/src/core/system_parameters/beidou_dnav_ephemeris.cc index c773e5054..26e4f7894 100644 --- a/src/core/system_parameters/beidou_dnav_ephemeris.cc +++ b/src/core/system_parameters/beidou_dnav_ephemeris.cc @@ -26,7 +26,7 @@ Beidou_Dnav_Ephemeris::Beidou_Dnav_Ephemeris() { auto gnss_sat = Gnss_Satellite(); - std::string _system("Beidou"); + const std::string _system("Beidou"); for (unsigned int i = 1; i < 36; i++) { satelliteBlock[i] = gnss_sat.what_block(_system, i); @@ -36,9 +36,8 @@ Beidou_Dnav_Ephemeris::Beidou_Dnav_Ephemeris() double Beidou_Dnav_Ephemeris::check_t(double time) { - double corrTime; - double half_week = 302400.0; // seconds - corrTime = time; + const double half_week = 302400.0; // seconds + double corrTime = time; if (time > half_week) { corrTime = time - 2.0 * half_week; @@ -53,8 +52,7 @@ double Beidou_Dnav_Ephemeris::check_t(double time) double Beidou_Dnav_Ephemeris::sv_clock_drift(double transmitTime) { - double dt; - dt = check_t(transmitTime - d_Toc); + double dt = check_t(transmitTime - d_Toc); for (int i = 0; i < 2; i++) { @@ -69,35 +67,30 @@ double Beidou_Dnav_Ephemeris::sv_clock_drift(double transmitTime) // compute the relativistic correction term double Beidou_Dnav_Ephemeris::sv_clock_relativistic_term(double transmitTime) { - double tk; - double a; - double n; - double n0; - double E; - double E_old; - double dE; - double M; - // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; + const double a = d_sqrt_A * d_sqrt_A; // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe); + const double tk = check_t(transmitTime - d_Toe); // Computed mean motion - n0 = sqrt(BEIDOU_GM / (a * a * a)); + const double n0 = sqrt(BEIDOU_GM / (a * a * a)); + // Corrected mean motion - n = n0 + d_Delta_n; + const double n = n0 + d_Delta_n; + // Mean anomaly - M = d_M_0 + n * tk; + double M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; + double E = M; + double E_old; + double dE; - // --- Iteratively compute eccentric anomaly ---------------------------- + // --- Iteratively compute eccentric anomaly ------------------------------- for (int ii = 1; ii < 20; ii++) { E_old = E; @@ -118,45 +111,31 @@ double Beidou_Dnav_Ephemeris::sv_clock_relativistic_term(double transmitTime) double Beidou_Dnav_Ephemeris::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 ---------------------------------------------- - + // Find satellite's position ----------------------------------------------- // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; + const double a = d_sqrt_A * d_sqrt_A; // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe); + double tk = check_t(transmitTime - d_Toe); // Computed mean motion - n0 = sqrt(BEIDOU_GM / (a * a * a)); + const double n0 = sqrt(BEIDOU_GM / (a * a * a)); // Corrected mean motion - n = n0 + d_Delta_n; + const double n = n0 + d_Delta_n; // Mean anomaly - M = d_M_0 + n * tk; + double M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; + double E = M; + double E_old; + double dE; - // --- Iteratively compute eccentric anomaly ---------------------------- + // --- Iteratively compute eccentric anomaly ------------------------------- for (int ii = 1; ii < 20; ii++) { E_old = E; @@ -170,27 +149,27 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime) } // Compute the true anomaly - double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); - double tmp_X = cos(E) - d_eccentricity; - nu = atan2(tmp_Y, tmp_X); + const double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); + const double tmp_X = cos(E) - d_eccentricity; + const double nu = atan2(tmp_Y, tmp_X); // Compute angle phi (argument of Latitude) - phi = nu + d_OMEGA; + double phi = nu + d_OMEGA; // Reduce phi to between 0 and 2*pi rad phi = fmod((phi), (2.0 * GNSS_PI)); // Correct argument of latitude - u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); + const double u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); // Correct radius - r = a * (1.0 - d_eccentricity * cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); + const double r = a * (1.0 - d_eccentricity * cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); // Correct inclination - i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi); + const double i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi); // Compute the angle between the ascending node and the Greenwich meridian - Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe; + double Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe; // Reduce to between 0 and 2*pi rad Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); @@ -201,7 +180,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime) d_satpos_Z = sin(u) * r * sin(i); // Satellite's velocity. Can be useful for Vector Tracking loops - double Omega_dot = d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT; + const double Omega_dot = d_OMEGA_DOT - BEIDOU_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); diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.cc b/src/core/system_parameters/beidou_dnav_navigation_message.cc index 9979f4877..33e83e05f 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.cc +++ b/src/core/system_parameters/beidou_dnav_navigation_message.cc @@ -29,7 +29,7 @@ Beidou_Dnav_Navigation_Message::Beidou_Dnav_Navigation_Message() { auto gnss_sat = Gnss_Satellite(); - std::string _system("Beidou"); + const std::string _system("Beidou"); for (uint32_t i = 1; i < 36; i++) { satelliteBlock[i] = gnss_sat.what_block(_system, i); @@ -70,7 +70,7 @@ uint64_t Beidou_Dnav_Navigation_Message::read_navigation_unsigned( const std::vector>& parameter) const { uint64_t value = 0ULL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) @@ -91,7 +91,7 @@ int64_t Beidou_Dnav_Navigation_Message::read_navigation_signed( const std::vector>& parameter) const { int64_t value = 0; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); // read the MSB and perform the sign extension if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) @@ -121,9 +121,8 @@ int64_t Beidou_Dnav_Navigation_Message::read_navigation_signed( double Beidou_Dnav_Navigation_Message::check_t(double time) { - double corrTime; - double half_week = 302400; // seconds - corrTime = time; + const double half_week = 302400; // seconds + double corrTime = time; if (time > half_week) { corrTime = time - 2 * half_week; @@ -138,8 +137,7 @@ double Beidou_Dnav_Navigation_Message::check_t(double time) double Beidou_Dnav_Navigation_Message::sv_clock_correction(double transmitTime) { - double dt; - dt = check_t(transmitTime - d_Toc); + const double 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; @@ -148,45 +146,30 @@ double Beidou_Dnav_Navigation_Message::sv_clock_correction(double transmitTime) void Beidou_Dnav_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 ---------------------------------------------- - + // Find satellite's position ----------------------------------------------- // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; + const double a = d_sqrt_A * d_sqrt_A; // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe_sf2); + const double tk = check_t(transmitTime - d_Toe_sf2); // Computed mean motion - n0 = sqrt(BEIDOU_GM / (a * a * a)); + const double n0 = sqrt(BEIDOU_GM / (a * a * a)); // Corrected mean motion - n = n0 + d_Delta_n; + const double n = n0 + d_Delta_n; // Mean anomaly - M = d_M_0 + n * tk; + double M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; - - // --- Iteratively compute eccentric anomaly ---------------------------- + double E = M; + double E_old; + double dE; + // --- Iteratively compute eccentric anomaly ------------------------------- for (int32_t ii = 1; ii < 20; ii++) { E_old = E; @@ -203,27 +186,27 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime) d_dtr = BEIDOU_F * d_eccentricity * d_sqrt_A * sin(E); // Compute the true anomaly - double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); - double tmp_X = cos(E) - d_eccentricity; - nu = atan2(tmp_Y, tmp_X); + const double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); + const double tmp_X = cos(E) - d_eccentricity; + const double nu = atan2(tmp_Y, tmp_X); // Compute angle phi (argument of Latitude) - phi = nu + d_OMEGA; + double phi = nu + d_OMEGA; // Reduce phi to between 0 and 2*pi rad phi = fmod((phi), (2 * GNSS_PI)); // Correct argument of latitude - u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi); + const double u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi); // Correct radius - r = a * (1 - d_eccentricity * cos(E)) + d_Crc * cos(2 * phi) + d_Crs * sin(2 * phi); + const double r = a * (1 - d_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); + const double 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 - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe_sf2; + double Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe_sf2; // Reduce to between 0 and 2*pi rad Omega = fmod((Omega + 2 * GNSS_PI), (2 * GNSS_PI)); @@ -234,7 +217,7 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime) d_satpos_Z = sin(u) * r * sin(i); // Satellite's velocity. Can be useful for Vector Tracking loops - double Omega_dot = d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT; + const double Omega_dot = d_OMEGA_DOT - BEIDOU_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); @@ -243,10 +226,8 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime) int32_t Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const& subframe) { - int32_t subframe_ID = 0; - std::bitset subframe_bits(subframe); - - subframe_ID = static_cast(read_navigation_unsigned(subframe_bits, D1_FRAID)); + const std::bitset subframe_bits(subframe); + const auto subframe_ID = static_cast(read_navigation_unsigned(subframe_bits, D1_FRAID)); // Perform crc computation (tbd) flag_crc_test = true; @@ -422,7 +403,6 @@ int32_t Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const& s int32_t SV_page_5; d_SOW_SF5 = static_cast(read_navigation_unsigned(subframe_bits, D1_SOW)); d_SOW = d_SOW_SF5; // Set transmission time - SV_page_5 = static_cast(read_navigation_unsigned(subframe_bits, D1_PNUM)); if (SV_page_5 < 7) @@ -538,13 +518,10 @@ int32_t Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const& s int32_t Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const& subframe) { - int32_t subframe_ID = 0; - int32_t page_ID = 0; + const std::bitset subframe_bits(subframe); - std::bitset subframe_bits(subframe); - - subframe_ID = static_cast(read_navigation_unsigned(subframe_bits, D2_FRAID)); - page_ID = static_cast(read_navigation_unsigned(subframe_bits, D2_PNUM)); + const auto subframe_ID = static_cast(read_navigation_unsigned(subframe_bits, D2_FRAID)); + const auto page_ID = static_cast(read_navigation_unsigned(subframe_bits, D2_PNUM)); // Perform crc computation (tbd) flag_crc_test = true; @@ -730,15 +707,15 @@ double Beidou_Dnav_Navigation_Message::utc_time(const double beidoutime_correcte { double t_utc; double t_utc_daytime; - double Delta_t_UTC = i_DeltaT_LS + d_A0UTC + d_A1UTC * (beidoutime_corrected); + const double Delta_t_UTC = i_DeltaT_LS + d_A0UTC + d_A1UTC * (beidoutime_corrected); // Determine if the effectivity time of the leap second event is in the past - int32_t weeksToLeapSecondEvent = i_WN_LSF - i_BEIDOU_week; + const int32_t weeksToLeapSecondEvent = i_WN_LSF - i_BEIDOU_week; if ((weeksToLeapSecondEvent) >= 0) // is not in the past { // Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s - int32_t secondOfLeapSecondEvent = i_DN * 24 * 60 * 60; + const int32_t secondOfLeapSecondEvent = i_DN * 24 * 60 * 60; if (weeksToLeapSecondEvent > 0) { t_utc_daytime = fmod(beidoutime_corrected - Delta_t_UTC, 86400); @@ -753,7 +730,7 @@ double Beidou_Dnav_Navigation_Message::utc_time(const double beidoutime_correcte { if ((beidoutime_corrected - secondOfLeapSecondEvent) < (static_cast(5) / static_cast(4)) * 24 * 60 * 60) { - int32_t W = fmod(beidoutime_corrected - Delta_t_UTC - 43200, 86400) + 43200; + const int32_t W = fmod(beidoutime_corrected - Delta_t_UTC - 43200, 86400) + 43200; t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - i_DeltaT_LS); } else @@ -768,7 +745,7 @@ double Beidou_Dnav_Navigation_Message::utc_time(const double beidoutime_correcte t_utc_daytime = fmod(beidoutime_corrected - Delta_t_UTC, 86400); } - double secondsOfWeekBeforeToday = 43200 * floor(beidoutime_corrected / 43200); + const double secondsOfWeekBeforeToday = 43200 * floor(beidoutime_corrected / 43200); t_utc = secondsOfWeekBeforeToday + t_utc_daytime; return t_utc; } diff --git a/src/core/system_parameters/galileo_ephemeris.cc b/src/core/system_parameters/galileo_ephemeris.cc index 68e92a540..d14d03d65 100644 --- a/src/core/system_parameters/galileo_ephemeris.cc +++ b/src/core/system_parameters/galileo_ephemeris.cc @@ -50,10 +50,9 @@ double Galileo_Ephemeris::Galileo_System_Time(double WN, double TOW) message provided through the TOW is synchronised to each satellite’s version of Galileo System Time (GST). * */ - double t = 0; - double sec_in_day = 86400; - double day_in_week = 7; - t = WN * sec_in_day * day_in_week + TOW; // second from the origin of the Galileo time + const double sec_in_day = 86400; + const double day_in_week = 7; + double t = WN * sec_in_day * day_in_week + TOW; // second from the origin of the Galileo time return t; } @@ -61,8 +60,7 @@ double Galileo_Ephemeris::Galileo_System_Time(double WN, double TOW) double Galileo_Ephemeris::sv_clock_drift(double transmitTime) { // Satellite Time Correction Algorithm, ICD 5.1.4 - double dt; - dt = transmitTime - t0c_4; + const double dt = transmitTime - t0c_4; Galileo_satClkDrift = af0_4 + af1_4 * dt + af2_4 * (dt * dt) + sv_clock_relativistic_term(transmitTime); // +Galileo_dtr; return Galileo_satClkDrift; } @@ -71,35 +69,28 @@ double Galileo_Ephemeris::sv_clock_drift(double transmitTime) // compute the relativistic correction term double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Satellite Time Correction Algorithm, ICD 5.1.4 { - double tk; - double a; - double n; - double n0; - double E; - double E_old; - double dE; - double M; - // Restore semi-major axis - a = A_1 * A_1; + const double a = A_1 * A_1; - n0 = sqrt(GALILEO_GM / (a * a * a)); + const double n0 = sqrt(GALILEO_GM / (a * a * a)); // Time from ephemeris reference epoch // t = WN_5*86400*7 + TOW_5; //WN_5*86400*7 are the second from the origin of the Galileo time - tk = transmitTime - t0e_1; + const double tk = transmitTime - t0e_1; // Corrected mean motion - n = n0 + delta_n_3; + const double n = n0 + delta_n_3; // Mean anomaly - M = M0_1 + n * tk; + double M = M0_1 + n * tk; // Reduce mean anomaly to between 0 and 2pi M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; + double E = M; + double E_old; + double dE; // --- Iteratively compute eccentric anomaly ---------------------------- for (int32_t ii = 1; ii < 20; ii++) @@ -122,46 +113,35 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Sa void Galileo_Ephemeris::satellitePosition(double transmitTime) { - // when this function in used, the input must be the transmitted time (t) in second computed by Galileo_System_Time (above function) - double tk; // Time from ephemeris reference epoch - double a; // Semi-major axis - double n; // Corrected mean motion - double n0; // Computed mean motion - double M; // Mean anomaly - double E; // Eccentric Anomaly (to be solved by iteration) - double E_old; - double dE; - double nu; // True anomaly - double phi; // Argument of Latitude - double u; // Correct argument of latitude - double r; // Correct radius - double i; - double Omega; + // when this function in used, the input must be the transmitted time (t) in + // seconds computed by Galileo_System_Time (above function) - // Find Galileo satellite's position ---------------------------------------------- + // Find Galileo satellite's position --------------------------------------- // Restore semi-major axis - a = A_1 * A_1; + const double a = A_1 * A_1; // Computed mean motion - n0 = sqrt(GALILEO_GM / (a * a * a)); + const double n0 = sqrt(GALILEO_GM / (a * a * a)); // Time from ephemeris reference epoch - tk = transmitTime - t0e_1; + const double tk = transmitTime - t0e_1; // Corrected mean motion - n = n0 + delta_n_3; + const double n = n0 + delta_n_3; // Mean anomaly - M = M0_1 + n * tk; + double M = M0_1 + n * tk; // Reduce mean anomaly to between 0 and 2pi M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; + double E = M; + double E_old; + double dE; - // --- Iteratively compute eccentric anomaly ---------------------------- + // --- Iteratively compute eccentric anomaly ------------------------------- for (int32_t ii = 1; ii < 20; ii++) { E_old = E; @@ -175,28 +155,27 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime) } // Compute the true anomaly - - double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E); - double tmp_X = cos(E) - e_1; - nu = atan2(tmp_Y, tmp_X); + const double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E); + const double tmp_X = cos(E) - e_1; + const double nu = atan2(tmp_Y, tmp_X); // Compute angle phi (argument of Latitude) - phi = nu + omega_2; + double phi = nu + omega_2; // Reduce phi to between 0 and 2*pi rad phi = fmod((phi), (2 * GNSS_PI)); // Correct argument of latitude - u = phi + C_uc_3 * cos(2 * phi) + C_us_3 * sin(2 * phi); + const double u = phi + C_uc_3 * cos(2 * phi) + C_us_3 * sin(2 * phi); // Correct radius - r = a * (1 - e_1 * cos(E)) + C_rc_3 * cos(2 * phi) + C_rs_3 * sin(2 * phi); + const double r = a * (1 - e_1 * cos(E)) + C_rc_3 * cos(2 * phi) + C_rs_3 * sin(2 * phi); // Correct inclination - i = i_0_2 + iDot_2 * tk + C_ic_4 * cos(2 * phi) + C_is_4 * sin(2 * phi); + const double i = i_0_2 + iDot_2 * tk + C_ic_4 * cos(2 * phi) + C_is_4 * sin(2 * phi); // Compute the angle between the ascending node and the Greenwich meridian - Omega = OMEGA_0_2 + (OMEGA_dot_3 - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * t0e_1; + double Omega = OMEGA_0_2 + (OMEGA_dot_3 - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * t0e_1; // Reduce to between 0 and 2*pi rad Omega = fmod((Omega + 2 * GNSS_PI), (2 * GNSS_PI)); @@ -207,7 +186,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime) d_satpos_Z = sin(u) * r * sin(i); // Satellite's velocity. Can be useful for Vector Tracking loops - double Omega_dot = OMEGA_dot_3 - GNSS_OMEGA_EARTH_DOT; + const double Omega_dot = OMEGA_dot_3 - GNSS_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); diff --git a/src/core/system_parameters/galileo_fnav_message.cc b/src/core/system_parameters/galileo_fnav_message.cc index 672b3b574..3ccf37101 100644 --- a/src/core/system_parameters/galileo_fnav_message.cc +++ b/src/core/system_parameters/galileo_fnav_message.cc @@ -36,10 +36,10 @@ using CRC_Galileo_FNAV_type = boost::crc_optimal<24, 0x1864CFBU, 0x0, 0x0, false void Galileo_Fnav_Message::split_page(const std::string& page_string) { - std::string message_word = page_string.substr(0, 214); - std::string CRC_data = page_string.substr(214, 24); - std::bitset Word_for_CRC_bits(message_word); - std::bitset<24> checksum(CRC_data); + const std::string message_word = page_string.substr(0, 214); + const std::string CRC_data = page_string.substr(214, 24); + const std::bitset Word_for_CRC_bits(message_word); + const std::bitset<24> checksum(CRC_data); if (_CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true) { flag_CRC_test = true; @@ -57,7 +57,6 @@ bool Galileo_Fnav_Message::_CRC_test(std::bitset b { CRC_Galileo_FNAV_type CRC_Galileo; - uint32_t crc_computed; // Galileo FNAV frame for CRC is not an integer multiple of bytes // it needs to be filled with zeroes at the start of the frame. // This operation is done in the transformation from bits to bytes @@ -72,7 +71,7 @@ bool Galileo_Fnav_Message::_CRC_test(std::bitset b CRC_Galileo.process_bytes(bytes.data(), GALILEO_FNAV_DATA_FRAME_BYTES); - crc_computed = CRC_Galileo.checksum(); + const uint32_t crc_computed = CRC_Galileo.checksum(); if (checksum == crc_computed) { return true; @@ -84,7 +83,7 @@ bool Galileo_Fnav_Message::_CRC_test(std::bitset b void Galileo_Fnav_Message::decode_page(const std::string& data) { - std::bitset data_bits(data); + const std::bitset data_bits(data); page_type = read_navigation_unsigned(data_bits, FNAV_PAGE_TYPE_BIT); switch (page_type) { @@ -241,9 +240,9 @@ void Galileo_Fnav_Message::decode_page(const std::string& data) FNAV_IODa_6 = static_cast(read_navigation_unsigned(data_bits, FNAV_IO_DA_6_BIT)); // Don't worry about omega pieces. If page 5 has not been received, all_ephemeris // flag will be set to false and the data won't be recorded.*/ - std::string omega0_2 = data.substr(10, 12); - std::string Omega0 = omega0_1 + omega0_2; - std::bitset omega_bits(Omega0); + const std::string omega0_2 = data.substr(10, 12); + const std::string Omega0 = omega0_1 + omega0_2; + const std::bitset omega_bits(Omega0); const std::vector> om_bit({{0, 12}}); FNAV_Omega0_2_6 = static_cast(read_navigation_signed(omega_bits, om_bit)); FNAV_Omega0_2_6 *= FNAV_OMEGA0_5_LSB; @@ -286,7 +285,7 @@ void Galileo_Fnav_Message::decode_page(const std::string& data) uint64_t Galileo_Fnav_Message::read_navigation_unsigned(std::bitset bits, const std::vector>& parameter) const { uint64_t value = 0ULL; - int num_of_slices = parameter.size(); + const int num_of_slices = parameter.size(); for (int i = 0; i < num_of_slices; i++) { for (int j = 0; j < parameter[i].second; j++) @@ -305,7 +304,7 @@ uint64_t Galileo_Fnav_Message::read_navigation_unsigned(std::bitset bits, const std::vector>& parameter) const { int64_t value = 0LL; - int num_of_slices = parameter.size(); + const int num_of_slices = parameter.size(); // read the MSB and perform the sign extension if (static_cast(bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[0].first]) == 1) diff --git a/src/core/system_parameters/galileo_navigation_message.cc b/src/core/system_parameters/galileo_navigation_message.cc index 360114db4..8bb37fe80 100644 --- a/src/core/system_parameters/galileo_navigation_message.cc +++ b/src/core/system_parameters/galileo_navigation_message.cc @@ -35,7 +35,6 @@ bool Galileo_Navigation_Message::CRC_test(std::bitset b { CRC_Galileo_INAV_type CRC_Galileo; - uint32_t crc_computed; // Galileo INAV frame for CRC is not an integer multiple of bytes // it needs to be filled with zeroes at the start of the frame. // This operation is done in the transformation from bits to bytes @@ -50,7 +49,7 @@ bool Galileo_Navigation_Message::CRC_test(std::bitset b CRC_Galileo.process_bytes(bytes.data(), GALILEO_DATA_FRAME_BYTES); - crc_computed = CRC_Galileo.checksum(); + const uint32_t crc_computed = CRC_Galileo.checksum(); if (checksum == crc_computed) { return true; @@ -62,7 +61,7 @@ bool Galileo_Navigation_Message::CRC_test(std::bitset b uint64_t Galileo_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector >& parameter) const { uint64_t value = 0ULL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) @@ -81,7 +80,7 @@ uint64_t Galileo_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector >& parameter) const { uint64_t value = 0ULL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) @@ -100,7 +99,7 @@ uint64_t Galileo_Navigation_Message::read_page_type_unsigned(std::bitset bits, const std::vector >& parameter) const { int64_t value = 0LL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); // read the MSB and perform the sign extension if (static_cast(bits[GALILEO_DATA_JK_BITS - parameter[0].first]) == 1) @@ -153,41 +152,40 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int32_t fla if (flag_even_word == 1) // An odd page has been received but the previous even page is kept in memory and it is considered to join pages { - std::string page_INAV_even = page_Even; - std::string page_INAV = page_INAV_even + page_Odd; // Join pages: Even + Odd = INAV page - std::string Even_bit = page_INAV.substr(0, 1); - std::string Page_type_even = page_INAV.substr(1, 1); - std::string nominal = "0"; + const std::string page_INAV_even = page_Even; + const std::string page_INAV = page_INAV_even + page_Odd; // Join pages: Even + Odd = INAV page + const std::string Even_bit = page_INAV.substr(0, 1); + const std::string Page_type_even = page_INAV.substr(1, 1); + const std::string nominal = "0"; - std::string Data_k = page_INAV.substr(2, 112); - std::string Odd_bit = page_INAV.substr(114, 1); - std::string Page_type_Odd = page_INAV.substr(115, 1); - std::string Data_j = page_INAV.substr(116, 16); + const std::string Data_k = page_INAV.substr(2, 112); + const std::string Odd_bit = page_INAV.substr(114, 1); + const std::string Page_type_Odd = page_INAV.substr(115, 1); + const std::string Data_j = page_INAV.substr(116, 16); - std::string Reserved_1 = page_INAV.substr(132, 40); - std::string SAR = page_INAV.substr(172, 22); - std::string Spare = page_INAV.substr(194, 2); - std::string CRC_data = page_INAV.substr(196, 24); - std::string Reserved_2 = page_INAV.substr(220, 8); - std::string Tail_odd = page_INAV.substr(228, 6); + const std::string Reserved_1 = page_INAV.substr(132, 40); + const std::string SAR = page_INAV.substr(172, 22); + const std::string Spare = page_INAV.substr(194, 2); + const std::string CRC_data = page_INAV.substr(196, 24); + const std::string Reserved_2 = page_INAV.substr(220, 8); + const std::string Tail_odd = page_INAV.substr(228, 6); // ************ CRC checksum control *******/ std::stringstream TLM_word_for_CRC_stream; TLM_word_for_CRC_stream << page_INAV; - std::string TLM_word_for_CRC; - TLM_word_for_CRC = TLM_word_for_CRC_stream.str().substr(0, GALILEO_DATA_FRAME_BITS); - std::bitset TLM_word_for_CRC_bits(TLM_word_for_CRC); - std::bitset<24> checksum(CRC_data); + const std::string TLM_word_for_CRC = TLM_word_for_CRC_stream.str().substr(0, GALILEO_DATA_FRAME_BITS); + const std::bitset TLM_word_for_CRC_bits(TLM_word_for_CRC); + const std::bitset<24> checksum(CRC_data); if (CRC_test(TLM_word_for_CRC_bits, checksum.to_ulong()) == true) { flag_CRC_test = true; // CRC correct: Decode word - std::string page_number_bits = Data_k.substr(0, 6); - std::bitset page_type_bits(page_number_bits); // from string to bitset + const std::string page_number_bits = Data_k.substr(0, 6); + const std::bitset page_type_bits(page_number_bits); // from string to bitset Page_type = static_cast(read_page_type_unsigned(page_type_bits, TYPE)); Page_type_time_stamp = Page_type; - std::string Data_jk_ephemeris = Data_k + Data_j; + const std::string Data_jk_ephemeris = Data_k + Data_j; page_jk_decoder(Data_jk_ephemeris.c_str()); } else @@ -200,7 +198,6 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int32_t fla else { page_Even = page_string.substr(0, 114); - std::string tail_Even = page_string.substr(114, 6); } } @@ -424,12 +421,10 @@ Galileo_Almanac_Helper Galileo_Navigation_Message::get_almanac() const int32_t Galileo_Navigation_Message::page_jk_decoder(const char* data_jk) { - int32_t page_number = 0; + const std::string data_jk_string = data_jk; + const std::bitset data_jk_bits(data_jk_string); - std::string data_jk_string = data_jk; - std::bitset data_jk_bits(data_jk_string); - - page_number = static_cast(read_navigation_unsigned(data_jk_bits, PAGE_TYPE_BIT)); + const auto page_number = static_cast(read_navigation_unsigned(data_jk_bits, PAGE_TYPE_BIT)); DLOG(INFO) << "Page number = " << page_number; switch (page_number) diff --git a/src/core/system_parameters/galileo_utc_model.cc b/src/core/system_parameters/galileo_utc_model.cc index f0d40b7dc..c95cc1457 100644 --- a/src/core/system_parameters/galileo_utc_model.cc +++ b/src/core/system_parameters/galileo_utc_model.cc @@ -27,12 +27,12 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int32_t WN) double t_Utc_daytime; double Delta_t_Utc = 0; // Determine if the effectivity time of the leap second event is in the past - int32_t weeksToLeapSecondEvent = WN_LSF_6 - (WN % 256); + const int32_t weeksToLeapSecondEvent = WN_LSF_6 - (WN % 256); if ((weeksToLeapSecondEvent) >= 0) // is not in the past { // Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s - int secondOfLeapSecondEvent = DN_6 * 24 * 60 * 60; + const int secondOfLeapSecondEvent = DN_6 * 24 * 60 * 60; if (std::abs(t_e - secondOfLeapSecondEvent) > 21600) { /* 5.1.7a GST->UTC case a @@ -53,7 +53,7 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int32_t WN) * the effective time is computed according to the following equations: */ Delta_t_Utc = Delta_tLS_6 + A0_6 + A1_6 * (t_e - t0t_6 + 604800 * static_cast((WN % 256) - WNot_6)); - double W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200; + const double W = fmod(t_e - Delta_t_Utc - 43200, 86400) + 43200; t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6); // implement something to handle a leap second event! } @@ -71,7 +71,7 @@ double Galileo_Utc_Model::GST_to_UTC_time(double t_e, int32_t WN) t_Utc_daytime = fmod(t_e - Delta_t_Utc, 86400); } - double secondsOfWeekBeforeToday = 86400 * floor(t_e / 86400); + const double secondsOfWeekBeforeToday = 86400 * floor(t_e / 86400); t_Utc = secondsOfWeekBeforeToday + t_Utc_daytime; return t_Utc; } diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index e28f67ba1..1e30ab188 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -43,14 +43,6 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset string_bits(GLONASS_GNAV_STRING_BITS); // Populate data and hamming code vectors @@ -65,7 +57,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset bits, const std::vector>& parameter) const { uint64_t value = 0ULL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) @@ -184,7 +175,7 @@ int64_t Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bitset string_bits(frame_string); + const std::bitset string_bits(frame_string); // Perform data verification and exit code if error in bit sequence flag_CRC_test = CRC_test(string_bits); diff --git a/src/core/system_parameters/glonass_gnav_utc_model.cc b/src/core/system_parameters/glonass_gnav_utc_model.cc index 3d2b3eddb..abe6de2e4 100644 --- a/src/core/system_parameters/glonass_gnav_utc_model.cc +++ b/src/core/system_parameters/glonass_gnav_utc_model.cc @@ -23,10 +23,8 @@ double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected) { - double t_utc; - // GLONASS Time is relative to UTC Moscow, so we simply add its time difference - t_utc = glonass_time_corrected + 3.0 * 3600.0 + d_tau_c; + double t_utc = glonass_time_corrected + 3.0 * 3600.0 + d_tau_c; return t_utc; } diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 3ec31bb3e..2cc9eb8a9 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -242,8 +242,7 @@ void Gnss_Satellite::set_PRN(uint32_t PRN_) int32_t Gnss_Satellite::get_rf_link() const { // Get satellite's rf link. Identifies the GLONASS Frequency Channel - int32_t rf_link_; - rf_link_ = rf_link; + int32_t rf_link_ = rf_link; return rf_link_; } @@ -258,8 +257,7 @@ void Gnss_Satellite::set_rf_link(int32_t rf_link_) uint32_t Gnss_Satellite::get_PRN() const { // Get satellite's PRN - uint32_t PRN_; - PRN_ = PRN; + uint32_t PRN_ = PRN; return PRN_; } @@ -267,8 +265,7 @@ uint32_t Gnss_Satellite::get_PRN() const std::string Gnss_Satellite::get_system() const { // Get the satellite system {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"} - std::string system_; - system_ = system; + std::string system_ = system; return system_; } @@ -283,8 +280,7 @@ std::string Gnss_Satellite::get_system_short() const std::string Gnss_Satellite::get_block() const { // Get the satellite block - std::string block_; - block_ = block; + std::string block_ = block; return block_; } diff --git a/src/core/system_parameters/gps_cnav_ephemeris.cc b/src/core/system_parameters/gps_cnav_ephemeris.cc index 243cba041..c6809e5bc 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.cc +++ b/src/core/system_parameters/gps_cnav_ephemeris.cc @@ -26,9 +26,8 @@ double Gps_CNAV_Ephemeris::check_t(double time) { - double corrTime; - double half_week = 302400.0; // seconds - corrTime = time; + const double half_week = 302400.0; // seconds + double corrTime = time; if (time > half_week) { corrTime = time - 2.0 * half_week; @@ -44,8 +43,7 @@ double Gps_CNAV_Ephemeris::check_t(double time) // 20.3.3.3.3.1 User Algorithm for SV Clock Correction. double Gps_CNAV_Ephemeris::sv_clock_drift(double transmitTime) { - double dt; - dt = check_t(transmitTime - d_Toc); + const double dt = check_t(transmitTime - d_Toc); d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime); // Correct satellite group delay @@ -58,35 +56,31 @@ double Gps_CNAV_Ephemeris::sv_clock_drift(double transmitTime) // compute the relativistic correction term double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime) { - double tk; - double a; - double n; - double n0; - double E; - double E_old; - double dE; - double M; const double A_REF = 26559710.0; // See IS-GPS-200K, pp. 163 - double d_sqrt_A = sqrt(A_REF + d_DELTA_A); + const double d_sqrt_A = sqrt(A_REF + d_DELTA_A); // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; + const double a = d_sqrt_A * d_sqrt_A; // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe1); + const double tk = check_t(transmitTime - d_Toe1); // Computed mean motion - n0 = sqrt(GPS_GM / (a * a * a)); + const double n0 = sqrt(GPS_GM / (a * a * a)); + // Corrected mean motion - n = n0 + d_Delta_n; + const double n = n0 + d_Delta_n; + // Mean anomaly - M = d_M_0 + n * tk; + const double M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi // M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; + double E = M; + double E_old; + double dE; // --- Iteratively compute eccentric anomaly ------------------------------- for (int32_t ii = 1; ii < 20; ii++) @@ -109,52 +103,38 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime) double Gps_CNAV_Ephemeris::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; - const double A_REF = 26559710.0; // See IS-GPS-200K, pp. 170 const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200K pp. 164 - double d_sqrt_A = sqrt(A_REF + d_DELTA_A); + const double d_sqrt_A = sqrt(A_REF + d_DELTA_A); // Find satellite's position ----------------------------------------------- // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; + const double a = d_sqrt_A * d_sqrt_A; // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe1); + double tk = check_t(transmitTime - d_Toe1); // Computed mean motion - n0 = sqrt(GPS_GM / (a * a * a)); + const double n0 = sqrt(GPS_GM / (a * a * a)); // Mean motion difference from computed value - double delta_n_a = d_Delta_n + 0.5 * d_DELTA_DOT_N * tk; + const double delta_n_a = d_Delta_n + 0.5 * d_DELTA_DOT_N * tk; // Corrected mean motion - n = n0 + delta_n_a; + const double n = n0 + delta_n_a; // Mean anomaly - M = d_M_0 + n * tk; + const double M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi // M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; - + double E = M; + double E_old; + double dE; // --- Iteratively compute eccentric anomaly ------------------------------- for (int32_t ii = 1; ii < 20; ii++) { @@ -169,28 +149,28 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) } // 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); + const double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); + const double tmp_X = cos(E) - d_e_eccentricity; + const double nu = atan2(tmp_Y, tmp_X); // Compute angle phi (argument of Latitude) - phi = nu + d_OMEGA; + const double phi = nu + d_OMEGA; // Reduce phi to between 0 and 2*pi rad // phi = fmod((phi), (2*GNSS_PI)); // Correct argument of latitude - u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi); + const double 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); + const double 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); + const double 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 - double d_OMEGA_DOT = OMEGA_DOT_REF * GNSS_PI + d_DELTA_OMEGA_DOT; - Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe1; + const double d_OMEGA_DOT = OMEGA_DOT_REF * GNSS_PI + d_DELTA_OMEGA_DOT; + const double Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe1; // Reduce to between 0 and 2*pi rad // Omega = fmod((Omega + 2*GNSS_PI), (2*GNSS_PI)); @@ -201,7 +181,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) d_satpos_Z = sin(u) * r * sin(i); // Satellite's velocity. Can be useful for Vector Tracking loops - double Omega_dot = d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT; + const double Omega_dot = d_OMEGA_DOT - GNSS_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); diff --git a/src/core/system_parameters/gps_cnav_navigation_message.cc b/src/core/system_parameters/gps_cnav_navigation_message.cc index 1e63821b2..18467ea0b 100644 --- a/src/core/system_parameters/gps_cnav_navigation_message.cc +++ b/src/core/system_parameters/gps_cnav_navigation_message.cc @@ -55,7 +55,7 @@ bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector>& parameter) const { uint64_t value = 0ULL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) @@ -74,7 +74,7 @@ uint64_t Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector>& parameter) const { int64_t value = 0LL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); // read the MSB and perform the sign extension if (static_cast(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first]) == 1) @@ -104,12 +104,11 @@ int64_t Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset data_bits) { - int32_t PRN; int32_t page_type; bool alert_flag; // common to all messages - PRN = static_cast(read_navigation_unsigned(data_bits, CNAV_PRN)); + const auto PRN = static_cast(read_navigation_unsigned(data_bits, CNAV_PRN)); ephemeris_record.i_satellite_PRN = PRN; d_TOW = static_cast(read_navigation_unsigned(data_bits, CNAV_TOW)); diff --git a/src/core/system_parameters/gps_cnav_utc_model.cc b/src/core/system_parameters/gps_cnav_utc_model.cc index 9df57c3b2..125baa210 100644 --- a/src/core/system_parameters/gps_cnav_utc_model.cc +++ b/src/core/system_parameters/gps_cnav_utc_model.cc @@ -28,12 +28,12 @@ double Gps_CNAV_Utc_Model::utc_time(double gpstime_corrected, int32_t i_GPS_week double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast(i_GPS_week - i_WN_T)); // Determine if the effectivity time of the leap second event is in the past - int32_t weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week; + const int32_t weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week; if (weeksToLeapSecondEvent >= 0) // is not in the past { // Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s - int32_t secondOfLeapSecondEvent = i_DN * 24 * 60 * 60; + const int32_t secondOfLeapSecondEvent = i_DN * 24 * 60 * 60; if (weeksToLeapSecondEvent > 0) { t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); @@ -59,7 +59,7 @@ double Gps_CNAV_Utc_Model::utc_time(double gpstime_corrected, int32_t i_GPS_week * proper accommodation of the leap second event with a possible week number * transition is provided by the following expression for UTC: */ - int32_t W = static_cast(fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400)) + 43200; + const int32_t W = static_cast(fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400)) + 43200; t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS); // implement something to handle a leap second event! } @@ -78,12 +78,12 @@ double Gps_CNAV_Utc_Model::utc_time(double gpstime_corrected, int32_t i_GPS_week * and the user's current time does not fall in the time span as given above * in 20.3.3.5.2.4b,*/ /* FOR CNAV: Replace the 20.3.3.5.2.4c with 30.3.3.6.2 UTC and GPS Time as follows */ - double tmp_d = (gpstime_corrected - d_t_OT + 604800 * static_cast(i_GPS_week - i_WN_T)); + const double tmp_d = (gpstime_corrected - d_t_OT + 604800 * static_cast(i_GPS_week - i_WN_T)); Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * tmp_d + d_A2 * tmp_d * tmp_d; t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); } - double secondsOfWeekBeforeToday = 86400 * floor(gpstime_corrected / 86400); + const double secondsOfWeekBeforeToday = 86400 * floor(gpstime_corrected / 86400); t_utc = secondsOfWeekBeforeToday + t_utc_daytime; return t_utc; } diff --git a/src/core/system_parameters/gps_ephemeris.cc b/src/core/system_parameters/gps_ephemeris.cc index 0d3c69627..cd1105e9a 100644 --- a/src/core/system_parameters/gps_ephemeris.cc +++ b/src/core/system_parameters/gps_ephemeris.cc @@ -28,7 +28,7 @@ Gps_Ephemeris::Gps_Ephemeris() { auto gnss_sat = Gnss_Satellite(); - std::string _system("GPS"); + const std::string _system("GPS"); for (uint32_t i = 1; i < 33; i++) { satelliteBlock[i] = gnss_sat.what_block(_system, i); @@ -38,9 +38,8 @@ Gps_Ephemeris::Gps_Ephemeris() double Gps_Ephemeris::check_t(double time) { - double corrTime; - double half_week = 302400.0; // seconds - corrTime = time; + const double half_week = 302400.0; // seconds + double corrTime = time; if (time > half_week) { corrTime = time - 2.0 * half_week; @@ -65,8 +64,7 @@ double Gps_Ephemeris::sv_clock_drift(double transmitTime) // } // d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); - double dt; - dt = check_t(transmitTime - d_Toc); + const double dt = check_t(transmitTime - d_Toc); d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime); // Correct satellite group delay d_satClkDrift -= d_TGD; @@ -78,34 +76,28 @@ double Gps_Ephemeris::sv_clock_drift(double transmitTime) // compute the relativistic correction term double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime) { - double tk; - double a; - double n; - double n0; - double E; - double E_old; - double dE; - double M; - // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; + const double a = d_sqrt_A * d_sqrt_A; // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe); + const double tk = check_t(transmitTime - d_Toe); // Computed mean motion - n0 = sqrt(GPS_GM / (a * a * a)); + const double n0 = sqrt(GPS_GM / (a * a * a)); + // Corrected mean motion - n = n0 + d_Delta_n; + const double n = n0 + d_Delta_n; + // Mean anomaly - M = d_M_0 + n * tk; + const double M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi // M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; - + double E = M; + double E_old; + double dE; // --- Iteratively compute eccentric anomaly ---------------------------- for (int32_t ii = 1; ii < 20; ii++) { @@ -127,45 +119,30 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime) double Gps_Ephemeris::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 ---------------------------------------------- - + // Find satellite's position ----------------------------------------------- // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; + const double a = d_sqrt_A * d_sqrt_A; // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe); + double tk = check_t(transmitTime - d_Toe); // Computed mean motion - n0 = sqrt(GPS_GM / (a * a * a)); + const double n0 = sqrt(GPS_GM / (a * a * a)); // Corrected mean motion - n = n0 + d_Delta_n; + const double n = n0 + d_Delta_n; // Mean anomaly - M = d_M_0 + n * tk; + const double M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi // M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // Initial guess of eccentric anomaly - E = M; - - // --- Iteratively compute eccentric anomaly ---------------------------- + double E = M; + double E_old; + double dE; + // --- Iteratively compute eccentric anomaly ------------------------------- for (int32_t ii = 1; ii < 20; ii++) { E_old = E; @@ -179,27 +156,27 @@ double Gps_Ephemeris::satellitePosition(double transmitTime) } // 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); + const double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); + const double tmp_X = cos(E) - d_e_eccentricity; + const double nu = atan2(tmp_Y, tmp_X); // Compute angle phi (argument of Latitude) - phi = nu + d_OMEGA; + const double phi = nu + d_OMEGA; // Reduce phi to between 0 and 2*pi rad // phi = fmod((phi), (2.0 * GNSS_PI)); // Correct argument of latitude - u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); + const double u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); // Correct radius - r = a * (1.0 - d_e_eccentricity * cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); + const double r = a * (1.0 - d_e_eccentricity * cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); // Correct inclination - i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi); + const double i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi); // Compute the angle between the ascending node and the Greenwich meridian - Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe; + const double Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe; // Reduce to between 0 and 2*pi rad // Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); @@ -210,7 +187,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime) d_satpos_Z = sin(u) * r * sin(i); // Satellite's velocity. Can be useful for Vector Tracking loops - double Omega_dot = d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT; + const double Omega_dot = d_OMEGA_DOT - GNSS_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); diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index 4bde9386a..c2a4c23c8 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -31,7 +31,7 @@ Gps_Navigation_Message::Gps_Navigation_Message() { auto gnss_sat = Gnss_Satellite(); - std::string _system("GPS"); + const std::string _system("GPS"); for (uint32_t i = 1; i < 33; i++) { satelliteBlock[i] = gnss_sat.what_block(_system, i); @@ -68,7 +68,7 @@ bool Gps_Navigation_Message::read_navigation_bool(std::bitset uint64_t Gps_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector>& parameter) const { uint64_t value = 0ULL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); for (int32_t i = 0; i < num_of_slices; i++) { for (int32_t j = 0; j < parameter[i].second; j++) @@ -87,7 +87,7 @@ uint64_t Gps_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector>& parameter) const { int64_t value = 0LL; - int32_t num_of_slices = parameter.size(); + const int32_t num_of_slices = parameter.size(); // read the MSB and perform the sign extension if (static_cast(bits[GPS_SUBFRAME_BITS - parameter[0].first]) == 1) @@ -117,7 +117,6 @@ int64_t Gps_Navigation_Message::read_navigation_signed(std::bitset(read_navigation_unsigned(subframe_bits, SUBFRAME_ID)); + const auto subframe_ID = static_cast(read_navigation_unsigned(subframe_bits, SUBFRAME_ID)); // Decode all 5 sub-frames switch (subframe_ID) @@ -368,12 +367,12 @@ double Gps_Navigation_Message::utc_time(const double gpstime_corrected) const double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast((i_GPS_week - i_WN_T))); // Determine if the effectivity time of the leap second event is in the past - int32_t weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week; + const int32_t weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week; if ((weeksToLeapSecondEvent) >= 0) // is not in the past { // Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s - int32_t secondOfLeapSecondEvent = i_DN * 24 * 60 * 60; + const int32_t secondOfLeapSecondEvent = i_DN * 24 * 60 * 60; if (weeksToLeapSecondEvent > 0) { t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); @@ -399,7 +398,7 @@ double Gps_Navigation_Message::utc_time(const double gpstime_corrected) const * proper accommodation of the leap second event with a possible week number * transition is provided by the following expression for UTC: */ - int32_t W = static_cast(fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400)) + 43200; + const int32_t W = static_cast(fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400)) + 43200; t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS); // implement something to handle a leap second event! } @@ -421,7 +420,7 @@ double Gps_Navigation_Message::utc_time(const double gpstime_corrected) const t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); } - double secondsOfWeekBeforeToday = 43200 * floor(gpstime_corrected / 43200); + const double secondsOfWeekBeforeToday = 43200 * floor(gpstime_corrected / 43200); t_utc = secondsOfWeekBeforeToday + t_utc_daytime; return t_utc; }