diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h index 93f238733..78d22f2b3 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.h +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -57,18 +57,17 @@ public: return role_; } - //! Returns "RTKLIB_Pvt" + //! Returns "RTKLIB_PVT" inline std::string implementation() override { return "RTKLIB_PVT"; } - void clear_ephemeris(); - std::map get_gps_ephemeris(); - std::map get_galileo_ephemeris(); - std::map get_gps_almanac(); - std::map get_galileo_almanac(); - + void clear_ephemeris() override; + std::map get_gps_ephemeris() override; + std::map get_galileo_ephemeris() override; + std::map get_gps_almanac() override; + std::map get_galileo_almanac() override; void connect(gr::top_block_sptr top_block) override; void disconnect(gr::top_block_sptr top_block) override; @@ -91,7 +90,7 @@ public: double* height_m, double* ground_speed_kmh, double* course_over_ground_deg, - time_t* UTC_time); + time_t* UTC_time) override; private: rtklib_pvt_cc_sptr pvt_; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 98202bb68..762a36f1c 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -36,7 +36,6 @@ #include "gnss_sdr_create_directory.h" #include #include -#include #include #include #include @@ -47,7 +46,6 @@ #include #include #include -#include #if OLD_BOOST #include namespace bc = boost::math; @@ -909,7 +907,7 @@ bool rtklib_pvt_cc::get_latest_PVT(double* longitude_deg, *height_m = d_ls_pvt->get_height(); *ground_speed_kmh = d_ls_pvt->get_speed_over_ground() * 3600.0 / 1000.0; *course_over_ground_deg = d_ls_pvt->get_course_over_ground(); - *UTC_time = boost::posix_time::to_time_t(d_ls_pvt->get_position_UTC_time()); + *UTC_time = to_time_t(d_ls_pvt->get_position_UTC_time()); return true; } @@ -919,6 +917,7 @@ bool rtklib_pvt_cc::get_latest_PVT(double* longitude_deg, } } + int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items __attribute__((unused))) { diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index bf53bfd3d..c26f923d4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -40,6 +40,8 @@ #include "rtcm_printer.h" #include "pvt_conf.h" #include "rtklib_solver.h" +#include +#include #include #include #include @@ -135,6 +137,11 @@ private: bool d_xml_storage; std::string xml_base_path; + inline std::time_t to_time_t(boost::posix_time::ptime pt) + { + return (pt - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1))).total_seconds(); + } + public: rtklib_pvt_cc(uint32_t nchannels, diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 6f269b7c7..2bded706f 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -150,8 +150,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, do // 4- fill the observations vector with the corrected observables obs.resize(valid_obs + 1, 1); obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - this->get_time_offset_s() * GALILEO_C_m_s; - this->set_visible_satellites_ID(valid_obs, galileo_ephemeris_iter->second.i_satellite_PRN); - this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz); Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time); @@ -213,8 +211,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, do double Code_bias_m = P1_P2 / (1.0 - Gamma); obs.resize(valid_obs + 1, 1); obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s - Code_bias_m - this->get_time_offset_s() * GPS_C_m_s; - this->set_visible_satellites_ID(valid_obs, gps_ephemeris_iter->second.i_satellite_PRN); - this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz); // SV ECEF DEBUG OUTPUT LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN @@ -265,8 +261,6 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, do // 4- fill the observations vector with the corrected observables obs.resize(valid_obs + 1, 1); obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s; - this->set_visible_satellites_ID(valid_obs, gps_cnav_ephemeris_iter->second.i_satellite_PRN); - this->set_visible_satellites_CN0_dB(valid_obs, gnss_observables_iter->second.CN0_dB_hz); GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week; GPS_week = GPS_week % 1024; //Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV) diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index 8525d952f..f7c48b0aa 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -236,9 +236,6 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, double* elev = 0; double* dist = 0; Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2)); - this->set_visible_satellites_Az(i, *azim); - this->set_visible_satellites_El(i, *elev); - this->set_visible_satellites_Distance(i, *dist); if (traveltime < 0.1 && nmbOfSatellites > 3) { @@ -253,7 +250,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, else { //--- Find delay due to troposphere (in meters) - Ls_Pvt::tropo(&trop, sin(this->get_visible_satellites_El(i) * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); + Ls_Pvt::tropo(&trop, sin(*elev * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0); if (trop > 5.0) trop = 0.0; //check for erratic values } } @@ -280,9 +277,6 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, } } - //-- compute the Dilution Of Precision values - //this->set_Q(arma::inv(arma::htrans(A) * A)); - // check the consistency of the PVT solution if (((fabs(pos(3)) * 1000.0) / GPS_C_m_s) > GPS_STARTOFFSET_ms * 2) { diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 0ecbd15eb..55c52324b 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -562,6 +562,7 @@ bool Pvt_Solution::is_averaging() const return d_flag_averaging; } + bool Pvt_Solution::is_valid_position() const { return b_valid_position; @@ -611,172 +612,3 @@ void Pvt_Solution::set_num_valid_observations(int num) { d_valid_observations = num; } - - -bool Pvt_Solution::set_visible_satellites_ID(size_t index, unsigned int prn) -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Setting sat ID to channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return false; - } - else - { - if (prn >= PVT_MAX_PRN) - { - LOG(WARNING) << "Setting to channel " << index << " a PRN of " << prn << " (the maximum is " << PVT_MAX_PRN << ")"; - return false; - } - else - { - d_visible_satellites_IDs[index] = prn; - return true; - } - } -} - - -unsigned int Pvt_Solution::get_visible_satellites_ID(size_t index) const -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Getting sat ID for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return 0; - } - else - { - return d_visible_satellites_IDs[index]; - } -} - - -bool Pvt_Solution::set_visible_satellites_El(size_t index, double el) -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Setting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return false; - } - else - { - if (el > 90.0) - { - LOG(WARNING) << "Setting a sat elevation > 90 [degrees]. Saturating to 90"; - d_visible_satellites_El[index] = 90.0; - } - else - { - if (el < -90.0) - { - LOG(WARNING) << "Setting a sat elevation < -90 [degrees]. Saturating to -90"; - d_visible_satellites_El[index] = -90.0; - } - else - { - d_visible_satellites_El[index] = el; - } - } - return true; - } -} - - -double Pvt_Solution::get_visible_satellites_El(size_t index) const -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Getting sat elevation for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return 0.0; - } - else - { - return d_visible_satellites_El[index]; - } -} - - -bool Pvt_Solution::set_visible_satellites_Az(size_t index, double az) -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return false; - } - else - { - d_visible_satellites_Az[index] = az; - return true; - } -} - - -double Pvt_Solution::get_visible_satellites_Az(size_t index) const -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Getting sat azimuth for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return 0.0; - } - else - { - return d_visible_satellites_Az[index]; - } -} - - -bool Pvt_Solution::set_visible_satellites_Distance(size_t index, double dist) -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Setting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return false; - } - else - { - d_visible_satellites_Distance[index] = dist; - return true; - } -} - - -double Pvt_Solution::get_visible_satellites_Distance(size_t index) const -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Getting sat distance for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return 0.0; - } - else - { - return d_visible_satellites_Distance[index]; - } -} - - -bool Pvt_Solution::set_visible_satellites_CN0_dB(size_t index, double cn0) -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Setting sat Cn0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return false; - } - else - { - d_visible_satellites_CN0_dB[index] = cn0; - return true; - } -} - - -double Pvt_Solution::get_visible_satellites_CN0_dB(size_t index) const -{ - if (index >= PVT_MAX_CHANNELS) - { - LOG(WARNING) << "Getting received CN0 for channel " << index << " (the maximum is " << PVT_MAX_CHANNELS << ")"; - return 0.0; - } - else - { - return d_visible_satellites_CN0_dB[index]; - } -} diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 585703d68..5c21de7c6 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -72,12 +72,6 @@ private: boost::posix_time::ptime d_position_UTC_time; int d_valid_observations; - int d_visible_satellites_IDs[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites - double d_visible_satellites_El[PVT_MAX_CHANNELS] = {}; // Array with the LOS Elevation of the valid satellites - double d_visible_satellites_Az[PVT_MAX_CHANNELS] = {}; // Array with the LOS Azimuth of the valid satellites - double d_visible_satellites_Distance[PVT_MAX_CHANNELS] = {}; // Array with the LOS Distance of the valid satellites - double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS] = {}; // Array with the IDs of the valid satellites - public: Pvt_Solution(); @@ -110,21 +104,6 @@ public: int get_num_valid_observations() const; //!< Get the number of valid pseudorange observations (valid satellites) void set_num_valid_observations(int num); //!< Set the number of valid pseudorange observations (valid satellites) - bool set_visible_satellites_ID(size_t index, unsigned int prn); //!< Set the ID of the visible satellite index channel - unsigned int get_visible_satellites_ID(size_t index) const; //!< Get the ID of the visible satellite index channel - - bool set_visible_satellites_El(size_t index, double el); //!< Set the LOS Elevation, in degrees, of the visible satellite index channel - double get_visible_satellites_El(size_t index) const; //!< Get the LOS Elevation, in degrees, of the visible satellite index channel - - bool set_visible_satellites_Az(size_t index, double az); //!< Set the LOS Azimuth, in degrees, of the visible satellite index channel - double get_visible_satellites_Az(size_t index) const; //!< Get the LOS Azimuth, in degrees, of the visible satellite index channel - - bool set_visible_satellites_Distance(size_t index, double dist); //!< Set the LOS Distance of the visible satellite index channel - double get_visible_satellites_Distance(size_t index) const; //!< Get the LOS Distance of the visible satellite index channel - - bool set_visible_satellites_CN0_dB(size_t index, double cn0); //!< Set the CN0 in dB of the visible satellite index channel - double get_visible_satellites_CN0_dB(size_t index) const; //!< Get the CN0 in dB of the visible satellite index channel - //averaging void perform_pos_averaging(); void set_averaging_depth(int depth); //!< Set length of averaging window diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index b418dbdba..1481dea0e 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -777,9 +777,9 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ for (int i = 0; i < MAXSAT; i++) { - nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */ - nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */ - nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L5/E5 */ + nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; // L1/E1 + nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; // L2 + nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; // L5/E5 } result = rtkpos(&rtk_, obs_data, valid_obs + glo_valid_obs, &nav_data); @@ -788,20 +788,20 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ { LOG(INFO) << "RTKLIB rtkpos error"; DLOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf; - this->set_time_offset_s(0.0); //reset rx time estimation + this->set_time_offset_s(0.0); // reset rx time estimation this->set_num_valid_observations(0); } else { - this->set_num_valid_observations(rtk_.sol.ns); //record the number of valid satellites used by the PVT solver + this->set_num_valid_observations(rtk_.sol.ns); // record the number of valid satellites used by the PVT solver pvt_sol = rtk_.sol; // DOP computation unsigned int used_sats = 0; for (unsigned int i = 0; i < MAXSAT; i++) { + pvt_ssat[i] = rtk_.ssat[i]; if (rtk_.ssat[i].vs == 1) { - pvt_ssat[i] = rtk_.ssat[i]; used_sats++; } } diff --git a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc index 535cca464..ccf97ecaa 100644 --- a/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/pvt/nmea_printer_test.cc @@ -160,13 +160,20 @@ TEST_F(NmeaPrinterTest, PrintLine) std::shared_ptr pvt_solution = std::make_shared(12, "filename", false, false, rtk); boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), - boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc - pvt_solution->set_position_UTC_time(pt); + boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); + std::time_t tim = (pt - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1))).total_seconds(); + gtime_t gtime; + gtime.time = tim; + gtime.sec = 0.0; - arma::vec pos = {49.27416667, -123.18533333, 0}; - pvt_solution->set_rx_pos(pos); - - pvt_solution->set_valid_position(true); + pvt_solution->pvt_sol.rr[0] = -2282104.0; //49.27416667; + pvt_solution->pvt_sol.rr[1] = -3489369.0; //-123.18533333; + pvt_solution->pvt_sol.rr[2] = 4810507.0; // 0 + pvt_solution->pvt_sol.rr[3] = 0.0; + pvt_solution->pvt_sol.rr[4] = 0.0; + pvt_solution->pvt_sol.rr[5] = 0.0; + pvt_solution->pvt_sol.stat = 1; // SOLQ_FIX + pvt_solution->pvt_sol.time = gtime; bool flag_nmea_output_file = true; ASSERT_NO_THROW({ @@ -184,46 +191,7 @@ TEST_F(NmeaPrinterTest, PrintLine) std::size_t found = line.find(GPRMC); if (found != std::string::npos) { - EXPECT_EQ(line, "$GPRMC,225446.000,A,4916.4500,N,12311.1199,W,0.00,0.00,191194,,*1c\r"); - } - } - test_file.close(); - } - EXPECT_EQ(0, remove(filename.c_str())) << "Failure deleting a temporary file."; -} - - -TEST_F(NmeaPrinterTest, PrintLineLessthan10min) -{ - std::string filename("nmea_test.nmea"); - std::shared_ptr pvt_solution = std::make_shared(12, "filename", false, false, rtk); - - boost::posix_time::ptime pt(boost::gregorian::date(1994, boost::date_time::Nov, 19), - boost::posix_time::hours(22) + boost::posix_time::minutes(54) + boost::posix_time::seconds(46)); // example from http://aprs.gids.nl/nmea/#rmc - pvt_solution->set_position_UTC_time(pt); - - arma::vec pos = {49.07416667, -123.02527778, 0}; - pvt_solution->set_rx_pos(pos); - - pvt_solution->set_valid_position(true); - - bool flag_nmea_output_file = true; - ASSERT_NO_THROW({ - std::shared_ptr nmea_printer = std::make_shared(filename, flag_nmea_output_file, false, ""); - nmea_printer->Print_Nmea_Line(pvt_solution, false); - }) << "Failure printing NMEA messages."; - - std::ifstream test_file(filename); - std::string line; - std::string GPRMC("$GPRMC"); - if (test_file.is_open()) - { - while (getline(test_file, line)) - { - std::size_t found = line.find(GPRMC); - if (found != std::string::npos) - { - EXPECT_EQ(line, "$GPRMC,225446.000,A,4904.4500,N,12301.5166,W,0.00,0.00,191194,,*1a\r"); + EXPECT_EQ(line, "$GPRMC,225436.00,A,4916.4497617,N,12311.1202744,W,0.00,0.00,191194,0.0,E,D*21\r"); } } test_file.close();