From ae722fd354202b954572c718d91b7929c8f0ff81 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Wed, 7 Nov 2018 21:12:00 +0100 Subject: [PATCH] Clean Pvt_Solution class --- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 6 - src/algorithms/PVT/libs/ls_pvt.cc | 8 +- src/algorithms/PVT/libs/pvt_solution.cc | 170 +---------------------- src/algorithms/PVT/libs/pvt_solution.h | 21 --- 4 files changed, 2 insertions(+), 203 deletions(-) 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