mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-18 21:23:02 +00:00
Code Issues Releases Wiki Activity

Sort constants

Browse Source 
no more GPS_L1_PI, GPS_L2_PI, etc. Values are written in a single point.
This commit is contained in:
Carles Fernandez 2020-07-05 20:20:02 +02:00
parent 6395eecf95
commit d3b7557dcf
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
79 changed files with 629 additions and 697 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc
View File

@ -1825,42 +1825,42 @@ void rtklib_pvt_gs::apply_rx_clock_offset(std::map<int, Gnss_Synchro>& observabl
{
// all observables in the map are valid
observables_iter->second.RX_time -= rx_clock_offset_s;
observables_iter->second.Pseudorange_m -= rx_clock_offset_s * SPEED_OF_LIGHT;
observables_iter->second.Pseudorange_m -= rx_clock_offset_s * SPEED_OF_LIGHT_M_S;
switch (d_mapStringValues[observables_iter->second.Signal])
{
case evGPS_1C:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1 * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1 * TWO_PI;
break;
case evGPS_L5:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ5 * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ5 * TWO_PI;
break;
case evSBAS_1C:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1 * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1 * TWO_PI;
break;
case evGAL_1B:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1 * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1 * TWO_PI;
break;
case evGAL_5X:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ5 * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ5 * TWO_PI;
break;
case evGPS_2S:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2 * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2 * TWO_PI;
break;
case evBDS_B3:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ3_BDS * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ3_BDS * TWO_PI;
break;
case evGLO_1G:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1_GLO * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1_GLO * TWO_PI;
break;
case evGLO_2G:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2_GLO * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2_GLO * TWO_PI;
break;
case evBDS_B1:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1_BDS * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ1_BDS * TWO_PI;
break;
case evBDS_B2:
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2_BDS * PI_2;
observables_iter->second.Carrier_phase_rads -= rx_clock_offset_s * FREQ2_BDS * TWO_PI;
break;
default:
break;
@ -1932,43 +1932,43 @@ void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset()
switch (d_mapStringValues[observables_iter->second.Signal])
{
case evGPS_1C:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1;
break;
case evGPS_L5:
wavelength_m = SPEED_OF_LIGHT / FREQ5;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ5;
break;
case evSBAS_1C:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1;
break;
case evGAL_1B:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1;
break;
case evGAL_5X:
wavelength_m = SPEED_OF_LIGHT / FREQ5;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ5;
break;
case evGPS_2S:
wavelength_m = SPEED_OF_LIGHT / FREQ2;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2;
break;
case evBDS_B3:
wavelength_m = SPEED_OF_LIGHT / FREQ3_BDS;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ3_BDS;
break;
case evGLO_1G:
wavelength_m = SPEED_OF_LIGHT / FREQ1_GLO;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_GLO;
break;
case evGLO_2G:
wavelength_m = SPEED_OF_LIGHT / FREQ2_GLO;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_GLO;
break;
case evBDS_B1:
wavelength_m = SPEED_OF_LIGHT / FREQ1_BDS;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_BDS;
break;
case evBDS_B2:
wavelength_m = SPEED_OF_LIGHT / FREQ2_BDS;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_BDS;
break;
default:
break;
}
double wrap_carrier_phase_rad = fmod(observables_iter->second.Carrier_phase_rads, PI_2);
d_initial_carrier_phase_offset_estimation_rads.at(observables_iter->second.Channel_ID) = PI_2 * round(observables_iter->second.Pseudorange_m / wavelength_m) - observables_iter->second.Carrier_phase_rads + wrap_carrier_phase_rad;
double wrap_carrier_phase_rad = fmod(observables_iter->second.Carrier_phase_rads, TWO_PI);
d_initial_carrier_phase_offset_estimation_rads.at(observables_iter->second.Channel_ID) = TWO_PI * round(observables_iter->second.Pseudorange_m / wavelength_m) - observables_iter->second.Carrier_phase_rads + wrap_carrier_phase_rad;
d_channel_initialized.at(observables_iter->second.Channel_ID) = true;
DLOG(INFO) << "initialized carrier phase at channel " << observables_iter->second.Channel_ID;
}

20
src/algorithms/PVT/libs/hybrid_ls_pvt.cc
View File

@ -119,7 +119,7 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// COMMON RX TIME PVT ALGORITHM
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GALILEO_C_M_S;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time);
@ -136,7 +136,7 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_M_S - this->get_time_offset_s() * GALILEO_C_M_S;
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * SPEED_OF_LIGHT_M_S - this->get_time_offset_s() * SPEED_OF_LIGHT_M_S;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; // for GST
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
@ -174,7 +174,7 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_M_S;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S;
// 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
@ -194,10 +194,10 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// See IS-GPS-200K section 20.3.3.3.3.2
double sqrt_Gamma = GPS_L1_FREQ_HZ / GPS_L2_FREQ_HZ;
double Gamma = sqrt_Gamma * sqrt_Gamma;
double P1_P2 = (1.0 - Gamma) * (gps_ephemeris_iter->second.d_TGD * GPS_C_M_S);
double P1_P2 = (1.0 - Gamma) * (gps_ephemeris_iter->second.d_TGD * SPEED_OF_LIGHT_M_S);
double Code_bias_m = P1_P2 / (1.0 - Gamma);
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_M_S - Code_bias_m - this->get_time_offset_s() * GPS_C_M_S;
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * SPEED_OF_LIGHT_M_S - Code_bias_m - this->get_time_offset_s() * SPEED_OF_LIGHT_M_S;
// SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
@ -229,7 +229,7 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time
double Rx_time = hybrid_current_time;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / GPS_C_M_S;
double Tx_time = Rx_time - gnss_observables_iter->second.Pseudorange_m / SPEED_OF_LIGHT_M_S;
// 2- compute the clock drift using the clock model (broadcast) for this SV
SV_clock_bias_s = gps_cnav_ephemeris_iter->second.sv_clock_drift(Tx_time);
@ -247,7 +247,7 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
// 4- fill the observations vector with the corrected observables
obs.resize(valid_obs + 1, 1);
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_M_S + SV_clock_bias_s * GPS_C_M_S;
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * SPEED_OF_LIGHT_M_S + SV_clock_bias_s * SPEED_OF_LIGHT_M_S;
GPS_week = gps_cnav_ephemeris_iter->second.i_GPS_week;
GPS_week = GPS_week % 1024; // Necessary due to the increase of WN bits in CNAV message (10 in GPS NAV and 13 in CNAV)
@ -297,17 +297,17 @@ bool Hybrid_Ls_Pvt::get_PVT(std::map<int, Gnss_Synchro> gnss_observables_map, do
DLOG(INFO) << " Executing Bancroft algorithm...";
rx_position_and_time = bancroftPos(satpos.t(), obs);
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
this->set_time_offset_s(rx_position_and_time(3) / GPS_C_M_S); // save time for the next iteration [meters]->[seconds]
this->set_time_offset_s(rx_position_and_time(3) / SPEED_OF_LIGHT_M_S); // save time for the next iteration [meters]->[seconds]
}
// Execute WLS using previous position as the initialization point
rx_position_and_time = leastSquarePos(satpos, obs, W);
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
this->set_time_offset_s(this->get_time_offset_s() + rx_position_and_time(3) / GPS_C_M_S); // accumulate the rx time error for the next iteration [meters]->[seconds]
this->set_time_offset_s(this->get_time_offset_s() + rx_position_and_time(3) / SPEED_OF_LIGHT_M_S); // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "Hybrid Position at TOW=" << hybrid_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
DLOG(INFO) << "Accumulated rx clock error=" << this->get_time_offset_s() << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_M_S << " [s]";
DLOG(INFO) << "Accumulated rx clock error=" << this->get_time_offset_s() << " clock error for this iteration=" << rx_position_and_time(3) / SPEED_OF_LIGHT_M_S << " [s]";
// Compute GST and Gregorian time
if (GST != 0.0)

10
src/algorithms/PVT/libs/ls_pvt.cc
View File

@ -75,7 +75,7 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
{
int z = B(i, 2);
double rho = (x - pos(0)) * (x - pos(0)) + (y - pos(1)) * (y - pos(1)) + (z - pos(2)) * (z - pos(2));
traveltime = sqrt(rho) / GPS_C_M_S;
traveltime = sqrt(rho) / SPEED_OF_LIGHT_M_S;
}
double angle = traveltime * 7.292115147e-5;
double cosa = cos(angle);
@ -208,7 +208,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
(X(1, i) - pos(1)) +
(X(2, i) - pos(2)) *
(X(2, i) - pos(2));
traveltime = sqrt(rho2) / GPS_C_M_S;
traveltime = sqrt(rho2) / SPEED_OF_LIGHT_M_S;
// --- Correct satellite position (do to earth rotation) -------
Rot_X = Ls_Pvt::rotateSatellite(traveltime, X.col(i)); // armadillo
@ -232,7 +232,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
else
{
// --- Find delay due to troposphere (in meters)
Ls_Pvt::tropo(&trop, sin(*elev * GPS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
Ls_Pvt::tropo(&trop, sin(*elev * GNSS_PI / 180.0), h / 1000.0, 1013.0, 293.0, 50.0, 0.0, 0.0, 0.0);
if (trop > 5.0)
{
trop = 0.0; // check for erratic values
@ -263,9 +263,9 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat& satpos, const arma::vec& obs,
}
// check the consistency of the PVT solution
if (((fabs(pos(3)) * 1000.0) / GPS_C_M_S) > GPS_STARTOFFSET_MS * 2)
if (((fabs(pos(3)) * 1000.0) / SPEED_OF_LIGHT_M_S) > GPS_STARTOFFSET_MS * 2)
{
LOG(WARNING) << "Receiver time offset out of range! Estimated RX Time error [s]:" << pos(3) / GPS_C_M_S;
LOG(WARNING) << "Receiver time offset out of range! Estimated RX Time error [s]:" << pos(3) / SPEED_OF_LIGHT_M_S;
throw std::runtime_error("Receiver time offset out of range!");
}
return pos;

6
src/algorithms/PVT/libs/pvt_solution.cc
View File

@ -63,7 +63,7 @@ arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec
// -- Find rotation angle --------------------------------------------------
double omegatau;
omegatau = OMEGA_EARTH_DOT * traveltime;
omegatau = GNSS_OMEGA_EARTH_DOT * traveltime;
// -- Build a rotation matrix ----------------------------------------------
arma::mat R3 = {{cos(omegatau), sin(omegatau), 0.0},
@ -116,8 +116,8 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection)
}
}
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_latitude_d = phi * 180.0 / GNSS_PI;
d_longitude_d = lambda * 180.0 / GNSS_PI;
d_height_m = h;
// todo: refactor this class. Mix of duplicated functions, use either RTKLIB geodetic functions or geofunctions.h
return 0;

40
src/algorithms/PVT/libs/rinex_printer.cc
View File

@ -8795,7 +8795,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
int32_t ssi = Rinex_Printer::signalStrength(observables_iter->second.CN0_dB_hz);
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GLONASS L1 CA PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GLONASS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -8901,7 +8901,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Glonass_Gnav_Ephemeri
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GLONASS L1 CA PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GLONASS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -9176,7 +9176,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GPS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -9247,7 +9247,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GLONASS CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GLONASS_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -9436,7 +9436,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GPS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -9505,7 +9505,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& g
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GLONASS CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GLONASS_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -9692,7 +9692,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GPS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -9758,7 +9758,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ga
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GLONASS CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GLONASS_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -9903,7 +9903,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
int32_t ssi = Rinex_Printer::signalStrength(observables_iter->second.CN0_dB_hz);
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GPS L1 CA PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GPS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -10010,7 +10010,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GPS L1 CA PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GPS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -10138,7 +10138,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& e
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GPS L2 PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GPS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -10374,7 +10374,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& eph, c
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// GPS CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -10620,7 +10620,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Galileo_Ephemeris& ep
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// Galileo CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -10826,7 +10826,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// PHASE
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / GPS_TWO_PI, 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(observables_iter->second.Carrier_phase_rads / TWO_PI, 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -10892,7 +10892,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// Galileo CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -11127,7 +11127,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& e
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -11189,7 +11189,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_CNAV_Ephemeris& e
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// Galileo CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -11453,7 +11453,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -11515,7 +11515,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Gps_Ephemeris& gps_ep
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// Galileo CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (GALILEO_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');
@ -11696,7 +11696,7 @@ void Rinex_Printer::log_rinex_obs(std::fstream& out, const Beidou_Dnav_Ephemeris
lineObs += Rinex_Printer::rightJustify(Rinex_Printer::asString<int32_t>(ssi), 1);
// CARRIER PHASE
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (BEIDOU_DNAV_TWO_PI), 3), 14);
lineObs += Rinex_Printer::rightJustify(asString(iter->second.Carrier_phase_rads / (TWO_PI), 3), 14);
if (lli == 0)
{
lineObs += std::string(1, ' ');

88
src/algorithms/PVT/libs/rtcm.cc
View File

@ -3808,11 +3808,11 @@ int32_t Rtcm::set_DF011(const Gnss_Synchro& gnss_synchro)
int32_t Rtcm::set_DF012(const Gnss_Synchro& gnss_synchro)
{
const double lambda = GPS_C_M_S / GPS_L1_FREQ_HZ;
const double lambda = SPEED_OF_LIGHT_M_S / GPS_L1_FREQ_HZ;
double ambiguity = std::floor(gnss_synchro.Pseudorange_m / 299792.458);
double gps_L1_pseudorange = std::round((gnss_synchro.Pseudorange_m - ambiguity * 299792.458) / 0.02);
double gps_L1_pseudorange_c = gps_L1_pseudorange * 0.02 + ambiguity * 299792.458;
double L1_phaserange_c = gnss_synchro.Carrier_phase_rads / GPS_TWO_PI;
double L1_phaserange_c = gnss_synchro.Carrier_phase_rads / TWO_PI;
double L1_phaserange_c_r = std::fmod(L1_phaserange_c - gps_L1_pseudorange_c / lambda + 1500.0, 3000.0) - 1500.0;
auto gps_L1_phaserange_minus_L1_pseudorange = static_cast<int64_t>(std::round(L1_phaserange_c_r * lambda / 0.0005));
DF012 = std::bitset<20>(gps_L1_phaserange_minus_L1_pseudorange);
@ -3870,12 +3870,12 @@ int32_t Rtcm::set_DF017(const Gnss_Synchro& gnss_synchroL1, const Gnss_Synchro&
int32_t Rtcm::set_DF018(const Gnss_Synchro& gnss_synchroL1, const Gnss_Synchro& gnss_synchroL2)
{
const double lambda2 = GPS_C_M_S / GPS_L2_FREQ_HZ;
const double lambda2 = SPEED_OF_LIGHT_M_S / GPS_L2_FREQ_HZ;
int32_t l2_phaserange_minus_l1_pseudorange = 0xFFF80000;
double ambiguity = std::floor(gnss_synchroL1.Pseudorange_m / 299792.458);
double gps_L1_pseudorange = std::round((gnss_synchroL1.Pseudorange_m - ambiguity * 299792.458) / 0.02);
double gps_L1_pseudorange_c = gps_L1_pseudorange * 0.02 + ambiguity * 299792.458;
double L2_phaserange_c = gnss_synchroL2.Carrier_phase_rads / GPS_TWO_PI;
double L2_phaserange_c = gnss_synchroL2.Carrier_phase_rads / TWO_PI;
double L1_phaserange_c_r = std::fmod(L2_phaserange_c - gps_L1_pseudorange_c / lambda2 + 1500.0, 3000.0) - 1500.0;
if (std::fabs(L1_phaserange_c_r * lambda2) <= 262.1435)
@ -4100,11 +4100,11 @@ int32_t Rtcm::set_DF041(const Gnss_Synchro& gnss_synchro)
int32_t Rtcm::set_DF042(const Gnss_Synchro& gnss_synchro)
{
const double lambda = GLONASS_C_M_S / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)));
const double lambda = SPEED_OF_LIGHT_M_S / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)));
double ambiguity = std::floor(gnss_synchro.Pseudorange_m / 599584.92);
double glonass_L1_pseudorange = std::round((gnss_synchro.Pseudorange_m - ambiguity * 599584.92) / 0.02);
double glonass_L1_pseudorange_c = glonass_L1_pseudorange * 0.02 + ambiguity * 299792.458;
double L1_phaserange_c = gnss_synchro.Carrier_phase_rads / GLONASS_TWO_PI;
double L1_phaserange_c = gnss_synchro.Carrier_phase_rads / TWO_PI;
double L1_phaserange_c_r = std::fmod(L1_phaserange_c - glonass_L1_pseudorange_c / lambda + 1500.0, 3000.0) - 1500.0;
auto glonass_L1_phaserange_minus_L1_pseudorange = static_cast<int64_t>(std::round(L1_phaserange_c_r * lambda / 0.0005));
DF042 = std::bitset<20>(glonass_L1_phaserange_minus_L1_pseudorange);
@ -4163,12 +4163,12 @@ int32_t Rtcm::set_DF047(const Gnss_Synchro& gnss_synchroL1, const Gnss_Synchro&
// TODO Need to consider frequency channel in this fields
int32_t Rtcm::set_DF048(const Gnss_Synchro& gnss_synchroL1, const Gnss_Synchro& gnss_synchroL2)
{
const double lambda2 = GLONASS_C_M_S / GLONASS_L2_CA_FREQ_HZ;
const double lambda2 = SPEED_OF_LIGHT_M_S / GLONASS_L2_CA_FREQ_HZ;
int32_t l2_phaserange_minus_l1_pseudorange = 0xFFF80000;
double ambiguity = std::floor(gnss_synchroL1.Pseudorange_m / 599584.92);
double glonass_L1_pseudorange = std::round((gnss_synchroL1.Pseudorange_m - ambiguity * 599584.92) / 0.02);
double glonass_L1_pseudorange_c = glonass_L1_pseudorange * 0.02 + ambiguity * 599584.92;
double L2_phaserange_c = gnss_synchroL2.Carrier_phase_rads / GLONASS_TWO_PI;
double L2_phaserange_c = gnss_synchroL2.Carrier_phase_rads / TWO_PI;
double L1_phaserange_c_r = std::fmod(L2_phaserange_c - glonass_L1_pseudorange_c / lambda2 + 1500.0, 3000.0) - 1500.0;
if (std::fabs(L1_phaserange_c_r * lambda2) <= 262.1435)
@ -5284,7 +5284,7 @@ std::string Rtcm::set_DF396(const std::map<int32_t, Gnss_Synchro>& observables)
int32_t Rtcm::set_DF397(const Gnss_Synchro& gnss_synchro)
{
double meters_to_miliseconds = GPS_C_M_S * 0.001;
double meters_to_miliseconds = SPEED_OF_LIGHT_M_S * 0.001;
double rough_range_s = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
uint32_t int_ms = 0;
@ -5309,7 +5309,7 @@ int32_t Rtcm::set_DF397(const Gnss_Synchro& gnss_synchro)
int32_t Rtcm::set_DF398(const Gnss_Synchro& gnss_synchro)
{
double meters_to_miliseconds = GPS_C_M_S * 0.001;
double meters_to_miliseconds = SPEED_OF_LIGHT_M_S * 0.001;
double rough_range_m = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
uint32_t rr_mod_ms;
if ((rough_range_m <= 0.0) || (rough_range_m > meters_to_miliseconds * 255.0))
@ -5333,23 +5333,23 @@ int32_t Rtcm::set_DF399(const Gnss_Synchro& gnss_synchro)
if (sig == "1C")
{
lambda = GPS_C_M_S / GPS_L1_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GPS_L1_FREQ_HZ;
}
if (sig == "2S")
{
lambda = GPS_C_M_S / GPS_L2_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GPS_L2_FREQ_HZ;
}
if (sig == "5X")
{
lambda = GPS_C_M_S / GALILEO_E5A_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GALILEO_E5A_FREQ_HZ;
}
if (sig == "1B")
{
lambda = GPS_C_M_S / GALILEO_E1_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GALILEO_E1_FREQ_HZ;
}
if (sig == "7X")
{
lambda = GPS_C_M_S / 1.207140e9; // Galileo_E1b_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / 1.207140e9; // Galileo_E1b_FREQ_HZ;
}
double rough_phase_range_rate_ms = std::round(-gnss_synchro.Carrier_Doppler_hz * lambda);
@ -5369,7 +5369,7 @@ int32_t Rtcm::set_DF399(const Gnss_Synchro& gnss_synchro)
int32_t Rtcm::set_DF400(const Gnss_Synchro& gnss_synchro)
{
double meters_to_miliseconds = GPS_C_M_S * 0.001;
double meters_to_miliseconds = SPEED_OF_LIGHT_M_S * 0.001;
double rough_range_m = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
double psrng_s;
int32_t fine_pseudorange;
@ -5396,7 +5396,7 @@ int32_t Rtcm::set_DF400(const Gnss_Synchro& gnss_synchro)
int32_t Rtcm::set_DF401(const Gnss_Synchro& gnss_synchro)
{
double meters_to_miliseconds = GPS_C_M_S * 0.001;
double meters_to_miliseconds = SPEED_OF_LIGHT_M_S * 0.001;
double rough_range_m = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
double phrng_m;
int64_t fine_phaserange;
@ -5408,39 +5408,39 @@ int32_t Rtcm::set_DF401(const Gnss_Synchro& gnss_synchro)
if ((sig == "1C") && (sys == "G"))
{
lambda = GPS_C_M_S / GPS_L1_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GPS_L1_FREQ_HZ;
}
if ((sig == "2S") && (sys == "G"))
{
lambda = GPS_C_M_S / GPS_L2_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GPS_L2_FREQ_HZ;
}
if ((sig == "5X") && (sys == "E"))
{
lambda = GPS_C_M_S / GALILEO_E5A_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GALILEO_E5A_FREQ_HZ;
}
if ((sig == "1B") && (sys == "E"))
{
lambda = GPS_C_M_S / GALILEO_E1_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GALILEO_E1_FREQ_HZ;
}
if ((sig == "7X") && (sys == "E"))
{
lambda = GPS_C_M_S / 1.207140e9; // Galileo_E1b_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / 1.207140e9; // Galileo_E1b_FREQ_HZ;
}
if ((sig == "1C") && (sys == "R"))
{
lambda = GLONASS_C_M_S / ((GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))));
lambda = SPEED_OF_LIGHT_M_S / ((GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN))));
}
if ((sig == "2C") && (sys == "R"))
{
// TODO Need to add slot number and freq number to gnss_syncro
lambda = GLONASS_C_M_S / (GLONASS_L2_CA_FREQ_HZ);
lambda = SPEED_OF_LIGHT_M_S / (GLONASS_L2_CA_FREQ_HZ);
}
phrng_m = (gnss_synchro.Carrier_phase_rads / GPS_TWO_PI) * lambda - rough_range_m;
phrng_m = (gnss_synchro.Carrier_phase_rads / TWO_PI) * lambda - rough_range_m;
/* Subtract phase - pseudorange integer cycle offset */
/* TODO: check LLI! */
double cp = gnss_synchro.Carrier_phase_rads / GPS_TWO_PI; // ?
double cp = gnss_synchro.Carrier_phase_rads / TWO_PI; // ?
if (std::fabs(phrng_m - cp) > 1171.0)
{
cp = std::round(phrng_m / lambda) * lambda;
@ -5517,32 +5517,32 @@ int32_t Rtcm::set_DF404(const Gnss_Synchro& gnss_synchro)
if ((sig_ == "1C") && (sys_ == "G"))
{
lambda = GPS_C_M_S / GPS_L1_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GPS_L1_FREQ_HZ;
}
if ((sig_ == "2S") && (sys_ == "G"))
{
lambda = GPS_C_M_S / GPS_L2_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GPS_L2_FREQ_HZ;
}
if ((sig_ == "5X") && (sys_ == "E"))
{
lambda = GPS_C_M_S / GALILEO_E5A_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GALILEO_E5A_FREQ_HZ;
}
if ((sig_ == "1B") && (sys_ == "E"))
{
lambda = GPS_C_M_S / GALILEO_E1_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GALILEO_E1_FREQ_HZ;
}
if ((sig_ == "7X") && (sys_ == "E"))
{
lambda = GPS_C_M_S / 1.207140e9; // Galileo_E1b_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / 1.207140e9; // Galileo_E1b_FREQ_HZ;
}
if ((sig_ == "1C") && (sys_ == "R"))
{
lambda = GLONASS_C_M_S / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)));
lambda = SPEED_OF_LIGHT_M_S / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)));
}
if ((sig_ == "2C") && (sys_ == "R"))
{
// TODO Need to add slot number and freq number to gnss syncro
lambda = GLONASS_C_M_S / (GLONASS_L2_CA_FREQ_HZ);
lambda = SPEED_OF_LIGHT_M_S / (GLONASS_L2_CA_FREQ_HZ);
}
double rough_phase_range_rate = std::round(-gnss_synchro.Carrier_Doppler_hz * lambda);
double phrr = (-gnss_synchro.Carrier_Doppler_hz * lambda - rough_phase_range_rate);
@ -5567,7 +5567,7 @@ int32_t Rtcm::set_DF404(const Gnss_Synchro& gnss_synchro)
int32_t Rtcm::set_DF405(const Gnss_Synchro& gnss_synchro)
{
double meters_to_miliseconds = GPS_C_M_S * 0.001;
double meters_to_miliseconds = SPEED_OF_LIGHT_M_S * 0.001;
double rough_range_m = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
double psrng_s;
int64_t fine_pseudorange;
@ -5594,7 +5594,7 @@ int32_t Rtcm::set_DF405(const Gnss_Synchro& gnss_synchro)
int32_t Rtcm::set_DF406(const Gnss_Synchro& gnss_synchro)
{
int64_t fine_phaserange_ex;
double meters_to_miliseconds = GPS_C_M_S * 0.001;
double meters_to_miliseconds = SPEED_OF_LIGHT_M_S * 0.001;
double rough_range_m = std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10) * meters_to_miliseconds * TWO_N10;
double phrng_m;
double lambda = 0.0;
@ -5604,38 +5604,38 @@ int32_t Rtcm::set_DF406(const Gnss_Synchro& gnss_synchro)
if ((sig_ == "1C") && (sys_ == "G"))
{
lambda = GPS_C_M_S / GPS_L1_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GPS_L1_FREQ_HZ;
}
if ((sig_ == "2S") && (sys_ == "G"))
{
lambda = GPS_C_M_S / GPS_L2_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GPS_L2_FREQ_HZ;
}
if ((sig_ == "5X") && (sys_ == "E"))
{
lambda = GPS_C_M_S / GALILEO_E5A_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GALILEO_E5A_FREQ_HZ;
}
if ((sig_ == "1B") && (sys_ == "E"))
{
lambda = GPS_C_M_S / GALILEO_E1_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / GALILEO_E1_FREQ_HZ;
}
if ((sig_ == "7X") && (sys_ == "E"))
{
lambda = GPS_C_M_S / 1.207140e9; // Galileo_E1b_FREQ_HZ;
lambda = SPEED_OF_LIGHT_M_S / 1.207140e9; // Galileo_E1b_FREQ_HZ;
}
if ((sig_ == "1C") && (sys_ == "R"))
{
lambda = GLONASS_C_M_S / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)));
lambda = SPEED_OF_LIGHT_M_S / (GLONASS_L1_CA_FREQ_HZ + (GLONASS_L1_CA_DFREQ_HZ * GLONASS_PRN.at(gnss_synchro.PRN)));
}
if ((sig_ == "2C") && (sys_ == "R"))
{
// TODO Need to add slot number and freq number to gnss syncro
lambda = GLONASS_C_M_S / (GLONASS_L2_CA_FREQ_HZ);
lambda = SPEED_OF_LIGHT_M_S / (GLONASS_L2_CA_FREQ_HZ);
}
phrng_m = (gnss_synchro.Carrier_phase_rads / GPS_TWO_PI) * lambda - rough_range_m;
phrng_m = (gnss_synchro.Carrier_phase_rads / TWO_PI) * lambda - rough_range_m;
/* Subtract phase - pseudorange integer cycle offset */
/* TODO: check LLI! */
double cp = gnss_synchro.Carrier_phase_rads / GPS_TWO_PI; // ?
double cp = gnss_synchro.Carrier_phase_rads / TWO_PI; // ?
if (std::fabs(phrng_m - cp) > 1171.0)
{
cp = std::round(phrng_m / lambda) * lambda;

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

@ -959,7 +959,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
{
// the receiver clock offset is expressed in [meters], so we convert it into [s]
// add also the clock offset from gps to galileo (pvt_sol.dtr[2])
rx_position_and_time(3) = pvt_sol.dtr[2] + pvt_sol.dtr[0] / GPS_C_M_S;
rx_position_and_time(3) = pvt_sol.dtr[2] + pvt_sol.dtr[0] / SPEED_OF_LIGHT_M_S;
}
this->set_rx_pos(rx_position_and_time.rows(0, 2)); // save ECEF position for the next iteration
@ -1059,7 +1059,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
this->set_rx_vel(rx_vel_enu);
double clock_drift_ppm = pvt_sol.dtr[5] / GPS_C_M_S * 1e6;
double clock_drift_ppm = pvt_sol.dtr[5] / SPEED_OF_LIGHT_M_S * 1e6;
this->set_clock_drift_ppm(clock_drift_ppm);
// User clock drift [ppm]

4
src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc
View File

@ -25,6 +25,7 @@
*/
#include "galileo_e5a_noncoherent_iq_acquisition_caf_cc.h"
#include "MATH_CONSTANTS.h"
#include "gnss_sdr_make_unique.h"
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
@ -224,7 +225,6 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_mag = 0.0;
d_input_power = 0.0;
const double GALILEO_TWO_PI = 6.283185307179600;
// Count the number of bins
d_num_doppler_bins = 0;
@ -240,7 +240,7 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GALILEO_TWO_PI * doppler / static_cast<float>(d_fs_in);
float phase_step_rad = TWO_PI * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}

5
src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc
View File

@ -19,6 +19,7 @@
*/
#include "galileo_pcps_8ms_acquisition_cc.h"
#include "MATH_CONSTANTS.h"
#include "gnss_sdr_make_unique.h"
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
@ -151,7 +152,7 @@ void galileo_pcps_8ms_acquisition_cc::init()
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_mag = 0.0;
d_input_power = 0.0;
const double GALILEO_TWO_PI = 6.283185307179600;
// Count the number of bins
d_num_doppler_bins = 0;
for (auto doppler = static_cast<int32_t>(-d_doppler_max);
@ -165,7 +166,7 @@ void galileo_pcps_8ms_acquisition_cc::init()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = static_cast<float>(GALILEO_TWO_PI) * doppler / static_cast<float>(d_fs_in);
float phase_step_rad = static_cast<float>(TWO_PI) * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}

8
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@ -23,8 +23,8 @@
*/
#include "pcps_acquisition.h"
#include "GLONASS_L1_L2_CA.h" // for GLONASS_TWO_PI
#include "GPS_L1_CA.h" // for GPS_TWO_PI
#include "GLONASS_L1_L2_CA.h" // for GLONASS_PRN
#include "MATH_CONSTANTS.h" // for TWO_PI
#include "gnss_frequencies.h"
#include "gnss_sdr_create_directory.h"
#include "gnss_sdr_make_unique.h"
@ -258,11 +258,11 @@ void pcps_acquisition::update_local_carrier(own::span<gr_complex> carrier_vector
float phase_step_rad;
if (d_acq_parameters.use_automatic_resampler)
{
phase_step_rad = GPS_TWO_PI * freq / static_cast<float>(d_acq_parameters.resampled_fs);
phase_step_rad = TWO_PI * freq / static_cast<float>(d_acq_parameters.resampled_fs);
}
else
{
phase_step_rad = GPS_TWO_PI * freq / static_cast<float>(d_acq_parameters.fs_in);
phase_step_rad = TWO_PI * freq / static_cast<float>(d_acq_parameters.fs_in);
}
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(carrier_vector.data(), -phase_step_rad, _phase.data(), carrier_vector.size());

4
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc
View File

@ -20,7 +20,7 @@
*/
#include "pcps_acquisition_fine_doppler_cc.h"
#include "GPS_L1_CA.h"
#include "GPS_L1_CA.h" // for GPS_L1_CA_CHIP_PERIOD_S
#include "gnss_sdr_create_directory.h"
#include "gnss_sdr_make_unique.h"
#include "gps_sdr_signal_processing.h"
@ -231,7 +231,7 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
doppler_hz = d_doppler_step * doppler_index - d_config_doppler_max;
// doppler search steps
// compute the carrier doppler wipe-off signal and store it
phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
phase_step_rad = static_cast<float>(TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
float _phase[1];
_phase[0] = 0;
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase, d_fft_size);

4
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc
View File

@ -20,7 +20,7 @@
*/
#include "pcps_assisted_acquisition_cc.h"
#include "GPS_L1_CA.h"
#include "MATH_CONSTANTS.h"
#include "concurrent_map.h"
#include "gnss_sdr_make_unique.h"
#include "gps_acq_assist.h"
@ -221,7 +221,7 @@ void pcps_assisted_acquisition_cc::redefine_grid()
doppler_hz = d_doppler_min + d_doppler_step * doppler_index;
// doppler search steps
// compute the carrier doppler wipe-off signal and store it
phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
phase_step_rad = static_cast<float>(TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}

4
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc
View File

@ -24,7 +24,7 @@
*/
#include "pcps_cccwsr_acquisition_cc.h"
#include "GPS_L1_CA.h" // GPS_TWO_PI
#include "MATH_CONSTANTS.h" // TWO_PI
#include "gnss_sdr_make_unique.h"
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
@ -175,7 +175,7 @@ void pcps_cccwsr_acquisition_cc::init()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
float phase_step_rad = TWO_PI * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}

4
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc
View File

@ -38,7 +38,7 @@
*/
#include "pcps_opencl_acquisition_cc.h"
#include "GPS_L1_CA.h" // GPS_TWO_PI
#include "MATH_CONSTANTS.h" // TWO_PI
#include "gnss_sdr_make_unique.h"
#include "opencl/fft_base_kernels.h"
#include "opencl/fft_internal.h"
@ -282,7 +282,7 @@ void pcps_opencl_acquisition_cc::init()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
int doppler = -static_cast<int>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler / static_cast<float>(d_fs_in);
float phase_step_rad = static_cast<float>(TWO_PI) * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);

4
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc
View File

@ -18,7 +18,7 @@
*/
#include "pcps_quicksync_acquisition_cc.h"
#include "GPS_L1_CA.h"
#include "MATH_CONSTANTS.h"
#include "gnss_sdr_make_unique.h"
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
@ -195,7 +195,7 @@ void pcps_quicksync_acquisition_cc::init()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
float phase_step_rad = TWO_PI * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_samples_per_code * d_folding_factor);
}

4
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc
View File

@ -38,7 +38,7 @@
*/
#include "pcps_tong_acquisition_cc.h"
#include "GPS_L1_CA.h" // for GPS_TWO_PI
#include "MATH_CONSTANTS.h" // for TWO_PI
#include "gnss_sdr_make_unique.h"
#include <glog/logging.h>
#include <gnuradio/io_signature.h>
@ -182,7 +182,7 @@ void pcps_tong_acquisition_cc::init()
for (uint32_t doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
int32_t doppler = -static_cast<int32_t>(d_doppler_max) + d_doppler_step * doppler_index;
float phase_step_rad = GPS_TWO_PI * doppler / static_cast<float>(d_fs_in);
float phase_step_rad = TWO_PI * doppler / static_cast<float>(d_fs_in);
std::array<float, 1> _phase{};
volk_gnsssdr_s32f_sincos_32fc(d_grid_doppler_wipeoffs[doppler_index].data(), -phase_step_rad, _phase.data(), d_fft_size);
}

16
src/algorithms/acquisition/libs/fpga_acquisition.cc
View File

@ -23,14 +23,14 @@
*/
#include "fpga_acquisition.h"
#include "GPS_L1_CA.h" // for GPS_TWO_PI
#include <glog/logging.h> // for LOG
#include <cmath> // for log2
#include <fcntl.h> // libraries used by the GIPO
#include <iostream> // for operator<<
#include <sys/mman.h> // libraries used by the GIPO
#include <unistd.h> // for write, close, read, ssize_t
#include <utility> // for move
#include "MATH_CONSTANTS.h" // for TWO_PI
#include <glog/logging.h> // for LOG
#include <cmath> // for log2
#include <fcntl.h> // libraries used by the GIPO
#include <iostream> // for operator<<
#include <sys/mman.h> // libraries used by the GIPO
#include <unistd.h> // for write, close, read, ssize_t
#include <utility> // for move
#ifndef TEMP_FAILURE_RETRY

7
src/algorithms/libs/gnss_signal_processing.cc
View File

@ -4,7 +4,6 @@
* regardless of system used
* \author Luis Esteve, 2012. luis(at)epsilon-formacion.com
*
* Detailed description of the file here if needed.
*
* -------------------------------------------------------------------------
*
@ -21,7 +20,7 @@
*/
#include "gnss_signal_processing.h"
#include "GPS_L1_CA.h"
#include "MATH_CONSTANTS.h"
#include <gnuradio/fxpt_nco.h>
#include <cstddef> // for size_t
@ -31,7 +30,7 @@ const auto AUX_CEIL2 = [](float x) { return static_cast<int32_t>(static_cast<int
void complex_exp_gen(own::span<std::complex<float>> _dest, double _f, double _fs)
{
gr::fxpt_nco d_nco;
d_nco.set_freq((GPS_TWO_PI * _f) / _fs);
d_nco.set_freq((TWO_PI * _f) / _fs);
d_nco.sincos(_dest.data(), _dest.size(), 1);
}
@ -39,7 +38,7 @@ void complex_exp_gen(own::span<std::complex<float>> _dest, double _f, double _fs
void complex_exp_gen_conj(own::span<std::complex<float>> _dest, double _f, double _fs)
{
gr::fxpt_nco d_nco;
d_nco.set_freq(-(GPS_TWO_PI * _f) / _fs);
d_nco.set_freq(-(TWO_PI * _f) / _fs);
d_nco.sincos(_dest.data(), _dest.size(), 1);
}

5
src/algorithms/libs/rtklib/rtklib.h
View File

@ -42,6 +42,7 @@
#include <cstdlib>
#include <netinet/in.h>
#include <pthread.h>
#include <string>
/* macros --------------------------------------------------------------------*/
@ -1293,8 +1294,8 @@ const double CHISQR[100] = {/* chi-sqr(n) (alpha=0.001) */
const double LAM_CARR[MAXFREQ] = {/* carrier wave length (m) */
SPEED_OF_LIGHT / FREQ1, SPEED_OF_LIGHT / FREQ2, SPEED_OF_LIGHT / FREQ5, SPEED_OF_LIGHT / FREQ6, SPEED_OF_LIGHT / FREQ7,
SPEED_OF_LIGHT / FREQ8, SPEED_OF_LIGHT / FREQ9};
SPEED_OF_LIGHT_M_S / FREQ1, SPEED_OF_LIGHT_M_S / FREQ2, SPEED_OF_LIGHT_M_S / FREQ5, SPEED_OF_LIGHT_M_S / FREQ6, SPEED_OF_LIGHT_M_S / FREQ7,
SPEED_OF_LIGHT_M_S / FREQ8, SPEED_OF_LIGHT_M_S / FREQ9};
const int STRFMT_RTCM2 = 0; /* stream format: RTCM 2 */
const int STRFMT_RTCM3 = 1; /* stream format: RTCM 3 */

26
src/algorithms/libs/rtklib/rtklib_conversions.cc
View File

@ -18,7 +18,7 @@
*/
#include "rtklib_conversions.h"
#include "MATH_CONSTANTS.h" // for PI, PI_2
#include "MATH_CONSTANTS.h" // for GNSS_PI, TWO_PI
#include "beidou_dnav_ephemeris.h" // for Beidou_Dnav_Ephemeris
#include "galileo_almanac.h" // for Galileo_Almanac
#include "galileo_ephemeris.h" // for Galileo_Ephemeris
@ -41,7 +41,7 @@ obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs, const Gnss_Synchro& gnss_synchro
rtklib_obs.D[band] = gnss_synchro.Carrier_Doppler_hz;
rtklib_obs.P[band] = gnss_synchro.Pseudorange_m;
rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / PI_2;
rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / TWO_PI;
switch (band)
{
@ -364,7 +364,7 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
rtklib_sat.OMG0 = gps_cnav_eph.d_OMEGA0;
// Compute the angle between the ascending node and the Greenwich meridian
const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200K pp. 164
double d_OMEGA_DOT = OMEGA_DOT_REF * PI + gps_cnav_eph.d_DELTA_OMEGA_DOT;
double d_OMEGA_DOT = OMEGA_DOT_REF * GNSS_PI + gps_cnav_eph.d_DELTA_OMEGA_DOT;
rtklib_sat.OMGd = d_OMEGA_DOT;
rtklib_sat.omg = gps_cnav_eph.d_OMEGA;
rtklib_sat.i0 = gps_cnav_eph.d_i_0;
@ -434,11 +434,11 @@ alm_t alm_to_rtklib(const Gps_Almanac& gps_alm)
rtklib_alm.toa = toa;
rtklib_alm.A = gps_alm.d_sqrt_A * gps_alm.d_sqrt_A;
rtklib_alm.e = gps_alm.d_e_eccentricity;
rtklib_alm.i0 = (gps_alm.d_Delta_i + 0.3) * PI;
rtklib_alm.OMG0 = gps_alm.d_OMEGA0 * PI;
rtklib_alm.OMGd = gps_alm.d_OMEGA_DOT * PI;
rtklib_alm.omg = gps_alm.d_OMEGA * PI;
rtklib_alm.M0 = gps_alm.d_M_0 * PI;
rtklib_alm.i0 = (gps_alm.d_Delta_i + 0.3) * GNSS_PI;
rtklib_alm.OMG0 = gps_alm.d_OMEGA0 * GNSS_PI;
rtklib_alm.OMGd = gps_alm.d_OMEGA_DOT * GNSS_PI;
rtklib_alm.omg = gps_alm.d_OMEGA * GNSS_PI;
rtklib_alm.M0 = gps_alm.d_M_0 * GNSS_PI;
rtklib_alm.f0 = gps_alm.d_A_f0;
rtklib_alm.f1 = gps_alm.d_A_f1;
rtklib_alm.toas = gps_alm.i_Toa;
@ -464,11 +464,11 @@ alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm)
rtklib_alm.A = 5440.588203494 + gal_alm.d_Delta_sqrt_A;
rtklib_alm.A = rtklib_alm.A * rtklib_alm.A;
rtklib_alm.e = gal_alm.d_e_eccentricity;
rtklib_alm.i0 = (gal_alm.d_Delta_i + 56.0 / 180.0) * PI;
rtklib_alm.OMG0 = gal_alm.d_OMEGA0 * PI;
rtklib_alm.OMGd = gal_alm.d_OMEGA_DOT * PI;
rtklib_alm.omg = gal_alm.d_OMEGA * PI;
rtklib_alm.M0 = gal_alm.d_M_0 * PI;
rtklib_alm.i0 = (gal_alm.d_Delta_i + 56.0 / 180.0) * GNSS_PI;
rtklib_alm.OMG0 = gal_alm.d_OMEGA0 * GNSS_PI;
rtklib_alm.OMGd = gal_alm.d_OMEGA_DOT * GNSS_PI;
rtklib_alm.omg = gal_alm.d_OMEGA * GNSS_PI;
rtklib_alm.M0 = gal_alm.d_M_0 * GNSS_PI;
rtklib_alm.f0 = gal_alm.d_A_f0;
rtklib_alm.f1 = gal_alm.d_A_f1;
rtklib_alm.toas = gal_alm.i_Toa;

32
src/algorithms/libs/rtklib/rtklib_ephemeris.cc
View File

@ -43,9 +43,9 @@ const double MU_GAL = 3.986004418e14; /* earth gravitational constant ref [7]
const double MU_BDS = 3.986004418e14; /* earth gravitational constant ref [9] */
const double J2_GLO = 1.0826257e-3; /* 2nd zonal harmonic of geopot ref [2] */
const double OMGE_GLO = 7.292115e-5; /* earth angular velocity (rad/s) ref [2] */
const double OMGE_GAL = 7.2921151467e-5; /* earth angular velocity (rad/s) ref [7] */
const double OMGE_BDS = 7.292115e-5; /* earth angular velocity (rad/s) ref [9] */
const double OMGE_GLO = GLONASS_OMEGA_EARTH_DOT; /* earth angular velocity (rad/s) ref [2] */
const double OMGE_GAL = GNSS_OMEGA_EARTH_DOT; /* earth angular velocity (rad/s) ref [7] */
const double OMGE_BDS = BEIDOU_OMEGA_EARTH_DOT; /* earth angular velocity (rad/s) ref [9] */
const double SIN_5 = -0.0871557427476582; /* sin(-5.0 deg) */
const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */
@ -54,12 +54,12 @@ const double ERREPH_GLO = 5.0; /* error of glonass ephemeris (m) */
const double TSTEP = 60.0; /* integration step glonass ephemeris (s) */
const double RTOL_KEPLER = 1e-13; /* relative tolerance for Kepler equation */
const double DEFURASSR = 0.15; /* default accuracy of ssr corr (m) */
const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */
const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT; /* max clock correction of ssr (m) */
const double MAXAGESSR = 90.0; /* max age of ssr orbit and clock (s) */
const double MAXAGESSR_HRCLK = 10.0; /* max age of ssr high-rate clock (s) */
const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) */
const double DEFURASSR = 0.15; /* default accuracy of ssr corr (m) */
const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */
const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT_M_S; /* max clock correction of ssr (m) */
const double MAXAGESSR = 90.0; /* max age of ssr orbit and clock (s) */
const double MAXAGESSR_HRCLK = 10.0; /* max age of ssr high-rate clock (s) */
const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) */
const int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */
@ -147,7 +147,7 @@ void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts)
u = atan2(sqrt(1.0 - alm->e * alm->e) * sinE, cosE - alm->e) + alm->omg;
r = alm->A * (1.0 - alm->e * cosE);
i = alm->i0;
O = alm->OMG0 + (alm->OMGd - DEFAULT_OMEGA_EARTH_DOT) * tk - DEFAULT_OMEGA_EARTH_DOT * alm->toas;
O = alm->OMG0 + (alm->OMGd - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * alm->toas;
x = r * cos(u);
y = r * sin(u);
sinO = sin(O);
@ -250,7 +250,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
break;
default:
mu = MU_GPS;
omge = DEFAULT_OMEGA_EARTH_DOT;
omge = GNSS_OMEGA_EARTH_DOT;
break;
}
M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk;
@ -310,7 +310,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
*dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk;
/* relativity correction */
*dts -= 2.0 * sqrt(mu * eph->A) * eph->e * sinE / std::pow(SPEED_OF_LIGHT, 2.0);
*dts -= 2.0 * sqrt(mu * eph->A) * eph->e * sinE / std::pow(SPEED_OF_LIGHT_M_S, 2.0);
/* position and clock error variance */
*var = var_uraeph(eph->sva);
@ -906,7 +906,7 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
dts[1] = eph->f1 + 2.0 * eph->f2 * tk;
/* relativity correction */
dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT_M_S / SPEED_OF_LIGHT_M_S;
}
/* radial-along-cross directions in ecef */
if (!normv3(rs + 3, ea))
@ -930,8 +930,8 @@ int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
{
rs[i] += -(er[i] * deph[0] + ea[i] * deph[1] + ec[i] * deph[2]) + dant[i];
}
/* t_corr = t_sv - (dts(brdc) + dclk(ssr) / SPEED_OF_LIGHT) (ref [10] eq.3.12-7) */
dts[0] += dclk / SPEED_OF_LIGHT;
/* t_corr = t_sv - (dts(brdc) + dclk(ssr) / SPEED_OF_LIGHT_M_S) (ref [10] eq.3.12-7) */
dts[0] += dclk / SPEED_OF_LIGHT_M_S;
/* variance by ssr ura */
*var = var_urassr(ssr->ura);
@ -1058,7 +1058,7 @@ void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav,
continue;
}
/* transmission time by satellite clock */
time[i] = timeadd(obs[i].time, -pr / SPEED_OF_LIGHT);
time[i] = timeadd(obs[i].time, -pr / SPEED_OF_LIGHT_M_S);
/* satellite clock bias by broadcast ephemeris */
if (!ephclk(time[i], teph, obs[i].sat, nav, &dt))

6
src/algorithms/libs/rtklib/rtklib_ionex.cc
View File

@ -488,7 +488,7 @@ void readtec(const char *file, nav_t *nav, int opt)
/* P1-P2 dcb */
for (i = 0; i < MAXSAT; i++)
{
nav->cbias[i][0] = SPEED_OF_LIGHT * dcb[i] * 1e-9; /* ns->m */
nav->cbias[i][0] = SPEED_OF_LIGHT_M_S * dcb[i] * 1e-9; /* ns->m */
}
}
@ -622,13 +622,13 @@ int iondelay(gtime_t time, const tec_t *tec, const double *pos,
if (opt & 2)
{
/* modified single layer mapping function (M-SLM) ref [2] */
rp = tec->rb / (tec->rb + hion) * sin(0.9782 * (PI / 2.0 - azel[1]));
rp = tec->rb / (tec->rb + hion) * sin(0.9782 * (GNSS_PI / 2.0 - azel[1]));
fs = 1.0 / sqrt(1.0 - rp * rp);
}
if (opt & 1)
{
/* earth rotation correction (sun-fixed coordinate) */
posp[1] += 2.0 * PI * timediff(time, tec->time) / 86400.0;
posp[1] += 2.0 * GNSS_PI * timediff(time, tec->time) / 86400.0;
}
/* interpolate tec grid data */
if (!interptec(tec, i, posp, &vtec, &rms))

26
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -60,7 +60,7 @@ double gettgd(int sat, const nav_t *nav)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].tgd[0];
return SPEED_OF_LIGHT_M_S * nav->eph[i].tgd[0];
}
return 0.0;
}
@ -74,7 +74,7 @@ double getiscl1(int sat, const nav_t *nav)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].isc[0];
return SPEED_OF_LIGHT_M_S * nav->eph[i].isc[0];
}
return 0.0;
}
@ -87,7 +87,7 @@ double getiscl2(int sat, const nav_t *nav)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].isc[1];
return SPEED_OF_LIGHT_M_S * nav->eph[i].isc[1];
}
return 0.0;
}
@ -100,7 +100,7 @@ double getiscl5i(int sat, const nav_t *nav)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].isc[2];
return SPEED_OF_LIGHT_M_S * nav->eph[i].isc[2];
}
return 0.0;
}
@ -113,7 +113,7 @@ double getiscl5q(int sat, const nav_t *nav)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].isc[3];
return SPEED_OF_LIGHT_M_S * nav->eph[i].isc[3];
}
return 0.0;
}
@ -497,7 +497,7 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
continue;
}
/* pseudorange residual */
v[nv] = P - (r + dtr - SPEED_OF_LIGHT * dts[i * 2] + dion + dtrp);
v[nv] = P - (r + dtr - SPEED_OF_LIGHT_M_S * dts[i * 2] + dion + dtrp);
/* design matrix */
for (j = 0; j < NX; j++)
@ -665,11 +665,11 @@ int estpos(const obsd_t *obs, int n, const double *rs, const double *dts,
if (norm_rtk(dx, NX) < 1e-4)
{
sol->type = 0;
sol->time = timeadd(obs[0].time, -x[3] / SPEED_OF_LIGHT);
sol->dtr[0] = x[3] / SPEED_OF_LIGHT; /* receiver clock bias (s) */
sol->dtr[1] = x[4] / SPEED_OF_LIGHT; /* glo-gps time offset (s) */
sol->dtr[2] = x[5] / SPEED_OF_LIGHT; /* gal-gps time offset (s) */
sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* bds-gps time offset (s) */
sol->time = timeadd(obs[0].time, -x[3] / SPEED_OF_LIGHT_M_S);
sol->dtr[0] = x[3] / SPEED_OF_LIGHT_M_S; /* receiver clock bias (s) */
sol->dtr[1] = x[4] / SPEED_OF_LIGHT_M_S; /* glo-gps time offset (s) */
sol->dtr[2] = x[5] / SPEED_OF_LIGHT_M_S; /* gal-gps time offset (s) */
sol->dtr[3] = x[6] / SPEED_OF_LIGHT_M_S; /* bds-gps time offset (s) */
for (j = 0; j < 6; j++)
{
sol->rr[j] = j < 3 ? x[j] : 0.0;
@ -891,10 +891,10 @@ int resdop(const obsd_t *obs, int n, const double *rs, const double *dts,
}
/* range rate with earth rotation correction */
rate = dot(vs, e, 3) + DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0] - rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]);
rate = dot(vs, e, 3) + GNSS_OMEGA_EARTH_DOT / SPEED_OF_LIGHT_M_S * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0] - rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]);
/* doppler residual */
v[nv] = -lam * obs[i].D[band] - (rate + x[3] - SPEED_OF_LIGHT * dts[1 + i * 2]);
v[nv] = -lam * obs[i].D[band] - (rate + x[3] - SPEED_OF_LIGHT_M_S * dts[1 + i * 2]);
/* design matrix */
for (j = 0; j < 4; j++)

28
src/algorithms/libs/rtklib/rtklib_ppp.cc
View File

@ -45,7 +45,7 @@ double lam_LC(int i, int j, int k)
const double f2 = FREQ2;
const double f5 = FREQ5;
return SPEED_OF_LIGHT / (i * f1 + j * f2 + k * f5);
return SPEED_OF_LIGHT_M_S / (i * f1 + j * f2 + k * f5);
}
@ -63,9 +63,9 @@ double L_LC(int i, int j, int k, const double *L)
{
return 0.0;
}
L1 = SPEED_OF_LIGHT / f1 * L[0];
L2 = SPEED_OF_LIGHT / f2 * L[1];
L5 = SPEED_OF_LIGHT / f5 * L[2];
L1 = SPEED_OF_LIGHT_M_S / f1 * L[0];
L2 = SPEED_OF_LIGHT_M_S / f2 * L[1];
L5 = SPEED_OF_LIGHT_M_S / f5 * L[2];
return (i * f1 * L1 + j * f2 * L2 + k * f5 * L5) / (i * f1 + j * f2 + k * f5);
}
@ -773,7 +773,7 @@ void pppoutsolstat(rtk_t *rtk, int level, FILE *fp)
/* receiver clocks */
i = IC_PPP(0, &rtk->opt);
fprintf(fp, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week, tow, rtk->sol.stat, 1, rtk->x[i] * 1E9 / SPEED_OF_LIGHT, rtk->x[i + 1] * 1E9 / SPEED_OF_LIGHT,
week, tow, rtk->sol.stat, 1, rtk->x[i] * 1E9 / SPEED_OF_LIGHT_M_S, rtk->x[i + 1] * 1E9 / SPEED_OF_LIGHT_M_S,
0.0, 0.0);
/* tropospheric parameters */
@ -858,7 +858,7 @@ void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs)
ang = acos(cosa);
/* test eclipse */
if (ang < PI / 2.0 || r * sin(ang) > RE_WGS84)
if (ang < GNSS_PI / 2.0 || r * sin(ang) > RE_WGS84)
{
continue;
}
@ -990,7 +990,7 @@ int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel,
P2_C2 = nav->cbias[obs->sat - 1][2];
if (opt->sateph == EPHOPT_LEX)
{
P1_C1 = nav->lexeph[obs->sat - 1].isc[0] * SPEED_OF_LIGHT; /* ISC_L1C/A */
P1_C1 = nav->lexeph[obs->sat - 1].isc[0] * SPEED_OF_LIGHT_M_S; /* ISC_L1C/A */
}
if (L1 == 0.0 || L2 == 0.0 || P1 == 0.0 || P2 == 0.0)
{
@ -1048,7 +1048,7 @@ double gettgd_ppp(int sat, const nav_t *nav)
{
continue;
}
return SPEED_OF_LIGHT * nav->eph[i].tgd[0];
return SPEED_OF_LIGHT_M_S * nav->eph[i].tgd[0];
}
return 0.0;
}
@ -1249,7 +1249,7 @@ void udclk_ppp(rtk_t *rtk)
{
dtr = i == 0 ? rtk->sol.dtr[0] : rtk->sol.dtr[0] + rtk->sol.dtr[i];
}
initx(rtk, SPEED_OF_LIGHT * dtr, VAR_CLK, IC_PPP(i, &rtk->opt));
initx(rtk, SPEED_OF_LIGHT_M_S * dtr, VAR_CLK, IC_PPP(i, &rtk->opt));
}
}
@ -1258,7 +1258,7 @@ void udclk_ppp(rtk_t *rtk)
void udtrop_ppp(rtk_t *rtk)
{
double pos[3];
double azel[] = {0.0, PI / 2.0};
double azel[] = {0.0, GNSS_PI / 2.0};
double ztd;
double var;
int i = IT_PPP(&rtk->opt);
@ -1420,7 +1420,7 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
k++;
}
/* correct phase-code jump to enssure phase-code coherency */
if (k >= 2 && fabs(offset / k) > 0.0005 * SPEED_OF_LIGHT)
if (k >= 2 && fabs(offset / k) > 0.0005 * SPEED_OF_LIGHT_M_S)
{
for (i = 0; i < MAXSAT; i++)
{
@ -1431,7 +1431,7 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
}
}
trace(2, "phase-code jump corrected: %s n=%2d dt=%12.9fs\n",
time_str(rtk->sol.time, 0), k, offset / k / SPEED_OF_LIGHT);
time_str(rtk->sol.time, 0), k, offset / k / SPEED_OF_LIGHT_M_S);
}
for (i = 0; i < n && i < MAXOBS; i++)
{
@ -1515,7 +1515,7 @@ double prectrop(gtime_t time, const double *pos, const double *azel,
const prcopt_t *opt, const double *x, double *dtdx,
double *var)
{
const double zazel[] = {0.0, PI / 2.0};
const double zazel[] = {0.0, GNSS_PI / 2.0};
double zhd;
double m_h;
double m_w;
@ -1660,7 +1660,7 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do
continue;
}
/* satellite clock and tropospheric delay */
r += -SPEED_OF_LIGHT * dts[i * 2] + dtrp;
r += -SPEED_OF_LIGHT_M_S * dts[i * 2] + dtrp;
trace(5, "sat=%2d azel=%6.1f %5.1f dtrp=%.3f dantr=%6.3f %6.3f dants=%6.3f %6.3f phw=%6.3f\n",
sat, azel[i * 2] * R2D, azel[1 + i * 2] * R2D, dtrp, dantr[0], dantr[1], dants[0],

22
src/algorithms/libs/rtklib/rtklib_preceph.cc
View File

@ -539,12 +539,12 @@ int readdcbf(const char *file, nav_t *nav, const sta_t *sta)
if (i < MAXRCV)
{
j = !strcmp(str1, "G") ? 0 : 1;
nav->rbias[i][j][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
nav->rbias[i][j][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; /* ns -> m */
}
}
else if ((sat = satid2no(str1)))
{ /* satellite dcb */
nav->cbias[sat - 1][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */
{ /* satellite dcb */
nav->cbias[sat - 1][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; /* ns -> m */
}
}
fclose(fp);
@ -692,8 +692,8 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
p[1][j] = pos[1];
#else
/* correciton for earh rotation ver.2.4.0 */
sinl = sin(DEFAULT_OMEGA_EARTH_DOT * t[j]);
cosl = cos(DEFAULT_OMEGA_EARTH_DOT * t[j]);
sinl = sin(GNSS_OMEGA_EARTH_DOT * t[j]);
cosl = cos(GNSS_OMEGA_EARTH_DOT * t[j]);
p[0][j] = cosl * pos[0] - sinl * pos[1];
p[1][j] = sinl * pos[0] + cosl * pos[1];
#endif
@ -732,14 +732,14 @@ int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs,
{
if ((dts[0] = c[0]) != 0.0)
{
std = nav->peph[index].std[sat - 1][3] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
std = nav->peph[index].std[sat - 1][3] * SPEED_OF_LIGHT_M_S - EXTERR_CLK * t[0];
}
}
else if (t[1] >= 0.0)
{
if ((dts[0] = c[1]) != 0.0)
{
std = nav->peph[index + 1].std[sat - 1][3] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
std = nav->peph[index + 1].std[sat - 1][3] * SPEED_OF_LIGHT_M_S + EXTERR_CLK * t[1];
}
}
else if (c[0] != 0.0 && c[1] != 0.0)
@ -808,7 +808,7 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
{
return 0;
}
std = nav->pclk[index].std[sat - 1][0] * SPEED_OF_LIGHT - EXTERR_CLK * t[0];
std = nav->pclk[index].std[sat - 1][0] * SPEED_OF_LIGHT_M_S - EXTERR_CLK * t[0];
}
else if (t[1] >= 0.0)
{
@ -816,13 +816,13 @@ int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts,
{
return 0;
}
std = nav->pclk[index + 1].std[sat - 1][0] * SPEED_OF_LIGHT + EXTERR_CLK * t[1];
std = nav->pclk[index + 1].std[sat - 1][0] * SPEED_OF_LIGHT_M_S + EXTERR_CLK * t[1];
}
else if (c[0] != 0.0 && c[1] != 0.0)
{
dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]);
i = t[0] < -t[1] ? 0 : 1;
std = nav->pclk[index + i].std[sat - 1][0] * SPEED_OF_LIGHT + EXTERR_CLK * fabs(t[i]);
std = nav->pclk[index + i].std[sat - 1][0] * SPEED_OF_LIGHT_M_S + EXTERR_CLK * fabs(t[i]);
}
else
{
@ -988,7 +988,7 @@ int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt,
/* relativistic effect correction */
if (dtss[0] != 0.0)
{
dts[0] = dtss[0] - 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT;
dts[0] = dtss[0] - 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT_M_S / SPEED_OF_LIGHT_M_S;
dts[1] = (dtst[0] - dtss[0]) / tt;
}
else

10
src/algorithms/libs/rtklib/rtklib_rtcm3.cc
View File

@ -874,7 +874,7 @@ int decode_type1010(rtcm_t *rtcm)
if (ppr1 != static_cast<int>(0xFFF80000))
{
rtcm->obs.data[index].P[0] = pr1;
lam1 = SPEED_OF_LIGHT / (FREQ1_GLO + DFRQ1_GLO * (freq - 7));
lam1 = SPEED_OF_LIGHT_M_S / (FREQ1_GLO + DFRQ1_GLO * (freq - 7));
cp1 = adjcp(rtcm, sat, 0, ppr1 * 0.0005 / lam1);
rtcm->obs.data[index].L[0] = pr1 / lam1 + cp1;
}
@ -978,7 +978,7 @@ int decode_type1012(rtcm_t *rtcm)
pr1 = pr1 * 0.02 + amb * PRUNIT_GLO;
if (ppr1 != static_cast<int>(0xFFF80000))
{
lam1 = SPEED_OF_LIGHT / (FREQ1_GLO + DFRQ1_GLO * (freq - 7));
lam1 = SPEED_OF_LIGHT_M_S / (FREQ1_GLO + DFRQ1_GLO * (freq - 7));
rtcm->obs.data[index].P[0] = pr1;
cp1 = adjcp(rtcm, sat, 0, ppr1 * 0.0005 / lam1);
rtcm->obs.data[index].L[0] = pr1 / lam1 + cp1;
@ -993,7 +993,7 @@ int decode_type1012(rtcm_t *rtcm)
}
if (ppr2 != static_cast<int>(0xFFF80000))
{
lam2 = SPEED_OF_LIGHT / (FREQ2_GLO + DFRQ2_GLO * (freq - 7));
lam2 = SPEED_OF_LIGHT_M_S / (FREQ2_GLO + DFRQ2_GLO * (freq - 7));
cp2 = adjcp(rtcm, sat, 1, ppr2 * 0.0005 / lam2);
rtcm->obs.data[index].L[1] = pr1 / lam2 + cp2;
}
@ -3203,8 +3203,8 @@ void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r,
if (sys == SYS_GLO && ex && ex[i] <= 13)
{
fn = ex[i] - 7;
wl = SPEED_OF_LIGHT / ((freq[k] == 2 ? FREQ2_GLO : FREQ1_GLO) +
(freq[k] == 2 ? DFRQ2_GLO : DFRQ1_GLO) * fn);
wl = SPEED_OF_LIGHT_M_S / ((freq[k] == 2 ? FREQ2_GLO : FREQ1_GLO) +
(freq[k] == 2 ? DFRQ2_GLO : DFRQ1_GLO) * fn);
}
/* pseudorange (m) */
if (r[i] != 0.0 && pr[j] > -1E12)

6
src/algorithms/libs/rtklib/rtklib_rtcm3.h
View File

@ -37,9 +37,9 @@
/* constants -----------------------------------------------------------------*/
const double PRUNIT_GPS = 299792.458; /* rtcm ver.3 unit of gps pseudorange (m) */
const double PRUNIT_GLO = 599584.916; /* rtcm ver.3 unit of glonass pseudorange (m) */
const double RANGE_MS = SPEED_OF_LIGHT * 0.001; /* range in 1 ms */
const double PRUNIT_GPS = 299792.458; /* rtcm ver.3 unit of gps pseudorange (m) */
const double PRUNIT_GLO = 599584.916; /* rtcm ver.3 unit of glonass pseudorange (m) */
const double RANGE_MS = SPEED_OF_LIGHT_M_S * 0.001; /* range in 1 ms */
/* ssr update intervals ------------------------------------------------------*/

68
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

@ -2023,7 +2023,7 @@ double utc2gmst(gtime_t t, double ut1_utc)
gmst0 = 24110.54841 + 8640184.812866 * t1 + 0.093104 * t2 - 6.2E-6 * t3;
gmst = gmst0 + 1.002737909350795 * ut;
return fmod(gmst, 86400.0) * PI / 43200.0; /* 0 <= gmst <= 2*PI */
return fmod(gmst, 86400.0) * GNSS_PI / 43200.0; /* 0 <= gmst <= 2*PI */
}
@ -2250,7 +2250,7 @@ void ecef2pos(const double *r, double *pos)
v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
z = r[2] + v * e2 * sinp;
}
pos[0] = r2 > 1e-12 ? atan(z / sqrt(r2)) : (r[2] > 0.0 ? PI / 2.0 : -PI / 2.0);
pos[0] = r2 > 1e-12 ? atan(z / sqrt(r2)) : (r[2] > 0.0 ? GNSS_PI / 2.0 : -GNSS_PI / 2.0);
pos[1] = r2 > 1e-12 ? atan2(r[1], r[0]) : 0.0;
pos[2] = sqrt(r2 + z * z) - v;
}
@ -2396,7 +2396,7 @@ void ast_args(double t, double *f)
{
f[i] += fc[i][j + 1] * tt[j];
}
f[i] = fmod(f[i] * AS2R, 2.0 * PI);
f[i] = fmod(f[i] * AS2R, 2.0 * GNSS_PI);
}
}
@ -4343,58 +4343,58 @@ double satwavelen(int sat, int frq, const nav_t *nav)
{
continue;
}
return SPEED_OF_LIGHT / (freq_glo[frq] + dfrq_glo[frq] * nav->geph[i].frq);
return SPEED_OF_LIGHT_M_S / (freq_glo[frq] + dfrq_glo[frq] * nav->geph[i].frq);
}
}
else if (frq == 2)
{ /* L3 */
return SPEED_OF_LIGHT / FREQ3_GLO;
return SPEED_OF_LIGHT_M_S / FREQ3_GLO;
}
}
else if (sys == SYS_BDS)
{
if (frq == 0)
{
return SPEED_OF_LIGHT / FREQ1_BDS; /* B1 */
return SPEED_OF_LIGHT_M_S / FREQ1_BDS; /* B1 */
}
if (frq == 1)
{
return SPEED_OF_LIGHT / FREQ2_BDS; /* B2 */
return SPEED_OF_LIGHT_M_S / FREQ2_BDS; /* B2 */
}
if (frq == 2)
{
return SPEED_OF_LIGHT / FREQ3_BDS; /* B3 */
return SPEED_OF_LIGHT_M_S / FREQ3_BDS; /* B3 */
}
}
else
{
if (frq == 0)
{
return SPEED_OF_LIGHT / FREQ1; /* L1/E1 */
return SPEED_OF_LIGHT_M_S / FREQ1; /* L1/E1 */
}
if (frq == 1)
{
return SPEED_OF_LIGHT / FREQ2; /* L2 */
return SPEED_OF_LIGHT_M_S / FREQ2; /* L2 */
}
if (frq == 2)
{
return SPEED_OF_LIGHT / FREQ5; /* L5/E5a */
return SPEED_OF_LIGHT_M_S / FREQ5; /* L5/E5a */
}
if (frq == 3)
{
return SPEED_OF_LIGHT / FREQ6; /* L6/LEX */
return SPEED_OF_LIGHT_M_S / FREQ6; /* L6/LEX */
}
if (frq == 4)
{
return SPEED_OF_LIGHT / FREQ7; /* E5b */
return SPEED_OF_LIGHT_M_S / FREQ7; /* E5b */
}
if (frq == 5)
{
return SPEED_OF_LIGHT / FREQ8; /* E5a+b */
return SPEED_OF_LIGHT_M_S / FREQ8; /* E5a+b */
}
if (frq == 6)
{
return SPEED_OF_LIGHT / FREQ9; /* S */
return SPEED_OF_LIGHT_M_S / FREQ9; /* S */
}
}
return 0.0;
@ -4427,7 +4427,7 @@ double geodist(const double *rs, const double *rr, double *e)
{
e[i] /= r;
}
return r + DEFAULT_OMEGA_EARTH_DOT * (rs[0] * rr[1] - rs[1] * rr[0]) / SPEED_OF_LIGHT;
return r + GNSS_OMEGA_EARTH_DOT * (rs[0] * rr[1] - rs[1] * rr[0]) / SPEED_OF_LIGHT_M_S;
}
@ -4442,7 +4442,7 @@ double geodist(const double *rs, const double *rr, double *e)
double satazel(const double *pos, const double *e, double *azel)
{
double az = 0.0;
double el = PI / 2.0;
double el = GNSS_PI / 2.0;
double enu[3];
if (pos[2] > -RE_WGS84)
@ -4451,7 +4451,7 @@ double satazel(const double *pos, const double *e, double *azel)
az = dot(enu, enu, 2) < 1e-12 ? 0.0 : atan2(enu[0], enu[1]);
if (az < 0.0)
{
az += 2 * PI;
az += 2 * GNSS_PI;
}
el = asin(enu[2]);
}
@ -4549,10 +4549,10 @@ double ionmodel(gtime_t t, const double *ion, const double *pos,
}
/* earth centered angle (semi-circle) */
psi = 0.0137 / (azel[1] / PI + 0.11) - 0.022;
psi = 0.0137 / (azel[1] / GNSS_PI + 0.11) - 0.022;
/* subionospheric latitude/longitude (semi-circle) */
phi = pos[0] / PI + psi * cos(azel[0]);
phi = pos[0] / GNSS_PI + psi * cos(azel[0]);
if (phi > 0.416)
{
phi = 0.416;
@ -4561,26 +4561,26 @@ double ionmodel(gtime_t t, const double *ion, const double *pos,
{
phi = -0.416;
}
lam = pos[1] / PI + psi * sin(azel[0]) / cos(phi * PI);
lam = pos[1] / GNSS_PI + psi * sin(azel[0]) / cos(phi * GNSS_PI);
/* geomagnetic latitude (semi-circle) */
phi += 0.064 * cos((lam - 1.617) * PI);
phi += 0.064 * cos((lam - 1.617) * GNSS_PI);
/* local time (s) */
tt = 43200.0 * lam + time2gpst(t, &week);
tt -= floor(tt / 86400.0) * 86400.0; /* 0 <= tt<86400 */
/* slant factor */
f = 1.0 + 16.0 * pow(0.53 - azel[1] / PI, 3.0);
f = 1.0 + 16.0 * pow(0.53 - azel[1] / GNSS_PI, 3.0);
/* ionospheric delay */
amp = ion[0] + phi * (ion[1] + phi * (ion[2] + phi * ion[3]));
per = ion[4] + phi * (ion[5] + phi * (ion[6] + phi * ion[7]));
amp = amp < 0.0 ? 0.0 : amp;
per = per < 72000.0 ? 72000.0 : per;
x = 2.0 * PI * (tt - 50400.0) / per;
x = 2.0 * GNSS_PI * (tt - 50400.0) / per;
return SPEED_OF_LIGHT * f * (fabs(x) < 1.57 ? 5E-9 + amp * (1.0 + x * x * (-0.5 + x * x / 24.0)) : 5E-9);
return SPEED_OF_LIGHT_M_S * f * (fabs(x) < 1.57 ? 5E-9 + amp * (1.0 + x * x * (-0.5 + x * x / 24.0)) : 5E-9);
}
@ -4596,7 +4596,7 @@ double ionmapf(const double *pos, const double *azel)
{
return 1.0;
}
return 1.0 / cos(asin((RE_WGS84 + pos[2]) / (RE_WGS84 + HION) * sin(PI / 2.0 - azel[1])));
return 1.0 / cos(asin((RE_WGS84 + pos[2]) / (RE_WGS84 + HION) * sin(GNSS_PI / 2.0 - azel[1])));
}
@ -4621,16 +4621,16 @@ double ionppp(const double *pos, const double *azel, double re,
double tanap;
rp = re / (re + hion) * cos(azel[1]);
ap = PI / 2.0 - azel[1] - asin(rp);
ap = GNSS_PI / 2.0 - azel[1] - asin(rp);
sinap = sin(ap);
tanap = tan(ap);
cosaz = cos(azel[0]);
posp[0] = asin(sin(pos[0]) * cos(ap) + cos(pos[0]) * sinap * cosaz);
if ((pos[0] > 70.0 * D2R && tanap * cosaz > tan(PI / 2.0 - pos[0])) ||
(pos[0] < -70.0 * D2R && -tanap * cosaz > tan(PI / 2.0 + pos[0])))
if ((pos[0] > 70.0 * D2R && tanap * cosaz > tan(GNSS_PI / 2.0 - pos[0])) ||
(pos[0] < -70.0 * D2R && -tanap * cosaz > tan(GNSS_PI / 2.0 + pos[0])))
{
posp[1] = pos[1] + PI - asin(sinap * sin(azel[0]) / cos(posp[0]));
posp[1] = pos[1] + GNSS_PI - asin(sinap * sin(azel[0]) / cos(posp[0]));
}
else
{
@ -4673,7 +4673,7 @@ double tropmodel(gtime_t time __attribute__((unused)), const double *pos, const
e = 6.108 * humi * exp((17.15 * temp - 4684.0) / (temp - 38.45));
/* saastamoninen model */
z = PI / 2.0 - azel[1];
z = GNSS_PI / 2.0 - azel[1];
trph = 0.0022768 * pres / (1.0 - 0.00266 * cos(2.0 * pos[0]) - 0.00028 * hgt / 1e3) / cos(z);
trpw = 0.002277 * (1255.0 / temp + 0.05) * e / cos(z);
return trph + trpw;
@ -4743,7 +4743,7 @@ double nmf(gtime_t time, const double pos[], const double azel[],
/* year from doy 28, added half a year for southern latitudes */
y = (time2doy(time) - 28.0) / 365.25 + (lat < 0.0 ? 0.5 : 0.0);
cosy = cos(2.0 * PI * y);
cosy = cos(2.0 * GNSS_PI * y);
lat = fabs(lat);
for (i = 0; i < 3; i++)
@ -4799,7 +4799,7 @@ double tropmapf(gtime_t time, const double pos[], const double azel[],
lat = pos[0];
lon = pos[1];
hgt = pos[2] - geoidh(pos); /* height in m (mean sea level) */
zd = PI / 2.0 - azel[1];
zd = GNSS_PI / 2.0 - azel[1];
/* call GMF */
gmf_(&mjd, &lat, &lon, &hgt, &zd, &gmfh, &gmfw);
@ -5390,7 +5390,7 @@ void windupcorr(gtime_t time, const double *rs, const double *rr, double *phw)
{
cosp = 1.0;
}
ph = acos(cosp) / 2.0 / PI;
ph = acos(cosp) / 2.0 / GNSS_PI;
cross3(ds, dr, drs);
if (dot(ek, drs, 3) < 0.0)
{

12
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
View File

@ -547,7 +547,7 @@ double varerr(int sat __attribute((unused)), int sys, double el, double bl, doub
double a;
double b;
double c = opt->err[3] * bl / 1e4;
double d = SPEED_OF_LIGHT * opt->sclkstab * dt;
double d = SPEED_OF_LIGHT_M_S * opt->sclkstab * dt;
double fact = 1.0;
double sinel = sin(el);
int i = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : 0);
@ -1255,8 +1255,8 @@ void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav,
return;
}
f1 = SPEED_OF_LIGHT / lam[0];
f2 = SPEED_OF_LIGHT / lam[1];
f1 = SPEED_OF_LIGHT_M_S / lam[0];
f2 = SPEED_OF_LIGHT_M_S / lam[1];
C1 = std::pow(f1, 2.0) / (std::pow(f1, 2.0) - std::pow(f2, 2.0));
C2 = -std::pow(f2, 2.0) / (std::pow(f1, 2.0) - std::pow(f2, 2.0));
dant_if = C1 * dant[0] + C2 * dant[1];
@ -1362,7 +1362,7 @@ int zdres(int base, const obsd_t *obs, int n, const double *rs,
}
/* satellite clock-bias */
r += -SPEED_OF_LIGHT * dts[i * 2];
r += -SPEED_OF_LIGHT_M_S * dts[i * 2];
/* troposphere delay model (hydrostatic) */
zhd = tropmodel(obs[0].time, pos, zazel, 0.0);
@ -1533,7 +1533,7 @@ double gloicbcorr(int sat1 __attribute((unused)), int sat2 __attribute((unused))
return 0.0;
}
dfreq = (SPEED_OF_LIGHT / lam1 - SPEED_OF_LIGHT / lam2) / (f == 0 ? DFRQ1_GLO : DFRQ2_GLO);
dfreq = (SPEED_OF_LIGHT_M_S / lam1 - SPEED_OF_LIGHT_M_S / lam2) / (f == 0 ? DFRQ1_GLO : DFRQ2_GLO);
return opt->exterr.gloicb[f] * 0.01 * dfreq; /* (m) */
}
@ -1758,7 +1758,7 @@ int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x,
/* glonass receiver h/w bias term */
if (rtk->opt.glomodear == 2 && sysi == SYS_GLO && sysj == SYS_GLO && ff < NFREQGLO)
{
df = (SPEED_OF_LIGHT / lami - SPEED_OF_LIGHT / lamj) / 1E6; /* freq-difference (MHz) */
df = (SPEED_OF_LIGHT_M_S / lami - SPEED_OF_LIGHT_M_S / lamj) / 1E6; /* freq-difference (MHz) */
v[nv] -= df * x[IL_RTK(ff, opt)];
if (H)
{

8
src/algorithms/libs/rtklib/rtklib_sbas.cc
View File

@ -1127,7 +1127,7 @@ double sbstropcorr(gtime_t time, const double *pos, const double *azel,
fabs(pos[2] - pos_[2]) > 1.0)
{
getmet(pos[0] * R2D, met);
c = cos(2.0 * PI * (time2doy(time) - (pos[0] >= 0.0 ? 28.0 : 211.0)) / 365.25);
c = cos(2.0 * GNSS_PI * (time2doy(time) - (pos[0] >= 0.0 ? 28.0 : 211.0)) / 365.25);
for (i = 0; i < 5; i++)
{
met[i] -= met[i + 5] * c;
@ -1177,7 +1177,7 @@ int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
*ddts = p->lcorr.daf0 + p->lcorr.daf1 * t;
trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n",
sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT);
sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT_M_S);
return 1;
}
@ -1278,10 +1278,10 @@ int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs,
rs[i] += drs[i];
}
dts[0] += dclk + prc / SPEED_OF_LIGHT;
dts[0] += dclk + prc / SPEED_OF_LIGHT_M_S;
trace(5, "sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n",
sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT, *var);
sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT_M_S, *var);
return 1;
}

2
src/algorithms/libs/rtklib/rtklib_tides.cc
View File

@ -190,7 +190,7 @@ void tide_oload(gtime_t tut, const double *odisp, double *denu)
a[1] = (279.69668 + 36000.768930485 * t + 3.03E-4 * t2) * D2R; /* H0 */
a[2] = (270.434358 + 481267.88314137 * t - 0.001133 * t2 + 1.9E-6 * t3) * D2R; /* S0 */
a[3] = (334.329653 + 4069.0340329577 * t - 0.010325 * t2 - 1.2E-5 * t3) * D2R; /* P0 */
a[4] = 2.0 * PI;
a[4] = 2.0 * GNSS_PI;
/* displacements by 11 constituents */
for (i = 0; i < 11; i++)

32
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
View File

@ -19,9 +19,9 @@
*/
#include "hybrid_observables_gs.h"
#include "GPS_L1_CA.h" // for GPS_STARTOFFSET_MS, GPS_TWO_PI
#include "MATH_CONSTANTS.h" // for SPEED_OF_LIGHT
#include "MATH_CONSTANTS.h" // for SPEED_OF_LIGHT_M_S, TWO_PI
#include "gnss_circular_deque.h"
#include "gnss_frequencies.h"
#include "gnss_sdr_create_directory.h"
#include "gnss_sdr_make_unique.h"
#include "gnss_synchro.h"
@ -529,7 +529,7 @@ void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data) con
traveltime_ms = 604800000.0 + current_T_rx_TOW_ms - it->interp_TOW_ms;
}
it->RX_time = current_T_rx_TOW_s;
it->Pseudorange_m = traveltime_ms * SPEED_OF_LIGHT_MS;
it->Pseudorange_m = traveltime_ms * SPEED_OF_LIGHT_M_MS;
it->Flag_valid_pseudorange = true;
// debug code
// std::cout << "[" << it->Channel_ID << "] interp_TOW_ms: " << it->interp_TOW_ms << std::endl;
@ -555,37 +555,37 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
switch (d_mapStringValues[it->Signal])
{
case evGPS_1C:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1;
break;
case evGPS_L5:
wavelength_m = SPEED_OF_LIGHT / FREQ5;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ5;
break;
case evSBAS_1C:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1;
break;
case evGAL_1B:
wavelength_m = SPEED_OF_LIGHT / FREQ1;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1;
break;
case evGAL_5X:
wavelength_m = SPEED_OF_LIGHT / FREQ5;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ5;
break;
case evGPS_2S:
wavelength_m = SPEED_OF_LIGHT / FREQ2;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2;
break;
case evBDS_B3:
wavelength_m = SPEED_OF_LIGHT / FREQ3_BDS;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ3_BDS;
break;
case evGLO_1G:
wavelength_m = SPEED_OF_LIGHT / FREQ1_GLO;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_GLO;
break;
case evGLO_2G:
wavelength_m = SPEED_OF_LIGHT / FREQ2_GLO;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_GLO;
break;
case evBDS_B1:
wavelength_m = SPEED_OF_LIGHT / FREQ1_BDS;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ1_BDS;
break;
case evBDS_B2:
wavelength_m = SPEED_OF_LIGHT / FREQ2_BDS;
wavelength_m = SPEED_OF_LIGHT_M_S / FREQ2_BDS;
break;
default:
break;
@ -599,7 +599,7 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
// Hatch filter algorithm (https://insidegnss.com/can-you-list-all-the-properties-of-the-carrier-smoothing-filter/)
double r_sm = d_channel_last_pseudorange_smooth[it->Channel_ID];
double factor = ((d_smooth_filter_M - 1.0) / d_smooth_filter_M);
it->Pseudorange_m = factor * r_sm + (1.0 / d_smooth_filter_M) * it->Pseudorange_m + wavelength_m * (factor / PI_2) * (it->Carrier_phase_rads - d_channel_last_carrier_phase_rads[it->Channel_ID]);
it->Pseudorange_m = factor * r_sm + (1.0 / d_smooth_filter_M) * it->Pseudorange_m + wavelength_m * (factor / TWO_PI) * (it->Carrier_phase_rads - d_channel_last_carrier_phase_rads[it->Channel_ID]);
}
d_channel_last_pseudorange_smooth[it->Channel_ID] = it->Pseudorange_m;
d_channel_last_carrier_phase_rads[it->Channel_ID] = it->Carrier_phase_rads;
@ -731,7 +731,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Carrier_Doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI;
tmp_double = out[i][0].Carrier_phase_rads / TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][0].Pseudorange_m;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));

4
src/algorithms/signal_generator/gnuradio_blocks/signal_generator_c.cc
View File

@ -276,7 +276,7 @@ int signal_generator_c::general_work(int noutput_items __attribute__((unused)),
for (unsigned int sat = 0; sat < num_sats_; sat++)
{
float phase_step_rad = -static_cast<float>(GPS_TWO_PI) * doppler_Hz_[sat] / static_cast<float>(fs_in_);
float phase_step_rad = -static_cast<float>(TWO_PI) * doppler_Hz_[sat] / static_cast<float>(fs_in_);
std::array<float, 1> _phase{};
_phase[0] = -start_phase_rad_[sat];
volk_gnsssdr_s32f_sincos_32fc(complex_phase_.data(), -phase_step_rad, _phase.data(), vector_length_);
@ -314,7 +314,7 @@ int signal_generator_c::general_work(int noutput_items __attribute__((unused)),
else if (system_[sat] == "R")
{
phase_step_rad = -static_cast<float>(GPS_TWO_PI) * (freq + (DFRQ1_GLO * GLONASS_PRN.at(PRN_[sat])) + doppler_Hz_[sat]) / static_cast<float>(fs_in_);
phase_step_rad = -static_cast<float>(TWO_PI) * (freq + (DFRQ1_GLO * GLONASS_PRN.at(PRN_[sat])) + doppler_Hz_[sat]) / static_cast<float>(fs_in_);
// std::cout << "sat " << PRN_[sat] << " SG - Freq = " << (freq + (DFRQ1_GLO * GLONASS_PRN.at(PRN_[sat]))) << " Doppler = " << doppler_Hz_[sat] << std::endl;
_phase[0] = -start_phase_rad_[sat];
volk_gnsssdr_s32f_sincos_32fc(complex_phase_.data(), -phase_step_rad, _phase.data(), vector_length_);

2
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc
View File

@ -760,7 +760,7 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
if (d_flag_PLL_180_deg_phase_locked == true)
{
// correct the accumulated phase for the Costas loop phase shift, if required
current_symbol.Carrier_phase_rads += GALILEO_PI;
current_symbol.Carrier_phase_rads += GNSS_PI;
}
if (d_dump == true)

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc
View File

@ -502,7 +502,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__
if (d_flag_PLL_180_deg_phase_locked == true)
{
// correct the accumulated phase for the Costas loop phase shift, if required
current_symbol.Carrier_phase_rads += GPS_PI;
current_symbol.Carrier_phase_rads += GNSS_PI;
}
if (d_dump == true)

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc
View File

@ -239,7 +239,7 @@ int gps_l2c_telemetry_decoder_gs::general_work(int noutput_items __attribute__((
if (d_flag_PLL_180_deg_phase_locked == true)
{
// correct the accumulated phase for the Costas loop phase shift, if required
current_synchro_data.Carrier_phase_rads += GPS_L2_PI;
current_synchro_data.Carrier_phase_rads += GNSS_PI;
}
current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc
View File

@ -251,7 +251,7 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u
if (d_flag_PLL_180_deg_phase_locked == true)
{
// correct the accumulated phase for the Costas loop phase shift, if required
current_synchro_data.Carrier_phase_rads += GPS_L5_PI;
current_synchro_data.Carrier_phase_rads += GNSS_PI;
}
current_synchro_data.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_synchro_data.Flag_valid_word = d_flag_valid_word;

20
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@ -589,7 +589,7 @@ void dll_pll_veml_tracking::start_tracking()
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / d_trk_parameters.fs_in;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / d_trk_parameters.fs_in;
d_carrier_phase_rate_step_rad = 0.0;
d_carr_ph_history.clear();
d_code_ph_history.clear();
@ -953,19 +953,19 @@ void dll_pll_veml_tracking::run_dll_pll()
if (d_cloop)
{
// Costas loop discriminator, insensitive to 180 deg phase transitions
d_carr_phase_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2;
d_carr_phase_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / TWO_PI;
}
else
{
// Secondary code acquired. No symbols transition should be present in the signal
d_carr_phase_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2;
d_carr_phase_error_hz = pll_four_quadrant_atan(d_P_accu) / TWO_PI;
}
if ((d_pull_in_transitory == true and d_trk_parameters.enable_fll_pull_in == true) or d_trk_parameters.enable_fll_steady_state)
{
// FLL discriminator
// d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI;
d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI;
// d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / TWO_PI;
d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / TWO_PI;
d_P_accu_old = d_P_accu;
// std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl;
@ -1084,7 +1084,7 @@ void dll_pll_veml_tracking::update_tracking_vars()
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / d_trk_parameters.fs_in;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / d_trk_parameters.fs_in;
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
if (d_trk_parameters.high_dyn)
{
@ -1105,15 +1105,15 @@ void dll_pll_veml_tracking::update_tracking_vars()
d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples;
}
}
// std::cout << d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / PI_2 << std::endl;
// std::cout << d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / TWO_PI << std::endl;
// remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad += static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2);
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
// carrier phase accumulator
// double a = d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
// double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples);
// std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl;
// std::cout << fmod(b, TWO_PI) / fmod(a, TWO_PI) << std::endl;
d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
// ################### DLL COMMANDS #################################################
@ -1317,7 +1317,7 @@ void dll_pll_veml_tracking::log_data()
tmp_float = d_carrier_doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier phase rate [Hz/s]
tmp_float = d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / PI_2;
tmp_float = d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));

20
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.cc
View File

@ -522,7 +522,7 @@ void dll_pll_veml_tracking_fpga::start_tracking()
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / d_trk_parameters.fs_in;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / d_trk_parameters.fs_in;
// filter initialization
d_carrier_loop_filter.initialize(static_cast<float>(d_acq_carrier_doppler_hz)); // initialize the carrier filter
@ -697,19 +697,19 @@ void dll_pll_veml_tracking_fpga::run_dll_pll()
if (d_cloop)
{
// Costas loop discriminator, insensitive to 180 deg phase transitions
d_carr_phase_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / PI_2;
d_carr_phase_error_hz = pll_cloop_two_quadrant_atan(d_P_accu) / TWO_PI;
}
else
{
// Secondary code acquired. No symbols transition should be present in the signal
d_carr_phase_error_hz = pll_four_quadrant_atan(d_P_accu) / PI_2;
d_carr_phase_error_hz = pll_four_quadrant_atan(d_P_accu) / TWO_PI;
}
if ((d_pull_in_transitory == true and d_trk_parameters.enable_fll_pull_in == true) or d_trk_parameters.enable_fll_steady_state)
{
// FLL discriminator
// d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI;
d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / GPS_TWO_PI;
// d_carr_freq_error_hz = fll_four_quadrant_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / TWO_PI;
d_carr_freq_error_hz = fll_diff_atan(d_P_accu_old, d_P_accu, 0, d_current_correlation_time_s) / TWO_PI;
d_P_accu_old = d_P_accu;
// std::cout << "d_carr_freq_error_hz: " << d_carr_freq_error_hz << std::endl;
@ -829,7 +829,7 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars()
// ################## PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / d_trk_parameters.fs_in;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / d_trk_parameters.fs_in;
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
if (d_trk_parameters.high_dyn)
{
@ -850,15 +850,15 @@ void dll_pll_veml_tracking_fpga::update_tracking_vars()
d_carrier_phase_rate_step_rad = (tmp_cp2 - tmp_cp1) / tmp_samples;
}
}
// std::cout << d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / PI_2 << std::endl;
// std::cout << d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / TWO_PI << std::endl;
// remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad += static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_integration_length_samples) * static_cast<double>(d_current_integration_length_samples));
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, PI_2);
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
// carrier phase accumulator
// double a = d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
// double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples);
// std::cout << fmod(b, PI_2) / fmod(a, PI_2) << std::endl;
// std::cout << fmod(b, TWO_PI) / fmod(a, TWO_PI) << std::endl;
d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast<double>(d_current_integration_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_integration_length_samples) * static_cast<double>(d_current_integration_length_samples));
// ################## DLL COMMANDS #################################################
@ -1076,7 +1076,7 @@ void dll_pll_veml_tracking_fpga::log_data()
tmp_float = d_carrier_doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier phase rate [Hz/s]
tmp_float = d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / PI_2;
tmp_float = d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));

8
src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
View File

@ -318,7 +318,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri
// perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu.set_input_output_vectors(d_correlator_outs.data(), in);
double carr_phase_step_rad = GALILEO_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
double carr_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
double code_phase_step_half_chips = (2.0 * d_code_freq_chips) / (static_cast<double>(d_fs_in));
double rem_code_phase_half_chips = d_rem_code_phase_samples * (2.0 * d_code_freq_chips / d_fs_in);
multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(
@ -356,10 +356,10 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attri
// New code Doppler frequency estimation
d_code_freq_chips = GALILEO_E1_CODE_CHIP_RATE_CPS + ((d_carrier_doppler_hz * GALILEO_E1_CODE_CHIP_RATE_CPS) / GALILEO_E1_FREQ_HZ);
// carrier phase accumulator for (K) doppler estimation
d_acc_carrier_phase_rad -= GPS_TWO_PI * d_carrier_doppler_hz * GALILEO_E1_CODE_PERIOD_S;
d_acc_carrier_phase_rad -= TWO_PI * d_carrier_doppler_hz * GALILEO_E1_CODE_PERIOD_S;
// remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GALILEO_E1_CODE_PERIOD_S;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
d_rem_carr_phase_rad = d_rem_carr_phase_rad + TWO_PI * d_carrier_doppler_hz * GALILEO_E1_CODE_PERIOD_S;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
// ################## DLL ##########################################################
// DLL discriminator, carrier loop filter implementation and NCO command generation (TCP_connector)

26
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -261,10 +261,10 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_cc::start_tracking()
// d_carrier_doppler_hz = d_acq_carrier_doppler_hz + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN));
// d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
// d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_frequency_hz = d_acq_carrier_doppler_hz + (DFRQ1_GLO * static_cast<double>(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)));
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
@ -582,8 +582,8 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter += static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GLONASS_TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GLONASS_TWO_PI;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * TWO_PI;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.fs = d_fs_in;
*out[0] = current_synchro_data;
@ -666,10 +666,10 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
// remnant code phase [chips]
d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GLONASS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), TWO_PI);
// UPDATE ACCUMULATED CARRIER PHASE
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// disable tracking loop and inform telemetry decoder
enable_dll_pll = false;
@ -695,7 +695,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
{
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GLONASS_TWO_PI; // prompt output
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / TWO_PI; // prompt output
d_carrier_doppler_old_hz = d_carrier_doppler_hz;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
@ -730,12 +730,12 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GLONASS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GLONASS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
@ -783,7 +783,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
@ -802,7 +802,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; // todo: project the carrier doppler
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
}
@ -850,7 +850,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(uint64_t));
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_cycles * GLONASS_TWO_PI;
tmp_float = d_acc_carrier_phase_cycles * TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier and code frequency
tmp_float = d_carrier_doppler_hz;

24
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -260,7 +260,7 @@ void glonass_l1_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_carrier_frequency_hz); // The carrier loop filter implements the Doppler accumulator
@ -583,8 +583,8 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter += static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GLONASS_TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GLONASS_TWO_PI;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * TWO_PI;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.fs = d_fs_in;
*out[0] = current_synchro_data;
@ -666,10 +666,10 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
// remnant code phase [chips]
d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GLONASS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), TWO_PI);
// UPDATE ACCUMULATED CARRIER PHASE
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// disable tracking loop and inform telemetry decoder
enable_dll_pll = false;
@ -694,7 +694,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
{
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())) / GLONASS_TWO_PI; // prompt output
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())) / TWO_PI; // prompt output
d_carrier_doppler_old_hz = d_carrier_doppler_hz;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
@ -729,12 +729,12 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GLONASS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GLONASS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
@ -783,7 +783,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
@ -802,7 +802,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs_16sc[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; // todo: project the carrier doppler
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
}
@ -849,7 +849,7 @@ int glonass_l1_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(uint64_t));
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_cycles * GLONASS_TWO_PI;
tmp_float = d_acc_carrier_phase_cycles * TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier and code frequency
tmp_float = d_carrier_doppler_hz;

12
src/algorithms/tracking/gnuradio_blocks/glonass_l1_ca_dll_pll_tracking_cc.cc
View File

@ -206,8 +206,8 @@ void Glonass_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_carrier_frequency_hz = d_acq_carrier_doppler_hz + (DFRQ1_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN));
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_doppler_phase_step_rad = GLONASS_TWO_PI * (d_carrier_doppler_hz) / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_doppler_phase_step_rad = TWO_PI * (d_carrier_doppler_hz) / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(); // initialize the carrier filter
@ -546,7 +546,7 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
// ################## PLL ##########################################################
// PLL discriminator
// Update PLL discriminator [rads/Ti -> Secs/Ti]
carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GLONASS_TWO_PI; // prompt output
carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / TWO_PI; // prompt output
// Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation
@ -575,11 +575,11 @@ int Glonass_L1_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_doppler_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_doppler_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
// remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GLONASS_TWO_PI);
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
// carrier phase accumulator
d_acc_carrier_phase_rad -= d_carrier_doppler_phase_step_rad * d_current_prn_length_samples;

26
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc
View File

@ -258,10 +258,10 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_cc::start_tracking()
// d_carrier_doppler_hz = d_acq_carrier_doppler_hz + (DFRQ2_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN));
// d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
// d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_frequency_hz = d_acq_carrier_doppler_hz + (DFRQ2_GLO * static_cast<double>(GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN)));
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_carrier_frequency_hz); // The carrier loop filter implements the Doppler accumulator
@ -579,8 +579,8 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter += static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GLONASS_TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GLONASS_TWO_PI;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * TWO_PI;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.fs = d_fs_in;
*out[0] = current_synchro_data;
@ -663,10 +663,10 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
// remnant code phase [chips]
d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GLONASS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), TWO_PI);
// UPDATE ACCUMULATED CARRIER PHASE
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// disable tracking loop and inform telemetry decoder
enable_dll_pll = false;
@ -692,7 +692,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
{
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GLONASS_TWO_PI; // prompt output
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / TWO_PI; // prompt output
d_carrier_doppler_old_hz = d_carrier_doppler_hz;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
@ -727,12 +727,12 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
//################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GLONASS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GLONASS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
//################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
@ -780,7 +780,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
@ -799,7 +799,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; // todo: project the carrier doppler
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
}
@ -843,7 +843,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_cc::general_work(int noutput_items __at
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(uint64_t));
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_cycles * GLONASS_TWO_PI;
tmp_float = d_acc_carrier_phase_cycles * TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier and code frequency
tmp_float = d_carrier_doppler_hz;

24
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc
View File

@ -258,7 +258,7 @@ void glonass_l2_ca_dll_pll_c_aid_tracking_sc::start_tracking()
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_carrier_frequency_hz); // The carrier loop filter implements the Doppler accumulator
@ -581,8 +581,8 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(samples_offset);
d_sample_counter += static_cast<uint64_t>(samples_offset); // count for the processed samples
d_pull_in = false;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GLONASS_TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GLONASS_TWO_PI;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / TWO_PI;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * TWO_PI;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.fs = d_fs_in;
*out[0] = current_synchro_data;
@ -664,10 +664,10 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
// remnant code phase [chips]
d_rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_chips / static_cast<double>(d_fs_in));
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), GLONASS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + d_carrier_phase_step_rad * static_cast<double>(d_correlation_length_samples), TWO_PI);
// UPDATE ACCUMULATED CARRIER PHASE
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI;
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// disable tracking loop and inform telemetry decoder
enable_dll_pll = false;
@ -692,7 +692,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
{
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())) / GLONASS_TWO_PI; // prompt output
d_carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(std::complex<float>(d_correlator_outs_16sc[1].real(), d_correlator_outs_16sc[1].imag())) / TWO_PI; // prompt output
d_carrier_doppler_old_hz = d_carrier_doppler_hz;
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
@ -727,12 +727,12 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / GLONASS_TWO_PI;
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * d_correlation_length_samples / TWO_PI;
// UPDATE ACCUMULATED CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GLONASS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GLONASS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
@ -781,7 +781,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!)
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
@ -800,7 +800,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs_16sc[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GLONASS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; // todo: project the carrier doppler
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
}
@ -847,7 +847,7 @@ int glonass_l2_ca_dll_pll_c_aid_tracking_sc::general_work(int noutput_items __at
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(uint64_t));
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_cycles * GLONASS_TWO_PI;
tmp_float = d_acc_carrier_phase_cycles * TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier and code frequency
tmp_float = d_carrier_doppler_hz;

12
src/algorithms/tracking/gnuradio_blocks/glonass_l2_ca_dll_pll_tracking_cc.cc
View File

@ -207,8 +207,8 @@ void Glonass_L2_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_carrier_frequency_hz = d_acq_carrier_doppler_hz + (DFRQ2_GLO * GLONASS_PRN.at(d_acquisition_gnss_synchro->PRN));
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_doppler_phase_step_rad = GLONASS_TWO_PI * (d_carrier_doppler_hz) / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_doppler_phase_step_rad = TWO_PI * (d_carrier_doppler_hz) / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(); // initialize the carrier filter
@ -547,7 +547,7 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
// ################## PLL ##########################################################
// PLL discriminator
// Update PLL discriminator [rads/Ti -> Secs/Ti]
carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GLONASS_TWO_PI; // prompt output
carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / TWO_PI; // prompt output
// Carrier discriminator filter
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
// New carrier Doppler frequency estimation
@ -576,11 +576,11 @@ int Glonass_L2_Ca_Dll_Pll_Tracking_cc::general_work(int noutput_items __attribut
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_doppler_phase_step_rad = GLONASS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = GLONASS_TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
d_carrier_doppler_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_frequency_hz / static_cast<double>(d_fs_in);
// remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples;
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GLONASS_TWO_PI);
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
// carrier phase accumulator
d_acc_carrier_phase_rad -= d_carrier_doppler_phase_step_rad * d_current_prn_length_samples;

12
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc
View File

@ -200,7 +200,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::start_tracking()
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz); // The carrier loop filter implements the Doppler accumulator
@ -361,7 +361,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
// ################## PLL ##########################################################
// Update PLL discriminator [rads/Ti -> Secs/Ti]
carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; // prompt output
carr_phase_error_secs_Ti = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / TWO_PI; // prompt output
// Carrier discriminator filter
// NOTICE: The carrier loop filter includes the Carrier Doppler accumulator, as described in Kaplan
// d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_phase_error_filt_secs_ti/INTEGRATION_TIME;
@ -400,14 +400,14 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
// UPDATE REMNANT CARRIER PHASE
CORRECTED_INTEGRATION_TIME_S = (static_cast<double>(d_correlation_length_samples) / static_cast<double>(d_fs_in));
// remnant carrier phase [rad]
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, GPS_TWO_PI);
d_rem_carrier_phase_rad = fmod(d_rem_carrier_phase_rad + TWO_PI * d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S, TWO_PI);
// UPDATE CARRIER PHASE ACCUULATOR
// carrier phase accumulator prior to update the PLL estimators (accumulated carrier in this loop depends on the old estimations!)
d_acc_carrier_phase_cycles -= d_carrier_doppler_hz * CORRECTED_INTEGRATION_TIME_S;
// ################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// ################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
@ -453,7 +453,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_correlation_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_phase_rads = TWO_PI * d_acc_carrier_phase_cycles;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
@ -503,7 +503,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work(int noutput_items __attribut
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(uint64_t));
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_cycles * GPS_TWO_PI;
tmp_float = d_acc_carrier_phase_cycles * TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier and code frequency
tmp_float = d_carrier_doppler_hz;

16
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc
View File

@ -185,9 +185,9 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD_S)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD_S));
// covariances (static)
double sigma2_carrier_phase = GPS_TWO_PI / 4;
double sigma2_carrier_phase = TWO_PI / 4;
double sigma2_doppler = 450;
double sigma2_doppler_rate = pow(4.0 * GPS_TWO_PI, 2) / 12.0;
double sigma2_doppler_rate = pow(4.0 * TWO_PI, 2) / 12.0;
kf_P_x_ini = arma::zeros(2, 2);
kf_P_x_ini(0, 0) = sigma2_carrier_phase;
@ -202,7 +202,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
kf_F = arma::zeros(2, 2);
kf_F(0, 0) = 1.0;
kf_F(0, 1) = GPS_TWO_PI * GPS_L1_CA_CODE_PERIOD_S;
kf_F(0, 1) = TWO_PI * GPS_L1_CA_CODE_PERIOD_S;
kf_F(1, 0) = 0.0;
kf_F(1, 1) = 1.0;
@ -225,7 +225,7 @@ Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
kf_Q(2, 2) = GPS_L1_CA_CODE_PERIOD_S;
kf_F = arma::resize(kf_F, 3, 3);
kf_F(0, 2) = 0.5 * GPS_TWO_PI * pow(GPS_L1_CA_CODE_PERIOD_S, 2);
kf_F(0, 2) = 0.5 * TWO_PI * pow(GPS_L1_CA_CODE_PERIOD_S, 2);
kf_F(1, 2) = GPS_L1_CA_CODE_PERIOD_S;
kf_F(2, 0) = 0.0;
kf_F(2, 1) = 0.0;
@ -305,7 +305,7 @@ void Gps_L1_Ca_Kf_Tracking_cc::start_tracking()
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_dopplerrate_hz2 = 0;
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// DLL filter initialization
d_code_loop_filter.initialize(); // initialize the code filter
@ -753,7 +753,7 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus
//################### NCO COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// carrier phase accumulator
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
@ -872,11 +872,11 @@ int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unus
tmp_float = d_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// Kalman commands
tmp_float = static_cast<float>(d_carr_phase_error_rad * GPS_TWO_PI);
tmp_float = static_cast<float>(d_carr_phase_error_rad * TWO_PI);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = static_cast<float>(d_carr_phase_sigma2);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = static_cast<float>(d_rem_carr_phase_rad * GPS_TWO_PI);
tmp_float = static_cast<float>(d_rem_carr_phase_rad * TWO_PI);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// DLL commands
tmp_float = code_error_chips;

2
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc
View File

@ -358,7 +358,7 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work(int noutput_items __attrib
// perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu.set_input_output_vectors(d_correlator_outs.data(), in);
double carr_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
double carr_phase_step_rad = TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
double rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in);
multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad,

4
src/algorithms/tracking/libs/tracking_discriminators.cc
View File

@ -30,11 +30,11 @@ double phase_unwrap(double phase_rad)
{
if (phase_rad >= HALF_PI)
{
return phase_rad - PI;
return phase_rad - GNSS_PI;
}
if (phase_rad <= -HALF_PI)
{
return phase_rad + PI;
return phase_rad + GNSS_PI;
}
else
{

3
src/core/system_parameters/Beidou_B1I.h
View File

@ -21,10 +21,11 @@
#ifndef GNSS_SDR_BEIDOU_B1I_H
#define GNSS_SDR_BEIDOU_B1I_H
#include "gnss_frequencies.h"
#include <cstdint>
// carrier and code frequencies
constexpr double BEIDOU_B1I_FREQ_HZ = 1.561098e9; //!< B1I [Hz]
constexpr double BEIDOU_B1I_FREQ_HZ = FREQ1_BDS; //!< B1I [Hz]
constexpr double BEIDOU_B1I_CODE_RATE_CPS = 2.046e6; //!< Beidou B1I code rate [chips/s]
constexpr double BEIDOU_B1I_CODE_LENGTH_CHIPS = 2046.0; //!< Beidou B1I code length [chips]
constexpr double BEIDOU_B1I_CODE_PERIOD_S = 0.001; //!< Beidou B1I code period [seconds]

3
src/core/system_parameters/Beidou_B3I.h
View File

@ -20,10 +20,11 @@
#ifndef GNSS_SDR_BEIDOU_B3I_H
#define GNSS_SDR_BEIDOU_B3I_H
#include "gnss_frequencies.h"
#include <cstdint>
// carrier and code frequencies
constexpr double BEIDOU_B3I_FREQ_HZ = 1.268520e9; //!< BeiDou B3I [Hz]
constexpr double BEIDOU_B3I_FREQ_HZ = FREQ3_BDS; //!< BeiDou B3I [Hz]
constexpr double BEIDOU_B3I_CODE_RATE_CPS = 10.23e6; //!< BeiDou B3I code rate [chips/s]
constexpr double BEIDOU_B3I_CODE_LENGTH_CHIPS = 10230.0; //!< BeiDou B3I code length [chips]
constexpr double BEIDOU_B3I_CODE_PERIOD_S = 0.001; //!< BeiDou B3I code period [seconds]

7
src/core/system_parameters/Beidou_DNAV.h
View File

@ -25,13 +25,6 @@
#include <utility>
#include <vector>
constexpr double BEIDOU_DNAV_C_M_S = 299792458.0; //!< The speed of light, [m/s]
constexpr double BEIDOU_DNAV_C_M_MS = 299792.4580; //!< The speed of light, [m/ms]
constexpr double BEIDOU_DNAV_PI = 3.1415926535898; //!< BeiDou DNAV Pi
constexpr double BEIDOU_DNAV_TWO_PI = 6.2831853071796; //!< BeiDou DNAV 2Pi
constexpr double BEIDOU_DNAV_OMEGA_EARTH_DOT = 7.2921150e-5; //!< Earth rotation rate, [rad/s] as defined in CGCS2000
constexpr double BEIDOU_DNAV_GM = 3.986004418e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] as defined in CGCS2000
constexpr double BEIDOU_DNAV_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)] F=-2(GM)^.5/C^2
// DNAV SCALE FACTORS
// EPH

1
src/core/system_parameters/CMakeLists.txt
View File

@ -63,7 +63,6 @@ set(SYSTEM_PARAMETERS_HEADERS
beidou_dnav_almanac.h
beidou_dnav_utc_model.h
display.h
Galileo_constants.h
Galileo_E1.h
Galileo_E5a.h
Galileo_E5b.h

54
src/core/system_parameters/GLONASS_L1_L2_CA.h
View File

@ -31,16 +31,10 @@
// Physical constants
constexpr double GLONASS_C_M_S = SPEED_OF_LIGHT; //!< The speed of light, [m/s]
constexpr double GLONASS_C_M_MS = 299792.4580; //!< The speed of light, [m/ms]
constexpr double GLONASS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200K
constexpr double GLONASS_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200K
constexpr double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s]
constexpr double GLONASS_GM = 398600.4418e9; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
constexpr double GLONASS_F_M_A = 0.35e9; //!< Gravitational constant of atmosphere [m^3/s^2]
constexpr double GLONASS_SEMI_MAJOR_AXIS = 6378136; //!< Semi-major axis of Earth [m]
constexpr double GLONASS_FLATTENING = 1.0 / 29825784.0; //!< Flattening parameter
constexpr double GLONASS_GRAVITY = 97803284; //!< Equatorial acceleration of gravity [mGal]
constexpr double GLONASS_GRAVITY = 97803284.0; //!< Equatorial acceleration of gravity [mGal]
constexpr double GLONASS_GRAVITY_CORRECTION = 0.87; //!< Correction to acceleration of gravity at sea-level due to Atmosphere[uGal]
constexpr double GLONASS_J2 = 1082625.75e-9; //!< Second zonal harmonic of the geopotential
constexpr double GLONASS_J4 = -2370.89e-9; //!< Fourth zonal harmonic of the geopotential
@ -76,7 +70,7 @@ constexpr double GLONASS_L2_CA_CODE_RATE_CPS = 0.511e6; //!< GLONASS L1 C/A
constexpr double GLONASS_L2_CA_CODE_LENGTH_CHIPS = 511.0; //!< GLONASS L1 C/A code length [chips]
constexpr double GLONASS_L2_CA_CODE_PERIOD_S = 0.001; //!< GLONASS L1 C/A code period [seconds]
constexpr double GLONASS_L2_CA_CHIP_PERIOD_S = 1.9569e-06; //!< GLONASS L1 C/A chip period [seconds]
constexpr double GLONASS_L2_CA_SYMBOL_RATE_BPS = 1000;
constexpr double GLONASS_L2_CA_SYMBOL_RATE_BPS = 1000.0;
constexpr double GLONASS_L1_CA_FREQ_HZ = FREQ1_GLO; //!< L1 [Hz]
constexpr double GLONASS_L1_CA_DFREQ_HZ = DFRQ1_GLO; //!< Freq Bias for GLONASS L1 [Hz]
@ -84,10 +78,31 @@ constexpr double GLONASS_L1_CA_CODE_RATE_CPS = 0.511e6; //!< GLONASS L1 C/A
constexpr double GLONASS_L1_CA_CODE_LENGTH_CHIPS = 511.0; //!< GLONASS L1 C/A code length [chips]
constexpr double GLONASS_L1_CA_CODE_PERIOD_S = 0.001; //!< GLONASS L1 C/A code period [seconds]
constexpr double GLONASS_L1_CA_CHIP_PERIOD_S = 1.9569e-06; //!< GLONASS L1 C/A chip period [seconds]
constexpr double GLONASS_L1_CA_SYMBOL_RATE_BPS = 1000;
constexpr double GLONASS_L1_CA_SYMBOL_RATE_BPS = 1000.0;
constexpr int32_t GLONASS_CA_NBR_SATS = 24; // STRING DATA WITHOUT PREAMBLE
// OBSERVABLE HISTORY DEEP FOR INTERPOLATION
constexpr int32_t GLONASS_L1_CA_HISTORY_DEEP = 100;
// NAVIGATION MESSAGE DEMODULATION AND DECODING
#define GLONASS_GNAV_PREAMBLE \
{ \
1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0 \
}
constexpr double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.300;
constexpr int32_t GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30;
constexpr int32_t GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300;
constexpr int32_t GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 2000;
constexpr int32_t GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s]
constexpr int32_t GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT = 10;
constexpr int32_t GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT = 10;
constexpr int32_t GLONASS_GNAV_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND * GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s]
constexpr int32_t GLONASS_GNAV_STRING_SYMBOLS = 2000; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits]
constexpr int32_t GLONASS_GNAV_STRING_BITS = 85; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits]
constexpr int32_t GLONASS_GNAV_HAMMING_CODE_BITS = 8; //!< Number of bits in hamming code sequence of GNAV message
constexpr int32_t GLONASS_GNAV_DATA_SYMBOLS = 1700; // STRING DATA WITHOUT PREAMBLE
/*!
* \brief Record of leap seconds definition for GLOT to GPST conversion and vice versa
* \details Each entry is defined by an array of 7 elements consisting of yr,month,day,hr,min,sec,utc-gpst
@ -214,27 +229,6 @@ const std::map<uint32_t, int32_t> GLONASS_PRN = {
}, // Plane 3
{24, 2}}; // Plane 3
// OBSERVABLE HISTORY DEEP FOR INTERPOLATION
constexpr int32_t GLONASS_L1_CA_HISTORY_DEEP = 100;
// NAVIGATION MESSAGE DEMODULATION AND DECODING
#define GLONASS_GNAV_PREAMBLE \
{ \
1, 1, 1, 1, 1, 0, 0, 0, 1, 1, 0, 1, 1, 1, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 1, 0, 1, 1, 0 \
}
constexpr double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.300;
constexpr int32_t GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30;
constexpr int32_t GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300;
constexpr int32_t GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 2000;
constexpr int32_t GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s]
constexpr int32_t GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT = 10;
constexpr int32_t GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_PREAMBLE_BIT = 10;
constexpr int32_t GLONASS_GNAV_TELEMETRY_RATE_SYMBOLS_SECOND = GLONASS_GNAV_TELEMETRY_RATE_BITS_SECOND * GLONASS_GNAV_TELEMETRY_SYMBOLS_PER_BIT; //!< NAV message bit rate [symbols/s]
constexpr int32_t GLONASS_GNAV_STRING_SYMBOLS = 2000; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits]
constexpr int32_t GLONASS_GNAV_STRING_BITS = 85; //!< Number of bits per string in the GNAV message (85 data bits + 30 time mark bits) [bits]
constexpr int32_t GLONASS_GNAV_HAMMING_CODE_BITS = 8; //!< Number of bits in hamming code sequence of GNAV message
constexpr int32_t GLONASS_GNAV_DATA_SYMBOLS = 1700; // STRING DATA WITHOUT PREAMBLE
const std::vector<int32_t> GLONASS_GNAV_CRC_I_INDEX{9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43, 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84};
const std::vector<int32_t> GLONASS_GNAV_CRC_J_INDEX{9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84};
const std::vector<int32_t> GLONASS_GNAV_CRC_K_INDEX{10, 11, 12, 16, 17, 18, 19, 23, 24, 25, 26, 31, 32, 33, 34, 38, 39, 40, 41, 46, 47, 48, 49, 54, 55, 56, 57, 62, 63, 64, 65, 69, 70, 71, 72, 77, 78, 79, 80, 85};

16
src/core/system_parameters/GPS_CNAV.h
View File

@ -56,15 +56,15 @@ const std::vector<std::pair<int32_t, int32_t> > CNAV_A_DOT({{108, 25}});
constexpr double CNAV_A_DOT_LSB = TWO_N21;
const std::vector<std::pair<int32_t, int32_t> > CNAV_DELTA_N0({{133, 17}});
constexpr double CNAV_DELTA_N0_LSB = TWO_N44 * PI; // semi-circles to radians
constexpr double CNAV_DELTA_N0_LSB = TWO_N44 * GNSS_PI; // semi-circles to radians
const std::vector<std::pair<int32_t, int32_t> > CNAV_DELTA_N0_DOT({{150, 23}});
constexpr double CNAV_DELTA_N0_DOT_LSB = TWO_N57 * PI; // semi-circles to radians
constexpr double CNAV_DELTA_N0_DOT_LSB = TWO_N57 * GNSS_PI; // semi-circles to radians
const std::vector<std::pair<int32_t, int32_t> > CNAV_M0({{173, 33}});
constexpr double CNAV_M0_LSB = TWO_N32 * PI; // semi-circles to radians
constexpr double CNAV_M0_LSB = TWO_N32 * GNSS_PI; // semi-circles to radians
const std::vector<std::pair<int32_t, int32_t> > CNAV_E_ECCENTRICITY({{206, 33}});
constexpr double CNAV_E_ECCENTRICITY_LSB = TWO_N34;
const std::vector<std::pair<int32_t, int32_t> > CNAV_OMEGA({{239, 33}});
constexpr double CNAV_OMEGA_LSB = TWO_N32 * PI; // semi-circles to radians
constexpr double CNAV_OMEGA_LSB = TWO_N32 * GNSS_PI; // semi-circles to radians
const std::vector<std::pair<int32_t, int32_t> > CNAV_INTEGRITY_FLAG({{272, 1}});
const std::vector<std::pair<int32_t, int32_t> > CNAV_L2_PHASING_FLAG({{273, 1}});
@ -72,13 +72,13 @@ const std::vector<std::pair<int32_t, int32_t> > CNAV_L2_PHASING_FLAG({{273, 1}})
const std::vector<std::pair<int32_t, int32_t> > CNAV_TOE2({{39, 11}});
constexpr int32_t CNAV_TOE2_LSB = 300;
const std::vector<std::pair<int32_t, int32_t> > CNAV_OMEGA0({{50, 33}});
constexpr double CNAV_OMEGA0_LSB = TWO_N32 * PI; // semi-circles to radians
constexpr double CNAV_OMEGA0_LSB = TWO_N32 * GNSS_PI; // semi-circles to radians
const std::vector<std::pair<int32_t, int32_t> > CNAV_I0({{83, 33}});
constexpr double CNAV_I0_LSB = TWO_N32 * PI; // semi-circles to radians
constexpr double CNAV_I0_LSB = TWO_N32 * GNSS_PI; // semi-circles to radians
const std::vector<std::pair<int32_t, int32_t> > CNAV_DELTA_OMEGA_DOT({{116, 17}}); // Relative to REF = -2.6 x 10-9 semi-circles/second.
constexpr double CNAV_DELTA_OMEGA_DOT_LSB = TWO_N44 * PI; // semi-circles to radians
constexpr double CNAV_DELTA_OMEGA_DOT_LSB = TWO_N44 * GNSS_PI; // semi-circles to radians
const std::vector<std::pair<int32_t, int32_t> > CNAV_I0_DOT({{133, 15}});
constexpr double CNAV_I0_DOT_LSB = TWO_N44 * PI; // semi-circles to radians
constexpr double CNAV_I0_DOT_LSB = TWO_N44 * GNSS_PI; // semi-circles to radians
const std::vector<std::pair<int32_t, int32_t> > CNAV_CIS({{148, 16}});
constexpr double CNAV_CIS_LSB = TWO_N30;
const std::vector<std::pair<int32_t, int32_t> > CNAV_CIC({{164, 16}});

10
src/core/system_parameters/GPS_L1_CA.h
View File

@ -28,16 +28,6 @@
#include <vector>
// Physical constants
constexpr double GPS_C_M_S = SPEED_OF_LIGHT; //!< The speed of light, [m/s]
constexpr double GPS_C_M_MS = 299792.4580; //!< The speed of light, [m/ms]
constexpr double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200K
constexpr double GPS_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200K
constexpr double OMEGA_EARTH_DOT = DEFAULT_OMEGA_EARTH_DOT; //!< Earth rotation rate, [rad/s]
constexpr double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
constexpr double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)]
// carrier and code frequencies
constexpr double GPS_L1_FREQ_HZ = FREQ1; //!< L1 [Hz]
constexpr double GPS_L1_CA_CODE_RATE_CPS = 1.023e6; //!< GPS L1 C/A code rate [chips/s]

34
src/core/system_parameters/GPS_L2C.h
View File

@ -30,32 +30,25 @@
#include <vector>
// Physical constants
constexpr double GPS_L2_C_M_S = 299792458.0; //!< The speed of light, [m/s]
constexpr double GPS_L2_C_M_MS = 299792.4580; //!< The speed of light, [m/ms]
constexpr double GPS_L2_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200K
constexpr double GPS_L2_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200K
constexpr double GPS_L2_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s]
constexpr double GPS_L2_GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
constexpr double GPS_L2_F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)]
// carrier and code frequencies
constexpr double GPS_L2_FREQ_HZ = FREQ2; //!< L2 [Hz]
constexpr double GPS_L2_M_CODE_RATE_CPS = 0.5115e6; //!< GPS L2 M code rate [chips/s]
constexpr int32_t GPS_L2_M_CODE_LENGTH_CHIPS = 10230; //!< GPS L2 M code length [chips]
constexpr double GPS_L2_M_PERIOD_S = 0.02; //!< GPS L2 M code period [seconds]
constexpr double GPS_L2_L_CODE_RATE_CPS = 0.5115e6; //!< GPS L2 L code rate [chips/s]
constexpr int32_t GPS_L2_L_CODE_LENGTH_CHIPS = 767250; //!< GPS L2 L code length [chips]
constexpr double GPS_L2_FREQ_HZ = FREQ2; //!< L2 [Hz]
constexpr double GPS_L2_L_PERIOD_S = 1.5; //!< GPS L2 L code period [seconds]
constexpr double GPS_L2_M_CODE_RATE_CPS = 0.5115e6; //!< GPS L2 M code rate [chips/s]
constexpr double GPS_L2_M_PERIOD_S = 0.02; //!< GPS L2 M code period [seconds]
constexpr double GPS_L2_L_CODE_RATE_CPS = 0.5115e6; //!< GPS L2 L code rate [chips/s]
constexpr int32_t GPS_L2_M_CODE_LENGTH_CHIPS = 10230; //!< GPS L2 M code length [chips]
constexpr int32_t GPS_L2_L_CODE_LENGTH_CHIPS = 767250; //!< GPS L2 L code length [chips]
constexpr int32_t GPS_L2_CNAV_DATA_PAGE_BITS = 300; //!< GPS L2 CNAV page length, including preamble and CRC [bits]
constexpr int32_t GPS_L2_SYMBOLS_PER_BIT = 2;
constexpr int32_t GPS_L2_SAMPLES_PER_SYMBOL = 1;
constexpr int32_t GPS_L2_CNAV_DATA_PAGE_SYMBOLS = 600;
constexpr int32_t GPS_L2_CNAV_DATA_PAGE_DURATION_S = 12;
constexpr int32_t GPS_L2C_HISTORY_DEEP = 5;
// optimum parameters
constexpr uint32_t GPS_L2C_OPT_ACQ_FS_SPS = 2000000; //!< Sampling frequency that maximizes the acquisition SNR while using a non-multiple of chip rate
constexpr int32_t GPS_L2C_M_INIT_REG[115] =
{0742417664, 0756014035, 0002747144, 0066265724, // 1:4
0601403471, 0703232733, 0124510070, 0617316361, // 5:8
@ -87,10 +80,5 @@ constexpr int32_t GPS_L2C_M_INIT_REG[115] =
0706202440, 0705056276, 0020373522, 0746013617,
0132720621, 0434015513, 0566721727, 0140633660};
constexpr int32_t GPS_L2_CNAV_DATA_PAGE_BITS = 300; //!< GPS L2 CNAV page length, including preamble and CRC [bits]
constexpr int32_t GPS_L2_SYMBOLS_PER_BIT = 2;
constexpr int32_t GPS_L2_SAMPLES_PER_SYMBOL = 1;
constexpr int32_t GPS_L2_CNAV_DATA_PAGE_SYMBOLS = 600;
constexpr int32_t GPS_L2_CNAV_DATA_PAGE_DURATION_S = 12;
#endif // GNSS_SDR_GPS_L2C_H

27
src/core/system_parameters/GPS_L5.h
View File

@ -26,31 +26,18 @@
#include <cstdint>
// Physical constants
constexpr double GPS_L5_C_M_S = 299792458.0; //!< The speed of light, [m/s]
constexpr double GPS_L5_C_M_MS = 299792.4580; //!< The speed of light, [m/ms]
constexpr double GPS_L5_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200K
constexpr double GPS_L5_TWO_PI = 6.283185307179586; //!< 2Pi as defined in IS-GPS-200K
constexpr double GPS_L5_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s]
constexpr double GPS_L5_GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
constexpr double GPS_L5_F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)]
// carrier and code frequencies
constexpr double GPS_L5_FREQ_HZ = FREQ5; //!< L5 [Hz]
constexpr double GPS_L5I_CODE_RATE_CPS = 10.23e6; //!< GPS L5I code rate [chips/s]
constexpr double GPS_L5I_PERIOD_S = 0.001; //!< GPS L5I code period [seconds]
constexpr double GPS_L5I_SYMBOL_PERIOD_S = 0.01; //!< GPS L5I symbol period [seconds]
constexpr double GPS_L5Q_CODE_RATE_CPS = 10.23e6; //!< GPS L5Q code rate [chips/s]
constexpr double GPS_L5Q_PERIOD_S = 0.001; //!< GPS L5Q code period [seconds]
constexpr double GPS_L5_FREQ_HZ = FREQ5; //!< L5 [Hz]
constexpr double GPS_L5I_CODE_RATE_CPS = 10.23e6; //!< GPS L5I code rate [chips/s]
constexpr double GPS_L5I_PERIOD_S = 0.001; //!< GPS L5I code period [seconds]
constexpr double GPS_L5I_SYMBOL_PERIOD_S = 0.01; //!< GPS L5I symbol period [seconds]
constexpr double GPS_L5Q_CODE_RATE_CPS = 10.23e6; //!< GPS L5Q code rate [chips/s]
constexpr double GPS_L5Q_PERIOD_S = 0.001; //!< GPS L5Q code period [seconds]
constexpr int32_t GPS_L5Q_CODE_LENGTH_CHIPS = 10230; //!< GPS L5Q code length [chips]
constexpr int32_t GPS_L5I_CODE_LENGTH_CHIPS = 10230; //!< GPS L5I code length [chips]
constexpr int32_t GPS_L5I_PERIOD_MS = 1; //!< GPS L5I code period [ms]
constexpr int32_t GPS_L5I_SYMBOL_PERIOD_MS = 10; //!< GPS L5I symbol period [ms]
constexpr int32_t GPS_L5Q_CODE_LENGTH_CHIPS = 10230; //!< GPS L5Q code length [chips]
constexpr int32_t GPS_L5_HISTORY_DEEP = 5;
// optimum parameters

1
src/core/system_parameters/Galileo_E1.h
View File

@ -22,7 +22,6 @@
#ifndef GNSS_SDR_GALILEO_E1_H
#define GNSS_SDR_GALILEO_E1_H
#include "Galileo_constants.h"
#include "MATH_CONSTANTS.h"
#include "gnss_frequencies.h"
#include <cstddef> // for size_t

33
src/core/system_parameters/Galileo_constants.h
View File

@ -1,33 +0,0 @@
/*!
* \file Galileo_constants.h
* \brief Defines constants for Galileo
* \author Carles Fernandez-Prades, 2020. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GALILEO_CONSTANTS_H
#define GNSS_SDR_GALILEO_CONSTANTS_H
// Physical constants for Galileo
constexpr double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD
constexpr double GALILEO_TWO_PI = 6.283185307179600; //!< 2*Pi as defined in GALILEO ICD
constexpr double GALILEO_GM = 3.986004418e14; //!< Geocentric gravitational constant[m^3/s^2]
constexpr double GALILEO_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Mean angular velocity of the Earth [rad/s]
constexpr double GALILEO_C_M_S = 299792458.0; //!< The speed of light, [m/s]
constexpr double GALILEO_C_M_MS = 299792.4580; //!< The speed of light, [m/ms]
constexpr double GALILEO_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)]
#endif // GNSS_SDR_GALILEO_CONSTANTS_H

170
src/core/system_parameters/MATH_CONSTANTS.h
View File

@ -20,91 +20,105 @@
#ifndef GNSS_SDR_MATH_CONSTANTS_H
#define GNSS_SDR_MATH_CONSTANTS_H
#include <string>
constexpr double GNSS_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Default Earth rotation rate, [rad/s]
constexpr double SPEED_OF_LIGHT_M_S = 299792458.0; //!< Speed of light in vacuum [m/s]
constexpr double SPEED_OF_LIGHT_M_MS = 299792.4580; //!< Speed of light in vacuum [m/ms]
/* Constants for scaling the ephemeris found in the data message
the format is the following: TWO_N5 -> 2^-5, TWO_P4 -> 2^4, PI_TWO_N43 -> Pi*2^-43, etc etc
Additionally some of the PI*2^N terms are used in the tracking stuff
TWO_PX ==> 2^X
TWO_NX ==> 2^-X
PI_TWO_NX ==> Pi*2^-X
PI_TWO_PX ==> Pi*2^X
ONE_PI_TWO_PX = (1/Pi)*2^X
*/
// Physical constants for GPS
constexpr double GPS_GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] IS-GPS-200K, pag 92
constexpr double GPS_F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)], IS-GPS-200K, pag. 92
constexpr double HALF_PI = 1.570796326794897; //!< pi/2
constexpr double PI = 3.1415926535897932; //!< pi
constexpr double PI_2 = 2.0 * PI; //!< 2 * pi
// Physical constants for Galileo
constexpr double GALILEO_GM = 3.986004418e14; //!< Geocentric gravitational constant[m^3/s^2], OS SIS ICD v1.3, pag. 44
constexpr double GALILEO_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)]. OS SIS ICD v1.3, pag. 47
constexpr double TWO_P3 = (8); //!< 2^3
constexpr double TWO_P4 = (16); //!< 2^4
constexpr double TWO_P11 = (2048); //!< 2^11
constexpr double TWO_P12 = (4096); //!< 2^12
constexpr double TWO_P14 = (16384); //!< 2^14
constexpr double TWO_P16 = (65536); //!< 2^16
constexpr double TWO_P19 = (524288); //!< 2^19
constexpr double TWO_P31 = (2147483648.0); //!< 2^31
constexpr double TWO_P32 = (4294967296.0); //!< 2^32 this is too big for an int so add the x.0
constexpr double TWO_P56 = (7.205759403792794e+016); //!< 2^56
constexpr double TWO_P57 = (1.441151880758559e+017); //!< 2^57
// Physical constants for GLONASS
constexpr double GLONASS_OMEGA_EARTH_DOT = 7.292115e-5; //!< Earth rotation rate, [rad/s] ICD L1, L2 GLONASS Edition 5.1 2008 pag. 55
constexpr double GLONASS_GM = 398600.44e9; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2]
constexpr double TWO_N2 = (0.25); //!< 2^-2
constexpr double TWO_N5 = (0.03125); //!< 2^-5
constexpr double TWO_N6 = (0.015625); //!< 2^-6
constexpr double TWO_N8 = (0.00390625); //!< 2^-8
constexpr double TWO_N9 = (0.001953125); //!< 2^-9
constexpr double TWO_N10 = (0.0009765625); //!< 2^-10
constexpr double TWO_N11 = (4.882812500000000e-004); //!< 2^-11
constexpr double TWO_N14 = (0.00006103515625); //!< 2^-14
constexpr double TWO_N15 = (0.00003051757813); //!< 2^-15
constexpr double TWO_N16 = (0.0000152587890625); //!< 2^-16
constexpr double TWO_N17 = (7.629394531250000e-006); //!< 2^-17
constexpr double TWO_N18 = (3.814697265625000e-006); //!< 2^-18
constexpr double TWO_N19 = (1.907348632812500e-006); //!< 2^-19
constexpr double TWO_N20 = (9.536743164062500e-007); //!< 2^-20
constexpr double TWO_N21 = (4.768371582031250e-007); //!< 2^-21
constexpr double TWO_N23 = (1.192092895507810e-007); //!< 2^-23
constexpr double TWO_N24 = (5.960464477539063e-008); //!< 2^-24
constexpr double TWO_N25 = (2.980232238769531e-008); //!< 2^-25
constexpr double TWO_N27 = (7.450580596923828e-009); //!< 2^-27
constexpr double TWO_N29 = (1.862645149230957e-009); //!< 2^-29
constexpr double TWO_N30 = (9.313225746154785e-010); //!< 2^-30
constexpr double TWO_N31 = (4.656612873077393e-010); //!< 2^-31
constexpr double TWO_N32 = (2.328306436538696e-010); //!< 2^-32
constexpr double TWO_N33 = (1.164153218269348e-010); //!< 2^-33
constexpr double TWO_N34 = (5.82076609134674e-011); //!< 2^-34
constexpr double TWO_N35 = (2.91038304567337e-011); //!< 2^-35
constexpr double TWO_N38 = (3.637978807091713e-012); //!< 2^-38
constexpr double TWO_N39 = (1.818989403545856e-012); //!< 2^-39
constexpr double TWO_N40 = (9.094947017729280e-013); //!< 2^-40
constexpr double TWO_N43 = (1.136868377216160e-013); //!< 2^-43
constexpr double TWO_N44 = (5.684341886080802e-14); //!< 2^-44
constexpr double TWO_N46 = (1.4210854715202e-014); //!< 2^-46
constexpr double TWO_N48 = (3.552713678800501e-15); //!< 2^-46
// Physical constants for Beidou
constexpr double BEIDOU_OMEGA_EARTH_DOT = 7.2921150e-5; //!< Earth rotation rate, [rad/s] as defined in BDS-SIS-ICD-B1I-3.0 2019-02, pag. 3
constexpr double BEIDOU_GM = 3.986004418e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] as defined in CGCS2000
constexpr double BEIDOU_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)] F=-2(GM)^.5/C^2
constexpr double TWO_N50 = (8.881784197001252e-016); //!< 2^-50
constexpr double TWO_N51 = (4.44089209850063e-016); //!< 2^-51
constexpr double TWO_N55 = (2.775557561562891e-017); //!< 2^-55
constexpr double TWO_N57 = (6.938893903907228e-18); //!< 2^-57
constexpr double TWO_N59 = (1.73472347597681e-018); //!< 2^-59
constexpr double TWO_N60 = (8.673617379884036e-19); //!< 2^-60
constexpr double TWO_N66 = (1.3552527156068805425093160010874271392822265625e-20); //!< 2^-66
constexpr double TWO_N68 = (3.388131789017201e-21); //!< 2^-68
constexpr double GNSS_PI = 3.1415926535898; //!< pi constant as defined for GNSS
constexpr double HALF_PI = GNSS_PI / 2.0; //!< pi/2
constexpr double TWO_PI = 2.0 * GNSS_PI; //!< 2 * pi
constexpr double PI_TWO_N19 = (5.992112452678286e-006); //!< Pi*2^-19
constexpr double PI_TWO_N43 = (3.571577341960839e-013); //!< Pi*2^-43
constexpr double PI_TWO_N31 = (1.462918079267160e-009); //!< Pi*2^-31
constexpr double PI_TWO_N38 = (1.142904749427469e-011); //!< Pi*2^-38
constexpr double PI_TWO_N23 = (3.745070282923929e-007); //!< Pi*2^-23
constexpr double D2R = (PI / 180.0); //!< deg to rad
constexpr double R2D = (180.0 / PI); //!< rad to deg
constexpr double SC2RAD = 3.1415926535898; //!< semi-circle to radian (IS-GPS)
constexpr double AS2R = (D2R / 3600.0); //!< arc sec to radian
// Constants for scaling the ephemeris found in the data message
// the format is the following: TWO_N5 -> 2^-5, TWO_P4 -> 2^4, PI_TWO_N43 -> Pi*2^-43, etc etc
// Additionally some of the PI*2^N terms are used in the tracking stuff
// TWO_PX ==> 2^X
// TWO_NX ==> 2^-X
// PI_TWO_NX ==> Pi*2^-X
constexpr double DEFAULT_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Default Earth rotation rate, [rad/s]
constexpr double SPEED_OF_LIGHT = 299792458.0; //!< [m/s]
constexpr double SPEED_OF_LIGHT_MS = 299792.4580; //!< [m/ms]
constexpr double AU = 149597870691.0; //!< 1 Astronomical Unit AU (m) distance from Earth to the Sun.
constexpr double TWO_P3 = 8.0; //!< 2^3
constexpr double TWO_P4 = 16.0; //!< 2^4
constexpr double TWO_P11 = 2048.0; //!< 2^11
constexpr double TWO_P12 = 4096.0; //!< 2^12
constexpr double TWO_P14 = 16384.0; //!< 2^14
constexpr double TWO_P16 = 65536.0; //!< 2^16
constexpr double TWO_P19 = 524288.0; //!< 2^19
constexpr double TWO_P31 = 2147483648.0; //!< 2^31
constexpr double TWO_P32 = 4294967296.0; //!< 2^32
constexpr double TWO_P56 = 7.205759403792794e+016; //!< 2^56
constexpr double TWO_P57 = 1.441151880758559e+017; //!< 2^57
constexpr double TWO_N2 = 0.25; //!< 2^-2
constexpr double TWO_N5 = 0.03125; //!< 2^-5
constexpr double TWO_N6 = 0.015625; //!< 2^-6
constexpr double TWO_N8 = 0.00390625; //!< 2^-8
constexpr double TWO_N9 = 0.001953125; //!< 2^-9
constexpr double TWO_N10 = 0.0009765625; //!< 2^-10
constexpr double TWO_N11 = 4.882812500000000e-004; //!< 2^-11
constexpr double TWO_N14 = 0.00006103515625; //!< 2^-14
constexpr double TWO_N15 = 0.00003051757813; //!< 2^-15
constexpr double TWO_N16 = 0.0000152587890625; //!< 2^-16
constexpr double TWO_N17 = 7.629394531250000e-006; //!< 2^-17
constexpr double TWO_N18 = 3.814697265625000e-006; //!< 2^-18
constexpr double TWO_N19 = 1.907348632812500e-006; //!< 2^-19
constexpr double TWO_N20 = 9.536743164062500e-007; //!< 2^-20
constexpr double TWO_N21 = 4.768371582031250e-007; //!< 2^-21
constexpr double TWO_N23 = 1.192092895507810e-007; //!< 2^-23
constexpr double TWO_N24 = 5.960464477539063e-008; //!< 2^-24
constexpr double TWO_N25 = 2.980232238769531e-008; //!< 2^-25
constexpr double TWO_N27 = 7.450580596923828e-009; //!< 2^-27
constexpr double TWO_N29 = 1.862645149230957e-009; //!< 2^-29
constexpr double TWO_N30 = 9.313225746154785e-010; //!< 2^-30
constexpr double TWO_N31 = 4.656612873077393e-010; //!< 2^-31
constexpr double TWO_N32 = 2.328306436538696e-010; //!< 2^-32
constexpr double TWO_N33 = 1.164153218269348e-010; //!< 2^-33
constexpr double TWO_N34 = 5.82076609134674e-011; //!< 2^-34
constexpr double TWO_N35 = 2.91038304567337e-011; //!< 2^-35
constexpr double TWO_N38 = 3.637978807091713e-012; //!< 2^-38
constexpr double TWO_N39 = 1.818989403545856e-012; //!< 2^-39
constexpr double TWO_N40 = 9.094947017729280e-013; //!< 2^-40
constexpr double TWO_N43 = 1.136868377216160e-013; //!< 2^-43
constexpr double TWO_N44 = 5.684341886080802e-14; //!< 2^-44
constexpr double TWO_N46 = 1.4210854715202e-014; //!< 2^-46
constexpr double TWO_N48 = 3.552713678800501e-15; //!< 2^-46
constexpr double TWO_N50 = 8.881784197001252e-016; //!< 2^-50
constexpr double TWO_N51 = 4.44089209850063e-016; //!< 2^-51
constexpr double TWO_N55 = 2.775557561562891e-017; //!< 2^-55
constexpr double TWO_N57 = 6.938893903907228e-18; //!< 2^-57
constexpr double TWO_N59 = 1.73472347597681e-018; //!< 2^-59
constexpr double TWO_N60 = 8.673617379884036e-19; //!< 2^-60
constexpr double TWO_N66 = 1.3552527156068805425093160010874271392822265625e-20; //!< 2^-66
constexpr double TWO_N68 = 3.388131789017201e-21; //!< 2^-68
constexpr double PI_TWO_N19 = 5.992112452678286e-006; //!< Pi*2^-19
constexpr double PI_TWO_N43 = 3.571577341960839e-013; //!< Pi*2^-43
constexpr double PI_TWO_N31 = 1.462918079267160e-009; //!< Pi*2^-31
constexpr double PI_TWO_N38 = 1.142904749427469e-011; //!< Pi*2^-38
constexpr double PI_TWO_N23 = 3.745070282923929e-007; //!< Pi*2^-23
constexpr double D2R = (GNSS_PI / 180.0); //!< deg to rad
constexpr double R2D = (180.0 / GNSS_PI); //!< rad to deg
constexpr double SC2RAD = GNSS_PI; //!< semi-circle to radian (IS-GPS)
constexpr double AS2R = (D2R / 3600.0); //!< arc sec to radian
constexpr double AU = 149597870691.0; //!< 1 Astronomical Unit AU (m) distance from Earth to the Sun.
#endif // GNSS_SDR_MATH_CONSTANTS_H

24
src/core/system_parameters/beidou_dnav_ephemeris.cc
View File

@ -85,14 +85,14 @@ double Beidou_Dnav_Ephemeris::sv_clock_relativistic_term(double transmitTime)
tk = check_t(transmitTime - d_Toe);
// Computed mean motion
n0 = sqrt(BEIDOU_DNAV_GM / (a * a * a));
n0 = sqrt(BEIDOU_GM / (a * a * a));
// Corrected mean motion
n = n0 + d_Delta_n;
// Mean anomaly
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2.0 * BEIDOU_DNAV_PI), (2.0 * BEIDOU_DNAV_PI));
M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
@ -102,7 +102,7 @@ double Beidou_Dnav_Ephemeris::sv_clock_relativistic_term(double transmitTime)
{
E_old = E;
E = M + d_eccentricity * sin(E);
dE = fmod(E - E_old, 2.0 * BEIDOU_DNAV_PI);
dE = fmod(E - E_old, 2.0 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -111,7 +111,7 @@ double Beidou_Dnav_Ephemeris::sv_clock_relativistic_term(double transmitTime)
}
// Compute relativistic correction term
d_dtr = BEIDOU_DNAV_F * d_eccentricity * d_sqrt_A * sin(E);
d_dtr = BEIDOU_F * d_eccentricity * d_sqrt_A * sin(E);
return d_dtr;
}
@ -142,7 +142,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
tk = check_t(transmitTime - d_Toe);
// Computed mean motion
n0 = sqrt(BEIDOU_DNAV_GM / (a * a * a));
n0 = sqrt(BEIDOU_GM / (a * a * a));
// Corrected mean motion
n = n0 + d_Delta_n;
@ -151,7 +151,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2.0 * BEIDOU_DNAV_PI), (2.0 * BEIDOU_DNAV_PI));
M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
@ -161,7 +161,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
{
E_old = E;
E = M + d_eccentricity * sin(E);
dE = fmod(E - E_old, 2.0 * BEIDOU_DNAV_PI);
dE = fmod(E - E_old, 2.0 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -178,7 +178,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2.0 * BEIDOU_DNAV_PI));
phi = fmod((phi), (2.0 * GNSS_PI));
// Correct argument of latitude
u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi);
@ -190,10 +190,10 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi);
// Compute the angle between the ascending node and the Greenwich meridian
Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_DNAV_OMEGA_EARTH_DOT) * tk - BEIDOU_DNAV_OMEGA_EARTH_DOT * d_Toe;
Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe;
// Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2.0 * BEIDOU_DNAV_PI), (2.0 * BEIDOU_DNAV_PI));
Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@ -201,7 +201,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = d_OMEGA_DOT - BEIDOU_DNAV_OMEGA_EARTH_DOT;
double Omega_dot = d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i);
@ -212,7 +212,7 @@ double Beidou_Dnav_Ephemeris::satellitePosition(double transmitTime)
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */
dtr_s -= 2.0 * sqrt(BEIDOU_DNAV_GM * a) * d_eccentricity * sin(E) / (BEIDOU_DNAV_C_M_S * BEIDOU_DNAV_C_M_S);
dtr_s -= 2.0 * sqrt(BEIDOU_GM * a) * d_eccentricity * sin(E) / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S);
return dtr_s;
}

16
src/core/system_parameters/beidou_dnav_navigation_message.cc
View File

@ -174,7 +174,7 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime)
tk = check_t(transmitTime - d_Toe_sf2);
// Computed mean motion
n0 = sqrt(BEIDOU_DNAV_GM / (a * a * a));
n0 = sqrt(BEIDOU_GM / (a * a * a));
// Corrected mean motion
n = n0 + d_Delta_n;
@ -183,7 +183,7 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime)
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * BEIDOU_DNAV_PI), (2 * BEIDOU_DNAV_PI));
M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
@ -193,7 +193,7 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime)
{
E_old = E;
E = M + d_eccentricity * sin(E);
dE = fmod(E - E_old, 2 * BEIDOU_DNAV_PI);
dE = fmod(E - E_old, 2 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -202,7 +202,7 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime)
}
// Compute relativistic correction term
d_dtr = BEIDOU_DNAV_F * d_eccentricity * d_sqrt_A * sin(E);
d_dtr = BEIDOU_F * d_eccentricity * d_sqrt_A * sin(E);
// Compute the true anomaly
double tmp_Y = sqrt(1.0 - d_eccentricity * d_eccentricity) * sin(E);
@ -213,7 +213,7 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime)
phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2 * BEIDOU_DNAV_PI));
phi = fmod((phi), (2 * GNSS_PI));
// Correct argument of latitude
u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi);
@ -225,10 +225,10 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime)
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2 * phi) + d_Cis * sin(2 * phi);
// Compute the angle between the ascending node and the Greenwich meridian
Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_DNAV_OMEGA_EARTH_DOT) * tk - BEIDOU_DNAV_OMEGA_EARTH_DOT * d_Toe_sf2;
Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe_sf2;
// Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2 * BEIDOU_DNAV_PI), (2 * BEIDOU_DNAV_PI));
Omega = fmod((Omega + 2 * GNSS_PI), (2 * GNSS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@ -236,7 +236,7 @@ void Beidou_Dnav_Navigation_Message::satellitePosition(double transmitTime)
d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = d_OMEGA_DOT - BEIDOU_DNAV_OMEGA_EARTH_DOT;
double Omega_dot = d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i);

16
src/core/system_parameters/galileo_ephemeris.cc
View File

@ -96,7 +96,7 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Sa
M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * GALILEO_PI), (2 * GALILEO_PI));
M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
@ -106,7 +106,7 @@ double Galileo_Ephemeris::sv_clock_relativistic_term(double transmitTime) // Sa
{
E_old = E;
E = M + e_1 * sin(E);
dE = fmod(E - E_old, 2 * GALILEO_PI);
dE = fmod(E - E_old, 2 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -156,7 +156,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
M = M0_1 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
M = fmod((M + 2 * GALILEO_PI), (2 * GALILEO_PI));
M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
@ -166,7 +166,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
{
E_old = E;
E = M + e_1 * sin(E);
dE = fmod(E - E_old, 2 * GALILEO_PI);
dE = fmod(E - E_old, 2 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -184,7 +184,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
phi = nu + omega_2;
// Reduce phi to between 0 and 2*pi rad
phi = fmod((phi), (2 * GALILEO_PI));
phi = fmod((phi), (2 * GNSS_PI));
// Correct argument of latitude
u = phi + C_uc_3 * cos(2 * phi) + C_us_3 * sin(2 * phi);
@ -196,10 +196,10 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
i = i_0_2 + iDot_2 * tk + C_ic_4 * cos(2 * phi) + C_is_4 * sin(2 * phi);
// Compute the angle between the ascending node and the Greenwich meridian
Omega = OMEGA_0_2 + (OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT) * tk - GALILEO_OMEGA_EARTH_DOT * t0e_1;
Omega = OMEGA_0_2 + (OMEGA_dot_3 - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * t0e_1;
// Reduce to between 0 and 2*pi rad
Omega = fmod((Omega + 2 * GALILEO_PI), (2 * GALILEO_PI));
Omega = fmod((Omega + 2 * GNSS_PI), (2 * GNSS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@ -207,7 +207,7 @@ void Galileo_Ephemeris::satellitePosition(double transmitTime)
d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = OMEGA_dot_3 - GALILEO_OMEGA_EARTH_DOT;
double Omega_dot = OMEGA_dot_3 - GNSS_OMEGA_EARTH_DOT;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i);

30
src/core/system_parameters/glonass_gnav_navigation_message.cc
View File

@ -397,8 +397,8 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
gnav_almanac[i_alm_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20;
flag_almanac_str_6 = true;
@ -409,7 +409,7 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
// --- It is string 7 ----------------------------------------------
if (flag_almanac_str_6 == true)
{
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14;
@ -447,8 +447,8 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
gnav_almanac[i_alm_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20;
flag_almanac_str_8 = true;
@ -459,7 +459,7 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
// --- It is string 9 ----------------------------------------------
if (flag_almanac_str_8 == true)
{
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14;
@ -492,8 +492,8 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
gnav_almanac[i_alm_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20;
flag_almanac_str_10 = true;
@ -504,7 +504,7 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
// --- It is string 11 ---------------------------------------------
if (flag_almanac_str_10 == true)
{
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14;
@ -536,8 +536,8 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
gnav_almanac[i_alm_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20;
flag_almanac_str_12 = true;
@ -548,7 +548,7 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
// --- It is string 13 ---------------------------------------------
if (flag_almanac_str_12 == true)
{
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14;
@ -587,8 +587,8 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
gnav_almanac[i_alm_satellite_slot_number - 1].d_M_n_A = static_cast<double>(read_navigation_unsigned(string_bits, M_N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_n_A = static_cast<double>(read_navigation_unsigned(string_bits, N_A));
gnav_almanac[i_alm_satellite_slot_number - 1].d_tau_n_A = static_cast<double>(read_navigation_unsigned(string_bits, TAU_N_A)) * TWO_N18;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_lambda_n_A = static_cast<double>(read_navigation_signed(string_bits, LAMBDA_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_i_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_I_N_A)) * TWO_N20 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_epsilon_n_A = static_cast<double>(read_navigation_unsigned(string_bits, EPSILON_N_A)) * TWO_N20;
flag_almanac_str_14 = true;
@ -599,7 +599,7 @@ int32_t Glonass_Gnav_Navigation_Message::string_decoder(const std::string& frame
// --- It is string 15 ----------------------------------------------
if (d_frame_ID != 5 and flag_almanac_str_14 == true)
{
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GLONASS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_omega_n_A = static_cast<double>(read_navigation_signed(string_bits, OMEGA_N_A)) * TWO_N15 * GNSS_PI;
gnav_almanac[i_alm_satellite_slot_number - 1].d_t_lambda_n_A = static_cast<double>(read_navigation_unsigned(string_bits, T_LAMBDA_N_A)) * TWO_N5;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_N_A)) * TWO_N9;
gnav_almanac[i_alm_satellite_slot_number - 1].d_Delta_T_n_A_dot = static_cast<double>(read_navigation_signed(string_bits, DELTA_T_DOT_N_A)) * TWO_N14;

45
src/core/system_parameters/gps_cnav_ephemeris.cc
View File

@ -20,7 +20,7 @@
*/
#include "gps_cnav_ephemeris.h"
#include "MATH_CONSTANTS.h" // for PI, SPEED_OF_LIGHT
#include "MATH_CONSTANTS.h" // for GNSS_PI, SPEED_OF_LIGHT_M_S, F, GPS_GM
#include <cmath>
@ -66,9 +66,7 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime)
double E_old;
double dE;
double M;
const double GM = 3.986005e14; // Universal gravitational constant times the mass of the Earth, [m^3/s^2]
const double F = -4.442807633e-10; // Constant, [s/(m)^(1/2)]
const double A_REF = 26559710.0; // See IS-GPS-200K, pp. 163
const double A_REF = 26559710.0; // See IS-GPS-200K, pp. 163
double d_sqrt_A = sqrt(A_REF + d_DELTA_A);
// Restore semi-major axis
@ -78,24 +76,24 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime)
tk = check_t(transmitTime - d_Toe1);
// Computed mean motion
n0 = sqrt(GM / (a * a * a));
n0 = sqrt(GPS_GM / (a * a * a));
// Corrected mean motion
n = n0 + d_Delta_n;
// Mean anomaly
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2.0 * GPS_L2_PI), (2.0 * GPS_L2_PI));
// M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
// --- Iteratively compute eccentric anomaly ----------------------------
// --- Iteratively compute eccentric anomaly -------------------------------
for (int32_t ii = 1; ii < 20; ii++)
{
E_old = E;
E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old, 2.0 * PI);
dE = fmod(E - E_old, 2.0 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -104,7 +102,7 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime)
}
// Compute relativistic correction term
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
d_dtr = GPS_F * d_e_eccentricity * d_sqrt_A * sin(E);
return d_dtr;
}
@ -126,13 +124,12 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
double i;
double Omega;
const double A_REF = 26559710.0; // See IS-GPS-200K, pp. 170
const double A_REF = 26559710.0; // See IS-GPS-200K, pp. 170
const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200K pp. 164
double d_sqrt_A = sqrt(A_REF + d_DELTA_A);
const double GM = 3.986005e14; // Universal gravitational constant times the mass of the Earth, [m^3/s^2]
const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200K pp. 164
const double OMEGA_EARTH_DOT = 7.2921151467e-5; // Earth rotation rate, [rad/s]
// Find satellite's position ----------------------------------------------
// Find satellite's position -----------------------------------------------
// Restore semi-major axis
a = d_sqrt_A * d_sqrt_A;
@ -141,7 +138,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
tk = check_t(transmitTime - d_Toe1);
// Computed mean motion
n0 = sqrt(GM / (a * a * a));
n0 = sqrt(GPS_GM / (a * a * a));
// Mean motion difference from computed value
double delta_n_a = d_Delta_n + 0.5 * d_DELTA_DOT_N * tk;
@ -153,17 +150,17 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2 * GPS_L2_PI), (2 * GPS_L2_PI));
// M = fmod((M + 2 * GNSS_PI), (2 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
// --- Iteratively compute eccentric anomaly ----------------------------
// --- Iteratively compute eccentric anomaly -------------------------------
for (int32_t ii = 1; ii < 20; ii++)
{
E_old = E;
E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old, 2 * PI);
dE = fmod(E - E_old, 2 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -180,7 +177,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad
// phi = fmod((phi), (2*GPS_L2_PI));
// phi = fmod((phi), (2*GNSS_PI));
// Correct argument of latitude
u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi);
@ -192,11 +189,11 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2 * phi) + d_Cis * sin(2 * phi);
// Compute the angle between the ascending node and the Greenwich meridian
double d_OMEGA_DOT = OMEGA_DOT_REF * PI + d_DELTA_OMEGA_DOT;
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT) * tk - OMEGA_EARTH_DOT * d_Toe1;
double d_OMEGA_DOT = OMEGA_DOT_REF * GNSS_PI + d_DELTA_OMEGA_DOT;
Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe1;
// Reduce to between 0 and 2*pi rad
// Omega = fmod((Omega + 2*GPS_L2_PI), (2*GPS_L2_PI));
// Omega = fmod((Omega + 2*GNSS_PI), (2*GNSS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@ -204,7 +201,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = d_OMEGA_DOT - OMEGA_EARTH_DOT;
double Omega_dot = d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i);
@ -215,7 +212,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime)
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */
dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (SPEED_OF_LIGHT * SPEED_OF_LIGHT);
dtr_s -= 2.0 * sqrt(GPS_GM * a) * d_e_eccentricity * sin(E) / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S);
return dtr_s;
}

24
src/core/system_parameters/gps_ephemeris.cc
View File

@ -94,14 +94,14 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime)
tk = check_t(transmitTime - d_Toe);
// Computed mean motion
n0 = sqrt(GM / (a * a * a));
n0 = sqrt(GPS_GM / (a * a * a));
// Corrected mean motion
n = n0 + d_Delta_n;
// Mean anomaly
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI));
// M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
@ -111,7 +111,7 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime)
{
E_old = E;
E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old, 2.0 * GPS_PI);
dE = fmod(E - E_old, 2.0 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -120,7 +120,7 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime)
}
// Compute relativistic correction term
d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E);
d_dtr = GPS_F * d_e_eccentricity * d_sqrt_A * sin(E);
return d_dtr;
}
@ -151,7 +151,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
tk = check_t(transmitTime - d_Toe);
// Computed mean motion
n0 = sqrt(GM / (a * a * a));
n0 = sqrt(GPS_GM / (a * a * a));
// Corrected mean motion
n = n0 + d_Delta_n;
@ -160,7 +160,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
M = d_M_0 + n * tk;
// Reduce mean anomaly to between 0 and 2pi
// M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI));
// M = fmod((M + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// Initial guess of eccentric anomaly
E = M;
@ -170,7 +170,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
{
E_old = E;
E = M + d_e_eccentricity * sin(E);
dE = fmod(E - E_old, 2.0 * GPS_PI);
dE = fmod(E - E_old, 2.0 * GNSS_PI);
if (fabs(dE) < 1e-12)
{
// Necessary precision is reached, exit from the loop
@ -187,7 +187,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
phi = nu + d_OMEGA;
// Reduce phi to between 0 and 2*pi rad
// phi = fmod((phi), (2.0 * GPS_PI));
// phi = fmod((phi), (2.0 * GNSS_PI));
// Correct argument of latitude
u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi);
@ -199,10 +199,10 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi);
// Compute the angle between the ascending node and the Greenwich meridian
Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT) * tk - OMEGA_EARTH_DOT * d_Toe;
Omega = d_OMEGA0 + (d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT) * tk - GNSS_OMEGA_EARTH_DOT * d_Toe;
// Reduce to between 0 and 2*pi rad
// Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI));
// Omega = fmod((Omega + 2.0 * GNSS_PI), (2.0 * GNSS_PI));
// --- Compute satellite coordinates in Earth-fixed coordinates
d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega);
@ -210,7 +210,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
d_satpos_Z = sin(u) * r * sin(i);
// Satellite's velocity. Can be useful for Vector Tracking loops
double Omega_dot = d_OMEGA_DOT - OMEGA_EARTH_DOT;
double Omega_dot = d_OMEGA_DOT - GNSS_OMEGA_EARTH_DOT;
d_satvel_X = -Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega);
d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega);
d_satvel_Z = d_satpos_Y * sin(i);
@ -221,7 +221,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime)
double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk;
/* relativity correction */
dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_C_M_S * GPS_C_M_S);
dtr_s -= 2.0 * sqrt(GPS_GM * a) * d_e_eccentricity * sin(E) / (SPEED_OF_LIGHT_M_S * SPEED_OF_LIGHT_M_S);
return dtr_s;
}

4
src/tests/unit-tests/arithmetic/complex_carrier_test.cc
View File

@ -35,7 +35,7 @@ TEST(ComplexCarrierTest, StandardComplexImplementation)
auto* output = new std::complex<float>[FLAGS_size_carrier_test];
const double _f = 2000.0;
const double _fs = 2000000.0;
const auto phase_step = static_cast<double>((GPS_TWO_PI * _f) / _fs);
const auto phase_step = static_cast<double>((TWO_PI * _f) / _fs);
double phase = 0.0;
std::chrono::time_point<std::chrono::system_clock> start, end;
@ -75,7 +75,7 @@ TEST(ComplexCarrierTest, C11ComplexImplementation)
std::vector<std::complex<float>> output(FLAGS_size_carrier_test);
const double _f = 2000.0;
const double _fs = 2000000.0;
const auto phase_step = static_cast<double>((GPS_TWO_PI * _f) / _fs);
const auto phase_step = static_cast<double>((TWO_PI * _f) / _fs);
double phase = 0.0;
std::chrono::time_point<std::chrono::system_clock> start, end;

1
src/tests/unit-tests/control-plane/control_thread_test.cc
View File

@ -251,4 +251,5 @@ TEST_F(ControlThreadTest /*unused*/, StopReceiverProgrammatically /*unused*/)
}
stop_receiver_thread.join();
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}

2
src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc
View File

@ -2108,7 +2108,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
index2 = arma::find(true_obs_vec.at(min_pr_ch_id).col(0) >= measured_obs_vec.at(min_pr_ch_id).col(0)(0), 1, "first");
if ((!index2.empty()) and (index2(0) > 0))
{
receiver_time_offset_ref_channel_s = (true_obs_vec.at(min_pr_ch_id).col(1)(index2(0)) - measured_obs_vec.at(min_pr_ch_id).col(4)(0)) / GPS_C_M_S;
receiver_time_offset_ref_channel_s = (true_obs_vec.at(min_pr_ch_id).col(1)(index2(0)) - measured_obs_vec.at(min_pr_ch_id).col(4)(0)) / SPEED_OF_LIGHT_M_S;
std::cout << "Ref. channel initial Receiver time offset " << receiver_time_offset_ref_channel_s(0) * 1e3 << " [ms]" << std::endl;
}
else

2
src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test_fpga.cc
View File

@ -2271,7 +2271,7 @@ TEST_F(HybridObservablesTestFpga, ValidationOfResults)
index2 = arma::find(true_obs_vec.at(min_pr_ch_id).col(0) >= measured_obs_vec.at(min_pr_ch_id).col(0)(0), 1, "first");
if ((!index2.empty()) and (index2(0) > 0))
{
receiver_time_offset_ref_channel_s = (true_obs_vec.at(min_pr_ch_id).col(1)(index2(0)) - measured_obs_vec.at(min_pr_ch_id).col(4)(0)) / GPS_C_M_S;
receiver_time_offset_ref_channel_s = (true_obs_vec.at(min_pr_ch_id).col(1)(index2(0)) - measured_obs_vec.at(min_pr_ch_id).col(4)(0)) / SPEED_OF_LIGHT_M_S;
std::cout << "Ref. channel initial Receiver time offset " << receiver_time_offset_ref_channel_s(0) * 1e3 << " [ms]" << std::endl;
}
else

2
src/tests/unit-tests/signal-processing-blocks/pvt/rtcm_test.cc
View File

@ -559,7 +559,7 @@ TEST(RtcmTest, MSM1)
EXPECT_EQ(ref_id, rtcm->bin_to_uint(MSM1_bin.substr(size_header + size_msg_length + 12, 12)));
EXPECT_EQ(0, MSM1_bin.substr(size_header + size_msg_length + 169, Nsat * Nsig).compare("101101")); // check cell mask
double meters_to_miliseconds = GPS_C_M_S * 0.001;
double meters_to_miliseconds = SPEED_OF_LIGHT_M_S * 0.001;
unsigned int rough_range_1 = static_cast<unsigned int>(std::floor(std::round(gnss_synchro.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu;
unsigned int rough_range_2 = static_cast<unsigned int>(std::floor(std::round(gnss_synchro2.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu;
unsigned int rough_range_4 = static_cast<unsigned int>(std::floor(std::round(gnss_synchro3.Pseudorange_m / meters_to_miliseconds / TWO_N10)) + 0.5) & 0x3FFu;

4
src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc
View File

@ -686,7 +686,7 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
while (trk_dump.read_binary_obs())
{
trk_timestamp_s(epoch_counter) = static_cast<double>(trk_dump.PRN_start_sample_count) / static_cast<double>(baseband_sampling_freq);
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI;
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / TWO_PI;
trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz;
double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast<double>(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast<double>(baseband_sampling_freq), 1.0e-3) / 1.0e-3);
@ -767,7 +767,7 @@ TEST_F(GpsL1CADllPllTrackingTest, ValidationOfResults)
code_phase_error_chips = check_results_codephase(true_timestamp_s, true_prn_delay_chips, trk_timestamp_s, trk_prn_delay_chips, mean_error, std_dev_error, rmse);
for (double code_phase_error_chip : code_phase_error_chips)
{
code_phase_error_meters.push_back(GPS_L1_CA_CHIP_PERIOD_S * code_phase_error_chip * GPS_C_M_S);
code_phase_error_meters.push_back(GPS_L1_CA_CHIP_PERIOD_S * code_phase_error_chip * SPEED_OF_LIGHT_M_S);
}
mean_code_phase_error.push_back(mean_error);
std_dev_code_phase_error.push_back(std_dev_error);

2
src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test_fpga.cc
View File

@ -606,7 +606,7 @@ TEST_F(GpsL1CADllPllTrackingTestFpga, ValidationOfResultsFpga)
while (trk_dump.read_binary_obs())
{
trk_timestamp_s(epoch_counter) = static_cast<double>(trk_dump.PRN_start_sample_count) / static_cast<double>(baseband_sampling_freq);
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI;
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / TWO_PI;
trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz;
double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast<double>(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast<double>(baseband_sampling_freq), 1.0e-3) / 1.0e-3);

2
src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc
View File

@ -509,7 +509,7 @@ TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults)
while (trk_dump.read_binary_obs())
{
trk_timestamp_s(epoch_counter) = static_cast<double>(trk_dump.PRN_start_sample_count) / static_cast<double>(baseband_sampling_freq);
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI;
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / TWO_PI;
trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz;
double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast<double>(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast<double>(baseband_sampling_freq), 1.0e-3) / 1.0e-3);

2
src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc
View File

@ -943,7 +943,7 @@ TEST_F(TrackingPullInTest, ValidationOfResults)
while (trk_dump.read_binary_obs())
{
trk_timestamp_s(epoch_counter) = static_cast<double>(trk_dump.PRN_start_sample_count) / static_cast<double>(baseband_sampling_freq);
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI;
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / TWO_PI;
trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz;
double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast<double>(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast<double>(baseband_sampling_freq), 1.0e-3) / 1.0e-3);

2
src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test_fpga.cc
View File

@ -1128,7 +1128,7 @@ TEST_F(TrackingPullInTestFpga, ValidationOfResults)
while (trk_dump.read_binary_obs())
{
trk_timestamp_s(epoch_counter) = static_cast<double>(trk_dump.PRN_start_sample_count) / static_cast<double>(baseband_sampling_freq);
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI;
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / TWO_PI;
trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz;
double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast<double>(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast<double>(baseband_sampling_freq), 1.0e-3) / 1.0e-3);

6
src/utils/front-end-cal/front_end_cal.cc
View File

@ -263,8 +263,8 @@ arma::vec FrontEndCal::lla2ecef(const arma::vec &lla)
double R = 6378137.0;
arma::vec ellipsoid = "0.0 0.0";
double phi = (lla(0) / 360.0) * GPS_TWO_PI;
double lambda = (lla(1) / 360.0) * GPS_TWO_PI;
double phi = (lla(0) / 360.0) * TWO_PI;
double lambda = (lla(1) / 360.0) * TWO_PI;
ellipsoid(0) = R;
ellipsoid(1) = sqrt(1.0 - (1.0 - f) * (1.0 - f));
@ -345,7 +345,7 @@ double FrontEndCal::estimate_doppler_from_eph(unsigned int PRN, double tow, doub
// Doppler estimation
arma::vec Doppler_Hz;
Doppler_Hz = (obs_to_sat_velocity / GPS_C_M_S) * GPS_L1_FREQ_HZ;
Doppler_Hz = (obs_to_sat_velocity / SPEED_OF_LIGHT_M_S) * GPS_L1_FREQ_HZ;
double mean_Doppler_Hz;
mean_Doppler_Hz = arma::mean(Doppler_Hz);
return mean_Doppler_Hz;