From 79fa7ce6ca2d824c38cf2052b38e1bc3d0eadc5a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 8 Nov 2018 15:11:55 +0100 Subject: [PATCH] Remove duplicated code --- src/algorithms/PVT/libs/hybrid_ls_pvt.cc | 2 +- src/algorithms/PVT/libs/pvt_solution.cc | 48 +++--------------------- src/algorithms/PVT/libs/pvt_solution.h | 2 +- src/algorithms/PVT/libs/rtklib_solver.cc | 4 +- 4 files changed, 11 insertions(+), 45 deletions(-) diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 2bded706f..d807a82db 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -338,7 +338,7 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_observables_map, do boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); this->set_position_UTC_time(p_time); - cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); + cart_to_geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time) << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 6071f38b0..0703421b1 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -87,49 +87,13 @@ arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec } -int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) +int Pvt_Solution::cart_to_geo(double X, double Y, double Z, int elipsoid_selection) { - /* Conversion of Cartesian coordinates (X,Y,Z) to geographical - coordinates (latitude, longitude, h) on a selected reference ellipsoid. - - Choices of Reference Ellipsoid for Geographical Coordinates - 0. International Ellipsoid 1924 - 1. International Ellipsoid 1967 - 2. World Geodetic System 1972 - 3. Geodetic Reference System 1980 - 4. World Geodetic System 1984 - */ - - const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0}; - const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563}; - - double lambda = atan2(Y, X); - double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection])); - double c = a[elipsoid_selection] * sqrt(1.0 + ex2); - double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection]))); - - double h = 0.1; - double oldh = 0.0; - double N; - int iterations = 0; - do - { - oldh = h; - N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); - phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h))))); - h = sqrt(X * X + Y * Y) / cos(phi) - N; - iterations = iterations + 1; - if (iterations > 100) - { - DLOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh; - break; - } - } - while (std::abs(h - oldh) > 1.0e-12); - d_latitude_d = phi * 180.0 / GPS_PI; - d_longitude_d = lambda * 180.0 / GPS_PI; - d_height_m = h; - //todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h + arma::vec XYZ = {X, Y, Z}; + arma::vec LLH = cart2geo(XYZ, elipsoid_selection); + d_latitude_d = LLH(0) * 180.0 / GPS_PI; + d_longitude_d = LLH(1) * 180.0 / GPS_PI; + d_height_m = LLH(2); return 0; } diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index bfdc217b7..1367cc38f 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -127,7 +127,7 @@ public: * 4 - World Geodetic System 1984. * */ - int cart2geo(double X, double Y, double Z, int elipsoid_selection); + int cart_to_geo(double X, double Y, double Z, int elipsoid_selection); /*! * \brief Tropospheric correction diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 1481dea0e..f5964b22e 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -99,6 +99,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag } } + bool rtklib_solver::save_matfile() { // READ DUMP FILE @@ -395,6 +396,7 @@ bool rtklib_solver::save_matfile() return true; } + rtklib_solver::~rtklib_solver() { if (d_dump_file.is_open() == true) @@ -868,7 +870,7 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); p_time += boost::posix_time::microseconds(static_cast(round(rtklib_utc_time.sec * 1e6))); this->set_position_UTC_time(p_time); - cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); + cart_to_geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time) << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()