mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-05-17 23:04:08 +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
Show Stats 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 = 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_ls_pvt->set_averaging_depth(d_averaging_depth);
d_sample_counter = 0;
d_last_sample_nav_output = 0; d_last_sample_nav_output = 0;
b_rinex_header_written = false; 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) bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff)
{ {
/* Fill Sys V message structures */ /* 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 , 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))) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused)))
{ {
//std::cout << "noutput items: " << noutput_items << std::endl; Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
//std::cout << "ninput items: " << ninput_items[0] << std::endl;
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
double d_rx_time[noutput_items]; double d_rx_time[ninput_items[0]];
for(unsigned int item = 0; item < noutput_items; item++) for(unsigned int item = 0; item < ninput_items[0]; item++)
{ {
d_sample_counter++; //d_sample_counter++;
unsigned int gps_channel = 0; unsigned int gps_channel = 0;
unsigned int gal_channel = 0; unsigned int gal_channel = 0;
gnss_observables_map.clear(); gnss_observables_map.clear();
//print_receiver_status(in);
print_receiver_status(in);
// ############ 1. READ PSEUDORANGES #### // ############ 1. READ PSEUDORANGES ####
for (unsigned int i = 0; i < d_nchannels; i++) 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, Gps_CNAV_Ephemeris>::iterator gps_cnav_ephemeris_iter;
std::map<int, Gnss_Synchro>::iterator gnss_observables_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 * TYPE | RECEIVER
* 0 | Unknown * 0 | Unknown
@ -528,7 +513,7 @@ Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
if (gnss_observables_map.size() > 0) if (gnss_observables_map.size() > 0)
{ {
// compute on the fly PVT solution // 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; bool pvt_result;
pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time[item], d_flag_averaging); 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; << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl;
ttff_msgbuf ttff; ttff_msgbuf ttff;
ttff.mtype = 1; ttff.mtype = 1;
ttff.ttff = d_sample_counter; ttff.ttff = d_rx_time[item]; //d_sample_counter;
send_sys_v_ttff_msg(ttff); send_sys_v_ttff_msg(ttff);
first_fix = false; 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 // Limit the RINEX navigation output rate
// Notice that d_sample_counter period is 4ms (for Galileo correlators) // 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 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); 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(); galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin();
gps_ephemeris_iter = d_ls_pvt->gps_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(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++ ) 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); 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; std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); 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((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++ ) 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); 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; std::map<int,Galileo_Ephemeris>::iterator gal_ephemeris_iter;
gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); 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(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++ ) 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); 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; std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); 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(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++ ) 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); 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++ ) 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); 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(); //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
//galileo_ephemeris_iter = d_ls_pvt->galileo_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++; 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()) 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); 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()) 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 // 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) 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 << " 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) 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]"; << " [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) /* 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 consume_each(ninput_items[0]);
return noutput_items; 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_MT1097_rate_ms;
int d_rtcm_MSM_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 int d_last_status_print_seg; //for status printer
unsigned int d_nchannels; unsigned int d_nchannels;
@ -133,7 +132,6 @@ private:
bool d_flag_averaging; bool d_flag_averaging;
int d_output_rate_ms; int d_output_rate_ms;
int d_display_rate_ms; int d_display_rate_ms;
long unsigned int d_sample_counter;
long unsigned int d_last_sample_nav_output; long unsigned int d_last_sample_nav_output;
std::shared_ptr<Rinex_Printer> rp; 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 "hybrid_ls_pvt.h"
#include <glog/logging.h> #include <glog/logging.h>
#include "GPS_L1_CA.h"
#include "Galileo_E1.h" #include "Galileo_E1.h"
#include "GPS_L1_CA.h"
#include "GPS_L2C.h"
using google::LogMessage; using google::LogMessage;
@ -71,7 +72,8 @@ hybrid_ls_pvt::~hybrid_ls_pvt()
d_dump_file.close(); 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,Gnss_Synchro>::iterator gnss_observables_iter;
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_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 GST = 0.0;
double secondsperweek = 604800.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 TX_time_corrected_s = 0.0;
double SV_clock_bias_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 int valid_obs = 0; //valid observations counter
for(gnss_observables_iter = gnss_observables_map.begin(); for(gnss_observables_iter = gnss_observables_map.begin();
gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter != gnss_observables_map.end();
gnss_observables_iter++) 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; W(valid_obs) = 1;
// COMMON RX TIME PVT ALGORITHM // 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 / GALILEO_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV // 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; 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, Rx_time); GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, hybrid_current_time);
// SV ECEF DEBUG OUTPUT // SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN 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) // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time // 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 / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect // 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 // 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; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
//compute satellite position, clock bias + relativistic correction double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
double dts = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
//store satellite positions in a matrix //store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); 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 P1_P2=(1.0-Gamma)*(gps_ephemeris_iter->second.d_TGD* GPS_C_m_s);
double Code_bias_m= P1_P2/(1.0-Gamma); double Code_bias_m= P1_P2/(1.0-Gamma);
obs.resize(valid_obs + 1, 1); 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_IDs[valid_obs] = gps_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;
// SV ECEF DEBUG OUTPUT // SV ECEF DEBUG OUTPUT
LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN LOG(INFO) << "(new)ECEF GPS L1 CA satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN
<< " TX Time corrected="<<TX_time_corrected_s << " TX Time corrected="<<TX_time_corrected_s << " X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] X=" << gps_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(valid_obs) << " [m]"; << " [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++; valid_obs++;
// compute the UTC time for this SV (just to print the associated UTC timestamp) // compute the UTC time for this SV (just to print the associated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week; 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) // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files)
// first estimate of transmit time // 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 / GPS_C_m_s;
// 2- compute the clock drift using the clock model (broadcast) for this SV // 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); 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 // 3- compute the current ECEF position for this SV using corrected TX time
TX_time_corrected_s = Tx_time - SV_clock_bias_s; 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; //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); 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 //store satellite positions in a matrix
satpos.resize(3, valid_obs + 1); satpos.resize(3, valid_obs + 1);
satpos(0, valid_obs) = gps_cnav_ephemeris_iter->second.d_satpos_X; 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 // 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 + 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_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; 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****************************************************** // ****** SOLVE LEAST SQUARES******************************************************
// ******************************************************************************** // ********************************************************************************
d_valid_observations = valid_obs; d_valid_observations = valid_obs;
LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs; LOG(INFO) << "HYBRID PVT: valid observations=" << valid_obs;
if(valid_obs >= 4) 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) << "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
@ -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_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) << "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]"; 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 // 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]"; << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]";
// ###### Compute DOPs ######## // ###### Compute DOPs ########
compute_DOP(); hybrid_ls_pvt::compute_DOP();
// ######## LOG FILE ######### // ######## LOG FILE #########
if(d_flag_dump_enabled == true) 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; double tmp_double;
// PVT GPS time // PVT GPS time
tmp_double = Rx_time; tmp_double = hybrid_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
// ECEF User Position East [m] // ECEF User Position East [m]
tmp_double = rx_position_and_time(0); 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 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();
LOG(WARNING) << "satpos=" << satpos;
LOG(WARNING) << "obs=" << obs;
LOG(WARNING) << "W=" << W;
b_valid_position = false; b_valid_position = false;
} }
} }