mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Revert "Remove duplicated code" 

This reverts commit 79fa7ce6ca.
This commit is contained in:
Carles Fernandez 2018-11-08 17:13:11 +01:00
parent a544112a84
commit 2206969be6
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
4 changed files with 45 additions and 11 deletions
Show Stats 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);
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);
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);
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,13 +87,49 @@ arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec
}
int Pvt_Solution::cart_to_geo(double X, double Y, double Z, int elipsoid_selection)
int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
{
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);
/* 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
return 0;
}

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

@ -127,7 +127,7 @@ public:
* 4 - World Geodetic System 1984.
*
*/
int cart_to_geo(double X, double Y, double Z, int elipsoid_selection);
int cart2geo(double X, double Y, double Z, int elipsoid_selection);
/*!
* \brief Tropospheric correction

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

@ -99,7 +99,6 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
}
}
bool rtklib_solver::save_matfile()
{
// READ DUMP FILE
@ -396,7 +395,6 @@ bool rtklib_solver::save_matfile()
return true;
}
rtklib_solver::~rtklib_solver()
{
if (d_dump_file.is_open() == true)
@ -870,7 +868,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);
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);
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);
DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()