From ea8e605fb5389c53464c25f1df644aaff9e7b173 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 3 Nov 2016 00:07:05 +0100 Subject: [PATCH] Add work on the hybrid receiver --- src/algorithms/PVT/adapters/hybrid_pvt.cc | 60 +++- .../PVT/gnuradio_blocks/hybrid_pvt_cc.cc | 337 +++++++++++++++--- .../PVT/gnuradio_blocks/hybrid_pvt_cc.h | 15 +- src/algorithms/PVT/libs/hybrid_ls_pvt.h | 3 + .../adapters/hybrid_observables.cc | 18 +- .../gnuradio_blocks/hybrid_observables_cc.cc | 90 ++--- 6 files changed, 398 insertions(+), 125 deletions(-) diff --git a/src/algorithms/PVT/adapters/hybrid_pvt.cc b/src/algorithms/PVT/adapters/hybrid_pvt.cc index dcd202bc4..98b7a3836 100644 --- a/src/algorithms/PVT/adapters/hybrid_pvt.cc +++ b/src/algorithms/PVT/adapters/hybrid_pvt.cc @@ -110,8 +110,66 @@ HybridPvt::HybridPvt(ConfigurationInterface* configuration, //std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); //std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); + // Infer the type of receiver + /* + * TYPE | RECEIVER + * 0 | Unknown + * 1 | GPS L1 C/A + * 2 | GPS L2C + * 3 | GPS L5 + * 4 | Galileo E1B + * 5 | Galileo E5a + * 6 | Galileo E5b + * 7 | GPS L1 C/A + GPS L2C + * 8 | GPS L1 C/A + GPS L5 + * 9 | GPS L1 C/A + Galileo E1B + * 10 | GPS L1 C/A + Galileo E5a + * 11 | GPS L1 C/A + Galileo E5b + * 12 | Galileo E1B + GPS L2C + * 13 | Galileo E1B + GPS L5 + * 14 | Galileo E1B + Galileo E5a + * 15 | Galileo E1B + Galileo E5b + * 16 | GPS L2C + GPS L5 + * 17 | GPS L2C + Galileo E5a + * 18 | GPS L2C + Galileo E5b + * 19 | GPS L5 + Galileo E5a + * 20 | GPS L5 + Galileo E5b + * 21 | GPS L1 C/A + Galileo E1B + GPS L2C + * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + */ + int gps_1C_count = configuration->property("Channels_1C.count", 0); + int gps_2S_count = configuration->property("Channels_2S.count", 0); + int gal_1B_count = configuration->property("Channels_1B.count", 0); + int gal_E5a_count = configuration->property("Channels_5X.count", 0); // GPS L5 or Galileo E5a ? + int gal_E5b_count = configuration->property("Channels_7X.count", 0); + + unsigned int type_of_receiver = 0; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 1; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 2; + + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 4; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 5; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 6; + + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 7; + //if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 8; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 9; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 10; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 11; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 12; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 13; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 14; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 15; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 16; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 17; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 18; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 19; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 20; + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 21; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count = 0)) type_of_receiver = 22; + // make PVT object - pvt_ = hybrid_make_pvt_cc(in_streams_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname); + pvt_ = hybrid_make_pvt_cc(in_streams_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname, type_of_receiver); DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; } diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index 6e8757641..c0b7281c5 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -57,7 +57,8 @@ hybrid_make_pvt_cc(unsigned int nchannels, unsigned short rtcm_tcp_port, unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname) + std::string rtcm_dump_devname, + const unsigned int type_of_receiver) { return hybrid_pvt_cc_sptr(new hybrid_pvt_cc(nchannels, dump, @@ -74,7 +75,8 @@ hybrid_make_pvt_cc(unsigned int nchannels, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, - rtcm_dump_devname)); + rtcm_dump_devname, + type_of_receiver)); } @@ -148,6 +150,32 @@ void hybrid_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) d_ls_pvt->galileo_almanac = *galileo_almanac; DLOG(INFO) << "New Galileo Almanac has arrived "; } + + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV message ### + std::shared_ptr gps_cnav_ephemeris; + gps_cnav_ephemeris = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; + DLOG(INFO) << "New GPS CNAV ephemeris record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV IONO ### + std::shared_ptr gps_cnav_iono; + gps_cnav_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_cnav_iono = *gps_cnav_iono; + DLOG(INFO) << "New CNAV IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV UTC MODEL ### + std::shared_ptr gps_cnav_utc_model; + gps_cnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_cnav_utc_model = *gps_cnav_utc_model; + DLOG(INFO) << "New CNAV UTC record has arrived "; + } else { LOG(WARNING) << "msg_handler_telemetry unknown object type!"; @@ -171,7 +199,7 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname, bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname) : + unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname, const unsigned int type_of_receiver) : gr::block("hybrid_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::io_signature::make(0, 0, sizeof(gr_complex))) @@ -182,6 +210,7 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump d_nchannels = nchannels; d_dump_filename = dump_filename; std::string dump_ls_pvt_filename = dump_filename; + type_of_rx = type_of_receiver; // GPS Ephemeris data message port in this->message_port_register_in(pmt::mp("telemetry")); @@ -296,7 +325,7 @@ hybrid_pvt_cc::~hybrid_pvt_cc() -bool hybrid_pvt_cc::pseudoranges_pairCompare_min(const std::pair& a, const std::pair& b) +bool hybrid_pvt_cc::observables_pairCompare_min(const std::pair& a, const std::pair& b) { return (a.second.Pseudorange_m) < (b.second.Pseudorange_m); } @@ -340,7 +369,7 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v unsigned int gps_channel = 0; unsigned int gal_channel = 0; - gnss_pseudoranges_map.clear(); + gnss_observables_map.clear(); Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer @@ -351,7 +380,7 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if (in[i][0].Flag_valid_pseudorange == true) { - gnss_pseudoranges_map.insert(std::pair(i, in[i][0])); // store valid pseudoranges in a map. + gnss_observables_map.insert(std::pair(i, in[i][0])); // store valid observables in a map. //d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges) d_TOW_at_curr_symbol_constellation = in[i][0].d_TOW_at_current_symbol; // d_TOW_at_current_symbol not corrected by delta t (just for debug) d_rx_time = in[i][0].d_TOW_hybrid_at_current_symbol; // hybrid rx time, all the channels have the same RX timestamp (common RX time pseudoranges) @@ -371,20 +400,57 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time } } + if(d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + std::map::iterator tmp_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][0].PRN); + if(tmp_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time + } + } } } + std::map::iterator galileo_ephemeris_iter; + std::map::iterator gps_ephemeris_iter; + std::map::iterator gps_cnav_ephemeris_iter; + std::map::iterator gnss_observables_iter; + + /* + * TYPE | RECEIVER + * 0 | Unknown + * 1 | GPS L1 C/A + * 2 | GPS L2C + * 3 | GPS L5 + * 4 | Galileo E1B + * 5 | Galileo E5a + * 6 | Galileo E5b + * 7 | GPS L1 C/A + GPS L2C + * 8 | GPS L1 C/A + GPS L5 + * 9 | GPS L1 C/A + Galileo E1B + * 10 | GPS L1 C/A + Galileo E5a + * 11 | GPS L1 C/A + Galileo E5b + * 12 | Galileo E1B + GPS L2C + * 13 | Galileo E1B + GPS L5 + * 14 | Galileo E1B + Galileo E5a + * 15 | Galileo E1B + Galileo E5b + * 16 | GPS L2C + GPS L5 + * 17 | GPS L2C + Galileo E5a + * 18 | GPS L2C + Galileo E5b + * 19 | GPS L5 + Galileo E5a + * 20 | GPS L5 + Galileo E5b + * 21 | GPS L1 C/A + Galileo E1B + GPS L2C + * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + */ + // ############ 2 COMPUTE THE PVT ################################ - // ToDo: relax this condition because the receiver should work even with NO GALILEO SATELLITES - //if (gnss_pseudoranges_map.size() > 0 and d_ls_pvt->galileo_ephemeris_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() > 0) - if (gnss_pseudoranges_map.size() > 0) + if (gnss_observables_map.size() > 0) { - //std::cout << "Both GPS and Galileo ephemeris map have been filled " << std::endl; // compute on the fly PVT solution if ((d_sample_counter % d_output_rate_ms) == 0) { bool pvt_result; - pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging); + pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); if (pvt_result == true) { @@ -394,19 +460,117 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if (!b_rinex_header_writen) // & we have utc data in nav message! { - std::map::iterator galileo_ephemeris_iter; galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - std::map::iterator gps_ephemeris_iter; gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + + if(type_of_rx == 1) // GPS L1 C/A only { - if (arrived_galileo_almanac) + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_writen = true; // do not write header anymore + } + } + if(type_of_rx == 2) // GPS L2C only + { + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); + b_rinex_header_writen = true; // do not write header anymore + } + } + if(type_of_rx == 4) // Galileo E1B only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_writen = true; // do not write header anymore + } + } + if(type_of_rx == 5) // Galileo E5a only + { + std::string signal("5X"); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_writen = true; // do not write header anymore + } + } + if(type_of_rx == 6) // Galileo E5b only + { + std::string signal("7X"); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_writen = true; // do not write header anymore + } + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + { + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_writen = true; // do not write header anymore + } + } + + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("1B"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); b_rinex_header_writen = true; // do not write header anymore } } + if(type_of_rx == 10) // GPS L1 C/A + Galileo E5a + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("5X"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_writen = true; // do not write header anymore + } + } + if(type_of_rx == 11) // GPS L1 C/A + Galileo E5b + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("7X"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_writen = true; // do not write header anymore + } + } + if(type_of_rx == 14) // Galileo E1B + Galileo E5a + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) + { + std::string gal_signal("1B 5X"); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_writen = true; // do not write header anymore + } + } + if(type_of_rx == 15) // Galileo E1B + Galileo E5b + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) + { + std::string gal_signal("1B 7X"); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_writen = true; // do not write header anymore + } + } } if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s) { @@ -414,58 +578,123 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v // Notice that d_sample_counter period is 4ms (for Galileo correlators) if ((d_sample_counter - d_last_sample_nav_output) >= 6000) { - rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + if(type_of_rx == 1) // GPS L1 C/A only + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map); + } + if(type_of_rx == 2) // GPS L2C only + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); + } + if( (type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) ) // Galileo + { + rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); + } + if((type_of_rx == 9) || (type_of_rx == 10) || (type_of_rx == 11)) // GPS L1 C/A + Galileo + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + } + + + + + d_last_sample_nav_output = d_sample_counter; } - std::map::iterator galileo_ephemeris_iter; galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - std::map::iterator gps_ephemeris_iter; gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + + if(type_of_rx == 1) // GPS L1 C/A only { - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); + b_rinex_header_updated = true; + } } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + if(type_of_rx == 2) // GPS L2C only { - rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_updated = true; + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); + b_rinex_header_updated = true; + } } + if(type_of_rx == 4) // Galileo E1B only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; + } + } + } if(b_rtcm_writing_started) { if(((d_sample_counter % (d_rtcm_MT1019_rate_ms / 4)) == 0) && (d_rtcm_MT1019_rate_ms != 0)) { - for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + for(gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) { d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); } } if(((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4)) == 0) && (d_rtcm_MT1045_rate_ms != 0)) { - for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) + for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); } } if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) || ((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0)) { - std::map::iterator gnss_pseudoranges_iter; - std::map::iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - std::map::iterator gal_ephemeris_iter; - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); unsigned int i = 0; - for (gnss_pseudoranges_iter = gnss_pseudoranges_map.begin(); gnss_pseudoranges_iter != gnss_pseudoranges_map.end(); gnss_pseudoranges_iter++) + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) { - std::string system(&gnss_pseudoranges_iter->second.System, 1); + std::string system(&gnss_observables_iter->second.System, 1); if(gps_channel == 0) { if(system.compare("G") == 0) { // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_pseudoranges_iter->second.PRN); + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { gps_channel = i; @@ -476,8 +705,8 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if(system.compare("E") == 0) { - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_pseudoranges_iter->second.PRN); - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { gal_channel = i; } @@ -488,16 +717,16 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) && (d_rtcm_MT1097_rate_ms != 0) ) { - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } if(((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0) && (d_rtcm_MT1077_rate_ms != 0) ) { if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_pseudoranges_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } } } @@ -513,27 +742,23 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v } if(d_rtcm_MT1045_rate_ms != 0) { - for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) + for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); } } - std::map::iterator gnss_pseudoranges_iter; - std::map::iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - std::map::iterator gal_ephemeris_iter; - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; - for (gnss_pseudoranges_iter = gnss_pseudoranges_map.begin(); gnss_pseudoranges_iter != gnss_pseudoranges_map.end(); gnss_pseudoranges_iter++) + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) { - std::string system(&gnss_pseudoranges_iter->second.System, 1); + std::string system(&gnss_observables_iter->second.System, 1); if(gps_channel == 0) { if(system.compare("G") == 0) { // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_pseudoranges_iter->second.PRN); + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) { gps_channel = i; @@ -544,8 +769,8 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v { if(system.compare("E") == 0) { - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_pseudoranges_iter->second.PRN); - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) { gal_channel = i; } @@ -556,12 +781,12 @@ int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_v if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_pseudoranges_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0) ) + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0) ) { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map, 0, 0, 0, 0, 0); + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); } b_rtcm_writing_started = true; } diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h index 21ccfeff4..1dba21fc9 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h @@ -65,7 +65,8 @@ hybrid_pvt_cc_sptr hybrid_make_pvt_cc(unsigned int n_channels, unsigned short rtcm_tcp_port, unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname); + std::string rtcm_dump_devname, + const unsigned int type_of_receiver); /*! * \brief This class implements a block that computes the PVT solution with Galileo E1 signals @@ -88,7 +89,8 @@ private: unsigned short rtcm_tcp_port, unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname); + std::string rtcm_dump_devname, + const unsigned int type_of_receiver); hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump_filename, int averaging_depth, @@ -103,7 +105,8 @@ private: unsigned short rtcm_tcp_port, unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname); + std::string rtcm_dump_devname, + const unsigned int type_of_receiver); void msg_handler_telemetry(pmt::pmt_t msg); @@ -137,8 +140,10 @@ private: double d_rx_time; double d_TOW_at_curr_symbol_constellation; std::shared_ptr d_ls_pvt; - std::map gnss_pseudoranges_map; - bool pseudoranges_pairCompare_min(const std::pair& a, const std::pair& b); + std::map gnss_observables_map; + bool observables_pairCompare_min(const std::pair& a, const std::pair& b); + + unsigned int type_of_rx; bool first_fix; key_t sysv_msg_key; diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h index c085d29e4..99f54e23e 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.h @@ -74,6 +74,9 @@ public: Gps_Utc_Model gps_utc_model; Gps_Iono gps_iono; + Gps_CNAV_Iono gps_cnav_iono; + Gps_CNAV_Utc_Model gps_cnav_utc_model; + double d_galileo_current_time; int count_valid_position; diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index 716853193..e1d539d74 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -42,9 +42,9 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration, std::string role, unsigned int in_streams, unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) + role_(role), + in_streams_(in_streams), + out_streams_(out_streams) { std::string default_dump_filename = "./observables.dat"; DLOG(INFO) << "role " << role; @@ -52,13 +52,13 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration, dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); unsigned int default_depth = 0; if (GPS_L1_CA_HISTORY_DEEP == GALILEO_E1_HISTORY_DEEP) - { - default_depth = GPS_L1_CA_HISTORY_DEEP; - } + { + default_depth = GPS_L1_CA_HISTORY_DEEP; + } else - { - default_depth = 100; - } + { + default_depth = 100; + } unsigned int history_deep = configuration->property(role + ".averaging_depth", default_depth); observables_ = hybrid_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep); DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index ab22fd881..9a88419ba 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -92,30 +92,17 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, } - hybrid_observables_cc::~hybrid_observables_cc() { d_dump_file.close(); } - -//bool Hybrid_pairCompare_gnss_synchro_Prn_delay_ms(const std::pair& a, const std::pair& b) -//{ -// return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms); -//} - - bool Hybrid_pairCompare_gnss_synchro_d_TOW_hybrid_at_current_symbol(const std::pair& a, const std::pair& b) { return (a.second.d_TOW_hybrid_at_current_symbol) < (b.second.d_TOW_hybrid_at_current_symbol); } -bool Hybrid_pairCompare_gnss_synchro_d_TOW_at_current_symbol(const std::pair& a, const std::pair& b) -{ - return (a.second.d_TOW_at_current_symbol) < (b.second.d_TOW_at_current_symbol); -} - int hybrid_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items, @@ -127,7 +114,6 @@ int hybrid_observables_cc::general_work (int noutput_items, Gnss_Synchro current_gnss_synchro[d_nchannels]; std::map current_gnss_synchro_map; - //std::map current_gnss_synchro_map_gps_only; std::map::iterator gnss_synchro_iter; if (d_nchannels != ninput_items.size()) @@ -151,37 +137,33 @@ int hybrid_observables_cc::general_work (int noutput_items, { //record the word structure in a map for pseudorange computation current_gnss_synchro_map.insert(std::pair(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - // if (current_gnss_synchro[i].System == 'G') - // { - // current_gnss_synchro_map_gps_only.insert(std::pair(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - // } //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE ####### d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz); d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads); // save TOW history d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol); if (d_carrier_doppler_queue_hz[i].size() > history_deep) - { - d_carrier_doppler_queue_hz[i].pop_front(); - } + { + d_carrier_doppler_queue_hz[i].pop_front(); + } if (d_acc_carrier_phase_queue_rads[i].size() > history_deep) - { - d_acc_carrier_phase_queue_rads[i].pop_front(); - } + { + d_acc_carrier_phase_queue_rads[i].pop_front(); + } if (d_symbol_TOW_queue_s[i].size() > history_deep) - { - d_symbol_TOW_queue_s[i].pop_front(); - } + { + d_symbol_TOW_queue_s[i].pop_front(); + } } - else + else { - // Clear the observables history for this channel - if (d_symbol_TOW_queue_s[i].size() > 0) - { - d_symbol_TOW_queue_s[i].clear(); - d_carrier_doppler_queue_hz[i].clear(); - d_acc_carrier_phase_queue_rads[i].clear(); - } + // Clear the observables history for this channel + if (d_symbol_TOW_queue_s[i].size() > 0) + { + d_symbol_TOW_queue_s[i].clear(); + d_carrier_doppler_queue_hz[i].clear(); + d_acc_carrier_phase_queue_rads[i].clear(); + } } } @@ -189,8 +171,19 @@ int hybrid_observables_cc::general_work (int noutput_items, * 2. Compute RAW pseudoranges using COMMON RECEPTION TIME algorithm. Use only the valid channels (channels that are tracking a satellite) */ DLOG(INFO) << "gnss_synchro set size=" << current_gnss_synchro_map.size(); + double traveltime_ms; + double pseudorange_m; + double delta_rx_time_ms; + double delta_TOW_ms; + arma::vec symbol_TOW_vec_s; + arma::vec dopper_vec_hz; + arma::vec dopper_vec_interp_hz; + arma::vec acc_phase_vec_rads; + arma::vec acc_phase_vec_interp_rads; + arma::vec desired_symbol_TOW(1); + double start_offset_ms = 0.0; - if(current_gnss_synchro_map.size() > 0)// and current_gnss_synchro_map_gps_only.size()>0) + if(current_gnss_synchro_map.size() > 0) { /* * 2.1 Use CURRENT set of measurements and find the nearest satellite @@ -206,30 +199,19 @@ int hybrid_observables_cc::general_work (int noutput_items, //int reference_channel= gnss_synchro_iter->second.Channel_ID; // Now compute RX time differences due to the PRN alignment in the correlators - double traveltime_ms; - double pseudorange_m; - double delta_rx_time_ms; - double delta_TOW_ms; - arma::vec symbol_TOW_vec_s; - arma::vec dopper_vec_hz; - arma::vec dopper_vec_interp_hz; - arma::vec acc_phase_vec_rads; - arma::vec acc_phase_vec_interp_rads; - arma::vec desired_symbol_TOW(1); - double start_offset_ms = 0.0; for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++) { // check and correct synchronization in cross-system pseudoranges! delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms; delta_TOW_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_hybrid_at_current_symbol) * 1000.0; if(gnss_synchro_iter->second.System == 'E') - { - start_offset_ms = GALILEO_STARTOFFSET_ms; - } + { + start_offset_ms = GALILEO_STARTOFFSET_ms; + } if(gnss_synchro_iter->second.System == 'G') - { - start_offset_ms = GPS_STARTOFFSET_ms; - } + { + start_offset_ms = GPS_STARTOFFSET_ms; + } //compute the pseudorange traveltime_ms = delta_TOW_ms + delta_rx_time_ms + start_offset_ms; pseudorange_m = traveltime_ms * GALILEO_C_m_ms; // [m] @@ -244,7 +226,7 @@ int hybrid_observables_cc::general_work (int noutput_items, //current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_hybrid_at_current_symbol = round(d_TOW_reference * 1000) / 1000 + GPS_STARTOFFSET_ms / 1000.0; + current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_hybrid_at_current_symbol = round(d_TOW_reference * 1000) / 1000 + start_offset_ms / 1000.0; if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep) { // compute interpolated observation values for Doppler and Accumulate carrier phase