mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Improve const correctness

This commit is contained in:
Carles Fernandez 2020-07-19 14:26:15 +02:00
parent c0f81dd9e2
commit 52980978f5
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
14 changed files with 244 additions and 376 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

89
src/core/system_parameters/beidou_dnav_ephemeris.cc
View File

@ -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);

93
src/core/system_parameters/beidou_dnav_navigation_message.cc
View File

@ -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<std::pair<int32_t, int32_t>>& 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<std::pair<int32_t, int32_t>>& 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<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe);
subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_FRAID));
const std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe);
const auto subframe_ID = static_cast<int>(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<double>(read_navigation_unsigned(subframe_bits, D1_SOW));
d_SOW = d_SOW_SF5; // Set transmission time
SV_page_5 = static_cast<int>(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<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe);
std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe);
subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_FRAID));
page_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_PNUM));
const auto subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_FRAID));
const auto page_ID = static_cast<int>(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<double>(5) / static_cast<double>(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;
}

87
src/core/system_parameters/galileo_ephemeris.cc
View File

@ -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);

23
src/core/system_parameters/galileo_fnav_message.cc
View File

@ -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<GALILEO_FNAV_DATA_FRAME_BITS> 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<GALILEO_FNAV_DATA_FRAME_BITS> 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<GALILEO_FNAV_DATA_FRAME_BITS> 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<GALILEO_FNAV_DATA_FRAME_BITS> 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<GALILEO_FNAV_DATA_FRAME_BITS> b
void Galileo_Fnav_Message::decode_page(const std::string& data)
{
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> data_bits(data);
const std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> 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<int32_t>(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<GALILEO_FNAV_DATA_FRAME_BITS> omega_bits(Omega0);
const std::string omega0_2 = data.substr(10, 12);
const std::string Omega0 = omega0_1 + omega0_2;
const std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> omega_bits(Omega0);
const std::vector<std::pair<int32_t, int32_t>> om_bit({{0, 12}});
FNAV_Omega0_2_6 = static_cast<double>(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<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& 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<GALILEO_FNAV
int64_t Galileo_Fnav_Message::read_navigation_signed(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[0].first]) == 1)

61
src/core/system_parameters/galileo_navigation_message.cc
View File

@ -35,7 +35,6 @@ bool Galileo_Navigation_Message::CRC_test(std::bitset<GALILEO_DATA_FRAME_BITS> 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<GALILEO_DATA_FRAME_BITS> 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<GALILEO_DATA_FRAME_BITS> b
uint64_t Galileo_Navigation_Message::read_navigation_unsigned(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& 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<GALILE
uint64_t Galileo_Navigation_Message::read_page_type_unsigned(std::bitset<GALILEO_PAGE_TYPE_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& 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<GALILEO
int64_t Galileo_Navigation_Message::read_navigation_signed(std::bitset<GALILEO_DATA_JK_BITS> bits, const std::vector<std::pair<int32_t, int32_t> >& 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<int>(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<GALILEO_DATA_FRAME_BITS> 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<GALILEO_DATA_FRAME_BITS> 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<GALILEO_PAGE_TYPE_BITS> page_type_bits(page_number_bits); // from string to bitset
const std::string page_number_bits = Data_k.substr(0, 6);
const std::bitset<GALILEO_PAGE_TYPE_BITS> page_type_bits(page_number_bits); // from string to bitset
Page_type = static_cast<int32_t>(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<GALILEO_DATA_JK_BITS> data_jk_bits(data_jk_string);
std::string data_jk_string = data_jk;
std::bitset<GALILEO_DATA_JK_BITS> data_jk_bits(data_jk_string);
page_number = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, PAGE_TYPE_BIT));
const auto page_number = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, PAGE_TYPE_BIT));
DLOG(INFO) << "Page number = " << page_number;
switch (page_number)

8
src/core/system_parameters/galileo_utc_model.cc
View File

@ -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<double>((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;
}

32
src/core/system_parameters/glonass_gnav_navigation_message.cc
View File

@ -43,14 +43,6 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
{
uint32_t sum_bits = 0;
int32_t sum_hamming = 0;
int32_t C1 = 0;
int32_t C2 = 0;
int32_t C3 = 0;
int32_t C4 = 0;
int32_t C5 = 0;
int32_t C6 = 0;
int32_t C7 = 0;
int32_t C_Sigma = 0;
std::vector<uint32_t> 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<GLONASS_GNAV_STRING_B
{
sum_bits += string_bits[i - 1];
}
C1 = string_bits[0] ^ (sum_bits % 2);
const int32_t C1 = string_bits[0] ^ (sum_bits % 2);
// Compute C2 term
sum_bits = 0;
@ -73,7 +65,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
{
sum_bits += string_bits[j - 1];
}
C2 = (string_bits[1]) ^ (sum_bits % 2);
const int32_t C2 = (string_bits[1]) ^ (sum_bits % 2);
// Compute C3 term
sum_bits = 0;
@ -81,7 +73,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
{
sum_bits += string_bits[k - 1];
}
C3 = string_bits[2] ^ (sum_bits % 2);
const int32_t C3 = string_bits[2] ^ (sum_bits % 2);
// Compute C4 term
sum_bits = 0;
@ -89,7 +81,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
{
sum_bits += string_bits[l - 1];
}
C4 = string_bits[3] ^ (sum_bits % 2);
const int32_t C4 = string_bits[3] ^ (sum_bits % 2);
// Compute C5 term
sum_bits = 0;
@ -97,7 +89,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
{
sum_bits += string_bits[m - 1];
}
C5 = string_bits[4] ^ (sum_bits % 2);
const int32_t C5 = string_bits[4] ^ (sum_bits % 2);
// Compute C6 term
sum_bits = 0;
@ -105,7 +97,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
{
sum_bits += string_bits[n - 1];
}
C6 = string_bits[5] ^ (sum_bits % 2);
const int32_t C6 = string_bits[5] ^ (sum_bits % 2);
// Compute C7 term
sum_bits = 0;
@ -113,11 +105,10 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
{
sum_bits += string_bits[p - 1];
}
C7 = string_bits[6] ^ (sum_bits % 2);
const int32_t C7 = string_bits[6] ^ (sum_bits % 2);
// Compute C_Sigma term
sum_bits = 0;
sum_hamming = 0;
for (int q : GLONASS_GNAV_CRC_Q_INDEX)
{
sum_bits += string_bits[q - 1];
@ -126,7 +117,7 @@ bool Glonass_Gnav_Navigation_Message::CRC_test(std::bitset<GLONASS_GNAV_STRING_B
{
sum_hamming += string_bits[q];
}
C_Sigma = (sum_hamming % 2) ^ (sum_bits % 2);
const int32_t C_Sigma = (sum_hamming % 2) ^ (sum_bits % 2);
// Verification of the data
// (a-i) All checksums (C1,...,C7 and C_Sigma) are equal to zero
@ -164,7 +155,7 @@ bool Glonass_Gnav_Navigation_Message::read_navigation_bool(std::bitset<GLONASS_G
uint64_t Glonass_Gnav_Navigation_Message::read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& 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<GLON
{
int64_t value = 0LL;
int64_t sign = 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 (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1)
{
@ -246,11 +237,10 @@ uint32_t Glonass_Gnav_Navigation_Message::get_frame_number(uint32_t satellite_sl
int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame_string)
{
int32_t J = 0;
d_string_ID = 0U;
d_frame_ID = 0U;
// Unpack bytes to bits
std::bitset<GLONASS_GNAV_STRING_BITS> string_bits(frame_string);
const std::bitset<GLONASS_GNAV_STRING_BITS> string_bits(frame_string);
// Perform data verification and exit code if error in bit sequence
flag_CRC_test = CRC_test(string_bits);

4
src/core/system_parameters/glonass_gnav_utc_model.cc
View File

@ -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;
}

12
src/core/system_parameters/gnss_satellite.cc
View File

@ -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_;
}

88
src/core/system_parameters/gps_cnav_ephemeris.cc
View File

@ -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);

7
src/core/system_parameters/gps_cnav_navigation_message.cc
View File

@ -55,7 +55,7 @@ bool Gps_CNAV_Navigation_Message::read_navigation_bool(std::bitset<GPS_CNAV_DATA
uint64_t Gps_CNAV_Navigation_Message::read_navigation_unsigned(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& 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<GPS_C
int64_t Gps_CNAV_Navigation_Message::read_navigation_signed(std::bitset<GPS_CNAV_DATA_PAGE_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(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<GPS_CNAV
void Gps_CNAV_Navigation_Message::decode_page(std::bitset<GPS_CNAV_DATA_PAGE_BITS> data_bits)
{
int32_t PRN;
int32_t page_type;
bool alert_flag;
// common to all messages
PRN = static_cast<int32_t>(read_navigation_unsigned(data_bits, CNAV_PRN));
const auto PRN = static_cast<int32_t>(read_navigation_unsigned(data_bits, CNAV_PRN));
ephemeris_record.i_satellite_PRN = PRN;
d_TOW = static_cast<int32_t>(read_navigation_unsigned(data_bits, CNAV_TOW));

10
src/core/system_parameters/gps_cnav_utc_model.cc
View File

@ -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<double>(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<int32_t>(fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400)) + 43200;
const int32_t W = static_cast<int32_t>(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<double>(i_GPS_week - i_WN_T));
const double tmp_d = (gpstime_corrected - d_t_OT + 604800 * static_cast<double>(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;
}

89
src/core/system_parameters/gps_ephemeris.cc
View File

@ -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);

17
src/core/system_parameters/gps_navigation_message.cc
View File

@ -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<GPS_SUBFRAME_BITS>
uint64_t Gps_Navigation_Message::read_navigation_unsigned(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& 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<GPS_SUBFRA
int64_t Gps_Navigation_Message::read_navigation_signed(std::bitset<GPS_SUBFRAME_BITS> bits, const std::vector<std::pair<int32_t, int32_t>>& 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<int>(bits[GPS_SUBFRAME_BITS - parameter[0].first]) == 1)
@ -117,7 +117,6 @@ int64_t Gps_Navigation_Message::read_navigation_signed(std::bitset<GPS_SUBFRAME_
int32_t Gps_Navigation_Message::subframe_decoder(char* subframe)
{
int32_t subframe_ID = 0;
uint32_t gps_word;
// UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE
@ -133,7 +132,7 @@ int32_t Gps_Navigation_Message::subframe_decoder(char* subframe)
}
}
subframe_ID = static_cast<int32_t>(read_navigation_unsigned(subframe_bits, SUBFRAME_ID));
const auto subframe_ID = static_cast<int32_t>(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<double>((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<int32_t>(fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400)) + 43200;
const int32_t W = static_cast<int32_t>(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;
}