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

bug_fix: Fixes bugs in telemetry decoding interface 

Fixes several bugs with the telemetry decoder interface and clean up the
code with unused methods and members of the ephemeris object
This commit is contained in:
Damian Miralles 2017-08-23 15:16:07 -07:00 committed by Damian Miralles
parent 305a81a413
commit 85f7e333bb
7 changed files with 80 additions and 798 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
src/algorithms/PVT/libs/rinex_printer.cc
View File

@ -633,6 +633,8 @@ void Rinex_Printer::rinex_nav_header(std::fstream& out, const Gps_Iono& gps_iono
void Rinex_Printer::rinex_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac)
{
if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning
//Avoid compiler warning, there is not time system correction between Galileo and GLONASS
if(galileo_almanac.A_0G_10){}
std::string line;
stringVersion = "3.02";
version = 3;
@ -2047,6 +2049,8 @@ void Rinex_Printer::update_nav_header(std::fstream& out, const Gps_Iono& gps_ion
void Rinex_Printer::update_nav_header(std::fstream& out, const Galileo_Iono& galileo_iono, const Galileo_Utc_Model& galileo_utc_model, const Galileo_Almanac& galileo_almanac, const Glonass_Gnav_Utc_Model& glonass_gnav_utc_model, const Glonass_Gnav_Almanac& glonass_gnav_almanac)
{
if(glonass_gnav_almanac.i_satellite_freq_channel){} //Avoid compiler warning
//Avoid compiler warning, there is not time system correction between Galileo and GLONASS
if(galileo_almanac.A_0G_10){}
std::vector<std::string> data;
std::string line_aux;
@ -3035,7 +3039,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::map<int, Galileo
}
void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephemeris& eph, const double d_TOW_first_observation, const std::string bands)
void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephemeris& eph, const double d_TOW_first_observation, const std::string glonass_bands)
{
if(eph.d_m){} //Avoid compiler warning
std::string line;
@ -3203,23 +3207,43 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephem
numberTypesObservations = 4;
strm << numberTypesObservations;
line += Rinex_Printer::rightJustify(strm.str(), 3);
// per type of observation
// GLONASS L1 PSEUDORANGE
line += std::string(1, ' ');
line += observationType["PSEUDORANGE"];
line += observationCode["GLONASS_G1_CA"];
// GLONASS L1 PHASE
line += std::string(1, ' ');
line += observationType["CARRIER_PHASE"];
line += observationCode["GLONASS_G1_CA"];
// GLONASS DOPPLER L1
line += std::string(1, ' ');
line += observationType["DOPPLER"];
line += observationCode["GLONASS_G1_CA"];
// GLONASS L1 CA SIGNAL STRENGTH
line += std::string(1, ' ');
line += observationType["SIGNAL_STRENGTH"];
line += observationCode["GLONASS_G1_CA"];
std::string signal_ = "1C";
std::size_t found_1C = glonass_bands.find(signal_);
signal_ = "2C";
std::size_t found_2C = glonass_bands.find(signal_);
if(found_1C != std::string::npos)
{
line += std::string(1, ' ');
line += observationType["PSEUDORANGE"];
line += observationCode["GLONASS_G1_CA"];
line += std::string(1, ' ');
line += observationType["CARRIER_PHASE"];
line += observationCode["GLONASS_G1_CA"];
line += std::string(1, ' ');
line += observationType["DOPPLER"];
line += observationCode["GLONASS_G1_CA"];
line += std::string(1, ' ');
line += observationType["SIGNAL_STRENGTH"];
line += observationCode["GLONASS_G1_CA"];
}
if(found_2C != std::string::npos)
{
line += std::string(1, ' ');
line += observationType["PSEUDORANGE"];
line += observationCode["GLONASS_G2_CA"];
line += std::string(1, ' ');
line += observationType["CARRIER_PHASE"];
line += observationCode["GLONASS_G2_CA"];
line += std::string(1, ' ');
line += observationType["DOPPLER"];
line += observationCode["GLONASS_G2_CA"];
line += std::string(1, ' ');
line += observationType["SIGNAL_STRENGTH"];
line += observationCode["GLONASS_G2_CA"];
}
line += std::string(60-line.size(), ' ');
line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20);
@ -5382,6 +5406,9 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
// RINEX observations timestamps are GPS timestamps.
std::string line;
// Avoid compiler warning
if(glonass_band.size()){}
boost::posix_time::ptime p_glonass_time = Rinex_Printer::compute_GLONASS_time(eph, obs_time);
std::string timestring = boost::posix_time::to_iso_string(p_glonass_time);
//double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time));

86
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.cc
View File

@ -69,19 +69,21 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc(
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "Initializing GLONASS L1 CA TELEMETRY PROCESSING";
// TODO. WHAT IS THIS?
// Define the number of sampes per symbol. Notice that GLONASS has to rates,
//one for the navigation data and the other for the preamble information
d_samples_per_symbol = ( GLONASS_L1_CODE_CHIP_RATE_HZ / GLONASS_L1_CA_CODE_LENGTH_CHIPS ) / GLONASS_L1_CA_SYMBOL_RATE_BPS;
// set the preamble
// Set the preamble information
unsigned short int preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS] = GLONASS_GNAV_PREAMBLE;
d_symbols_per_preamble = GLONASS_GNAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol;
// Since preamble rate is different than navigation data rate we use a constant
d_symbols_per_preamble = GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS;
memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, GLONASS_GNAV_PREAMBLE_LENGTH_BITS*sizeof(unsigned short int));
// preamble bits to sampled symbols
d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * d_symbols_per_preamble);
int n = 0;
for (int i = 0; i < GLONASS_GNAV_PREAMBLE_LENGTH_BITS; i++)
for (int i = 0; i < GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT; i++)
{
for (unsigned int j = 0; j < d_samples_per_symbol; j++)
{
@ -130,40 +132,26 @@ glonass_l1_ca_telemetry_decoder_cc::~glonass_l1_ca_telemetry_decoder_cc()
}
void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols,int frame_length)
void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *frame_symbols,int frame_length)
{
double page_part_symbols_deint[frame_length];
// 2. Viterbi decoder
// 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder)
// 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180<38>
for (int i = 0; i < frame_length; i++)
// 1. Transform from symbols to bits
std::string frame_string;
for(int i = 0; i < (frame_length); i++)
{
if ((i + 1) % 2 == 0)
if (frame_symbols[i] > 0)
{
page_part_symbols_deint[i] = -page_part_symbols_deint[i];
}
}
int page_part_bits[frame_length/2];
viterbi_decoder(page_part_symbols_deint, page_part_bits);
// 3. Call the Galileo page decoder
std::string page_String;
for(int i = 0; i < (frame_length/2); i++)
{
if (page_part_bits[i] > 0)
{
page_String.push_back('1');
frame_string.push_back('1');
}
else
{
page_String.push_back('0');
frame_string.push_back('0');
}
}
// 2. Call the GLONASS GNAV string decoder
d_nav.decode_string(page_String);
// 3. Check operation executed correctly
if(d_nav.flag_CRC_test == true)
{
LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite " << d_satellite;
@ -175,14 +163,11 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols
LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite;
}
// 4. Push the new navigation data to the queues
if (d_nav.have_new_ephemeris() == true)
{
// get object for this SV (mandatory)
std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
@ -196,19 +181,10 @@ void glonass_l1_ca_telemetry_decoder_cc::decode_string(double *page_part_symbols
{
std::shared_ptr<Glonass_Gnav_Almanac> tmp_obj= std::make_shared<Glonass_Gnav_Almanac>(d_nav.get_almanac());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
//debug
std::cout << "GLONASS GNAV almanac received!" << std::endl;
DLOG(INFO) << "Current parameters:";
DLOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol;
DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0;
delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (d_TOW_at_current_symbol - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64)));
DLOG(INFO) << "delta_t=" << delta_t << "[s]";
}
}
int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
@ -226,7 +202,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib
consume_each(1);
d_flag_preamble = false;
unsigned int required_symbols=GLONASS_GNAV_PAGE_SYMBOLS+d_symbols_per_preamble;
unsigned int required_symbols=GLONASS_GNAV_FRAME_BITS+d_symbols_per_preamble;
if (d_symbol_history.size()>required_symbols)
{
@ -283,7 +259,7 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib
// NEW GLONASS string received
// 0. fetch the symbols into an array
int frame_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble;
double page_part_symbols[frame_length];
double frame_symbols[frame_length];
//******* SYMBOL TO BIT *******
if (d_symbol_history.at(0).Flag_valid_symbol_output == true)
@ -342,36 +318,20 @@ int glonass_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attrib
if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true)
//update TOW at the preamble instant
{
if(d_nav.flag_TOW_5 == true) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
//TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_current_symbol = d_nav.TOW_5 + GALILEO_INAV_PAGE_PART_SECONDS+((double)required_symbols)*GALILEO_E1_CODE_PERIOD; //-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND;
d_nav.flag_TOW_5 = false;
}
d_TOW_at_current_symbol = floor((d_nav.d_TOW + 2*GLONASS_L1_CA_CODE_PERIOD + GLONASS_CA_PREAMBLE_DURATION_S)*1000.0)/1000.0;
else if(d_nav.flag_TOW_6 == true) //page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{
//TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_current_symbol = d_nav.TOW_6 + GALILEO_INAV_PAGE_PART_SECONDS+((double)required_symbols)*GALILEO_E1_CODE_PERIOD;//-GALILEO_E1_CODE_PERIOD;//+ (double)GALILEO_INAV_PREAMBLE_LENGTH_BITS/(double)GALILEO_TELEMETRY_RATE_BITS_SECOND;
d_nav.flag_TOW_6 = false;
}
else
{
//this page has no timing information
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD;// + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD;
}
}
else //if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E1_CODE_PERIOD;
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L1_CA_CODE_PERIOD;
}
//if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived
{
delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64)));
}
// if(d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) //all GGTO parameters arrived
// {
// delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64)));
// }
if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true)
{

3
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_cc.h
View File

@ -77,12 +77,13 @@ private:
glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump);
glonass_l1_ca_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump);
void decode_word(double *symbols);
void decode_string(double *symbols, int frame_length);
//!< Preamble decoding
unsigned short int d_preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS];
int *d_preambles_symbols;
unsigned int d_samples_per_symbol;
unsigned int d_samples_per_preamble_symbol;
int d_symbols_per_preamble;
//!< Storage for incoming data

15
src/core/system_parameters/GLONASS_L1_CA.h
View File

@ -94,13 +94,14 @@ const double GLONASS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (
const int GLONASS_L1_CA_HISTORY_DEEP = 100;
// NAVIGATION MESSAGE DEMODULATION AND DECODING
#define GLONASS_CA_PREAMBLE {1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0}
const int GLONASS_CA_PREAMBLE_LENGTH_BITS = 30;
const int GLONASS_CA_PREAMBLE_LENGTH_SYMBOLS = 300;
const double GLONASS_CA_PREAMBLE_DURATION_S = 0.3;
const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s]
const int GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT = 10;
const int GLONASS_CA_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_CA_TELEMETRY_RATE_BITS_SECOND*GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s]
#define GLONASS_GNAV_PREAMBLE {1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0}
const int GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30;
const int GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300;
const double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.3;
const int GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s]
const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT = 10;
const int GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT = 10;
const int GLONASS_GNAV_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND*GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s]
const int GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 1700;
const int GLONASS_GNAV_FRAME_BITS = 1725; //!< Number of chips per frame in the GNAV message 15 strings*(85 data bits + 30 time mark bits)[bits]
const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds]

661
src/core/system_parameters/glonass_gnav_ephemeris.cc
View File

@ -81,18 +81,6 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris()
d_tau_c = 0.0;
d_TOW = 0.0; // tow of the start of frame
d_WN = 0.0; // week number of the start of frame
// 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]
}
@ -107,122 +95,6 @@ boost::posix_time::ptime Glonass_Gnav_Ephemeris::compute_GLONASS_time(const doub
}
void Glonass_Gnav_Ephemeris::gravitational_perturbations()
{
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 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_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 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
T = (27392.375 + sigma_days + (t_e/86400))/(36525);
tau_prime = GLONASS_TAU_0 + GLONASS_TAU_1 * T;
Omega_moon = GLONASS_MOON_OMEGA_0 + GLONASS_MOON_OMEGA_1*T;
q_moon = GLONASS_MOON_Q0 + GLONASS_MOON_Q1*T;
q_sun = GLONASS_SUN_Q0 + GLONASS_SUN_Q1*T;
xi_star = 1 - (cos(Omega_moon)*cos(Omega_moon))*(1 - cos(GLONASS_MOON_INCLINATION));
eta_star = sin(Omega_moon)*(sin(GLONASS_MOON_INCLINATION));
zeta_star = cos(Omega_moon)*(sin(GLONASS_MOON_INCLINATION));
xi_11 = sin(Omega_moon)*cos(Omega_moon)*(1 - cos(GLONASS_MOON_INCLINATION));
xi_12 = 1 - (sin(Omega_moon)*sin(Omega_moon))*(1 - cos(GLONASS_MOON_INCLINATION));
eta_11 = xi_star*cos(GLONASS_EARTH_INCLINATION) - xi_star*sin(GLONASS_EARTH_INCLINATION);
eta_12 = xi_11*cos(GLONASS_EARTH_INCLINATION) + eta_star*sin(GLONASS_EARTH_INCLINATION);
zeta_11 = xi_star*sin(GLONASS_EARTH_INCLINATION) + zeta_star*cos(GLONASS_EARTH_INCLINATION);
zeta_12 = xi_11*sin(GLONASS_EARTH_INCLINATION) + eta_star*cos(GLONASS_EARTH_INCLINATION);
/// \TODO why is this calling sin(E_moon) in the same line where it is being computed
E_moon = q_moon + GLONASS_MOON_ECCENTRICITY*sin(E_moon);
sin_theta_moon = sqrt(1 - GLONASS_MOON_ECCENTRICITY*GLONASS_MOON_ECCENTRICITY)*sin(E_moon)/(1 - GLONASS_MOON_ECCENTRICITY*cos(E_moon));
cos_theta_moon = cos(E_moon - GLONASS_MOON_ECCENTRICITY)/(1 - GLONASS_MOON_ECCENTRICITY*cos(E_moon));
theta_moon = asin(sin_theta_moon);
xi_moon_e = sin(theta_moon + tau_prime)*xi_11 + cos(theta_moon + tau_prime)*xi_12;
eta_moon_e = sin(theta_moon + tau_prime)*eta_11 + cos(theta_moon + tau_prime)*eta_12;
zeta_moon_e = sin(theta_moon + tau_prime)*zeta_11 + cos(theta_moon + tau_prime)*zeta_12;
r_moon_e = GLONASS_MOON_SEMI_MAJOR_AXIS*(1-GLONASS_MOON_ECCENTRICITY*cos(E_moon));
/// \TODO why is this calling sin(E_moon) in the same line where it is being computed
E_sun = q_sun + GLONASS_SUN_ECCENTRICITY*sin(E_sun);
sin_theta_sun = sqrt(1 - GLONASS_SUN_ECCENTRICITY*GLONASS_SUN_ECCENTRICITY)*sin(E_sun)/(1 - GLONASS_SUN_ECCENTRICITY*cos(E_sun));
cos_theta_sun = cos(E_sun - GLONASS_SUN_ECCENTRICITY)/(1 - GLONASS_SUN_ECCENTRICITY*cos(E_sun));
theta_sun = asin(sin_theta_sun);
xi_sun_e = (cos_theta_sun*cos(GLONASS_SUN_OMEGA) - sin_theta_sun*sin(GLONASS_SUN_OMEGA));
eta_sun_e = (sin_theta_sun*cos(GLONASS_SUN_OMEGA) + cos_theta_sun*sin(GLONASS_SUN_OMEGA))*cos(GLONASS_EARTH_INCLINATION);
zeta_sun_e = (sin_theta_sun*cos(GLONASS_SUN_OMEGA) + cos_theta_sun*sin(GLONASS_SUN_OMEGA))*sin(GLONASS_EARTH_INCLINATION);
r_sun_e = GLONASS_SUN_SEMI_MAJOR_AXIS*(1-GLONASS_SUN_ECCENTRICITY*cos(E_sun));
//<! Gravitational computation
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/(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;
d_Jz_moon = mu_bar_moon*(zeta_moon_e - z_bar_moon)/pow(Delta_o_moon,3.0) - zeta_moon_e;
d_Jx_sun = mu_bar_sun*(xi_sun_e - x_bar_sun)/pow(Delta_o_sun,3.0) - xi_sun_e;
d_Jy_sun = mu_bar_sun*(eta_sun_e - y_bar_sun)/pow(Delta_o_sun,3.0) - eta_sun_e;
d_Jz_sun = mu_bar_sun*(zeta_sun_e - z_bar_sun)/pow(Delta_o_sun,3.0) - zeta_sun_e;
}
double Glonass_Gnav_Ephemeris::check_t(double time)
{
double corrTime;
@ -251,536 +123,3 @@ 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)
{
// Compute relativistic correction term
d_dtr = 0.0;
return d_dtr;
}
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
d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt);
// Find integration time
tk = dt - d_satClkDrift;
// Check if to integrate forward or backward
if(tk < 0)
{
tau = -60;
}
else
{
tau = 60;
}
// x,y,z coordinates to meters
x_0 = d_Xn * 1e3;
y_0 = d_Yn * 1e3;
z_0 = d_Zn * 1e3;
// x,y,z velocities to meters/s
Vx_0 = d_VXn * 1e3;
Vy_0 = d_VYn * 1e3;
Vz_0 = d_VZn * 1e3;
// x,y,z accelerations to meters/sec^2
Ax_0 = d_AXn * 1e3;
Ay_0 = d_AYn * 1e3;
Az_0 = d_AZn * 1e3;
for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau)
{
// Check if last integration step. If last integration step, make one more step that has the remaining time length...
if(fabs(numberOfIntegrations) > fabs(tk))
{
// if there is more time left to integrate...
if(fmod(tk,tau) != 0)
{
tau = fmod(tk,tau);
}
else // otherwise make a zero step.
{
tau = 0;
}
}
// Runge-Kutta Integration Stage K1
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 * 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 = Vx_0;
dy_1 = Vy_0;
dz_1 = Vz_0;
// Runge-Kutta Integration Stage K2
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 = 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 * 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 = 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 = 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;
r2_3 = r2*r2*r2;
r2_5 = r2*r2*r2*r2*r2;
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 = Vx_0 + tau * dVx_3;
Vy_3 = Vy_0 + tau * dVy_3;
Vz_3 = Vz_0 + tau * dVz_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 * 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
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 );
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 = x_0;
d_satpos_Y = y_0;
d_satpos_Z = z_0;
d_satvel_X = Vx_0;
d_satvel_Y = Vy_0;
d_satvel_Z = Vz_0;
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);
//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);
return 0;
}
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
d_satClkDrift = -(d_tau_n + /*d_tau_c*/ - d_gamma_n * dt);
// Find integration time
tk = dt - d_satClkDrift;
// Check if to integrate forward or backward
if (tk < 0)
{
tau = -60;
}
else
{
tau = 60;
}
// Coordinates transformation to an inertial reference frame
// x,y,z coordinates to meters
x_0 = d_Xn * 1e3;
y_0 = d_Yn * 1e3;
z_0 = d_Zn * 1e3;
// x,y,z velocities to meters/s
Vx_0 = d_VXn * 1e3;
Vy_0 = d_VYn * 1e3;
Vz_0 = d_VZn * 1e3;
// x,y,z accelerations to meters/sec^2
Ax_0 = d_AXn * 1e3;
Ay_0 = d_AYn * 1e3;
Az_0 = d_AZn * 1e3;
for(numberOfIntegrations = tau; numberOfIntegrations < tk+tau; numberOfIntegrations += tau)
{
// Check if last integration step. If last integration step, make one more step that has the remaining time length...
if(fabs(numberOfIntegrations) > fabs(tk))
{
// if there is more time left to integrate...
if (fmod(tk,tau) != 0)
{
tau = fmod(tk,tau);
}
else // otherwise make a zero step.
{
tau = 0;
}
}
// Runge-Kutta Integration Stage K1
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 = Vx_0;
dy_1 = Vy_0;
dz_1 = Vz_0;
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 = Vx_0;
dy_1 = Vy_0;
dz_1 = Vz_0;
// Runge-Kutta Integration Stage K2
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 = 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 * 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 = 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 = 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;
r2_3 = r2*r2*r2;
r2_5 = r2*r2*r2*r2*r2;
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 = Vx_0 + tau * dVx_3;
Vy_3 = Vy_0 + tau * dVy_3;
Vz_3 = Vz_0 + tau * dVz_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 * 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
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 );
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);
//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);
return d_dtr;
}

48
src/core/system_parameters/glonass_gnav_ephemeris.h
View File

@ -60,16 +60,6 @@ private:
*/
double check_t(double time);
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:
double d_m; //!< String number within frame [dimensionless]
double d_t_k; //!< GLONASS Time (UTC(SU) + 3 h) referenced to the beginning of the frame within the current day [s]
@ -115,19 +105,6 @@ public:
double d_TOW; // tow of the start of frame
double d_WN; // week number of the start of frame
// satellite positions after RK4 Integration
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 after RK4 Integration
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 after RK4 Integration
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>
/*!
@ -171,37 +148,12 @@ public:
archive & make_nvp("d_l5th_n", d_l5th_n); //!< Health flag for nth satellite; ln = 0 indicates the n-th satellite is helthy, ln = 1 indicates malfunction of this nth satellite [dimensionless]
}
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Algorithm A.3.1.2 in GLONASS ICD v5.1
* and compute the clock bias term including relativistic effect (return value)
* \param transmitTime Time of ephemeris transmission
* \return clock bias of satellite
*/
double simplified_satellite_position(double transmitTime);
/*!
* \brief Compute the ECEF SV coordinates and ECEF velocity
* Implementation of Algorithm A.3.1.1 in GLONASS ICD v5.1
* and compute the clock bias term including relativistic effect (return value)
* \param transmitTime Time of ephemeris transmission
* \return clock bias of satellite
*/
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
* (IS-GPS-200E, 20.3.3.3.3.1)
*/
double sv_clock_drift(double transmitTime, double timeCorrUTC);
/*!
* \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction
* (IS-GPS-200E, 20.3.3.3.3.1)
*/
double sv_clock_relativistic_term(double transmitTime);
/*!
* \brief Computes the GLONASS System Time and returns a boost::posix_time::ptime object
* \ param offset_time Is the start of day offset to compute the time

2
src/core/system_parameters/glonass_gnav_utc_model.h
View File

@ -1,7 +1,9 @@
/*!
* \file glonass_gnav_utc_model.h
* \brief Interface of a GLONASS GNAV UTC MODEL storage
* \note Code added as part of GSoC 2017 program
* \author Damian Miralles, 2017. dmiralles2009(at)gmail.com
* \see <a href="http://russianspacesystems.ru/wp-content/uploads/2016/08/ICD_GLONASS_eng_v5.1.pdf">GLONASS ICD</a>
*
* -------------------------------------------------------------------------
*