mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 15:23:04 +00:00
Code Issues Releases Wiki Activity

Add work on the PVT block

Browse Source
This commit is contained in:
Carles Fernandez
2017-04-27 15:07:44 +02:00
parent 5fe34321f2
commit 560164711b
3 changed files with 48 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

@@ -285,7 +285,6 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump
d_ls_pvt = std::make_shared<rtklib_solver>((int)nchannels, dump_ls_pvt_filename, d_dump, rtklib_options);
d_ls_pvt->set_averaging_depth(d_averaging_depth);
d_sample_counter = 0;
d_last_sample_nav_output = 0;
b_rinex_header_written = false;
@@ -408,20 +407,6 @@ bool rtklib_pvt_cc::observables_pairCompare_min(const std::pair<int,Gnss_Synchro
}
void rtklib_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchronization_data)
{
// Print the current receiver status using std::cout every second
int current_rx_seg = floor((double)channels_synchronization_data[0][0].Tracking_sample_counter/(double)channels_synchronization_data[0][0].fs);
if ( current_rx_seg != d_last_status_print_seg)
{
d_last_status_print_seg = current_rx_seg;
std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl << std::flush;
//DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
// << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
}
}
bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff)
{
/* Fill Sys V message structures */
@@ -441,21 +426,18 @@ bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff)
int rtklib_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items ,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused)))
{
//std::cout << "noutput items: " << noutput_items << std::endl;
//std::cout << "ninput items: " << ninput_items[0] << std::endl;
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
double d_rx_time[noutput_items];
for(unsigned int item = 0; item < noutput_items; item++)
double d_rx_time[ninput_items[0]];
for(unsigned int item = 0; item < ninput_items[0]; item++)
{
d_sample_counter++;
//d_sample_counter++;
unsigned int gps_channel = 0;
unsigned int gal_channel = 0;
gnss_observables_map.clear();
print_receiver_status(in);
//print_receiver_status(in);
// ############ 1. READ PSEUDORANGES ####
for (unsigned int i = 0; i < d_nchannels; i++)
@@ -497,6 +479,9 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
std::map<int, Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
std::map<int, Gnss_Synchro>::iterator gnss_observables_iter;
long int rx_time_ms = static_cast<long int>((d_rx_time[item] * 1000.0));
/*
* TYPE | RECEIVER
* 0 | Unknown
@@ -528,7 +513,7 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
if (gnss_observables_map.size() > 0)
{
// compute on the fly PVT solution
if ((d_sample_counter % d_output_rate_ms) == 0)
if ((rx_time_ms % d_output_rate_ms) == 0)
{
bool pvt_result;
pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time[item], d_flag_averaging);
@@ -548,7 +533,7 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
ttff_msgbuf ttff;
ttff.mtype = 1;
ttff.ttff = d_sample_counter;
ttff.ttff = d_rx_time[item]; //d_sample_counter;
send_sys_v_ttff_msg(ttff);
first_fix = false;
}
@@ -675,7 +660,7 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
{
// Limit the RINEX navigation output rate
// Notice that d_sample_counter period is 4ms (for Galileo correlators)
if ((d_sample_counter - d_last_sample_nav_output) >= 6000)
if ((d_rx_time[item] - d_last_sample_nav_output) >= 6000)
{
if(type_of_rx == 1) // GPS L1 C/A only
{
@@ -702,7 +687,7 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map);
}
d_last_sample_nav_output = d_sample_counter;
d_last_sample_nav_output = d_rx_time[item]; //d_sample_counter;
}
galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin();
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
@@ -833,14 +818,14 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
{
if(type_of_rx == 1) // GPS L1 C/A
{
if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0)
if((rx_time_ms % d_rtcm_MT1019_rate_ms) == 0)
{
for(std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ )
{
d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second);
}
}
if((d_sample_counter % d_rtcm_MSM_rate_ms) == 0)
if((rx_time_ms % d_rtcm_MSM_rate_ms) == 0)
{
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
@@ -852,14 +837,14 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
}
if((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo
{
if((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4) ) == 0)
if((rx_time_ms % (d_rtcm_MT1045_rate_ms / 4) ) == 0)
{
for(std::map<int,Galileo_Ephemeris>::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ )
{
d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second);
}
}
if((d_sample_counter % (d_rtcm_MSM_rate_ms / 4) ) == 0)
if((rx_time_ms % (d_rtcm_MSM_rate_ms / 4) ) == 0)
{
std::map<int,Galileo_Ephemeris>::iterator gal_ephemeris_iter;
gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin();
@@ -871,14 +856,14 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
}
if(type_of_rx == 7) // GPS L1 C/A + GPS L2C
{
if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0)
if((rx_time_ms % d_rtcm_MT1019_rate_ms) == 0)
{
for(std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ )
{
d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second);
}
}
if((d_sample_counter % d_rtcm_MSM_rate_ms) == 0)
if((rx_time_ms % d_rtcm_MSM_rate_ms) == 0)
{
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
@@ -892,21 +877,21 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
}
if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B
{
if(((d_sample_counter % (d_rtcm_MT1019_rate_ms / 4)) == 0) && (d_rtcm_MT1019_rate_ms != 0))
if(((rx_time_ms % (d_rtcm_MT1019_rate_ms / 4)) == 0) && (d_rtcm_MT1019_rate_ms != 0))
{
for(gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ )
{
d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second);
}
}
if(((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4)) == 0) && (d_rtcm_MT1045_rate_ms != 0))
if(((rx_time_ms % (d_rtcm_MT1045_rate_ms / 4)) == 0) && (d_rtcm_MT1045_rate_ms != 0))
{
for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ )
{
d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second);
}
}
if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) || ((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0))
if(((rx_time_ms % (d_rtcm_MT1097_rate_ms / 4) ) == 0) || ((rx_time_ms % (d_rtcm_MT1077_rate_ms / 4) ) == 0))
{
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
@@ -939,7 +924,7 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
}
i++;
}
if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) && (d_rtcm_MT1097_rate_ms != 0) )
if(((rx_time_ms % (d_rtcm_MT1097_rate_ms / 4) ) == 0) && (d_rtcm_MT1097_rate_ms != 0) )
{
if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end())
@@ -947,7 +932,7 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time[item], gnss_observables_map, 0, 0, 0, 0, 0);
}
}
if(((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0) && (d_rtcm_MT1077_rate_ms != 0) )
if(((rx_time_ms % (d_rtcm_MT1077_rate_ms / 4) ) == 0) && (d_rtcm_MT1077_rate_ms != 0) )
{
if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
{
@@ -1070,14 +1055,14 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
}
// DEBUG MESSAGE: Display position in console output
if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true)
if (((rx_time_ms % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true)
{
std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m] " << rx_time_ms<<std::endl;
LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
<< " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " UTC2 using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d
<< " [deg], Height= " << d_ls_pvt->d_height_m << " [m]";
/* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time)
@@ -1108,6 +1093,6 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
}
}
}
consume_each(noutput_items); //one by one
return noutput_items;
consume_each(ninput_items[0]);
return ninput_items[0];
}

2
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@@ -123,7 +123,6 @@ private:
int d_rtcm_MT1097_rate_ms;
int d_rtcm_MSM_rate_ms;
void print_receiver_status(Gnss_Synchro** channels_synchronization_data);
int d_last_status_print_seg; //for status printer
unsigned int d_nchannels;
@@ -133,7 +132,6 @@ private:
bool d_flag_averaging;
int d_output_rate_ms;
int d_display_rate_ms;
long unsigned int d_sample_counter;
long unsigned int d_last_sample_nav_output;
std::shared_ptr<Rinex_Printer> rp;

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

@@ -31,8 +31,9 @@
#include "hybrid_ls_pvt.h"
#include <glog/logging.h>
#include "GPS_L1_CA.h"
#include "Galileo_E1.h"
#include "GPS_L1_CA.h"
#include "GPS_L2C.h"
using google::LogMessage;
@@ -71,7 +72,8 @@ hybrid_ls_pvt::~hybrid_ls_pvt()
d_dump_file.close();
}
bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double Rx_time, bool flag_averaging)
bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, double hybrid_current_time, bool flag_averaging)
{
std::map<int,Gnss_Synchro>::iterator gnss_observables_iter;
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
@@ -88,6 +90,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
double GST = 0.0;
double secondsperweek = 604800.0;
//double utc_tx_corrected = 0.0; //utc computed at tx_time_corrected, added for Galileo constellation (in GPS utc is directly computed at TX_time_corrected_s)
double TX_time_corrected_s = 0.0;
double SV_clock_bias_s = 0.0;
@@ -98,7 +101,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// ********************************************************************************
int valid_obs = 0; //valid observations counter
for(gnss_observables_iter = gnss_observables_map.begin();
gnss_observables_iter != gnss_observables_map.end();
gnss_observables_iter++)
@@ -118,6 +120,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
W(valid_obs) = 1;
// 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;
// 2- compute the clock drift using the clock model (broadcast) for this SV
@@ -140,7 +143,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
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
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, Rx_time);
GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
@@ -174,15 +177,15 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 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;
// 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);
SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD;
// 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
//compute satellite position, clock bias + relativistic correction
double dts = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix
satpos.resize(3, valid_obs + 1);
@@ -198,31 +201,17 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
double P1_P2=(1.0-Gamma)*(gps_ephemeris_iter->second.d_TGD* GPS_C_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 + dts * GPS_C_m_s-Code_bias_m-d_rx_dt_s * GPS_C_m_s;
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr * GPS_C_m_s-Code_bias_m-d_rx_dt_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
// SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
<< " TX Time corrected="<<TX_time_corrected_s
<< " [m] X=" << gps_ephemeris_iter->second.d_satpos_X
<< " TX Time corrected="<<TX_time_corrected_s << " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]";
//*** debug
if (valid_obs==0)
{
gtime_t rx_time=gpst2time(adjgpsweek(gps_ephemeris_iter->second.i_GPS_week),Rx_time);
gtime_t tx_time=gpst2time(adjgpsweek(gps_ephemeris_iter->second.i_GPS_week),Tx_time);
printf("RINEX RX TIME: %s,%f, TX TIME: %s,%f\n\r",time_str(rx_time,3),rx_time.sec,time_str(tx_time,3),tx_time.sec);
}
std::flush(std::cout);
gtime_t tx_time_corr=gpst2time(adjgpsweek(gps_ephemeris_iter->second.i_GPS_week),TX_time_corrected_s);
printf("SAT TX TIME [%i]: %s,%f PR:%f dt:%f\n\r",valid_obs,time_str(tx_time_corr,3),tx_time_corr.sec, obs(valid_obs),dts);
std::flush(std::cout);
//*** end debug
valid_obs++;
// compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
@@ -245,15 +234,17 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 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;
// 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);
// 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s;
//std::cout<<"TX time["<<gps_cnav_ephemeris_iter->second.i_satellite_PRN<<"]="<<TX_time_corrected_s<<std::endl;
double dtr = gps_cnav_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//std::cout<<"L2 Tx_time: "<<Tx_time<<" SV_clock_bias_s: "<<SV_clock_bias_s<<" dtr: "<<dtr<<std::endl;
//store satellite positions in a matrix
satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X;
@@ -262,7 +253,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// 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 - d_rx_dt_s * GPS_C_m_s;
obs(valid_obs) = gnss_observables_iter->second.Pseudorange_m + dtr*GPS_C_m_s + SV_clock_bias_s * GPS_C_m_s;
d_visible_satellites_IDs[valid_obs] = gps_cnav_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_observables_iter->second.CN0_dB_hz;
@@ -296,6 +287,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
// ****** SOLVE LEAST SQUARES******************************************************
// ********************************************************************************
d_valid_observations = valid_obs;
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
if(valid_obs >= 4)
@@ -304,7 +296,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W;
try
{
// check if this is the initial position computation
@@ -323,7 +314,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
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]
DLOG(INFO) << "Hybrid Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time;
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=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]";
// Compute GST and Gregorian time
@@ -349,7 +340,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
<< " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
// ###### Compute DOPs ########
compute_DOP();
hybrid_ls_pvt::compute_DOP();
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
@@ -359,7 +350,7 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
{
double tmp_double;
// PVT GPS time
tmp_double = Rx_time;
tmp_double = hybrid_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m]
tmp_double = rx_position_and_time(0);
@@ -396,9 +387,6 @@ bool hybrid_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_observables_map, dou
{
d_rx_dt_s = 0; //reset rx time estimation
LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what();
LOG(WARNING) << "satpos=" << satpos;
LOG(WARNING) << "obs=" << obs;
LOG(WARNING) << "W=" << W;
b_valid_position = false;
}
}