diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 152fc6f1d..6ef870b65 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -180,7 +180,37 @@ void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) d_ls_pvt->galileo_almanac = *galileo_almanac; DLOG(INFO) << "New Galileo Almanac has arrived "; } - + //**************** GLONASS GNAV Telemetry ************************** + else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GLONASS GNAV EPHEMERIS ### + std::shared_ptr glonass_gnav_eph; + glonass_gnav_eph = boost::any_cast>(pmt::any_ref(msg)); + // TODO Add GLONASS with gps week number and tow, + // insert new ephemeris record + DLOG(INFO) << "GLONASS GNAV New Ephemeris record inserted in global map with TOW =" << glonass_gnav_eph->TOW_5 + << ", GLONASS GNAV Week Number =" << glonass_gnav_eph->WN_5 + << " and Ephemeris IOD = " << glonass_gnav_eph->IOD_ephemeris; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->glonass_gnav_ephemeris_map[glonass_gnav_eph->i_satellite_PRN] = *glonass_gnav_eph; + } + else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GLONASS GNAV UTC MODEL ### + std::shared_ptr glonass_gnav_utc_model; + glonass_gnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->glonass_gnav_utc_model = *glonass_gnav_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + else if(pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GLONASS GNAV Almanac ### + std::shared_ptr glonass_gnav_almanac; + glonass_gnav_almanac = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->glonass_gnav_almanac = *glonass_gnav_almanac; + DLOG(INFO) << "New GLONASS GNAV Almanac has arrived "; + } else { LOG(WARNING) << "msg_handler_telemetry unknown object type!"; @@ -403,6 +433,28 @@ rtklib_pvt_cc::~rtklib_pvt_cc() LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty"; } + //save GLONASS GNAV ephemeris to XML file + file_name = "eph_GLONASS_GNAV.xml"; + + if (d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->glonass_gnav_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved GLONASS GNAV Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save GLONASS GNAV Ephemeris, map is empty"; + } if (d_dump_file.is_open() == true) { try @@ -455,6 +507,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite bool flag_write_RINEX_nav_output = false; unsigned int gps_channel = 0; unsigned int gal_channel = 0; + unsigned int glo_channel = 0; gnss_observables_map.clear(); const Gnss_Synchro **in = reinterpret_cast(&input_items[0]); // Get the input buffer pointer @@ -467,10 +520,13 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite std::map::const_iterator tmp_eph_iter_gps = d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_gal = d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN); std::map::const_iterator tmp_eph_iter_cnav = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN); + std::map::const_iterator tmp_eph_iter_glo_gnav = d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN); if(((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 0)) || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1B") == 0)) - || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0))) + || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0)) + || ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) + || ((tmp_eph_iter_glo_gnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2C") == 0))) { // store valid observables in a map. gnss_observables_map.insert(std::pair(i, in[i][epoch])); @@ -497,6 +553,14 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time } } + if(d_ls_pvt->glonass_gnav_ephemeris_map.size() > 0) + { + if(tmp_eph_iter_glo_gnav != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->glonass_gnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + } } @@ -613,6 +677,15 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite * 20 | GPS L5 + Galileo E5b * 21 | GPS L1 C/A + Galileo E1B + GPS L2C * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + * 23 | GLONASS L1 C/A + * 24 | GLONASS L2 C/A + * 25 | GLONASS L1 C/A + GLONASS L2 C/A + * 26 | GLONASS L1 C/A + GPS L1 C/A + * 27 | GLONASS L1 C/A + GPS L2C + * 28 | GLONASS L1 C/A + GPS L5 + * 29 | GLONASS L1 C/A + Galileo E1B + * 30 | GLONASS L1 C/A + Galileo E5a + * 31 | GLONASS L1 C/A + Galileo E5b */ // ####################### RINEX FILES ################# @@ -620,6 +693,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite std::map::const_iterator galileo_ephemeris_iter; std::map::const_iterator gps_ephemeris_iter; std::map::const_iterator gps_cnav_ephemeris_iter; + std::map::iterator glonass_gnav_ephemeris_iter; std::map::const_iterator gnss_observables_iter; if (!b_rinex_header_written) // & we have utc data in nav message! @@ -627,6 +701,7 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); if(type_of_rx == 1) // GPS L1 C/A only { @@ -736,6 +811,87 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite b_rinex_header_written = true; // do not write header anymore } } + if(type_of_rx == 23) // GLONASS L1 C/A only + { + std::string signal("1C"); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 24) // GLONASS L2 C/A only + { + std::string signal("2C"); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 25) // GLONASS L1 C/A + GLONASS L2 C/A + { + std::string signal("1C 2C"); + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) + { + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + + if(type_of_rx == 26) // GLONASS L1 C/A + GPS L1 C/A + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) ) + { + std::string gps_signal("1C 1C"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 27) // GLONASS L1 C/A + GPS L2C + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.cend()) ) + { + std::string gps_signal("1C 1C"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 29) // GLONASS L1 C/A + Galileo E1B + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) + { + std::string gal_signal("1C 1B"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 30) // GLONASS L1 C/A + Galileo E5a + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) + { + std::string gal_signal("1C 5X"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 31) // GLONASS L1 C/A + Galileo E5b + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.cend()) && (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.cend()) ) + { + std::string gal_signal("1C 7X"); + rp->rinex_obs_header(rp->obsFile, glonass_gnav_ephemris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } } if(b_rinex_header_written) // The header is already written, we can now log the navigation message data { @@ -765,10 +921,27 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite { rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); } + if((type_of_rx == 23) || (type_of_rx == 24) || (type_of_rx == 25)) // GLONASS L1 C/A, GLONASS L2 C/A + { + rp->log_rinex_nav(rp->navGloFile, d_ls_pvt->glonass_gnav_ephemeris_map); + } + if((type_of_rx == 26)) // GLONASS L1 C/A + GPS L1 C/A + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->gps_ephemeris_map); + } + if((type_of_rx == 27)) // GLONASS L1 C/A + GPS L2C + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->gps_cnav_ephemeris_map); + } + if((type_of_rx == 29) || (type_of_rx == 30) || (type_of_rx == 31)) // GLONASS L1 C/A + Galileo + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->glonass_gnav_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + } } galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.cbegin(); gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.cbegin(); gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.cbegin(); + glonass_gnav_ephemeris_iter = d_ls_pvt->glonass_gnav_ephemeris_map.cbegin(); // Log observables into the RINEX file if(flag_write_RINEX_obs_output) @@ -890,6 +1063,120 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite b_rinex_header_updated = true; } } + if(type_of_rx == 23) // GLONASS L1 C/A only + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C"); + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 24) // GLONASS L2 C/A only + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "2C"); + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_nav_header(rp->navGloFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 25) // GLONASS L1 C/A + GLONASS L2 C/A + { + if (glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1C 2C"); + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 26) // GLONASS L1 C/A + GPS L1 C/A + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gps_signal("1C 1C"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_ephemeris_iter->second, d_rx_time, gps_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_updated = true; // do not write header anymore + } + } + if(type_of_rx == 27) // GLONASS L1 C/A + GPS L1 C/A + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) + { + std::string gps_signal("1C 2S"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gps_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); + b_rinex_header_updated = true; // do not write header anymore + } + } + if(type_of_rx == 29) // GLONASS L1 C/A + Galileo E1B + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) + { + std::string gal_signal("1C 1B"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; // do not write header anymore + } + } + if(type_of_rx == 30) // GLONASS L1 C/A + Galileo E5a + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) + { + std::string gal_signal("1C 5X"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; // do not write header anymore + } + } + if(type_of_rx == 31) // GLONASS L1 C/A + Galileo E5b + { + if ((glonass_gnav_ephemeris_iter != d_ls_pvt->glonass_gnav_ephemeris_map.end()) && (gal_ephemeris_iter != d_ls_pvt->gal_ephemeris_map.end()) ) + { + std::string gal_signal("1C 7X"); + rp->log_rinex_obs(rp->obsFile, glonass_gnav_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + + } + if (!b_rinex_header_updated && (d_ls_pvt->glonass_gnav_utc_model.d_tau_c != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->glonass_gnav_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->glonass_gnav_utc_model, d_ls_pvt->glonass_gnav_almanac, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; // do not write header anymore + } + } } } diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index 97ab793a7..b7d55d67f 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -727,6 +727,3 @@ std::string Nmea_Printer::get_GPGGA() return sentence_str.str(); //$GPGGA,104427.591,5920.7009,N,01803.2938,E,1,05,3.3,78.2,M,23.2,M,0.0,0000*4A } - - - diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 344888f67..b46fe7576 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -56,6 +56,7 @@ #include "rtklib_conversions.h" #include "GPS_L1_CA.h" #include "Galileo_E1.h" +#include "GLONASS_L1_CA.h" using google::LogMessage; @@ -114,6 +115,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ std::map::const_iterator galileo_ephemeris_iter; std::map::const_iterator gps_ephemeris_iter; std::map::const_iterator gps_cnav_ephemeris_iter; + std::map::const_iterator glonass_gnav_ephemeris_iter; this->set_averaging_flag(flag_averaging); @@ -131,7 +133,9 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ { switch(gnss_observables_iter->second.System) { - case 'E': + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) { std::string sig_(gnss_observables_iter->second.Signal); // Galileo E1 @@ -270,6 +274,76 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ } break; } + case 'R': //TODO This should be using rtk lib nomenclature + { + std::string sig_(gnss_observables_iter->second.Signal); + // GLONASS GNAV L1 + if(sig_.compare("1C") == 0) + { + // 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) + { + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.WN_5, + 0);//TODO are THESE VALUES OK + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + + } + // GLONASS GNAV L2 + if(sig_.compare("2C") == 0) + { + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.end()) + { + bool found_L1_obs=false; + for (int i = 0; i < valid_obs; i++) + { + // TODO what is this? + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN+NSATGPS+NSATGLO))) + { + obs_data[i] = insert_obs_to_rtklib(obs_data[i], + gnss_observables_iter->second, + glonass_gnav_ephemeris_iter->second.WN_5, + 2);//Band 3 (L5/E5) + found_L1_obs=true; + break; + } + } + if (!found_L1_obs) + { + //insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN_5, + 2); //Band 3 (L5/E5) + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + + + } + break; + } default : DLOG(INFO) << "Hybrid observables: Unknown GNSS"; break; @@ -365,6 +439,7 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ } } } + } } return this->is_valid_position(); } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 6f441f4a6..274289138 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -62,6 +62,7 @@ #include "galileo_navigation_message.h" #include "gps_navigation_message.h" #include "gps_cnav_navigation_message.h" +#include "glonass_gnav_navigation_message.h" #include "gnss_synchro.h" #include "pvt_solution.h" @@ -86,6 +87,7 @@ public: std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris std::map gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris + std::map glonass_gnav_ephemeris_map; //!< Map storing new GLONASS GNAV Ephmeris Galileo_Utc_Model galileo_utc_model; Galileo_Iono galileo_iono; @@ -97,7 +99,18 @@ public: Gps_CNAV_Iono gps_cnav_iono; Gps_CNAV_Utc_Model gps_cnav_utc_model; + Glonass_Gnav_Utc_Model glonass_gnav_utc_model; //!< GLONASS GNAV UTC Model + Glonass_Gnav_Almanac glonass_gnav_almanac; //!< GLONASS GNAV Almanac Model + int count_valid_position; + + bool d_flag_dump_enabled; + + sol_t pvt_sol; + rtk_t rtk_; + + std::string d_dump_filename; + std::ofstream d_dump_file; }; #endif diff --git a/src/core/system_parameters/glonass_gnav_navigation_message.cc b/src/core/system_parameters/glonass_gnav_navigation_message.cc index d7c059d7e..96608b736 100644 --- a/src/core/system_parameters/glonass_gnav_navigation_message.cc +++ b/src/core/system_parameters/glonass_gnav_navigation_message.cc @@ -449,7 +449,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_7 = true; } @@ -482,7 +482,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_9 = true; } @@ -512,7 +512,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_11 = true; } @@ -543,7 +543,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_13 = true; } @@ -582,7 +582,7 @@ int Glonass_Gnav_Navigation_Message::string_decoder(char * frame_string) gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A = static_cast(read_navigation_unsigned(string_bits, DELTA_T_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast(read_navigation_unsigned(string_bits, DELTA_T_DOT_N_A)); gnav_almanac[i_satellite_slot_number - 1].d_H_n_A = static_cast(read_navigation_unsigned(string_bits, H_N_A)); - gnav_ephemeris.d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); + gnav_almanac[i_satellite_slot_number - 1].d_l_n = static_cast(read_navigation_unsigned(string_bits, ALM_L_N)); flag_almanac_str_15 = true; } @@ -631,6 +631,7 @@ bool Glonass_Gnav_Navigation_Message::have_new_ephemeris() //Check if we have a //if all ephemeris pages have the same IOD, then they belong to the same block if ((gnav_ephemeris.d_t_b == 0) ) { + std::cout << "Ephemeris (1, 2, 3, 4) have been received and belong to the same batch" << std::endl; flag_ephemeris_str_1 = false;// clear the flag flag_ephemeris_str_2 = false;// clear the flag flag_ephemeris_str_3 = false;// clear the flag diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index b192224fb..6e0a6e4d8 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -157,7 +157,7 @@ void Gnss_Satellite::set_PRN(unsigned int PRN_) PRN = PRN_; } } - else if (system.compare("Glonass") == 0) + else if (system.compare("GLONASS") == 0) { if (PRN_ < 1 or PRN_ > 24) { @@ -356,7 +356,7 @@ std::string Gnss_Satellite::what_block(const std::string& system_, unsigned int } - if (system_.compare("Glonass") == 0) + if (system_.compare("GLONASS") == 0) { switch ( PRN_ ) { @@ -557,6 +557,3 @@ void Gnss_Satellite::set_block(const std::string& system_, unsigned int PRN_) { block = what_block(system_, PRN_); } - - -