From da6a15e6377621ffef41f51789105a6d872ab47c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 5 Sep 2014 16:38:44 +0200 Subject: [PATCH] Adding Mixed GPS/Galileo observations. --- .../PVT/gnuradio_blocks/hybrid_pvt_cc.cc | 16 +- src/algorithms/PVT/libs/rinex_printer.cc | 349 +++++++++++++++++- src/algorithms/PVT/libs/rinex_printer.h | 10 + 3 files changed, 357 insertions(+), 18 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index 5169dd148..c04784e4b 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -229,7 +229,7 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items, { if (arrived_galileo_almanac) { - //rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time); 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 } @@ -244,12 +244,14 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items, 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(); - // 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_pseudoranges_map); - // } + 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()) ) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map); + } } } } diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index defb71b77..71180be41 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -63,12 +63,13 @@ Rinex_Printer::Rinex_Printer() Rinex_Printer::navGalFile.open(navGalfilename, std::ios::out | std::ios::app); Rinex_Printer::navMixFile.open(navMixfilename, std::ios::out | std::ios::app); - // RINEX v3.00 codes + // RINEX v3.02 codes satelliteSystem["GPS"] = "G"; satelliteSystem["GLONASS"] = "R"; satelliteSystem["SBAS payload"] = "S"; satelliteSystem["Galileo"] = "E"; satelliteSystem["Beidou"] = "C"; + satelliteSystem["Mixed"] = "M"; observationCode["GPS_L1_CA"] = "1C"; // "1C" GPS L1 C/A observationCode["GPS_L1_P"] = "1P"; // "1P" GPS L1 P @@ -155,10 +156,10 @@ Rinex_Printer::Rinex_Printer() stringVersion = "2.10"; } else if ( FLAGS_RINEX_version.compare("2.10") == 0 ) - { - version = 2; - stringVersion = "2.10"; - } + { + version = 2; + stringVersion = "2.10"; + } else { LOG(ERROR) << "Unknown RINEX version " << FLAGS_RINEX_version << " (must be 2.11 or 3.01)" << std::endl; @@ -1662,13 +1663,12 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris eph, doub void Rinex_Printer::rinex_obs_header(std::ofstream& out, Galileo_Ephemeris eph, double d_TOW_first_observation) { - std::string line; version = 3; // -------- Line 1 line = std::string(5, ' '); - line += "3.01"; + line += "3.02"; line += std::string(11, ' '); line += Rinex_Printer::leftJustify("OBSERVATION DATA", 20); line += satelliteSystem["Galileo"]; @@ -1724,7 +1724,7 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Galileo_Ephemeris eph, // -------- Line MARKER TYPE //line.clear(); - //line += Rinex_Printer::leftJustify("GROUND_CRAFT", 20); // put a flag or a property + //line += Rinex_Printer::leftJustify("NON_GEODETIC", 20); // put a flag or a property //line += std::string(40, ' '); //line += Rinex_Printer::leftJustify("MARKER TYPE", 20); //Rinex_Printer::lengthCheck(line); @@ -1863,14 +1863,240 @@ void Rinex_Printer::rinex_obs_header(std::ofstream& out, Galileo_Ephemeris eph, } +void Rinex_Printer::rinex_obs_header(std::ofstream& out, Gps_Ephemeris gps_eph, Galileo_Ephemeris galileo_eph, double d_TOW_first_observation) +{ + std::string line; + version = 3; + + // -------- Line 1 + line = std::string(5, ' '); + line += "3.02"; + line += std::string(11, ' '); + line += Rinex_Printer::leftJustify("OBSERVATION DATA", 20); + line += satelliteSystem["Mixed"]; + line += std::string(19, ' '); + line += std::string("RINEX VERSION / TYPE"); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 2 + line.clear(); + line += Rinex_Printer::leftJustify("G = GPS R = GLONASS E = GALILEO S = GEO M = MIXED", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line 3 + line.clear(); + line += Rinex_Printer::getLocalTime(); + line += std::string("PGM / RUN BY / DATE"); + line += std::string(1, ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("MIXED (GPS/GALILEO) OBSERVATION DATA FILE GENERATED BY GNSS-SDR", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + std::string gnss_sdr_version(GNSS_SDR_VERSION); + line += "GNSS-SDR VERSION "; + line += Rinex_Printer::leftJustify(gnss_sdr_version, 43); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line COMMENT + line.clear(); + line += Rinex_Printer::leftJustify("See http://gnss-sdr.org", 60); + line += Rinex_Printer::leftJustify("COMMENT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER NAME + line.clear(); + line += Rinex_Printer::leftJustify("DEFAULT MARKER NAME", 60); // put a flag or a property, + line += Rinex_Printer::leftJustify("MARKER NAME", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line MARKER TYPE + //line.clear(); + //line += Rinex_Printer::leftJustify("NON_GEODETIC", 20); // put a flag or a property + //line += std::string(40, ' '); + //line += Rinex_Printer::leftJustify("MARKER TYPE", 20); + //Rinex_Printer::lengthCheck(line); + //out << line << std::endl; + + // -------- Line OBSERVER / AGENCY + line.clear(); + std::string username; + char c_username[20] = {0}; + int nGet = getlogin_r(c_username, sizeof(c_username) - 1); + if (nGet == 0) + { + username = c_username; + } + else + { + username = "UNKNOWN USER"; + } + line += leftJustify(username, 20); + line += Rinex_Printer::leftJustify("CTTC", 40); // add flag and property + line += Rinex_Printer::leftJustify("OBSERVER / AGENCY", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- Line REC / TYPE VERS + line.clear(); + line += Rinex_Printer::leftJustify("GNSS-SDR", 20); // add flag and property + line += Rinex_Printer::leftJustify("Software Receiver", 20); // add flag and property + //line += Rinex_Printer::leftJustify(google::VersionString(), 20); // add flag and property + if(gnss_sdr_version.length() > 20) gnss_sdr_version.resize(9, ' '); + line += Rinex_Printer::leftJustify(gnss_sdr_version, 20); + line += Rinex_Printer::leftJustify("REC # / TYPE / VERS", 20); + lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA TYPE + line.clear(); + line += Rinex_Printer::leftJustify("Antenna number", 20); // add flag and property + line += Rinex_Printer::leftJustify("Antenna type", 20); // add flag and property + line += std::string(20, ' '); + line += Rinex_Printer::leftJustify("ANT # / TYPE", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- APPROX POSITION (optional for moving platforms) + // put here real data! + double antena_x = 0.0; + double antena_y = 0.0; + double antena_z = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_x, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_y, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_z, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("APPROX POSITION XYZ", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- ANTENNA: DELTA H/E/N + // put here real data! + double antena_h = 0.0; + double antena_e = 0.0; + double antena_n = 0.0; + line.clear(); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_h, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_e, 4), 14); + line += Rinex_Printer::rightJustify(Rinex_Printer::asString(antena_n, 4), 14); + line += std::string(18, ' '); + line += Rinex_Printer::leftJustify("ANTENNA: DELTA H/E/N", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- SYS / OBS TYPES + // one line per available system + line.clear(); + line += satelliteSystem["GPS"]; + line += std::string(2, ' '); + std::stringstream strm; + numberTypesObservations = 4; + strm << numberTypesObservations; + line += Rinex_Printer::rightJustify(strm.str(), 3); + // per type of observation + // GPS L1 PSEUDORANGE + line += std::string(1, ' '); + line += observationType["PSEUDORANGE"]; + line += observationCode["GPS_L1_CA"]; + // GPS L1 PHASE + line += std::string(1, ' '); + line += observationType["CARRIER_PHASE"]; + line += observationCode["GPS_L1_CA"]; + // GPS DOPPLER L1 + line += std::string(1, ' '); + line += observationType["DOPPLER"]; + line += observationCode["GPS_L1_CA"]; + // GPS L1 CA SIGNAL STRENGTH + line += std::string(1, ' '); + line += observationType["SIGNAL_STRENGTH"]; + line += observationCode["GPS_L1_CA"]; + line += std::string(60-line.size(), ' '); + line += Rinex_Printer::leftJustify("SYS / # / OBS TYPES", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + line.clear(); + 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 += 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); + line += std::string(40, ' '); + line += Rinex_Printer::leftJustify("SIGNAL STRENGTH UNIT", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- TIME OF FIRST OBS + line.clear(); + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(gps_eph, d_TOW_first_observation); + std::string timestring=boost::posix_time::to_iso_string(p_gps_time); + std::string year (timestring, 0, 4); + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + double gps_t = d_TOW_first_observation; + double seconds = fmod(gps_t, 60); + line += Rinex_Printer::rightJustify(year, 6); + line += Rinex_Printer::rightJustify(month, 6); + line += Rinex_Printer::rightJustify(day, 6); + line += Rinex_Printer::rightJustify(hour, 6); + line += Rinex_Printer::rightJustify(minutes, 6); + line += Rinex_Printer::rightJustify(asString(seconds, 7), 13); + line += Rinex_Printer::rightJustify(std::string("GPS"), 8); + line += std::string(9, ' '); + line += Rinex_Printer::leftJustify("TIME OF FIRST OBS", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + // -------- end of header + line.clear(); + line += std::string(60, ' '); + line += Rinex_Printer::leftJustify("END OF HEADER", 20); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; +} void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris eph, double obs_time, std::map pseudoranges) { - // RINEX observations timestamps are GPS timestamps. - + // RINEX observations timestamps are GPS timestamps. std::string line; - boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph,obs_time); + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(eph, obs_time); std::string timestring = boost::posix_time::to_iso_string(p_gps_time); //double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time)); //double gps_t = eph.sv_clock_correction(obs_time); @@ -2155,6 +2381,106 @@ void Rinex_Printer::log_rinex_obs(std::ofstream& out, Galileo_Ephemeris eph, dou } +void Rinex_Printer::log_rinex_obs(std::ofstream& out, Gps_Ephemeris gps_eph, Galileo_Ephemeris galileo_eph, double gps_obs_time, std::map pseudoranges) +{ + std::string line; + + boost::posix_time::ptime p_gps_time = Rinex_Printer::compute_GPS_time(gps_eph, gps_obs_time); + std::string timestring = boost::posix_time::to_iso_string(p_gps_time); + //double utc_t = nav_msg.utc_time(nav_msg.sv_clock_correction(obs_time)); + //double gps_t = eph.sv_clock_correction(obs_time); + double gps_t = gps_obs_time; + + std::string month (timestring, 4, 2); + std::string day (timestring, 6, 2); + std::string hour (timestring, 9, 2); + std::string minutes (timestring, 11, 2); + + std::string year (timestring, 0, 4); + line += std::string(1, '>'); + line += std::string(1, ' '); + line += year; + line += std::string(1, ' '); + line += month; + line += std::string(1, ' '); + line += day; + line += std::string(1, ' '); + line += hour; + line += std::string(1, ' '); + line += minutes; + + line += std::string(1, ' '); + double seconds = fmod(gps_t, 60); + // Add extra 0 if seconds are < 10 + if (seconds < 10) + { + line +=std::string(1, '0'); + } + line += Rinex_Printer::asString(seconds, 7); + line += std::string(2, ' '); + // Epoch flag 0: OK 1: power failure between previous and current epoch <1: Special event + line += std::string(1, '0'); + + //Number of satellites observed in current epoch + int numSatellitesObserved = 0; + std::map::iterator pseudoranges_iter; + for(pseudoranges_iter = pseudoranges.begin(); + pseudoranges_iter != pseudoranges.end(); + pseudoranges_iter++) + { + numSatellitesObserved++; + } + line += Rinex_Printer::rightJustify(boost::lexical_cast(numSatellitesObserved), 3); + + // Receiver clock offset (optional) + //line += rightJustify(asString(clockOffset, 12), 15); + + line += std::string(80 - line.size(), ' '); + Rinex_Printer::lengthCheck(line); + out << line << std::endl; + + std::string s; + for(pseudoranges_iter = pseudoranges.begin(); + pseudoranges_iter != pseudoranges.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 ((int)pseudoranges_iter->first < 10) lineObs += std::string(1, '0'); + lineObs += boost::lexical_cast((int)pseudoranges_iter->first); + lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_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); + } + + // PHASE + lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Carrier_phase_rads/GPS_TWO_PI, 3), 14); + + // DOPPLER + lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.Carrier_Doppler_hz, 3), 14); + + // SIGNAL STRENGTH + lineObs += Rinex_Printer::rightJustify(asString(pseudoranges_iter->second.CN0_dB_hz, 3), 14); + + if (lineObs.size() < 80) lineObs += std::string(80 - lineObs.size(), ' '); + out << lineObs << std::endl; + } +} + + + // represents GPS time (week, TOW) in the date time format of the Gregorian calendar. // -> Leap years are considered, but leap seconds not. void Rinex_Printer::to_date_time(int gps_week, int gps_tow, int &year, int &month, int &day, int &hour, int &minute, int &second) @@ -2225,6 +2551,7 @@ void Rinex_Printer::to_date_time(int gps_week, int gps_tow, int &year, int &mont } + void Rinex_Printer::log_rinex_sbs(std::ofstream& out, Sbas_Raw_Msg sbs_message) { // line 1: PRN / EPOCH / RCVR diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index 7fa1ed725..60744a128 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -115,6 +115,11 @@ public: */ void rinex_obs_header(std::ofstream& out, Galileo_Ephemeris eph, double d_TOW_first_observation); + /*! + * \brief Generates the Mixed (GPS/Galileo) Observation data header + */ + void rinex_obs_header(std::ofstream& out, Gps_Ephemeris gps_eph, Galileo_Ephemeris galileo_eph, double d_TOW_first_observation); + /*! * \brief Generates the SBAS raw data header */ @@ -160,6 +165,11 @@ public: */ void log_rinex_obs(std::ofstream& out, Galileo_Ephemeris eph, double obs_time, std::map pseudoranges); + /*! + * \brief Writes Galileo observables into the RINEX file + */ + void log_rinex_obs(std::ofstream& out, Gps_Ephemeris gps_eph, Galileo_Ephemeris galileo_eph, double gps_obs_time, std::map pseudoranges); + /*! * \brief Represents GPS time in the date time format. Leap years are considered, but leap seconds are not. */