mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 12:40:35 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-11-08 17:16:51 +01:00
commit be9a9a7f9e
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); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
this->set_position_UTC_time(p_time); 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) DLOG(INFO) << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() << " 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}; /* Conversion of Cartesian coordinates (X,Y,Z) to geographical
arma::vec LLH = cart2geo(XYZ, elipsoid_selection); coordinates (latitude, longitude, h) on a selected reference ellipsoid.
d_latitude_d = LLH(0) * 180.0 / GPS_PI;
d_longitude_d = LLH(1) * 180.0 / GPS_PI; Choices of Reference Ellipsoid for Geographical Coordinates
d_height_m = LLH(2); 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; return 0;
} }

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

@ -127,7 +127,7 @@ public:
* 4 - World Geodetic System 1984. * 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 * \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() bool rtklib_solver::save_matfile()
{ {
// READ DUMP FILE // READ DUMP FILE
@ -396,7 +395,6 @@ bool rtklib_solver::save_matfile()
return true; return true;
} }
rtklib_solver::~rtklib_solver() rtklib_solver::~rtklib_solver()
{ {
if (d_dump_file.is_open() == true) 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::from_time_t(rtklib_utc_time.time);
p_time += boost::posix_time::microseconds(static_cast<long>(round(rtklib_utc_time.sec * 1e6))); p_time += boost::posix_time::microseconds(static_cast<long>(round(rtklib_utc_time.sec * 1e6)));
this->set_position_UTC_time(p_time); 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) DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude() << " is Lat = " << this->get_latitude() << " [deg], Long = " << this->get_longitude()