mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 20:20:35 +00:00
Remove functions now present in geofunctions
This commit is contained in:
parent
61d67a7642
commit
6b6dc63dfc
@ -31,6 +31,7 @@
|
||||
|
||||
#include "ls_pvt.h"
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "geofunctions.h"
|
||||
#include <glog/logging.h>
|
||||
#include <exception>
|
||||
#include <stdexcept>
|
||||
@ -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)
|
||||
{
|
||||
|
@ -31,6 +31,7 @@
|
||||
|
||||
#include "pvt_solution.h"
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "geofunctions.h"
|
||||
#include <glog/logging.h>
|
||||
#include <exception>
|
||||
|
||||
@ -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;
|
||||
|
@ -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
|
||||
*
|
||||
|
Loading…
Reference in New Issue
Block a user