mirror of https://github.com/gnss-sdr/gnss-sdr
gnav: Adding GLONASS GNAV Navigation Message Operations
Adding code for navigation message computation for GLONASS GNAV code. Continues to fix the almanac position computation and fixes bugs in code due to compilation errors, coding style and Doxygen documentation
This commit is contained in:
Damian Miralles
Damian Miralles
parent
f8f3574090
commit
c24710c585
|
|
@ -6,6 +6,7 @@ docs/latex
|
|||
docs/GNSS-SDR_manual.pdf
|
||||
src/tests/data/output.dat
|
||||
thirdparty/
|
||||
.settings
|
||||
.project
|
||||
.cproject
|
||||
/install
|
||||
|
|
|
|||
|
|
@ -39,6 +39,10 @@ set(SYSTEM_PARAMETERS_SOURCES
|
|||
gps_cnav_iono.cc
|
||||
gps_cnav_utc_model.cc
|
||||
rtcm.cc
|
||||
glonass_gnav_ephemeris.cc
|
||||
glonass_gnav_almanac.cc
|
||||
glonass_gnav_utc_model.cc
|
||||
glonass_gnav_navigation_message.cc
|
||||
)
|
||||
|
||||
|
||||
|
|
@ -59,4 +63,3 @@ add_library(gnss_system_parameters ${SYSTEM_PARAMETERS_SOURCES} ${SYSTEM_PARAMET
|
|||
source_group(Headers FILES ${SYSTEM_PARAMETERS_HEADERS})
|
||||
add_dependencies(gnss_system_parameters rtklib_lib glog-${glog_RELEASE})
|
||||
target_link_libraries(gnss_system_parameters rtklib_lib ${Boost_LIBRARIES})
|
||||
|
||||
|
|
|
|||
|
|
@ -43,7 +43,7 @@ const double GLONASS_C_m_ms = 299792.4580; //!< The spe
|
|||
const double GLONASS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E
|
||||
const double GLONASS_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200E
|
||||
const double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s]
|
||||
const double GLONASS_GM = 398600.4418e9; //!< Universal gravitational constant times the mass of the Earth, [km^3/s^2]
|
||||
const double GLONASS_GM = 398600.4418e9; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
|
||||
const double GLONASS_fM_a = 0.35e9; //!< Gravitational constant of atmosphere [m^3/s^2]
|
||||
const double GLONASS_SEMI_MAJOR_AXIS = 6378136; //!< Semi-major axis of Earth [m]
|
||||
const double GLONASS_FLATTENING = 1/29825784; //!< Flattening parameter
|
||||
|
|
@ -56,26 +56,26 @@ const double GLONASS_J8 = 1.40e-11; //!< Eighth
|
|||
const double GLONASS_U0 = 62636861.4; //!< Normal potential at surface of common terrestrial ellipsoid [m^2/s^2]
|
||||
const double GLONASS_C20 = -1082.63e-6; //!< Second zonal coefficient of spherical harmonic expansion
|
||||
const double GLONASS_EARTH_RADIUS = 6378.136; //!< Equatorial radius of Earth [km]
|
||||
const double GLONASS_EARTH_INCLINATION = 23°26'33'' //!< Mean inclination of ecliptic to equator (23°26'33'').
|
||||
const double GLONASS_EARTH_INCLINATION = 0.000409148809899e3; //!< Mean inclination of ecliptic to equator (23 deg 26 min 33 sec) [rad]
|
||||
|
||||
const double GLONASS_TAU_0 = -334°19′46′′,40;
|
||||
const double GLONASS_TAU_1 = 4069°02′02′′,52;
|
||||
const double GLONASS_TAU_0 = -0.005835151531174e3; //!< (-334 deg 19 min 46.40 sec) [rad];
|
||||
const double GLONASS_TAU_1 = 0.071018041257371e3; //!< (4069 deg 02 min 02.52 sec) [rad];
|
||||
|
||||
const double GLONASS_MOON_Q0 = -63°53′43′′,41;
|
||||
const double GLONASS_MOON_Q1 = 477198°50′56′′,79;
|
||||
const double GLONASS_MOON_OMEGA_0 = 259°10′59′′,79;
|
||||
const double GLONASS_MOON_OMEGA_1 = -1934°08′31′′,23;
|
||||
const double GLONASS_MOON_Q0 = -0.001115184961435e3; //!< (-63 deg 53 min 43.41 sec) [rad]
|
||||
const double GLONASS_MOON_Q1 = 8.328691103668023e3; //!< (477198 deg 50 min 56.79 sec) [rad]
|
||||
const double GLONASS_MOON_OMEGA_0 = 0.004523601514852e3; //!< (259 deg 10 min 59.79 sec) [rad]
|
||||
const double GLONASS_MOON_OMEGA_1 = -0.033757146246552e3; //!< (-1934 deg 08 min 31.23 sec) [rad]
|
||||
const double GLONASS_MOON_GM = 4902.835; //!< Lunar gravitational constant [km^3/s^2]
|
||||
const double GLONASS_MOON_SEMI_MAJOR_AXIS = 3.84385243e5; //!< Semi-major axis of lunar orbit [km];
|
||||
const double GLONASS_MOON_ECCENTRICITY = 0.054900489; //!< Eccentricity of lunar orbit
|
||||
const double GLONASS_MOON_INCLINATION = 5°08'43.4'' //!< Inclination of lunar orbit to ecliptic plane (5°08'43.4'')
|
||||
const double GLONASS_MOON_INCLINATION = 0.000089803977407e3; //!< Inclination of lunar orbit to ecliptic plane (5 deg 08 min 43.4 sec) [rad]
|
||||
|
||||
const double GLONASS_SUN_OMEGA = 281°13′15′′,0 + 6189′′, 03Т;
|
||||
const double GLONASS_SUN_Q0 = 358°28′33′′,04;
|
||||
const double GLONASS_SUN_Q1 = 129596579′′,10;
|
||||
const double GLONASS_SUN_GM = 0.1325263e12; //!< Solar gravitational constant [km^3/s^2]
|
||||
const double GLONASS_SUN_SEMI_MAJOR_AXIS = 1.49598e8; //!< Semi-major axis of solar orbit [km];
|
||||
const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit
|
||||
const double GLONASS_SUN_OMEGA = 0.004908229466869e3; //!< TODO What is this operation in the seconds with T?(281 deg 13 min 15.0 + 6189.03*Т sec) [rad]
|
||||
const double GLONASS_SUN_Q0 = 0.006256583774423e3; //!< (358 deg 28 min 33.04 sec) [rad]
|
||||
const double GLONASS_SUN_Q1 = 0e3; //!< TODO Why is the value greater than 60?(129596579.10 sec) [rad]
|
||||
const double GLONASS_SUN_GM = 0.1325263e12; //!< Solar gravitational constant [km^3/s^2]
|
||||
const double GLONASS_SUN_SEMI_MAJOR_AXIS = 1.49598e8; //!< Semi-major axis of solar orbit [km];
|
||||
const double GLONASS_SUN_ECCENTRICITY = 0.016719; //!< Eccentricity of solar orbit
|
||||
|
||||
// carrier and code frequencies
|
||||
const double GLONASS_L1_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz]
|
||||
|
|
@ -133,7 +133,7 @@ const std::vector<std::pair<int,int>> Y_N({{51,27}});
|
|||
const std::vector<std::pair<int,int>> P3({{6,1}});
|
||||
const std::vector<std::pair<int,int>> GAMMA_N({{7,11}});
|
||||
const std::vector<std::pair<int,int>> P({{19,2}});
|
||||
const std::vector<std::pair<int,int>> L_N({{21,1}});
|
||||
const std::vector<std::pair<int,int>> EPH_L_N({{21,1}});
|
||||
const std::vector<std::pair<int,int>> Z_N_DOT ({{22,24}});
|
||||
const std::vector<std::pair<int,int>> Z_N_DOT_DOT ({{46,5}});
|
||||
const std::vector<std::pair<int,int>> Z_N({{51,27}});
|
||||
|
|
@ -153,7 +153,7 @@ const std::vector<std::pair<int,int>> N_A({{6,11}});
|
|||
const std::vector<std::pair<int,int>> TAU_C({{17,32}});
|
||||
const std::vector<std::pair<int,int>> N_4({{50,5}});
|
||||
const std::vector<std::pair<int,int>> TAU_GPS({{55,22}});
|
||||
const std::vector<std::pair<int,int>> L_N({{77,1}});
|
||||
const std::vector<std::pair<int,int>> ALM_L_N({{77,1}});
|
||||
|
||||
// STRING 6, 8, 10, 12, 14
|
||||
const std::vector<std::pair<int,int>> C_N({{6,1}});
|
||||
|
|
@ -170,7 +170,7 @@ const std::vector<std::pair<int,int>> T_LAMBDA_N_A({{22,21}});
|
|||
const std::vector<std::pair<int,int>> DELTA_T_N_A({{43,22}});
|
||||
const std::vector<std::pair<int,int>> DELTA_T_DOT_N_A({{65,7}});
|
||||
const std::vector<std::pair<int,int>> H_N_A({{72,5}});
|
||||
const std::vector<std::pair<int,int>> L_N({{77,1}});
|
||||
//const std::vector<std::pair<int,int>> L_N({{77,1}});
|
||||
|
||||
|
||||
#endif /* GNSS_SDR_GLONASS_L1_CA_H_ */
|
||||
|
|
|
|||
|
|
@ -1,8 +1,7 @@
|
|||
/*!
|
||||
* \file glonass_gnav_almanac.cc
|
||||
* \brief Interface of a GLONASS GNAV ALMANAC storage
|
||||
*
|
||||
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
|
||||
* \brief Interface of a GLONASS GNAV ALMANAC storage as described in GLONASS ICD (Edition 5.1)
|
||||
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
|
||||
* \author Damian Miralles , 2017. dmiralles2009(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
|
|
@ -29,29 +28,149 @@
|
|||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "glonass_gnav_almanac.h"
|
||||
#include <cmath>
|
||||
#include "GLONASS_L1_CA.h"
|
||||
#include "gnss_satellite.h"
|
||||
|
||||
Glonass_Gnav_Almanac::Glonass_Gnav_Almanac()
|
||||
{
|
||||
int i_satellite_freq_channel = 0;
|
||||
double d_tau_c = 0.0;
|
||||
double d_tau_gps = 0.0;
|
||||
double d_N_4 = 0.0;
|
||||
double d_N_A = 0.0;
|
||||
double d_n_A = 0.0;
|
||||
double d_H_n_A = 0.0;
|
||||
double d_lambda_n_A = 0.0;
|
||||
double d_t_lambda_n_A = 0.0;
|
||||
double d_Delta_i_n_A = 0.0;
|
||||
double d_Delta_T_n_A = 0.0;
|
||||
double d_Delta_T_n_A_dot = 0.0;
|
||||
double d_epsilon_n_A = 0.0;
|
||||
double d_omega_n_A = 0.0;
|
||||
double d_M_n_A = 0.0;
|
||||
double d_B1 = 0.0;
|
||||
double d_B2 = 0.0;
|
||||
double d_KP = 0.0;
|
||||
double d_tau_n_A = 0.0;
|
||||
double d_C_n_A = 0.0;
|
||||
i_satellite_freq_channel = 0;
|
||||
d_tau_c = 0.0;
|
||||
d_tau_gps = 0.0;
|
||||
d_N_4 = 0.0;
|
||||
d_N_A = 0.0;
|
||||
d_n_A = 0.0;
|
||||
d_H_n_A = 0.0;
|
||||
d_lambda_n_A = 0.0;
|
||||
d_t_lambda_n_A = 0.0;
|
||||
d_Delta_i_n_A = 0.0;
|
||||
d_Delta_T_n_A = 0.0;
|
||||
d_Delta_T_n_A_dot = 0.0;
|
||||
d_epsilon_n_A = 0.0;
|
||||
d_omega_n_A = 0.0;
|
||||
d_M_n_A = 0.0;
|
||||
d_KP = 0.0;
|
||||
d_tau_n_A = 0.0;
|
||||
d_C_n_A = 0.0;
|
||||
}
|
||||
|
||||
void Glonass_Gnav_Almanac::satellite_position(double N_i, double t_i)
|
||||
{
|
||||
double T_nom = 43200; // [seconds]
|
||||
double i_nom = D2R*63.0; // [rad]
|
||||
|
||||
double Delta_t = 0.0;
|
||||
double i = 0.0;
|
||||
double T = 0.0;
|
||||
double n = 0.0;
|
||||
double a = 0.0;
|
||||
double lambda_dot = 0.0;
|
||||
double omega_dot = 0.0;
|
||||
double lambda = 0.0;
|
||||
double omega = 0.0;
|
||||
double E_P = 0.0;
|
||||
double Delta_T = 0.0;
|
||||
double M = 0.0;
|
||||
double E = 0.0;
|
||||
double E_old = 0.0;
|
||||
double dE = 0.0;
|
||||
|
||||
double e1_x = 0.0;
|
||||
double e1_y = 0.0;
|
||||
double e1_z = 0.0;
|
||||
|
||||
double e2_x = 0.0;
|
||||
double e2_y = 0.0;
|
||||
double e2_z = 0.0;
|
||||
// Compute time difference to reference time
|
||||
Delta_t = (N_i - d_N_A) * 86400 + (t_i + d_t_lambda_n_A);
|
||||
|
||||
// Compute the actual inclination
|
||||
i = i_nom + d_Delta_i_n_A;
|
||||
|
||||
// Compute the actual orbital period:
|
||||
T = T_nom + d_Delta_T_n_A;
|
||||
|
||||
// Compute the mean motion
|
||||
n = 2*GLONASS_PI/T;
|
||||
|
||||
// Compute the semi-major axis:
|
||||
a = cbrt(GLONASS_GM/(n*n));
|
||||
|
||||
// Compute correction to longitude of ascending node
|
||||
lambda_dot = -10*pow(GLONASS_SEMI_MAJOR_AXIS / a, 7/2)*D2R*cos(i)/86400;
|
||||
|
||||
// Compute correction to argument of perigee
|
||||
omega_dot = 5*pow(GLONASS_SEMI_MAJOR_AXIS / a, 7/2)*D2R*(5*cos(i)*cos(i) - 1)/86400;
|
||||
|
||||
// Compute corrected longitude of ascending node:
|
||||
lambda = d_lambda_n_A + (lambda_dot - GLONASS_OMEGA_EARTH_DOT)*Delta_t;
|
||||
|
||||
// Compute corrected argument of perigee:
|
||||
omega = d_omega_n_A + omega_dot*Delta_t;
|
||||
|
||||
// Compute eccentric anomaly at point P: Note: P is that point of the orbit the true anomaly of which is identical to the argument of perigee.
|
||||
E_P = 2*atan(tan((omega/2)*(sqrt((1 - d_epsilon_n_A)*(1 + d_epsilon_n_A)))));
|
||||
|
||||
// Compute time difference to perigee passing
|
||||
if (omega < GLONASS_PI)
|
||||
{
|
||||
Delta_T = (E_P - d_epsilon_n_A*sin(E_P))/n;
|
||||
}
|
||||
else
|
||||
{
|
||||
Delta_T = (E_P - d_epsilon_n_A*sin(E_P))/n + T;
|
||||
}
|
||||
|
||||
// Compute mean anomaly at epoch t_i:
|
||||
M = n * (Delta_t - Delta_T);
|
||||
|
||||
// Compute eccentric anomaly at epoch t_i. Note: Kepler’s equation has to be solved iteratively
|
||||
|
||||
// Initial guess of eccentric anomaly
|
||||
E = M;
|
||||
|
||||
// --- Iteratively compute eccentric anomaly ----------------------------
|
||||
for (int ii = 1; ii < 20; ii++)
|
||||
{
|
||||
E_old = E;
|
||||
E = M + d_epsilon_n_A * sin(E);
|
||||
dE = fmod(E - E_old, 2.0 * GLONASS_PI);
|
||||
if (fabs(dE) < 1e-12)
|
||||
{
|
||||
//Necessary precision is reached, exit from the loop
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Compute position in orbital coordinate system
|
||||
d_satpos_Xo = a*cos(E) - d_epsilon_n_A;
|
||||
d_satpos_Yo = a*sqrt(1 - d_epsilon_n_A*d_epsilon_n_A)*sin(E);
|
||||
d_satpos_Zo = a*0;
|
||||
|
||||
// Compute velocity in orbital coordinate system
|
||||
d_satvel_Xo = a/(1-d_epsilon_n_A*cos(E))*(-n*sin(E));
|
||||
d_satvel_Yo = a/(1-d_epsilon_n_A*cos(E))*(n*sqrt(1 - d_epsilon_n_A*d_epsilon_n_A)*cos(E));
|
||||
d_satvel_Zo = a/(1-d_epsilon_n_A*cos(E))*(0);
|
||||
|
||||
// Determine orientation vectors of orbital coordinate system in ECEF system
|
||||
e1_x = cos(omega)*cos(lambda) - sin(omega)*sin(lambda);
|
||||
e1_y = cos(omega)*sin(lambda) + sin(omega)*cos(lambda)*cos(i);
|
||||
e1_z = sin(omega)*sin(i);
|
||||
|
||||
e2_x = -sin(omega)*cos(lambda) - sin(omega)*sin(lambda)*cos(i);
|
||||
e2_y = -sin(omega)*sin(lambda) + cos(omega)*cos(lambda)*cos(i);
|
||||
e2_z = cos(omega)*sin(i);
|
||||
|
||||
// Convert position from orbital to ECEF system
|
||||
d_satpos_X = d_satpos_Xo*e1_x + d_satpos_Xo*e2_x;
|
||||
d_satpos_Y = d_satpos_Yo*e1_z + d_satpos_Yo*e2_y;
|
||||
d_satpos_Z = d_satpos_Zo*e1_z + d_satpos_Zo*e2_z;
|
||||
|
||||
// Convert position from orbital to ECEF system
|
||||
d_satvel_X = d_satvel_Xo*e1_x + d_satvel_Xo*e2_x + GLONASS_OMEGA_EARTH_DOT*d_satpos_Y;
|
||||
d_satvel_Y = d_satvel_Yo*e1_z + d_satvel_Yo*e2_y - GLONASS_OMEGA_EARTH_DOT*d_satpos_X;
|
||||
d_satvel_Z = d_satvel_Zo*e1_z + d_satvel_Zo*e2_z;
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -32,16 +32,20 @@
|
|||
#ifndef GNSS_SDR_GLONASS_ALMANAC_H_
|
||||
#define GNSS_SDR_GLONASS_ALMANAC_H_
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include "boost/assign.hpp"
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
/*!
|
||||
* \brief This class is a storage for the GLONASS SV ALMANAC data as described in IS-GPS-200E
|
||||
* \todo Add proper links for GLONASS ICD
|
||||
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
|
||||
* \brief This class is a storage for the GLONASS SV ALMANAC data as described GLONASS ICD (Edition 5.1)
|
||||
*
|
||||
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
|
||||
*/
|
||||
class Glonass_Gnav_Almanac
|
||||
{
|
||||
public:
|
||||
int i_satellite_freq_channel; //!< SV PRN NUMBER
|
||||
int i_satellite_freq_channel; //!< SV Frequency Channel NUMBER
|
||||
double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s]
|
||||
double d_tau_gps; //!< Correction to GPS time to GLONASS time [day]
|
||||
double d_N_4; //!< Four year interval number starting from 1996 [4 year interval]
|
||||
|
|
@ -50,17 +54,64 @@ public:
|
|||
double d_H_n_A; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless]
|
||||
double d_lambda_n_A; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles]
|
||||
double d_t_lambda_n_A; //!< Time of first ascending node passage [s]
|
||||
double d_Delta_i_n_A //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles]
|
||||
double d_Delta_i_n_A; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles]
|
||||
double d_Delta_T_n_A; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period]
|
||||
double d_Delta_T_n_A_dot; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2]
|
||||
double d_epsilon_n_A; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless]
|
||||
double d_omega_n_A; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles]
|
||||
double d_M_n_A; //!< Type of satellite n_A [dimensionless]
|
||||
double d_B1; //!< Coefficient to determine DeltaUT1 [s]
|
||||
double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd]
|
||||
double d_KP; //!< Notification on forthcoming leap second correction of UTC [dimensionless]
|
||||
double d_tau_n_A; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s]
|
||||
double d_C_n_A; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless]
|
||||
|
||||
// satellite positions
|
||||
double d_satpos_Xo; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
|
||||
double d_satpos_Yo; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
|
||||
double d_satpos_Zo; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
|
||||
// Satellite velocity
|
||||
double d_satvel_Xo; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Yo; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Zo; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
|
||||
// satellite positions
|
||||
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
|
||||
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
|
||||
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
|
||||
// Satellite velocity
|
||||
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
|
||||
template<class Archive>
|
||||
/*!
|
||||
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the almanac data on disk file.
|
||||
*/
|
||||
void serialize(Archive& archive, const unsigned int version)
|
||||
{
|
||||
using boost::serialization::make_nvp;
|
||||
if(version){};
|
||||
|
||||
archive & make_nvp("i_satellite_freq_channel", i_satellite_freq_channel);
|
||||
archive & make_nvp("d_tau_c", d_tau_c);
|
||||
archive & make_nvp("d_tau_gps", d_tau_gps);
|
||||
archive & make_nvp("d_N_4", d_N_4);
|
||||
archive & make_nvp("d_N_A", d_N_A);
|
||||
archive & make_nvp("d_n_A", d_n_A);
|
||||
archive & make_nvp("d_H_n_A", d_H_n_A);
|
||||
archive & make_nvp("d_lambda_n_A", d_lambda_n_A);
|
||||
archive & make_nvp("d_t_lambda_n_A", d_t_lambda_n_A);
|
||||
archive & make_nvp("d_Delta_i_n_A", d_Delta_i_n_A);
|
||||
archive & make_nvp("d_Delta_T_n_A", d_Delta_T_n_A);
|
||||
archive & make_nvp("d_Delta_T_n_A_dot", d_Delta_T_n_A_dot);
|
||||
archive & make_nvp("d_epsilon_n_A", d_epsilon_n_A);
|
||||
archive & make_nvp("d_omega_n_A", d_omega_n_A);
|
||||
archive & make_nvp("d_M_n_A", d_M_n_A);
|
||||
archive & make_nvp("d_KP", d_KP);
|
||||
archive & make_nvp("d_tau_n_A", d_tau_n_A);
|
||||
archive & make_nvp("d_C_n_A", d_C_n_A);
|
||||
}
|
||||
|
||||
void satellite_position(double N_i, double t_i);
|
||||
/*!
|
||||
* Default constructor
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -1,9 +1,11 @@
|
|||
/*!
|
||||
* \file gps_ephemeris.cc
|
||||
* \brief Interface of a GPS EPHEMERIS storage and orbital model functions
|
||||
* \file glonass_gnav_ephemeris.cc
|
||||
* \brief Interface of a GLONASS GNAV EPHEMERIS storage and orbital model functions
|
||||
*
|
||||
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
|
||||
* \author Javier Arribas, 2013. jarribas(at)cttc.es
|
||||
* See Russian Institute of Space Device Engineering,
|
||||
“Global Navigation Satellite System GLONASS Interface Control Document,
|
||||
Navigational radiosignal in bands L1, L2,” Moscow, Edition 5.1, 2008.
|
||||
* \author Damian Miralles, 2017. dmiralles2009(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
|
|
@ -30,12 +32,12 @@
|
|||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "gps_ephemeris.h"
|
||||
#include "glonass_gnav_ephemeris.h"
|
||||
#include <cmath>
|
||||
#include "GLONASS_L1_CA.h"
|
||||
#include "gnss_satellite.h"
|
||||
|
||||
Gps_Ephemeris::Gps_Ephemeris()
|
||||
Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris()
|
||||
{
|
||||
i_satellite_freq_channel = 0;
|
||||
d_m = 0.0; //!< String number within frame [dimensionless]
|
||||
|
|
@ -75,53 +77,56 @@ Gps_Ephemeris::Gps_Ephemeris()
|
|||
}
|
||||
|
||||
|
||||
void Glonass_GNAV_Ephemeris::gravitational_perturbations()
|
||||
void Glonass_Gnav_Ephemeris::gravitational_perturbations()
|
||||
{
|
||||
double T;
|
||||
double sigma_days;
|
||||
double tau_prime;
|
||||
double Omega_moon;
|
||||
double q_moon;
|
||||
double q_sun;
|
||||
double Tau_11; double Tau_12;
|
||||
double Eta_11; double Eta_12;
|
||||
double xi_11; double xi_12;
|
||||
double xi_star;
|
||||
double eta_star;
|
||||
double zeta_star;
|
||||
double E_moon;
|
||||
double E_sun;
|
||||
double xi_11;
|
||||
double xi_12;
|
||||
double eta_11;
|
||||
double eta_12;
|
||||
double zeta_11;
|
||||
double zeta_12;
|
||||
double T = 0.0;
|
||||
double sigma_days = 0.0;
|
||||
double tau_prime = 0.0;
|
||||
double Omega_moon = 0.0;
|
||||
double q_moon = 0.0;
|
||||
double q_sun = 0.0;
|
||||
|
||||
double sin_theta_moon;
|
||||
double cos_theta_moon;
|
||||
double theta_moon;
|
||||
double xi_moon_e;
|
||||
double eta_moon_e;
|
||||
double zeta_moon_e;
|
||||
double xi_star = 0.0;
|
||||
double eta_star = 0.0;
|
||||
double zeta_star = 0.0;
|
||||
double E_moon = 0.0;
|
||||
double E_sun = 0.0;
|
||||
double xi_11 = 0.0;
|
||||
double xi_12 = 0.0;
|
||||
double eta_11 = 0.0;
|
||||
double eta_12 = 0.0;
|
||||
double zeta_11 = 0.0;
|
||||
double zeta_12 = 0.0;
|
||||
|
||||
double sin_theta_sun;
|
||||
double cos_theta_sun;
|
||||
double theta_sun;
|
||||
double xi_sun_e;
|
||||
double eta_sun_e;
|
||||
double zeta_sun_e;
|
||||
double sin_theta_moon = 0.0;
|
||||
double cos_theta_moon = 0.0;
|
||||
double theta_moon = 0.0;
|
||||
double xi_moon_e = 0.0;
|
||||
double eta_moon_e = 0.0;
|
||||
double zeta_moon_e = 0.0;
|
||||
double r_moon_e = 0.0;
|
||||
|
||||
double r_moon_e;
|
||||
double mu_bar_moon;
|
||||
double x_bar_moon;
|
||||
double y_bar_moon;
|
||||
double Delta_o_moon;
|
||||
double mu_bar_sun;
|
||||
double x_bar_sun;
|
||||
double y_bar_sun;
|
||||
double Delta_o_sun;
|
||||
double sin_theta_sun = 0.0;
|
||||
double cos_theta_sun = 0.0;
|
||||
double theta_sun = 0.0;
|
||||
double xi_sun_e = 0.0;
|
||||
double eta_sun_e = 0.0;
|
||||
double zeta_sun_e = 0.0;
|
||||
double r_sun_e = 0.0;
|
||||
|
||||
double mu_bar_moon = 0.0;
|
||||
double x_bar_moon = 0.0;
|
||||
double y_bar_moon = 0.0;
|
||||
double z_bar_moon = 0.0;
|
||||
double Delta_o_moon = 0.0;
|
||||
double mu_bar_sun = 0.0;
|
||||
double x_bar_sun = 0.0;
|
||||
double y_bar_sun = 0.0;
|
||||
double z_bar_sun = 0.0;
|
||||
double Delta_o_sun = 0.0;
|
||||
|
||||
|
||||
double t_e = 0.0;
|
||||
|
||||
//<! Directive Cosine computation
|
||||
/// \TODO Need to define sigma days
|
||||
|
|
@ -165,17 +170,17 @@ void Glonass_GNAV_Ephemeris::gravitational_perturbations()
|
|||
r_sun_e = GLONASS_SUN_SEMI_MAJOR_AXIS*(1-GLONASS_SUN_ECCENTRICITY*cos(E_sun));
|
||||
|
||||
//<! Gravitational computation
|
||||
mu_bar_moon = GLONASS_MOON_GM/(d_r_moon_e*d_r_moon_e);
|
||||
x_bar_moon = d_satpos_X/d_r_moon_e;
|
||||
y_bar_moon = d_satpos_Y/d_r_moon_e;
|
||||
z_bar_moon = d_satpos_Z/d_r_moon_e;
|
||||
Delta_o_moon = sqrt((d_xi_moon_e - x_bar_moon)*(d_xi_moon_e - x_bar_moon) + (d_eta_moon_e - y_bar_moon)*(d_eta_moon_e - y_bar_moon) + (d_zeta_moon_e - z_bar_moon)*(d_zeta_moon_e - z_bar_moon));
|
||||
mu_bar_moon = GLONASS_MOON_GM/(r_moon_e*r_moon_e);
|
||||
x_bar_moon = d_Xn/r_moon_e;
|
||||
y_bar_moon = d_Yn/r_moon_e;
|
||||
z_bar_moon = d_Zn/r_moon_e;
|
||||
Delta_o_moon = sqrt((xi_moon_e - x_bar_moon)*(xi_moon_e - x_bar_moon) + (eta_moon_e - y_bar_moon)*(eta_moon_e - y_bar_moon) + (zeta_moon_e - z_bar_moon)*(zeta_moon_e - z_bar_moon));
|
||||
|
||||
mu_bar_sun = GLONASS_MOON_GM/(d_r_sun_e*d_r_sun_e);
|
||||
x_bar_sun = d_satpos_X/d_r_sun_e;
|
||||
y_bar_sun = d_satpos_Y/d_r_sun_e;
|
||||
z_bar_sun = d_satpos_Z/d_r_sun_e;
|
||||
Delta_o_sun = sqrt((d_xi_sun_e - x_bar_sun)*(d_xi_sun_e - x_bar_sun) + (d_eta_sun_e - y_bar_sun)*(d_eta_sun_e - y_bar_sun) + (d_zeta_sun_e - z_bar_sun)*(d_zeta_sun_e - z_bar_sun));
|
||||
mu_bar_sun = GLONASS_MOON_GM/(r_sun_e*r_sun_e);
|
||||
x_bar_sun = d_Xn/r_sun_e;
|
||||
y_bar_sun = d_Yn/r_sun_e;
|
||||
z_bar_sun = d_Zn/r_sun_e;
|
||||
Delta_o_sun = sqrt((xi_sun_e - x_bar_sun)*(xi_sun_e - x_bar_sun) + (eta_sun_e - y_bar_sun)*(eta_sun_e - y_bar_sun) + (zeta_sun_e - z_bar_sun)*(zeta_sun_e - z_bar_sun));
|
||||
|
||||
d_Jx_moon = mu_bar_moon*(xi_moon_e - x_bar_moon)/pow(Delta_o_moon,3.0) - xi_moon_e;
|
||||
d_Jy_moon = mu_bar_moon*(eta_moon_e - y_bar_moon)/pow(Delta_o_moon,3.0) - eta_moon_e;
|
||||
|
|
@ -188,7 +193,7 @@ void Glonass_GNAV_Ephemeris::gravitational_perturbations()
|
|||
}
|
||||
|
||||
|
||||
double Glonass_GNAV_Ephemeris::check_t(double time)
|
||||
double Glonass_Gnav_Ephemeris::check_t(double time)
|
||||
{
|
||||
double corrTime;
|
||||
double half_day = 43200.0; // seconds
|
||||
|
|
@ -204,9 +209,9 @@ double Glonass_GNAV_Ephemeris::check_t(double time)
|
|||
return corrTime;
|
||||
}
|
||||
|
||||
|
||||
// FIXME Fix reference here
|
||||
// 20.3.3.3.3.1 User Algorithm for SV Clock Correction.
|
||||
double Glonass_GNAV_Ephemeris::sv_clock_drift(double transmitTime, double timeCorrUTC)
|
||||
double Glonass_Gnav_Ephemeris::sv_clock_drift(double transmitTime, double timeCorrUTC)
|
||||
{
|
||||
double dt;
|
||||
dt = check_t(transmitTime - d_t_b);
|
||||
|
|
@ -217,93 +222,148 @@ double Glonass_GNAV_Ephemeris::sv_clock_drift(double transmitTime, double timeCo
|
|||
return d_satClkDrift;
|
||||
}
|
||||
|
||||
|
||||
// TODO is this the right formula
|
||||
// compute the relativistic correction term
|
||||
double Glonass_GNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime)
|
||||
double Glonass_Gnav_Ephemeris::sv_clock_relativistic_term(double transmitTime)
|
||||
{
|
||||
double tk;
|
||||
double a;
|
||||
double n;
|
||||
double n0;
|
||||
double E;
|
||||
double E_old;
|
||||
double dE;
|
||||
double M;
|
||||
|
||||
// Restore semi-major axis
|
||||
a = d_sqrt_A * d_sqrt_A;
|
||||
|
||||
// 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.0 * GPS_PI), (2.0 * 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.0 * 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);
|
||||
d_dtr = 0.0;
|
||||
return d_dtr;
|
||||
}
|
||||
|
||||
|
||||
double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime)
|
||||
double Glonass_Gnav_Ephemeris::simplified_satellite_position(double transmitTime)
|
||||
{
|
||||
double dt = 0.0;
|
||||
double tau = 0.0;
|
||||
double tk = 0.0;
|
||||
int numberOfIntegrations = 0;
|
||||
|
||||
// RK 1
|
||||
double x_0 = 0.0;
|
||||
double y_0 = 0.0;
|
||||
double z_0 = 0.0;
|
||||
|
||||
double Vx_0 = 0.0;
|
||||
double Vy_0 = 0.0;
|
||||
double Vz_0 = 0.0;
|
||||
|
||||
double Ax_0 = 0.0;
|
||||
double Ay_0 = 0.0;
|
||||
double Az_0 = 0.0;
|
||||
|
||||
double r0 = 0.0;
|
||||
double r0_2 = 0.0;
|
||||
double r0_3 = 0.0;
|
||||
double r0_5 = 0.0;
|
||||
|
||||
double dVx_1 = 0.0;
|
||||
double dVy_1 = 0.0;
|
||||
double dVz_1 = 0.0;
|
||||
|
||||
double dx_1 = 0.0;
|
||||
double dy_1 = 0.0;
|
||||
double dz_1 = 0.0;
|
||||
|
||||
// Runge-Kutta Integration Stage K2
|
||||
double x_1 = 0.0;
|
||||
double y_1 = 0.0;
|
||||
double z_1 = 0.0;
|
||||
|
||||
double Vx_1 = 0.0;
|
||||
double Vy_1 = 0.0;
|
||||
double Vz_1 = 0.0;
|
||||
|
||||
double r1 = 0.0;
|
||||
double r1_2 = 0.0;
|
||||
double r1_3 = 0.0;
|
||||
double r1_5 = 0.0;
|
||||
|
||||
double dVx_2 = 0.0;
|
||||
double dVy_2 = 0.0;
|
||||
double dVz_2 = 0.0;
|
||||
|
||||
double dx_2 = 0.0;
|
||||
double dy_2 = 0.0;
|
||||
double dz_2 = 0.0;
|
||||
|
||||
// Runge-Kutta Integration Stage K3
|
||||
double x_2 = 0.0;
|
||||
double y_2 = 0.0;
|
||||
double z_2 = 0.0;
|
||||
|
||||
double Vx_2 = 0.0;
|
||||
double Vy_2 = 0.0;
|
||||
double Vz_2 = 0.0;
|
||||
|
||||
double r2 = 0.0;
|
||||
double r2_2 = 0.0;
|
||||
double r2_3 = 0.0;
|
||||
double r2_5 = 0.0;
|
||||
|
||||
double dVx_3 = 0.0;
|
||||
double dVy_3 = 0.0;
|
||||
double dVz_3 = 0.0;
|
||||
|
||||
double dx_3 = 0.0;
|
||||
double dy_3 = 0.0;
|
||||
double dz_3 = 0.0;
|
||||
|
||||
// Runge-Kutta Integration Stage K4
|
||||
double x_3 = 0.0;
|
||||
double y_3 = 0.0;
|
||||
double z_3 = 0.0;
|
||||
|
||||
double Vx_3 = 0.0;
|
||||
double Vy_3 = 0.0;
|
||||
double Vz_3 = 0.0;
|
||||
|
||||
double r3 = 0.0;
|
||||
double r3_2 = 0.0;
|
||||
double r3_3 = 0.0;
|
||||
double r3_5 = 0.0;
|
||||
|
||||
double dVx_4 = 0.0;
|
||||
double dVy_4 = 0.0;
|
||||
double dVz_4 = 0.0;
|
||||
|
||||
double dx_4 = 0.0;
|
||||
double dy_4 = 0.0;
|
||||
double dz_4 = 0.0;
|
||||
|
||||
// Find time difference
|
||||
dt = check_t(transmitTime - d_t_b);
|
||||
|
||||
// Calculate clock correction
|
||||
satClkCorr = -(d_tau_n + d_tau_c - d_gamma_n * dt);
|
||||
d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt);
|
||||
|
||||
// Find integration time
|
||||
tk = dt - satClkCorr;
|
||||
tk = dt - d_satClkDrift;
|
||||
|
||||
// Check if to integrate forward or backward
|
||||
if tk < 0
|
||||
tau = -60;
|
||||
if(tk < 0)
|
||||
{
|
||||
tau = -60;
|
||||
}
|
||||
else
|
||||
tau = 60;
|
||||
end
|
||||
{
|
||||
tau = 60;
|
||||
}
|
||||
|
||||
// x,y,z coordinates to meters
|
||||
x0 = d_satpos_X * 1e3;
|
||||
y0 = d_satpos_Y * 1e3;
|
||||
z0 = d_satpos_Z * 1e3;
|
||||
x_0 = d_Xn * 1e3;
|
||||
y_0 = d_Yn * 1e3;
|
||||
z_0 = d_Zn * 1e3;
|
||||
|
||||
// x,y,z velocities to meters/s
|
||||
Vx0 = d_satvel_X * 1e3;
|
||||
Vy0 = d_satvel_Y * 1e3;
|
||||
Vz0 = d_satvel_Z * 1e3;
|
||||
Vx_0 = d_VXn * 1e3;
|
||||
Vy_0 = d_VYn * 1e3;
|
||||
Vz_0 = d_VZn * 1e3;
|
||||
|
||||
// x,y,z accelerations to meters/sec^2
|
||||
Ax0 = d_satacc_X * 1e3;
|
||||
Ay0 = d_satacc_Y * 1e3;
|
||||
Az0 = d_satacc_Z * 1e3;
|
||||
Ax_0 = d_AXn * 1e3;
|
||||
Ay_0 = d_AYn * 1e3;
|
||||
Az_0 = d_AZn * 1e3;
|
||||
|
||||
for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau)
|
||||
{
|
||||
|
|
@ -311,9 +371,9 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime)
|
|||
if(fabs(numberOfIntegrations) > fabs(tk))
|
||||
{
|
||||
// if there is more time left to integrate...
|
||||
if mod(tk,tau) ~= 0
|
||||
if(fmod(tk,tau) != 0)
|
||||
{
|
||||
tau = mod(time,tau);
|
||||
tau = fmod(tk,tau);
|
||||
}
|
||||
else // otherwise make a zero step.
|
||||
{
|
||||
|
|
@ -322,49 +382,49 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime)
|
|||
}
|
||||
|
||||
// Runge-Kutta Integration Stage K1
|
||||
r0 = sqrt(x0*x0 + y0*y0 + z0*z0);
|
||||
r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0);
|
||||
r0_2 = r0*r0;
|
||||
r0_3 = r0*r0*r0;
|
||||
r0_5 = r0*r0*r0*r0*r0;
|
||||
|
||||
dVx_1 = - GLONASS_GM / r0_3 * x0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * x0 * (1 - 5 * (z0*z0) / r0_2) + (GLONASS_OMEGA_EARTH_DOT*GLONASS_OMEGA_EARTH_DOT) * x0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy0 + Ax0;
|
||||
dVy_1 = - GLONASS_GM / r0_3 * y0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * y0 * (1 - 5 * (z0*z0) / r0_2) + (GLONASS_OMEGA_EARTH_DOT*GLONASS_OMEGA_EARTH_DOT) * y0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx0 + Ay0;
|
||||
dVz_1 = - GLONASS_GM / r0_3 * z0 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a * GLONASS_a) / r0_5 * z0 * (3 - 5 * (z0*z0) / r0_2) + Az0;
|
||||
dVx_1 = - GLONASS_GM / r0_3 * x_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * x_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_0 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_0 + Ax_0;
|
||||
dVy_1 = - GLONASS_GM / r0_3 * y_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * y_0 * (1 - 5 * (z_0*z_0) / r0_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_0 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_0 + Ay_0;
|
||||
dVz_1 = - GLONASS_GM / r0_3 * z_0 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r0_5 * z_0 * (3 - 5 * (z_0*z_0) / r0_2) + Az_0;
|
||||
|
||||
dx_1 = Vx0;
|
||||
dy_1 = Vy0;
|
||||
dz_1 = Vz0;
|
||||
dx_1 = Vx_0;
|
||||
dy_1 = Vy_0;
|
||||
dz_1 = Vz_0;
|
||||
|
||||
// Runge-Kutta Integration Stage K2
|
||||
Vx_1 = Vx0 + 1/2 * tau * dVx_1;
|
||||
Vy_1 = Vy0 + 1/2 * tau * dVy_1;
|
||||
Vz_1 = Vz0 + 1/2 * tau * dVz_1;
|
||||
Vx_1 = Vx_0 + 1/2 * tau * dVx_1;
|
||||
Vy_1 = Vy_0 + 1/2 * tau * dVy_1;
|
||||
Vz_1 = Vz_0 + 1/2 * tau * dVz_1;
|
||||
|
||||
x_1 = x0 + 1/2 * tau * dx_1;
|
||||
y_1 = y0 + 1/2 * tau * dy_1;
|
||||
z_1 = z0 + 1/2 * tau * dz_1;
|
||||
x_1 = x_0 + 1/2 * tau * dx_1;
|
||||
y_1 = y_0 + 1/2 * tau * dy_1;
|
||||
z_1 = z_0 + 1/2 * tau * dz_1;
|
||||
|
||||
r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 );
|
||||
r1_2 = r1*r1;
|
||||
r1_3 = r1*r1*r1;
|
||||
r1_5 = r1*r1*r1*r1*r1;
|
||||
|
||||
dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax0;
|
||||
dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay0;
|
||||
dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az0;
|
||||
dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0;
|
||||
dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0;
|
||||
dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0;
|
||||
|
||||
dx_2 = Vx_1;
|
||||
dy_2 = Vy_1;
|
||||
dz_2 = Vz_1;
|
||||
|
||||
// Runge-Kutta Integration Stage K2
|
||||
Vx_2 = Vx0 + 1/2 * tau * dVx_2;
|
||||
Vy_2 = Vy0 + 1/2 * tau * dVy_2;
|
||||
Vz_2 = Vz0 + 1/2 * tau * dVz_2;
|
||||
Vx_2 = Vx_0 + 1/2 * tau * dVx_2;
|
||||
Vy_2 = Vy_0 + 1/2 * tau * dVy_2;
|
||||
Vz_2 = Vz_0 + 1/2 * tau * dVz_2;
|
||||
|
||||
x_2 = x0 + 1/2 * tau * dx_2;
|
||||
y_2 = y0 + 1/2 * tau * dy_2;
|
||||
z_2 = z0 + 1/2 * tau * dz_2;
|
||||
x_2 = x_0 + 1/2 * tau * dx_2;
|
||||
y_2 = y_0 + 1/2 * tau * dy_2;
|
||||
z_2 = z_0 + 1/2 * tau * dz_2;
|
||||
|
||||
r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 );
|
||||
r2_2 = r2*r2;
|
||||
|
|
@ -372,109 +432,205 @@ double Glonass_GNAV_Ephemeris::simplifiedSatellitePosition(double transmitTime)
|
|||
r2_5 = r2*r2*r2*r2*r2;
|
||||
|
||||
|
||||
dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax0;
|
||||
dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay0;
|
||||
dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az0;
|
||||
dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0;
|
||||
dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0;
|
||||
dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0;
|
||||
|
||||
dx_3 = Vx_2;
|
||||
dy_3 = Vy_2;
|
||||
dz_3 = Vz_2;
|
||||
|
||||
// Runge-Kutta Integration Stage K3
|
||||
Vx_3 = Vx0 + tau * dVx_3;
|
||||
Vy_3 = Vy0 + tau * dVy_3;
|
||||
Vz_3 = Vz0 + tau * dVz_3;
|
||||
Vx_3 = Vx_0 + tau * dVx_3;
|
||||
Vy_3 = Vy_0 + tau * dVy_3;
|
||||
Vz_3 = Vz_0 + tau * dVz_3;
|
||||
|
||||
x_3 = x0 + tau * dx_3;
|
||||
y_3 = y0 + tau * dy_3;
|
||||
z_3 = z0 + tau * dz_3;
|
||||
x_3 = x_0 + tau * dx_3;
|
||||
y_3 = y_0 + tau * dy_3;
|
||||
z_3 = z_0 + tau * dz_3;
|
||||
|
||||
r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 );
|
||||
r3_2 = r3*r3;
|
||||
r3_3 = r3*r3*r3;
|
||||
r3_5 = r3*r3*r3*r3*r3;
|
||||
|
||||
dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax0;
|
||||
dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay0;
|
||||
dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az0;
|
||||
dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0;
|
||||
dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + pow(GLONASS_OMEGA_EARTH_DOT, 2) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0;
|
||||
dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0;
|
||||
|
||||
dx_4 = Vx_3;
|
||||
dy_4 = Vy_3;
|
||||
dz_4 = Vz_3;
|
||||
|
||||
// Final results showcased here
|
||||
Vx0 = Vx0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 );
|
||||
Vy0 = Vy0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 );
|
||||
Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 );
|
||||
Vx_0 = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 );
|
||||
Vy_0 = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 );
|
||||
Vz_0 = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 );
|
||||
|
||||
x0 = x0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 );
|
||||
y0 = y0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 );
|
||||
z0 = z0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 );
|
||||
x_0 = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 );
|
||||
y_0 = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 );
|
||||
z_0 = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 );
|
||||
|
||||
}
|
||||
|
||||
// Reasign position, velocities and accelerations for next integration
|
||||
d_satpos_X = x0;
|
||||
d_satpos_Y = y0;
|
||||
d_satpos_Z = z0;
|
||||
d_satpos_X = x_0;
|
||||
d_satpos_Y = y_0;
|
||||
d_satpos_Z = z_0;
|
||||
|
||||
d_satvel_X = Vx0;
|
||||
d_satvel_Y = Vy0;
|
||||
d_satvel_Z = Vz0;
|
||||
d_satvel_X = Vx_0;
|
||||
d_satvel_Y = Vy_0;
|
||||
d_satvel_Z = Vz_0;
|
||||
|
||||
d_satacc_X = d_satacc_X; // No change in accelerations reported over interval
|
||||
d_satacc_Y = d_satacc_Y; // No change in accelerations reported over interval
|
||||
d_satacc_Z = d_satacc_Z; // No change in accelerations reported over interval
|
||||
d_satacc_X = d_AXn; // No change in accelerations reported over interval
|
||||
d_satacc_Y = d_AYn; // No change in accelerations reported over interval
|
||||
d_satacc_Z = d_AZn; // No change in accelerations reported over interval
|
||||
|
||||
// Time from ephemeris reference clock
|
||||
tk = check_t(transmitTime - d_Toc);
|
||||
//tk = check_t(transmitTime - d_Toc);
|
||||
|
||||
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
|
||||
//double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
|
||||
|
||||
/* relativity correction */
|
||||
dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s);
|
||||
//dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s);
|
||||
|
||||
return dtr_s;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime)
|
||||
double Glonass_Gnav_Ephemeris::satellite_position(double transmitTime)
|
||||
{
|
||||
double dt = 0.0;
|
||||
double tk = 0.0;
|
||||
double tau = 0.0;
|
||||
int numberOfIntegrations = 0;
|
||||
|
||||
// RK 1
|
||||
double x_0 = 0.0;
|
||||
double y_0 = 0.0;
|
||||
double z_0 = 0.0;
|
||||
|
||||
double Vx_0 = 0.0;
|
||||
double Vy_0 = 0.0;
|
||||
double Vz_0 = 0.0;
|
||||
|
||||
double Ax_0 = 0.0;
|
||||
double Ay_0 = 0.0;
|
||||
double Az_0 = 0.0;
|
||||
|
||||
double r0 = 0.0;
|
||||
double r0_2 = 0.0;
|
||||
double r0_3 = 0.0;
|
||||
double r0_5 = 0.0;
|
||||
|
||||
double dVx_1 = 0.0;
|
||||
double dVy_1 = 0.0;
|
||||
double dVz_1 = 0.0;
|
||||
|
||||
double dx_1 = 0.0;
|
||||
double dy_1 = 0.0;
|
||||
double dz_1 = 0.0;
|
||||
|
||||
// Runge-Kutta Integration Stage K2
|
||||
double x_1 = 0.0;
|
||||
double y_1 = 0.0;
|
||||
double z_1 = 0.0;
|
||||
|
||||
double Vx_1 = 0.0;
|
||||
double Vy_1 = 0.0;
|
||||
double Vz_1 = 0.0;
|
||||
|
||||
double r1 = 0.0;
|
||||
double r1_2 = 0.0;
|
||||
double r1_3 = 0.0;
|
||||
double r1_5 = 0.0;
|
||||
|
||||
double dVx_2 = 0.0;
|
||||
double dVy_2 = 0.0;
|
||||
double dVz_2 = 0.0;
|
||||
|
||||
double dx_2 = 0.0;
|
||||
double dy_2 = 0.0;
|
||||
double dz_2 = 0.0;
|
||||
|
||||
// Runge-Kutta Integration Stage K3
|
||||
double x_2 = 0.0;
|
||||
double y_2 = 0.0;
|
||||
double z_2 = 0.0;
|
||||
|
||||
double Vx_2 = 0.0;
|
||||
double Vy_2 = 0.0;
|
||||
double Vz_2 = 0.0;
|
||||
|
||||
double r2 = 0.0;
|
||||
double r2_2 = 0.0;
|
||||
double r2_3 = 0.0;
|
||||
double r2_5 = 0.0;
|
||||
|
||||
double dVx_3 = 0.0;
|
||||
double dVy_3 = 0.0;
|
||||
double dVz_3 = 0.0;
|
||||
|
||||
double dx_3 = 0.0;
|
||||
double dy_3 = 0.0;
|
||||
double dz_3 = 0.0;
|
||||
|
||||
// Runge-Kutta Integration Stage K4
|
||||
double x_3 = 0.0;
|
||||
double y_3 = 0.0;
|
||||
double z_3 = 0.0;
|
||||
|
||||
double Vx_3 = 0.0;
|
||||
double Vy_3 = 0.0;
|
||||
double Vz_3 = 0.0;
|
||||
|
||||
double r3 = 0.0;
|
||||
double r3_2 = 0.0;
|
||||
double r3_3 = 0.0;
|
||||
double r3_5 = 0.0;
|
||||
|
||||
double dVx_4 = 0.0;
|
||||
double dVy_4 = 0.0;
|
||||
double dVz_4 = 0.0;
|
||||
|
||||
double dx_4 = 0.0;
|
||||
double dy_4 = 0.0;
|
||||
double dz_4 = 0.0;
|
||||
|
||||
// Find time difference
|
||||
dt = check_t(transmitTime - d_t_b);
|
||||
|
||||
// Calculate clock correction
|
||||
satClkCorr = -(d_tau_n + d_tau_c - d_gamma_n * dt);
|
||||
d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt);
|
||||
|
||||
// Find integration time
|
||||
tk = dt - satClkCorr;
|
||||
tk = dt - d_satClkDrift;
|
||||
|
||||
// Check if to integrate forward or backward
|
||||
if tk < 0
|
||||
tau = -60;
|
||||
if (tk < 0)
|
||||
{
|
||||
tau = -60;
|
||||
}
|
||||
else
|
||||
tau = 60;
|
||||
end
|
||||
{
|
||||
tau = 60;
|
||||
}
|
||||
|
||||
// Coordinates transformation to an inertial reference frame
|
||||
// x,y,z coordinates to meters
|
||||
x0 = d_satpos_X * 1e3;
|
||||
y0 = d_satpos_Y * 1e3;
|
||||
z0 = d_satpos_Z * 1e3;
|
||||
x_0 = d_Xn * 1e3;
|
||||
y_0 = d_Yn * 1e3;
|
||||
z_0 = d_Zn * 1e3;
|
||||
|
||||
// x,y,z velocities to meters/s
|
||||
Vx0 = d_satvel_X * 1e3;
|
||||
Vy0 = d_satvel_Y * 1e3;
|
||||
Vz0 = d_satvel_Z * 1e3;
|
||||
Vx_0 = d_VXn * 1e3;
|
||||
Vy_0 = d_VYn * 1e3;
|
||||
Vz_0 = d_VZn * 1e3;
|
||||
|
||||
// x,y,z accelerations to meters/sec^2
|
||||
Ax0 = d_satacc_X * 1e3;
|
||||
Ay0 = d_satacc_Y * 1e3;
|
||||
Az0 = d_satacc_Z * 1e3;
|
||||
Ax_0 = d_AXn * 1e3;
|
||||
Ay_0 = d_AYn * 1e3;
|
||||
Az_0 = d_AZn * 1e3;
|
||||
|
||||
for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau)
|
||||
{
|
||||
|
|
@ -482,9 +638,9 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime)
|
|||
if(fabs(numberOfIntegrations) > fabs(tk))
|
||||
{
|
||||
// if there is more time left to integrate...
|
||||
if mod(tk,tau) ~= 0
|
||||
if (fmod(tk,tau) != 0)
|
||||
{
|
||||
tau = mod(time,tau);
|
||||
tau = fmod(tk,tau);
|
||||
}
|
||||
else // otherwise make a zero step.
|
||||
{
|
||||
|
|
@ -493,53 +649,53 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime)
|
|||
}
|
||||
|
||||
// Runge-Kutta Integration Stage K1
|
||||
r0 = sqrt(x0*x0 + y0*y0 + z0*z0);
|
||||
r0 = sqrt(x_0*x_0 + y_0*y_0 + z_0*z_0);
|
||||
r0_2 = r0*r0;
|
||||
r0_3 = r0*r0*r0;
|
||||
r0_5 = r0*r0*r0*r0*r0;
|
||||
|
||||
dx_1 = Vx0;
|
||||
dy_1 = Vy0;
|
||||
dz_1 = Vz0;
|
||||
dx_1 = Vx_0;
|
||||
dy_1 = Vy_0;
|
||||
dz_1 = Vz_0;
|
||||
|
||||
dVx_1 = - GLONASS_GM / r0_3 * x0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * x0 * (1 - 5 * 1 / (z0*z0)) + Jx_moon + Jx_sun;
|
||||
dVy_1 = - GLONASS_GM / r0_3 * y0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * y0 * (1 - 5 * 1 / (z0*z0)) + Jy_moon + Jy_sun;
|
||||
dVz_1 = - GLONASS_GM / r0_3 * z0 + 3/2 * GLONASS_C20 * GLONASS_GM * (GLONASS_EARTH_RADIUS * GLONASS_EARTH_RADIUS) / r0_5 * z0 * (3 - 5 * 1 / (z0*z0)) + Jz_moon + Jz_sun;
|
||||
dVx_1 = - GLONASS_GM / r0_3 * x_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * x_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jx_moon + d_Jx_sun;
|
||||
dVy_1 = - GLONASS_GM / r0_3 * y_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * y_0 * (1 - 5 * 1 / (z_0*z_0)) + d_Jy_moon + d_Jy_sun;
|
||||
dVz_1 = - GLONASS_GM / r0_3 * z_0 + 3/2 * GLONASS_C20 * GLONASS_GM * pow(GLONASS_EARTH_RADIUS, 2) / r0_5 * z_0 * (3 - 5 * 1 / (z_0*z_0)) + d_Jz_moon + d_Jz_sun;
|
||||
|
||||
dx_1 = Vx0;
|
||||
dy_1 = Vy0;
|
||||
dz_1 = Vz0;
|
||||
dx_1 = Vx_0;
|
||||
dy_1 = Vy_0;
|
||||
dz_1 = Vz_0;
|
||||
|
||||
// Runge-Kutta Integration Stage K2
|
||||
Vx_1 = Vx0 + 1/2 * tau * dVx_1;
|
||||
Vy_1 = Vy0 + 1/2 * tau * dVy_1;
|
||||
Vz_1 = Vz0 + 1/2 * tau * dVz_1;
|
||||
Vx_1 = Vx_0 + 1/2 * tau * dVx_1;
|
||||
Vy_1 = Vy_0 + 1/2 * tau * dVy_1;
|
||||
Vz_1 = Vz_0 + 1/2 * tau * dVz_1;
|
||||
|
||||
x_1 = x0 + 1/2 * tau * dx_1;
|
||||
y_1 = y0 + 1/2 * tau * dy_1;
|
||||
z_1 = z0 + 1/2 * tau * dz_1;
|
||||
x_1 = x_0 + 1/2 * tau * dx_1;
|
||||
y_1 = y_0 + 1/2 * tau * dy_1;
|
||||
z_1 = z_0 + 1/2 * tau * dz_1;
|
||||
|
||||
r1 = sqrt( x_1*x_1 + y_1*y_1 + z_1*z_1 );
|
||||
r1_2 = r1*r1;
|
||||
r1_3 = r1*r1*r1;
|
||||
r1_5 = r1*r1*r1*r1*r1;
|
||||
|
||||
dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax0;
|
||||
dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay0;
|
||||
dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az0;
|
||||
dVx_2 = - GLONASS_GM / r1_3 * x_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * x_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_1 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_1 + Ax_0;
|
||||
dVy_2 = - GLONASS_GM / r1_3 * y_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * y_1 * (1 - 5 * (z_1*z_1) / r1_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_1 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_1 + Ay_0;
|
||||
dVz_2 = - GLONASS_GM / r1_3 * z_1 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r1_5 * z_1 * (3 - 5 * (z_1*z_1) / r1_2) + Az_0;
|
||||
|
||||
dx_2 = Vx_1;
|
||||
dy_2 = Vy_1;
|
||||
dz_2 = Vz_1;
|
||||
|
||||
// Runge-Kutta Integration Stage K2
|
||||
Vx_2 = Vx0 + 1/2 * tau * dVx_2;
|
||||
Vy_2 = Vy0 + 1/2 * tau * dVy_2;
|
||||
Vz_2 = Vz0 + 1/2 * tau * dVz_2;
|
||||
Vx_2 = Vx_0 + 1/2 * tau * dVx_2;
|
||||
Vy_2 = Vy_0 + 1/2 * tau * dVy_2;
|
||||
Vz_2 = Vz_0 + 1/2 * tau * dVz_2;
|
||||
|
||||
x_2 = x0 + 1/2 * tau * dx_2;
|
||||
y_2 = y0 + 1/2 * tau * dy_2;
|
||||
z_2 = z0 + 1/2 * tau * dz_2;
|
||||
x_2 = x_0 + 1/2 * tau * dx_2;
|
||||
y_2 = y_0 + 1/2 * tau * dy_2;
|
||||
z_2 = z_0 + 1/2 * tau * dz_2;
|
||||
|
||||
r2 = sqrt( x_2*x_2 + y_2*y_2 + z_2*z_2 );
|
||||
r2_2 = r2*r2;
|
||||
|
|
@ -547,54 +703,54 @@ double Glonass_GNAV_Ephemeris::satellitePosition(double transmitTime)
|
|||
r2_5 = r2*r2*r2*r2*r2;
|
||||
|
||||
|
||||
dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax0;
|
||||
dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay0;
|
||||
dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az0;
|
||||
dVx_3 = - GLONASS_GM / r2_3 * x_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * x_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_2 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_2 + Ax_0;
|
||||
dVy_3 = - GLONASS_GM / r2_3 * y_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * y_2 * (1 - 5 * (z_2*z_2) / r2_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_2 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_2 + Ay_0;
|
||||
dVz_3 = - GLONASS_GM / r2_3 * z_2 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r2_5 * z_2 * (3 - 5 * (z_2*z_2) / r2_2) + Az_0;
|
||||
|
||||
dx_3 = Vx_2;
|
||||
dy_3 = Vy_2;
|
||||
dz_3 = Vz_2;
|
||||
|
||||
// Runge-Kutta Integration Stage K3
|
||||
Vx_3 = Vx0 + tau * dVx_3;
|
||||
Vy_3 = Vy0 + tau * dVy_3;
|
||||
Vz_3 = Vz0 + tau * dVz_3;
|
||||
Vx_3 = Vx_0 + tau * dVx_3;
|
||||
Vy_3 = Vy_0 + tau * dVy_3;
|
||||
Vz_3 = Vz_0 + tau * dVz_3;
|
||||
|
||||
x_3 = x0 + tau * dx_3;
|
||||
y_3 = y0 + tau * dy_3;
|
||||
z_3 = z0 + tau * dz_3;
|
||||
x_3 = x_0 + tau * dx_3;
|
||||
y_3 = y_0 + tau * dy_3;
|
||||
z_3 = z_0 + tau * dz_3;
|
||||
|
||||
r3 = sqrt( x_3*x_3 + y_3*y_3 + z_3*z_3 );
|
||||
r3_2 = r3*r3;
|
||||
r3_3 = r3*r3*r3;
|
||||
r3_5 = r3*r3*r3*r3*r3;
|
||||
|
||||
dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax0;
|
||||
dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay0;
|
||||
dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * (GLONASS_a*GLONASS_a) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az0;
|
||||
dVx_4 = - GLONASS_GM / r3_3 * x_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * x_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * x_3 + 2 * GLONASS_OMEGA_EARTH_DOT * Vy_3 + Ax_0;
|
||||
dVy_4 = - GLONASS_GM / r3_3 * y_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * y_3 * (1 - 5 * (z_3*z_3) / r3_2) + (GLONASS_OMEGA_EARTH_DOT * GLONASS_OMEGA_EARTH_DOT) * y_3 - 2 * GLONASS_OMEGA_EARTH_DOT * Vx_3 + Ay_0;
|
||||
dVz_4 = - GLONASS_GM / r3_3 * z_3 - 3/2 * GLONASS_J2 * GLONASS_GM * pow(GLONASS_SEMI_MAJOR_AXIS, 2) / r3_5 * z_3 * (3 - 5 * (z_3*z_3) / r3_2) + Az_0;
|
||||
|
||||
dx_4 = Vx_3;
|
||||
dy_4 = Vy_3;
|
||||
dz_4 = Vz_3;
|
||||
|
||||
// Final results showcased here
|
||||
Vx0 = Vx0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 );
|
||||
Vy0 = Vy0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 );
|
||||
Vz0 = Vz0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 );
|
||||
d_satvel_X = Vx_0 + 1/6 * tau * ( dVx_1 + 2 * dVx_2 + 2 * dVx_3 + dVx_4 );
|
||||
d_satvel_Y = Vy_0 + 1/6 * tau * ( dVy_1 + 2 * dVy_2 + 2 * dVy_3 + dVy_4 );
|
||||
d_satvel_Z = Vz_0 + 1/6 * tau * ( dVz_1 + 2 * dVz_2 + 2 * dVz_3 + dVz_4 );
|
||||
|
||||
x0 = x0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 );
|
||||
y0 = y0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 );
|
||||
z0 = z0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 );
|
||||
d_satpos_X = x_0 + 1/6 * tau * ( dx_1 + 2 * dx_2 + 2 * dx_3 + dx_4 );
|
||||
d_satpos_Y = y_0 + 1/6 * tau * ( dy_1 + 2 * dy_2 + 2 * dy_3 + dy_4 );
|
||||
d_satpos_Z = z_0 + 1/6 * tau * ( dz_1 + 2 * dz_2 + 2 * dz_3 + dz_4 );
|
||||
|
||||
}
|
||||
|
||||
// Time from ephemeris reference clock
|
||||
tk = check_t(transmitTime - d_Toc);
|
||||
//tk = check_t(transmitTime - d_Toc);
|
||||
|
||||
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
|
||||
//double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
|
||||
|
||||
/* relativity correction */
|
||||
dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s);
|
||||
//dtr_s -= 2.0 * sqrt(GM * GLONASS_a) * d_e_eccentricity * sin(E) / (GPS_C_m_s * GPS_C_m_s);
|
||||
|
||||
return dtr_s;
|
||||
return d_dtr;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,11 +41,11 @@
|
|||
|
||||
|
||||
/*!
|
||||
* \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD Edition 5.1
|
||||
* \brief This class is a storage and orbital model functions for the GLONASS SV ephemeris data as described in GLONASS ICD (Edition 5.1)
|
||||
*
|
||||
* See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II
|
||||
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
|
||||
*/
|
||||
class Glonass_GNAV_Ephemeris
|
||||
class Glonass_Gnav_Ephemeris
|
||||
{
|
||||
private:
|
||||
/*
|
||||
|
|
@ -57,10 +57,15 @@ private:
|
|||
*/
|
||||
double check_t(double time);
|
||||
|
||||
double gravitational_perturbations(const double mu);
|
||||
void gravitational_perturbations();
|
||||
|
||||
double d_Jx_moon; //!< Moon gravitational perturbation
|
||||
double d_Jy_moon; //!< Moon gravitational perturbation
|
||||
double d_Jz_moon; //!< Moon gravitational perturbation
|
||||
|
||||
double d_Jx_sun; //!< Sun gravitational perturbation
|
||||
double d_Jy_sun; //!< Sun gravitational perturbation
|
||||
double d_Jz_sun; //!< Sun gravitational perturbation
|
||||
|
||||
public:
|
||||
unsigned int i_satellite_freq_channel; // SV Frequency Channel Number
|
||||
|
|
@ -70,18 +75,20 @@ public:
|
|||
double d_M; //!< Type of satellite transmitting navigation signal [dimensionless]
|
||||
double d_gamma_n; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless]
|
||||
double d_tau_n; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te),
|
||||
|
||||
// satellite positions
|
||||
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
|
||||
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
|
||||
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
|
||||
double d_Xn; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
|
||||
double d_Yn; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
|
||||
double d_Zn; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
|
||||
// Satellite velocity
|
||||
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_VXn; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_VYn; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_VZn; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
// Satellite acceleration
|
||||
double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_AXn; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_AYn; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_AZn; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
|
||||
double d_B_n; //!< Health flag [dimensionless]
|
||||
double d_P; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless]
|
||||
double d_N_T; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days]
|
||||
|
|
@ -99,6 +106,19 @@ public:
|
|||
double d_satClkDrift; //!< GPS clock error
|
||||
double d_dtr; //!< relativistic clock correction term
|
||||
|
||||
// satellite positions
|
||||
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
|
||||
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
|
||||
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
|
||||
// Satellite velocity
|
||||
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
// Satellite acceleration
|
||||
double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
|
||||
template<class Archive>
|
||||
|
||||
/*!
|
||||
|
|
@ -137,7 +157,7 @@ public:
|
|||
* \param transmitTime Time of ephemeris transmission
|
||||
* \return clock bias of satellite
|
||||
*/
|
||||
double simplifiedSatellitePosition(double transmitTime);
|
||||
double simplified_satellite_position(double transmitTime);
|
||||
|
||||
/*!
|
||||
* \brief Compute the ECEF SV coordinates and ECEF velocity
|
||||
|
|
@ -146,7 +166,7 @@ public:
|
|||
* \param transmitTime Time of ephemeris transmission
|
||||
* \return clock bias of satellite
|
||||
*/
|
||||
double satellitePosition(double transmitTime);
|
||||
double satellite_position(double transmitTime);
|
||||
|
||||
/*!
|
||||
* \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction
|
||||
|
|
@ -164,7 +184,7 @@ public:
|
|||
/*!
|
||||
* Default constructor
|
||||
*/
|
||||
Glonass_GNAV_Ephemeris();
|
||||
Glonass_Gnav_Ephemeris();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -0,0 +1,577 @@
|
|||
/*!
|
||||
m * \file glonass_gnav_navigation_message.cc
|
||||
* \brief Implementation of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1)
|
||||
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
|
||||
* \author Damian Miralles, 2017. dmiralles2009(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "glonass_gnav_navigation_message.h"
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <gnss_satellite.h>
|
||||
|
||||
|
||||
void Glonass_Gnav_Navigation_Message::reset()
|
||||
{
|
||||
b_valid_ephemeris_set_flag = false;
|
||||
double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
|
||||
d_TOW_SF1 = 0; //!< Time of GPS Week from HOW word of Subframe 1 [s]
|
||||
d_TOW_SF2 = 0; //!< Time of GPS Week from HOW word of Subframe 2 [s]
|
||||
d_TOW_SF3 = 0; //!< Time of GPS Week from HOW word of Subframe 3 [s]
|
||||
d_TOW_SF4 = 0; //!< Time of GPS Week from HOW word of Subframe 4 [s]
|
||||
d_TOW_SF5 = 0; //!< Time of GPS Week from HOW word of Subframe 5 [s]
|
||||
|
||||
d_m = 0.0; //!< String number within frame [dimensionless]
|
||||
d_t_k = 0.0; //!< Time referenced to the beginning of the frame within the current day [hours, minutes, seconds]
|
||||
d_t_b = 0.0; //!< Index of a time interval within current day according to UTC(SU) + 03 hours 00 min. [minutes]
|
||||
d_M = 0.0; //!< Type of satellite transmitting navigation signal [dimensionless]
|
||||
d_gamma_n = 0.0; //!< Relative deviation of predicted carrier frequency value of n- satellite from nominal value at the instant tb [dimensionless]
|
||||
d_tau_n = 0.0; //!< Correction to the nth satellite time (tn) relative to GLONASS time (te),
|
||||
d_B_n = 0.0; //!< Health flag [dimensionless]
|
||||
d_P = 0.0; //!< Technological parameter of control segment, indication the satellite operation mode in respect of time parameters [dimensionless]
|
||||
d_N_T = 0.0; //!< Current date, calendar number of day within four-year interval starting from the 1-st of January in a leap year [days]
|
||||
d_F_T = 0.0; //!< Parameter that provides the predicted satellite user range accuracy at time tb [dimensionless]
|
||||
d_n = 0.0; //!< Index of the satellite transmitting given navigation signal. It corresponds to a slot number within GLONASS constellation
|
||||
d_Delta_tau_n = 0.0; //!< Time difference between navigation RF signal transmitted in L2 sub- band and aviation RF signal transmitted in L1 sub-band by nth satellite. [dimensionless]
|
||||
d_E_n = 0.0; //!< Characterises "age" of a current information [days]
|
||||
d_P_1 = 0.0; //!< Flag of the immediate data updating.
|
||||
d_P_2 = 0.0; //!< Flag of oddness ("1") or evenness ("0") of the value of (tb) [dimensionless]
|
||||
d_P_3 = 0.0; //!< Flag indicating a number of satellites for which almanac is transmitted within given frame: "1" corresponds to 5 satellites and "0" corresponds to 4 satellites [dimensionless]
|
||||
d_P_4 = 0.0; //!< Flag to show that ephemeris parameters are present. "1" indicates that updated ephemeris or frequency/time parameters have been uploaded by the control segment [dimensionless]
|
||||
d_l_n = 0.0; //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless]
|
||||
|
||||
// Almanac and Not Inmediate Information
|
||||
d_tau_c = 0.0; //!< GLONASS time scale correction to UTC(SU) time. [s]
|
||||
d_tau_gps = 0.0; //!< Correction to GPS time to GLONASS time [day]
|
||||
d_N_4 = 0.0; //!< Four year interval number starting from 1996 [4 year interval]
|
||||
d_N_A = 0.0; //!< Calendar day number within the four-year period beginning since the leap year [days]
|
||||
d_n_A = 0.0; //!< Conventional number of satellite within GLONASS space segment [dimensionless]
|
||||
d_H_n_A = 0.0; //!< Carrier frequency number of navigation RF signal transmitted by d_nA satellite [dimensionless]
|
||||
d_lambda_n_A = 0.0; //!< Longitude of the first (within the d_NA day) ascending node of d_nA [semi-circles]
|
||||
d_t_lambda_n_A = 0.0; //!< Time of first ascending node passage [s]
|
||||
d_Delta_i_n_A = 0.0; //!< Correction of the mean value of inclination of d_n_A satellite at instant t_lambda_n_A [semi-circles]
|
||||
d_Delta_T_n_A = 0.0; //!< Correction to the mean value of Draconian period of d_n_A satellite at instant t_lambda_n_A[s / orbital period]
|
||||
d_Delta_T_n_A_dot = 0.0; //!< Rate of change of Draconian period of d_n_A satellite at instant t_lambda_n_A [s / orbital period^2]
|
||||
d_epsilon_n_A = 0.0; //!< Eccentricity of d_n_A satellite at instant t_lambda_n_A [dimensionless]
|
||||
d_omega_n_A = 0.0; //!< Argument of preigree of d_n_A satellite at instant t_lambdan_A [semi-circles]
|
||||
d_M_n_A = 0.0; //!< Type of satellite n_A [dimensionless]
|
||||
d_B1 = 0.0; //!< Coefficient to determine DeltaUT1 [s]
|
||||
d_B2 = 0.0; //!< Coefficient to determine DeltaUT1 [s/msd]
|
||||
d_KP = 0.0; //!< Notification on forthcoming leap second correction of UTC [dimensionless]
|
||||
d_tau_n_A = 0.0; //!< Coarse value of d_n_A satellite time correction to GLONASS time at instant t_lambdan_A[s]
|
||||
d_C_n_A = 0.0; //!< Generalized “unhealthy flag” of n_A satellite at instant of almanac upload [dimensionless]
|
||||
|
||||
std::map<int,std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus
|
||||
|
||||
|
||||
/*! \brief If true, enhanced level of integrity assurance.
|
||||
*
|
||||
* If false, indicates that the conveying signal is provided with the legacy level of integrity assurance.
|
||||
* That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound
|
||||
* value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less
|
||||
* than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of
|
||||
* integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73
|
||||
* times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an
|
||||
* accompanying alert, is less than 1E-8 per hour.
|
||||
*/
|
||||
b_integrity_status_flag = false;
|
||||
b_alert_flag = false; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk.
|
||||
b_antispoofing_flag = false; //!< If true, the AntiSpoofing mode is ON in that SV
|
||||
|
||||
// Clock terms
|
||||
d_satClkCorr = 0.0; // Satellite clock error
|
||||
d_dtr = 0.0; // Relativistic clock correction term
|
||||
d_satClkDrift = 0.0; // Satellite clock drift
|
||||
|
||||
// satellite identification info
|
||||
int i_channel_ID = 0;
|
||||
int i_satellite_freq_channel = 0; //!< SV PRN NUMBER
|
||||
|
||||
// time synchro
|
||||
d_subframe_timestamp_ms = 0; //[ms]
|
||||
|
||||
// UTC parameters
|
||||
bool flag_utc_model_valid = false; //!< If set, it indicates that the UTC model parameters are filled
|
||||
|
||||
// satellite positions
|
||||
d_satpos_X = 0.0; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
|
||||
d_satpos_Y = 0.0; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
|
||||
d_satpos_Z = 0.0; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
|
||||
// Satellite velocity
|
||||
d_satvel_X = 0.0; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
d_satvel_Y = 0.0; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
d_satvel_Z = 0.0; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
// Satellite acceleration
|
||||
d_satacc_X = 0.0; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
d_satacc_Y = 0.0; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
d_satacc_Z = 0.0; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
|
||||
auto gnss_sat = Gnss_Satellite();
|
||||
std::string _system ("GLONASS");
|
||||
//TODO SHould number of channels be hardcoded?
|
||||
for(unsigned int i = 1; i < 14; i++)
|
||||
{
|
||||
satelliteBlock[i] = gnss_sat.what_block(_system, i);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Glonass_Gnav_Navigation_Message::Glonass_Gnav_Navigation_Message()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
|
||||
bool Glonass_Gnav_Navigation_Message::read_navigation_bool(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter)
|
||||
{
|
||||
bool value;
|
||||
|
||||
if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1)
|
||||
{
|
||||
value = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
value = false;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
|
||||
unsigned long int Glonass_Gnav_Navigation_Message::read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter)
|
||||
{
|
||||
unsigned long int value = 0;
|
||||
int num_of_slices = parameter.size();
|
||||
for (int i = 0; i < num_of_slices; i++)
|
||||
{
|
||||
for (int j = 0; j < parameter[i].second; j++)
|
||||
{
|
||||
value <<= 1; //shift left
|
||||
if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1)
|
||||
{
|
||||
value += 1; // insert the bit
|
||||
}
|
||||
}
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
|
||||
signed long int Glonass_Gnav_Navigation_Message::read_navigation_signed(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter)
|
||||
{
|
||||
signed long int value = 0;
|
||||
int num_of_slices = parameter.size();
|
||||
// Discriminate between 64 bits and 32 bits compiler
|
||||
int long_int_size_bytes = sizeof(signed long int);
|
||||
if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system
|
||||
{
|
||||
// read the MSB and perform the sign extension
|
||||
if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1)
|
||||
{
|
||||
value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable
|
||||
}
|
||||
else
|
||||
{
|
||||
value &= 0;
|
||||
}
|
||||
|
||||
for (int i = 0; i < num_of_slices; i++)
|
||||
{
|
||||
for (int j = 0; j < parameter[i].second; j++)
|
||||
{
|
||||
value <<= 1; //shift left
|
||||
value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable)
|
||||
if (bits[GLONASS_GNAV_STRING_BITS - parameter[i].first - j] == 1)
|
||||
{
|
||||
value += 1; // insert the bit
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else // we assume we are in a 32 bits system
|
||||
{
|
||||
// read the MSB and perform the sign extension
|
||||
if (bits[GLONASS_GNAV_STRING_BITS - parameter[0].first] == 1)
|
||||
{
|
||||
value ^= 0xFFFFFFFF;
|
||||
}
|
||||
else
|
||||
{
|
||||
value &= 0;
|
||||
}
|
||||
|
||||
for (int i = 0; i < num_of_slices; i++)
|
||||
{
|
||||
for (int j = 0; j < parameter[i].second; j++)
|
||||
{
|
||||
value <<= 1; //shift left
|
||||
value &= 0xFFFFFFFE; //reset the corresponding bit
|
||||
if (bits[GPS_SUBFRAME_BITS - parameter[i].first - j] == 1)
|
||||
{
|
||||
value += 1; // insert the bit
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string, int frame_ID)
|
||||
{
|
||||
int string_ID = 0;
|
||||
|
||||
// UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE
|
||||
std::bitset<GLONASS_GNAV_STRING_BITS> string_bits(std::string(frame_string));
|
||||
|
||||
string_ID = static_cast<int>(read_navigation_unsigned(string_bits, STRING_ID));
|
||||
|
||||
// Decode all 15 string messages
|
||||
switch (string_ID)
|
||||
{
|
||||
case 1:
|
||||
//--- It is string 1 -----------------------------------------------
|
||||
gnav_ephemeris.d_P_1 = static_cast<double>(read_navigation_unsigned(string_bits, P1));
|
||||
gnav_ephemeris.d_t_k = static_cast<double>(read_navigation_unsigned(string_bits, T_K));
|
||||
gnav_ephemeris.d_VXn = static_cast<double>(read_navigation_signed(string_bits, X_N_DOT));
|
||||
gnav_ephemeris.d_AXn = static_cast<double>(read_navigation_signed(string_bits, X_N_DOT_DOT));
|
||||
gnav_ephemeris.d_Xn = static_cast<double>(read_navigation_signed(string_bits, X_N));
|
||||
|
||||
break;
|
||||
|
||||
case 2:
|
||||
//--- It is string 2 -----------------------------------------------
|
||||
gnav_ephemeris.d_B_n = static_cast<double>(read_navigation_unsigned(string_bits, B_N));
|
||||
gnav_ephemeris.d_P_2 = static_cast<double>(read_navigation_unsigned(string_bits, P2));
|
||||
gnav_ephemeris.d_t_b = static_cast<double>(read_navigation_unsigned(string_bits, T_B));
|
||||
gnav_ephemeris.d_VYn = static_cast<double>(read_navigation_signed(string_bits, Y_N_DOT));
|
||||
gnav_ephemeris.d_AYn = static_cast<double>(read_navigation_signed(string_bits, Y_N_DOT_DOT));
|
||||
gnav_ephemeris.d_Yn = static_cast<double>(read_navigation_signed(string_bits, Y_N));
|
||||
|
||||
break;
|
||||
|
||||
case 3:
|
||||
// --- It is string 3 ----------------------------------------------
|
||||
gnav_ephemeris.d_P_3 = static_cast<double>(read_navigation_unsigned(string_bits, P3));
|
||||
gnav_ephemeris.d_gamma_n = static_cast<double>(read_navigation_signed(string_bits, GAMMA_N));
|
||||
gnav_ephemeris.d_P = static_cast<double>(read_navigation_unsigned(string_bits, P));
|
||||
gnav_ephemeris.d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, EPH_L_N));
|
||||
gnav_ephemeris.d_VZn = static_cast<double>(read_navigation_signed(string_bits, Z_N_DOT));
|
||||
gnav_ephemeris.d_AZn = static_cast<double>(read_navigation_signed(string_bits, Z_N_DOT_DOT));
|
||||
gnav_ephemeris.d_Zn = static_cast<double>(read_navigation_signed(string_bits, Z_N));
|
||||
|
||||
break;
|
||||
|
||||
case 4:
|
||||
// --- It is subframe 4 --------------------------------------------
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
gnav_ephemeris.d_tau_n = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N));
|
||||
gnav_ephemeris.d_Delta_tau_n = static_cast<double>(read_navigation_signed(string_bits, DELTA_TAU_N));
|
||||
gnav_ephemeris.d_E_n = static_cast<double>(read_navigation_unsigned(string_bits, E_N));
|
||||
gnav_ephemeris.d_P_4 = static_cast<double>(read_navigation_unsigned(string_bits, P4));
|
||||
gnav_ephemeris.d_F_T = static_cast<double>(read_navigation_unsigned(string_bits, F_T));
|
||||
gnav_ephemeris.d_N_T = static_cast<double>(read_navigation_unsigned(string_bits, N_T));
|
||||
gnav_ephemeris.d_n = static_cast<double>(read_navigation_unsigned(string_bits, N));
|
||||
gnav_ephemeris.d_M = static_cast<double>(read_navigation_unsigned(string_bits, M));
|
||||
|
||||
break;
|
||||
|
||||
case 5:
|
||||
// --- It is string 5 ----------------------------------------------
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
d_N_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
d_tau_c = static_cast<double>(read_navigation_unsigned(string_bits, TAU_C));
|
||||
d_N_4 = static_cast<double>(read_navigation_unsigned(string_bits, N_4));
|
||||
d_tau_c = static_cast<double>(read_navigation_unsigned(string_bits, TAU_C));
|
||||
d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
|
||||
|
||||
break;
|
||||
|
||||
case 6:
|
||||
// --- It is string 6 ----------------------------------------------
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
|
||||
|
||||
flag_almanac_str_6 = true;
|
||||
|
||||
break;
|
||||
|
||||
case 7:
|
||||
// --- It is string 7 ----------------------------------------------
|
||||
if (flag_almanac_str_6 == true)
|
||||
{
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
|
||||
|
||||
flag_almanac_str_7 = true;
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
case 8:
|
||||
// --- It is string 8 ----------------------------------------------
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
|
||||
|
||||
flag_almanac_str_8 = true;
|
||||
|
||||
break;
|
||||
|
||||
case 9:
|
||||
// --- It is string 9 ----------------------------------------------
|
||||
if (flag_almanac_str_8 == true)
|
||||
{
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
|
||||
|
||||
flag_almanac_str_9 = true;
|
||||
}
|
||||
case 10:
|
||||
// --- It is string 8 ----------------------------------------------
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
|
||||
|
||||
flag_almanac_str_10 = true;
|
||||
|
||||
break;
|
||||
|
||||
case 11:
|
||||
// --- It is string 9 ----------------------------------------------
|
||||
if (flag_almanac_str_10 == true)
|
||||
{
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
|
||||
|
||||
flag_almanac_str_11 = true;
|
||||
}
|
||||
break;
|
||||
case 12:
|
||||
// --- It is string 8 ----------------------------------------------
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
|
||||
|
||||
flag_almanac_str_12 = true;
|
||||
|
||||
break;
|
||||
|
||||
case 13:
|
||||
// --- It is string 9 ----------------------------------------------
|
||||
if (flag_almanac_str_12 == true)
|
||||
{
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
|
||||
|
||||
flag_almanac_str_13 = true;
|
||||
}
|
||||
case 14:
|
||||
// --- It is string 8 ----------------------------------------------
|
||||
if( frame_number == 5)
|
||||
{
|
||||
gnav_utc_model.B1 = static_cast<double>(read_navigation_unsigned(string_bits, B1));
|
||||
gnav_utc_model.B2 = static_cast<double>(read_navigation_unsigned(string_bits, B2));
|
||||
}
|
||||
else
|
||||
{
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
i_satellite_slot_number = = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_C_N = static_cast<double>(read_navigation_unsigned(string_bits, C_N));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_I_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A));
|
||||
|
||||
flag_almanac_str_14 = true;
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
|
||||
case 15:
|
||||
// --- It is string 9 ----------------------------------------------
|
||||
if (frame_number != 5 and flag_almanac_str_14 = true )
|
||||
{
|
||||
// TODO signed vs unsigned reading from datasheet
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_unsigned(string_bits, OMEGA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast<double>(read_navigation_unsigned(string_bits, H_N_A));
|
||||
gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast<double>(read_navigation_unsigned(string_bits, ALM_L_N));
|
||||
|
||||
flag_almanac_str_15 = true;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
} // switch subframeID ...
|
||||
|
||||
return subframe_ID;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
double Glonass_Gnav_Navigation_Message::utc_time(const double glonass_time_corrected) const
|
||||
{
|
||||
double t_utc;
|
||||
|
||||
t_utc = glonass_time_corrected + 3*3600 + d_tau_c;
|
||||
return t_utc;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Glonass_Gnav_Ephemeris Glonass_Gnav_Navigation_Message::get_ephemeris()
|
||||
{
|
||||
Glonass_Gnav_Ephemeris ephemeris;
|
||||
|
||||
ephemeris.i_satellite_freq_channel = i_satellite_freq_channel;
|
||||
ephemeris.d_m = d_m;
|
||||
ephemeris.d_t_k = d_t_k;
|
||||
ephemeris.d_t_b = d_t_b;
|
||||
ephemeris.d_M = d_M;
|
||||
ephemeris.d_gamma_n = d_gamma_n;
|
||||
ephemeris.d_tau_n = d_tau_n;
|
||||
// satellite positions
|
||||
ephemeris.d_satpos_X = d_satpos_X;
|
||||
ephemeris.d_satpos_Y = d_satpos_Y;
|
||||
ephemeris.d_satpos_Z = d_satpos_Z;
|
||||
// Satellite velocity
|
||||
ephemeris.d_satvel_X = d_satvel_X;
|
||||
ephemeris.d_satvel_Y = d_satvel_Y;
|
||||
ephemeris.d_satvel_Z = d_satvel_Z;
|
||||
// Satellite acceleration
|
||||
ephemeris.d_satacc_X = d_satacc_X;
|
||||
ephemeris.d_satacc_Y = d_satacc_Y;
|
||||
ephemeris.d_satacc_Z = d_satacc_Z;
|
||||
ephemeris.d_B_n = d_B_n;
|
||||
ephemeris.d_P = d_P;
|
||||
ephemeris.d_N_T = d_N_T;
|
||||
ephemeris.d_F_T = d_F_T;
|
||||
ephemeris.d_n = d_n;
|
||||
ephemeris.d_Delta_tau_n = d_Delta_tau_n;
|
||||
ephemeris.d_E_n = d_E_n;
|
||||
ephemeris.d_P_1 = d_P_1;
|
||||
ephemeris.d_P_2 = d_P_2;
|
||||
ephemeris.d_P_3 = d_P_3;
|
||||
ephemeris.d_P_4 = d_P_4;
|
||||
ephemeris.d_l_n = d_l_n;
|
||||
|
||||
// clock terms derived from ephemeris data
|
||||
ephemeris.d_satClkDrift = d_satClkDrift;
|
||||
ephemeris.d_dtr = d_dtr;
|
||||
|
||||
return ephemeris;
|
||||
}
|
||||
|
||||
|
||||
Glonass_Gnav_Utc_Model Glonass_Gnav_Navigation_Message::get_utc_model()
|
||||
{
|
||||
Gps_Utc_Model utc_model;
|
||||
utc_model.valid = flag_utc_model_valid;
|
||||
// UTC parameters
|
||||
utc_model.d_A1 = d_A1;
|
||||
utc_model.d_A0 = d_A0;
|
||||
utc_model.d_t_OT = d_t_OT;
|
||||
utc_model.i_WN_T = i_WN_T;
|
||||
utc_model.d_DeltaT_LS = d_DeltaT_LS;
|
||||
utc_model.i_WN_LSF = i_WN_LSF;
|
||||
utc_model.i_DN = i_DN;
|
||||
utc_model.d_DeltaT_LSF = d_DeltaT_LSF;
|
||||
// warning: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue
|
||||
flag_utc_model_valid = false;
|
||||
return utc_model;
|
||||
}
|
||||
|
||||
|
||||
bool Glonass_Gnav_Navigation_Message::satellite_validation()
|
||||
{
|
||||
bool flag_data_valid = false;
|
||||
b_valid_ephemeris_set_flag = false;
|
||||
|
||||
// First Step:
|
||||
// check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception
|
||||
// and check if the data have been filled (!=0)
|
||||
if (d_TOW_SF1 != 0 and d_TOW_SF2 != 0 and d_TOW_SF3 != 0)
|
||||
{
|
||||
if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!= -1)
|
||||
{
|
||||
flag_data_valid = true;
|
||||
b_valid_ephemeris_set_flag = true;
|
||||
}
|
||||
}
|
||||
return flag_data_valid;
|
||||
}
|
||||
|
|
@ -0,0 +1,203 @@
|
|||
/*!
|
||||
* \file glonass_gnav_navigation_message.h
|
||||
* \brief Interface of a GLONASS GNAV Data message decoder as described in GLONASS ICD (Edition 5.1)
|
||||
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf
|
||||
* \author Damian Miralles, 2017. dmiralles2009(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_GLONASS_GNAV_NAVIGATION_MESSAGE_H_
|
||||
#define GNSS_SDR_GLONASS_GNAV_NAVIGATION_MESSAGE_H_
|
||||
|
||||
|
||||
#include <bitset>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include "GLONASS_L1_CA.h"
|
||||
#include "glonass_gnav_ephemeris.h"
|
||||
//#include "gps_iono.h"
|
||||
#include "glonass_gnav_almanac.h"
|
||||
#include "glonass_gnav_utc_model.h"
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This class decodes a GLONASS GNAV Data message as described in GLONASS ICD (Edition 5.1)
|
||||
*
|
||||
* See http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdfdescribed in IS-GPS-200E
|
||||
*
|
||||
*/
|
||||
class Glonass_Gnav_Navigation_Message
|
||||
{
|
||||
private:
|
||||
unsigned long int read_navigation_unsigned(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter);
|
||||
signed long int read_navigation_signed(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter);
|
||||
bool read_navigation_bool(std::bitset<GLONASS_GNAV_STRING_BITS> bits, const std::vector<std::pair<int,int>> parameter);
|
||||
bool _CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits,boost::uint32_t checksum);
|
||||
|
||||
unsigned int get_frame_number(unsigned int satellite_slot_number);
|
||||
|
||||
public:
|
||||
bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check
|
||||
int Page_type_time_stamp;
|
||||
int flag_even_word;
|
||||
std::string page_Even;
|
||||
bool flag_CRC_test;
|
||||
|
||||
Glonass_Gnav_Ephemeris gnav_ephemeris; //!< Ephemeris information decoded
|
||||
Glonass_Gnav_Iono gnav_iono; //!< Iono corrections information
|
||||
Glonass_Gnav_Utc_Model gnav_utc_model; //!< UTC model information
|
||||
Glonass_Gnav_Almanac gnav_almanac[24]; //!< Almanac information for all 24 satellites
|
||||
|
||||
//!< Ephmeris Flags
|
||||
bool flag_all_ephemeris; //!< Flag indicating that all strings containing ephemeris have been received
|
||||
bool flag_ephemeris_str_1; //!< Flag indicating that ephemeris 1/4 (word 1) have been received
|
||||
bool flag_ephemeris_str_2; //!< Flag indicating that ephemeris 2/4 (word 2) have been received
|
||||
bool flag_ephemeris_str_3; //!< Flag indicating that ephemeris 3/4 (word 3) have been received
|
||||
bool flag_ephemeris_str_4; //!< Flag indicating that ephemeris 4/4 (word 4) have been received
|
||||
|
||||
bool flag_iono_and_GST; //!< Flag indicating that ionospheric and GST parameters (word 5) have been received
|
||||
bool flag_TOW_5;
|
||||
bool flag_TOW_6;
|
||||
bool flag_TOW_set; //!< it is true when page 5 or page 6 arrives
|
||||
bool flag_utc_model; //!< Flag indicating that utc model parameters (word 6) have been received
|
||||
|
||||
bool flag_all_almanac; //!< Flag indicating that all almanac have been received
|
||||
bool flag_almanac_str_6; //!< Flag indicating that almanac 1/4 (word 7) have been received
|
||||
bool flag_almanac_str_7; //!< Flag indicating that almanac 2/4 (word 8) have been received
|
||||
bool flag_almanac_str_8; //!< Flag indicating that almanac 3/4 (word 9) have been received
|
||||
bool flag_almanac_str_9; //!< Flag indicating that almanac 4/4 (word 10) have been received
|
||||
bool flag_almanac_str_10; //!< Flag indicating that almanac 4/4 (word 10) have been received
|
||||
bool flag_almanac_str_11; //!< Flag indicating that almanac 4/4 (word 10) have been received
|
||||
bool flag_almanac_str_12; //!< Flag indicating that almanac 4/4 (word 10) have been received
|
||||
bool flag_almanac_str_13; //!< Flag indicating that almanac 4/4 (word 10) have been received
|
||||
bool flag_almanac_str_14; //!< Flag indicating that almanac 4/4 (word 10) have been received
|
||||
bool flag_almanac_str_15; //!< Flag indicating that almanac 4/4 (word 10) have been received
|
||||
|
||||
//broadcast orbit 1
|
||||
//TODO Need to send the information regarding the frame number
|
||||
double d_TOW; //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s]
|
||||
double d_TOW_F1; //!< Time of GPS Week from HOW word of Subframe 1 [s]
|
||||
double d_TOW_F2; //!< Time of GPS Week from HOW word of Subframe 2 [s]
|
||||
double d_TOW_F3; //!< Time of GPS Week from HOW word of Subframe 3 [s]
|
||||
double d_TOW_F4; //!< Time of GPS Week from HOW word of Subframe 4 [s]
|
||||
double d_TOW_F5; //!< Time of GPS Week from HOW word of Subframe 5 [s]
|
||||
|
||||
// Clock terms
|
||||
double d_satClkCorr; // Satellite clock error
|
||||
double d_dtr; // Relativistic clock correction term
|
||||
double d_satClkDrift; // Satellite clock drift
|
||||
|
||||
// satellite identification info
|
||||
int i_channel_ID;
|
||||
unsigned int i_satellite_PRN; //!< SV PRN Number
|
||||
int i_satellite_freq_channel; //!< SV Frequency Slot Number
|
||||
int i_satellite_slot_number; //!< SV Orbit Slot Number
|
||||
|
||||
// UTC parameters
|
||||
bool flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled
|
||||
|
||||
// satellite positions
|
||||
double d_satpos_X; //!< Earth-fixed coordinate x of the satellite in PZ-90.02 coordinate system [km].
|
||||
double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite in PZ-90.02 coordinate system [km]
|
||||
double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite in PZ-90.02 coordinate system [km]
|
||||
// Satellite velocity
|
||||
double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite in PZ-90.02 coordinate system [km/s]
|
||||
// Satellite acceleration
|
||||
double d_satacc_X; //!< Earth-fixed acceleration coordinate x of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_satacc_Y; //!< Earth-fixed acceleration coordinate y of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
double d_satacc_Z; //!< Earth-fixed acceleration coordinate z of the satellite in PZ-90.02 coordinate system [km/s^2]
|
||||
|
||||
/*!
|
||||
* \brief Reset GLONASS GNAV Navigation Information
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/*!
|
||||
* \brief Obtain a GLONASS GNAV SV Ephemeris class filled with current SV data
|
||||
*/
|
||||
Glonass_Gnav_Ephemeris get_ephemeris();
|
||||
|
||||
/*!
|
||||
// TODO Should I keep this function ?
|
||||
* \brief Obtain a GPS ionospheric correction parameters class filled with current SV data
|
||||
*/
|
||||
Glonass_Gnav_Iono get_iono();
|
||||
|
||||
/*!
|
||||
* \brief Obtain a GLONASS GNAV UTC model parameters class filled with current SV data
|
||||
*/
|
||||
Glonass_Gnav_Utc_Model get_utc_model();
|
||||
|
||||
/*
|
||||
* \brief Returns a Galileo_Almanac object filled with the latest navigation data received
|
||||
*/
|
||||
Glonass_Gnav_Almanac get_almanac();
|
||||
|
||||
/*
|
||||
* \brief Returns true if new Ephemeris has arrived. The flag is set to false when the function is executed
|
||||
*/
|
||||
bool have_new_ephemeris();
|
||||
|
||||
/*
|
||||
* \brief Returns true if new Iono model has arrived. The flag is set to false when the function is executed
|
||||
*/
|
||||
bool have_new_iono();
|
||||
|
||||
/*
|
||||
* \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed
|
||||
*/
|
||||
bool have_new_utc_model();
|
||||
|
||||
/*
|
||||
* \brief Returns true if new UTC model has arrived. The flag is set to false when the function is executed
|
||||
*/
|
||||
bool have_new_almanac();
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \brief Decodes the GLONASS GNAV frame
|
||||
*/
|
||||
int string_decoder(char *string, int frame_ID);
|
||||
|
||||
/*!
|
||||
* \brief Computes the Coordinated Universal Time (UTC) and returns it in [s]
|
||||
*/
|
||||
double utc_time(const double gpstime_corrected) const;
|
||||
|
||||
/*!
|
||||
* Default constructor
|
||||
*/
|
||||
Glonass_Gnav_Navigation_Message();
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
@ -35,6 +35,11 @@ Glonass_Gnav_Utc_Model::Glonass_Gnav_Utc_Model()
|
|||
{
|
||||
valid = false;
|
||||
d_tau_c = 0.0;
|
||||
d_tau_gps = 0.0;
|
||||
d_N_4 = 0.0;
|
||||
d_N_A = 0.0;
|
||||
d_B1 = 0.0;
|
||||
d_B2 = 0.0;
|
||||
}
|
||||
|
||||
double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected)
|
||||
|
|
@ -43,6 +48,6 @@ double Glonass_Gnav_Utc_Model::utc_time(double glonass_time_corrected)
|
|||
|
||||
// GLONASS Time is relative to UTC Moscow, so we simply add its time difference
|
||||
t_utc = glonass_time_corrected + 3*3600 + d_tau_c;
|
||||
|
||||
|
||||
return t_utc;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -45,7 +45,30 @@ class Glonass_Gnav_Utc_Model
|
|||
public:
|
||||
bool valid;
|
||||
// UTC
|
||||
double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s]
|
||||
double d_tau_c; //!< GLONASS time scale correction to UTC(SU) time. [s]
|
||||
double d_tau_gps; //!< Correction to GPS time to GLONASS time [day]
|
||||
double d_N_4; //!< Four year interval number starting from 1996 [4 year interval]
|
||||
double d_N_A; //!< Calendar day number within the four-year period beginning since the leap year [days]
|
||||
double d_B1; //!< Coefficient to determine DeltaUT1 [s]
|
||||
double d_B2; //!< Coefficient to determine DeltaUT1 [s/msd]
|
||||
|
||||
template<class Archive>
|
||||
/*!
|
||||
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the almanac data on disk file.
|
||||
*/
|
||||
void serialize(Archive& archive, const unsigned int version)
|
||||
{
|
||||
using boost::serialization::make_nvp;
|
||||
if(version){};
|
||||
|
||||
archive & make_nvp("valid",valid);
|
||||
archive & make_nvp("d_tau_c", d_tau_c);
|
||||
archive & make_nvp("d_tau_gps", d_tau_gps);
|
||||
archive & make_nvp("d_N_4", d_N_4);
|
||||
archive & make_nvp("d_N_A", d_N_A);
|
||||
archive & make_nvp("d_B1", d_B1);
|
||||
archive & make_nvp("d_B2", d_B2);
|
||||
}
|
||||
|
||||
/*!
|
||||
* Default constructor
|
||||
|
|
@ -58,18 +81,6 @@ public:
|
|||
*/
|
||||
double utc_time(double glonass_time_corrected);
|
||||
|
||||
template<class Archive>
|
||||
/*
|
||||
* \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file.
|
||||
*/
|
||||
void serialize(Archive& archive, const unsigned int version)
|
||||
{
|
||||
using boost::serialization::make_nvp;
|
||||
if(version){};
|
||||
archive & make_nvp("valid",valid);
|
||||
archive & make_nvp("d_tau_c",d_tau_c);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -59,6 +59,11 @@
|
|||
|
||||
#include "sbas_ephemeris.h"
|
||||
|
||||
#include "glonass_gnav_ephemeris.h"
|
||||
#include "glonass_gnav_almanac.h"
|
||||
#include "glonass_gnav_iono.h"
|
||||
#include "glonass_gnav_utc_model.h"
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
DECLARE_string(log_dir);
|
||||
|
|
@ -139,7 +144,8 @@ DECLARE_string(log_dir);
|
|||
#endif
|
||||
#endif
|
||||
|
||||
|
||||
#include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc"
|
||||
#include "unit-tests/system-parameters/glonass_gnav_almanac_test.cc"
|
||||
|
||||
// For GPS NAVIGATION (L1)
|
||||
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
|
||||
|
|
|
|||
|
|
@ -0,0 +1,76 @@
|
|||
/*!
|
||||
* \file code_generation_test.cc
|
||||
* \brief This file implements tests for the generation of complex exponentials.
|
||||
* \author Carles Fernandez-Prades, 2014. cfernandez(at)cttc.es
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include <complex>
|
||||
#include <ctime>
|
||||
#include "gps_sdr_signal_processing.h"
|
||||
#include "gnss_signal_processing.h"
|
||||
|
||||
|
||||
|
||||
#include <complex>
|
||||
#include <ctime>
|
||||
#include "gnss_signal_processing.h"
|
||||
#include "glonass_gnav_almanac.h"
|
||||
|
||||
// See A 3.2.3
|
||||
TEST(GlonassGnavEphemerisTest, SatellitePosition)
|
||||
{
|
||||
double N_i = 615; // [days]
|
||||
double t_i = 33300.0 // [seconds]
|
||||
double Xoi = 10947.021572; // [km]
|
||||
double Yoi = 13078.978287; // [km]
|
||||
double Zoi = 18922.063362; // [km]
|
||||
double Vxoi = -3.375497 // [m/s]
|
||||
double Vyoi = -0.161453; // [Кm/s]
|
||||
double Vzoi = 2.060844; // [Кm/s]
|
||||
|
||||
Glonass_Gnav_Ephemeris gnav_almanac;
|
||||
|
||||
gnav_almanac.d_N_A = 615; // [days]
|
||||
gnav_almanac.d_lambda_n_A = -0.189986229; // [half cycles]
|
||||
gnav_almanac.d_t_lambda_n_A = 27122.09375; // [second]
|
||||
gnav_almanac.d_Delta_i_n_A = 0.011929512; // [half cycle]
|
||||
gnav_almanac.d_Delta_T_n_A = -2655.76171875; // [seconds]
|
||||
gnav_almanac.d_Delta_T_n_A_dot = 0.000549316; // [Secjnds/cycle2]
|
||||
gnav_almanac.d_epsilon_n_A = 0.001482010; // [unitless]
|
||||
gnav_almanac.d_omega_n_A = 0.440277100; // [Half cycle]
|
||||
|
||||
gnav_almanac.satellite_position(N_i, t_i);
|
||||
|
||||
ASSERT_TRUE(gnav_almanac.d_satpos_Xo - Xoi < DBL_EPSILON );
|
||||
ASSERT_TRUE(gnav_almanac.d_satpos_Yo - Yoi < DBL_EPSILON );
|
||||
ASSERT_TRUE(gnav_almanac.d_satpos_Zo - Zoi < DBL_EPSILON );
|
||||
ASSERT_TRUE(gnav_almanac.d_satvel_Xo - Vxoi < DBL_EPSILON );
|
||||
ASSERT_TRUE(gnav_almanac.d_satvel_Yo - Vyoi < DBL_EPSILON );
|
||||
ASSERT_TRUE(gnav_almanac.d_satvel_Zo - Vzoi < DBL_EPSILON );
|
||||
}
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
/*!
|
||||
* \file code_generation_test.cc
|
||||
* \brief This file implements tests for the generation of complex exponentials.
|
||||
* \author Carles Fernandez-Prades, 2014. cfernandez(at)cttc.es
|
||||
*
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include <complex>
|
||||
#include <ctime>
|
||||
#include "gnss_signal_processing.h"
|
||||
#include "glonass_gnav_ephemeris.h"
|
||||
|
||||
|
||||
TEST(GlonassGnavEphemerisTest, SatellitePosition)
|
||||
{
|
||||
Glonass_Gnav_Ephemeris gnav_eph;
|
||||
|
||||
gnav_eph.d_Xn = 12317.934082000;
|
||||
gnav_eph.d_Yn = -2245.13232422;
|
||||
gnav_eph.d_Zn = 22212.8173828;
|
||||
gnav_eph.d_VXn = -1.25356674194;
|
||||
gnav_eph.d_VYn = 2.774200439450;
|
||||
gnav_eph.d_VZn = 0.9808206558230000;
|
||||
gnav_eph.d_AXn = -0.931322574616e-9;
|
||||
gnav_eph.d_AYn = 0.0000000000000000;
|
||||
gnav_eph.d_AZn = -0.186264514923e-8;
|
||||
|
||||
gnav_eph.simplified_satellite_position(60);
|
||||
}
|
||||
Loading…
Reference in New Issue