mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-04 09:13:05 +00:00
Code Issues Releases Wiki Activity

Remove duplicated code

Browse Source
This commit is contained in:
Carles Fernandez
2018-11-08 15:11:55 +01:00
parent 98726ef036
commit 79fa7ce6ca
4 changed files with 11 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@@ -338,7 +338,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int, Gnss_Synchro> 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<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
cart_to_geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(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()

48
src/algorithms/PVT/libs/pvt_solution.cc
View File

@@ -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;
}

2
src/algorithms/PVT/libs/pvt_solution.h
View File

@@ -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

4
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@@ -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<int, Gnss_Synchro> &gnss_observables_
p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
p_time += boost::posix_time::microseconds(static_cast<long>(round(rtklib_utc_time.sec * 1e6)));
this->set_position_UTC_time(p_time);
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);
cart_to_geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(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()