mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-06 02:03:04 +00:00
Code Issues Releases Wiki Activity

Major update to improve the GNSS-SDR pseudorange precision and correct some PVT bugs:

Browse Source 
- Updated all available trackings to generate the tracking_timestamp_secs taking into account the remainder code phase.
- Updated the telemetry decoder to track the number of symbol shifted from the preamble start symbol (to be used in observables).
- Updated observables to align the reference channel symbol with the corresponding symbols in the other channels and compute pseudorranges using the common transmission time algorithm.
- Updated PVT to independize the display output rate from the RINEX and KML log files. New options available in config file!
- Some minor improvements and code cleaning.

git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@193 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas
2012-04-11 10:43:35 +00:00
parent 89a67c93e8
commit 9d34147e0f
33 changed files with 932 additions and 502 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc
View File

@@ -63,10 +63,15 @@ GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration,
bool flag_averaging;
flag_averaging = configuration->property(role + ".flag_averaging", false);
int output_rate_ms;
output_rate_ms = configuration->property(role + ".output_rate_ms", 500);
int display_rate_ms;
display_rate_ms = configuration->property(role + ".display_rate_ms", 500);
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
pvt_ = gps_l1_ca_make_pvt_cc(in_streams_, queue_, dump_, dump_filename_, averaging_depth, flag_averaging);
pvt_ = gps_l1_ca_make_pvt_cc(in_streams_, queue_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms);
DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")";
// set the navigation msg queue;

133
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc
View File

@@ -40,41 +40,49 @@
#include <glog/log_severity.h>
#include <glog/logging.h>
#include "control_message_factory.h"
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
using google::LogMessage;
gps_l1_ca_pvt_cc_sptr
gps_l1_ca_make_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging)
gps_l1_ca_make_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms)
{
return gps_l1_ca_pvt_cc_sptr(new gps_l1_ca_pvt_cc(nchannels, queue, dump, dump_filename, averaging_depth, flag_averaging));
return gps_l1_ca_pvt_cc_sptr(new gps_l1_ca_pvt_cc(nchannels, queue, dump, dump_filename, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms));
}
gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging) :
gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms) :
gr_block ("gps_l1_ca_pvt_cc", gr_make_io_signature (nchannels, nchannels, sizeof(Gnss_Synchro)),
gr_make_io_signature(1, 1, sizeof(gr_complex)))
{
d_output_rate_ms = output_rate_ms;
d_display_rate_ms=display_rate_ms;
d_queue = queue;
d_dump = dump;
d_nchannels = nchannels;
d_dump_filename = dump_filename;
std::string dump_ls_pvt_filename;
dump_ls_pvt_filename=dump_filename;
std::string kml_dump_filename;
kml_dump_filename = d_dump_filename;
kml_dump_filename.append(".kml");
d_kml_dump.set_headers(kml_dump_filename);
d_dump_filename.append(".dat");
d_dump_filename.append("_raw.dat");
dump_ls_pvt_filename.append("_ls_pvt.dat");
d_averaging_depth = averaging_depth;
d_flag_averaging = flag_averaging;
d_ls_pvt = new gps_l1_ca_ls_pvt(nchannels,d_dump_filename,d_dump);
d_ls_pvt = new gps_l1_ca_ls_pvt(nchannels,dump_ls_pvt_filename,d_dump);
d_ls_pvt->set_averaging_depth(d_averaging_depth);
d_ephemeris_clock_s = 0.0;
d_sample_counter = 0;
d_tx_time=0.0;
b_rinex_header_writen = false;
rp = new Rinex_Printer();
@@ -82,6 +90,24 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr que
{
nav_data_map[i] = Gps_Navigation_Message();
}
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout << "PVT dump enabled Log file: " << d_dump_filename.c_str() << std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "Exception opening PVT dump file " << e.what() << std::endl;
}
}
}
}
@@ -127,8 +153,6 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
{
double pr = gnss_pseudoranges_iter->second.Pseudorange_m;
pseudoranges[gnss_pseudoranges_iter->first] = pr;
// std::cout << "Pseudoranges(SV ID,pseudorange [m]) =(" << gnss_pseudoranges_iter->first << ","
// << gnss_pseudoranges_iter->second.pseudorange_m << ")" <<std::endl;
}
// ############ 1. READ EPHEMERIS FROM QUEUE ######################
@@ -143,41 +167,86 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
<< nav_msg.satelliteBlock[nav_msg.i_satellite_PRN]
<< ")" << std::endl;
d_last_nav_msg = nav_msg;
d_ls_pvt->d_ephemeris[nav_msg.i_channel_ID] = nav_msg;
nav_data_map[nav_msg.i_channel_ID] = nav_msg;
if (nav_msg.b_valid_ephemeris_set_flag==true)
{
d_ls_pvt->d_ephemeris[nav_msg.i_channel_ID] = nav_msg;
nav_data_map[nav_msg.i_channel_ID] = nav_msg;
}
// **** update pseudoranges clock ****
if (nav_msg.i_satellite_PRN == gnss_pseudoranges_iter->second.PRN)
{
d_ephemeris_clock_s = d_last_nav_msg.d_TOW;
d_ephemeris_timestamp_ms = d_last_nav_msg.d_subframe1_timestamp_ms;
d_ephemeris_timestamp_ms = d_last_nav_msg.d_subframe_timestamp_ms;
}
}
// ############ 2. COMPUTE THE PVT ################################
// write the pseudoranges to RINEX OBS file
// 1- need a valid clock
if (d_ephemeris_clock_s > 0 and d_last_nav_msg.i_satellite_PRN > 0)
if (d_ephemeris_clock_s > 0 and d_last_nav_msg.i_satellite_PRN > 0 and d_last_nav_msg.b_valid_ephemeris_set_flag==true)
{
// compute on the fly PVT solution
//std::cout<<"diff_clock_ephemeris="<<(gnss_pseudoranges_iter->second.timestamp_ms-d_ephemeris_timestamp_ms)/1000.0<<"\r\n";
if (d_ls_pvt->get_PVT(gnss_pseudoranges_map,
d_ephemeris_clock_s + (gnss_pseudoranges_iter->second.Pseudorange_timestamp_ms - d_ephemeris_timestamp_ms)/1000.0,
d_flag_averaging) == true)
{
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);
if (!b_rinex_header_writen) // & we have utc data in nav message!
{
rp->rinex_nav_header(rp->navFile, d_last_nav_msg);
rp->rinex_obs_header(rp->obsFile, d_last_nav_msg);
b_rinex_header_writen = true; // do not write header anymore
}
if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s)
{
rp->log_rinex_nav(rp->navFile, nav_data_map);
rp->log_rinex_obs(rp->obsFile, d_last_nav_msg, pseudoranges);
}
}
double clock_error;
double satellite_tx_time_using_timestamps;
//for GPS L1 C/A: t_tx=TOW+N_symbols_from_TOW*T_symbol
//Notice that the TOW is decoded AFTER processing the subframe -> we ned to add ONE subframe duration to t_tx
d_tx_time=d_ephemeris_clock_s + gnss_pseudoranges_iter->second.Pseudorange_symbol_shift/(double)GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND+GPS_SUBFRAME_SECONDS;
//Perform an extra check to verify the TOW update (the ephemeris queue is ASYNCHRONOUS to the GNU Radio Gnss_Synchro sample stream)
//-> compute the t_tx_timestamps using the symbols timestamp (it is affected by code Doppler, but it is not wrapped like N_symbols_from_TOW)
satellite_tx_time_using_timestamps=d_ephemeris_clock_s + (gnss_pseudoranges_iter->second.Pseudorange_timestamp_ms-d_ephemeris_timestamp_ms)/1000.0;
//->compute the absolute error between both T_tx
clock_error=std::abs(d_tx_time-satellite_tx_time_using_timestamps);
// -> The symbol conter N_symbols_from_TOW will be resetted every new received telemetry word, if the TOW is not uptated, both t_tx and t_tx_timestamps times will difer by more than 1 seconds.
if (clock_error<1){
// compute on the fly PVT solution
//mod 8/4/2012 Set the PVT computation rate in this block
if ((d_sample_counter % d_output_rate_ms) == 0)
{
if (d_ls_pvt->get_PVT(gnss_pseudoranges_map,d_tx_time,d_flag_averaging) == true)
{
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);
if (!b_rinex_header_writen) // & we have utc data in nav message!
{
rp->rinex_nav_header(rp->navFile, d_last_nav_msg);
rp->rinex_obs_header(rp->obsFile, d_last_nav_msg);
b_rinex_header_writen = true; // do not write header anymore
}
if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s)
{
rp->log_rinex_nav(rp->navFile, nav_data_map);
rp->log_rinex_obs(rp->obsFile, d_last_nav_msg, pseudoranges);
}
}
}
if (((d_sample_counter % 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)
<< " 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;
}
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++)
{
tmp_double = in[i][0].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = in[i][0].Pseudorange_symbol_shift;
d_dump_file.write((char*)&tmp_double, sizeof(double));
d_dump_file.write((char*)&d_tx_time, sizeof(double));
}
}
catch (std::ifstream::failure e)
{
std::cout << "Exception writing observables dump file " << e.what() << std::endl;
}
}
}
}
consume_each(1); //one by one

10
src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h
View File

@@ -48,7 +48,7 @@ class gps_l1_ca_pvt_cc;
typedef boost::shared_ptr<gps_l1_ca_pvt_cc> gps_l1_ca_pvt_cc_sptr;
gps_l1_ca_pvt_cc_sptr
gps_l1_ca_make_pvt_cc(unsigned int n_channels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging);
gps_l1_ca_make_pvt_cc(unsigned int n_channels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms);
/*!
* \brief This class implements a block that computes the PVT solution
@@ -59,9 +59,9 @@ class gps_l1_ca_pvt_cc : public gr_block
private:
friend gps_l1_ca_pvt_cc_sptr
gps_l1_ca_make_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging);
gps_l1_ca_make_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms);
gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging);
gps_l1_ca_pvt_cc(unsigned int nchannels, gr_msg_queue_sptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms);
gr_msg_queue_sptr d_queue;
bool d_dump;
@@ -75,7 +75,8 @@ private:
int d_averaging_depth;
bool d_flag_averaging;
int d_output_rate_ms;
int d_display_rate_ms;
long unsigned int d_sample_counter;
Kml_Printer d_kml_dump;
@@ -85,6 +86,7 @@ private:
double d_ephemeris_clock_s;
double d_ephemeris_timestamp_ms;
double d_tx_time;
gps_l1_ca_ls_pvt *d_ls_pvt;
std::map<int,Gps_Navigation_Message> nav_data_map;

17
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc
View File

@@ -47,7 +47,8 @@ gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels,std::string dump_filename, bool
d_dump_filename = dump_filename;
d_flag_dump_enabled = flag_dump_to_file;
d_averaging_depth = 0;
d_GPS_current_time=0;;
b_valid_position=false;
// ############# ENABLE DATA FILE LOG #################
if (d_flag_dump_enabled == true)
{
@@ -277,7 +278,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
obs(i) = 1; // to avoid algorithm problems (divide by zero)
}
}
std::cout<<"PVT: valid observations="<<valid_obs<<std::endl;
LOG_AT_LEVEL(INFO) <<"PVT: valid observations="<<valid_obs<<std::endl;
if (valid_obs>=4)
{
arma::vec mypos;
@@ -286,11 +287,15 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
cart2geo(mypos(0), mypos(1), mypos(2), 4);
// Compute UTC time and print PVT solution
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + 604800*(double)GPS_week);
boost::posix_time::time_duration t = boost::posix_time::seconds(utc + 604800.0*(double)GPS_week);
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
std::cout << "Position at " << boost::posix_time::to_simple_string(p_time)
d_position_UTC_time = p_time;
GPS_current_time=GPS_corrected_time;
LOG_AT_LEVEL(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << std::endl;
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
@@ -355,6 +360,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
d_avg_latitude_d = d_avg_latitude_d / (double)d_averaging_depth;
d_avg_longitude_d = d_avg_longitude_d / (double)d_averaging_depth;
d_avg_height_m = d_avg_height_m / (double)d_averaging_depth;
b_valid_position=true;
return true; //indicates that the returned position is valid
}
else
@@ -368,6 +374,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
d_avg_latitude_d = d_latitude_d;
d_avg_longitude_d = d_longitude_d;
d_avg_height_m = d_height_m;
b_valid_position=false;
return false;//indicates that the returned position is not valid yet
// output the average, although it will not have the full historic available
// d_avg_latitude_d=0;
@@ -386,11 +393,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
}
else
{
b_valid_position=true;
return true;//indicates that the returned position is valid
}
}
else
{
b_valid_position=false;
return false;
}
}

7
src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h
View File

@@ -44,6 +44,7 @@
#include "gps_navigation_message.h"
#include "GPS_L1_CA.h"
#include "armadillo"
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
@@ -59,7 +60,11 @@ private:
public:
int d_nchannels; //! Number of available channels for positioning
Gps_Navigation_Message* d_ephemeris;
double d_pseudoranges_time_ms;
double d_GPS_current_time;
boost::posix_time::ptime d_position_UTC_time;
bool b_valid_position;
double d_latitude_d; //! Latitude in degrees
double d_longitude_d; //! Longitude in degrees
double d_height_m; //! Height [m]

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

@@ -41,11 +41,15 @@ bool Kml_Printer::set_headers(std::string filename)
time ( &rawtime );
timeinfo = localtime ( &rawtime );
kml_file.open(filename.c_str());
if (kml_file.is_open())
{
DLOG(INFO) << "KML printer writing on " << filename.c_str();
// Set iostream numeric format and precision
kml_file.setf(kml_file.fixed,kml_file.floatfield);
kml_file<<std::setprecision(14);
kml_file << "<?xml version=\"1.0\" encoding=\"UTF-8\"?>" << std::endl
<< "<kml xmlns=\"http://www.opengis.net/kml/2.2\">" << std::endl
<< " <Document>" << std::endl

207
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc
View File

@@ -65,7 +65,7 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
d_dump = dump;
d_nchannels = nchannels;
d_output_rate_ms = output_rate_ms;
d_history_prn_delay_ms = new std::deque<double>[d_nchannels];
d_history_gnss_synchro_deque = new std::deque<Gnss_Synchro>[d_nchannels];
d_dump_filename = dump_filename;
d_flag_averaging = flag_averaging;
@@ -90,13 +90,23 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, gr_ms
gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc()
{
d_dump_file.close();
delete[] d_history_prn_delay_ms;
delete[] d_history_gnss_synchro_deque;
}
bool pairCompare_gnss_synchro( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
bool pairCompare_gnss_synchro_Prn_delay_ms( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
return (a.second.Preamble_delay_ms) < (b.second.Preamble_delay_ms);
return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms);
}
bool pairCompare_gnss_synchro_preamble_symbol_count( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
return (a.second.Preamble_symbol_counter) < (b.second.Preamble_symbol_counter);
}
bool pairCompare_gnss_synchro_preamble_delay_ms( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{
return (a.second.Preamble_timestamp_ms) < (b.second.Preamble_timestamp_ms);
}
@@ -121,30 +131,20 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
Gnss_Synchro current_gnss_synchro[d_nchannels];
std::map<int,Gnss_Synchro> gps_words;
std::map<int,Gnss_Synchro>::iterator gps_words_iter;
std::map<int,double>::iterator current_prn_timestamps_ms_iter;
std::map<int,double> current_prn_timestamps_ms;
std::map<int,Gnss_Synchro> current_gnss_synchro_map;
std::map<int,Gnss_Synchro> gnss_synchro_aligned_map;
std::map<int,Gnss_Synchro>::iterator gnss_synchro_iter;
double min_preamble_delay_ms;
double max_preamble_delay_ms;
double pseudoranges_timestamp_ms;
double traveltime_ms;
double pseudorange_m;
double delta_timestamp_ms;
double min_delta_timestamp_ms;
double actual_min_prn_delay_ms;
double current_prn_delay_ms;
int history_shift = 0;
int pseudoranges_reference_sat_ID = 0;
unsigned int pseudoranges_reference_sat_channel_ID = 0;
d_sample_counter++; //count for the processed samples
bool flag_history_ok = true; //flag to indicate that all the queues have filled their timestamp history
bool flag_history_ok = true; //flag to indicate that all the queues have filled their GNSS SYNCHRO history
/*
* 1. Read the GNSS SYNCHRO objects from available channels to obtain the preamble timestamp, current PRN start time and accumulated carrier phase
* 1. Read the GNSS SYNCHRO objects from available channels
*/
for (unsigned int i=0; i<d_nchannels ; i++)
{
@@ -153,18 +153,18 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word
{
gps_words.insert(std::pair<int,Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); //record the word structure in a map for pseudoranges
current_gnss_synchro_map.insert(std::pair<int,Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); //record the word structure in a map for pseudoranges
// RECORD PRN start timestamps history
if (d_history_prn_delay_ms[i].size()<MAX_TOA_DELAY_MS)
if (d_history_gnss_synchro_deque[i].size()<MAX_TOA_DELAY_MS)
{
d_history_prn_delay_ms[i].push_front(current_gnss_synchro[i].Prn_delay_ms);
d_history_gnss_synchro_deque[i].push_front(current_gnss_synchro[i]);
flag_history_ok = false; // at least one channel need more samples
}
else
{
//clearQueue(d_history_prn_delay_ms[i]); //clear the queue as the preamble arrives
d_history_prn_delay_ms[i].pop_back();
d_history_prn_delay_ms[i].push_front(current_gnss_synchro[i].Prn_delay_ms);
d_history_gnss_synchro_deque[i].pop_back();
d_history_gnss_synchro_deque[i].push_front(current_gnss_synchro[i]);
}
}
}
@@ -176,80 +176,89 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
{
current_gnss_synchro[i].Flag_valid_pseudorange = false;
current_gnss_synchro[i].Pseudorange_m = 0.0;
current_gnss_synchro[i].Pseudorange_timestamp_ms = 0.0;
current_gnss_synchro[i].Pseudorange_symbol_shift = 0.0;
}
/*
* 2. Compute RAW pseudorranges: Use only the valid channels (channels that are tracking a satellite)
*/
if(gps_words.size() > 0 and flag_history_ok == true)
if(current_gnss_synchro_map.size() > 0 and flag_history_ok == true)
{
/*
* 2.1 find the minimum preamble timestamp (nearest satellite, will be the reference)
*/
// The nearest satellite, first preamble to arrive
gps_words_iter = min_element(gps_words.begin(), gps_words.end(), pairCompare_gnss_synchro);
min_preamble_delay_ms = gps_words_iter->second.Preamble_delay_ms; //[ms]
pseudoranges_reference_sat_ID = gps_words_iter->second.PRN; // it is the reference!
pseudoranges_reference_sat_channel_ID = gps_words_iter->second.Channel_ID;
/*
* 2.1 Find the correct symbol timestamp in the gnss_synchro history: we have to compare timestamps between channels on the SAME symbol
* (common TX time algorithm)
*/
// The farthest satellite, last preamble to arrive
gps_words_iter = max_element(gps_words.begin(), gps_words.end(), pairCompare_gnss_synchro);
max_preamble_delay_ms = gps_words_iter->second.Preamble_delay_ms;
min_delta_timestamp_ms = gps_words_iter->second.Prn_delay_ms - max_preamble_delay_ms; //[ms]
double min_preamble_delay_ms;
double max_preamble_delay_ms;
int current_symbol=0;
int reference_channel;
int history_shift;
Gnss_Synchro tmp_gnss_synchro;
// check if this is a valid set of observations
if ((max_preamble_delay_ms - min_preamble_delay_ms) < MAX_TOA_DELAY_MS)
{
// Now we have to determine were we are in time, compared with the last preamble! -> we select the corresponding history
/*!
* \todo Explain this better!
*/
//bool flag_preamble_navigation_now=true;
// find again the minimum CURRENT minimum preamble time, taking into account the preamble timeshift
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{
delta_timestamp_ms = (gps_words_iter->second.Prn_delay_ms - gps_words_iter->second.Preamble_delay_ms) - min_delta_timestamp_ms;
history_shift = round(delta_timestamp_ms);
//std::cout<<"history_shift="<<history_shift<<"\r\n";
current_prn_timestamps_ms.insert(std::pair<int,double>(gps_words_iter->second.Channel_ID, d_history_prn_delay_ms[gps_words_iter->second.Channel_ID][history_shift]));
// debug: preamble position test
//if ((d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]-gps_words_iter->second.preamble_delay_ms)<0.1)
//{std::cout<<"ch "<<gps_words_iter->second.channel_ID<<" current_prn_time-last_preamble_prn_time="<<
// d_history_prn_delay_ms[gps_words_iter->second.channel_ID][history_shift]-gps_words_iter->second.preamble_delay_ms<<"\r\n";
//}else{
// flag_preamble_navigation_now=false;
//}
}
gnss_synchro_iter = min_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_preamble_delay_ms);
min_preamble_delay_ms = gnss_synchro_iter->second.Preamble_timestamp_ms; //[ms]
//if (flag_preamble_navigation_now==true)
//{
//std::cout<<"PREAMBLE NAVIGATION NOW!\r\n";
//d_sample_counter=0;
//}
current_prn_timestamps_ms_iter = min_element(current_prn_timestamps_ms.begin(), current_prn_timestamps_ms.end(), pairCompare_double);
gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_preamble_delay_ms);
max_preamble_delay_ms = gnss_synchro_iter->second.Preamble_timestamp_ms; //[ms]
actual_min_prn_delay_ms = current_prn_timestamps_ms_iter->second;
if ((max_preamble_delay_ms-min_preamble_delay_ms)< MAX_TOA_DELAY_MS)
{
// we have a valid information set. Its time to align the symbols information
// what is the most delayed symbol in the current set? -> this will be the reference symbol
gnss_synchro_iter=min_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_preamble_symbol_count);
current_symbol=gnss_synchro_iter->second.Preamble_symbol_counter;
reference_channel=gnss_synchro_iter->second.Channel_ID;
// save it in the aligned symbols map
gnss_synchro_aligned_map.insert(std::pair<int,Gnss_Synchro>(gnss_synchro_iter->second.Channel_ID, gnss_synchro_iter->second));
pseudoranges_timestamp_ms = actual_min_prn_delay_ms; //save the shortest pseudorange timestamp to compute the current GNSS timestamp
/*
* 2.2 compute the pseudoranges
*/
// Now find where the same symbols were in the rest of the channels searching in the symbol history
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
{
//TODO: Replace the loop using current current_symbol-Preamble_symbol_counter
if (reference_channel!=gnss_synchro_iter->second.Channel_ID)
{
// compute the required symbol history shift in order to match the reference symbol
history_shift=gnss_synchro_iter->second.Preamble_symbol_counter-current_symbol;
if (history_shift<(int)MAX_TOA_DELAY_MS)// and history_shift>=0)
{
tmp_gnss_synchro= d_history_gnss_synchro_deque[gnss_synchro_iter->second.Channel_ID][history_shift];
gnss_synchro_aligned_map.insert(std::pair<int,Gnss_Synchro>(gnss_synchro_iter->second.Channel_ID,tmp_gnss_synchro));
}
}
}
}
for(gps_words_iter = gps_words.begin(); gps_words_iter != gps_words.end(); gps_words_iter++)
{
// #### compute the pseudorange for this satellite ###
/*
* 3 Compute the pseudorranges using the aligned data map
*/
double min_symbol_timestamp_ms;
double max_symbol_timestamp_ms;
gnss_synchro_iter = min_element(gnss_synchro_aligned_map.begin(), gnss_synchro_aligned_map.end(), pairCompare_gnss_synchro_Prn_delay_ms);
min_symbol_timestamp_ms = gnss_synchro_iter->second.Prn_timestamp_ms; //[ms]
current_prn_delay_ms = current_prn_timestamps_ms.at(gps_words_iter->second.Channel_ID);
traveltime_ms = current_prn_delay_ms - actual_min_prn_delay_ms + GPS_STARTOFFSET_ms; //[ms]
//std::cout<<"delta_time_ms="<<current_prn_delay_ms-actual_min_prn_delay_ms<<"\r\n";
pseudorange_m = traveltime_ms*GPS_C_m_ms; // [m]
// update the pseudorange object
current_gnss_synchro[gps_words_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
current_gnss_synchro[gps_words_iter->second.Channel_ID].Pseudorange_timestamp_ms = pseudoranges_timestamp_ms;
current_gnss_synchro[gps_words_iter->second.Channel_ID].Flag_valid_pseudorange = true;
}
}
gnss_synchro_iter = max_element(gnss_synchro_aligned_map.begin(), gnss_synchro_aligned_map.end(), pairCompare_gnss_synchro_Prn_delay_ms);
max_symbol_timestamp_ms = gnss_synchro_iter->second.Prn_timestamp_ms; //[ms]
// check again if this is a valid set of observations
if ((max_symbol_timestamp_ms - min_symbol_timestamp_ms) < MAX_TOA_DELAY_MS)
/*
* 2.3 compute the pseudoranges
*/
{
for(gnss_synchro_iter = gnss_synchro_aligned_map.begin(); gnss_synchro_iter != gnss_synchro_aligned_map.end(); gnss_synchro_iter++)
{
traveltime_ms = gnss_synchro_iter->second.Prn_timestamp_ms - min_symbol_timestamp_ms + GPS_STARTOFFSET_ms; //[ms]
pseudorange_m = traveltime_ms*GPS_C_m_ms; // [m]
// update the pseudorange object
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID]=gnss_synchro_iter->second;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_symbol_shift = (double)current_symbol; // number of symbols shifted from preamble start symbol
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_timestamp_ms=max_symbol_timestamp_ms;
}
}
}
@@ -261,13 +270,13 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++)
{
tmp_double = current_gnss_synchro[i].Preamble_delay_ms;
tmp_double = current_gnss_synchro[i].Preamble_timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Prn_delay_ms;
tmp_double = current_gnss_synchro[i].Prn_timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].Pseudorange_timestamp_ms;
tmp_double = current_gnss_synchro[i].Pseudorange_symbol_shift;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_gnss_synchro[i].PRN;
d_dump_file.write((char*)&tmp_double, sizeof(double));
@@ -280,21 +289,19 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
}
consume_each(1); //one by one
if ((d_sample_counter % d_output_rate_ms) == 0)
{
// mod 8/4/2012: always make the observables output
//if ((d_sample_counter % d_output_rate_ms) == 0)
// {
for (unsigned int i=0; i<d_nchannels ; i++)
{
*out[i] = current_gnss_synchro[i];
}
return 1; //Output the observables
}
else
{
return 0; //hold on
}
// }
//else
// {
// return 0; //hold on
// }
}

6
src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h
View File

@@ -41,6 +41,7 @@
#include "gps_navigation_message.h"
#include "rinex_printer.h"
#include "GPS_L1_CA.h"
#include "gnss_synchro.h"
class gps_l1_ca_observables_cc;
@@ -83,7 +84,10 @@ private:
std::string d_dump_filename;
std::ofstream d_dump_file;
std::deque<double> *d_history_prn_delay_ms;
//std::deque<double> *d_history_prn_delay_ms;
std::deque<Gnss_Synchro> *d_history_gnss_synchro_deque;
concurrent_queue<Gps_Navigation_Message> *d_nav_queue; // Navigation ephemeris queue
};

82
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@@ -41,6 +41,7 @@
#include <gnuradio/gr_io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
#include <boost/lexical_cast.hpp>
#include "control_message_factory.h"
#include "gnss_synchro.h"
@@ -92,7 +93,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
d_vector_length = vector_length;
d_samples_per_bit = ( GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS ) / GPS_CA_TELEMETRY_RATE_BITS_SECOND;
d_fs_in = fs_in;
d_preamble_duration_seconds = (1.0 / GPS_CA_TELEMETRY_RATE_BITS_SECOND) * GPS_CA_PREAMBLE_LENGTH_BITS;
//d_preamble_duration_seconds = (1.0 / GPS_CA_TELEMETRY_RATE_BITS_SECOND) * GPS_CA_PREAMBLE_LENGTH_BITS;
//std::cout<<"d_preamble_duration_seconds="<<d_preamble_duration_seconds<<"\r\n";
// set the preamble
unsigned short int preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE;
@@ -124,7 +125,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
d_symbol_accumulator=0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_preamble_time_seconds=0;
d_flag_frame_sync = false;
d_GPS_frame_4bytes = 0;
d_prev_GPS_frame_4bytes = 0;
@@ -175,37 +176,19 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
int corr_value = 0;
int preamble_diff;
int preamble_diff=0;
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
d_sample_counter++; //count for the processed samples
DLOG(INFO) << "Sample counter: " << d_sample_counter;
// ########### Output the tracking data to navigation and PVT ##########
const Gnss_Synchro **in = (const Gnss_Synchro **) &input_items[0]; //Get the input samples pointer
/*!
* \todo Check the HOW GPS time computation, taking into account that the preamble correlation last 160 symbols, which is 160 ms in GPS CA L1
*/
// FIFO history to get the exact timestamp of the first symbol of the preamble
// if (d_prn_start_sample_history.size()<160)
// {
// // fill the queue
// d_prn_start_sample_history.push_front(in[2][0]);
// consume_each(1); //one by one
// return 1;
// }else{
// d_prn_start_sample_history.pop_back();
// d_prn_start_sample_history.push_front(in[2][0]);
// }
// TODO Optimize me!
//******* preamble correlation ********
for (unsigned int i=0; i<d_samples_per_bit*8; i++)
{
if (in[0][i].Prompt_Q < 0) // symbols clipping
if (in[0][i].Prompt_I < 0) // symbols clipping
{
corr_value -= d_preambles_symbols[i];
}
@@ -237,7 +220,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
d_GPS_FSM.Event_gps_word_preamble();
d_flag_preamble = true;
d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P)
d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble
d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs;// - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble
d_preamble_code_phase_seconds = in[0][0].Code_phase_secs;
if (!d_flag_frame_sync)
@@ -262,11 +245,9 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
}
}
//******* code error accumulator *****
//d_preamble_phase-=in[3][0];
//******* SYMBOL TO BIT *******
d_symbol_accumulator += in[0][d_samples_per_bit*8 - 1].Prompt_Q; // accumulate the input value in d_symbol_accumulator
d_symbol_accumulator += in[0][d_samples_per_bit*8 - 1].Prompt_I; // accumulate the input value in d_symbol_accumulator
d_symbol_accumulator_counter++;
if (d_symbol_accumulator_counter == 20)
{
@@ -324,7 +305,6 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
// output the frame
consume_each(1); //one by one
DLOG(INFO) << "TELEMETRY PROCESSED for satellite " << this->d_satellite;
Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block
@@ -333,12 +313,28 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
//2. Add the telemetry decoder information
current_synchro_data.Flag_valid_word=(d_flag_frame_sync == true and d_flag_parity == true);
current_synchro_data.Flag_preamble= d_flag_preamble;
current_synchro_data.Preamble_delay_ms= d_preamble_time_seconds*1000.0;
current_synchro_data.Prn_delay_ms = (in[0][0].Tracking_timestamp_secs - d_preamble_duration_seconds)*1000.0;
current_synchro_data.Preamble_code_phase_ms = d_preamble_code_phase_seconds*1000.0;
current_synchro_data.Preamble_code_phase_correction_ms = (in[0][0].Code_phase_secs - d_preamble_code_phase_seconds)*1000.0;
//gps_synchro.satellite_PRN = this->d_satellite.get_PRN(); //is already filled...
//gps_synchro.channel_ID = d_channel;
current_synchro_data.Preamble_timestamp_ms= d_preamble_time_seconds*1000.0;
current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs*1000.0;
current_synchro_data.Preamble_symbol_counter=fmod((double)(d_sample_counter - d_preamble_index),6000); //not corrected the preamble correlation lag! -> to be taken into account in TX Time
if(d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
tmp_double = current_synchro_data.Preamble_timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_synchro_data.Prn_timestamp_ms;
d_dump_file.write((char*)&tmp_double, sizeof(double));
tmp_double = current_synchro_data.Preamble_symbol_counter;
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e)
{
std::cout << "Exception writing observables dump file " << e.what() << std::endl;
}
}
//3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_synchro_data;
@@ -359,5 +355,25 @@ void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
d_channel = channel;
d_GPS_FSM.i_channel_ID = channel;
LOG_AT_LEVEL(INFO) << "Navigation channel set to " << channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename="telemetry";
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout << "Telemetry decoder dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str() << std::endl;
}
catch (std::ifstream::failure e)
{
std::cout << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl;
}
}
}
}

2
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

@@ -112,7 +112,7 @@ private:
int d_word_number;
long d_fs_in;
double d_preamble_duration_seconds;
//double d_preamble_duration_seconds;
// navigation message vars
Gps_Navigation_Message d_nav;
GpsL1CaSubframeFsm d_GPS_FSM;

27
src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc
View File

@@ -230,22 +230,23 @@ void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
std::cout << "NAVIGATION FSM: received subframe " << subframe_ID << " for satellite " << Gnss_Satellite(std::string("GPS"), i_satellite_PRN) << std::endl;
d_nav.i_satellite_PRN = i_satellite_PRN;
d_nav.i_channel_ID = i_channel_ID;
if (subframe_ID == 1)
{
d_nav.d_subframe1_timestamp_ms = this->d_preamble_time_ms;
//std::cout<<"NAVIGATION FSM: set subframe 1 preamble timestamp for satellite "<<d_nav.i_satellite_PRN<<std::endl;
}
d_nav.d_subframe_timestamp_ms = this->d_preamble_time_ms;
d_nav.b_update_tow_flag=true;
/*!
* \todo change satellite validation to subframe 5 because it will have a complete set of ephemeris parameters
*/
if (subframe_ID == 3)
{ // if the subframe is the 5th, then
if (d_nav.satellite_validation()) // if all the satellite ephemeris parameters are good, then
{
// Send the procesed satellite ephemeris packet
d_nav_queue->push(d_nav);
}
}
// if (subframe_ID == 3)
// { // if the subframe is the 5th, then
// if (d_nav.satellite_validation()) // if all the satellite ephemeris parameters are good, then
// {
// // Send the procesed satellite ephemeris packet
// d_nav_queue->push(d_nav);
//
// }
// }
d_nav.satellite_validation();
d_nav_queue->push(d_nav);
}

200
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc
View File

@@ -113,10 +113,10 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
d_acquisition_gnss_synchro=NULL;
d_if_freq = if_freq;
d_fs_in = fs_in;
d_if_freq = (double)if_freq;
d_fs_in = (double)fs_in;
d_vector_length = vector_length;
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
d_early_late_spc_chips = (double)early_late_space_chips; // Define early-late offset (in chips)
d_dump_filename = dump_filename;
// Initialize tracking variables ==========================================
@@ -144,10 +144,11 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
// sample synchronization
d_sample_counter = 0;
d_sample_counter_seconds = 0;
d_acq_sample_stamp = 0;
d_last_seg = 0;// this is for debug output only
d_code_phase_samples=0;
d_enable_tracking = false;
d_current_prn_length_samples = (int)d_vector_length;
@@ -181,31 +182,31 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking()
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
unsigned long int acq_trk_diff_samples;
float acq_trk_diff_seconds;
double acq_trk_diff_seconds;
acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in;
acq_trk_diff_seconds = (double)acq_trk_diff_samples / d_fs_in;
//doppler effect
// Fd=(C/(C+Vr))*F
float radial_velocity;
double radial_velocity;
radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
float T_chip_mod_seconds;
float T_prn_mod_seconds;
float T_prn_mod_samples;
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_hz = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds = 1 / d_code_freq_hz;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in;
d_next_prn_length_samples = round(T_prn_mod_samples);
T_prn_mod_samples = T_prn_mod_seconds * d_fs_in;
d_current_prn_length_samples = round(T_prn_mod_samples);
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
float T_prn_diff_seconds;
double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
double T_prn_true_samples = T_prn_true_seconds * d_fs_in;
double T_prn_diff_seconds;
T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
float N_prn_diff;
double N_prn_diff;
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
float corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * (float)d_fs_in), T_prn_true_samples);
double corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * d_fs_in), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
{
@@ -231,11 +232,8 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking()
d_rem_carr_phase = 0;
d_FLL_discriminator_hz = 0;
d_rem_code_phase_samples = 0;
d_next_rem_code_phase_samples = 0;
d_acc_carrier_phase_rad = 0;
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0,1);
@@ -263,12 +261,12 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking()
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
{
float tcode_chips;
float rem_code_phase_chips;
float code_phase_step_chips;
double tcode_chips;
double rem_code_phase_chips;
double code_phase_step_chips;
int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
code_phase_step_chips = d_code_freq_hz / ((float)d_fs_in);
code_phase_step_chips = d_code_freq_hz / d_fs_in;
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in);
// unified loop for E, P, L code vectors
tcode_chips = -rem_code_phase_chips;
@@ -282,7 +280,6 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
d_late_code[i] = d_ca_code[associated_chip_index];
tcode_chips = tcode_chips + code_phase_step_chips;
}
//d_code_phase_samples=d_code_phase_samples+(float)d_fs_in*GPS_L1_CA_CODE_LENGTH_CHIPS*(1/d_code_freq_hz-1/GPS_L1_CA_CODE_RATE_HZ);
}
@@ -291,8 +288,8 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier()
{
float phase, phase_step;
phase_step = (float)GPS_TWO_PI * d_carrier_doppler_hz / (float)d_fs_in;
double phase, phase_step;
phase_step = GPS_TWO_PI * d_carrier_doppler_hz / d_fs_in;
phase = d_rem_carr_phase;
for(int i = 0; i < d_current_prn_length_samples; i++)
{
@@ -332,47 +329,52 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
float code_error_chips = 0;
float correlation_time_s = 0;
float PLL_discriminator_hz = 0;
float carr_nco_hz = 0;
double code_error_chips = 0;
double correlation_time_s = 0;
double PLL_discriminator_hz = 0;
double carr_nco_hz = 0;
// get the sample in and out pointers
const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
d_Prompt_prev = *d_Prompt; // for the FLL discriminator
if (d_enable_tracking == true)
{
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data;
// Fill the acquisition data
current_synchro_data=*d_acquisition_gnss_synchro;
/*
* Receiver signal alignment
*/
if (d_pull_in == true)
{
int samples_offset;
float acq_trk_shif_correction_samples;
double acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter-d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_next_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_next_prn_length_samples);
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((double)acq_to_trk_delay_samples, (double)d_current_prn_length_samples);
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset)/(double)d_fs_in);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0;
current_synchro_data.Prompt_Q=0.0;
current_synchro_data.Tracking_timestamp_secs=(double)d_sample_counter/d_fs_in;
current_synchro_data.Carrier_phase_rads=0.0;
current_synchro_data.Code_phase_secs=0.0;
current_synchro_data.CN0_dB_hz=0.0;
current_synchro_data.Flag_valid_tracking=false;
*out[0] =current_synchro_data;
return 1;
}
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data;
// Fill the acquisition data
current_synchro_data=*d_acquisition_gnss_synchro;
// get the sample in and out pointers
const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
// Update the prn length based on code freq (variable) and
// sampling frequency (fixed)
// variable code PRN sample block size
d_current_prn_length_samples = d_next_prn_length_samples;
update_local_code();
update_local_carrier();
@@ -399,7 +401,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0;
current_synchro_data.Prompt_Q=0.0;
current_synchro_data.Tracking_timestamp_secs=d_sample_counter_seconds;
current_synchro_data.Tracking_timestamp_secs=(double)d_sample_counter/d_fs_in;
current_synchro_data.Carrier_phase_rads=0.0;
current_synchro_data.Code_phase_secs=0.0;
current_synchro_data.CN0_dB_hz=0.0;
@@ -417,7 +419,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
code_error_chips = dll_nc_e_minus_l_normalized(*d_Early,*d_Late);
//compute FLL error
correlation_time_s = ((float)d_current_prn_length_samples) / (float)d_fs_in;
correlation_time_s = ((double)d_current_prn_length_samples) / d_fs_in;
if (d_FLL_wait == 1)
{
d_Prompt_prev = *d_Prompt;
@@ -425,18 +427,18 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
}
else
{
d_FLL_discriminator_hz = fll_four_quadrant_atan(d_Prompt_prev, *d_Prompt, 0, correlation_time_s) / (float)GPS_TWO_PI;
d_FLL_discriminator_hz = fll_four_quadrant_atan(d_Prompt_prev, *d_Prompt, 0, correlation_time_s) / GPS_TWO_PI;
d_Prompt_prev = *d_Prompt;
d_FLL_wait = 1;
}
// Compute PLL error
PLL_discriminator_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / (float)GPS_TWO_PI;
PLL_discriminator_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_TWO_PI;
/*
* \todo Update FLL assistance algorithm!
*/
if ((((float)d_sample_counter - (float)d_acq_sample_stamp) / (float)d_fs_in) > 3)
if ((((double)d_sample_counter - (double)d_acq_sample_stamp) / d_fs_in) > 3)
{
d_FLL_discriminator_hz = 0; //disconnect the FLL after the initial lock
}
@@ -444,8 +446,8 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
* DLL and FLL+PLL filter and get current carrier Doppler and code frequency
*/
carr_nco_hz = d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz, PLL_discriminator_hz, correlation_time_s);
d_carrier_doppler_hz = (float)d_if_freq + carr_nco_hz;
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - (((d_carrier_doppler_hz - (float)d_if_freq) * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ) - code_error_chips;
d_carrier_doppler_hz = d_if_freq + carr_nco_hz;
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - (((d_carrier_doppler_hz - d_if_freq) * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ) - code_error_chips;
/*!
* \todo Improve the lock detection algorithm!
@@ -475,8 +477,6 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout << "Channel " << d_channel << " loss of lock!" << std::endl;
// tracking_message = 3; //loss of lock
// d_channel_internal_queue->push(tracking_message);
ControlMessageFactory* cmf = new ControlMessageFactory();
if (d_queue != gr_msg_queue_sptr()) {
d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
@@ -487,16 +487,6 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I=(double)(*d_Prompt).real();
current_synchro_data.Prompt_Q=(double)(*d_Prompt).imag();
current_synchro_data.Tracking_timestamp_secs=d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads=(double)d_acc_carrier_phase_rad;
current_synchro_data.Code_phase_secs=(double)d_code_phase_samples * (1/(float)d_fs_in);
current_synchro_data.CN0_dB_hz=(double)d_CN0_SNV_dB_Hz;
*out[0] =current_synchro_data;
// ########## DEBUG OUTPUT
/*!
* \todo The stop timer has to be moved to the signal source!
@@ -508,11 +498,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
{
d_last_seg = floor(d_sample_counter/d_fs_in);
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
//std::string sys = &d_acquisition_gnss_synchro->System;
//std::cout << sys << ", " << sys.substr(0,1) << std::endl;
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
//if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
}
}
else
@@ -520,35 +506,33 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
if (floor(d_sample_counter/d_fs_in) != d_last_seg)
{
d_last_seg = floor(d_sample_counter/d_fs_in);
//std::string sys2 = &d_acquisition_gnss_synchro->System;
//std::cout << sys2 << ", " << sys2.substr(0,1) << std::endl;
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
//predict the next loop PRN period length prediction
float T_chip_seconds;
float T_prn_seconds;
float T_prn_samples;
float K_blk_samples;
double T_chip_seconds;
double T_prn_seconds;
double T_prn_samples;
double K_blk_samples;
T_chip_seconds = 1/d_code_freq_hz;
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * (float)d_fs_in;
d_rem_code_phase_samples = d_next_rem_code_phase_samples;
T_prn_samples = T_prn_seconds * d_fs_in;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples;
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete sample
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error
// Update the current PRN delay (code phase in samples)
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
d_code_phase_samples = d_code_phase_samples + T_prn_samples - T_prn_true_samples;
if (d_code_phase_samples < 0)
{
d_code_phase_samples = T_prn_true_samples + d_code_phase_samples;
}
d_code_phase_samples = fmod(d_code_phase_samples, T_prn_true_samples);
d_next_prn_length_samples = round(K_blk_samples); //round to a discrete sample
d_next_rem_code_phase_samples = K_blk_samples - d_next_prn_length_samples; //rounding error
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I=(double)(*d_Prompt).imag();
current_synchro_data.Prompt_Q=(double)(*d_Prompt).real();
// Tracking_timestamp_secs is aligned with the PRN start sample
current_synchro_data.Tracking_timestamp_secs=((double)d_sample_counter+(double)d_current_prn_length_samples+d_rem_code_phase_samples)/d_fs_in;
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, Code_phase_secs=0
current_synchro_data.Code_phase_secs=0;
current_synchro_data.Carrier_phase_rads=d_acc_carrier_phase_rad;
current_synchro_data.CN0_dB_hz=d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_tracking=true;
*out[0] =current_synchro_data;
}
else
{
@@ -567,6 +551,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
float tmp_float;
double tmp_double;
prompt_I = (*d_Prompt).imag();
prompt_Q = (*d_Prompt).real();
tmp_E=std::abs<float>(*d_Early);
@@ -585,28 +570,38 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
//tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
// accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
tmp_float=(float)d_acc_carrier_phase_rad;
d_dump_file.write((char*)&tmp_float, sizeof(float));
// carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
tmp_float=(float)d_carrier_doppler_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=(float)d_code_freq_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float));
//PLL commands
d_dump_file.write((char*)&PLL_discriminator_hz, sizeof(float));
d_dump_file.write((char*)&carr_nco_hz, sizeof(float));
tmp_float=(float)PLL_discriminator_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=(float)carr_nco_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float));
//DLL commands
d_dump_file.write((char*)&code_error_chips, sizeof(float));
d_dump_file.write((char*)&d_code_phase_samples, sizeof(float));
tmp_float=(float)code_error_chips;
d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=(float)d_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float));
// CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
tmp_float=(float)d_CN0_SNV_dB_Hz;
d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=(float)d_carrier_lock_test;
d_dump_file.write((char*)&tmp_float, sizeof(float));
// AUX vars (for debug purposes)
tmp_float = 0;
tmp_float = (float)d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float));
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e)
{
@@ -614,7 +609,6 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
}
}
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter_seconds = d_sample_counter_seconds + (((double)d_current_prn_length_samples) / (double)d_fs_in);
d_sample_counter += d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}

35
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h
View File

@@ -142,8 +142,8 @@ private:
bool d_dump;
unsigned int d_channel;
int d_last_seg;
long d_if_freq;
long d_fs_in;
double d_if_freq;
double d_fs_in;
gr_complex* d_ca_code;
@@ -159,44 +159,43 @@ private:
gr_complex d_Prompt_prev;
float d_early_late_spc_chips;
double d_early_late_spc_chips;
float d_carrier_doppler_hz;
float d_code_freq_hz;
float d_code_phase_samples;
double d_carrier_doppler_hz;
double d_code_freq_hz;
double d_code_phase_samples;
int d_current_prn_length_samples;
int d_next_prn_length_samples;
//int d_next_prn_length_samples;
int d_FLL_wait;
float d_rem_carr_phase;
float d_rem_code_phase_samples;
float d_next_rem_code_phase_samples;
double d_rem_carr_phase;
double d_rem_code_phase_samples;
//double d_next_rem_code_phase_samples;
bool d_pull_in;
// acquisition
float d_acq_code_phase_samples;
float d_acq_carrier_doppler_hz;
double d_acq_code_phase_samples;
double d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// FLL + PLL filter
float d_FLL_discriminator_hz; // This is a class variable because FLL needs to have memory
double d_FLL_discriminator_hz; // This is a class variable because FLL needs to have memory
Tracking_FLL_PLL_filter d_carrier_loop_filter;
float d_acc_carrier_phase_rad;
double d_acc_carrier_phase_rad;
unsigned long int d_sample_counter;
double d_sample_counter_seconds;
unsigned long int d_acq_sample_stamp;
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
float d_carrier_lock_test;
float d_CN0_SNV_dB_Hz;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
float d_carrier_lock_threshold;
double d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;

39
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
View File

@@ -154,7 +154,7 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
// sample synchronization
d_sample_counter = 0;
d_sample_counter_seconds = 0;
//d_sample_counter_seconds = 0;
d_acq_sample_stamp = 0;
d_enable_tracking = false;
@@ -322,10 +322,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Tracking_cc()
}
/* Tracking signal processing
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
*/
@@ -357,7 +353,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
//std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n";
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset) / (double)d_fs_in);
//d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset) / (double)d_fs_in);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n";
@@ -403,7 +399,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0;
current_synchro_data.Prompt_Q=0.0;
current_synchro_data.Tracking_timestamp_secs=d_sample_counter_seconds;
current_synchro_data.Tracking_timestamp_secs=(double)d_sample_counter/(double)d_fs_in;
current_synchro_data.Carrier_phase_rads=0.0;
current_synchro_data.Code_phase_secs=0.0;
current_synchro_data.CN0_dB_hz=0.0;
@@ -441,17 +437,6 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
T_prn_samples = T_prn_seconds * d_fs_in;
d_rem_code_phase_samples = d_next_rem_code_phase_samples;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples;
// Update the current PRN delay (code phase in samples)
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
d_code_phase_samples = d_code_phase_samples + T_prn_samples - T_prn_true_samples;
if (d_code_phase_samples < 0)
{
d_code_phase_samples = T_prn_true_samples + d_code_phase_samples;
}
d_code_phase_samples = fmod(d_code_phase_samples, T_prn_true_samples);
d_next_prn_length_samples = round(K_blk_samples); //round to a discrete samples
d_next_rem_code_phase_samples = K_blk_samples - d_next_prn_length_samples; //rounding error
@@ -499,11 +484,13 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
current_synchro_data.Tracking_timestamp_secs = d_sample_counter_seconds;
current_synchro_data.Prompt_I = (double)(*d_Prompt).imag();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).real();
// Tracking_timestamp_secs is aligned with the PRN start sample
current_synchro_data.Tracking_timestamp_secs=((double)d_sample_counter+(double)d_next_prn_length_samples+(double)d_next_rem_code_phase_samples)/(double)d_fs_in;
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs=0;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
current_synchro_data.Code_phase_secs = (double)d_code_phase_samples * (1/(float)d_fs_in);
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
*out[0] = current_synchro_data;
@@ -554,6 +541,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
float tmp_float;
double tmp_double;
prompt_I = (*d_Prompt).imag();
prompt_Q = (*d_Prompt).real();
tmp_E = std::abs<float>(*d_Early);
@@ -591,9 +579,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes)
tmp_float=0;
tmp_float = d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float));
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e)
{
@@ -602,7 +591,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
}
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter_seconds = d_sample_counter_seconds + ( ((double)d_current_prn_length_samples) / (double)d_fs_in );
//d_sample_counter_seconds = d_sample_counter_seconds + ( ((double)d_current_prn_length_samples) / (double)d_fs_in );
d_sample_counter += d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}

2
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
View File

@@ -172,7 +172,7 @@ private:
//PRN period in samples
int d_current_prn_length_samples;
int d_next_prn_length_samples;
double d_sample_counter_seconds;
//double d_sample_counter_seconds;
//processing samples counters
unsigned long int d_sample_counter;

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

@@ -483,17 +483,6 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
T_prn_samples = T_prn_seconds * d_fs_in;
d_rem_code_phase_samples = d_next_rem_code_phase_samples;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples;
// Update the current PRN delay (code phase in samples)
float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in;
d_code_phase_samples = d_code_phase_samples + T_prn_samples - T_prn_true_samples;
if (d_code_phase_samples < 0)
{
d_code_phase_samples = T_prn_true_samples + d_code_phase_samples;
}
d_code_phase_samples = fmod(d_code_phase_samples, T_prn_true_samples);
d_next_prn_length_samples = round(K_blk_samples); //round to a discrete samples
d_next_rem_code_phase_samples = K_blk_samples - d_next_prn_length_samples; //rounding error
@@ -539,16 +528,6 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
current_synchro_data.Tracking_timestamp_secs = d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
current_synchro_data.Code_phase_secs = (double)d_code_phase_samples * (1/(float)d_fs_in);
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
*out[0] = current_synchro_data;
// ########## DEBUG OUTPUT
/*!
* \todo The stop timer has to be moved to the signal source!
@@ -576,6 +555,19 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
// Tracking_timestamp_secs is aligned with the PRN start sample
current_synchro_data.Tracking_timestamp_secs=((double)d_sample_counter+(double)d_next_prn_length_samples+(double)d_next_rem_code_phase_samples)/(double)d_fs_in;
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, Code_phase_secs=0
current_synchro_data.Code_phase_secs=0;
current_synchro_data.Tracking_timestamp_secs = d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
*out[0] = current_synchro_data;
}
else
{
@@ -600,6 +592,7 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
float tmp_float;
double tmp_double;
prompt_I = (*d_Prompt).imag();
prompt_Q = (*d_Prompt).real();
tmp_E = std::abs<float>(*d_Early);
@@ -637,9 +630,10 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes)
tmp_float=0;
tmp_float = d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float));
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double));
}
catch (std::ifstream::failure e)
{
@@ -648,7 +642,7 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
}
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter_seconds = d_sample_counter_seconds + ( ((double)d_current_prn_length_samples) / (double)d_fs_in );
//d_sample_counter_seconds = d_sample_counter_seconds + ( ((double)d_current_prn_length_samples) / (double)d_fs_in );
d_sample_counter += d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}