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