mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-05-28 20:24:10 +00:00
Code cleaning
This commit is contained in:
parent
f03d869def
commit
dbe08ade8c
@ -164,7 +164,8 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
|
|||||||
DLOG(INFO) << "satpos=" << satpos;
|
DLOG(INFO) << "satpos=" << satpos;
|
||||||
DLOG(INFO) << "obs="<< obs;
|
DLOG(INFO) << "obs="<< obs;
|
||||||
DLOG(INFO) << "W=" << W;
|
DLOG(INFO) << "W=" << W;
|
||||||
try{
|
try
|
||||||
|
{
|
||||||
// check if this is the initial position computation
|
// check if this is the initial position computation
|
||||||
if (d_rx_dt_s == 0)
|
if (d_rx_dt_s == 0)
|
||||||
{
|
{
|
||||||
@ -239,10 +240,11 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
|
|||||||
|
|
||||||
// MOVING AVERAGE PVT
|
// MOVING AVERAGE PVT
|
||||||
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
|
galileo_e1_ls_pvt::pos_averaging(flag_averaging);
|
||||||
}catch(const std::exception & e)
|
}
|
||||||
|
catch(const std::exception & e)
|
||||||
{
|
{
|
||||||
d_rx_dt_s=0;//reset rx time estimation
|
d_rx_dt_s = 0; //reset rx time estimation
|
||||||
LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
|
LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
|
||||||
b_valid_position = false;
|
b_valid_position = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -165,7 +165,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
|||||||
DLOG(INFO) << "obs=" << obs;
|
DLOG(INFO) << "obs=" << obs;
|
||||||
DLOG(INFO) << "W=" << W;
|
DLOG(INFO) << "W=" << W;
|
||||||
|
|
||||||
try{
|
try
|
||||||
|
{
|
||||||
// check if this is the initial position computation
|
// check if this is the initial position computation
|
||||||
if (d_rx_dt_s == 0)
|
if (d_rx_dt_s == 0)
|
||||||
{
|
{
|
||||||
@ -241,10 +242,11 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
|
|||||||
// MOVING AVERAGE PVT
|
// MOVING AVERAGE PVT
|
||||||
pos_averaging(flag_averaging);
|
pos_averaging(flag_averaging);
|
||||||
|
|
||||||
}catch(const std::exception & e)
|
}
|
||||||
|
catch(const std::exception & e)
|
||||||
{
|
{
|
||||||
d_rx_dt_s=0;//reset rx time estimation
|
d_rx_dt_s = 0; //reset rx time estimation
|
||||||
LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
|
LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
|
||||||
b_valid_position = false;
|
b_valid_position = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -132,15 +132,12 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
|||||||
satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
|
satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y;
|
||||||
satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
|
satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z;
|
||||||
|
|
||||||
|
|
||||||
// 4- fill the observations vector with the corrected observables
|
// 4- fill the observations vector with the corrected observables
|
||||||
obs.resize(valid_obs + 1, 1);
|
obs.resize(valid_obs + 1, 1);
|
||||||
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
|
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s;
|
||||||
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
|
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
|
||||||
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
|
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
|
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);
|
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
|
||||||
|
|
||||||
@ -153,7 +150,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
|||||||
|
|
||||||
valid_obs++;
|
valid_obs++;
|
||||||
}
|
}
|
||||||
|
|
||||||
else // the ephemeris are not available for this SV
|
else // the ephemeris are not available for this SV
|
||||||
{
|
{
|
||||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||||
@ -288,7 +284,8 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
|||||||
DLOG(INFO) << "satpos=" << satpos;
|
DLOG(INFO) << "satpos=" << satpos;
|
||||||
DLOG(INFO) << "obs=" << obs;
|
DLOG(INFO) << "obs=" << obs;
|
||||||
DLOG(INFO) << "W=" << W;
|
DLOG(INFO) << "W=" << W;
|
||||||
try{
|
try
|
||||||
|
{
|
||||||
// check if this is the initial position computation
|
// check if this is the initial position computation
|
||||||
if (d_rx_dt_s == 0)
|
if (d_rx_dt_s == 0)
|
||||||
{
|
{
|
||||||
@ -374,10 +371,11 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
|
|||||||
|
|
||||||
// MOVING AVERAGE PVT
|
// MOVING AVERAGE PVT
|
||||||
pos_averaging(flag_averaging);
|
pos_averaging(flag_averaging);
|
||||||
}catch(const std::exception & e)
|
}
|
||||||
|
catch(const std::exception & e)
|
||||||
{
|
{
|
||||||
d_rx_dt_s=0;//reset rx time estimation
|
d_rx_dt_s = 0; //reset rx time estimation
|
||||||
LOG(WARNING)<<"Problem with the solver, invalid solution!"<< e.what();
|
LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
|
||||||
b_valid_position = false;
|
b_valid_position = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -144,9 +144,9 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
|
|||||||
double omc = obs(j) - calc;
|
double omc = obs(j) - calc;
|
||||||
abs_omc(i) = std::abs(omc);
|
abs_omc(i) = std::abs(omc);
|
||||||
}
|
}
|
||||||
}// % j-loop
|
} // % j-loop
|
||||||
|
|
||||||
//discrimination between roots
|
// discrimination between roots
|
||||||
if (abs_omc(0) > abs_omc(1))
|
if (abs_omc(0) > abs_omc(1))
|
||||||
{
|
{
|
||||||
pos = possible_pos.col(1);
|
pos = possible_pos.col(1);
|
||||||
@ -155,7 +155,7 @@ arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs)
|
|||||||
{
|
{
|
||||||
pos = possible_pos.col(0);
|
pos = possible_pos.col(0);
|
||||||
}
|
}
|
||||||
}// % iter loop
|
} // % iter loop
|
||||||
return pos;
|
return pos;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -164,7 +164,7 @@ double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y)
|
|||||||
{
|
{
|
||||||
// LORENTZ Calculates the Lorentz inner product of the two
|
// LORENTZ Calculates the Lorentz inner product of the two
|
||||||
// 4 by 1 vectors x and y
|
// 4 by 1 vectors x and y
|
||||||
// Based ob code by:
|
// Based on code by:
|
||||||
// Kai Borre 04-22-95
|
// Kai Borre 04-22-95
|
||||||
// Copyright (c) by Kai Borre
|
// Copyright (c) by Kai Borre
|
||||||
// $Revision: 1.0 $ $Date: 1997/09/26 $
|
// $Revision: 1.0 $ $Date: 1997/09/26 $
|
||||||
@ -182,7 +182,7 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
|
|||||||
* Inputs:
|
* Inputs:
|
||||||
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
|
* satpos - Satellites positions in ECEF system: [X; Y; Z;]
|
||||||
* obs - Observations - the pseudorange measurements to each satellite
|
* obs - Observations - the pseudorange measurements to each satellite
|
||||||
* w - weigths vector
|
* w - weight vector
|
||||||
*
|
*
|
||||||
* Returns:
|
* Returns:
|
||||||
* pos - receiver position and receiver clock error
|
* pos - receiver position and receiver clock error
|
||||||
@ -289,9 +289,9 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs
|
|||||||
d_Q = arma::inv(arma::htrans(A) * A);
|
d_Q = arma::inv(arma::htrans(A) * A);
|
||||||
|
|
||||||
// check the consistency of the PVT solution
|
// 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) / GPS_C_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) / GPS_C_m_s;
|
||||||
throw std::runtime_error("Receiver time offset out of range!");
|
throw std::runtime_error("Receiver time offset out of range!");
|
||||||
}
|
}
|
||||||
return pos;
|
return pos;
|
||||||
|
Loading…
x
Reference in New Issue
Block a user