mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-18 21:23:02 +00:00
Code Issues Releases Wiki Activity

Improve const correctness

Browse Source
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() Beidou_Dnav_Ephemeris::Beidou_Dnav_Ephemeris()
{ {
auto gnss_sat = Gnss_Satellite(); auto gnss_sat = Gnss_Satellite();
std::string _system("Beidou"); const std::string _system("Beidou");
for (unsigned int i = 1; i < 36; i++) for (unsigned int i = 1; i < 36; i++)
{ {
satelliteBlock[i] = gnss_sat.what_block(_system, 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 Beidou_Dnav_Ephemeris::check_t(double time)
{ {
double corrTime; const double half_week = 302400.0; // seconds
double half_week = 302400.0; // seconds double corrTime = time;
corrTime = time;
if (time > half_week) if (time > half_week)
{ {
corrTime = time - 2.0 * 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 Beidou_Dnav_Ephemeris::sv_clock_drift(double transmitTime)
{ {
double dt; double dt = check_t(transmitTime - d_Toc);
dt = check_t(transmitTime - d_Toc);
for (int i = 0; i < 2; i++) 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 // compute the relativistic correction term
double Beidou_Dnav_Ephemeris::sv_clock_relativistic_term(double transmitTime) 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 // 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 // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe); const double tk = check_t(transmitTime - d_Toe);
// Computed mean motion // Computed mean motion
n0 = sqrt(BEIDOU_GM / (a * a * a)); const double n0 = sqrt(BEIDOU_GM / (a * a * a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; const double n = n0 + d_Delta_n;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; double M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly // 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++) for (int ii = 1; ii < 20; ii++)
{ {
E_old = E; 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 Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
{ {
double tk; // Find satellite's position -----------------------------------------------
double a;
double n;
double n0;
double M;
double E;
double E_old;
double dE;
double nu;
double phi;
double u;
double r;
double i;
double Omega;
// Find satellite's position ----------------------------------------------
// Restore semi-major axis // 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 // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe); double tk = check_t(transmitTime - d_Toe);
// Computed mean motion // Computed mean motion
n0 = sqrt(BEIDOU_GM / (a * a * a)); const double n0 = sqrt(BEIDOU_GM / (a * a * a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; const double n = n0 + d_Delta_n;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; double M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly // 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++) for (int ii = 1; ii < 20; ii++)
{ {
E_old = E; E_old = E;
@ -170,27 +149,27 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
} }
// Compute the true anomaly // Compute the true anomaly
double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); const double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E);
double tmp_X = cos(E) - d_eccentricity; const double tmp_X = cos(E) - d_eccentricity;
nu = atan2(tmp_Y, tmp_X); const double nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // Compute angle phi (argument of Latitude)
phi = nu + d_OMEGA; double phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2.0 * GNSS_PI)); phi = fmod((phi), (2.0 * GNSS_PI));
// Correct argument of latitude // 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 // 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 // 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 // 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 // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); 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); d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops // 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_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_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); 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() Beidou_Dnav_Navigation_Message::Beidou_Dnav_Navigation_Message()
{ {
auto gnss_sat = Gnss_Satellite(); auto gnss_sat = Gnss_Satellite();
std::string _system("Beidou"); const std::string _system("Beidou");
for (uint32_t i = 1; i < 36; i++) for (uint32_t i = 1; i < 36; i++)
{ {
satelliteBlock[i] = gnss_sat.what_block(_system, 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 const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{ {
uint64_t value = 0ULL; 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 i = 0; i < num_of_slices; i++)
{ {
for (int32_t j = 0; j < parameter[i].second; j++) 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 const std::vector<std::pair<int32_t, int32_t>>& parameter) const
{ {
int64_t value = 0; 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 // read the MSB and perform the sign extension
if (bits[BEIDOU_DNAV_SUBFRAME_DATA_BITS - parameter[0].first] == 1) 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 Beidou_Dnav_Navigation_Message::check_t(double time)
{ {
double corrTime; const double half_week = 302400; // seconds
double half_week = 302400; // seconds double corrTime = time;
corrTime = time;
if (time > half_week) if (time > half_week)
{ {
corrTime = time - 2 * 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 Beidou_Dnav_Navigation_Message::sv_clock_correction(double transmitTime)
{ {
double dt; const double dt = check_t(transmitTime - d_Toc);
dt = check_t(transmitTime - d_Toc);
d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr; d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr;
double correctedTime = transmitTime - d_satClkCorr; double correctedTime = transmitTime - d_satClkCorr;
return correctedTime; return correctedTime;
@ -148,45 +146,30 @@ double Beidou_Dnav_Navigation_Message::sv_clock_correction(double transmitTime)
void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime) void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime)
{ {
double tk; // Find satellite's position -----------------------------------------------
double a;
double n;
double n0;
double M;
double E;
double E_old;
double dE;
double nu;
double phi;
double u;
double r;
double i;
double Omega;
// Find satellite's position ----------------------------------------------
// Restore semi-major axis // 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 // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe_sf2); const double tk = check_t(transmitTime - d_Toe_sf2);
// Computed mean motion // Computed mean motion
n0 = sqrt(BEIDOU_GM / (a * a * a)); const double n0 = sqrt(BEIDOU_GM / (a * a * a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; const double n = n0 + d_Delta_n;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; double M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; double E = M;
double E_old;
// --- Iteratively compute eccentric anomaly ---------------------------- double dE;
// --- Iteratively compute eccentric anomaly -------------------------------
for (int32_t ii = 1; ii < 20; ii++) for (int32_t ii = 1; ii < 20; ii++)
{ {
E_old = E; 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); d_dtr = BEIDOU_F * d_eccentricity * d_sqrt_A * sin(E);
// Compute the true anomaly // Compute the true anomaly
double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E); const double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E);
double tmp_X = cos(E) - d_eccentricity; const double tmp_X = cos(E) - d_eccentricity;
nu = atan2(tmp_Y, tmp_X); const double nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // Compute angle phi (argument of Latitude)
phi = nu + d_OMEGA; double phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2 * GNSS_PI)); phi = fmod((phi), (2 * GNSS_PI));
// Correct argument of latitude // 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 // 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 // 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 // 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 // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2 * GNSS_PI), (2 * GNSS_PI)); 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); d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops // 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_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_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); 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 Beidou_Dnav_Navigation_Message::d1_subframe_decoder(std::string const& subframe)
{ {
int32_t subframe_ID = 0; const std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe);
std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe); const auto subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_FRAID));
subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_FRAID));
// Perform crc computation (tbd) // Perform crc computation (tbd)
flag_crc_test = true; 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; int32_t SV_page_5;
d_SOW_SF5 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SOW)); d_SOW_SF5 = static_cast<double>(read_navigation_unsigned(subframe_bits, D1_SOW));
d_SOW = d_SOW_SF5; // Set transmission time d_SOW = d_SOW_SF5; // Set transmission time
SV_page_5 = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_PNUM)); SV_page_5 = static_cast<int>(read_navigation_unsigned(subframe_bits, D1_PNUM));
if (SV_page_5 < 7) 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 Beidou_Dnav_Navigation_Message::d2_subframe_decoder(std::string const& subframe)
{ {
int32_t subframe_ID = 0; const std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe);
int32_t page_ID = 0;
std::bitset<BEIDOU_DNAV_SUBFRAME_DATA_BITS> subframe_bits(subframe); 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));
subframe_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_FRAID));
page_ID = static_cast<int>(read_navigation_unsigned(subframe_bits, D2_PNUM));
// Perform crc computation (tbd) // Perform crc computation (tbd)
flag_crc_test = true; 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;
double t_utc_daytime; 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 // 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 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 // 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) if (weeksToLeapSecondEvent > 0)
{ {
t_utc_daytime = fmod(beidoutime_corrected - Delta_t_UTC, 86400); 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) 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); t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - i_DeltaT_LS);
} }
else 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); 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; t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
return t_utc; 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). message provided through the TOW is synchronised to each satellite’s version of Galileo System Time (GST).
* *
*/ */
double t = 0; const double sec_in_day = 86400;
double sec_in_day = 86400; const double day_in_week = 7;
double day_in_week = 7; double t = WN * sec_in_day * day_in_week + TOW; // second from the origin of the Galileo time
t = WN * sec_in_day * day_in_week + TOW; // second from the origin of the Galileo time
return t; return t;
} }
@ -61,8 +60,7 @@ double Galileo_Ephemeris::Galileo_System_Time(double WN, double TOW)
double Galileo_Ephemeris::sv_clock_drift(double transmitTime) double Galileo_Ephemeris::sv_clock_drift(double transmitTime)
{ {
// Satellite Time Correction Algorithm, ICD 5.1.4 // Satellite Time Correction Algorithm, ICD 5.1.4
double dt; const double dt = transmitTime - t0c_4;
dt = transmitTime - t0c_4;
Galileo_satClkDrift = af0_4 + af1_4 * dt + af2_4 * (dt * dt) + sv_clock_relativistic_term(transmitTime); // +Galileo_dtr; Galileo_satClkDrift = af0_4 + af1_4 * dt + af2_4 * (dt * dt) + sv_clock_relativistic_term(transmitTime); // +Galileo_dtr;
return Galileo_satClkDrift; return Galileo_satClkDrift;
} }
@ -71,35 +69,28 @@ double Galileo_Ephemeris::sv_clock_drift(double transmitTime)
// compute the relativistic correction term // compute the relativistic correction term
double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Satellite Time Correction Algorithm, ICD 5.1.4 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 // 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 // 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 // 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 // Corrected mean motion
n = n0 + delta_n_3; const double n = n0 + delta_n_3;
// Mean anomaly // Mean anomaly
M = M0_1 + n * tk; double M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI));
// Initial guess of eccentric anomaly // 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++) 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) 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) // when this function in used, the input must be the transmitted time (t) in
double tk; // Time from ephemeris reference epoch // seconds computed by Galileo_System_Time (above function)
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;
// Find Galileo satellite's position ---------------------------------------------- // Find Galileo satellite's position ---------------------------------------
// Restore semi-major axis // Restore semi-major axis
a = A_1 * A_1; const double a = A_1 * A_1;
// Computed mean motion // Computed mean motion
n0 = sqrt(GALILEO_GM / (a * a * a)); const double n0 = sqrt(GALILEO_GM / (a * a * a));
// Time from ephemeris reference epoch // Time from ephemeris reference epoch
tk = transmitTime - t0e_1; const double tk = transmitTime - t0e_1;
// Corrected mean motion // Corrected mean motion
n = n0 + delta_n_3; const double n = n0 + delta_n_3;
// Mean anomaly // Mean anomaly
M = M0_1 + n * tk; double M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI));
// Initial guess of eccentric anomaly // 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++) for (int32_t ii = 1; ii < 20; ii++)
{ {
E_old = E; E_old = E;
@ -175,28 +155,27 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
} }
// Compute the true anomaly // Compute the true anomaly
const double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E);
double tmp_Y = sqrt(1.0 - e_1 * e_1) * sin(E); const double tmp_X = cos(E) - e_1;
double tmp_X = cos(E) - e_1; const double nu = atan2(tmp_Y, tmp_X);
nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // Compute angle phi (argument of Latitude)
phi = nu + omega_2; double phi = nu + omega_2;
// Reduce phi to between 0 and 2*pi rad // Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2 * GNSS_PI)); phi = fmod((phi), (2 * GNSS_PI));
// Correct argument of latitude // 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 // 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 // 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 // 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 // Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2 * GNSS_PI), (2 * GNSS_PI)); 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); d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops // 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_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_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); 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) void Galileo_Fnav_Message::split_page(const std::string& page_string)
{ {
std::string message_word = page_string.substr(0, 214); const std::string message_word = page_string.substr(0, 214);
std::string CRC_data = page_string.substr(214, 24); const std::string CRC_data = page_string.substr(214, 24);
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> Word_for_CRC_bits(message_word); const std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> Word_for_CRC_bits(message_word);
std::bitset<24> checksum(CRC_data); const std::bitset<24> checksum(CRC_data);
if (_CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true) if (_CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true)
{ {
flag_CRC_test = 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; CRC_Galileo_FNAV_type CRC_Galileo;
uint32_t crc_computed;
// Galileo FNAV frame for CRC is not an integer multiple of bytes // 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. // it needs to be filled with zeroes at the start of the frame.
// This operation is done in the transformation from bits to bytes // 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_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) if (checksum == crc_computed)
{ {
return true; 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) 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); page_type = read_navigation_unsigned(data_bits, FNAV_PAGE_TYPE_BIT);
switch (page_type) 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)); 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 // 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.*/ // flag will be set to false and the data won't be recorded.*/
std::string omega0_2 = data.substr(10, 12); const std::string omega0_2 = data.substr(10, 12);
std::string Omega0 = omega0_1 + omega0_2; const std::string Omega0 = omega0_1 + omega0_2;
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> omega_bits(Omega0); const std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> omega_bits(Omega0);
const std::vector<std::pair<int32_t, int32_t>> om_bit({{0, 12}}); 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 = static_cast<double>(read_navigation_signed(omega_bits, om_bit));
FNAV_Omega0_2_6 *= FNAV_OMEGA0_5_LSB; 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 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; 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 i = 0; i < num_of_slices; i++)
{ {
for (int j = 0; j < parameter[i].second; j++) 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 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; 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 // read the MSB and perform the sign extension
if (static_cast<int>(bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[0].first]) == 1) 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; CRC_Galileo_INAV_type CRC_Galileo;
uint32_t crc_computed;
// Galileo INAV frame for CRC is not an integer multiple of bytes // 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. // it needs to be filled with zeroes at the start of the frame.
// This operation is done in the transformation from bits to bytes // 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_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) if (checksum == crc_computed)
{ {
return true; 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 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; 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 i = 0; i < num_of_slices; i++)
{ {
for (int32_t j = 0; j < parameter[i].second; j++) 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 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; 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 i = 0; i < num_of_slices; i++)
{ {
for (int32_t j = 0; j < parameter[i].second; j++) 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 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; 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 // read the MSB and perform the sign extension
if (static_cast<int>(bits[GALILEO_DATA_JK_BITS - parameter[0].first]) == 1) 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 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; const std::string page_INAV_even = page_Even;
std::string page_INAV = page_INAV_even + page_Odd; // Join pages: Even + Odd = INAV page const std::string page_INAV = page_INAV_even + page_Odd; // Join pages: Even + Odd = INAV page
std::string Even_bit = page_INAV.substr(0, 1); const std::string Even_bit = page_INAV.substr(0, 1);
std::string Page_type_even = page_INAV.substr(1, 1); const std::string Page_type_even = page_INAV.substr(1, 1);
std::string nominal = "0"; const std::string nominal = "0";
std::string Data_k = page_INAV.substr(2, 112); const std::string Data_k = page_INAV.substr(2, 112);
std::string Odd_bit = page_INAV.substr(114, 1); const std::string Odd_bit = page_INAV.substr(114, 1);
std::string Page_type_Odd = page_INAV.substr(115, 1); const std::string Page_type_Odd = page_INAV.substr(115, 1);
std::string Data_j = page_INAV.substr(116, 16); const std::string Data_j = page_INAV.substr(116, 16);
std::string Reserved_1 = page_INAV.substr(132, 40); const std::string Reserved_1 = page_INAV.substr(132, 40);
std::string SAR = page_INAV.substr(172, 22); const std::string SAR = page_INAV.substr(172, 22);
std::string Spare = page_INAV.substr(194, 2); const std::string Spare = page_INAV.substr(194, 2);
std::string CRC_data = page_INAV.substr(196, 24); const std::string CRC_data = page_INAV.substr(196, 24);
std::string Reserved_2 = page_INAV.substr(220, 8); const std::string Reserved_2 = page_INAV.substr(220, 8);
std::string Tail_odd = page_INAV.substr(228, 6); const std::string Tail_odd = page_INAV.substr(228, 6);
// ************ CRC checksum control *******/ // ************ CRC checksum control *******/
std::stringstream TLM_word_for_CRC_stream; std::stringstream TLM_word_for_CRC_stream;
TLM_word_for_CRC_stream << page_INAV; TLM_word_for_CRC_stream << page_INAV;
std::string TLM_word_for_CRC; const std::string TLM_word_for_CRC = TLM_word_for_CRC_stream.str().substr(0, GALILEO_DATA_FRAME_BITS);
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);
std::bitset<GALILEO_DATA_FRAME_BITS> TLM_word_for_CRC_bits(TLM_word_for_CRC); const std::bitset<24> checksum(CRC_data);
std::bitset<24> checksum(CRC_data);
if (CRC_test(TLM_word_for_CRC_bits, checksum.to_ulong()) == true) if (CRC_test(TLM_word_for_CRC_bits, checksum.to_ulong()) == true)
{ {
flag_CRC_test = true; flag_CRC_test = true;
// CRC correct: Decode word // CRC correct: Decode word
std::string page_number_bits = Data_k.substr(0, 6); const 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::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 = static_cast<int32_t>(read_page_type_unsigned(page_type_bits, TYPE));
Page_type_time_stamp = Page_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()); page_jk_decoder(Data_jk_ephemeris.c_str());
} }
else else
@ -200,7 +198,6 @@ void Galileo_Navigation_Message::split_page(std::string page_string, int32_t fla
else else
{ {
page_Even = page_string.substr(0, 114); 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 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; const auto page_number = static_cast<int32_t>(read_navigation_unsigned(data_jk_bits, PAGE_TYPE_BIT));
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));
DLOG(INFO) << "Page number = " << page_number; DLOG(INFO) << "Page number = " << page_number;
switch (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 t_Utc_daytime;
double Delta_t_Utc = 0; double Delta_t_Utc = 0;
// Determine if the effectivity time of the leap second event is in the past // 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 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 // 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) if (std::abs(t_e - secondOfLeapSecondEvent) > 21600)
{ {
/* 5.1.7a GST->UTC case a /* 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: * 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)); 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); t_Utc_daytime = fmod(W, 86400 + Delta_tLSF_6 - Delta_tLS_6);
// implement something to handle a leap second event! // 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); 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; t_Utc = secondsOfWeekBeforeToday + t_Utc_daytime;
return t_Utc; 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; uint32_t sum_bits = 0;
int32_t sum_hamming = 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); std::vector<uint32_t> string_bits(GLONASS_GNAV_STRING_BITS);
// Populate data and hamming code vectors // 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]; 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 // Compute C2 term
sum_bits = 0; 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]; 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 // Compute C3 term
sum_bits = 0; 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]; 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 // Compute C4 term
sum_bits = 0; 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]; 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 // Compute C5 term
sum_bits = 0; 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]; 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 // Compute C6 term
sum_bits = 0; 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]; 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 // Compute C7 term
sum_bits = 0; 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]; 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 // Compute C_Sigma term
sum_bits = 0; sum_bits = 0;
sum_hamming = 0;
for (int q : GLONASS_GNAV_CRC_Q_INDEX) for (int q : GLONASS_GNAV_CRC_Q_INDEX)
{ {
sum_bits += string_bits[q - 1]; 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]; 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 // Verification of the data
// (a-i) All checksums (C1,...,C7 and C_Sigma) are equal to zero // (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 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; 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 i = 0; i < num_of_slices; i++)
{ {
for (int32_t j = 0; j < parameter[i].second; j++) 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 value = 0LL;
int64_t sign = 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 // read the MSB and perform the sign extension
if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1) 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 Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame_string)
{ {
int32_t J = 0; int32_t J = 0;
d_string_ID = 0U;
d_frame_ID = 0U; d_frame_ID = 0U;
// Unpack bytes to bits // 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 // Perform data verification and exit code if error in bit sequence
flag_CRC_test = CRC_test(string_bits); 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 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 // 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; 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 int32_t Gnss_Satellite::get_rf_link() const
{ {
// Get satellite's rf link. Identifies the GLONASS Frequency Channel // Get satellite's rf link. Identifies the GLONASS Frequency Channel
int32_t rf_link_; int32_t rf_link_ = rf_link;
rf_link_ = rf_link;
return 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 uint32_t Gnss_Satellite::get_PRN() const
{ {
// Get satellite's PRN // Get satellite's PRN
uint32_t PRN_; uint32_t PRN_ = PRN;
PRN_ = PRN;
return PRN_; return PRN_;
} }
@ -267,8 +265,7 @@ uint32_t Gnss_Satellite::get_PRN() const
std::string Gnss_Satellite::get_system() const std::string Gnss_Satellite::get_system() const
{ {
// Get the satellite system {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"} // Get the satellite system {"GPS", "Glonass", "SBAS", "Galileo", "Beidou"}
std::string system_; std::string system_ = system;
system_ = system;
return system_; return system_;
} }
@ -283,8 +280,7 @@ std::string Gnss_Satellite::get_system_short() const
std::string Gnss_Satellite::get_block() const std::string Gnss_Satellite::get_block() const
{ {
// Get the satellite block // Get the satellite block
std::string block_; std::string block_ = block;
block_ = block;
return 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 Gps_CNAV_Ephemeris::check_t(double time)
{ {
double corrTime; const double half_week = 302400.0; // seconds
double half_week = 302400.0; // seconds double corrTime = time;
corrTime = time;
if (time > half_week) if (time > half_week)
{ {
corrTime = time - 2.0 * 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. // 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
double Gps_CNAV_Ephemeris::sv_clock_drift(double transmitTime) double Gps_CNAV_Ephemeris::sv_clock_drift(double transmitTime)
{ {
double dt; const double dt = check_t(transmitTime - d_Toc);
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); d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime);
// Correct satellite group delay // Correct satellite group delay
@ -58,35 +56,31 @@ double Gps_CNAV_Ephemeris::sv_clock_drift(double transmitTime)
// compute the relativistic correction term // compute the relativistic correction term
double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime) 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 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 // 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 // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe1); const double tk = check_t(transmitTime - d_Toe1);
// Computed mean motion // Computed mean motion
n0 = sqrt(GPS_GM / (a * a * a)); const double n0 = sqrt(GPS_GM / (a * a * a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; const double n = n0 + d_Delta_n;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; const double M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly // 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++) 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 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 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 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 ----------------------------------------------- // Find satellite's position -----------------------------------------------
// Restore semi-major axis // 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 // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe1); double tk = check_t(transmitTime - d_Toe1);
// Computed mean motion // 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 // 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 // Corrected mean motion
n = n0 + delta_n_a; const double n = n0 + delta_n_a;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; const double M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI)); // M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI));
// Initial guess of eccentric anomaly // 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++) for (int32_t ii = 1; ii < 20; ii++)
{ {
@ -169,28 +149,28 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
} }
// Compute the true anomaly // Compute the true anomaly
double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); const double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E);
double tmp_X = cos(E) - d_e_eccentricity; const double tmp_X = cos(E) - d_e_eccentricity;
nu = atan2(tmp_Y, tmp_X); const double nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // 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 // Reduce phi to between 0 and 2*pi rad
// phi = fmod((phi), (2*GNSS_PI)); // phi = fmod((phi), (2*GNSS_PI));
// Correct argument of latitude // 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 // 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 // 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 // Compute the angle between the ascending node and the Greenwich meridian
double d_OMEGA_DOT = OMEGA_DOT_REF * GNSS_PI + d_DELTA_OMEGA_DOT; const 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 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 // Reduce to between 0 and 2*pi rad
// Omega = fmod((Omega + 2*GNSS_PI), (2*GNSS_PI)); // 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); d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops // 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_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_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); 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 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; 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 i = 0; i < num_of_slices; i++)
{ {
for (int32_t j = 0; j < parameter[i].second; j++) 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 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; 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 // read the MSB and perform the sign extension
if (static_cast<int>(bits[GPS_CNAV_DATA_PAGE_BITS - parameter[0].first]) == 1) 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) void Gps_CNAV_Navigation_Message::decode_page(std::bitset<GPS_CNAV_DATA_PAGE_BITS> data_bits)
{ {
int32_t PRN;
int32_t page_type; int32_t page_type;
bool alert_flag; bool alert_flag;
// common to all messages // 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; ephemeris_record.i_satellite_PRN = PRN;
d_TOW = static_cast<int32_t>(read_navigation_unsigned(data_bits, CNAV_TOW)); 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)); 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 // 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 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 // 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) if (weeksToLeapSecondEvent > 0)
{ {
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); 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 * proper accommodation of the leap second event with a possible week number
* transition is provided by the following expression for UTC: * 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); t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS);
// implement something to handle a leap second event! // 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 * and the user's current time does not fall in the time span as given above
* in 20.3.3.5.2.4b,*/ * 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 */ /* 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; 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); 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; t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
return t_utc; return t_utc;
} }

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

@ -28,7 +28,7 @@
Gps_Ephemeris::Gps_Ephemeris() Gps_Ephemeris::Gps_Ephemeris()
{ {
auto gnss_sat = Gnss_Satellite(); auto gnss_sat = Gnss_Satellite();
std::string _system("GPS"); const std::string _system("GPS");
for (uint32_t i = 1; i < 33; i++) for (uint32_t i = 1; i < 33; i++)
{ {
satelliteBlock[i] = gnss_sat.what_block(_system, 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 Gps_Ephemeris::check_t(double time)
{ {
double corrTime; const double half_week = 302400.0; // seconds
double half_week = 302400.0; // seconds double corrTime = time;
corrTime = time;
if (time > half_week) if (time > half_week)
{ {
corrTime = time - 2.0 * 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); // d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt);
double dt; const double dt = check_t(transmitTime - d_Toc);
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); d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime);
// Correct satellite group delay // Correct satellite group delay
d_satClkDrift -= d_TGD; d_satClkDrift -= d_TGD;
@ -78,34 +76,28 @@ double Gps_Ephemeris::sv_clock_drift(double transmitTime)
// compute the relativistic correction term // compute the relativistic correction term
double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime) 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 // 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 // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe); const double tk = check_t(transmitTime - d_Toe);
// Computed mean motion // Computed mean motion
n0 = sqrt(GPS_GM / (a * a * a)); const double n0 = sqrt(GPS_GM / (a * a * a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; const double n = n0 + d_Delta_n;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; const double M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly // 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++) 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 Gps_Ephemeris::satellitePosition(double transmitTime)
{ {
double tk; // Find satellite's position -----------------------------------------------
double a;
double n;
double n0;
double M;
double E;
double E_old;
double dE;
double nu;
double phi;
double u;
double r;
double i;
double Omega;
// Find satellite's position ----------------------------------------------
// Restore semi-major axis // 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 // Time from ephemeris reference epoch
tk = check_t(transmitTime - d_Toe); double tk = check_t(transmitTime - d_Toe);
// Computed mean motion // Computed mean motion
n0 = sqrt(GPS_GM / (a * a * a)); const double n0 = sqrt(GPS_GM / (a * a * a));
// Corrected mean motion // Corrected mean motion
n = n0 + d_Delta_n; const double n = n0 + d_Delta_n;
// Mean anomaly // Mean anomaly
M = d_M_0 + n * tk; const double M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi // Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly // Initial guess of eccentric anomaly
E = M; double E = M;
double E_old;
// --- Iteratively compute eccentric anomaly ---------------------------- double dE;
// --- Iteratively compute eccentric anomaly -------------------------------
for (int32_t ii = 1; ii < 20; ii++) for (int32_t ii = 1; ii < 20; ii++)
{ {
E_old = E; E_old = E;
@ -179,27 +156,27 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
} }
// Compute the true anomaly // Compute the true anomaly
double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); const double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E);
double tmp_X = cos(E) - d_e_eccentricity; const double tmp_X = cos(E) - d_e_eccentricity;
nu = atan2(tmp_Y, tmp_X); const double nu = atan2(tmp_Y, tmp_X);
// Compute angle phi (argument of Latitude) // 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 // Reduce phi to between 0 and 2*pi rad
// phi = fmod((phi), (2.0 * GNSS_PI)); // phi = fmod((phi), (2.0 * GNSS_PI));
// Correct argument of latitude // 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 // 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 // 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 // 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 // Reduce to between 0 and 2*pi rad
// Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI)); // 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); d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops // 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_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_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); 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() Gps_Navigation_Message::Gps_Navigation_Message()
{ {
auto gnss_sat = Gnss_Satellite(); auto gnss_sat = Gnss_Satellite();
std::string _system("GPS"); const std::string _system("GPS");
for (uint32_t i = 1; i < 33; i++) for (uint32_t i = 1; i < 33; i++)
{ {
satelliteBlock[i] = gnss_sat.what_block(_system, 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 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; 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 i = 0; i < num_of_slices; i++)
{ {
for (int32_t j = 0; j < parameter[i].second; j++) 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 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; 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 // read the MSB and perform the sign extension
if (static_cast<int>(bits[GPS_SUBFRAME_BITS - parameter[0].first]) == 1) 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 Gps_Navigation_Message::subframe_decoder(char* subframe)
{ {
int32_t subframe_ID = 0;
uint32_t gps_word; uint32_t gps_word;
// UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE // 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 // Decode all 5 sub-frames
switch (subframe_ID) 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))); 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 // 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 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 // 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) if (weeksToLeapSecondEvent > 0)
{ {
t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); 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 * proper accommodation of the leap second event with a possible week number
* transition is provided by the following expression for UTC: * 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); t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS);
// implement something to handle a leap second event! // 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); 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; t_utc = secondsOfWeekBeforeToday + t_utc_daytime;
return t_utc; return t_utc;
} }