mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-09-28 23:10:51 +00:00
Code Issues Releases Wiki Activity

Code cleaning

Browse Source
This commit is contained in:
Carles Fernandez 2017-05-08 21:30:41 +02:00
parent bb9346441c
commit 25bb7ae54d
2 changed files with 195 additions and 186 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -47,7 +47,9 @@
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
* -----------------------------------------------------------------------*/
#include "rtklib_solver.h" #include "rtklib_solver.h"
#include <glog/logging.h> #include <glog/logging.h>
@ -68,8 +70,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
d_flag_averaging = false; d_flag_averaging = false;
rtk_ = rtk; rtk_ = rtk;
pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, '0', '0', '0', 0, 0, 0 };
'0', '0', '0', 0, 0, 0 };
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true) if (d_flag_dump_enabled == true)
@ -96,6 +97,7 @@ rtklib_solver::~rtklib_solver()
d_dump_file.close(); d_dump_file.close();
} }
bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging) bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging)
{ {
std::map<int,Gnss_Synchro>::iterator gnss_observables_iter; std::map<int,Gnss_Synchro>::iterator gnss_observables_iter;
@ -151,10 +153,10 @@ bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
if (gps_ephemeris_iter != gps_ephemeris_map.end()) if (gps_ephemeris_iter != gps_ephemeris_map.end())
{ {
//convert ephemeris from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs]=eph_to_rtklib(gps_ephemeris_iter->second); eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second);
//convert observation from GNSS-SDR class to RTKLIB structure //convert observation from GNSS-SDR class to RTKLIB structure
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
obs_data[valid_obs]=insert_obs_to_rtklib(newobs, obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
gnss_observables_iter->second, gnss_observables_iter->second,
gps_ephemeris_iter->second.i_GPS_week, gps_ephemeris_iter->second.i_GPS_week,
0); 0);
@ -176,19 +178,21 @@ bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
{ {
// 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris // 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris
// (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure // (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure
for (int i=0;i<valid_obs;i++) for (int i = 0; i < valid_obs; i++)
{ {
if (eph_data[i].sat == static_cast<int>(gnss_observables_iter->second.PRN)) if (eph_data[i].sat == static_cast<int>(gnss_observables_iter->second.PRN))
{ {
eph_data[i]=eph_to_rtklib(gps_cnav_ephemeris_iter->second); eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
obs_data[valid_obs]=insert_obs_to_rtklib(obs_data[valid_obs], obs_data[valid_obs] = insert_obs_to_rtklib(obs_data[valid_obs],
gnss_observables_iter->second, gnss_observables_iter->second,
gps_cnav_ephemeris_iter->second.i_GPS_week, gps_cnav_ephemeris_iter->second.i_GPS_week,
1);//Band 2 (L2) 1);//Band 2 (L2)
break; break;
} }
} }
}else{ }
else
{
// 3. If not found, insert the GPS L2 ephemeris and the observation // 3. If not found, insert the GPS L2 ephemeris and the observation
//convert ephemeris from GNSS-SDR class to RTKLIB structure //convert ephemeris from GNSS-SDR class to RTKLIB structure
eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second);
@ -219,42 +223,44 @@ bool rtklib_solver::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// ********************************************************************** // **********************************************************************
b_valid_position = false; b_valid_position = false;
if (valid_obs>0) if (valid_obs > 0)
{ {
int result=0; int result = 0;
nav_t nav_data; nav_t nav_data;
nav_data.eph=eph_data; nav_data.eph = eph_data;
nav_data.n=valid_obs; nav_data.n = valid_obs;
for (int i=0; i< MAXSAT;i++) for (int i = 0; i < MAXSAT; i++)
{ {
nav_data.lam[i][0]=SPEED_OF_LIGHT/FREQ1; /* L1/E1 */ nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */
nav_data.lam[i][1]=SPEED_OF_LIGHT/FREQ2; /* L2 */ nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */
nav_data.lam[i][2]=SPEED_OF_LIGHT/FREQ5; /* L2 */ nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L2 */
} }
result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data); result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data);
if(result==0) if(result == 0)
{ {
LOG(INFO)<<"RTKLIB rtkpos error message: "<<rtk_.errbuf; LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf;
d_rx_dt_s = 0; //reset rx time estimation d_rx_dt_s = 0; //reset rx time estimation
d_valid_observations = 0; d_valid_observations = 0;
}else{ }
d_valid_observations=rtk_.sol.ns; //record the number of valid satellites used by the PVT solver else
pvt_sol=rtk_.sol; {
b_valid_position=true; d_valid_observations = rtk_.sol.ns; //record the number of valid satellites used by the PVT solver
pvt_sol = rtk_.sol;
b_valid_position = true;
arma::vec rx_position_and_time(4); arma::vec rx_position_and_time(4);
rx_position_and_time(0)=pvt_sol.rr[0]; rx_position_and_time(0) = pvt_sol.rr[0];
rx_position_and_time(1)=pvt_sol.rr[1]; rx_position_and_time(1) = pvt_sol.rr[1];
rx_position_and_time(2)=pvt_sol.rr[2]; rx_position_and_time(2) = pvt_sol.rr[2];
rx_position_and_time(3)=pvt_sol.dtr[0]; rx_position_and_time(3) = pvt_sol.dtr[0];
d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration
d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds]
DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
boost::posix_time::ptime p_time; boost::posix_time::ptime p_time;
gtime_t rtklib_utc_time=gpst2utc(pvt_sol.time); gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time);
p_time=boost::posix_time::from_time_t(rtklib_utc_time.time); p_time = boost::posix_time::from_time_t(rtklib_utc_time.time);
p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec*1e6)); p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
d_position_UTC_time = p_time; d_position_UTC_time = p_time;
cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4); cart2geo(static_cast<double>(rx_position_and_time(0)), static_cast<double>(rx_position_and_time(1)), static_cast<double>(rx_position_and_time(2)), 4);

5
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -47,7 +47,10 @@
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.*/ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
* -------------------------------------------------------------------------*/
#ifndef GNSS_SDR_RTKLIB_SOLVER_H_ #ifndef GNSS_SDR_RTKLIB_SOLVER_H_
#define GNSS_SDR_RTKLIB_SOLVER_H_ #define GNSS_SDR_RTKLIB_SOLVER_H_