mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-04-04 17:57:03 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
d26420a284
@ -87,7 +87,7 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc(
|
||||
|
||||
switch (d_frame_type)
|
||||
{
|
||||
case 1: //INAV
|
||||
case 1: // INAV
|
||||
{
|
||||
d_PRN_code_period_ms = static_cast<uint32_t>(GALILEO_E1_CODE_PERIOD_MS);
|
||||
d_samples_per_symbol = Galileo_E1_B_SAMPLES_PER_SYMBOL;
|
||||
@ -98,12 +98,13 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc(
|
||||
d_required_symbols = static_cast<uint32_t>(GALILEO_INAV_PAGE_SYMBOLS) + d_samples_per_preamble;
|
||||
// preamble bits to sampled symbols
|
||||
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
|
||||
d_secondary_code_samples = nullptr;
|
||||
d_frame_length_symbols = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS;
|
||||
CodeLength = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS;
|
||||
DataLength = (CodeLength / nn) - mm;
|
||||
break;
|
||||
}
|
||||
case 2: //FNAV
|
||||
case 2: // FNAV
|
||||
{
|
||||
d_PRN_code_period_ms = static_cast<uint32_t>(GALILEO_E5a_CODE_PERIOD_MS);
|
||||
d_samples_per_symbol = GALILEO_FNAV_CODES_PER_SYMBOL;
|
||||
@ -114,7 +115,6 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc(
|
||||
d_required_symbols = static_cast<uint32_t>(GALILEO_FNAV_SYMBOLS_PER_PAGE) * d_samples_per_symbol + d_samples_per_preamble;
|
||||
// preamble bits to sampled symbols
|
||||
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
|
||||
|
||||
d_secondary_code_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(Galileo_E5a_I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment()));
|
||||
d_frame_length_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
|
||||
CodeLength = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
|
||||
@ -142,7 +142,7 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc(
|
||||
{
|
||||
switch (d_frame_type)
|
||||
{
|
||||
case 1: //INAV
|
||||
case 1: // INAV
|
||||
{
|
||||
if (GALILEO_INAV_PREAMBLE.at(i) == '1')
|
||||
{
|
||||
@ -162,7 +162,7 @@ galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc(
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2: //FNAV for E5a-I
|
||||
case 2: // FNAV for E5a-I
|
||||
{
|
||||
// Galileo E5a data channel (E5a-I) still has a secondary code
|
||||
int m = 0;
|
||||
@ -412,6 +412,7 @@ void galileo_telemetry_decoder_cc::decode_FNAV_word(double *page_symbols, int32_
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void galileo_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite)
|
||||
{
|
||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
@ -525,7 +526,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
|
||||
// call the decoder
|
||||
switch (d_frame_type)
|
||||
{
|
||||
case 1: //INAV
|
||||
case 1: // INAV
|
||||
// NEW Galileo page part is received
|
||||
// 0. fetch the symbols into an array
|
||||
if (corr_value > 0) //normal PLL lock
|
||||
@ -544,7 +545,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
|
||||
}
|
||||
decode_INAV_word(d_page_part_symbols, d_frame_length_symbols);
|
||||
break;
|
||||
case 2: //FNAV
|
||||
case 2: // FNAV
|
||||
// NEW Galileo page part is received
|
||||
// 0. fetch the symbols into an array
|
||||
if (corr_value > 0) //normal PLL lock
|
||||
@ -620,7 +621,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
|
||||
{
|
||||
switch (d_frame_type)
|
||||
{
|
||||
case 1: //INAV
|
||||
case 1: // INAV
|
||||
{
|
||||
if (d_inav_nav.flag_TOW_set == true)
|
||||
{
|
||||
@ -647,7 +648,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2: //FNAV
|
||||
case 2: // FNAV
|
||||
{
|
||||
if (d_fnav_nav.flag_TOW_set == true)
|
||||
{
|
||||
@ -692,7 +693,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
|
||||
{
|
||||
switch (d_frame_type)
|
||||
{
|
||||
case 1: //INAV
|
||||
case 1: // INAV
|
||||
{
|
||||
if (d_inav_nav.flag_TOW_set == true)
|
||||
{
|
||||
@ -700,7 +701,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2: //FNAV
|
||||
case 2: // FNAV
|
||||
{
|
||||
if (d_fnav_nav.flag_TOW_set == true)
|
||||
{
|
||||
@ -720,7 +721,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
|
||||
|
||||
switch (d_frame_type)
|
||||
{
|
||||
case 1: //INAV
|
||||
case 1: // INAV
|
||||
{
|
||||
if (d_inav_nav.flag_TOW_set)
|
||||
{
|
||||
@ -734,7 +735,7 @@ int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((
|
||||
break;
|
||||
}
|
||||
|
||||
case 2: //FNAV
|
||||
case 2: // FNAV
|
||||
{
|
||||
if (d_fnav_nav.flag_TOW_set)
|
||||
{
|
||||
|
||||
@ -29,8 +29,8 @@
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_galileo_telemetry_decoder_cc_H
|
||||
#define GNSS_SDR_galileo_telemetry_decoder_cc_H
|
||||
#ifndef GNSS_SDR_GALILEO_TELEMETRY_DECODER_CC_H
|
||||
#define GNSS_SDR_GALILEO_TELEMETRY_DECODER_CC_H
|
||||
|
||||
|
||||
#include "Galileo_E1.h"
|
||||
@ -56,7 +56,6 @@ galileo_telemetry_decoder_cc_sptr galileo_make_telemetry_decoder_cc(const Gnss_S
|
||||
|
||||
/*!
|
||||
* \brief This class implements a block that decodes the INAV and FNAV data defined in Galileo ICD
|
||||
*
|
||||
*/
|
||||
class galileo_telemetry_decoder_cc : public gr::block
|
||||
{
|
||||
|
||||
@ -28,6 +28,7 @@ include_directories(
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
${MATIO_INCLUDE_DIRS}
|
||||
${ARMADILLO_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
|
||||
@ -37,5 +38,7 @@ add_library(system_testing_lib ${SYSTEM_TESTING_LIB_SOURCES} ${SYSTEM_TESTING_LI
|
||||
source_group(Headers FILES ${SYSTEM_TESTING_LIB_HEADERS})
|
||||
|
||||
if(NOT MATIO_FOUND)
|
||||
add_dependencies(system_testing_lib matio-${GNSSSDR_MATIO_LOCAL_VERSION})
|
||||
add_dependencies(system_testing_lib armadillo-${armadillo_RELEASE} matio-${GNSSSDR_MATIO_LOCAL_VERSION})
|
||||
else(NOT MATIO_FOUND)
|
||||
add_dependencies(system_testing_lib armadillo-${armadillo_RELEASE})
|
||||
endif(NOT MATIO_FOUND)
|
||||
@ -1,5 +1,5 @@
|
||||
/*!
|
||||
* \file geofunctions.h
|
||||
* \file geofunctions.cc
|
||||
* \brief A set of coordinate transformations functions and helpers,
|
||||
* some of them migrated from MATLAB, for geographic information systems.
|
||||
* \author Javier Arribas, 2018. jarribas(at)cttc.es
|
||||
@ -30,10 +30,10 @@
|
||||
*/
|
||||
#include "geofunctions.h"
|
||||
|
||||
#define STRP_G_SI 9.80665
|
||||
#define STRP_PI 3.1415926535898 //!< Pi as defined in IS-GPS-200E
|
||||
const double STRP_G_SI = 9.80665;
|
||||
const double STRP_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E
|
||||
|
||||
arma::mat Skew_symmetric(arma::vec a)
|
||||
arma::mat Skew_symmetric(const arma::vec &a)
|
||||
{
|
||||
arma::mat A = arma::zeros(3, 3);
|
||||
|
||||
@ -41,54 +41,55 @@ arma::mat Skew_symmetric(arma::vec a)
|
||||
<< a(2) << 0.0 << -a(0) << arma::endr
|
||||
<< -a(1) << a(0) << 0 << arma::endr;
|
||||
|
||||
// {{0, -a(2), a(1)},
|
||||
// {a(2), 0, -a(0)},
|
||||
// {-a(1), a(0), 0}};
|
||||
// {{0, -a(2), a(1)},
|
||||
// {a(2), 0, -a(0)},
|
||||
// {-a(1), a(0), 0}};
|
||||
return A;
|
||||
}
|
||||
|
||||
|
||||
double WGS84_g0(double Lat_rad)
|
||||
{
|
||||
double k = 0.001931853; //normal gravity constant
|
||||
double e2 = 0.00669438002290; //the square of the first numerical eccentricity
|
||||
double nge = 9.7803253359; //normal gravity value on the equator (m/sec^2)
|
||||
double b = sin(Lat_rad); //Lat in degrees
|
||||
const double k = 0.001931853; // normal gravity constant
|
||||
const double e2 = 0.00669438002290; // the square of the first numerical eccentricity
|
||||
const double nge = 9.7803253359; // normal gravity value on the equator (m/sec^2)
|
||||
double b = sin(Lat_rad); // Lat in degrees
|
||||
b = b * b;
|
||||
double g0 = nge * (1 + k * b) / (sqrt(1 - e2 * b));
|
||||
return g0;
|
||||
}
|
||||
|
||||
|
||||
double WGS84_geocentric_radius(double Lat_geodetic_rad)
|
||||
{
|
||||
//WGS84 earth model Geocentric radius (Eq. 2.88)
|
||||
// WGS84 earth model Geocentric radius (Eq. 2.88)
|
||||
const double WGS84_A = 6378137.0; // Semi-major axis of the Earth, a [m]
|
||||
const double WGS84_IF = 298.257223563; // Inverse flattening of the Earth
|
||||
const double WGS84_F = (1.0 / WGS84_IF); // The flattening of the Earth
|
||||
// double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m]
|
||||
double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); // Eccentricity of the Earth
|
||||
|
||||
double WGS84_A = 6378137.0; //Semi-major axis of the Earth, a [m]
|
||||
double WGS84_IF = 298.257223563; //Inverse flattening of the Earth
|
||||
double WGS84_F = (1 / WGS84_IF); //The flattening of the Earth
|
||||
//double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m]
|
||||
double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); //Eccentricity of the Earth
|
||||
|
||||
//transverse radius of curvature
|
||||
// transverse radius of curvature
|
||||
double R_E = WGS84_A / sqrt(1 -
|
||||
WGS84_E * WGS84_E *
|
||||
sin(Lat_geodetic_rad) *
|
||||
sin(Lat_geodetic_rad)); // (Eq. 2.66)
|
||||
|
||||
//gocentric radius at the Earth surface
|
||||
// geocentric radius at the Earth surface
|
||||
double r_eS = R_E * sqrt(cos(Lat_geodetic_rad) * cos(Lat_geodetic_rad) +
|
||||
(1 - WGS84_E * WGS84_E) * (1 - WGS84_E * WGS84_E) * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.88)
|
||||
return r_eS;
|
||||
}
|
||||
|
||||
|
||||
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
|
||||
{
|
||||
double lambda;
|
||||
double phi;
|
||||
double h;
|
||||
double dtr = STRP_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
|
||||
const double dtr = STRP_PI / 180.0;
|
||||
const double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
|
||||
const double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
|
||||
|
||||
// Transform x into geodetic coordinates
|
||||
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
|
||||
@ -147,9 +148,9 @@ int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::
|
||||
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
|
||||
{
|
||||
*h = 0;
|
||||
double tolsq = 1.e-10; // tolerance to accept convergence
|
||||
int maxit = 10; // max number of iterations
|
||||
double rtd = 180.0 / STRP_PI;
|
||||
const double tolsq = 1.e-10; // tolerance to accept convergence
|
||||
const int maxit = 10; // max number of iterations
|
||||
const double rtd = 180.0 / STRP_PI;
|
||||
|
||||
// compute square of eccentricity
|
||||
double esq;
|
||||
@ -165,7 +166,7 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub
|
||||
// first guess
|
||||
double P = sqrt(X * X + Y * Y); // P is distance from spin axis
|
||||
|
||||
//direct calculation of longitude
|
||||
// direct calculation of longitude
|
||||
if (P > 1.0E-20)
|
||||
{
|
||||
*dlambda = atan2(Y, X) * rtd;
|
||||
@ -241,27 +242,28 @@ int togeod(double *dphi, double *dlambda, double *h, double a, double finv, doub
|
||||
return 0;
|
||||
}
|
||||
|
||||
arma::mat Gravity_ECEF(arma::vec r_eb_e)
|
||||
|
||||
arma::mat Gravity_ECEF(const arma::vec &r_eb_e)
|
||||
{
|
||||
//Parameters
|
||||
double R_0 = 6378137; //WGS84 Equatorial radius in meters
|
||||
double mu = 3.986004418E14; //WGS84 Earth gravitational constant (m^3 s^-2)
|
||||
double J_2 = 1.082627E-3; //WGS84 Earth's second gravitational constant
|
||||
double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
|
||||
// Parameters
|
||||
const double R_0 = 6378137; // WGS84 Equatorial radius in meters
|
||||
const double mu = 3.986004418E14; // WGS84 Earth gravitational constant (m^3 s^-2)
|
||||
const double J_2 = 1.082627E-3; // WGS84 Earth's second gravitational constant
|
||||
const double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
|
||||
// Calculate distance from center of the Earth
|
||||
double mag_r = sqrt(arma::as_scalar(r_eb_e.t() * r_eb_e));
|
||||
// If the input position is 0,0,0, produce a dummy output
|
||||
arma::vec g = arma::zeros(3, 1);
|
||||
if (mag_r != 0)
|
||||
{
|
||||
//Calculate gravitational acceleration using (2.142)
|
||||
// Calculate gravitational acceleration using (2.142)
|
||||
double z_scale = 5 * pow((r_eb_e(2) / mag_r), 2);
|
||||
arma::vec tmp_vec = {(1 - z_scale) * r_eb_e(0),
|
||||
(1 - z_scale) * r_eb_e(1),
|
||||
(3 - z_scale) * r_eb_e(2)};
|
||||
arma::vec gamma_ = (-mu / pow(mag_r, 3)) * (r_eb_e + 1.5 * J_2 * pow(R_0 / mag_r, 2) * tmp_vec);
|
||||
|
||||
//Add centripetal acceleration using (2.133)
|
||||
// Add centripetal acceleration using (2.133)
|
||||
g(0) = gamma_(0) + pow(omega_ie, 2) * r_eb_e(0);
|
||||
g(1) = gamma_(1) + pow(omega_ie, 2) * r_eb_e(1);
|
||||
g(2) = gamma_(2);
|
||||
@ -269,20 +271,23 @@ arma::mat Gravity_ECEF(arma::vec r_eb_e)
|
||||
return g;
|
||||
}
|
||||
|
||||
arma::vec LLH_to_deg(arma::vec LLH)
|
||||
|
||||
arma::vec LLH_to_deg(arma::vec &LLH)
|
||||
{
|
||||
double rtd = 180.0 / STRP_PI;
|
||||
const double rtd = 180.0 / STRP_PI;
|
||||
LLH(0) = LLH(0) * rtd;
|
||||
LLH(1) = LLH(1) * rtd;
|
||||
return LLH;
|
||||
}
|
||||
|
||||
|
||||
double degtorad(double angleInDegrees)
|
||||
{
|
||||
double angleInRadians = (STRP_PI / 180.0) * angleInDegrees;
|
||||
return angleInRadians;
|
||||
}
|
||||
|
||||
|
||||
double radtodeg(double angleInRadians)
|
||||
{
|
||||
double angleInDegrees = (180.0 / STRP_PI) * angleInRadians;
|
||||
@ -296,15 +301,17 @@ double mstoknotsh(double MetersPerSeconds)
|
||||
return knots;
|
||||
}
|
||||
|
||||
|
||||
double mstokph(double MetersPerSeconds)
|
||||
{
|
||||
double kph = 3600.0 * MetersPerSeconds / 1e3;
|
||||
return kph;
|
||||
}
|
||||
|
||||
arma::vec CTM_to_Euler(arma::mat C)
|
||||
|
||||
arma::vec CTM_to_Euler(arma::mat &C)
|
||||
{
|
||||
//Calculate Euler angles using (2.23)
|
||||
// Calculate Euler angles using (2.23)
|
||||
arma::vec eul = arma::zeros(3, 1);
|
||||
eul(0) = atan2(C(1, 2), C(2, 2)); // roll
|
||||
if (C(0, 2) < -1.0) C(0, 2) = -1.0;
|
||||
@ -314,18 +321,19 @@ arma::vec CTM_to_Euler(arma::mat C)
|
||||
return eul;
|
||||
}
|
||||
|
||||
arma::mat Euler_to_CTM(arma::vec eul)
|
||||
|
||||
arma::mat Euler_to_CTM(const arma::vec &eul)
|
||||
{
|
||||
//Eq.2.15
|
||||
//Euler angles to Attitude matrix is equivalent to rotate the body
|
||||
//in the three axes:
|
||||
// Eq.2.15
|
||||
// Euler angles to Attitude matrix is equivalent to rotate the body
|
||||
// in the three axes:
|
||||
// arma::mat Ax= {{1,0,0}, {0,cos(Att_phi),sin(Att_phi)} ,{0,-sin(Att_phi),cos(Att_phi)}};
|
||||
// arma::mat Ay= {{cos(Att_theta), 0, -sin(Att_theta)}, {0,1,0} , {sin(Att_theta), 0, cos(Att_theta)}};
|
||||
// arma::mat Az= {{cos(Att_psi), sin(Att_psi), 0}, {-sin(Att_psi), cos(Att_psi), 0},{0,0,1}};
|
||||
// arma::mat C_b_n=Ax*Ay*Az; // Attitude expressed in the LOCAL FRAME (NED)
|
||||
// C_b_n=C_b_n.t();
|
||||
|
||||
//Precalculate sines and cosines of the Euler angles
|
||||
// Precalculate sines and cosines of the Euler angles
|
||||
double sin_phi = sin(eul(0));
|
||||
double cos_phi = cos(eul(0));
|
||||
double sin_theta = sin(eul(1));
|
||||
@ -334,7 +342,7 @@ arma::mat Euler_to_CTM(arma::vec eul)
|
||||
double cos_psi = cos(eul(2));
|
||||
|
||||
arma::mat C = arma::zeros(3, 3);
|
||||
//Calculate coordinate transformation matrix using (2.22)
|
||||
// Calculate coordinate transformation matrix using (2.22)
|
||||
C(0, 0) = cos_theta * cos_psi;
|
||||
C(0, 1) = cos_theta * sin_psi;
|
||||
C(0, 2) = -sin_theta;
|
||||
@ -347,7 +355,8 @@ arma::mat Euler_to_CTM(arma::vec eul)
|
||||
return C;
|
||||
}
|
||||
|
||||
arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection)
|
||||
|
||||
arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection)
|
||||
{
|
||||
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};
|
||||
@ -380,37 +389,37 @@ arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection)
|
||||
return LLH;
|
||||
}
|
||||
|
||||
void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n)
|
||||
|
||||
void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::mat &C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n)
|
||||
{
|
||||
//Compute the Latitude of the ECEF position
|
||||
LLH = cart2geo(r_eb_e, 4); //ECEF -> WGS84 geographical
|
||||
// Compute the Latitude of the ECEF position
|
||||
LLH = cart2geo(r_eb_e, 4); // ECEF -> WGS84 geographical
|
||||
|
||||
// Calculate ECEF to Geographical coordinate transformation matrix using (2.150)
|
||||
double cos_lat = cos(LLH(0));
|
||||
double sin_lat = sin(LLH(0));
|
||||
double cos_long = cos(LLH(1));
|
||||
double sin_long = sin(LLH(1));
|
||||
//C++11 and arma >= 5.2
|
||||
// C++11 and arma >= 5.2
|
||||
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
|
||||
// {-sin_long, cos_long, 0},
|
||||
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; //ECEF to Geo
|
||||
|
||||
//C++98 arma <5.2
|
||||
arma::mat C_e_n = arma::zeros(3, 3);
|
||||
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
|
||||
<< -sin_long << cos_long << 0 << arma::endr
|
||||
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; //ECEF to Geo
|
||||
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; // ECEF to Geo
|
||||
// Transform velocity using (2.73)
|
||||
v_eb_n = C_e_n * v_eb_e;
|
||||
|
||||
C_b_n = C_e_n * C_b_e; // Attitude conversion from ECEF to NED
|
||||
}
|
||||
|
||||
void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e)
|
||||
|
||||
void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e)
|
||||
{
|
||||
// Parameters
|
||||
double R_0 = 6378137; //WGS84 Equatorial radius in meters
|
||||
double e = 0.0818191908425; //WGS84 eccentricity
|
||||
double R_0 = 6378137; // WGS84 Equatorial radius in meters
|
||||
double e = 0.0818191908425; // WGS84 eccentricity
|
||||
|
||||
// Calculate transverse radius of curvature using (2.105)
|
||||
double R_E = R_0 / sqrt(1 - (e * sin(LLH(0))) * (e * sin(LLH(0))));
|
||||
@ -424,13 +433,11 @@ void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_
|
||||
(R_E + LLH(2)) * cos_lat * sin_long,
|
||||
((1 - e * e) * R_E + LLH(2)) * sin_lat};
|
||||
|
||||
//Calculate ECEF to Geo coordinate transformation matrix using (2.150)
|
||||
//C++11 and arma>=5.2
|
||||
// Calculate ECEF to Geo coordinate transformation matrix using (2.150)
|
||||
// C++11 and arma>=5.2
|
||||
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
|
||||
// {-sin_long, cos_long, 0},
|
||||
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}};
|
||||
//C++98 arma <5.2
|
||||
//Calculate ECEF to Geo coordinate transformation matrix using (2.150)
|
||||
arma::mat C_e_n = arma::zeros(3, 3);
|
||||
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
|
||||
<< -sin_long << cos_long << 0 << arma::endr
|
||||
@ -444,11 +451,11 @@ void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_
|
||||
}
|
||||
|
||||
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e)
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e)
|
||||
{
|
||||
//% Parameters
|
||||
double R_0 = 6378137; //WGS84 Equatorial radius in meters
|
||||
double e = 0.0818191908425; //WGS84 eccentricity
|
||||
// Parameters
|
||||
const double R_0 = 6378137; // WGS84 Equatorial radius in meters
|
||||
const double e = 0.0818191908425; // WGS84 eccentricity
|
||||
|
||||
// Calculate transverse radius of curvature using (2.105)
|
||||
double R_E = R_0 / sqrt(1 - pow(e * sin(L_b), 2));
|
||||
|
||||
@ -29,72 +29,72 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GEOFUNCTIONS_H
|
||||
#define GEOFUNCTIONS_H
|
||||
#ifndef GNSS_SDR_GEOFUNCTIONS_H
|
||||
#define GNSS_SDR_GEOFUNCTIONS_H
|
||||
|
||||
#include <armadillo>
|
||||
// %Skew_symmetric - Calculates skew-symmetric matrix
|
||||
arma::mat Skew_symmetric(arma::vec a);
|
||||
|
||||
arma::mat Skew_symmetric(const arma::vec &a); //!< Calculates skew-symmetric matrix
|
||||
|
||||
double WGS84_g0(double Lat_rad);
|
||||
|
||||
double WGS84_geocentric_radius(double Lat_geodetic_rad);
|
||||
|
||||
/* 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
|
||||
/*!
|
||||
* \brief 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
|
||||
*/
|
||||
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx);
|
||||
|
||||
/* 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
|
||||
/*!
|
||||
* \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.
|
||||
*
|
||||
* 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
|
||||
*/
|
||||
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
|
||||
|
||||
arma::mat Gravity_ECEF(const arma::vec &r_eb_e); //!< Calculates acceleration due to gravity resolved about ECEF-frame
|
||||
|
||||
//Gravitation_ECI - Calculates acceleration due to gravity resolved about
|
||||
//ECEF-frame
|
||||
arma::mat Gravity_ECEF(arma::vec r_eb_e);
|
||||
|
||||
/* 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
|
||||
/*!
|
||||
* \brief 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
|
||||
*/
|
||||
arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection);
|
||||
arma::vec cart2geo(const arma::vec &XYZ, int elipsoid_selection);
|
||||
|
||||
arma::vec LLH_to_deg(arma::vec LLH);
|
||||
arma::vec LLH_to_deg(arma::vec &LLH);
|
||||
|
||||
double degtorad(double angleInDegrees);
|
||||
|
||||
@ -104,53 +104,51 @@ double mstoknotsh(double MetersPerSeconds);
|
||||
|
||||
double mstokph(double Kph);
|
||||
|
||||
arma::vec CTM_to_Euler(arma::mat &C);
|
||||
|
||||
arma::vec CTM_to_Euler(arma::mat C);
|
||||
arma::mat Euler_to_CTM(const arma::vec &eul);
|
||||
|
||||
arma::mat Euler_to_CTM(arma::vec eul);
|
||||
|
||||
void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n);
|
||||
void ECEF_to_Geo(const arma::vec &r_eb_e, const arma::vec &v_eb_e, const arma::mat &C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n);
|
||||
|
||||
|
||||
// %
|
||||
// % Inputs:
|
||||
// % L_b latitude (rad)
|
||||
// % lambda_b longitude (rad)
|
||||
// % h_b height (m)
|
||||
// % v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
// % north, east, and down (m/s)
|
||||
// % C_b_n body-to-NED coordinate transformation matrix
|
||||
// %
|
||||
// % Outputs:
|
||||
// % r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
|
||||
// % along ECEF-frame axes (m)
|
||||
// % v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
// % ECEF-frame axes (m/s)
|
||||
// % C_b_e body-to-ECEF-frame coordinate transformation matrix
|
||||
//
|
||||
// % Copyright 2012, Paul Groves
|
||||
// % License: BSD; see license.txt for details
|
||||
|
||||
void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e);
|
||||
/*!
|
||||
* \brief From Geographic to ECEF coordinates
|
||||
*
|
||||
* Inputs:
|
||||
* LLH latitude (rad), longitude (rad), height (m)
|
||||
* v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
* north, east, and down (m/s)
|
||||
* C_b_n body-to-NED coordinate transformation matrix
|
||||
*
|
||||
* Outputs:
|
||||
* r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
|
||||
* along ECEF-frame axes (m)
|
||||
* v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
* ECEF-frame axes (m/s)
|
||||
* C_b_e body-to-ECEF-frame coordinate transformation matrix
|
||||
*
|
||||
*/
|
||||
void Geo_to_ECEF(const arma::vec &LLH, const arma::vec &v_eb_n, const arma::mat &C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e);
|
||||
|
||||
|
||||
//pv_Geo_to_ECEF - Converts curvilinear to Cartesian position and velocity
|
||||
//resolving axes from NED to ECEF
|
||||
//This function created 11/4/2012 by Paul Groves
|
||||
//%
|
||||
//% Inputs:
|
||||
//% L_b latitude (rad)
|
||||
//% lambda_b longitude (rad)
|
||||
//% h_b height (m)
|
||||
//% v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
//% north, east, and down (m/s)
|
||||
//%
|
||||
//% Outputs:
|
||||
//% r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
|
||||
//% along ECEF-frame axes (m)
|
||||
//% v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
//% ECEF-frame axes (m/s)
|
||||
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e);
|
||||
/*!
|
||||
* \brief Converts curvilinear to Cartesian position and velocity
|
||||
* resolving axes from NED to ECEF
|
||||
* This function created 11/4/2012 by Paul Groves
|
||||
*
|
||||
* Inputs:
|
||||
* L_b latitude (rad)
|
||||
* lambda_b longitude (rad)
|
||||
* h_b height (m)
|
||||
* v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
* north, east, and down (m/s)
|
||||
*
|
||||
* Outputs:
|
||||
* r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
|
||||
* along ECEF-frame axes (m)
|
||||
* v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
|
||||
* ECEF-frame axes (m/s)
|
||||
*/
|
||||
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, const arma::vec &v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e);
|
||||
|
||||
#endif
|
||||
|
||||
@ -29,7 +29,6 @@
|
||||
*/
|
||||
|
||||
#include "rtklib_solver_dump_reader.h"
|
||||
|
||||
#include <iostream>
|
||||
|
||||
bool rtklib_solver_dump_reader::read_binary_obs()
|
||||
@ -37,46 +36,20 @@ bool rtklib_solver_dump_reader::read_binary_obs()
|
||||
try
|
||||
{
|
||||
d_dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_ms), sizeof(TOW_at_current_symbol_ms));
|
||||
// std::cout << "TOW_at_current_symbol_ms: " << TOW_at_current_symbol_ms << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&week), sizeof(week));
|
||||
// std::cout << "week: " << week << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&RX_time), sizeof(RX_time));
|
||||
// std::cout << "RX_time: " << RX_time << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&clk_offset_s), sizeof(clk_offset_s));
|
||||
// std::cout << "clk_offset_s: " << clk_offset_s << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&rr[0]), sizeof(rr));
|
||||
// for (int n = 0; n < 6; n++)
|
||||
// {
|
||||
// std::cout << "rr: " << rr[n] << std::endl;
|
||||
// }
|
||||
d_dump_file.read(reinterpret_cast<char *>(&qr[0]), sizeof(qr));
|
||||
// for (int n = 0; n < 6; n++)
|
||||
// {
|
||||
// std::cout << "qr" << qr[n] << std::endl;
|
||||
// }
|
||||
d_dump_file.read(reinterpret_cast<char *>(&latitude), sizeof(latitude));
|
||||
//std::cout << "latitude: " << latitude << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&longitude), sizeof(longitude));
|
||||
//std::cout << "longitude: " << longitude << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&height), sizeof(height));
|
||||
//std::cout << "height: " << height << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&ns), sizeof(ns));
|
||||
// std::cout << "ns: " << (int)ns << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&status), sizeof(status));
|
||||
// std::cout << "status: " << (int)status << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&type), sizeof(type));
|
||||
// std::cout << "type: " << (int)type << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&AR_ratio), sizeof(AR_ratio));
|
||||
// std::cout << "AR_ratio: " << AR_ratio << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&AR_thres), sizeof(AR_thres));
|
||||
// std::cout << "AR_thres: " << AR_thres << std::endl;
|
||||
d_dump_file.read(reinterpret_cast<char *>(&dop[0]), sizeof(dop));
|
||||
|
||||
// for (int n = 0; n < 4; n++)
|
||||
// {
|
||||
// std::cout << "dop" << dop[n] << std::endl;
|
||||
// }
|
||||
// getchar();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
|
||||
@ -64,6 +64,7 @@ bool spirent_motion_csv_dump_reader::read_csv_obs()
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool spirent_motion_csv_dump_reader::parse_vector(std::vector<double> &vec)
|
||||
{
|
||||
try
|
||||
@ -114,6 +115,8 @@ bool spirent_motion_csv_dump_reader::parse_vector(std::vector<double> &vec)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool spirent_motion_csv_dump_reader::restart()
|
||||
{
|
||||
if (d_dump_file.is_open())
|
||||
@ -136,7 +139,7 @@ bool spirent_motion_csv_dump_reader::restart()
|
||||
|
||||
int64_t spirent_motion_csv_dump_reader::num_epochs()
|
||||
{
|
||||
int64_t nepoch = 0;
|
||||
int64_t nepoch = 0LL;
|
||||
std::string line;
|
||||
std::ifstream tmpfile(d_dump_filename.c_str());
|
||||
if (tmpfile.is_open())
|
||||
@ -182,6 +185,7 @@ bool spirent_motion_csv_dump_reader::open_obs_file(std::string out_file)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void spirent_motion_csv_dump_reader::close_obs_file()
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
@ -190,6 +194,7 @@ void spirent_motion_csv_dump_reader::close_obs_file()
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
spirent_motion_csv_dump_reader::spirent_motion_csv_dump_reader()
|
||||
{
|
||||
header_lines = 2;
|
||||
|
||||
@ -28,8 +28,8 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_spirent_motion_csv_dump_READER_H
|
||||
#define GNSS_SDR_spirent_motion_csv_dump_READER_H
|
||||
#ifndef GNSS_SDR_SPIRENT_MOTION_CSV_DUMP_READER_H
|
||||
#define GNSS_SDR_SPIRENT_MOTION_CSV_DUMP_READER_H
|
||||
|
||||
#include <boost/tokenizer.hpp>
|
||||
#include <cstdint>
|
||||
@ -49,7 +49,7 @@ public:
|
||||
void close_obs_file();
|
||||
|
||||
int header_lines;
|
||||
//dump variables
|
||||
// dump variables
|
||||
double TOW_ms;
|
||||
double Pos_X;
|
||||
double Pos_Y;
|
||||
@ -95,4 +95,4 @@ private:
|
||||
bool parse_vector(std::vector<double> &vec);
|
||||
};
|
||||
|
||||
#endif //GNSS_SDR_spirent_motion_csv_dump_READER_H
|
||||
#endif // GNSS_SDR_SPIRENT_MOTION_CSV_DUMP_READER_H
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user