mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-01-19 05:33:02 +00:00
Improve const correctness
This commit is contained in:
parent
c0f81dd9e2
commit
52980978f5
@ -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);
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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);
|
||||||
|
@ -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)
|
||||||
|
@ -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)
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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);
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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_;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -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);
|
||||||
|
@ -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));
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
@ -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);
|
||||||
|
@ -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;
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user