From 6b6dc63dfc8a09a7ad1f8f2670918f81c2b90f60 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 8 Nov 2018 01:14:17 +0100 Subject: [PATCH] Remove functions now present in geofunctions --- src/algorithms/PVT/libs/ls_pvt.cc | 5 +- src/algorithms/PVT/libs/pvt_solution.cc | 192 +----------------------- src/algorithms/PVT/libs/pvt_solution.h | 35 ----- 3 files changed, 9 insertions(+), 223 deletions(-) diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index f7c48b0aa..77e2c69a9 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -31,6 +31,7 @@ #include "ls_pvt.h" #include "GPS_L1_CA.h" +#include "geofunctions.h" #include #include #include @@ -235,12 +236,12 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs, double* azim = 0; double* elev = 0; double* dist = 0; - Ls_Pvt::topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2)); + topocent(azim, elev, dist, pos.subvec(0, 2), Rot_X - pos.subvec(0, 2)); if (traveltime < 0.1 && nmbOfSatellites > 3) { //--- Find receiver's height - Ls_Pvt::togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2)); + togeod(&dphi, &dlambda, &h, 6378137.0, 298.257223563, pos(0), pos(1), pos(2)); // Add troposphere correction if the receiver is below the troposphere if (h > 15000) { diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 55c52324b..6071f38b0 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -31,6 +31,7 @@ #include "pvt_solution.h" #include "GPS_L1_CA.h" +#include "geofunctions.h" #include #include @@ -133,125 +134,6 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) } -int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z) -{ - /* Subroutine to calculate geodetic coordinates latitude, longitude, - height given Cartesian coordinates X,Y,Z, and reference ellipsoid - values semi-major axis (a) and the inverse of flattening (finv). - - The output units of angular quantities will be in decimal degrees - (15.5 degrees not 15 deg 30 min). The output units of h will be the - same as the units of X,Y,Z,a. - - Inputs: - a - semi-major axis of the reference ellipsoid - finv - inverse of flattening of the reference ellipsoid - X,Y,Z - Cartesian coordinates - - Outputs: - dphi - latitude - dlambda - longitude - h - height above reference ellipsoid - - Based in a Matlab function by Kai Borre - */ - - *h = 0; - double tolsq = 1.e-10; // tolerance to accept convergence - int maxit = 10; // max number of iterations - double rtd = 180.0 / GPS_PI; - - // compute square of eccentricity - double esq; - if (finv < 1.0E-20) - { - esq = 0.0; - } - else - { - esq = (2.0 - 1.0 / finv) / finv; - } - - // first guess - double P = sqrt(X * X + Y * Y); // P is distance from spin axis - - //direct calculation of longitude - if (P > 1.0E-20) - { - *dlambda = atan2(Y, X) * rtd; - } - else - { - *dlambda = 0.0; - } - - // correct longitude bound - if (*dlambda < 0) - { - *dlambda = *dlambda + 360.0; - } - - double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0) - - double sinphi; - if (r > 1.0E-20) - { - sinphi = Z / r; - } - else - { - sinphi = 0.0; - } - *dphi = asin(sinphi); - - // initial value of height = distance from origin minus - // approximate distance from origin to surface of ellipsoid - if (r < 1.0E-20) - { - *h = 0; - return 1; - } - - *h = r - a * (1 - sinphi * sinphi / finv); - - // iterate - double cosphi; - double N_phi; - double dP; - double dZ; - double oneesq = 1.0 - esq; - - for (int i = 0; i < maxit; i++) - { - sinphi = sin(*dphi); - cosphi = cos(*dphi); - - // compute radius of curvature in prime vertical direction - N_phi = a / sqrt(1 - esq * sinphi * sinphi); - - // compute residuals in P and Z - dP = P - (N_phi + (*h)) * cosphi; - dZ = Z - (N_phi * oneesq + (*h)) * sinphi; - - // update height and latitude - *h = *h + (sinphi * dZ + cosphi * dP); - *dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h)); - - // test for convergence - if ((dP * dP + dZ * dZ) < tolsq) - { - break; - } - if (i == (maxit - 1)) - { - LOG(WARNING) << "The computation of geodetic coordinates did not converge"; - } - } - *dphi = (*dphi) * rtd; - return 0; -} - - int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km) { /* Inputs: @@ -359,73 +241,6 @@ int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb } -int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx) -{ - /* Transformation of vector dx into topocentric coordinate - system with origin at x - Inputs: - x - vector origin coordinates (in ECEF system [X; Y; Z;]) - dx - vector ([dX; dY; dZ;]). - - Outputs: - D - vector length. Units like the input - Az - azimuth from north positive clockwise, degrees - El - elevation angle, degrees - - Based on a Matlab function by Kai Borre - */ - - double lambda; - double phi; - double h; - double dtr = GPS_PI / 180.0; - double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84 - double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84 - - // Transform x into geodetic coordinates - Pvt_Solution::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2)); - - double cl = cos(lambda * dtr); - double sl = sin(lambda * dtr); - double cb = cos(phi * dtr); - double sb = sin(phi * dtr); - - arma::mat F = {{-sl, -sb * cl, cb * cl}, - {cl, -sb * sl, cb * sl}, - {0, 0, cb, sb}}; - - arma::vec local_vector; - - local_vector = arma::htrans(F) * dx; - - double E = local_vector(0); - double N = local_vector(1); - double U = local_vector(2); - - double hor_dis; - hor_dis = sqrt(E * E + N * N); - - if (hor_dis < 1.0E-20) - { - *Az = 0; - *El = 90; - } - else - { - *Az = atan2(E, N) / dtr; - *El = atan2(U, hor_dis) / dtr; - } - - if (*Az < 0) - { - *Az = *Az + 360.0; - } - - *D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); - return 0; -} - - void Pvt_Solution::set_averaging_depth(int depth) { d_averaging_depth = depth; @@ -519,26 +334,31 @@ double Pvt_Solution::get_height() const return d_height_m; } + double Pvt_Solution::get_speed_over_ground() const { return d_speed_over_ground_m_s; } + void Pvt_Solution::set_speed_over_ground(double speed_m_s) { d_speed_over_ground_m_s = speed_m_s; } + void Pvt_Solution::set_course_over_ground(double cog_deg) { d_course_over_ground_d = cog_deg; } + double Pvt_Solution::get_course_over_ground() const { return d_course_over_ground_d; } + double Pvt_Solution::get_avg_latitude() const { return d_avg_latitude_d; diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 5c21de7c6..bfdc217b7 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -129,41 +129,6 @@ public: */ int cart2geo(double X, double Y, double Z, int elipsoid_selection); - /*! - * \brief Transformation of vector dx into topocentric coordinate system with origin at x - * - * \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;]) - * \param[in] dx Vector ([dX; dY; dZ;]). - * - * \param[out] D Vector length. Units like the input - * \param[out] Az Azimuth from north positive clockwise, degrees - * \param[out] El Elevation angle, degrees - * - * Based on a Matlab function by Kai Borre - */ - int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx); - - /*! - * \brief Subroutine to calculate geodetic coordinates latitude, longitude, - * height given Cartesian coordinates X,Y,Z, and reference ellipsoid - * values semi-major axis (a) and the inverse of flattening (finv). - * - * The output units of angular quantities will be in decimal degrees - * (15.5 degrees not 15 deg 30 min). The output units of h will be the - * same as the units of X,Y,Z,a. - * - * \param[in] a - semi-major axis of the reference ellipsoid - * \param[in] finv - inverse of flattening of the reference ellipsoid - * \param[in] X,Y,Z - Cartesian coordinates - * - * \param[out] dphi - latitude - * \param[out] dlambda - longitude - * \param[out] h - height above reference ellipsoid - * - * Based in a Matlab function by Kai Borre - */ - int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); - /*! * \brief Tropospheric correction *