mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-11-17 07:14:55 +00:00
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
Damian Miralles
parent
305a81a413
commit
85f7e333bb
@ -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)
|
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
|
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;
|
std::string line;
|
||||||
stringVersion = "3.02";
|
stringVersion = "3.02";
|
||||||
version = 3;
|
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)
|
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
|
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::vector<std::string> data;
|
||||||
std::string line_aux;
|
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
|
if(eph.d_m){} //Avoid compiler warning
|
||||||
std::string line;
|
std::string line;
|
||||||
@ -3203,23 +3207,43 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Glonass_Gnav_Ephem
|
|||||||
numberTypesObservations = 4;
|
numberTypesObservations = 4;
|
||||||
strm << numberTypesObservations;
|
strm << numberTypesObservations;
|
||||||
line += Rinex_Printer::rightJustify(strm.str(), 3);
|
line += Rinex_Printer::rightJustify(strm.str(), 3);
|
||||||
// per type of observation
|
|
||||||
// GLONASS L1 PSEUDORANGE
|
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 += std::string(1, ' ');
|
||||||
line += observationType["PSEUDORANGE"];
|
line += observationType["PSEUDORANGE"];
|
||||||
line += observationCode["GLONASS_G1_CA"];
|
line += observationCode["GLONASS_G1_CA"];
|
||||||
// GLONASS L1 PHASE
|
|
||||||
line += std::string(1, ' ');
|
line += std::string(1, ' ');
|
||||||
line += observationType["CARRIER_PHASE"];
|
line += observationType["CARRIER_PHASE"];
|
||||||
line += observationCode["GLONASS_G1_CA"];
|
line += observationCode["GLONASS_G1_CA"];
|
||||||
// GLONASS DOPPLER L1
|
|
||||||
line += std::string(1, ' ');
|
line += std::string(1, ' ');
|
||||||
line += observationType["DOPPLER"];
|
line += observationType["DOPPLER"];
|
||||||
line += observationCode["GLONASS_G1_CA"];
|
line += observationCode["GLONASS_G1_CA"];
|
||||||
// GLONASS L1 CA SIGNAL STRENGTH
|
|
||||||
line += std::string(1, ' ');
|
line += std::string(1, ' ');
|
||||||
line += observationType["SIGNAL_STRENGTH"];
|
line += observationType["SIGNAL_STRENGTH"];
|
||||||
line += observationCode["GLONASS_G1_CA"];
|
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 += std::string(60-line.size(), ' ');
|
||||||
line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20);
|
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.
|
// RINEX observations timestamps are GPS timestamps.
|
||||||
std::string line;
|
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);
|
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);
|
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));
|
//double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time));
|
||||||
|
|||||||
@ -69,19 +69,21 @@ glonass_l1_ca_telemetry_decoder_cc::glonass_l1_ca_telemetry_decoder_cc(
|
|||||||
d_dump = dump;
|
d_dump = dump;
|
||||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||||
LOG(INFO) << "Initializing GLONASS L1 CA TELEMETRY PROCESSING";
|
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;
|
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;
|
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));
|
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
|
// preamble bits to sampled symbols
|
||||||
d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * d_symbols_per_preamble);
|
d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * d_symbols_per_preamble);
|
||||||
int n = 0;
|
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++)
|
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];
|
// 1. Transform from symbols to bits
|
||||||
|
std::string frame_string;
|
||||||
// 2. Viterbi decoder
|
for(int i = 0; i < (frame_length); i++)
|
||||||
// 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++)
|
|
||||||
{
|
{
|
||||||
if ((i + 1) % 2 == 0)
|
if (frame_symbols[i] > 0)
|
||||||
{
|
{
|
||||||
page_part_symbols_deint[i] = -page_part_symbols_deint[i];
|
frame_string.push_back('1');
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
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');
|
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
page_String.push_back('0');
|
frame_string.push_back('0');
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 2. Call the GLONASS GNAV string decoder
|
||||||
d_nav.decode_string(page_String);
|
d_nav.decode_string(page_String);
|
||||||
|
|
||||||
|
// 3. Check operation executed correctly
|
||||||
if(d_nav.flag_CRC_test == true)
|
if(d_nav.flag_CRC_test == true)
|
||||||
{
|
{
|
||||||
LOG(INFO) << "GLONASS GNAV CRC correct on channel " << d_channel << " from satellite " << d_satellite;
|
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;
|
LOG(INFO) << "GLONASS GNAV CRC error on channel " << d_channel << " from satellite " << d_satellite;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// 4. Push the new navigation data to the queues
|
// 4. Push the new navigation data to the queues
|
||||||
if (d_nav.have_new_ephemeris() == true)
|
if (d_nav.have_new_ephemeris() == true)
|
||||||
{
|
{
|
||||||
// get object for this SV (mandatory)
|
// get object for this SV (mandatory)
|
||||||
std::shared_ptr<Glonass_Gnav_Ephemeris> tmp_obj = std::make_shared<Glonass_Gnav_Ephemeris>(d_nav.get_ephemeris());
|
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));
|
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());
|
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));
|
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)),
|
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)
|
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);
|
consume_each(1);
|
||||||
|
|
||||||
d_flag_preamble = false;
|
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)
|
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
|
// NEW GLONASS string received
|
||||||
// 0. fetch the symbols into an array
|
// 0. fetch the symbols into an array
|
||||||
int frame_length = GLONASS_GNAV_STRING_SYMBOLS - d_symbols_per_preamble;
|
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 *******
|
//******* SYMBOL TO BIT *******
|
||||||
if (d_symbol_history.at(0).Flag_valid_symbol_output == true)
|
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)
|
if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true)
|
||||||
//update TOW at the preamble instant
|
//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)
|
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;
|
||||||
{
|
|
||||||
//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;
|
|
||||||
}
|
|
||||||
|
|
||||||
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
|
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_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
|
// 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)));
|
// 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)
|
if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true)
|
||||||
{
|
{
|
||||||
|
|||||||
@ -77,12 +77,13 @@ private:
|
|||||||
glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump);
|
glonass_l1_ca_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump);
|
||||||
glonass_l1_ca_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
|
//!< Preamble decoding
|
||||||
unsigned short int d_preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS];
|
unsigned short int d_preambles_bits[GLONASS_GNAV_PREAMBLE_LENGTH_BITS];
|
||||||
int *d_preambles_symbols;
|
int *d_preambles_symbols;
|
||||||
unsigned int d_samples_per_symbol;
|
unsigned int d_samples_per_symbol;
|
||||||
|
unsigned int d_samples_per_preamble_symbol;
|
||||||
int d_symbols_per_preamble;
|
int d_symbols_per_preamble;
|
||||||
|
|
||||||
//!< Storage for incoming data
|
//!< Storage for incoming data
|
||||||
|
|||||||
@ -94,13 +94,14 @@ const double GLONASS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (
|
|||||||
const int GLONASS_L1_CA_HISTORY_DEEP = 100;
|
const int GLONASS_L1_CA_HISTORY_DEEP = 100;
|
||||||
|
|
||||||
// NAVIGATION MESSAGE DEMODULATION AND DECODING
|
// 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}
|
#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_CA_PREAMBLE_LENGTH_BITS = 30;
|
const int GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30;
|
||||||
const int GLONASS_CA_PREAMBLE_LENGTH_SYMBOLS = 300;
|
const int GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300;
|
||||||
const double GLONASS_CA_PREAMBLE_DURATION_S = 0.3;
|
const double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.3;
|
||||||
const int GLONASS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s]
|
const int GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s]
|
||||||
const int GLONASS_CA_TELEMETRY_SYMBOLS_PER_BIT = 10;
|
const int GLONASS_GNAV_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]
|
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_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_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]
|
const int GLONASS_GNAV_FRAME_SECONDS = 30; //!< Subframe duration [seconds]
|
||||||
|
|||||||
@ -81,18 +81,6 @@ Glonass_Gnav_Ephemeris::Glonass_Gnav_Ephemeris()
|
|||||||
d_tau_c = 0.0;
|
d_tau_c = 0.0;
|
||||||
d_TOW = 0.0; // tow of the start of frame
|
d_TOW = 0.0; // tow of the start of frame
|
||||||
d_WN = 0.0; // week number 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 Glonass_Gnav_Ephemeris::check_t(double time)
|
||||||
{
|
{
|
||||||
double corrTime;
|
double corrTime;
|
||||||
@ -251,536 +123,3 @@ double Glonass_Gnav_Ephemeris::sv_clock_drift(double transmitTime, double timeCo
|
|||||||
|
|
||||||
return d_satClkDrift;
|
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;
|
|
||||||
}
|
|
||||||
|
|||||||
@ -60,16 +60,6 @@ private:
|
|||||||
*/
|
*/
|
||||||
double check_t(double time);
|
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:
|
public:
|
||||||
double d_m; //!< String number within frame [dimensionless]
|
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]
|
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_TOW; // tow of the start of frame
|
||||||
double d_WN; // week number 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>
|
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]
|
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
|
* \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)
|
* (IS-GPS-200E, 20.3.3.3.3.1)
|
||||||
*/
|
*/
|
||||||
double sv_clock_drift(double transmitTime, double timeCorrUTC);
|
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
|
* \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
|
* \ param offset_time Is the start of day offset to compute the time
|
||||||
|
|||||||
@ -1,7 +1,9 @@
|
|||||||
/*!
|
/*!
|
||||||
* \file glonass_gnav_utc_model.h
|
* \file glonass_gnav_utc_model.h
|
||||||
* \brief Interface of a GLONASS GNAV UTC MODEL storage
|
* \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
|
* \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>
|
||||||
*
|
*
|
||||||
* -------------------------------------------------------------------------
|
* -------------------------------------------------------------------------
|
||||||
*
|
*
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user