diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index d65f7aaf3..0a4a04124 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -3143,7 +3143,7 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Galileo_Ephemeris& } -void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation) +void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps_eph, const Galileo_Ephemeris& galileo_eph, const double d_TOW_first_observation, const std::string galileo_bands) { if(galileo_eph.e_1){} // avoid warning, not needed std::string line; @@ -3312,27 +3312,86 @@ void Rinex_Printer::rinex_obs_header(std::fstream& out, const Gps_Ephemeris& gps out << line << std::endl; line.clear(); + unsigned int number_of_observations_gal = 0; + std::string signal_("1B"); + std::size_t found_1B = galileo_bands.find(signal_); + if(found_1B != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + signal_ = "5X"; + std::size_t found_5X = galileo_bands.find(signal_); + if(found_5X != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + + line.clear(); + signal_ = "7X"; + std::size_t found_7X = galileo_bands.find(signal_); + if(found_7X != std::string::npos) + { + number_of_observations_gal = number_of_observations_gal + 4; + } + + line += satelliteSystem["Galileo"]; line += std::string(2, ' '); - line += Rinex_Printer::rightJustify("4", 3); - line += std::string(1, ' '); - line += observationType["PSEUDORANGE"]; - line += observationCode["GALILEO_E1_B"]; - line += std::string(1, ' '); - line += observationType["CARRIER_PHASE"]; - line += observationCode["GALILEO_E1_B"]; - line += std::string(1, ' '); - line += observationType["DOPPLER"]; - line += observationCode["GALILEO_E1_B"]; - line += std::string(1, ' '); - line += observationType["SIGNAL_STRENGTH"]; - line += observationCode["GALILEO_E1_B"]; + line += Rinex_Printer::rightJustify(std::to_string(number_of_observations_gal), 3); + + if(found_1B != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E1_B"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E1_B"]; + } + + if(found_5X != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E5a_IQ"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E5a_IQ"]; + } + + if(found_7X != std::string::npos) + { + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GALILEO_E5b_IQ"]; + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GALILEO_E5b_IQ"]; + } + line += std::string(60-line.size(), ' '); line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); Rinex_Printer::lengthCheck(line); out << line << std::endl; - // -------- Signal Strength units line.clear(); line += Rinex_Printer::leftJustify("DBHZ", 20); @@ -4218,7 +4277,6 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep } std::multimap total_map; - std::multimap::iterator total_iter; std::set available_prns; std::set::iterator it; for(pseudoranges_iter = pseudorangesE1B.begin(); @@ -4228,7 +4286,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep unsigned int prn_ = pseudoranges_iter->second.PRN; total_map.insert(std::pair(prn_, pseudoranges_iter->second)); it = available_prns.find(prn_); - if(it != available_prns.end()) + if(it == available_prns.end()) { available_prns.insert(prn_); } @@ -4241,7 +4299,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep unsigned int prn_ = pseudoranges_iter->second.PRN; total_map.insert(std::pair(prn_, pseudoranges_iter->second)); it = available_prns.find(prn_); - if(it != available_prns.end()) + if(it == available_prns.end()) { available_prns.insert(prn_); } @@ -4254,7 +4312,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep unsigned int prn_ = pseudoranges_iter->second.PRN; total_map.insert(std::pair(prn_, pseudoranges_iter->second)); it = available_prns.find(prn_); - if(it != available_prns.end()) + if(it == available_prns.end()) { available_prns.insert(prn_); } @@ -4268,18 +4326,17 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep Rinex_Printer::lengthCheck(line); out << line << std::endl; + std::string lineObs; std::pair ::iterator, std::multimap::iterator> ret; - for(total_iter = total_map.begin(); - total_iter != total_map.end(); - total_iter++) + for(it = available_prns.begin(); + it != available_prns.end(); + it++) { - std::string lineObs; lineObs.clear(); lineObs += satelliteSystem["Galileo"]; - if (static_cast(total_iter->first) < 10) lineObs += std::string(1, '0'); - lineObs += boost::lexical_cast(static_cast(total_iter->first)); - //lineObs += std::string(2, ' '); - ret = total_map.equal_range(total_iter->first); + if (static_cast(*it) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(*it)); + ret = total_map.equal_range(*it); for (std::multimap::iterator iter = ret.first; iter != ret.second; ++iter) { lineObs += Rinex_Printer::rightJustify(asString(iter->second.Pseudorange_m, 3), 14); @@ -4375,14 +4432,83 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep line += std::string(1, '0'); //Number of satellites observed in current epoch - int numSatellitesObserved = 0; - std::map::const_iterator pseudoranges_iter; + + //Get maps with observations + std::map pseudorangesG1C; + std::map pseudorangesE1B; + std::map pseudorangesE5A; + std::map pseudorangesE5B; + std::map::const_iterator pseudoranges_iter; + for(pseudoranges_iter = pseudoranges.begin(); pseudoranges_iter != pseudoranges.end(); pseudoranges_iter++) { - numSatellitesObserved++; + std::string system_(&pseudoranges_iter->second.System, 1); + std::string sig_(pseudoranges_iter->second.Signal); + if((system_.compare("E") == 0) && (sig_.compare("1B") == 0)) + { + pseudorangesE1B.insert(std::pair(pseudoranges_iter->first, pseudoranges_iter->second)); + } + if((system_.compare("E") == 0) && (sig_.compare("5X") == 0)) + { + pseudorangesE5A.insert(std::pair(pseudoranges_iter->first, pseudoranges_iter->second)); + } + if((system_.compare("E") == 0) && (sig_.compare("7X") == 0)) + { + pseudorangesE5B.insert(std::pair(pseudoranges_iter->first, pseudoranges_iter->second)); + } + if((system_.compare("G") == 0) && (sig_.compare("1C") == 0)) + { + pseudorangesG1C.insert(std::pair(pseudoranges_iter->first, pseudoranges_iter->second)); + } } + + std::multimap total_gal_map; + std::set available_gal_prns; + std::set::iterator it; + for(pseudoranges_iter = pseudorangesE1B.begin(); + pseudoranges_iter != pseudorangesE1B.end(); + pseudoranges_iter++) + { + unsigned int prn_ = pseudoranges_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, pseudoranges_iter->second)); + it = available_gal_prns.find(prn_); + if(it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + for(pseudoranges_iter = pseudorangesE5A.begin(); + pseudoranges_iter != pseudorangesE5A.end(); + pseudoranges_iter++) + { + unsigned int prn_ = pseudoranges_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, pseudoranges_iter->second)); + it = available_gal_prns.find(prn_); + if(it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + for(pseudoranges_iter = pseudorangesE5B.begin(); + pseudoranges_iter != pseudorangesE5B.end(); + pseudoranges_iter++) + { + unsigned int prn_ = pseudoranges_iter->second.PRN; + total_gal_map.insert(std::pair(prn_, pseudoranges_iter->second)); + it = available_gal_prns.find(prn_); + if(it == available_gal_prns.end()) + { + available_gal_prns.insert(prn_); + } + } + + int numGalSatellitesObserved = available_gal_prns.size(); + int numGpsSatellitesObserved = pseudorangesG1C.size(); + int numSatellitesObserved = numGalSatellitesObserved + numGpsSatellitesObserved; line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); // Receiver clock offset (optional) @@ -4393,18 +4519,18 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep out << line << std::endl; std::string s; - for(pseudoranges_iter = pseudoranges.begin(); - pseudoranges_iter != pseudoranges.end(); + std::string lineObs; + for(pseudoranges_iter = pseudorangesG1C.begin(); + pseudoranges_iter != pseudorangesG1C.end(); pseudoranges_iter++) { - std::string lineObs; lineObs.clear(); s.assign(1, pseudoranges_iter->second.System); if(s.compare("G") == 0) lineObs += satelliteSystem["GPS"]; - if(s.compare("E") == 0) lineObs += satelliteSystem["Galileo"]; - if (static_cast(pseudoranges_iter->first) < 10) lineObs += std::string(1, '0'); - lineObs += boost::lexical_cast(static_cast(pseudoranges_iter->first)); + if(s.compare("E") == 0) lineObs += satelliteSystem["Galileo"]; // should not happen + if (static_cast(pseudoranges_iter->second.PRN) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(pseudoranges_iter->second.PRN)); lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Pseudorange_m, 3), 14); //Loss of lock indicator (LLI) @@ -4452,6 +4578,67 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); out << lineObs << std::endl; } + + std::pair ::iterator, std::multimap::iterator> ret; + for(it = available_gal_prns.begin(); + it != available_gal_prns.end(); + it++) + { + lineObs.clear(); + lineObs += satelliteSystem["Galileo"]; + if (static_cast(*it) < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast(static_cast(*it)); + ret = total_gal_map.equal_range(*it); + for (std::multimap::iterator iter = ret.first; iter != ret.second; ++iter) + { + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Pseudorange_m, 3), 14); + + //Loss of lock indicator (LLI) + int lli = 0; // Include in the observation!! + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + + // Signal Strength Indicator (SSI) + int ssi = Rinex_Printer::signalStrength(iter->second.CN0_dB_hz); + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo CARRIER PHASE + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_Doppler_hz, 3), 14); + if (lli == 0) + { + lineObs += std::string(1, ' '); + } + else + { + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(lli), 1); + } + lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString(ssi), 1); + + // Galileo SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(iter->second.CN0_dB_hz, 3), 14); + } + + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } } diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index 4471d441c..f8913be43 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -132,9 +132,9 @@ public: void rinex_obs_header(std::fstream & out, const Galileo_Ephemeris & eph, const double d_TOW_first_observation, const std::string bands = "1B"); /*! - * \brief Generates the Mixed (GPS/Galileo) Observation data header + * \brief Generates the Mixed (GPS/Galileo) Observation data header. Example: galileo_bands("1B"), galileo_bands("1B 5X"), galileo_bands("5X"), ... Default: "1B". */ - void rinex_obs_header(std::fstream & out, const Gps_Ephemeris & gps_eph, const Galileo_Ephemeris & galileo_eph, const double d_TOW_first_observation); + void rinex_obs_header(std::fstream & out, const Gps_Ephemeris & gps_eph, const Galileo_Ephemeris & galileo_eph, const double d_TOW_first_observation, const std::string galileo_bands = "1B"); /*! * \brief Generates the SBAS raw data header diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc index 67830edf4..8e7e71c67 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/rinex_printer_test.cc @@ -28,7 +28,7 @@ * ------------------------------------------------------------------------- */ - +#include #include #include "rinex_printer.h" @@ -88,6 +88,51 @@ TEST(Rinex_Printer_Test, GalileoObsHeader) + +TEST(Rinex_Printer_Test, MixedObsHeader) +{ + std::string line_aux; + std::string line_aux2; + std::string line_str; + bool no_more_finds = false; + const Galileo_Ephemeris eph_gal = Galileo_Ephemeris(); + const Gps_Ephemeris eph_gps = Gps_Ephemeris(); + + std::shared_ptr rp1; + rp1 = std::make_shared(); + rp1->rinex_obs_header(rp1->obsFile, eph_gps, eph_gal, 0.0, "1B 5X"); + rp1->obsFile.seekp(0); + int systems_found = 0; + + while(!rp1->obsFile.eof()) + { + std::getline(rp1->obsFile, line_str); + if(!no_more_finds) + { + if (line_str.find("SYS / # / OBS TYPES", 59) != std::string::npos) + { + systems_found++; + if(systems_found == 1) + { + line_aux = std::string(line_str); + } + if(systems_found == 2) + { + line_aux2 = std::string(line_str); + no_more_finds = true; + } + } + } + } + + std::string expected_str("G 4 C1C L1C D1C S1C SYS / # / OBS TYPES "); + std::string expected_str2("E 8 C1B L1B D1B S1B C5X L5X D5X S5X SYS / # / OBS TYPES "); + EXPECT_EQ(0, expected_str.compare(line_aux)); + EXPECT_EQ(0, expected_str2.compare(line_aux2)); + if(remove(rp1->obsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; +} + + TEST(Rinex_Printer_Test, GalileoObsLog) { std::string line_aux; @@ -231,3 +276,112 @@ TEST(Rinex_Printer_Test, GalileoObsLogDualBand) } + +TEST(Rinex_Printer_Test, MixedObsLog) +{ + std::string line_aux; + std::string line_str; + bool no_more_finds = false; + const Galileo_Ephemeris eph_gal = Galileo_Ephemeris(); + const Gps_Ephemeris eph_gps = Gps_Ephemeris(); + + std::shared_ptr rp; + rp = std::make_shared(); + rp->rinex_obs_header(rp->obsFile, eph_gps, eph_gal, 0.0, "1B 5X"); + + std::map gnss_pseudoranges_map; + + Gnss_Synchro gs1 = Gnss_Synchro(); + Gnss_Synchro gs2 = Gnss_Synchro(); + Gnss_Synchro gs3 = Gnss_Synchro(); + Gnss_Synchro gs4 = Gnss_Synchro(); + Gnss_Synchro gs5 = Gnss_Synchro(); + Gnss_Synchro gs6 = Gnss_Synchro(); + Gnss_Synchro gs7 = Gnss_Synchro(); + Gnss_Synchro gs8 = Gnss_Synchro(); + + std::string sys = "G"; + gs1.System = *sys.c_str(); + gs2.System = *sys.c_str(); + gs3.System = *sys.c_str(); + gs4.System = *sys.c_str(); + + sys = "E"; + gs5.System = *sys.c_str(); + gs6.System = *sys.c_str(); + gs7.System = *sys.c_str(); + gs8.System = *sys.c_str(); + + std::string sig = "1C"; + std::memcpy((void*)gs1.Signal, sig.c_str(), 3); + std::memcpy((void*)gs2.Signal, sig.c_str(), 3); + std::memcpy((void*)gs3.Signal, sig.c_str(), 3); + std::memcpy((void*)gs4.Signal, sig.c_str(), 3); + + sig = "5X"; + std::memcpy((void*)gs5.Signal, sig.c_str(), 3); + std::memcpy((void*)gs6.Signal, sig.c_str(), 3); + + sig = "1B"; + std::memcpy((void*)gs7.Signal, sig.c_str(), 3); + std::memcpy((void*)gs8.Signal, sig.c_str(), 3); + + gs1.PRN = 3; + gs2.PRN = 8; + gs3.PRN = 14; + gs4.PRN = 16; + gs5.PRN = 3; + gs6.PRN = 16; + gs7.PRN = 14; + gs8.PRN = 16; + + gs2.Pseudorange_m = 22000002.1; + gs2.Carrier_phase_rads = 45.4; + gs2.Carrier_Doppler_hz = 321; + gs2.CN0_dB_hz = 39; + + gs4.Pseudorange_m = 22000000; + gs4.Carrier_phase_rads = 23.4; + gs4.Carrier_Doppler_hz = -1534; + gs4.CN0_dB_hz = 40; + + gs6.Pseudorange_m = 22000000; + gs6.Carrier_phase_rads = 52.1; + gs6.Carrier_Doppler_hz = 1534; + gs6.CN0_dB_hz = 41; + + gs8.Pseudorange_m = 22000000; + gs8.Carrier_phase_rads = 0.8; + gs8.Carrier_Doppler_hz = -20; + gs8.CN0_dB_hz = 42; + + gnss_pseudoranges_map.insert( std::pair(1,gs1) ); + gnss_pseudoranges_map.insert( std::pair(2,gs2) ); + gnss_pseudoranges_map.insert( std::pair(3,gs3) ); + gnss_pseudoranges_map.insert( std::pair(4,gs4) ); + gnss_pseudoranges_map.insert( std::pair(5,gs5) ); + gnss_pseudoranges_map.insert( std::pair(6,gs6) ); + gnss_pseudoranges_map.insert( std::pair(7,gs7) ); + gnss_pseudoranges_map.insert( std::pair(8,gs8) ); + + rp->log_rinex_obs(rp->obsFile, eph_gps, eph_gal, 0.0, gnss_pseudoranges_map); + + rp->obsFile.seekp(0); + + while(!rp->obsFile.eof()) + { + std::getline(rp->obsFile, line_str); + if(!no_more_finds) + { + if (line_str.find("E16", 0) != std::string::npos) + { + no_more_finds = true; + line_aux = std::string(line_str); + } + } + } + std::string expected_str("E16 22000000.000 7 0.127 7 -20.000 7 42.000 22000000.000 6 8.292 6 1534.000 6 41.000"); + EXPECT_EQ(0, expected_str.compare(line_aux)); + + if(remove(rp->obsfilename.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; +}