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

Code cleaning and improving documentation

Browse Source 
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@455 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez
2013-11-29 08:38:22 +00:00
parent e8601cf111
commit abd02e34a8
41 changed files with 1357 additions and 1569 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/algorithms/PVT/adapters/galileo_e1_pvt.h
View File

@@ -1,10 +1,8 @@
/*! /*!
* \file galileo_e1_pvt.h * \file galileo_e1_pvt.h
* \brief Interface of an adapter of a GALILEO E1 PVT solver block to a * \brief Interface of an adapter of a GALILEO E1 PVT solver block to a
* PvtInterface * PvtInterface.
* Position Velocity and Time * \author Javier Arribas, 2013. jarribas(at)cttc.es
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
@@ -43,12 +41,12 @@
class ConfigurationInterface; class ConfigurationInterface;
/*! /*!
* \brief This class implements a PvtInterface for GPS L1 C/A * \brief This class implements a PvtInterface for Galileo E1
*/ */
class GalileoE1Pvt : public PvtInterface class GalileoE1Pvt : public PvtInterface
{ {
public: public:
GalileoE1Pvt(ConfigurationInterface* configuration, GalileoE1Pvt(ConfigurationInterface* configuration,
std::string role, std::string role,
unsigned int in_streams, unsigned int in_streams,
unsigned int out_streams, unsigned int out_streams,
@@ -61,7 +59,7 @@ public:
return role_; return role_;
} }
//! Returns "GPS_L1_CA_PVT" //! Returns "GALILEO_E1_PVT"
std::string implementation() std::string implementation()
{ {
return "GALILEO_E1_PVT"; return "GALILEO_E1_PVT";

102
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc
View File

@@ -1,10 +1,11 @@
/*! /*!
* \file galileo_e1_pvt_cc.cc * \file galileo_e1_pvt_cc.cc
* \brief Implementation of a Position Velocity and Time computation block for GPS L1 C/A * \brief Implementation of a Position Velocity and Time computation block for GPS L1 C/A
* \author Javier Arribas, 2011. jarribas(at)cttc.es * \author Javier Arribas, 2013. jarribas(at)cttc.es
*
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -57,8 +58,8 @@ galileo_e1_make_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue>
galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname) : galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname) :
gr::block("galileo_e1_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::block("galileo_e1_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(gr_complex))) gr::io_signature::make(1, 1, sizeof(gr_complex)))
{ {
d_output_rate_ms = output_rate_ms; d_output_rate_ms = output_rate_ms;
@@ -83,7 +84,7 @@ galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr<g
d_averaging_depth = averaging_depth; d_averaging_depth = averaging_depth;
d_flag_averaging = flag_averaging; d_flag_averaging = flag_averaging;
d_ls_pvt = new galileo_e1_ls_pvt(nchannels,dump_ls_pvt_filename,d_dump); d_ls_pvt = new galileo_e1_ls_pvt(nchannels, dump_ls_pvt_filename, d_dump);
d_ls_pvt->set_averaging_depth(d_averaging_depth); d_ls_pvt->set_averaging_depth(d_averaging_depth);
d_sample_counter = 0; d_sample_counter = 0;
@@ -140,7 +141,7 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
for (unsigned int i=0; i<d_nchannels; i++) for (unsigned int i = 0; i < d_nchannels; i++)
{ {
if (in[i][0].Flag_valid_pseudorange == true) if (in[i][0].Flag_valid_pseudorange == true)
{ {
@@ -151,25 +152,25 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it
// ############ 1. READ EPHEMERIS/UTC_MODE/IONO FROM GLOBAL MAPS #### // ############ 1. READ EPHEMERIS/UTC_MODE/IONO FROM GLOBAL MAPS ####
if (global_galileo_ephemeris_map.size()>0) if (global_galileo_ephemeris_map.size() > 0)
{
d_ls_pvt->galileo_ephemeris_map = global_galileo_ephemeris_map.get_map_copy();
}
if (global_galileo_utc_model_map.size()>0)
{ {
// UTC MODEL data is shared for all the Galileo satellites. Read always at ID=0 d_ls_pvt->galileo_ephemeris_map = global_galileo_ephemeris_map.get_map_copy();
global_galileo_utc_model_map.read(0,d_ls_pvt->galileo_utc_model);
} }
if (global_galileo_iono_map.size()>0) if (global_galileo_utc_model_map.size() > 0)
{
// UTC MODEL data is shared for all the Galileo satellites. Read always at ID=0
global_galileo_utc_model_map.read(0, d_ls_pvt->galileo_utc_model);
}
if (global_galileo_iono_map.size() > 0)
{ {
// IONO data is shared for all the Galileo satellites. Read always at ID=0 // IONO data is shared for all the Galileo satellites. Read always at ID=0
global_galileo_iono_map.read(0,d_ls_pvt->galileo_iono); global_galileo_iono_map.read(0, d_ls_pvt->galileo_iono);
} }
// ############ 2 COMPUTE THE PVT ################################ // ############ 2 COMPUTE THE PVT ################################
if (gnss_pseudoranges_map.size() > 0 and d_ls_pvt->galileo_ephemeris_map.size() >0) if (gnss_pseudoranges_map.size() > 0 and d_ls_pvt->galileo_ephemeris_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 ((d_sample_counter % d_output_rate_ms) == 0)
@@ -178,39 +179,39 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it
pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging); pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging);
if (pvt_result==true) if (pvt_result == true)
{ {
d_kml_dump.print_position_galileo(d_ls_pvt, d_flag_averaging); d_kml_dump.print_position_galileo(d_ls_pvt, d_flag_averaging);
//ToDo: Implement Galileo RINEX and Galileo NMEA outputs //ToDo: Implement Galileo RINEX and Galileo NMEA outputs
// d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); // d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
// //
// if (!b_rinex_header_writen) // & we have utc data in nav message! // if (!b_rinex_header_writen) // & we have utc data in nav message!
// { // {
// 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();
// if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) // if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
// { // {
// rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second,d_rx_time); // rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second,d_rx_time);
// rp->rinex_nav_header(rp->navFile,d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); // rp->rinex_nav_header(rp->navFile,d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model);
// b_rinex_header_writen = true; // do not write header anymore // 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) // if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s)
// { // {
// // Limit the RINEX navigation output rate to 1/6 seg // // Limit the RINEX navigation output rate to 1/6 seg
// // Notice that d_sample_counter period is 1ms (for GPS correlators) // // Notice that d_sample_counter period is 1ms (for GPS correlators)
// if ((d_sample_counter-d_last_sample_nav_output)>=6000) // if ((d_sample_counter-d_last_sample_nav_output)>=6000)
// { // {
// rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map); // rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map);
// d_last_sample_nav_output=d_sample_counter; // d_last_sample_nav_output=d_sample_counter;
// } // }
// 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();
// if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) // if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end())
// { // {
// rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map); // rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map);
// } // }
// } // }
} }
} }
@@ -226,13 +227,14 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it
d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP << d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP <<
" GDOP = " << d_ls_pvt->d_GDOP <<std::endl; " GDOP = " << d_ls_pvt->d_GDOP <<std::endl;
} }
// // MULTIPLEXED FILE RECORDING - Record results to file
// MULTIPLEXED FILE RECORDING - Record results to file
if(d_dump == true) if(d_dump == true)
{ {
try try
{ {
double tmp_double; double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i = 0; i < d_nchannels; i++)
{ {
tmp_double = in[i][0].Pseudorange_m; tmp_double = in[i][0].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));

42
src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h
View File

@@ -48,13 +48,21 @@
#include "GPS_L1_CA.h" #include "GPS_L1_CA.h"
#include "Galileo_E1.h" #include "Galileo_E1.h"
class galileo_e1_pvt_cc; class galileo_e1_pvt_cc;
typedef boost::shared_ptr<galileo_e1_pvt_cc> galileo_e1_pvt_cc_sptr; typedef boost::shared_ptr<galileo_e1_pvt_cc> galileo_e1_pvt_cc_sptr;
galileo_e1_pvt_cc_sptr galileo_e1_pvt_cc_sptr galileo_e1_make_pvt_cc(unsigned int n_channels,
galileo_e1_make_pvt_cc(unsigned int n_channels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); boost::shared_ptr<gr::msg_queue> queue,
bool dump,
std::string dump_filename,
int averaging_depth,
bool flag_averaging,
int output_rate_ms,
int display_rate_ms,
bool flag_nmea_tty_port,
std::string nmea_dump_filename,
std::string nmea_dump_devname);
/*! /*!
* \brief This class implements a block that computes the PVT solution with Galileo E1 signals * \brief This class implements a block that computes the PVT solution with Galileo E1 signals
@@ -62,9 +70,27 @@ galileo_e1_make_pvt_cc(unsigned int n_channels, boost::shared_ptr<gr::msg_queue>
class galileo_e1_pvt_cc : public gr::block class galileo_e1_pvt_cc : public gr::block
{ {
private: private:
friend galileo_e1_pvt_cc_sptr friend galileo_e1_pvt_cc_sptr galileo_e1_make_pvt_cc(unsigned int nchannels,
galileo_e1_make_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); boost::shared_ptr<gr::msg_queue> queue,
galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); bool dump,
std::string dump_filename,
int averaging_depth,
bool flag_averaging,
int output_rate_ms,
int display_rate_ms,
bool flag_nmea_tty_port,
std::string nmea_dump_filename,
std::string nmea_dump_devname);
galileo_e1_pvt_cc(unsigned int nchannels,
boost::shared_ptr<gr::msg_queue> queue,
bool dump, std::string dump_filename,
int averaging_depth,
bool flag_averaging,
int output_rate_ms,
int display_rate_ms,
bool flag_nmea_tty_port,
std::string nmea_dump_filename,
std::string nmea_dump_devname);
boost::shared_ptr<gr::msg_queue> d_queue; boost::shared_ptr<gr::msg_queue> d_queue;
bool d_dump; bool d_dump;
bool b_rinex_header_writen; bool b_rinex_header_writen;
@@ -81,8 +107,8 @@ private:
Kml_Printer d_kml_dump; Kml_Printer d_kml_dump;
Nmea_Printer *d_nmea_printer; Nmea_Printer *d_nmea_printer;
double d_rx_time; double d_rx_time;
galileo_e1_ls_pvt *d_ls_pvt; /*modify PVT/libs/galileo_e1_ls_pvt*/ galileo_e1_ls_pvt *d_ls_pvt;
bool pseudoranges_pairCompare_min( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b); bool pseudoranges_pairCompare_min(std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b);
public: public:
~galileo_e1_pvt_cc (); //!< Default destructor ~galileo_e1_pvt_cc (); //!< Default destructor

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

@@ -156,7 +156,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer
for (unsigned int i=0; i<d_nchannels; i++) for (unsigned int i = 0; i < d_nchannels; i++)
{ {
if (in[i][0].Flag_valid_pseudorange == true) if (in[i][0].Flag_valid_pseudorange == true)
{ {
@@ -169,22 +169,22 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
d_ls_pvt->gps_ephemeris_map = global_gps_ephemeris_map.get_map_copy(); d_ls_pvt->gps_ephemeris_map = global_gps_ephemeris_map.get_map_copy();
if (global_gps_utc_model_map.size()>0) if (global_gps_utc_model_map.size() > 0)
{ {
// UTC MODEL data is shared for all the GPS satellites. Read always at ID=0 // UTC MODEL data is shared for all the GPS satellites. Read always at ID=0
global_gps_utc_model_map.read(0,d_ls_pvt->gps_utc_model); global_gps_utc_model_map.read(0, d_ls_pvt->gps_utc_model);
} }
if (global_gps_iono_map.size()>0) if (global_gps_iono_map.size() > 0)
{ {
// IONO data is shared for all the GPS satellites. Read always at ID=0 // IONO data is shared for all the GPS satellites. Read always at ID=0
global_gps_iono_map.read(0,d_ls_pvt->gps_iono); global_gps_iono_map.read(0, d_ls_pvt->gps_iono);
} }
// update SBAS data collections // update SBAS data collections
if (global_sbas_iono_map.size() > 0) if (global_sbas_iono_map.size() > 0)
{ {
// SBAS ionosperic correction is shared for all the GPS satellites. Read always at ID=0 // SBAS ionospheric correction is shared for all the GPS satellites. Read always at ID=0
global_sbas_iono_map.read(0, d_ls_pvt->sbas_iono); global_sbas_iono_map.read(0, d_ls_pvt->sbas_iono);
} }
d_ls_pvt->sbas_sat_corr_map = global_sbas_sat_corr_map.get_map_copy(); d_ls_pvt->sbas_sat_corr_map = global_sbas_sat_corr_map.get_map_copy();
@@ -216,7 +216,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
int gps_week = eph.i_GPS_week; int gps_week = eph.i_GPS_week;
double gps_sec = gs.d_TOW_at_current_symbol; double gps_sec = gs.d_TOW_at_current_symbol;
Sbas_Time_Relation time_rel(relative_rx_time,gps_week,gps_sec); Sbas_Time_Relation time_rel(relative_rx_time, gps_week, gps_sec);
sbas_raw_msg.relate(time_rel); sbas_raw_msg.relate(time_rel);
} }
@@ -236,7 +236,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
{ {
bool pvt_result; bool pvt_result;
pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging); pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging);
if (pvt_result==true) if (pvt_result == true)
{ {
d_kml_dump.print_position(d_ls_pvt, d_flag_averaging); d_kml_dump.print_position(d_ls_pvt, d_flag_averaging);
d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
@@ -289,7 +289,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
try try
{ {
double tmp_double; double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i = 0; i < d_nchannels ; i++)
{ {
tmp_double = in[i][0].Pseudorange_m; tmp_double = in[i][0].Pseudorange_m;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));

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

@@ -50,8 +50,17 @@ class gps_l1_ca_pvt_cc;
typedef boost::shared_ptr<gps_l1_ca_pvt_cc> gps_l1_ca_pvt_cc_sptr; typedef boost::shared_ptr<gps_l1_ca_pvt_cc> gps_l1_ca_pvt_cc_sptr;
gps_l1_ca_pvt_cc_sptr gps_l1_ca_pvt_cc_sptr gps_l1_ca_make_pvt_cc(unsigned int n_channels,
gps_l1_ca_make_pvt_cc(unsigned int n_channels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); boost::shared_ptr<gr::msg_queue> queue,
bool dump,
std::string dump_filename,
int averaging_depth,
bool flag_averaging,
int output_rate_ms,
int display_rate_ms,
bool flag_nmea_tty_port,
std::string nmea_dump_filename,
std::string nmea_dump_devname);
/*! /*!
* \brief This class implements a block that computes the PVT solution * \brief This class implements a block that computes the PVT solution
@@ -59,9 +68,28 @@ gps_l1_ca_make_pvt_cc(unsigned int n_channels, boost::shared_ptr<gr::msg_queue>
class gps_l1_ca_pvt_cc : public gr::block class gps_l1_ca_pvt_cc : public gr::block
{ {
private: private:
friend gps_l1_ca_pvt_cc_sptr friend gps_l1_ca_pvt_cc_sptr gps_l1_ca_make_pvt_cc(unsigned int nchannels,
gps_l1_ca_make_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); boost::shared_ptr<gr::msg_queue> queue,
gps_l1_ca_pvt_cc(unsigned int nchannels, boost::shared_ptr<gr::msg_queue> queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); bool dump,
std::string dump_filename,
int averaging_depth,
bool flag_averaging,
int output_rate_ms,
int display_rate_ms,
bool flag_nmea_tty_port,
std::string nmea_dump_filename,
std::string nmea_dump_devname);
gps_l1_ca_pvt_cc(unsigned int nchannels,
boost::shared_ptr<gr::msg_queue> queue,
bool dump,
std::string dump_filename,
int averaging_depth,
bool flag_averaging,
int output_rate_ms,
int display_rate_ms,
bool flag_nmea_tty_port,
std::string nmea_dump_filename,
std::string nmea_dump_devname);
boost::shared_ptr<gr::msg_queue> d_queue; boost::shared_ptr<gr::msg_queue> d_queue;
bool d_dump; bool d_dump;
bool b_rinex_header_writen; bool b_rinex_header_writen;

225
src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc
View File

@@ -3,9 +3,10 @@
* \brief Implementation of a Least Squares Position, Velocity, and Time * \brief Implementation of a Least Squares Position, Velocity, and Time
* (PVT) solver, based on K.Borre's Matlab receiver. * (PVT) solver, based on K.Borre's Matlab receiver.
* \author Javier Arribas, 2011. jarribas(at)cttc.es * \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -30,7 +31,6 @@
#include <armadillo> #include <armadillo>
#include "galileo_e1_ls_pvt.h" #include "galileo_e1_ls_pvt.h"
#include "Galileo_E1.h" #include "Galileo_E1.h"
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
@@ -39,7 +39,7 @@
using google::LogMessage; using google::LogMessage;
galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file) galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file)
{ {
// init empty ephemeris for all the available GNSS channels // init empty ephemeris for all the available GNSS channels
d_nchannels = nchannels; d_nchannels = nchannels;
@@ -145,16 +145,6 @@ arma::vec galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arm
omc = arma::zeros(nmbOfSatellites, 1); omc = arma::zeros(nmbOfSatellites, 1);
az = arma::zeros(1, nmbOfSatellites); az = arma::zeros(1, nmbOfSatellites);
el = arma::zeros(1, nmbOfSatellites); el = arma::zeros(1, nmbOfSatellites);
for (int i = 0; i < nmbOfSatellites; i++)
{
for (int j = 0; j < 4; j++)
{
A(i, j) = 0.0; //Armadillo
}
omc(i, 0) = 0.0;
az(0, i) = 0.0;
}
el = az;
arma::mat X = satpos; arma::mat X = satpos;
arma::vec Rot_X; arma::vec Rot_X;
double rho2; double rho2;
@@ -178,22 +168,23 @@ arma::vec galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arm
{ {
//--- Update equations ----------------------------------------- //--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0)) * rho2 = (X(0, i) - pos(0)) *
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) * (X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
(X(1, i) - pos(1)) + (X(2,i) - pos(2)) * (X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
(X(2,i) - pos(2)); (X(2, i) - pos(2));
traveltime = sqrt(rho2) / GALILEO_C_m_s; traveltime = sqrt(rho2) / GALILEO_C_m_s;
//--- Correct satellite position (do to earth rotation) -------- //--- Correct satellite position (do to earth rotation) --------
Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find DOA and range of satellites //--- Find DOA and range of satellites
topocent(&d_visible_satellites_Az[i], &d_visible_satellites_El[i], topocent(&d_visible_satellites_Az[i],
&d_visible_satellites_Distance[i], pos.subvec(0,2), Rot_X - pos.subvec(0,2)); &d_visible_satellites_El[i],
//[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); &d_visible_satellites_Distance[i],
pos.subvec(0,2),
Rot_X - pos.subvec(0, 2));
} }
//--- Apply the corrections ---------------------------------------- //--- Apply the corrections ----------------------------------------
omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0,2),2) - pos(3) - trop); // Armadillo omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0, 2), 2) - pos(3) - trop); // Armadillo
//--- Construct the A matrix --------------------------------------- //--- Construct the A matrix ---------------------------------------
//Armadillo //Armadillo
@@ -208,7 +199,7 @@ arma::vec galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arm
//--- Apply position update -------------------------------------------- //--- Apply position update --------------------------------------------
pos = pos + x; pos = pos + x;
if (arma::norm(x,2)<1e-4) if (arma::norm(x,2) < 1e-4)
{ {
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm) break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
} }
@@ -217,12 +208,11 @@ arma::vec galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arm
try try
{ {
//-- compute the Dilution Of Precision values //-- compute the Dilution Of Precision values
//arma::mat Q; d_Q = arma::inv(arma::htrans(A)*A);
d_Q = arma::inv(arma::htrans(A)*A);
} }
catch(std::exception& e) catch(std::exception& e)
{ {
d_Q=arma::zeros(4,4); d_Q = arma::zeros(4,4);
} }
return pos; return pos;
} }
@@ -234,9 +224,9 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter; std::map<int,Galileo_Ephemeris>::iterator galileo_ephemeris_iter;
int valid_pseudoranges = gnss_pseudoranges_map.size(); int valid_pseudoranges = gnss_pseudoranges_map.size();
arma::mat W = arma::eye(valid_pseudoranges,valid_pseudoranges); //channels weights matrix arma::mat W = arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix
arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
arma::mat satpos = arma::zeros(3,valid_pseudoranges); //satellite positions matrix arma::mat satpos = arma::zeros(3, valid_pseudoranges); //satellite positions matrix
int Galileo_week_number = 0; int Galileo_week_number = 0;
double utc = 0; double utc = 0;
@@ -245,8 +235,6 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
double TX_time_corrected_s; double TX_time_corrected_s;
double SV_clock_bias_s = 0; double SV_clock_bias_s = 0;
//double GST=0;
d_flag_averaging = flag_averaging; d_flag_averaging = flag_averaging;
// ******************************************************************************** // ********************************************************************************
@@ -259,7 +247,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
gnss_pseudoranges_iter++) gnss_pseudoranges_iter++)
{ {
// 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key // 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first); galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first);
if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
{ {
/*! /*!
@@ -277,7 +265,6 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
//JAVIER VERSION: //JAVIER VERSION:
double Rx_time = galileo_current_time; double Rx_time = galileo_current_time;
//to compute satellite position we need GST = WN+TOW (everything expressed in seconds) //to compute satellite position we need GST = WN+TOW (everything expressed in seconds)
//double Rx_time = galileo_current_time + Galileo_week_number*sec_in_day*day_in_week; //double Rx_time = galileo_current_time + Galileo_week_number*sec_in_day*day_in_week;
@@ -298,127 +285,122 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y; satpos(1,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Y;
satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z; satpos(2,obs_counter) = galileo_ephemeris_iter->second.d_satpos_Z;
// 5- fill the observations vector with the corrected pseudorranges // 5- fill the observations vector with the corrected pseudoranges
obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s*GALILEO_C_m_s; obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s*GALILEO_C_m_s;
d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN;
d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz; d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz;
valid_obs++; valid_obs++;
Galileo_week_number = galileo_ephemeris_iter->second.WN_5;//for GST Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST
//debug //debug
double GST=galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number,galileo_current_time); double GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, galileo_current_time);
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
// get time string gregorian calendar // get time string gregorian calendar
//std::cout<<"UTC_raw="<<utc<<std::endl;
boost::posix_time::time_duration t = boost::posix_time::seconds(utc); boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2) // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time; d_position_UTC_time = p_time;
//std::cout << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time)<<std::endl; DLOG(INFO) << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time);
//end debug //end debug
// 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
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X << " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(obs_counter) << " [m]" << std::endl; << " [m] PR_obs=" << obs(obs_counter) << " [m]";
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
// no valid pseudorange for the current SV // no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated W(obs_counter, obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero) obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first << std::endl; DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first;
} }
obs_counter++; obs_counter++;
} }
// // ********************************************************************************
// // ******************************************************************************** // ****** SOLVE LEAST SQUARES******************************************************
// // ****** SOLVE LEAST SQUARES****************************************************** // ********************************************************************************
// // ********************************************************************************
d_valid_observations = valid_obs; d_valid_observations = valid_obs;
DLOG(INFO) << "Galileo PVT: valid observations=" << valid_obs << std::endl; DLOG(INFO) << "Galileo PVT: valid observations=" << valid_obs;
if (valid_obs >= 4) if (valid_obs >= 4)
{ {
arma::vec mypos; arma::vec mypos;
DLOG(INFO) << "satpos=" << satpos << std::endl; DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs="<< obs << std::endl; DLOG(INFO) << "obs="<< obs;
DLOG(INFO) << "W=" << W <<std::endl; DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W); mypos = leastSquarePos(satpos, obs, W);
// Compute GST and Gregorian time // Compute GST and Gregorian time
double GST=galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number,galileo_current_time); double GST = galileo_ephemeris_iter->second.Galileo_System_Time(Galileo_week_number, galileo_current_time);
utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number);
// get time string gregorian calendar // get time string Gregorian calendar
boost::posix_time::time_duration t = boost::posix_time::seconds(utc); boost::posix_time::time_duration t = boost::posix_time::seconds(utc);
// 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2) // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
d_position_UTC_time = p_time; d_position_UTC_time = p_time;
//std::cout << "Galileo RX time at " << boost::posix_time::to_simple_string(p_time)<<std::endl; DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << mypos;
DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl;
cart2geo((double)mypos(0), (double)mypos(1), (double)mypos(2), 4); cart2geo((double)mypos(0), (double)mypos(1), (double)mypos(2), 4);
//ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km)
if (d_height_m>50000) if (d_height_m > 50000)
{ {
b_valid_position=false; b_valid_position = false;
return false; //erratic PVT return false;
} }
DLOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time) DLOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << std::endl; << " [deg], Height= " << d_height_m << " [m]";
// ###### Compute DOPs ######## // ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates // 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F = arma::zeros(3,3);
F(0,0) = -sin(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,1) = -sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,2) = cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
arma::mat F=arma::zeros(3,3); F(1,0) = cos((GPS_TWO_PI*d_longitude_d)/360.0);
F(0,0)=-sin(GPS_TWO_PI*(d_longitude_d/360.0)); F(1,1) = -sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(0,1)=-sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0)); F(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(0,2)=cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(1,0)=cos((GPS_TWO_PI*d_longitude_d)/360.0); F(2,0) = 0;
F(1,1)=-sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0); F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(1,2)=cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0); F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0));
F(2,0)=0; // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
F(2,1)=cos((GPS_TWO_PI*d_latitude_d)/360.0); arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
F(2,2)=sin((GPS_TWO_PI*d_latitude_d/360.0)); arma::mat DOP_ENU = arma::zeros(3, 3);
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS) try
{
arma::mat Q_ECEF=d_Q.submat( 0, 0, 2, 2); DOP_ENU = arma::htrans(F)*Q_ECEF*F;
arma::mat DOP_ENU=arma::zeros(3,3); d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP
try d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
{ d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
DOP_ENU=arma::htrans(F)*Q_ECEF*F; d_TDOP = sqrt(d_Q(3,3)); // TDOP
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP }
d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP catch(std::exception& ex)
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP {
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP d_GDOP = -1; // Geometric DOP
d_TDOP = sqrt(d_Q(3,3)); // TDOP d_PDOP = -1; // PDOP
}catch(std::exception& ex) d_HDOP = -1; // HDOP
{ d_VDOP = -1; // VDOP
d_GDOP = -1; // Geometric DOP d_TDOP = -1; // TDOP
d_PDOP = -1; // PDOP }
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
// ######## LOG FILE ######### // ######## LOG FILE #########
if(d_flag_dump_enabled == true) if(d_flag_dump_enabled == true)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try try
{ {
double tmp_double; double tmp_double;
// PVT GPS time // PVT GPS time
tmp_double = galileo_current_time; tmp_double = galileo_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]
@@ -466,7 +448,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
d_avg_latitude_d = 0; d_avg_latitude_d = 0;
d_avg_longitude_d = 0; d_avg_longitude_d = 0;
d_avg_height_m = 0; d_avg_height_m = 0;
for (unsigned int i=0; i<d_hist_longitude_d.size(); i++) for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++)
{ {
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
@@ -480,7 +462,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
} }
else else
{ {
//int current_depth=d_hist_longitude_d.size(); // int current_depth=d_hist_longitude_d.size();
// Push new values // Push new values
d_hist_longitude_d.push_front(d_longitude_d); d_hist_longitude_d.push_front(d_longitude_d);
d_hist_latitude_d.push_front(d_latitude_d); d_hist_latitude_d.push_front(d_latitude_d);
@@ -504,7 +486,7 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
b_valid_position = false; b_valid_position = false;
return false; return false;
} }
return false; return false;
} }
@@ -524,8 +506,8 @@ void galileo_e1_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_sele
const double a[5] = {6378388, 6378160, 6378135, 6378137, 6378137}; const double a[5] = {6378388, 6378160, 6378135, 6378137, 6378137};
const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563}; const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
double lambda = atan2(Y,X); double lambda = atan2(Y, X);
double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection])); double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1+ex2); double c = a[elipsoid_selection] * sqrt(1+ex2);
double phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection]))); double phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
@@ -537,7 +519,7 @@ void galileo_e1_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_sele
{ {
oldh = h; oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(Z / ((sqrt(X*X + Y*Y) * (1 - (2 -f[elipsoid_selection]) * f[elipsoid_selection] *N / (N + h) )))); phi = atan(Z / ((sqrt(X*X + Y*Y) * (1 - (2 - f[elipsoid_selection]) * f[elipsoid_selection] *N / (N + h) ))));
h = sqrt(X*X + Y*Y) / cos(phi) - N; h = sqrt(X*X + Y*Y) / cos(phi) - N;
iterations = iterations + 1; iterations = iterations + 1;
if (iterations > 100) if (iterations > 100)
@@ -579,7 +561,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
*h = 0; *h = 0;
double tolsq = 1.e-10; // tolerance to accept convergence double tolsq = 1.e-10; // tolerance to accept convergence
int maxit = 10; // max number of iterations int maxit = 10; // max number of iterations
double rtd = 180/GPS_PI; double rtd = 180/GPS_PI;
// compute square of eccentricity // compute square of eccentricity
double esq; double esq;
@@ -593,22 +575,24 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
} }
// first guess // first guess
double P = sqrt(X*X + Y*Y); // P is distance from spin axis double P = sqrt(X*X + Y*Y); // P is distance from spin axis
//direct calculation of longitude //direct calculation of longitude
if (P > 1.0E-20) if (P > 1.0E-20)
{ {
*dlambda = atan2(Y,X) * rtd; *dlambda = atan2(Y, X) * rtd;
} }
else else
{ {
*dlambda = 0; *dlambda = 0;
} }
// correct longitude bound // correct longitude bound
if (*dlambda < 0) if (*dlambda < 0)
{ {
*dlambda = *dlambda + 360.0; *dlambda = *dlambda + 360.0;
} }
double r = sqrt(P*P + Z*Z); // r is distance from origin (0,0,0) double r = sqrt(P*P + Z*Z); // r is distance from origin (0,0,0)
double sinphi; double sinphi;
@@ -630,7 +614,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
return; return;
} }
*h = r - a*(1-sinphi*sinphi/finv); *h = r - a*(1 - sinphi*sinphi/finv);
// iterate // iterate
double cosphi; double cosphi;
@@ -639,28 +623,28 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
double dZ; double dZ;
double oneesq = 1 - esq; double oneesq = 1 - esq;
for (int i=0; i<maxit; i++) for (int i = 0; i < maxit; i++)
{ {
sinphi = sin(*dphi); sinphi = sin(*dphi);
cosphi = cos(*dphi); cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction // compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq*sinphi*sinphi); N_phi = a / sqrt(1 - esq*sinphi*sinphi);
// compute residuals in P and Z // compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi; dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi*oneesq + (*h)) * sinphi; dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
// update height and latitude // update height and latitude
*h = *h + (sinphi*dZ + cosphi*dP); *h = *h + (sinphi*dZ + cosphi*dP);
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h)); *dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
// test for convergence // test for convergence
if ((dP*dP + dZ*dZ) < tolsq) if ((dP*dP + dZ*dZ) < tolsq)
{ {
break; break;
} }
if (i == (maxit-1)) if (i == (maxit - 1))
{ {
DLOG(INFO) << "The computation of geodetic coordinates did not converge"; DLOG(INFO) << "The computation of geodetic coordinates did not converge";
} }
@@ -684,7 +668,6 @@ void galileo_e1_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
Based on a Matlab function by Kai Borre Based on a Matlab function by Kai Borre
*/ */
double lambda; double lambda;
double phi; double phi;
double h; double h;
@@ -695,10 +678,10 @@ void galileo_e1_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
// Transform x into geodetic coordinates // Transform x into geodetic coordinates
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2)); togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr); double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr); double sl = sin(lambda * dtr);
double cb = cos(phi * dtr); double cb = cos(phi * dtr);
double sb = sin(phi * dtr); double sb = sin(phi * dtr);
arma::mat F = arma::zeros(3,3); arma::mat F = arma::zeros(3,3);
@@ -741,5 +724,5 @@ void galileo_e1_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
*Az = *Az + 360.0; *Az = *Az + 360.0;
} }
*D = sqrt(dx(0)*dx(0) + dx(1)*dx(1) + dx(2)*dx(2)); *D = sqrt(dx(0)*dx(0) + dx(1)*dx(1) + dx(2)*dx(2));
} }

42
src/algorithms/PVT/libs/galileo_e1_ls_pvt.h
View File

@@ -3,9 +3,10 @@
* \brief Interface of a Least Squares Position, Velocity, and Time (PVT) * \brief Interface of a Least Squares Position, Velocity, and Time (PVT)
* solver, based on K.Borre's Matlab receiver. * solver, based on K.Borre's Matlab receiver.
* \author Javier Arribas, 2011. jarribas(at)cttc.es * \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -38,7 +39,6 @@
#include <stdio.h> #include <stdio.h>
#include <sys/time.h> #include <sys/time.h>
#include <time.h> #include <time.h>
//#include <math.h>
#include <cmath> #include <cmath>
#include <map> #include <map>
#include <algorithm> #include <algorithm>
@@ -46,10 +46,10 @@
#include "galileo_navigation_message.h" #include "galileo_navigation_message.h"
#include "armadillo" #include "armadillo"
#include "boost/date_time/posix_time/posix_time.hpp" #include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include "galileo_ephemeris.h" #include "galileo_ephemeris.h"
#include "galileo_utc_model.h" #include "galileo_utc_model.h"
#define PVT_MAX_CHANNELS 24 #define PVT_MAX_CHANNELS 24
/*! /*!
@@ -63,21 +63,18 @@ private:
void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx); void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx);
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
public: public:
int d_nchannels; //! Number of available channels for positioning int d_nchannels; //!< Number of available channels for positioning
int d_valid_observations; //! Number of valid pseudorange observations (valid satellites) int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //! Array with the LOS Elevation of the valid satellites double d_visible_satellites_El[PVT_MAX_CHANNELS]; //!< Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //! Array with the LOS Azimuth of the valid satellites double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //!< Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //! Array with the LOS Distance of the valid satellites double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
Galileo_Navigation_Message* d_ephemeris; Galileo_Navigation_Message* d_ephemeris;
// new ephemeris storage std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map;
// new utc_model storage
Galileo_Utc_Model galileo_utc_model; Galileo_Utc_Model galileo_utc_model;
// new iono storage
Galileo_Iono galileo_iono; Galileo_Iono galileo_iono;
double d_galileo_current_time; double d_galileo_current_time;
@@ -85,26 +82,24 @@ public:
bool b_valid_position; bool b_valid_position;
double d_latitude_d; //! Latitude in degrees double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //! Longitude in degrees double d_longitude_d; //!< Longitude in degrees
double d_height_m; //! Height [m] double d_height_m; //!< Height [m]
//averaging //averaging
std::deque<double> d_hist_latitude_d; std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d; std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m; std::deque<double> d_hist_height_m;
int d_averaging_depth; //! Length of averaging window int d_averaging_depth; //!< Length of averaging window
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_latitude_d; //! Averaged latitude in degrees double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_longitude_d; //! Averaged longitude in degrees double d_avg_height_m; //!< Averaged height [m]
double d_avg_height_m; //! Averaged height [m]
double d_x_m; double d_x_m;
double d_y_m; double d_y_m;
double d_z_m; double d_z_m;
// DOP estimations // DOP estimations
arma::mat d_Q; arma::mat d_Q;
double d_GDOP; double d_GDOP;
double d_PDOP; double d_PDOP;
@@ -121,6 +116,7 @@ public:
void set_averaging_depth(int depth); void set_averaging_depth(int depth);
galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file); galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file);
~galileo_e1_ls_pvt(); ~galileo_e1_ls_pvt();
bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging); bool get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging);

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

@@ -144,16 +144,6 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
omc = arma::zeros(nmbOfSatellites, 1); omc = arma::zeros(nmbOfSatellites, 1);
az = arma::zeros(1, nmbOfSatellites); az = arma::zeros(1, nmbOfSatellites);
el = arma::zeros(1, nmbOfSatellites); el = arma::zeros(1, nmbOfSatellites);
for (int i = 0; i < nmbOfSatellites; i++)
{
for (int j = 0; j < 4; j++)
{
A(i, j) = 0.0; //Armadillo
}
omc(i, 0) = 0.0;
az(0, i) = 0.0;
}
el = az;
arma::mat X = satpos; arma::mat X = satpos;
arma::vec Rot_X; arma::vec Rot_X;
double rho2; double rho2;
@@ -207,7 +197,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
//--- Apply position update -------------------------------------------- //--- Apply position update --------------------------------------------
pos = pos + x; pos = pos + x;
if (arma::norm(x,2)<1e-4) if (arma::norm(x, 2) < 1e-4)
{ {
break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm) break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm)
} }
@@ -216,12 +206,11 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
try try
{ {
//-- compute the Dilution Of Precision values //-- compute the Dilution Of Precision values
//arma::mat Q; d_Q = arma::inv(arma::htrans(A)*A);
d_Q = arma::inv(arma::htrans(A)*A);
} }
catch(std::exception& e) catch(std::exception& e)
{ {
d_Q=arma::zeros(4,4); d_Q = arma::zeros(4, 4);
} }
return pos; return pos;
} }
@@ -233,9 +222,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter; std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
int valid_pseudoranges = gnss_pseudoranges_map.size(); int valid_pseudoranges = gnss_pseudoranges_map.size();
arma::mat W = arma::eye(valid_pseudoranges,valid_pseudoranges); //channels weights matrix arma::mat W = arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix
arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
arma::mat satpos = arma::zeros(3,valid_pseudoranges); //satellite positions matrix arma::mat satpos = arma::zeros(3, valid_pseudoranges); //satellite positions matrix
int GPS_week = 0; int GPS_week = 0;
double utc = 0; double utc = 0;
@@ -280,9 +269,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
TX_time_corrected_s = Tx_time - SV_clock_bias_s; TX_time_corrected_s = Tx_time - SV_clock_bias_s;
gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
satpos(0,obs_counter) = gps_ephemeris_iter->second.d_satpos_X; satpos(0, obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1,obs_counter) = gps_ephemeris_iter->second.d_satpos_Y; satpos(1, obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2,obs_counter) = gps_ephemeris_iter->second.d_satpos_Z; satpos(2, obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
// 5- fill the observations vector with the corrected pseudorranges // 5- fill the observations vector with the corrected pseudorranges
obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s*GPS_C_m_s; obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s*GPS_C_m_s;
@@ -295,19 +284,18 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
<< " X=" << gps_ephemeris_iter->second.d_satpos_X << " 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(obs_counter) << " [m]" << std::endl; << " [m] PR_obs=" << obs(obs_counter) << " [m]";
// compute the UTC time for this SV (just to print the asociated UTC timestamp) // compute the UTC time for this SV (just to print the asociated UTC timestamp)
GPS_week = gps_ephemeris_iter->second.i_GPS_week; GPS_week = gps_ephemeris_iter->second.i_GPS_week;
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week); utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
} }
else // the ephemeris are not available for this SV else // the ephemeris are not available for this SV
{ {
// no valid pseudorange for the current SV // no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated W(obs_counter, obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero) obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first << std::endl; DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
} }
obs_counter++; obs_counter++;
} }
@@ -316,22 +304,22 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
// ****** SOLVE LEAST SQUARES****************************************************** // ****** SOLVE LEAST SQUARES******************************************************
// ******************************************************************************** // ********************************************************************************
d_valid_observations = valid_obs; d_valid_observations = valid_obs;
DLOG(INFO) << "(new)PVT: valid observations=" << valid_obs << std::endl; DLOG(INFO) << "(new)PVT: valid observations=" << valid_obs;
if (valid_obs >= 4) if (valid_obs >= 4)
{ {
arma::vec mypos; arma::vec mypos;
DLOG(INFO) << "satpos=" << satpos << std::endl; DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs="<< obs << std::endl; DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << W <<std::endl; DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W); mypos = leastSquarePos(satpos, obs, W);
DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos << std::endl; DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos;
gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4); gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4);
//ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km)
if (d_height_m>50000) if (d_height_m > 50000)
{ {
b_valid_position=false; b_valid_position = false;
return false; //erratic PVT return false;
} }
// Compute UTC time and print PVT solution // Compute UTC time and print PVT solution
double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60) double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60)
@@ -342,47 +330,46 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
DLOG(INFO) << "(new)Position at " << boost::posix_time::to_simple_string(p_time) DLOG(INFO) << "(new)Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << std::endl; << " [deg], Height= " << d_height_m << " [m]";
// ###### Compute DOPs ######## // ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to ENU coordinates // 1- Rotation matrix from ECEF coordinates to ENU coordinates
// ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates
arma::mat F=arma::zeros(3,3);
F(0,0) = -sin(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,1) = -sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(0,2) = cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
arma::mat F=arma::zeros(3,3); F(1,0) = cos((GPS_TWO_PI*d_longitude_d)/360.0);
F(0,0)=-sin(GPS_TWO_PI*(d_longitude_d/360.0)); F(1,1) = -sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(0,1)=-sin(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0)); F(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(0,2)=cos(GPS_TWO_PI*(d_latitude_d/360.0))*cos(GPS_TWO_PI*(d_longitude_d/360.0));
F(1,0)=cos((GPS_TWO_PI*d_longitude_d)/360.0); F(2,0) = 0;
F(1,1)=-sin((GPS_TWO_PI*d_latitude_d)/360.0)*sin((GPS_TWO_PI*d_longitude_d)/360.0); F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(1,2)=cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0); F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0));
F(2,0)=0; // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
F(2,1)=cos((GPS_TWO_PI*d_latitude_d)/360.0); arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
F(2,2)=sin((GPS_TWO_PI*d_latitude_d/360.0)); arma::mat DOP_ENU = arma::zeros(3, 3);
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS) try
{
arma::mat Q_ECEF=d_Q.submat( 0, 0, 2, 2); DOP_ENU = arma::htrans(F)*Q_ECEF*F;
arma::mat DOP_ENU=arma::zeros(3,3); d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP
d_PDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1) + DOP_ENU(2, 2));// PDOP
try d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1)); // HDOP
{ d_VDOP = sqrt(DOP_ENU(2, 2)); // VDOP
DOP_ENU=arma::htrans(F)*Q_ECEF*F; d_TDOP = sqrt(d_Q(3, 3)); // TDOP
d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP }
d_PDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1) + DOP_ENU(2,2)); // PDOP catch(std::exception& ex)
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP {
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP d_GDOP = -1; // Geometric DOP
d_TDOP = sqrt(d_Q(3,3)); // TDOP d_PDOP = -1; // PDOP
}catch(std::exception& ex) d_HDOP = -1; // HDOP
{ d_VDOP = -1; // VDOP
d_GDOP = -1; // Geometric DOP d_TDOP = -1; // TDOP
d_PDOP = -1; // PDOP }
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
// ######## LOG FILE ######### // ######## LOG FILE #########
if(d_flag_dump_enabled == true) if(d_flag_dump_enabled == true)
@@ -418,7 +405,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
{ {
std::cout << "Exception writing PVT LS dump file "<< e.what() << std::endl; std::cout << "Exception writing PVT LS dump file " << e.what() << std::endl;
} }
} }
@@ -439,7 +426,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map,
d_avg_latitude_d = 0; d_avg_latitude_d = 0;
d_avg_longitude_d = 0; d_avg_longitude_d = 0;
d_avg_height_m = 0; d_avg_height_m = 0;
for (unsigned int i=0; i<d_hist_longitude_d.size(); i++) for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++)
{ {
d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i);
d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i);
@@ -497,8 +484,8 @@ void gps_l1_ca_ls_pvt::cart2geo(double X, double Y, double Z, int elipsoid_selec
const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563}; const double f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
double lambda = atan2(Y,X); double lambda = atan2(Y,X);
double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection])); double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1+ex2); double c = a[elipsoid_selection] * sqrt(1 + ex2);
double phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection]))); double phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1; double h = 0.1;
@@ -551,7 +538,7 @@ void gps_l1_ca_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a
*h = 0; *h = 0;
double tolsq = 1.e-10; // tolerance to accept convergence double tolsq = 1.e-10; // tolerance to accept convergence
int maxit = 10; // max number of iterations int maxit = 10; // max number of iterations
double rtd = 180/GPS_PI; double rtd = 180/GPS_PI;
// compute square of eccentricity // compute square of eccentricity
double esq; double esq;
@@ -611,28 +598,28 @@ void gps_l1_ca_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a
double dZ; double dZ;
double oneesq = 1 - esq; double oneesq = 1 - esq;
for (int i=0; i<maxit; i++) for (int i = 0; i < maxit; i++)
{ {
sinphi = sin(*dphi); sinphi = sin(*dphi);
cosphi = cos(*dphi); cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction // compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq*sinphi*sinphi); N_phi = a / sqrt(1 - esq*sinphi*sinphi);
// compute residuals in P and Z // compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi; dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi*oneesq + (*h)) * sinphi; dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
// update height and latitude // update height and latitude
*h = *h + (sinphi*dZ + cosphi*dP); *h = *h + (sinphi*dZ + cosphi*dP);
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h)); *dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
// test for convergence // test for convergence
if ((dP*dP + dZ*dZ) < tolsq) if ((dP*dP + dZ*dZ) < tolsq)
{ {
break; break;
} }
if (i == (maxit-1)) if (i == (maxit - 1))
{ {
DLOG(INFO) << "The computation of geodetic coordinates did not converge"; DLOG(INFO) << "The computation of geodetic coordinates did not converge";
} }
@@ -667,10 +654,10 @@ void gps_l1_ca_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
// Transform x into geodetic coordinates // Transform x into geodetic coordinates
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2)); togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr); double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr); double sl = sin(lambda * dtr);
double cb = cos(phi * dtr); double cb = cos(phi * dtr);
double sb = sin(phi * dtr); double sb = sin(phi * dtr);
arma::mat F = arma::zeros(3,3); arma::mat F = arma::zeros(3,3);
@@ -713,5 +700,5 @@ void gps_l1_ca_ls_pvt::topocent(double *Az, double *El, double *D, arma::vec x,
*Az = *Az + 360.0; *Az = *Az + 360.0;
} }
*D = sqrt(dx(0)*dx(0) + dx(1)*dx(1) + dx(2)*dx(2)); *D = sqrt(dx(0)*dx(0) + dx(1)*dx(1) + dx(2)*dx(2));
} }

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

@@ -45,11 +45,9 @@
#include "GPS_L1_CA.h" #include "GPS_L1_CA.h"
#include "armadillo" #include "armadillo"
#include "boost/date_time/posix_time/posix_time.hpp" #include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include "gps_ephemeris.h" #include "gps_ephemeris.h"
#include "gps_utc_model.h" #include "gps_utc_model.h"
#include "sbas_telemetry_data.h" #include "sbas_telemetry_data.h"
#include "sbas_ionospheric_correction.h" #include "sbas_ionospheric_correction.h"
#include "sbas_satellite_correction.h" #include "sbas_satellite_correction.h"
@@ -68,23 +66,21 @@ private:
void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx); void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx);
void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
public: public:
int d_nchannels; //! Number of available channels for positioning int d_nchannels; //!< Number of available channels for positioning
int d_valid_observations; //! Number of valid pseudorange observations (valid satellites) int d_valid_observations; //!< Number of valid pseudorange observations (valid satellites)
int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
double d_visible_satellites_El[PVT_MAX_CHANNELS]; //! Array with the LOS Elevation of the valid satellites double d_visible_satellites_El[PVT_MAX_CHANNELS]; //!< Array with the LOS Elevation of the valid satellites
double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //! Array with the LOS Azimuth of the valid satellites double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //!< Array with the LOS Azimuth of the valid satellites
double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //! Array with the LOS Distance of the valid satellites double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //!< Array with the LOS Distance of the valid satellites
double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //!< Array with the IDs of the valid satellites
Gps_Navigation_Message* d_ephemeris; Gps_Navigation_Message* d_ephemeris;
// new ephemeris storage // new ephemeris storage
std::map<int,Gps_Ephemeris> gps_ephemeris_map; std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new Gps_Ephemeris
// new utc_model storage
Gps_Utc_Model gps_utc_model; Gps_Utc_Model gps_utc_model;
// new iono storage
Gps_Iono gps_iono; Gps_Iono gps_iono;
// new SBAS storage
Sbas_Ionosphere_Correction sbas_iono; Sbas_Ionosphere_Correction sbas_iono;
std::map<int,Sbas_Satellite_Correction> sbas_sat_corr_map; std::map<int,Sbas_Satellite_Correction> sbas_sat_corr_map;
std::map<int,Sbas_Ephemeris> sbas_ephemeris_map; std::map<int,Sbas_Ephemeris> sbas_ephemeris_map;
@@ -94,26 +90,24 @@ public:
bool b_valid_position; bool b_valid_position;
double d_latitude_d; //! Latitude in degrees double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //! Longitude in degrees double d_longitude_d; //!< Longitude in degrees
double d_height_m; //! Height [m] double d_height_m; //!< Height [m]
//averaging //averaging
std::deque<double> d_hist_latitude_d; std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d; std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m; std::deque<double> d_hist_height_m;
int d_averaging_depth; //! Length of averaging window int d_averaging_depth; //!< Length of averaging window
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_latitude_d; //! Averaged latitude in degrees double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_longitude_d; //! Averaged longitude in degrees double d_avg_height_m; //!< Averaged height [m]
double d_avg_height_m; //! Averaged height [m]
double d_x_m; double d_x_m;
double d_y_m; double d_y_m;
double d_z_m; double d_z_m;
// DOP estimations // DOP estimations
arma::mat d_Q; arma::mat d_Q;
double d_GDOP; double d_GDOP;
double d_PDOP; double d_PDOP;

2
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h
View File

@@ -50,7 +50,7 @@ class ConfigurationInterface;
class GpsL1CaPcpsAssistedAcquisition: public AcquisitionInterface class GpsL1CaPcpsAssistedAcquisition: public AcquisitionInterface
{ {
public: public:
GpsL1CaPcpsAssistedAcquisition(ConfigurationInterface* configuration, GpsL1CaPcpsAssistedAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams, std::string role, unsigned int in_streams,
unsigned int out_streams, boost::shared_ptr<gr::msg_queue> queue); unsigned int out_streams, boost::shared_ptr<gr::msg_queue> queue);

234
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h
View File

@@ -84,158 +84,156 @@ class pcps_acquisition_cc: public gr::block
private: private:
friend pcps_acquisition_cc_sptr friend pcps_acquisition_cc_sptr
pcps_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump, gr::msg_queue::sptr queue, bool dump,
std::string dump_filename); std::string dump_filename);
pcps_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump, gr::msg_queue::sptr queue, bool dump,
std::string dump_filename); std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
long d_fs_in;
long d_fs_in; long d_freq;
long d_freq; int d_samples_per_ms;
int d_samples_per_ms;
int d_samples_per_code; int d_samples_per_code;
unsigned int d_doppler_resolution; unsigned int d_doppler_resolution;
float d_threshold; float d_threshold;
std::string d_satellite_str; std::string d_satellite_str;
unsigned int d_doppler_max; unsigned int d_doppler_max;
unsigned int d_doppler_step; unsigned int d_doppler_step;
unsigned int d_sampled_ms; unsigned int d_sampled_ms;
unsigned int d_max_dwells; unsigned int d_max_dwells;
unsigned int d_well_count; unsigned int d_well_count;
unsigned int d_fft_size; unsigned int d_fft_size;
unsigned long int d_sample_counter; unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs; gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins; unsigned int d_num_doppler_bins;
gr_complex* d_fft_codes; gr_complex* d_fft_codes;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
float* d_magnitude; float* d_magnitude;
float d_input_power; float d_input_power;
float d_test_statistics; float d_test_statistics;
bool d_bit_transition_flag; bool d_bit_transition_flag;
gr::msg_queue::sptr d_queue; gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue; concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file; std::ofstream d_dump_file;
bool d_active; bool d_active;
int d_state; int d_state;
bool d_dump; bool d_dump;
unsigned int d_channel; unsigned int d_channel;
std::string d_dump_filename; std::string d_dump_filename;
public: public:
/*! /*!
* \brief Default destructor. * \brief Default destructor.
*/ */
~pcps_acquisition_cc(); ~pcps_acquisition_cc();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
/*! /*!
* \brief Returns the maximum peak of grid search. * \brief Returns the maximum peak of grid search.
*/ */
unsigned int mag() unsigned int mag()
{ {
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float> * code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
void set_active(bool active) void set_active(bool active)
{ {
d_active = active; d_active = active;
} }
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
void set_channel(unsigned int channel) void set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of PCPS algorithm. * \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
void set_threshold(float threshold) void set_threshold(float threshold)
{ {
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
void set_doppler_max(unsigned int doppler_max) void set_doppler_max(unsigned int doppler_max)
{ {
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
void set_doppler_step(unsigned int doppler_step) void set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*! /*!
* \brief Set tracking channel internal queue. * \brief Set tracking channel internal queue.
* \param channel_internal_queue - Channel's internal blocks information queue. * \param channel_internal_queue - Channel's internal blocks information queue.
*/ */
void set_channel_queue(concurrent_queue<int> *channel_internal_queue) void set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{ {
d_channel_internal_queue = channel_internal_queue; d_channel_internal_queue = channel_internal_queue;
} }
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items); gr_vector_void_star &output_items);
}; };
#endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/ #endif /* GNSS_SDR_PCPS_ACQUISITION_CC_H_*/

54
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc
View File

@@ -98,20 +98,27 @@ pcps_assisted_acquisition_cc::pcps_assisted_acquisition_cc(
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
} }
void pcps_assisted_acquisition_cc::set_doppler_step(unsigned int doppler_step) void pcps_assisted_acquisition_cc::set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
void pcps_assisted_acquisition_cc::free_grid_memory() void pcps_assisted_acquisition_cc::free_grid_memory()
{ {
for (int i=0;i<d_num_doppler_points;i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
delete[] d_grid_data[i]; delete[] d_grid_data[i];
delete[] d_grid_doppler_wipeoffs[i]; delete[] d_grid_doppler_wipeoffs[i];
} }
delete d_grid_data; delete d_grid_data;
} }
pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc() pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
{ {
free(d_carrier); free(d_carrier);
@@ -139,17 +146,16 @@ void pcps_assisted_acquisition_cc::init()
d_gnss_synchro->Acq_doppler_hz = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0; d_gnss_synchro->Acq_samplestamp_samples = 0;
d_input_power = 0.0; d_input_power = 0.0;
d_state = 0; d_state = 0;
d_fft_if->execute(); // We need the FFT of local code d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code //Conjugate the local code
volk_32fc_conjugate_32fc_a(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size); volk_32fc_conjugate_32fc_a(d_fft_codes, d_fft_if->get_outbuf(), d_fft_size);
} }
void pcps_assisted_acquisition_cc::forecast (int noutput_items, void pcps_assisted_acquisition_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
@@ -157,6 +163,7 @@ void pcps_assisted_acquisition_cc::forecast (int noutput_items,
} }
void pcps_assisted_acquisition_cc::get_assistance() void pcps_assisted_acquisition_cc::get_assistance()
{ {
Gps_Acq_Assist gps_acq_assisistance; Gps_Acq_Assist gps_acq_assisistance;
@@ -175,7 +182,7 @@ void pcps_assisted_acquisition_cc::get_assistance()
} }
this->d_disable_assist = false; this->d_disable_assist = false;
std::cout << "Acq assist ENABLED for GPS SV "<< this->d_gnss_synchro->PRN <<" (Doppler max,Doppler min)=(" std::cout << "Acq assist ENABLED for GPS SV "<< this->d_gnss_synchro->PRN <<" (Doppler max,Doppler min)=("
<< d_doppler_max << "," << d_doppler_min << ")" << std::endl; << d_doppler_max << "," << d_doppler_min << ")" << std::endl;
} }
else else
{ {
@@ -183,20 +190,26 @@ void pcps_assisted_acquisition_cc::get_assistance()
std::cout << "Acq assist DISABLED for GPS SV "<< this->d_gnss_synchro->PRN << std::endl; std::cout << "Acq assist DISABLED for GPS SV "<< this->d_gnss_synchro->PRN << std::endl;
} }
} }
void pcps_assisted_acquisition_cc::reset_grid() void pcps_assisted_acquisition_cc::reset_grid()
{ {
d_well_count = 0; d_well_count = 0;
for (int i=0;i<d_num_doppler_points;i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
for (unsigned int j=0;j<d_fft_size;j++) for (unsigned int j = 0; j < d_fft_size; j++)
{ {
d_grid_data[i][j] = 0.0; d_grid_data[i][j] = 0.0;
} }
} }
} }
void pcps_assisted_acquisition_cc::redefine_grid() void pcps_assisted_acquisition_cc::redefine_grid()
{ {
if (this->d_disable_assist==true) if (this->d_disable_assist == true)
{ {
d_doppler_max = d_config_doppler_max; d_doppler_max = d_config_doppler_max;
d_doppler_min = d_config_doppler_min; d_doppler_min = d_config_doppler_min;
@@ -204,8 +217,8 @@ void pcps_assisted_acquisition_cc::redefine_grid()
// Create the search grid array // Create the search grid array
d_num_doppler_points = floor(std::abs(d_doppler_max-d_doppler_min)/d_doppler_step); d_num_doppler_points = floor(std::abs(d_doppler_max-d_doppler_min)/d_doppler_step);
d_grid_data=new float*[d_num_doppler_points]; d_grid_data = new float*[d_num_doppler_points];
for (int i=0; i<d_num_doppler_points; i++) for (int i = 0; i < d_num_doppler_points; i++)
{ {
d_grid_data[i] = new float[d_fft_size]; d_grid_data[i] = new float[d_fft_size];
} }
@@ -214,18 +227,19 @@ void pcps_assisted_acquisition_cc::redefine_grid()
int doppler_hz; int doppler_hz;
float phase_step_rad; float phase_step_rad;
d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_points]; d_grid_doppler_wipeoffs = new gr_complex*[d_num_doppler_points];
for (int doppler_index=0; doppler_index<d_num_doppler_points; doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
doppler_hz = d_doppler_min + d_doppler_step*doppler_index;
doppler_hz = d_doppler_min+d_doppler_step*doppler_index;
// doppler search steps // doppler search steps
// compute the carrier doppler wipe-off signal and store it // compute the carrier doppler wipe-off signal and store it
phase_step_rad = (float)GPS_TWO_PI*doppler_hz / (float)d_fs_in; phase_step_rad = (float)GPS_TWO_PI*doppler_hz / (float)d_fs_in;
d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size]; d_grid_doppler_wipeoffs[doppler_index] = new gr_complex[d_fft_size];
fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size,0, phase_step_rad); fxp_nco(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, 0, phase_step_rad);
} }
} }
double pcps_assisted_acquisition_cc::search_maximum() double pcps_assisted_acquisition_cc::search_maximum()
{ {
float magt = 0.0; float magt = 0.0;
@@ -274,6 +288,8 @@ double pcps_assisted_acquisition_cc::search_maximum()
return d_test_statistics; return d_test_statistics;
} }
float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items) float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_star &input_items)
{ {
const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer const gr_complex *in = (const gr_complex *)input_items[0]; //Get the input samples pointer
@@ -289,6 +305,8 @@ float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_st
return ( power / (float)d_fft_size); return ( power / (float)d_fft_size);
} }
int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items) int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
{ {
// initialize acquisition algorithm // initialize acquisition algorithm
@@ -305,7 +323,7 @@ int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_vo
float* p_tmp_vector; float* p_tmp_vector;
if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){}; if (posix_memalign((void**)&p_tmp_vector, 16, d_fft_size * sizeof(float)) == 0){};
for (int doppler_index=0; doppler_index<d_num_doppler_points; doppler_index++) for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{ {
// doppler search steps // doppler search steps
// Perform the carrier wipe-off // Perform the carrier wipe-off
@@ -325,7 +343,6 @@ int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_vo
volk_32fc_magnitude_squared_32f_a(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size); volk_32fc_magnitude_squared_32f_a(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
const float* old_vector = d_grid_data[doppler_index]; const float* old_vector = d_grid_data[doppler_index];
volk_32f_x2_add_32f_a(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size); volk_32f_x2_add_32f_a(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
} }
free(p_tmp_vector); free(p_tmp_vector);
return d_fft_size; return d_fft_size;
@@ -334,7 +351,6 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items) gr_vector_void_star &output_items)
{ {
/*! /*!
* TODO: High sensitivity acquisition algorithm: * TODO: High sensitivity acquisition algorithm:
* State Mechine: * State Mechine:
@@ -355,7 +371,7 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items,
switch (d_state) switch (d_state)
{ {
case 0: // S0. StandBy case 0: // S0. StandBy
if (d_active==true) d_state = 1; if (d_active == true) d_state = 1;
d_sample_counter += ninput_items[0]; // sample counter d_sample_counter += ninput_items[0]; // sample counter
consume_each(ninput_items[0]); consume_each(ninput_items[0]);
break; break;
@@ -371,7 +387,7 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items,
int consumed_samples; int consumed_samples;
consumed_samples = compute_and_accumulate_grid(input_items); consumed_samples = compute_and_accumulate_grid(input_items);
d_well_count++; d_well_count++;
if (d_well_count>=d_max_dwells) if (d_well_count >= d_max_dwells)
{ {
d_state=3; d_state=3;
} }
@@ -418,7 +434,6 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items,
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL // Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL
d_channel_internal_queue->push(1); // 1-> positive acquisition d_channel_internal_queue->push(1); // 1-> positive acquisition
@@ -437,7 +452,6 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items,
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power; DLOG(INFO) << "input signal power " << d_input_power;
d_active = false; d_active = false;
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL // Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL
d_channel_internal_queue->push(2); // 2-> negative acquisition d_channel_internal_queue->push(2); // 2-> negative acquisition

7
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h
View File

@@ -45,8 +45,8 @@
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
*/ */
#ifndef GNSS_SDR_PCPS_assisted_acquisition_cc_H_ #ifndef GNSS_SDR_PCPS_ASSISTED_ACQUISITION_CC_H_
#define GNSS_SDR_PCPS_assisted_acquisition_cc_H_ #define GNSS_SDR_PCPS_ASSISTED_ACQUISITION_CC_H_
#include <fstream> #include <fstream>
#include <gnuradio/block.h> #include <gnuradio/block.h>
@@ -75,7 +75,6 @@ pcps_make_assisted_acquisition_cc(int max_dwells, unsigned int sampled_ms,
* Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver", * Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver",
* Algorithm 1, for a pseudocode description of this implementation. * Algorithm 1, for a pseudocode description of this implementation.
*/ */
class pcps_assisted_acquisition_cc: public gr::block class pcps_assisted_acquisition_cc: public gr::block
{ {
private: private:
@@ -223,7 +222,6 @@ public:
*/ */
void set_doppler_step(unsigned int doppler_step); void set_doppler_step(unsigned int doppler_step);
/*! /*!
* \brief Set tracking channel internal queue. * \brief Set tracking channel internal queue.
* \param channel_internal_queue - Channel's internal blocks information queue. * \param channel_internal_queue - Channel's internal blocks information queue.
@@ -241,7 +239,6 @@ public:
gr_vector_void_star &output_items); gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast (int noutput_items, gr_vector_int &ninput_items_required);
}; };
#endif /* GNSS_SDR_PCPS_assisted_acquisition_cc_H_*/ #endif /* GNSS_SDR_PCPS_assisted_acquisition_cc_H_*/

219
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h
View File

@@ -69,162 +69,159 @@ class pcps_cccwsr_acquisition_cc: public gr::block
private: private:
friend pcps_cccwsr_acquisition_cc_sptr friend pcps_cccwsr_acquisition_cc_sptr
pcps_cccwsr_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_cccwsr_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
gr::msg_queue::sptr queue, bool dump, gr::msg_queue::sptr queue, bool dump,
std::string dump_filename); std::string dump_filename);
pcps_cccwsr_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_cccwsr_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
gr::msg_queue::sptr queue, bool dump, gr::msg_queue::sptr queue, bool dump,
std::string dump_filename); std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
long d_fs_in;
long d_fs_in; long d_freq;
long d_freq; int d_samples_per_ms;
int d_samples_per_ms;
int d_samples_per_code; int d_samples_per_code;
unsigned int d_doppler_resolution; unsigned int d_doppler_resolution;
float d_threshold; float d_threshold;
std::string d_satellite_str; std::string d_satellite_str;
unsigned int d_doppler_max; unsigned int d_doppler_max;
unsigned int d_doppler_step; unsigned int d_doppler_step;
unsigned int d_sampled_ms; unsigned int d_sampled_ms;
unsigned int d_max_dwells; unsigned int d_max_dwells;
unsigned int d_well_count; unsigned int d_well_count;
unsigned int d_fft_size; unsigned int d_fft_size;
unsigned long int d_sample_counter; unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs; gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins; unsigned int d_num_doppler_bins;
gr_complex* d_fft_code_data; gr_complex* d_fft_code_data;
gr_complex* d_fft_code_pilot; gr_complex* d_fft_code_pilot;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
float* d_magnitude; float* d_magnitude;
gr_complex* d_data_correlation; gr_complex* d_data_correlation;
gr_complex* d_pilot_correlation; gr_complex* d_pilot_correlation;
gr_complex* d_correlation_plus; gr_complex* d_correlation_plus;
gr_complex* d_correlation_minus; gr_complex* d_correlation_minus;
float d_input_power; float d_input_power;
float d_test_statistics; float d_test_statistics;
gr::msg_queue::sptr d_queue; gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue; concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file; std::ofstream d_dump_file;
bool d_active; bool d_active;
int d_state; int d_state;
bool d_dump; bool d_dump;
unsigned int d_channel; unsigned int d_channel;
std::string d_dump_filename; std::string d_dump_filename;
public: public:
/*! /*!
* \brief Default destructor. * \brief Default destructor.
*/ */
~pcps_cccwsr_acquisition_cc(); ~pcps_cccwsr_acquisition_cc();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
/*! /*!
* \brief Returns the maximum peak of grid search. * \brief Returns the maximum peak of grid search.
*/ */
unsigned int mag() unsigned int mag()
{ {
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for CCCWSR acquisition algorithm. * \brief Sets local code for CCCWSR acquisition algorithm.
* \param data_code - Pointer to the data PRN code. * \param data_code - Pointer to the data PRN code.
* \param pilot_code - Pointer to the pilot PRN code. * \param pilot_code - Pointer to the pilot PRN code.
*/ */
void set_local_code(std::complex<float> * code_data, std::complex<float> * code_pilot); void set_local_code(std::complex<float> * code_data, std::complex<float> * code_pilot);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
void set_active(bool active) void set_active(bool active)
{ {
d_active = active; d_active = active;
} }
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
void set_channel(unsigned int channel) void set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of CCCWSR algorithm. * \brief Set statistics threshold of CCCWSR algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
void set_threshold(float threshold) void set_threshold(float threshold)
{ {
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
void set_doppler_max(unsigned int doppler_max) void set_doppler_max(unsigned int doppler_max)
{ {
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
void set_doppler_step(unsigned int doppler_step) void set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*!
* \brief Set tracking channel internal queue.
* \param channel_internal_queue - Channel's internal blocks information queue.
*/
void set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{
d_channel_internal_queue = channel_internal_queue;
}
/*! /*!
* \brief Set tracking channel internal queue. * \brief Coherent Channel Combining With Sign Recovery Acquisition signal processing.
* \param channel_internal_queue - Channel's internal blocks information queue. */
*/ int general_work(int noutput_items, gr_vector_int &ninput_items,
void set_channel_queue(concurrent_queue<int> *channel_internal_queue) gr_vector_const_void_star &input_items,
{ gr_vector_void_star &output_items);
d_channel_internal_queue = channel_internal_queue;
}
/*!
* \brief Coherent Channel Combining With Sign Recovery Acquisition signal processing.
*/
int general_work(int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items);
}; };
#endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_*/ #endif /* GNSS_SDR_PCPS_CCCWSR_ACQUISITION_CC_H_*/

26
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc
View File

@@ -146,6 +146,8 @@ pcps_opencl_acquisition_cc::pcps_opencl_acquisition_cc(
} }
pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc() pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
{ {
if (d_num_doppler_bins > 0) if (d_num_doppler_bins > 0)
@@ -194,6 +196,8 @@ pcps_opencl_acquisition_cc::~pcps_opencl_acquisition_cc()
} }
} }
int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filename) int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filename)
{ {
//get all platforms (drivers) //get all platforms (drivers)
@@ -252,11 +256,11 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filen
d_cl_program = program; d_cl_program = program;
// create buffers on the device // create buffers on the device
d_cl_buffer_in = new cl::Buffer(d_cl_context,CL_MEM_READ_WRITE,sizeof(gr_complex)*d_fft_size); d_cl_buffer_in = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size);
d_cl_buffer_fft_codes = new cl::Buffer(d_cl_context,CL_MEM_READ_WRITE,sizeof(gr_complex)*d_fft_size_pow2); d_cl_buffer_fft_codes = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size_pow2);
d_cl_buffer_1 = new cl::Buffer(d_cl_context,CL_MEM_READ_WRITE,sizeof(gr_complex)*d_fft_size_pow2); d_cl_buffer_1 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size_pow2);
d_cl_buffer_2 = new cl::Buffer(d_cl_context,CL_MEM_READ_WRITE,sizeof(gr_complex)*d_fft_size_pow2); d_cl_buffer_2 = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(gr_complex)*d_fft_size_pow2);
d_cl_buffer_magnitude = new cl::Buffer(d_cl_context,CL_MEM_READ_WRITE,sizeof(float)*d_fft_size); d_cl_buffer_magnitude = new cl::Buffer(d_cl_context, CL_MEM_READ_WRITE, sizeof(float)*d_fft_size);
//create queue to which we will push commands for the device. //create queue to which we will push commands for the device.
d_cl_queue = new cl::CommandQueue(d_cl_context,d_cl_device); d_cl_queue = new cl::CommandQueue(d_cl_context,d_cl_device);
@@ -284,6 +288,8 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filen
return 0; return 0;
} }
void pcps_opencl_acquisition_cc::init() void pcps_opencl_acquisition_cc::init()
{ {
d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_delay_samples = 0.0;
@@ -669,13 +675,13 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
d_core_working = false; d_core_working = false;
} }
int pcps_opencl_acquisition_cc::general_work(int noutput_items, int pcps_opencl_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items) gr_vector_void_star &output_items)
{ {
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state) switch (d_state)
{ {
case 0: case 0:
@@ -708,7 +714,7 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
// Fill internal buffer with d_max_dwells signal blocks. This step ensures that // Fill internal buffer with d_max_dwells signal blocks. This step ensures that
// consecutive signal blocks will be processed in multi-dwell operation. This is // consecutive signal blocks will be processed in multi-dwell operation. This is
// essential when d_bit_transition_flag = true. // essential when d_bit_transition_flag = true.
unsigned int num_dwells = std::min((int)(d_max_dwells-d_in_dwell_count),ninput_items[0]); unsigned int num_dwells = std::min((int)(d_max_dwells-d_in_dwell_count), ninput_items[0]);
for (unsigned int i = 0; i < num_dwells; i++) for (unsigned int i = 0; i < num_dwells; i++)
{ {
memcpy(d_in_buffer[d_in_dwell_count++], (gr_complex*)input_items[i], memcpy(d_in_buffer[d_in_dwell_count++], (gr_complex*)input_items[i],
@@ -719,7 +725,7 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
if (ninput_items[0] > (int)num_dwells) if (ninput_items[0] > (int)num_dwells)
{ {
d_sample_counter += d_fft_size * (ninput_items[0]-num_dwells); d_sample_counter += d_fft_size * (ninput_items[0] - num_dwells);
} }
} }
else else
@@ -738,7 +744,7 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
// moment by the external thread (may have changed since checked in the switch()). // moment by the external thread (may have changed since checked in the switch()).
// If the external thread has already declared positive (d_state=2) or negative // If the external thread has already declared positive (d_state=2) or negative
// (d_state=3) acquisition, we don't have to process next block!! // (d_state=3) acquisition, we don't have to process next block!!
if ((d_well_count < d_in_dwell_count) && !d_core_working && d_state==1) if ((d_well_count < d_in_dwell_count) && !d_core_working && d_state == 1)
{ {
d_core_working = true; d_core_working = true;
if (d_opencl == 0) if (d_opencl == 0)

237
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h
View File

@@ -92,63 +92,62 @@ class pcps_opencl_acquisition_cc: public gr::block
private: private:
friend pcps_opencl_acquisition_cc_sptr friend pcps_opencl_acquisition_cc_sptr
pcps_make_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_make_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump, gr::msg_queue::sptr queue, bool dump,
std::string dump_filename); std::string dump_filename);
pcps_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells, pcps_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in, unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code, int samples_per_ms, int samples_per_code,
bool bit_transition_flag, bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump, gr::msg_queue::sptr queue, bool dump,
std::string dump_filename); std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
int init_opencl_environment(std::string kernel_filename); int init_opencl_environment(std::string kernel_filename);
long d_fs_in; long d_fs_in;
long d_freq; long d_freq;
int d_samples_per_ms; int d_samples_per_ms;
int d_samples_per_code; int d_samples_per_code;
unsigned int d_doppler_resolution; unsigned int d_doppler_resolution;
float d_threshold; float d_threshold;
std::string d_satellite_str; std::string d_satellite_str;
unsigned int d_doppler_max; unsigned int d_doppler_max;
unsigned int d_doppler_step; unsigned int d_doppler_step;
unsigned int d_sampled_ms; unsigned int d_sampled_ms;
unsigned int d_max_dwells; unsigned int d_max_dwells;
unsigned int d_well_count; unsigned int d_well_count;
unsigned int d_fft_size; unsigned int d_fft_size;
unsigned int d_fft_size_pow2; unsigned int d_fft_size_pow2;
int* d_max_doppler_indexs; int* d_max_doppler_indexs;
unsigned long int d_sample_counter; unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs; gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins; unsigned int d_num_doppler_bins;
gr_complex* d_fft_codes; gr_complex* d_fft_codes;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
float* d_magnitude; float* d_magnitude;
float d_input_power; float d_input_power;
float d_test_statistics; float d_test_statistics;
bool d_bit_transition_flag; bool d_bit_transition_flag;
gr::msg_queue::sptr d_queue; gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue; concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file; std::ofstream d_dump_file;
bool d_active; bool d_active;
int d_state; int d_state;
bool d_core_working; bool d_core_working;
bool d_dump; bool d_dump;
unsigned int d_channel; unsigned int d_channel;
std::string d_dump_filename; std::string d_dump_filename;
gr_complex* d_zero_vector; gr_complex* d_zero_vector;
gr_complex** d_in_buffer; gr_complex** d_in_buffer;
std::vector<unsigned long int> d_sample_counter_buffer; std::vector<unsigned long int> d_sample_counter_buffer;
@@ -174,104 +173,104 @@ public:
/*! /*!
* \brief Default destructor. * \brief Default destructor.
*/ */
~pcps_opencl_acquisition_cc(); ~pcps_opencl_acquisition_cc();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
/*! /*!
* \brief Returns the maximum peak of grid search. * \brief Returns the maximum peak of grid search.
*/ */
unsigned int mag() unsigned int mag()
{ {
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for PCPS acquisition algorithm. * \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float> * code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
void set_active(bool active) void set_active(bool active)
{ {
d_active = active; d_active = active;
} }
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
void set_channel(unsigned int channel) void set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of PCPS algorithm. * \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
void set_threshold(float threshold) void set_threshold(float threshold)
{ {
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
void set_doppler_max(unsigned int doppler_max) void set_doppler_max(unsigned int doppler_max)
{ {
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
void set_doppler_step(unsigned int doppler_step) void set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*! /*!
* \brief Set tracking channel internal queue. * \brief Set tracking channel internal queue.
* \param channel_internal_queue - Channel's internal blocks information queue. * \param channel_internal_queue - Channel's internal blocks information queue.
*/ */
void set_channel_queue(concurrent_queue<int> *channel_internal_queue) void set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{ {
d_channel_internal_queue = channel_internal_queue; d_channel_internal_queue = channel_internal_queue;
} }
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items); gr_vector_void_star &output_items);
void acquisition_core_volk(); void acquisition_core_volk();
void acquisition_core_opencl(); void acquisition_core_opencl();
}; };
#endif /* GNSS_SDR_pcps_opencl_acquisition_cc_H_*/ #endif

228
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h
View File

@@ -82,158 +82,156 @@ class pcps_tong_acquisition_cc: public gr::block
private: private:
friend pcps_tong_acquisition_cc_sptr friend pcps_tong_acquisition_cc_sptr
pcps_tong_make_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max, pcps_tong_make_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms, long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val, int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, gr::msg_queue::sptr queue, unsigned int tong_max_val, gr::msg_queue::sptr queue,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
pcps_tong_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max, pcps_tong_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms, long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val, int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, gr::msg_queue::sptr queue, unsigned int tong_max_val, gr::msg_queue::sptr queue,
bool dump, std::string dump_filename); bool dump, std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift, void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset); int doppler_offset);
long d_fs_in;
long d_fs_in; long d_freq;
long d_freq; int d_samples_per_ms;
int d_samples_per_ms;
int d_samples_per_code; int d_samples_per_code;
unsigned int d_doppler_resolution; unsigned int d_doppler_resolution;
float d_threshold; float d_threshold;
std::string d_satellite_str; std::string d_satellite_str;
unsigned int d_doppler_max; unsigned int d_doppler_max;
unsigned int d_doppler_step; unsigned int d_doppler_step;
unsigned int d_sampled_ms; unsigned int d_sampled_ms;
unsigned int d_well_count; unsigned int d_well_count;
unsigned int d_tong_count; unsigned int d_tong_count;
unsigned int d_tong_init_val; unsigned int d_tong_init_val;
unsigned int d_tong_max_val; unsigned int d_tong_max_val;
unsigned int d_fft_size; unsigned int d_fft_size;
unsigned long int d_sample_counter; unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs; gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins; unsigned int d_num_doppler_bins;
gr_complex* d_fft_codes; gr_complex* d_fft_codes;
float** d_grid_data; float** d_grid_data;
gr::fft::fft_complex* d_fft_if; gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft; gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro; Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase; unsigned int d_code_phase;
float d_doppler_freq; float d_doppler_freq;
float d_mag; float d_mag;
float* d_magnitude; float* d_magnitude;
float d_input_power; float d_input_power;
float d_test_statistics; float d_test_statistics;
gr::msg_queue::sptr d_queue; gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue; concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file; std::ofstream d_dump_file;
bool d_active; bool d_active;
int d_state; int d_state;
bool d_dump; bool d_dump;
unsigned int d_channel; unsigned int d_channel;
std::string d_dump_filename; std::string d_dump_filename;
public: public:
/*! /*!
* \brief Default destructor. * \brief Default destructor.
*/ */
~pcps_tong_acquisition_cc(); ~pcps_tong_acquisition_cc();
/*! /*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer * \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks. * to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks. * \param p_gnss_synchro Satellite information shared by the processing blocks.
*/ */
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_gnss_synchro = p_gnss_synchro; d_gnss_synchro = p_gnss_synchro;
} }
/*! /*!
* \brief Returns the maximum peak of grid search. * \brief Returns the maximum peak of grid search.
*/ */
unsigned int mag() unsigned int mag()
{ {
return d_mag; return d_mag;
} }
/*! /*!
* \brief Initializes acquisition algorithm. * \brief Initializes acquisition algorithm.
*/ */
void init(); void init();
/*! /*!
* \brief Sets local code for TONG acquisition algorithm. * \brief Sets local code for TONG acquisition algorithm.
* \param code - Pointer to the PRN code. * \param code - Pointer to the PRN code.
*/ */
void set_local_code(std::complex<float> * code); void set_local_code(std::complex<float> * code);
/*! /*!
* \brief Starts acquisition algorithm, turning from standby mode to * \brief Starts acquisition algorithm, turning from standby mode to
* active mode * active mode
* \param active - bool that activates/deactivates the block. * \param active - bool that activates/deactivates the block.
*/ */
void set_active(bool active) void set_active(bool active)
{ {
d_active = active; d_active = active;
} }
/*! /*!
* \brief Set acquisition channel unique ID * \brief Set acquisition channel unique ID
* \param channel - receiver channel. * \param channel - receiver channel.
*/ */
void set_channel(unsigned int channel) void set_channel(unsigned int channel)
{ {
d_channel = channel; d_channel = channel;
} }
/*! /*!
* \brief Set statistics threshold of TONG algorithm. * \brief Set statistics threshold of TONG algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012, * \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold). * Algorithm 1, for a definition of this threshold).
*/ */
void set_threshold(float threshold) void set_threshold(float threshold)
{ {
d_threshold = threshold; d_threshold = threshold;
} }
/*! /*!
* \brief Set maximum Doppler grid search * \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz]. * \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/ */
void set_doppler_max(unsigned int doppler_max) void set_doppler_max(unsigned int doppler_max)
{ {
d_doppler_max = doppler_max; d_doppler_max = doppler_max;
} }
/*! /*!
* \brief Set Doppler steps for the grid search * \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz]. * \param doppler_step - Frequency bin of the search grid [Hz].
*/ */
void set_doppler_step(unsigned int doppler_step) void set_doppler_step(unsigned int doppler_step)
{ {
d_doppler_step = doppler_step; d_doppler_step = doppler_step;
} }
/*! /*!
* \brief Set tracking channel internal queue. * \brief Set tracking channel internal queue.
* \param channel_internal_queue - Channel's internal blocks information queue. * \param channel_internal_queue - Channel's internal blocks information queue.
*/ */
void set_channel_queue(concurrent_queue<int> *channel_internal_queue) void set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{ {
d_channel_internal_queue = channel_internal_queue; d_channel_internal_queue = channel_internal_queue;
} }
/*! /*!
* \brief Parallel Code Phase Search Acquisition signal processing. * \brief Parallel Code Phase Search Acquisition signal processing.
*/ */
int general_work(int noutput_items, gr_vector_int &ninput_items, int general_work(int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items); gr_vector_void_star &output_items);
}; };
#endif /* GNSS_SDR_PCPS_TONG_acquisition_cc_H_ */ #endif /* GNSS_SDR_PCPS_TONG_acquisition_cc_H_ */

3
src/algorithms/observables/adapters/galileo_e1_observables.h
View File

@@ -41,7 +41,7 @@
class ConfigurationInterface; class ConfigurationInterface;
/*! /*!
* \brief This class implements an ObservablesInterface for Galileo E1 * \brief This class implements an ObservablesInterface for Galileo E1B
*/ */
class GalileoE1Observables : public ObservablesInterface class GalileoE1Observables : public ObservablesInterface
{ {
@@ -57,6 +57,7 @@ public:
return role_; return role_;
} }
//! Returns "Galileo_E1B_Observables"
std::string implementation() std::string implementation()
{ {
return "Galileo_E1B_Observables"; return "Galileo_E1B_Observables";

81
src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc
View File

@@ -1,6 +1,6 @@
/*! /*!
* \file gps_l1_ca_observables_cc.cc * \file galileo_e1_observables_cc.cc
* \brief Implementation of the pseudorange computation block for GPS L1 C/A * \brief Implementation of the pseudorange computation block for Galileo E1
* \author Mara Branzanti 2013. mara.branzanti(at)gmail.com * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com
* \author Javier Arribas 2013. jarribas(at)cttc.es * \author Javier Arribas 2013. jarribas(at)cttc.es
* *
@@ -92,12 +92,15 @@ galileo_e1_observables_cc::~galileo_e1_observables_cc()
d_dump_file.close(); d_dump_file.close();
} }
bool Galileo_pairCompare_gnss_synchro_Prn_delay_ms( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b) bool Galileo_pairCompare_gnss_synchro_Prn_delay_ms( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{ {
return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms); return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms);
} }
bool Galileo_pairCompare_gnss_synchro_d_TOW_at_current_symbol( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b) bool Galileo_pairCompare_gnss_synchro_d_TOW_at_current_symbol( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b)
{ {
return (a.second.d_TOW_at_current_symbol) < (b.second.d_TOW_at_current_symbol); return (a.second.d_TOW_at_current_symbol) < (b.second.d_TOW_at_current_symbol);
@@ -108,8 +111,8 @@ bool Galileo_pairCompare_gnss_synchro_d_TOW_at_current_symbol( std::pair<int,Gns
int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //Get the output pointer Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // Get the output pointer
Gnss_Synchro current_gnss_synchro[d_nchannels]; Gnss_Synchro current_gnss_synchro[d_nchannels];
std::map<int,Gnss_Synchro> current_gnss_synchro_map; std::map<int,Gnss_Synchro> current_gnss_synchro_map;
@@ -118,22 +121,23 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
/* /*
* 1. Read the GNSS SYNCHRO objects from available channels * 1. Read the GNSS SYNCHRO objects from available channels
*/ */
for (unsigned int i=0; i<d_nchannels ; i++) //legge gli input per ogni canale e prepare i dati da riempire for (unsigned int i = 0; i < d_nchannels; i++)
{ {
//Copy the telemetry decoder data to local copy //Copy the telemetry decoder data to local copy
current_gnss_synchro[i] = in[i][0]; current_gnss_synchro[i] = in[i][0];
/* /*
* 1.2 Assume no valid pseudoranges * 1.2 Assume no valid pseudoranges
*/ */
current_gnss_synchro[i].Flag_valid_pseudorange = false; current_gnss_synchro[i].Flag_valid_pseudorange = false;
current_gnss_synchro[i].Pseudorange_m = 0.0; current_gnss_synchro[i].Pseudorange_m = 0.0;
if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word, ciò viene definito nel telemetry decoder if (current_gnss_synchro[i].Flag_valid_word)
{ {
//record the word structure in a map for pseudorange computation //record the word structure in a map for pseudorange computation
current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
} }
} }
/*
/*
* 2. Compute RAW pseudoranges using COMMON RECEPTION TIME algorithm. Use only the valid channels (channels that are tracking a satellite) * 2. Compute RAW pseudoranges using COMMON RECEPTION TIME algorithm. Use only the valid channels (channels that are tracking a satellite)
*/ */
if(current_gnss_synchro_map.size() > 0) if(current_gnss_synchro_map.size() > 0)
@@ -142,45 +146,38 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
* 2.1 Use CURRENT set of measurements and find the nearest satellite * 2.1 Use CURRENT set of measurements and find the nearest satellite
* common RX time algorithm * common RX time algorithm
*/ */
//;
// what is the most recent symbol TOW in the current set? -> this will be the reference symbol // what is the most recent symbol TOW in the current set? -> this will be the reference symbol
gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), Galileo_pairCompare_gnss_synchro_d_TOW_at_current_symbol); gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), Galileo_pairCompare_gnss_synchro_d_TOW_at_current_symbol);
double d_TOW_reference = gnss_synchro_iter->second.d_TOW_at_current_symbol; double d_TOW_reference = gnss_synchro_iter->second.d_TOW_at_current_symbol;
double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms; double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms;
//int reference_channel= gnss_synchro_iter->second.Channel_ID; //int reference_channel= gnss_synchro_iter->second.Channel_ID;
// Now compute RX time differences due to the PRN alignement in the correlators
double traveltime_ms;
double pseudorange_m;
double delta_rx_time_ms;
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
{
// compute the required symbol history shift in order to match the reference symbol
delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms;
//std::cout<<"delta_rx_time_ms["<<gnss_synchro_iter->second.Channel_ID<<"]="<<delta_rx_time_ms<<std::endl;
//std::cout<<"d_TOW_at_current_symbol["<<gnss_synchro_iter->second.Channel_ID<<"]="<<gnss_synchro_iter->second.d_TOW_at_current_symbol<<std::endl;
//compute the pseudorange
traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms;
//std::cout<<"traveltime_ms="<<traveltime_ms<<std::endl;
pseudorange_m = traveltime_ms * GALILEO_C_m_ms; // [m]
//std::cout<<"pseudorange_m["<<gnss_synchro_iter->second.Channel_ID<<"]="<<pseudorange_m<<std::endl;
// 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].Flag_valid_pseudorange = true;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GALILEO_STARTOFFSET_ms/1000.0;
}
// Now compute RX time differences due to the PRN alignment in the correlators
double traveltime_ms;
double pseudorange_m;
double delta_rx_time_ms;
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
{
// compute the required symbol history shift in order to match the reference symbol
delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms;
//compute the pseudorange
traveltime_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms;
pseudorange_m = traveltime_ms * GALILEO_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].Flag_valid_pseudorange = true;
current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference*1000)/1000 + GALILEO_STARTOFFSET_ms/1000.0;
}
} }
if(d_dump == true) if(d_dump == true)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try try
{ {
double tmp_double; double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i = 0; i < d_nchannels ; i++)
{ {
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
@@ -201,7 +198,7 @@ int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &n
} }
consume_each(1); //one by one consume_each(1); //one by one
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i = 0; i < d_nchannels ; i++)
{ {
*out[i] = current_gnss_synchro[i]; *out[i] = current_gnss_synchro[i];
} }

2
src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h
View File

@@ -40,11 +40,9 @@
#include <boost/thread/mutex.hpp> #include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp> #include <boost/thread/thread.hpp>
#include "concurrent_queue.h" #include "concurrent_queue.h"
#include "galileo_navigation_message.h" #include "galileo_navigation_message.h"
#include "rinex_printer.h" #include "rinex_printer.h"
#include "Galileo_E1.h" #include "Galileo_E1.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
class galileo_e1_observables_cc; class galileo_e1_observables_cc;

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

@@ -90,6 +90,7 @@ gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc()
d_dump_file.close(); d_dump_file.close();
} }
bool pairCompare_gnss_synchro_Prn_delay_ms( 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.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms); return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms);
@@ -105,8 +106,8 @@ bool pairCompare_gnss_synchro_d_TOW_at_current_symbol( std::pair<int,Gnss_Synchr
int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //Get the output pointer Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // Get the output pointer
Gnss_Synchro current_gnss_synchro[d_nchannels]; Gnss_Synchro current_gnss_synchro[d_nchannels];
std::map<int,Gnss_Synchro> current_gnss_synchro_map; std::map<int,Gnss_Synchro> current_gnss_synchro_map;
@@ -116,7 +117,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
/* /*
* 1. Read the GNSS SYNCHRO objects from available channels * 1. Read the GNSS SYNCHRO objects from available channels
*/ */
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i = 0; i < d_nchannels; i++)
{ {
//Copy the telemetry decoder data to local copy //Copy the telemetry decoder data to local copy
current_gnss_synchro[i] = in[i][0]; current_gnss_synchro[i] = in[i][0];
@@ -131,6 +132,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); current_gnss_synchro_map.insert(std::pair<int, Gnss_Synchro>(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i]));
} }
} }
/* /*
* 2. Compute RAW pseudoranges using COMMON RECEPTION TIME algorithm. Use only the valid channels (channels that are tracking a satellite) * 2. Compute RAW pseudoranges using COMMON RECEPTION TIME algorithm. Use only the valid channels (channels that are tracking a satellite)
*/ */
@@ -140,22 +142,20 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
* 2.1 Use CURRENT set of measurements and find the nearest satellite * 2.1 Use CURRENT set of measurements and find the nearest satellite
* common RX time algorithm * common RX time algorithm
*/ */
//;
// what is the most recent symbol TOW in the current set? -> this will be the reference symbol // what is the most recent symbol TOW in the current set? -> this will be the reference symbol
gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_d_TOW_at_current_symbol); gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_d_TOW_at_current_symbol);
double d_TOW_reference = gnss_synchro_iter->second.d_TOW_at_current_symbol; double d_TOW_reference = gnss_synchro_iter->second.d_TOW_at_current_symbol;
double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms; double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms;
//int reference_channel= gnss_synchro_iter->second.Channel_ID; //int reference_channel= gnss_synchro_iter->second.Channel_ID;
// Now compute RX time differences due to the PRN alignement in the correlators // Now compute RX time differences due to the PRN alignment in the correlators
double traveltime_ms; double traveltime_ms;
double pseudorange_m; double pseudorange_m;
double delta_rx_time_ms; double delta_rx_time_ms;
for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++) for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++)
{ {
// compute the required symbol history shift in order to match the reference symbol // compute the required symbol history shift in order to match the reference symbol
delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms-d_ref_PRN_rx_time_ms; delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms;
//std::cout<<"delta_rx_time_ms="<<delta_rx_time_ms<<std::endl;
//compute the pseudorange //compute the pseudorange
traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms; traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms;
pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m]
@@ -167,14 +167,13 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
} }
} }
if(d_dump == true) if(d_dump == true)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try try
{ {
double tmp_double; double tmp_double;
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i = 0; i < d_nchannels; i++)
{ {
tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol;
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
@@ -195,10 +194,10 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni
} }
consume_each(1); //one by one consume_each(1); //one by one
for (unsigned int i=0; i<d_nchannels ; i++) for (unsigned int i = 0; i < d_nchannels; i++)
{ {
*out[i] = current_gnss_synchro[i]; *out[i] = current_gnss_synchro[i];
} }
return 1; //Output the observables return 1; // Output the observables
} }

13
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
View File

@@ -62,7 +62,7 @@ galileo_e1b_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, lo
void galileo_e1b_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required) void galileo_e1b_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = GALILEO_INAV_PAGE_SYMBOLS; //set the required sample history ninput_items_required[0] = GALILEO_INAV_PAGE_SYMBOLS; // set the required sample history
} }
@@ -73,13 +73,13 @@ void galileo_e1b_telemetry_decoder_cc::viterbi_decoder(double *page_part_symbols
int nn, KK, mm, max_states; int nn, KK, mm, max_states;
int g_encoder[2]; int g_encoder[2];
nn = 2; //Coding rate 1/n nn = 2; // Coding rate 1/n
KK = 7; //Constraint Length KK = 7; // Constraint Length
g_encoder[0] = 121; // Polynomial G1 g_encoder[0] = 121; // Polynomial G1
g_encoder[1] = 91; // Polinomial G2 g_encoder[1] = 91; // Polynomial G2
mm = KK - 1; mm = KK - 1;
max_states = 1 << mm; /* 2^mm */ max_states = 1 << mm; /* 2^mm */
DataLength = (CodeLength/nn) - mm; DataLength = (CodeLength/nn) - mm;
/* create appropriate transition matrices */ /* create appropriate transition matrices */
@@ -92,10 +92,9 @@ void galileo_e1b_telemetry_decoder_cc::viterbi_decoder(double *page_part_symbols
nsc_transit( out0, state0, 0, g_encoder, KK, nn ); nsc_transit( out0, state0, 0, g_encoder, KK, nn );
nsc_transit( out1, state1, 1, g_encoder, KK, nn ); nsc_transit( out1, state1, 1, g_encoder, KK, nn );
Viterbi( page_part_bits, out0, state0, out1, state1, Viterbi(page_part_bits, out0, state0, out1, state1,
page_part_symbols, KK, nn, DataLength ); page_part_symbols, KK, nn, DataLength );
/* Clean up memory */ /* Clean up memory */
free( out0 ); free( out0 );
free( out1 ); free( out1 );

13
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h
View File

@@ -1,12 +1,12 @@
/*! /*!
* \file galileo_e1b_telemetry_decoder_cc.h * \file galileo_e1b_telemetry_decoder_cc.h
* \brief Interface of a Galileo NAV message demodulator block * \brief Interface of a Galileo INAV message demodulator block
* \author Javier Arribas 2013 jarribas(at)cttc.es, * \author Javier Arribas 2013 jarribas(at)cttc.es,
* Mara Branzanti 2013 mara.branzanti(at)gmail.com * Mara Branzanti 2013 mara.branzanti(at)gmail.com
* *
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2010-2011 (see AUTHORS file for a list of contributors) * Copyright (C) 2010-2013 (see AUTHORS file for a list of contributors)
* *
* GNSS-SDR is a software defined Global Navigation * GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver * Satellite Systems receiver
@@ -42,7 +42,6 @@
#include "gnuradio/trellis/interleaver.h" #include "gnuradio/trellis/interleaver.h"
#include "gnuradio/trellis/permutation.h" #include "gnuradio/trellis/permutation.h"
#include "gnuradio/fec/viterbi.h" #include "gnuradio/fec/viterbi.h"
#include "gnss_satellite.h" #include "gnss_satellite.h"
#include "galileo_navigation_message.h" #include "galileo_navigation_message.h"
#include "galileo_ephemeris.h" #include "galileo_ephemeris.h"
@@ -50,19 +49,13 @@
#include "galileo_iono.h" #include "galileo_iono.h"
#include "galileo_utc_model.h" #include "galileo_utc_model.h"
// Galileo Navigation Message structures
#include "galileo_ephemeris.h"
#include "galileo_iono.h"
#include "galileo_almanac.h"
#include "galileo_utc_model.h"
class galileo_e1b_telemetry_decoder_cc; class galileo_e1b_telemetry_decoder_cc;
typedef boost::shared_ptr<galileo_e1b_telemetry_decoder_cc> galileo_e1b_telemetry_decoder_cc_sptr; typedef boost::shared_ptr<galileo_e1b_telemetry_decoder_cc> galileo_e1b_telemetry_decoder_cc_sptr;
galileo_e1b_telemetry_decoder_cc_sptr galileo_e1b_telemetry_decoder_cc_sptr galileo_e1b_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in, unsigned
galileo_e1b_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in, unsigned
int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump); int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump);
/*! /*!

2
src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc
View File

@@ -149,7 +149,7 @@ int sbas_l1_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_int
it != valid_msgs.end(); ++it) it != valid_msgs.end(); ++it)
{ {
int message_sample_offset = int message_sample_offset =
(sample_alignment?0:-1) (sample_alignment ? 0 : -1)
+ d_samples_per_symbol*(symbol_alignment ? -1 : 0) + d_samples_per_symbol*(symbol_alignment ? -1 : 0)
+ d_samples_per_symbol * d_symbols_per_bit * it->first; + d_samples_per_symbol * d_symbols_per_bit * it->first;
double message_sample_stamp = sample_stamp + ((double)message_sample_offset)/1000; double message_sample_stamp = sample_stamp + ((double)message_sample_offset)/1000;

6
src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h
View File

@@ -114,20 +114,19 @@ private:
void reset(); void reset();
/* /*
* samples length must be a multiple of two * samples length must be a multiple of two
* for block operation the * for block operation
*/ */
bool get_symbols(const std::vector<double> samples, std::vector<double> &symbols); bool get_symbols(const std::vector<double> samples, std::vector<double> &symbols);
private: private:
int d_n_smpls_in_history ; int d_n_smpls_in_history ;
double d_iir_par; double d_iir_par;
double d_corr_paired; double d_corr_paired;
double d_corr_shifted; double d_corr_shifted;
bool d_aligned; bool d_aligned;
double d_past_sample; double d_past_sample;
} d_sample_aligner; } d_sample_aligner;
// helper class for symbol alignment and viterbi decoding // helper class for symbol alignment and Viterbi decoding
class symbol_aligner_and_decoder class symbol_aligner_and_decoder
{ {
public: public:
@@ -140,7 +139,6 @@ private:
Viterbi_Decoder * d_vd1; Viterbi_Decoder * d_vd1;
Viterbi_Decoder * d_vd2; Viterbi_Decoder * d_vd2;
double d_past_symbol; double d_past_symbol;
} d_symbol_aligner_and_decoder; } d_symbol_aligner_and_decoder;

15
src/algorithms/telemetry_decoder/libs/viterbi_decoder.cc
View File

@@ -31,7 +31,6 @@
#include "viterbi_decoder.h" #include "viterbi_decoder.h"
#include <iostream> #include <iostream>
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
@@ -47,8 +46,8 @@
Viterbi_Decoder::Viterbi_Decoder(const int g_encoder[], const int KK, const int nn) Viterbi_Decoder::Viterbi_Decoder(const int g_encoder[], const int KK, const int nn)
{ {
d_nn = nn; //Coding rate 1/n d_nn = nn; // Coding rate 1/n
d_KK = KK; //Constraint Length d_KK = KK; // Constraint Length
// derived code properties // derived code properties
d_mm = d_KK - 1; d_mm = d_KK - 1;
@@ -97,7 +96,7 @@ void Viterbi_Decoder::reset()
Description: Uses the Viterbi algorithm to perform hard-decision decoding of a convolutional code. Description: Uses the Viterbi algorithm to perform hard-decision decoding of a convolutional code.
Input parameters: Input parameters:
r[] The received signal in LLR-form. For BPSK, must be in form r = 2*a*y/(sigma^2). r[] The received signal in LLR-form. For BPSK, must be in form r = 2*a*y/(sigma^2).
LL The number of data bits to be decoded (doen't inlcude the mm zero-tail-bits) LL The number of data bits to be decoded (doesn't include the mm zero-tail-bits)
Output parameters: Output parameters:
output_u_int[] Hard decisions on the data bits (without the mm zero-tail-bits) output_u_int[] Hard decisions on the data bits (without the mm zero-tail-bits)
*/ */
@@ -141,12 +140,10 @@ float Viterbi_Decoder::decode_continuous(const double sym[],
// since it depends on the future values -> traceback, but don't decode // since it depends on the future values -> traceback, but don't decode
state = do_traceback(traceback_depth); state = do_traceback(traceback_depth);
// traceback and decode // traceback and decode
decoding_length_mismatch = do_tb_and_decode(traceback_depth, nbits_requested, state, bits, decoding_length_mismatch = do_tb_and_decode(traceback_depth, nbits_requested, state, bits, d_indicator_metric);
d_indicator_metric);
nbits_decoded = nbits_requested + decoding_length_mismatch; nbits_decoded = nbits_requested + decoding_length_mismatch;
VLOG(FLOW) << "decoding length mismatch (continuous decoding): " VLOG(FLOW) << "decoding length mismatch (continuous decoding): " << decoding_length_mismatch;
<< decoding_length_mismatch;
return d_indicator_metric; return d_indicator_metric;
} }
@@ -332,8 +329,6 @@ int Viterbi_Decoder::do_tb_and_decode(int traceback_length, int requested_decodi
indicator_metric = 0; indicator_metric = 0;
for (it = d_trellis_paths.begin() + traceback_length + overstep_length; it < d_trellis_paths.end(); ++it) for (it = d_trellis_paths.begin() + traceback_length + overstep_length; it < d_trellis_paths.end(); ++it)
{ {
//VLOG(SAMPLE)<< "@t_out=" << t_out;
//VLOG(SAMPLE) << "tb&dec: @t=" << it->get_t() << " bit=" << it->get_bit_of_current_state(state) << " bm=" << it->get_metric_of_current_state(state);
if(it - (d_trellis_paths.begin() + traceback_length + overstep_length) < n_of_branches_for_indicator_metric) if(it - (d_trellis_paths.begin() + traceback_length + overstep_length) < n_of_branches_for_indicator_metric)
{ {
n_im++; n_im++;

7
src/algorithms/telemetry_decoder/libs/viterbi_decoder.h
View File

@@ -35,6 +35,9 @@
#include <deque> #include <deque>
#include <iostream> #include <iostream>
/*!
* \brief Class that implements a Viterbi decoder
*/
class Viterbi_Decoder class Viterbi_Decoder
{ {
public: public:
@@ -45,8 +48,8 @@ public:
/*! /*!
* \brief Uses the Viterbi algorithm to perform hard-decision decoding of a convolutional code. * \brief Uses the Viterbi algorithm to perform hard-decision decoding of a convolutional code.
* *
* \param input_c[] The received signal in LLR-form. For BPSK, must be in form r = 2*a*y/(sigma^2). * \param[in] input_c[] The received signal in LLR-form. For BPSK, must be in form r = 2*a*y/(sigma^2).
* \param LL The number of data bits to be decoded (does not include the mm zero-tail-bits) * \param[in] LL The number of data bits to be decoded (does not include the mm zero-tail-bits)
* *
* \return output_u_int[] Hard decisions on the data bits (without the mm zero-tail-bits) * \return output_u_int[] Hard decisions on the data bits (without the mm zero-tail-bits)
*/ */

12
src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h
View File

@@ -44,12 +44,12 @@
class ConfigurationInterface; class ConfigurationInterface;
/*!
* \brief This class implements a code DLL + carrier PLL/FLL Assisted tracking loop
*/
class GpsL1CaDllFllPllTracking : public TrackingInterface class GpsL1CaDllFllPllTracking : public TrackingInterface
{ {
public: public:
GpsL1CaDllFllPllTracking(ConfigurationInterface* configuration, GpsL1CaDllFllPllTracking(ConfigurationInterface* configuration,
std::string role, std::string role,
unsigned int in_streams, unsigned int in_streams,
@@ -80,19 +80,13 @@ public:
void set_channel(unsigned int channel); void set_channel(unsigned int channel);
void set_channel_queue(concurrent_queue<int> *channel_internal_queue); void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro); void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
void start_tracking(); void start_tracking();
private: private:
gps_l1_ca_dll_fll_pll_tracking_cc_sptr tracking_; gps_l1_ca_dll_fll_pll_tracking_cc_sptr tracking_;
size_t item_size_; size_t item_size_;
unsigned int channel_; unsigned int channel_;
std::string role_; std::string role_;
unsigned int in_streams_; unsigned int in_streams_;
unsigned int out_streams_; unsigned int out_streams_;

85
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc
View File

@@ -45,7 +45,6 @@
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <cmath> #include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
@@ -78,12 +77,14 @@ galileo_e1_dll_pll_veml_make_tracking_cc(
fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips, very_early_late_space_chips)); fs_in, vector_length, queue, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips, very_early_late_space_chips));
} }
void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items, void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call
} }
galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc(
long if_freq, long if_freq,
long fs_in, long fs_in,
@@ -123,7 +124,6 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc(
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
d_ca_code = new gr_complex[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 4)]; d_ca_code = new gr_complex[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 4)];
/* If an array is partitioned for more than one thread to operate on, /* If an array is partitioned for more than one thread to operate on,
* having the sub-array boundaries unaligned to cache lines could lead * having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory * to performance degradation. Here we allocate memory
@@ -145,7 +145,6 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc(
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Very_Late, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Very_Late, 16, sizeof(gr_complex)) == 0){};
//--- Initializations ------------------------------ //--- Initializations ------------------------------
// Initial code frequency basis of NCO // Initial code frequency basis of NCO
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ; d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ;
@@ -183,16 +182,21 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking()
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
// DLL/PLL filter initialization // DLL/PLL filter initialization
d_carrier_loop_filter.initialize(); //initialize the carrier filter d_carrier_loop_filter.initialize(); // initialize the carrier filter
d_code_loop_filter.initialize(); //initialize the code filter d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 2 (2 samples per chip) // generate local reference ALWAYS starting at chip 2 (2 samples per chip)
galileo_e1_code_gen_complex_sampled(&d_ca_code[2],d_acquisition_gnss_synchro->Signal, false, d_acquisition_gnss_synchro->PRN, 2*Galileo_E1_CODE_CHIP_RATE_HZ, 0); galileo_e1_code_gen_complex_sampled(&d_ca_code[2],
d_acquisition_gnss_synchro->Signal,
false,
d_acquisition_gnss_synchro->PRN,
2*Galileo_E1_CODE_CHIP_RATE_HZ,
0);
// Fill head and tail // Fill head and tail
d_ca_code[0] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS)]; d_ca_code[0] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS)];
d_ca_code[1] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS+1)]; d_ca_code[1] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 1)];
d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS+2)] = d_ca_code[2]; d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 2)] = d_ca_code[2];
d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS+3)] = d_ca_code[3]; d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS + 3)] = d_ca_code[3];
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0.0; d_rem_code_phase_samples = 0.0;
@@ -204,18 +208,19 @@ void galileo_e1_dll_pll_veml_tracking_cc::start_tracking()
d_current_prn_length_samples = d_vector_length; d_current_prn_length_samples = d_vector_length;
std::string sys_ = &d_acquisition_gnss_synchro->System; std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0,1); sys = sys_.substr(0, 1);
// DEBUG OUTPUT // DEBUG OUTPUT
std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; std::cout << "Tracking start on channel " << d_channel
DLOG(INFO) << "Start tracking for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " received" << std::endl; << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
DLOG(INFO) << "Start tracking for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " received";
// enable tracking // enable tracking
d_pull_in = true; d_pull_in = true;
d_enable_tracking = true; d_enable_tracking = true;
std::cout << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz std::cout << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples << std::endl; << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples << std::endl;
} }
@@ -243,7 +248,7 @@ void galileo_e1_dll_pll_veml_tracking_cc::update_local_code()
epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples*2;
for (int i=0; i<epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 2 + round(fmod(tcode_half_chips - 2*d_very_early_late_spc_chips, code_length_half_chips)); associated_chip_index = 2 + round(fmod(tcode_half_chips - 2*d_very_early_late_spc_chips, code_length_half_chips));
d_very_early_code[i] = d_ca_code[associated_chip_index]; d_very_early_code[i] = d_ca_code[associated_chip_index];
@@ -356,11 +361,11 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz;
// New code Doppler frequency estimation // New code Doppler frequency estimation
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation //carrier phase accumulator for (K) Doppler estimation
d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO //remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad=d_rem_carr_phase_rad+GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI* d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD;
d_rem_carr_phase_rad=fmod(d_rem_carr_phase_rad,GPS_TWO_PI); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## DLL ########################################################## // ################## DLL ##########################################################
// DLL discriminator // DLL discriminator
@@ -369,9 +374,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator //Code phase accumulator
float code_error_filt_secs; float code_error_filt_secs;
code_error_filt_secs=(Galileo_E1_CODE_PERIOD*code_error_filt_chips)/Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds]
//code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*(float)d_fs_in; //[seconds] //code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*(float)d_fs_in; //[seconds]
d_acc_code_phase_secs=d_acc_code_phase_secs+code_error_filt_secs; d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer // keep alignment parameters for the next input buffer
@@ -440,32 +445,29 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz; current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz; current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
// ########## DEBUG OUTPUT // ########## DEBUG OUTPUT
/*! /*!
* \todo The stop timer has to be moved to the signal source! * \todo The stop timer has to be moved to the signal source!
*/ */
// stream to collect cout calls to improve thread safety // stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream; std::stringstream tmp_str_stream;
if (floor(d_sample_counter / d_fs_in) != d_last_seg) if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{ {
d_last_seg = floor(d_sample_counter / d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
if (d_channel == 0)
{
// debug: Second counter in channel 0
tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush;
std::cout << tmp_str_stream.rdbuf() << std::flush;
}
tmp_str_stream << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", Doppler="<<d_carrier_doppler_hz<<" [Hz] CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
std::cout << tmp_str_stream.rdbuf() << std::flush;
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
//if (d_channel == 0 || d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
}
if (d_channel == 0)
{
// debug: Second counter in channel 0
tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush;
std::cout << tmp_str_stream.rdbuf() << std::flush;
}
tmp_str_stream << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", Doppler=" << d_carrier_doppler_hz << " [Hz] CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
std::cout << tmp_str_stream.rdbuf() << std::flush;
//if (d_channel == 0 || d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
}
} }
else else
{ {
@@ -492,7 +494,6 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect
tmp_P = std::abs<float>(*d_Prompt); tmp_P = std::abs<float>(*d_Prompt);
tmp_L = std::abs<float>(*d_Late); tmp_L = std::abs<float>(*d_Late);
tmp_VL = std::abs<float>(*d_Very_Late); tmp_VL = std::abs<float>(*d_Very_Late);
//std::cout<<"VE="<<tmp_VE<<",E="<<tmp_E<<",L="<<tmp_L<<",VL="<<tmp_VL<<std::endl;
try try
{ {

24
src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h
View File

@@ -4,11 +4,6 @@
* Minus Late) tracking block for Galileo E1 signals * Minus Late) tracking block for Galileo E1 signals
* \author Luis Esteve, 2012. luis(at)epsilon-formacion.com * \author Luis Esteve, 2012. luis(at)epsilon-formacion.com
* *
* Code DLL + carrier PLL according to the algorithms described in:
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach,
* Birkhauser, 2007
*
* ------------------------------------------------------------------------- * -------------------------------------------------------------------------
* *
* Copyright (C) 2012 (see AUTHORS file for a list of contributors) * Copyright (C) 2012 (see AUTHORS file for a list of contributors)
@@ -42,19 +37,15 @@
#include <boost/thread/thread.hpp> #include <boost/thread/thread.hpp>
#include <gnuradio/block.h> #include <gnuradio/block.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
#include "concurrent_queue.h" #include "concurrent_queue.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h" #include "tracking_2nd_PLL_filter.h"
#include "correlator.h" #include "correlator.h"
class galileo_e1_dll_pll_veml_tracking_cc; class galileo_e1_dll_pll_veml_tracking_cc;
typedef boost::shared_ptr<galileo_e1_dll_pll_veml_tracking_cc> typedef boost::shared_ptr<galileo_e1_dll_pll_veml_tracking_cc> galileo_e1_dll_pll_veml_tracking_cc_sptr;
galileo_e1_dll_pll_veml_tracking_cc_sptr;
galileo_e1_dll_pll_veml_tracking_cc_sptr galileo_e1_dll_pll_veml_tracking_cc_sptr
galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq, galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq,
@@ -72,11 +63,9 @@ galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq,
* \brief This class implements a code DLL + carrier PLL VEML (Very Early * \brief This class implements a code DLL + carrier PLL VEML (Very Early
* Minus Late) tracking block for Galileo E1 signals * Minus Late) tracking block for Galileo E1 signals
*/ */
class galileo_e1_dll_pll_veml_tracking_cc: public gr::block class galileo_e1_dll_pll_veml_tracking_cc: public gr::block
{ {
public: public:
~galileo_e1_dll_pll_veml_tracking_cc(); ~galileo_e1_dll_pll_veml_tracking_cc();
void set_channel(unsigned int channel); void set_channel(unsigned int channel);
@@ -84,15 +73,17 @@ public:
void start_tracking(); void start_tracking();
void set_channel_queue(concurrent_queue<int> *channel_internal_queue); void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
/*!
* \brief Code DLL + carrier PLL according to the algorithms described in:
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach,
* Birkhauser, 2007
*/
int general_work (int noutput_items, gr_vector_int &ninput_items, int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private: private:
friend galileo_e1_dll_pll_veml_tracking_cc_sptr friend galileo_e1_dll_pll_veml_tracking_cc_sptr
galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq, galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq,
long fs_in, unsigned long fs_in, unsigned
@@ -196,7 +187,6 @@ private:
std::map<std::string, std::string> systemName; std::map<std::string, std::string> systemName;
std::string sys; std::string sys;
}; };
#endif //GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H #endif //GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H

235
src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc
View File

@@ -49,11 +49,9 @@
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <cmath> #include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
#include <boost/asio.hpp> #include <boost/asio.hpp>
#include "tcp_packet_data.h" #include "tcp_packet_data.h"
@@ -67,8 +65,7 @@
using google::LogMessage; using google::LogMessage;
galileo_e1_tcp_connector_tracking_cc_sptr galileo_e1_tcp_connector_tracking_cc_sptr galileo_e1_tcp_connector_make_tracking_cc(
galileo_e1_tcp_connector_make_tracking_cc(
long if_freq, long if_freq,
long fs_in, long fs_in,
unsigned int vector_length, unsigned int vector_length,
@@ -89,9 +86,10 @@ galileo_e1_tcp_connector_make_tracking_cc(
void Galileo_E1_Tcp_Connector_Tracking_cc::forecast (int noutput_items, void Galileo_E1_Tcp_Connector_Tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required) gr_vector_int &ninput_items_required)
{ {
ninput_items_required[0] = (int)d_vector_length*2; //set the required available samples in each call ninput_items_required[0] = (int)d_vector_length*2; // set the required available samples in each call
} }
Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc( Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
long if_freq, long if_freq,
long fs_in, long fs_in,
@@ -118,7 +116,7 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
// Initialize tracking ========================================== // Initialize tracking ==========================================
//--- DLL variables -------------------------------------------------------- //--- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips) d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
d_very_early_late_spc_chips = very_early_late_space_chips; // Define very-early-late offset (in chips) d_very_early_late_spc_chips = very_early_late_space_chips; // Define very-early-late offset (in chips)
//--- TCP CONNECTOR variables -------------------------------------------------------- //--- TCP CONNECTOR variables --------------------------------------------------------
@@ -152,7 +150,6 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Very_Late, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Very_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ; d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ;
@@ -178,23 +175,24 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
d_CN0_SNV_dB_Hz = 0; d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD; d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD;
systemName["G"] = std::string("GPS");
systemName["R"] = std::string("GLONASS");
systemName["S"] = std::string("SBAS");
systemName["E"] = std::string("Galileo"); systemName["E"] = std::string("Galileo");
systemName["C"] = std::string("Compass");
} }
void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking() void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking()
{ {
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
// generate local reference ALWAYS starting at chip 2 (2 samples per chip) // generate local reference ALWAYS starting at chip 2 (2 samples per chip)
galileo_e1_code_gen_complex_sampled(&d_ca_code[2],d_acquisition_gnss_synchro->Signal, false, d_acquisition_gnss_synchro->PRN, 2*Galileo_E1_CODE_CHIP_RATE_HZ, 0); galileo_e1_code_gen_complex_sampled(&d_ca_code[2],
d_acquisition_gnss_synchro->Signal,
false,
d_acquisition_gnss_synchro->PRN,
2*Galileo_E1_CODE_CHIP_RATE_HZ,
0);
// Fill head and tail // Fill head and tail
d_ca_code[0] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS)]; d_ca_code[0] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS)];
d_ca_code[1] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS+1)]; d_ca_code[1] = d_ca_code[(int)(2*Galileo_E1_B_CODE_LENGTH_CHIPS+1)];
@@ -215,14 +213,14 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking()
// DEBUG OUTPUT // DEBUG OUTPUT
std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
DLOG(INFO) << "Start tracking for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " received" << std::endl; DLOG(INFO) << "Start tracking for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " received";
// enable tracking // enable tracking
d_pull_in = true; d_pull_in = true;
d_enable_tracking = true; d_enable_tracking = true;
std::cout << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz std::cout << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples << std::endl; << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples << std::endl;
} }
@@ -250,7 +248,7 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::update_local_code()
epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples + very_early_late_spc_samples*2;
for (int i=0; i<epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 2 + round(fmod(tcode_half_chips - 2*d_very_early_late_spc_chips, code_length_half_chips)); associated_chip_index = 2 + round(fmod(tcode_half_chips - 2*d_very_early_late_spc_chips, code_length_half_chips));
d_very_early_code[i] = d_ca_code[associated_chip_index]; d_very_early_code[i] = d_ca_code[associated_chip_index];
@@ -262,12 +260,13 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::update_local_code()
memcpy(d_very_late_code, &d_very_early_code[2*very_early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_very_late_code, &d_very_early_code[2*very_early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex));
} }
void Galileo_E1_Tcp_Connector_Tracking_cc::update_local_carrier() void Galileo_E1_Tcp_Connector_Tracking_cc::update_local_carrier()
{ {
float phase_rad, phase_step_rad; float phase_rad, phase_step_rad;
// Compute the carrier phase step for the K-1 carrier doppler estimation // Compute the carrier phase step for the K-1 carrier Doppler estimation
phase_step_rad = (float)GPS_TWO_PI*d_carrier_doppler_hz / (float)d_fs_in; phase_step_rad = (float)GPS_TWO_PI*d_carrier_doppler_hz / (float)d_fs_in;
// Initialize the carrier phase with the remanent carrier phase of the K-2 loop // Initialize the carrier phase with the remnant carrier phase of the K-2 loop
phase_rad = d_rem_carr_phase_rad; phase_rad = d_rem_carr_phase_rad;
for(int i = 0; i < d_current_prn_length_samples; i++) for(int i = 0; i < d_current_prn_length_samples; i++)
{ {
@@ -303,7 +302,6 @@ Galileo_E1_Tcp_Connector_Tracking_cc::~Galileo_E1_Tcp_Connector_Tracking_cc()
int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
// process vars // process vars
float carr_error_filt_hz; float carr_error_filt_hz;
float code_error_filt_chips; float code_error_filt_chips;
@@ -312,33 +310,32 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
if (d_enable_tracking == true) if (d_enable_tracking == true)
{ {
if (d_pull_in == true) if (d_pull_in == true)
{ {
/* /*
* Signal alignment (skip samples until the incoming signal is aligned with local replica) * Signal alignment (skip samples until the incoming signal is aligned with local replica)
*/ */
int samples_offset; int samples_offset;
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples); acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples);
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false; d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica consume_each(samples_offset); //shift input to perform alignment with local replica
return 1; 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;
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder // Block input data and block output stream pointers
Gnss_Synchro current_synchro_data; const gr_complex* in = (gr_complex*) input_items[0];
// Fill the acquisition data Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
current_synchro_data = *d_acquisition_gnss_synchro;
// Block input data and block output stream pointers // Generate local code and carrier replicas (using \hat{f}_d(k-1))
const gr_complex* in = (gr_complex*) input_items[0];
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
// Generate local code and carrier replicas (using \hat{f}_d(k-1))
update_local_code(); update_local_code();
update_local_carrier(); update_local_carrier();
@@ -363,7 +360,19 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
d_control_id++; d_control_id++;
//! Send and receive a TCP packet //! Send and receive a TCP packet
boost::array<float, NUM_TX_VARIABLES_GALILEO_E1> tx_variables_array = {{d_control_id,(*d_Very_Early).real(),(*d_Very_Early).imag(),(*d_Early).real(),(*d_Early).imag(),(*d_Late).real(),(*d_Late).imag(),(*d_Very_Late).real(),(*d_Very_Late).imag(),(*d_Prompt).real(),(*d_Prompt).imag(),d_acq_carrier_doppler_hz,1}}; boost::array<float, NUM_TX_VARIABLES_GALILEO_E1> tx_variables_array = {{d_control_id,
(*d_Very_Early).real(),
(*d_Very_Early).imag(),
(*d_Early).real(),
(*d_Early).imag(),
(*d_Late).real(),
(*d_Late).imag(),
(*d_Very_Late).real(),
(*d_Very_Late).imag(),
(*d_Prompt).real(),
(*d_Prompt).imag(),
d_acq_carrier_doppler_hz,
1}};
d_tcp_com.send_receive_tcp_packet_galileo_e1(tx_variables_array, &tcp_data); d_tcp_com.send_receive_tcp_packet_galileo_e1(tx_variables_array, &tcp_data);
// ################## PLL ########################################################## // ################## PLL ##########################################################
@@ -374,10 +383,10 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
// New code Doppler frequency estimation // New code Doppler frequency estimation
d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ); d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ + ((d_carrier_doppler_hz * Galileo_E1_CODE_CHIP_RATE_HZ) / Galileo_E1_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation //carrier phase accumulator for (K) doppler estimation
d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO //remnant carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad=d_rem_carr_phase_rad+GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD; d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD;
d_rem_carr_phase_rad=fmod(d_rem_carr_phase_rad,GPS_TWO_PI); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## DLL ########################################################## // ################## DLL ##########################################################
// DLL discriminator, carrier loop filter implementation and NCO command generation (TCP_connector) // DLL discriminator, carrier loop filter implementation and NCO command generation (TCP_connector)
@@ -388,66 +397,64 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
d_acc_code_phase_secs=d_acc_code_phase_secs+code_error_filt_secs; d_acc_code_phase_secs=d_acc_code_phase_secs+code_error_filt_secs;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer // keep alignment parameters for the next input buffer
float T_chip_seconds; float T_chip_seconds;
float T_prn_seconds; float T_prn_seconds;
float T_prn_samples; float T_prn_samples;
float K_blk_samples; float K_blk_samples;
// Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation // Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation
T_chip_seconds = 1 / d_code_freq_chips; T_chip_seconds = 1 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * (float)d_fs_in; T_prn_samples = T_prn_seconds * (float)d_fs_in;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs*(float)d_fs_in; K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs*(float)d_fs_in;
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// ####### CN0 ESTIMATION AND LOCK DETECTORS ###### // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{ {
// fill buffer with prompt correlator output values // fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt; d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
d_cn0_estimation_counter++; d_cn0_estimation_counter++;
} }
else else
{ {
d_cn0_estimation_counter = 0; d_cn0_estimation_counter = 0;
// Code lock indicator // Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS); d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E1_B_CODE_LENGTH_CHIPS);
// Carrier lock indicator // Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
{
d_carrier_lock_fail_counter++;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ;
ControlMessageFactory* cmf = new ControlMessageFactory();
if (d_queue != gr::msg_queue::sptr())
{
d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
}
delete cmf;
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
}
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
{
d_carrier_lock_fail_counter++;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{
std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ;
ControlMessageFactory* cmf = new ControlMessageFactory();
if (d_queue != gr::msg_queue::sptr())
{
d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
}
delete cmf;
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
}
}
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
// Tracking_timestamp_secs is aligned with the PRN start sample // 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; 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 // 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.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad; current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
@@ -467,7 +474,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
d_last_seg = floor(d_sample_counter / d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl; std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) 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; << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
} }
} }
else else
@@ -475,8 +482,9 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
if (floor(d_sample_counter / d_fs_in) != d_last_seg) if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{ {
d_last_seg = floor(d_sample_counter / d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) std::cout << "Tracking CH " << d_channel
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
} }
} }
} }
@@ -546,7 +554,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples; tmp_float = d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples); tmp_double = (double)(d_sample_counter+d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
@@ -554,9 +562,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
std::cout << "Exception writing trk dump file " << e.what() << std::endl; std::cout << "Exception writing trk dump file " << e.what() << std::endl;
} }
} }
// if(d_current_prn_length_samples!=d_vector_length) consume_each(d_current_prn_length_samples); // this is needed in gr::block derivates
// std::cout << "d_current_prn_length_samples = " << d_current_prn_length_samples << std::endl;
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter += d_current_prn_length_samples; //count for the processed samples 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 return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
} }
@@ -587,12 +593,12 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::set_channel(unsigned int channel)
} }
} }
//! Listen for connections on a TCP port //! Listen for connections on a TCP port
if (d_listen_connection == true) if (d_listen_connection == true)
{ {
d_port = d_port_ch0 + d_channel; d_port = d_port_ch0 + d_channel;
d_listen_connection = d_tcp_com.listen_tcp_connection(d_port,d_port_ch0); d_listen_connection = d_tcp_com.listen_tcp_connection(d_port, d_port_ch0);
} }
} }
@@ -602,13 +608,12 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::set_channel_queue(concurrent_queue<in
d_channel_internal_queue = channel_internal_queue; d_channel_internal_queue = channel_internal_queue;
} }
void Galileo_E1_Tcp_Connector_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void Galileo_E1_Tcp_Connector_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_acquisition_gnss_synchro = p_gnss_synchro; d_acquisition_gnss_synchro = p_gnss_synchro;
// Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); // Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
//DLOG(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples; //DLOG(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
//DLOG(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz; //DLOG(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
//DLOG(INFO) << "Tracking Satellite set to " << d_satellite; //DLOG(INFO) << "Tracking Satellite set to " << d_satellite;
} }

10
src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h
View File

@@ -50,10 +50,9 @@
#include "tcp_communication.h" #include "tcp_communication.h"
class Galileo_E1_Tcp_Connector_Tracking_cc; class Galileo_E1_Tcp_Connector_Tracking_cc;
typedef boost::shared_ptr<Galileo_E1_Tcp_Connector_Tracking_cc>
galileo_e1_tcp_connector_tracking_cc_sptr; typedef boost::shared_ptr<Galileo_E1_Tcp_Connector_Tracking_cc> galileo_e1_tcp_connector_tracking_cc_sptr;
galileo_e1_tcp_connector_tracking_cc_sptr galileo_e1_tcp_connector_tracking_cc_sptr
galileo_e1_tcp_connector_make_tracking_cc(long if_freq, galileo_e1_tcp_connector_make_tracking_cc(long if_freq,
@@ -75,7 +74,6 @@ galileo_e1_tcp_connector_make_tracking_cc(long if_freq,
class Galileo_E1_Tcp_Connector_Tracking_cc: public gr::block class Galileo_E1_Tcp_Connector_Tracking_cc: public gr::block
{ {
public: public:
~Galileo_E1_Tcp_Connector_Tracking_cc(); ~Galileo_E1_Tcp_Connector_Tracking_cc();
void set_channel(unsigned int channel); void set_channel(unsigned int channel);
@@ -83,15 +81,11 @@ public:
void start_tracking(); void start_tracking();
void set_channel_queue(concurrent_queue<int> *channel_internal_queue); void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
int general_work (int noutput_items, gr_vector_int &ninput_items, int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private: private:
friend galileo_e1_tcp_connector_tracking_cc_sptr friend galileo_e1_tcp_connector_tracking_cc_sptr
galileo_e1_tcp_connector_make_tracking_cc(long if_freq, galileo_e1_tcp_connector_make_tracking_cc(long if_freq,
long fs_in, unsigned long fs_in, unsigned

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

@@ -36,7 +36,6 @@
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include "gps_l1_ca_dll_fll_pll_tracking_cc.h" #include "gps_l1_ca_dll_fll_pll_tracking_cc.h"
//#include "gnss_signal_processing.h"
#include "gps_sdr_signal_processing.h" #include "gps_sdr_signal_processing.h"
#include "GPS_L1_CA.h" #include "GPS_L1_CA.h"
#include "tracking_discriminators.h" #include "tracking_discriminators.h"
@@ -48,7 +47,6 @@
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <cmath> #include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
@@ -65,11 +63,10 @@
using google::LogMessage; using google::LogMessage;
gps_l1_ca_dll_fll_pll_tracking_cc_sptr gps_l1_ca_dll_fll_pll_tracking_cc_sptr gps_l1_ca_dll_fll_pll_make_tracking_cc(
gps_l1_ca_dll_fll_pll_make_tracking_cc( long if_freq,
long if_freq, long fs_in,
long fs_in, unsigned
unsigned
int vector_length, int vector_length,
boost::shared_ptr<gr::msg_queue> queue, boost::shared_ptr<gr::msg_queue> queue,
bool dump, std::string dump_filename, bool dump, std::string dump_filename,
@@ -122,7 +119,7 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
// Initialize tracking variables ========================================== // Initialize tracking variables ==========================================
d_carrier_loop_filter.set_params(fll_bw_hz,pll_bw_hz,order); d_carrier_loop_filter.set_params(fll_bw_hz, pll_bw_hz,order);
d_code_loop_filter = Tracking_2nd_DLL_filter(GPS_L1_CA_CODE_PERIOD); d_code_loop_filter = Tracking_2nd_DLL_filter(GPS_L1_CA_CODE_PERIOD);
d_code_loop_filter.set_DLL_BW(dll_bw_hz); d_code_loop_filter.set_DLL_BW(dll_bw_hz);
@@ -142,7 +139,6 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// space for carrier wipeoff and signal baseband vectors // space for carrier wipeoff and signal baseband vectors
if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// correlator outputs (scalar)
if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
@@ -151,11 +147,8 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(
d_sample_counter = 0; d_sample_counter = 0;
d_acq_sample_stamp = 0; d_acq_sample_stamp = 0;
d_last_seg = 0;// this is for debug output only d_last_seg = 0;// this is for debug output only
d_code_phase_samples = 0;
d_code_phase_samples=0;
d_enable_tracking = false; d_enable_tracking = false;
d_current_prn_length_samples = (int)d_vector_length; d_current_prn_length_samples = (int)d_vector_length;
// CN0 estimation and lock detector buffers // CN0 estimation and lock detector buffers
@@ -181,7 +174,6 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking()
/* /*
* correct the code phase according to the delay between acq and trk * correct the code phase according to the delay between acq and trk
*/ */
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
@@ -251,13 +243,8 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::start_tracking()
d_enable_tracking = true; d_enable_tracking = true;
std::cout << "PULL-IN Doppler [Hz]= " << d_carrier_doppler_hz std::cout << "PULL-IN Doppler [Hz]= " << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples << " Code Phase correction [samples]=" << delay_correction_samples
<< " PULL-IN Code Phase [samples]= " << d_acq_code_phase_samples << std::endl; << " PULL-IN Code Phase [samples]= " << d_acq_code_phase_samples << std::endl;
} }
@@ -279,12 +266,12 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
tcode_chips = -rem_code_phase_chips; tcode_chips = -rem_code_phase_chips;
// Alternative EPL code generation (40% of speed improvement!) // Alternative EPL code generation (40% of speed improvement!)
early_late_spc_samples=round(d_early_late_spc_chips/code_phase_step_chips); early_late_spc_samples = round(d_early_late_spc_chips/code_phase_step_chips);
epl_loop_length_samples=d_current_prn_length_samples+early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples*2;
for (int i=0; i<epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
d_early_code[i]=d_ca_code[associated_chip_index]; d_early_code[i] = d_ca_code[associated_chip_index];
tcode_chips = tcode_chips + code_phase_step_chips; tcode_chips = tcode_chips + code_phase_step_chips;
} }
@@ -309,7 +296,7 @@ void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_code()
void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier() void Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::update_local_carrier()
{ {
double phase, phase_step; double phase, phase_step;
phase_step = GPS_TWO_PI * d_carrier_doppler_hz / d_fs_in; phase_step = GPS_TWO_PI * d_carrier_doppler_hz / d_fs_in;
phase = d_rem_carr_phase; phase = d_rem_carr_phase;
for(int i = 0; i < d_current_prn_length_samples; i++) for(int i = 0; i < d_current_prn_length_samples; i++)
@@ -340,31 +327,27 @@ Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::~Gps_L1_Ca_Dll_Fll_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
*/
int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
double code_error_chips = 0; double code_error_chips = 0;
double code_error_filt_chips =0; double code_error_filt_chips = 0;
double correlation_time_s = 0; double correlation_time_s = 0;
double PLL_discriminator_hz = 0; double PLL_discriminator_hz = 0;
double carr_nco_hz = 0; double carr_nco_hz = 0;
// get the sample in and out pointers // get the sample in and out pointers
const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer 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 Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // block output streams pointer
d_Prompt_prev = *d_Prompt; // for the FLL discriminator d_Prompt_prev = *d_Prompt; // for the FLL discriminator
if (d_enable_tracking == true) if (d_enable_tracking == true)
{ {
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data; Gnss_Synchro current_synchro_data;
// Fill the acquisition data // Fill the acquisition data
current_synchro_data=*d_acquisition_gnss_synchro; current_synchro_data = *d_acquisition_gnss_synchro;
/* /*
* Receiver signal alignment * Receiver signal alignment
*/ */
@@ -381,21 +364,20 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
d_pull_in = false; d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica consume_each(samples_offset); //shift input to perform alignment with local replica
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q=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.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in;
current_synchro_data.Carrier_phase_rads=0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs=0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz=0.0; current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking=false; current_synchro_data.Flag_valid_tracking = false;
*out[0] =current_synchro_data; *out[0] = current_synchro_data;
return 1; return 1;
} }
update_local_code(); update_local_code();
update_local_carrier(); update_local_carrier();
@@ -412,33 +394,33 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
is_unaligned()); is_unaligned());
// check for samples consistency (this should be done before in the receiver / here only if the source is a file) // check for samples consistency (this should be done before in the receiver / here only if the source is a file)
if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{ {
const int samples_available = ninput_items[0]; const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available; d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter; LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available); consume_each(samples_available);
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q=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.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in;
current_synchro_data.Carrier_phase_rads=0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs=0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz=0.0; current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking=false; current_synchro_data.Flag_valid_tracking = false;
*out[0] =current_synchro_data; *out[0] =current_synchro_data;
return 1; return 1;
} }
/* /*
* DLL, FLL, and PLL discriminators * DLL, FLL, and PLL discriminators
*/ */
// Compute DLL error // Compute DLL error
code_error_chips = dll_nc_e_minus_l_normalized(*d_Early,*d_Late); code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late);
// Compute DLL filtered error // Compute DLL filtered error
code_error_filt_chips=d_code_loop_filter.get_code_nco(code_error_chips); code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips);
//compute FLL error //compute FLL error
correlation_time_s = ((double)d_current_prn_length_samples) / d_fs_in; correlation_time_s = ((double)d_current_prn_length_samples) / d_fs_in;
@@ -463,7 +445,6 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
carr_nco_hz = d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz, PLL_discriminator_hz, correlation_time_s); carr_nco_hz = d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz, PLL_discriminator_hz, correlation_time_s);
d_carrier_doppler_hz = d_if_freq + carr_nco_hz; 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); 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);
/*! /*!
@@ -495,11 +476,12 @@ 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) if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{ {
std::cout << "Channel " << d_channel << " loss of lock!" << std::endl; std::cout << "Channel " << d_channel << " loss of lock!" << std::endl;
ControlMessageFactory* cmf = new ControlMessageFactory(); ControlMessageFactory* cmf = new ControlMessageFactory();
if (d_queue != gr::msg_queue::sptr()) { if (d_queue != gr::msg_queue::sptr())
d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); {
} d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
delete cmf; }
delete cmf;
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
} }
@@ -564,7 +546,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
*d_Prompt = gr_complex(0,0); *d_Prompt = gr_complex(0,0);
*d_Late = gr_complex(0,0); *d_Late = gr_complex(0,0);
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
*out[0]=*d_acquisition_gnss_synchro; *out[0] = *d_acquisition_gnss_synchro;
} }
@@ -578,9 +560,9 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
double tmp_double; double tmp_double;
prompt_I = (*d_Prompt).real(); prompt_I = (*d_Prompt).real();
prompt_Q = (*d_Prompt).imag(); prompt_Q = (*d_Prompt).imag();
tmp_E=std::abs<float>(*d_Early); tmp_E = std::abs<float>(*d_Early);
tmp_P=std::abs<float>(*d_Prompt); tmp_P = std::abs<float>(*d_Prompt);
tmp_L=std::abs<float>(*d_Late); tmp_L = std::abs<float>(*d_Late);
try try
{ {
// EPR // EPR
@@ -591,40 +573,39 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
d_dump_file.write((char*)&prompt_I, sizeof(float)); d_dump_file.write((char*)&prompt_I, sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float)); d_dump_file.write((char*)&prompt_Q, sizeof(float));
// PRN start sample stamp // PRN start sample stamp
//tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int)); d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
// accumulated carrier phase // accumulated carrier phase
tmp_float=(float)d_acc_carrier_phase_rad; tmp_float = (float)d_acc_carrier_phase_rad;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
// carrier and code frequency // carrier and code frequency
tmp_float=(float)d_carrier_doppler_hz; tmp_float = (float)d_carrier_doppler_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=(float)d_code_freq_hz; tmp_float = (float)d_code_freq_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
//PLL commands //PLL commands
tmp_float=(float)PLL_discriminator_hz; tmp_float = (float)PLL_discriminator_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=(float)carr_nco_hz; tmp_float = (float)carr_nco_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
//DLL commands //DLL commands
tmp_float=(float)code_error_chips; tmp_float = (float)code_error_chips;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=(float)code_error_filt_chips; tmp_float = (float)code_error_filt_chips;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
// CN0 and carrier lock test // CN0 and carrier lock test
tmp_float=(float)d_CN0_SNV_dB_Hz; tmp_float = (float)d_CN0_SNV_dB_Hz;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=(float)d_carrier_lock_test; tmp_float = (float)d_carrier_lock_test;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float = (float)d_rem_code_phase_samples; tmp_float = (float)d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples); tmp_double = (double)(d_sample_counter + d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
} }
catch (std::ifstream::failure e) catch (std::ifstream::failure e)
@@ -632,7 +613,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
std::cout << "Exception writing trk dump file "<< e.what() << std::endl; std::cout << "Exception writing trk dump file "<< e.what() << std::endl;
} }
} }
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates
d_sample_counter += d_current_prn_length_samples; //count for the processed samples 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 return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
} }

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

@@ -43,13 +43,11 @@
#include <boost/thread/thread.hpp> #include <boost/thread/thread.hpp>
#include <gnuradio/block.h> #include <gnuradio/block.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h" #include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h" #include "gps_sdr_signal_processing.h"
#include "tracking_FLL_PLL_filter.h" #include "tracking_FLL_PLL_filter.h"
#include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_DLL_filter.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
//#include "GPS_L1_CA.h"
#include "correlator.h" #include "correlator.h"
class Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc; class Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc;
@@ -71,7 +69,6 @@ gps_l1_ca_dll_fll_pll_make_tracking_cc(
float dll_bw_hz, float dll_bw_hz,
float early_late_space_chips); float early_late_space_chips);
//class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
/*! /*!
* \brief This class implements a DLL and a FLL assisted PLL tracking loop block * \brief This class implements a DLL and a FLL assisted PLL tracking loop block
@@ -79,7 +76,6 @@ gps_l1_ca_dll_fll_pll_make_tracking_cc(
class Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc: public gr::block class Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc: public gr::block
{ {
public: public:
~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc(); ~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc();
void set_channel(unsigned int channel); void set_channel(unsigned int channel);
@@ -103,10 +99,7 @@ public:
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private: private:
friend gps_l1_ca_dll_fll_pll_tracking_cc_sptr friend gps_l1_ca_dll_fll_pll_tracking_cc_sptr
gps_l1_ca_dll_fll_pll_make_tracking_cc( gps_l1_ca_dll_fll_pll_make_tracking_cc(
long if_freq, long if_freq,
@@ -163,7 +156,6 @@ private:
double d_early_late_spc_chips; double d_early_late_spc_chips;
double d_carrier_doppler_hz; double d_carrier_doppler_hz;
double d_code_freq_hz; double d_code_freq_hz;
double d_code_phase_samples; double d_code_phase_samples;

91
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc
View File

@@ -100,9 +100,6 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{ {
//gr_sync_decimator ("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
// initialize internal vars // initialize internal vars
d_queue = queue; d_queue = queue;
d_dump = dump; d_dump = dump;
@@ -113,7 +110,6 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
// Initialize tracking ========================================== // Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_bw_hz); d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); d_carrier_loop_filter.set_PLL_BW(pll_bw_hz);
@@ -124,12 +120,12 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
// Get space for a vector with the C/A code replica sampled 1x/chip // Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2]; d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2];
/* If an array is partitioned for more than one thread to operate on, /* If an array is partitioned for more than one thread to operate on,
* having the sub-array boundaries unaligned to cache lines could lead * having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory * to performance degradation. Here we allocate memory
* (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes * (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes
*/ */
// todo: do something if posix_memalign fails // todo: do something if posix_memalign fails
// Get space for the resampled early / prompt / late local replicas // Get space for the resampled early / prompt / late local replicas
if (posix_memalign((void**)&d_early_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; if (posix_memalign((void**)&d_early_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
@@ -137,12 +133,10 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// space for carrier wipeoff and signal baseband vectors // space for carrier wipeoff and signal baseband vectors
if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// correlator outputs (scalar)
if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;
@@ -177,15 +171,13 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(
systemName["C"] = std::string("Compass"); systemName["C"] = std::string("Compass");
} }
void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking() void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
{ {
/* // correct the code phase according to the delay between acq and trk
* correct the code phase according to the delay between acq and trk
*/
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
unsigned long int acq_trk_diff_samples; unsigned long int acq_trk_diff_samples;
float acq_trk_diff_seconds; float acq_trk_diff_seconds;
@@ -204,7 +196,6 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
T_chip_mod_seconds = 1/d_code_freq_chips; T_chip_mod_seconds = 1/d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; 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; T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in;
d_current_prn_length_samples = round(T_prn_mod_samples); 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_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
@@ -220,24 +211,21 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
} }
delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
d_acq_code_phase_samples = corrected_acq_phase_samples; d_acq_code_phase_samples = corrected_acq_phase_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization // DLL/PLL filter initialization
d_carrier_loop_filter.initialize(); //initialize the carrier filter d_carrier_loop_filter.initialize(); //initialize the carrier filter
d_code_loop_filter.initialize(); //initialize the code filter d_code_loop_filter.initialize(); //initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip) // generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0); gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0);
d_ca_code[0] = d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS]; d_ca_code[0] = d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1]; d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
//****************************************************************************** //******************************************************************************
// Experimental: pre-sampled local signal replica at nominal code frequency. // Experimental: pre-sampled local signal replica at nominal code frequency.
// No code doppler correction // No code doppler correction
double tcode_chips; double tcode_chips;
int associated_chip_index; int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
@@ -252,7 +240,7 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
// Alternative EPL code generation (40% of speed improvement!) // Alternative EPL code generation (40% of speed improvement!)
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
epl_loop_length_samples = d_current_prn_length_samples +early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples +early_late_spc_samples*2;
for (int i=0; i<epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index]; d_early_code[i] = d_ca_code[associated_chip_index];
@@ -261,7 +249,6 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::start_tracking()
memcpy(d_prompt_code, &d_early_code[early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_prompt_code, &d_early_code[early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex));
memcpy(d_late_code, &d_early_code[early_late_spc_samples*2], d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_late_code, &d_early_code[early_late_spc_samples*2], d_current_prn_length_samples* sizeof(gr_complex));
//****************************************************************************** //******************************************************************************
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
@@ -307,7 +294,7 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::update_local_code()
//EPL code generation //EPL code generation
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples*2;
for (int i=0; i<epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index]; d_early_code[i] = d_ca_code[associated_chip_index];
@@ -327,6 +314,7 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::update_local_carrier()
//sse_nco(d_carr_sign, d_current_prn_length_samples,d_rem_carr_phase_rad, phase_step_rad); //sse_nco(d_carr_sign, d_current_prn_length_samples,d_rem_carr_phase_rad, phase_step_rad);
} }
Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc() Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc()
{ {
d_dump_file.close(); d_dump_file.close();
@@ -344,17 +332,12 @@ Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc()
} }
/* Tracking signal processing // Tracking signal processing
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
*/
int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
// stream to collect cout calls to improve thread safety // stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream; std::stringstream tmp_str_stream;
float carr_error_hz; float carr_error_hz;
float carr_error_filt_hz; float carr_error_filt_hz;
float code_error_chips; float code_error_chips;
@@ -368,20 +351,14 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
if (d_pull_in == true) if (d_pull_in == true)
{ {
int samples_offset; int samples_offset;
// 28/11/2011 ACQ to TRK transition BUG CORRECTION
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples); acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples);
//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); 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_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false; d_pull_in = false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n"; consume_each(samples_offset); //shift input to perform alignment with local replica
consume_each(samples_offset); //shift input to perform alignement with local replica
return 1; return 1;
} }
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
@@ -390,13 +367,11 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
current_synchro_data = *d_acquisition_gnss_synchro; current_synchro_data = *d_acquisition_gnss_synchro;
// Block input data and block output stream pointers // Block input data and block output stream pointers
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
// Generate local code and carrier replicas (using \hat{f}_d(k-1)) // Generate local code and carrier replicas (using \hat{f}_d(k-1))
//update_local_code(); //disabled in the speed optimized tracking! //update_local_code(); //disabled in the speed optimized tracking!
update_local_carrier(); update_local_carrier();
// perform Early, Prompt and Late correlation // perform Early, Prompt and Late correlation
@@ -436,7 +411,6 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD*code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds] code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD*code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer // keep alignment parameters for the next input buffer
float T_chip_seconds; float T_chip_seconds;
@@ -451,7 +425,6 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// ####### CN0 ESTIMATION AND LOCK DETECTORS ###### // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{ {
@@ -489,7 +462,6 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
} }
} }
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
// Tracking_timestamp_secs is aligned with the PRN start sample // Tracking_timestamp_secs is aligned with the PRN start sample
@@ -505,24 +477,23 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
/*! /*!
* \todo The stop timer has to be moved to the signal source! * \todo The stop timer has to be moved to the signal source!
*/ */
if (floor(d_sample_counter / d_fs_in) != d_last_seg) if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{ {
d_last_seg = floor(d_sample_counter / d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
if (d_channel == 0) if (d_channel == 0)
{ {
// debug: Second counter in channel 0 // debug: Second counter in channel 0
tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush;
std::cout << tmp_str_stream.rdbuf() << std::flush; std::cout << tmp_str_stream.rdbuf() << std::flush;
} }
tmp_str_stream << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) tmp_str_stream << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", Doppler="<<d_carrier_doppler_hz<<" [Hz] CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; << ", Doppler=" << d_carrier_doppler_hz << " [Hz] CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
std::cout << tmp_str_stream.rdbuf() << std::flush; std::cout << tmp_str_stream.rdbuf() << std::flush;
LOG(INFO) << tmp_str_stream.rdbuf();
//std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl; //if (d_channel == 0 || d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
//if (d_channel == 0 || d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock! }
}
} }
else else
{ {
@@ -581,7 +552,7 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples; tmp_float = d_rem_code_phase_samples;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_double=(double)(d_sample_counter+d_current_prn_length_samples); tmp_double = (double)(d_sample_counter + d_current_prn_length_samples);
d_dump_file.write((char*)&tmp_double, sizeof(double)); d_dump_file.write((char*)&tmp_double, sizeof(double));
} }
catch (std::ifstream::failure& e) catch (std::ifstream::failure& e)
@@ -602,7 +573,7 @@ void Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::set_channel(unsigned int channel)
d_channel = channel; d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel; LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump==true) if (d_dump == true)
{ {
if (d_dump_file.is_open() == false) if (d_dump_file.is_open() == false)
{ {

40
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h
View File

@@ -35,7 +35,7 @@
*/ */
#ifndef GNSS_SDR_GPS_L1_CA_DLL_PLL_OPTIM_TRACKING_CC_H #ifndef GNSS_SDR_GPS_L1_CA_DLL_PLL_OPTIM_TRACKING_CC_H
#define GNSS_SDR_GPS_L1_CA_DLL_PLL_OPTIM_TRACKING_CC_H #define GNSS_SDR_GPS_L1_CA_DLL_PLL_OPTIM_TRACKING_CC_H
#include <fstream> #include <fstream>
#include <queue> #include <queue>
@@ -43,33 +43,27 @@
#include <boost/thread/thread.hpp> #include <boost/thread/thread.hpp>
#include <gnuradio/block.h> #include <gnuradio/block.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h" #include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h" #include "gps_sdr_signal_processing.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h" #include "tracking_2nd_PLL_filter.h"
#include "correlator.h" #include "correlator.h"
class Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc; class Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc;
typedef boost::shared_ptr<Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc>
gps_l1_ca_dll_pll_optim_tracking_cc_sptr;
gps_l1_ca_dll_pll_optim_tracking_cc_sptr typedef boost::shared_ptr<Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc> gps_l1_ca_dll_pll_optim_tracking_cc_sptr;
gps_l1_ca_dll_pll_make_optim_tracking_cc(long if_freq,
long fs_in, unsigned gps_l1_ca_dll_pll_optim_tracking_cc_sptr gps_l1_ca_dll_pll_make_optim_tracking_cc(long if_freq,
int vector_length, long fs_in,
boost::shared_ptr<gr::msg_queue> queue, unsigned int vector_length,
bool dump, boost::shared_ptr<gr::msg_queue> queue,
std::string dump_filename, bool dump,
float pll_bw_hz, std::string dump_filename,
float dll_bw_hz, float pll_bw_hz,
float early_late_space_chips); float dll_bw_hz,
float early_late_space_chips);
//class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
/*! /*!
* \brief This class implements a DLL + PLL tracking loop block * \brief This class implements a DLL + PLL tracking loop block
@@ -77,7 +71,6 @@ gps_l1_ca_dll_pll_make_optim_tracking_cc(long if_freq,
class Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc: public gr::block class Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc: public gr::block
{ {
public: public:
~Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc(); ~Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc();
void set_channel(unsigned int channel); void set_channel(unsigned int channel);
@@ -85,20 +78,11 @@ public:
void start_tracking(); void start_tracking();
void set_channel_queue(concurrent_queue<int> *channel_internal_queue); void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
/*
* \brief just like gr_block::general_work, only this arranges to call consume_each for you
*
* The user must override work to define the signal processing code
*/
int general_work (int noutput_items, gr_vector_int &ninput_items, int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private: private:
friend gps_l1_ca_dll_pll_optim_tracking_cc_sptr friend gps_l1_ca_dll_pll_optim_tracking_cc_sptr
gps_l1_ca_dll_pll_make_optim_tracking_cc(long if_freq, gps_l1_ca_dll_pll_make_optim_tracking_cc(long if_freq,
long fs_in, unsigned long fs_in, unsigned

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

@@ -45,7 +45,6 @@
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <cmath> #include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
@@ -100,9 +99,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{ {
//gr_sync_decimator ("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
// initialize internal vars // initialize internal vars
d_queue = queue; d_queue = queue;
d_dump = dump; d_dump = dump;
@@ -112,7 +108,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
// Initialize tracking ========================================== // Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_bw_hz); d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); d_carrier_loop_filter.set_PLL_BW(pll_bw_hz);
@@ -135,12 +130,10 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// space for carrier wipeoff and signal baseband vectors // space for carrier wipeoff and signal baseband vectors
if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){}; if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
// correlator outputs (scalar)
if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ; d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;
@@ -175,12 +168,12 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
systemName["C"] = std::string("Compass"); systemName["C"] = std::string("Compass");
} }
void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
{ {
/* /*
* correct the code phase according to the delay between acq and trk * correct the code phase according to the delay between acq and trk
*/ */
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
@@ -222,9 +215,10 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
d_acq_code_phase_samples = corrected_acq_phase_samples; d_acq_code_phase_samples = corrected_acq_phase_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization // DLL/PLL filter initialization
d_carrier_loop_filter.initialize(); //initialize the carrier filter d_carrier_loop_filter.initialize(); // initialize the carrier filter
d_code_loop_filter.initialize(); //initialize the code filter d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip) // generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0); gps_l1_ca_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, 0);
@@ -244,7 +238,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
// DEBUG OUTPUT // DEBUG OUTPUT
std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
DLOG(INFO) << "Start tracking for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " received" << std::endl; DLOG(INFO) << "Start tracking for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " received";
// enable tracking // enable tracking
d_pull_in = true; d_pull_in = true;
@@ -266,8 +260,8 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
int associated_chip_index; int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
double code_phase_step_chips; double code_phase_step_chips;
int early_late_spc_samples; int early_late_spc_samples;
int epl_loop_length_samples; int epl_loop_length_samples;
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
code_phase_step_chips = ((double)d_code_freq_chips) / ((double)d_fs_in); code_phase_step_chips = ((double)d_code_freq_chips) / ((double)d_fs_in);
@@ -277,12 +271,12 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
// Alternative EPL code generation (40% of speed improvement!) // Alternative EPL code generation (40% of speed improvement!)
early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips); early_late_spc_samples = round(d_early_late_spc_chips / code_phase_step_chips);
epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples*2;
for (int i=0; i<epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index]; d_early_code[i] = d_ca_code[associated_chip_index];
tcode_chips = tcode_chips + code_phase_step_chips; tcode_chips = tcode_chips + code_phase_step_chips;
} }
memcpy(d_prompt_code,&d_early_code[early_late_spc_samples],d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_prompt_code,&d_early_code[early_late_spc_samples],d_current_prn_length_samples* sizeof(gr_complex));
memcpy(d_late_code,&d_early_code[early_late_spc_samples*2],d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_late_code,&d_early_code[early_late_spc_samples*2],d_current_prn_length_samples* sizeof(gr_complex));
@@ -326,14 +320,10 @@ 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
*/
int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
// process vars // process vars
float carr_error_hz; float carr_error_hz;
float carr_error_filt_hz; float carr_error_filt_hz;
@@ -342,26 +332,21 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
if (d_enable_tracking == true) if (d_enable_tracking == true)
{ {
/* // Receiver signal alignment
* Receiver signal alignment
*/
if (d_pull_in == true) if (d_pull_in == true)
{ {
int samples_offset; int samples_offset;
// 28/11/2011 ACQ to TRK transition BUG CORRECTION
float acq_trk_shif_correction_samples; float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples); acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples);
//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); 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 // /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_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false; d_pull_in = false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n"; //std::cout<<" samples_offset="<<samples_offset<<"\r\n";
consume_each(samples_offset); //shift input to perform alignement with local replica consume_each(samples_offset); //shift input to perform alignment with local replica
return 1; return 1;
} }
@@ -371,7 +356,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
current_synchro_data = *d_acquisition_gnss_synchro; current_synchro_data = *d_acquisition_gnss_synchro;
// Block input data and block output stream pointers // Block input data and block output stream pointers
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignment
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
// Generate local code and carrier replicas (using \hat{f}_d(k-1)) // Generate local code and carrier replicas (using \hat{f}_d(k-1))
@@ -391,26 +376,26 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
is_unaligned()); is_unaligned());
// check for samples consistency (this should be done before in the receiver / here only if the source is a file) // check for samples consistency (this should be done before in the receiver / here only if the source is a file)
if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true ) // or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{ {
const int samples_available = ninput_items[0]; const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available; d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter; LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available); consume_each(samples_available);
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0; current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/(double)d_fs_in; 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.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0; current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking = false; current_synchro_data.Flag_valid_tracking = false;
*out[0] = current_synchro_data; *out[0] = current_synchro_data;
return 1; return 1;
} }
// ################## PLL ########################################################## // ################## PLL ##########################################################
// PLL discriminator // PLL discriminator
@@ -422,21 +407,20 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
// New code Doppler frequency estimation // New code Doppler frequency estimation
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
//carrier phase accumulator for (K) doppler estimation //carrier phase accumulator for (K) doppler estimation
d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+GPS_TWO_PI*d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD; d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD;
//remanent carrier phase to prevent overflow in the code NCO //remanent carrier phase to prevent overflow in the code NCO
d_rem_carr_phase_rad=d_rem_carr_phase_rad+GPS_TWO_PI*d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD; d_rem_carr_phase_rad = d_rem_carr_phase_rad+GPS_TWO_PI*d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD;
d_rem_carr_phase_rad=fmod(d_rem_carr_phase_rad,GPS_TWO_PI); d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## DLL ########################################################## // ################## DLL ##########################################################
// DLL discriminator // DLL discriminator
code_error_chips =dll_nc_e_minus_l_normalized(*d_Early, *d_Late); //[chips/Ti] code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late); //[chips/Ti]
// Code discriminator filter // Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator //Code phase accumulator
float code_error_filt_secs; float code_error_filt_secs;
code_error_filt_secs=(GPS_L1_CA_CODE_PERIOD*code_error_filt_chips)/GPS_L1_CA_CODE_RATE_HZ; //[seconds] code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD*code_error_filt_chips)/GPS_L1_CA_CODE_RATE_HZ; //[seconds]
d_acc_code_phase_secs=d_acc_code_phase_secs+code_error_filt_secs; d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer // keep alignment parameters for the next input buffer
@@ -444,7 +428,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
float T_prn_seconds; float T_prn_seconds;
float T_prn_samples; float T_prn_samples;
float K_blk_samples; float K_blk_samples;
// Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
T_chip_seconds = 1 / d_code_freq_chips; T_chip_seconds = 1 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples = T_prn_seconds * (float)d_fs_in; T_prn_samples = T_prn_seconds * (float)d_fs_in;
@@ -452,45 +436,43 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// ####### CN0 ESTIMATION AND LOCK DETECTORS ###### // ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{ {
// fill buffer with prompt correlator output values // fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt; d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
d_cn0_estimation_counter++; d_cn0_estimation_counter++;
} }
else else
{ {
d_cn0_estimation_counter = 0; d_cn0_estimation_counter = 0;
// Code lock indicator // Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS);
// Carrier lock indicator // Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
// Loss of lock detection // Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
{ {
d_carrier_lock_fail_counter++; d_carrier_lock_fail_counter++;
} }
else else
{ {
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
} }
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{ {
std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ; std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ;
ControlMessageFactory* cmf = new ControlMessageFactory(); ControlMessageFactory* cmf = new ControlMessageFactory();
if (d_queue != gr::msg_queue::sptr()) if (d_queue != gr::msg_queue::sptr())
{ {
d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
} }
delete cmf; delete cmf;
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
} }
} }
// ########### Output the tracking data to navigation and PVT ########## // ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
// Tracking_timestamp_secs is aligned with the PRN start sample // Tracking_timestamp_secs is aligned with the PRN start sample
@@ -514,8 +496,7 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in
d_last_seg = floor(d_sample_counter / d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl; std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) 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; << ", 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! //if (d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
} }
} }
@@ -596,7 +577,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 consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates
d_sample_counter += d_current_prn_length_samples; //count for the processed samples 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 return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
} }
@@ -608,7 +589,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_channel(unsigned int channel)
d_channel = channel; d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel; LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump==true) if (d_dump == true)
{ {
if (d_dump_file.is_open() == false) if (d_dump_file.is_open() == false)
{ {

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

@@ -42,17 +42,13 @@
#include <boost/thread/thread.hpp> #include <boost/thread/thread.hpp>
#include <gnuradio/block.h> #include <gnuradio/block.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h" #include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h" #include "gps_sdr_signal_processing.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h" #include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h" #include "tracking_2nd_PLL_filter.h"
#include "correlator.h" #include "correlator.h"
class Gps_L1_Ca_Dll_Pll_Tracking_cc; class Gps_L1_Ca_Dll_Pll_Tracking_cc;
typedef boost::shared_ptr<Gps_L1_Ca_Dll_Pll_Tracking_cc> typedef boost::shared_ptr<Gps_L1_Ca_Dll_Pll_Tracking_cc>
@@ -69,7 +65,7 @@ gps_l1_ca_dll_pll_make_tracking_cc(long if_freq,
float dll_bw_hz, float dll_bw_hz,
float early_late_space_chips); float early_late_space_chips);
//class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
/*! /*!
* \brief This class implements a DLL + PLL tracking loop block * \brief This class implements a DLL + PLL tracking loop block
@@ -77,7 +73,6 @@ gps_l1_ca_dll_pll_make_tracking_cc(long if_freq,
class Gps_L1_Ca_Dll_Pll_Tracking_cc: public gr::block class Gps_L1_Ca_Dll_Pll_Tracking_cc: public gr::block
{ {
public: public:
~Gps_L1_Ca_Dll_Pll_Tracking_cc(); ~Gps_L1_Ca_Dll_Pll_Tracking_cc();
void set_channel(unsigned int channel); void set_channel(unsigned int channel);
@@ -85,20 +80,12 @@ public:
void start_tracking(); void start_tracking();
void set_channel_queue(concurrent_queue<int> *channel_internal_queue); void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
/*
* \brief just like gr_block::general_work, only this arranges to call consume_each for you
*
* The user must override work to define the signal processing code
*/
int general_work (int noutput_items, gr_vector_int &ninput_items, int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private: private:
friend gps_l1_ca_dll_pll_tracking_cc_sptr friend gps_l1_ca_dll_pll_tracking_cc_sptr
gps_l1_ca_dll_pll_make_tracking_cc(long if_freq, gps_l1_ca_dll_pll_make_tracking_cc(long if_freq,
long fs_in, unsigned long fs_in, unsigned

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

@@ -47,7 +47,6 @@
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
#include <cmath> #include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
@@ -105,9 +104,6 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc(
gr::block("Gps_L1_Ca_Tcp_Connector_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::block("Gps_L1_Ca_Tcp_Connector_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{ {
//gr_sync_decimator ("Gps_L1_Ca_Tcp_Connector_Tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
// initialize internal vars // initialize internal vars
d_queue = queue; d_queue = queue;
d_dump = dump; d_dump = dump;
@@ -117,7 +113,6 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc(
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
// Initialize tracking ========================================== // Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_bw_hz); d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_PLL_BW(pll_bw_hz); d_carrier_loop_filter.set_PLL_BW(pll_bw_hz);
@@ -133,9 +128,6 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc(
// Initialization of local code replica // Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip // Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2]; d_ca_code = new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 2];
d_carr_sign = new gr_complex[d_vector_length*2]; d_carr_sign = new gr_complex[d_vector_length*2];
/* If an array is partitioned for more than one thread to operate on, /* If an array is partitioned for more than one thread to operate on,
@@ -155,7 +147,6 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::Gps_L1_Ca_Tcp_Connector_Tracking_cc(
if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){}; if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------ //--- Perform initializations ------------------------------
// define initial code frequency basis of NCO // define initial code frequency basis of NCO
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ; d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ;
@@ -206,12 +197,12 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::start_tracking()
// jarribas: this patch correct a situation where the tracking sample counter // jarribas: this patch correct a situation where the tracking sample counter
// is equal to 0 (remains in the initial state) at the first acquisition to tracking transition // is equal to 0 (remains in the initial state) at the first acquisition to tracking transition
// of the receiver operation when is connecting to simulink server. // of the receiver operation when is connecting to simulink server.
// if (d_sample_counter<d_acq_sample_stamp) // if (d_sample_counter<d_acq_sample_stamp)
// { // {
// acq_trk_diff_samples=0; //disable the correction // acq_trk_diff_samples=0; //disable the correction
// }else{ // }else{
// acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length; // acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
// } // }
acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length; acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
std::cout << "acq_trk_diff_samples=" << acq_trk_diff_samples << std::endl; std::cout << "acq_trk_diff_samples=" << acq_trk_diff_samples << std::endl;
acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in; acq_trk_diff_seconds = (float)acq_trk_diff_samples / (float)d_fs_in;
@@ -293,8 +284,8 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::update_local_code()
int associated_chip_index; int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS; int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
double code_phase_step_chips; double code_phase_step_chips;
int early_late_spc_samples; int early_late_spc_samples;
int epl_loop_length_samples; int epl_loop_length_samples;
// unified loop for E, P, L code vectors // unified loop for E, P, L code vectors
code_phase_step_chips = ((double)d_code_freq_hz) / ((double)d_fs_in); code_phase_step_chips = ((double)d_code_freq_hz) / ((double)d_fs_in);
@@ -302,17 +293,17 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::update_local_code()
tcode_chips = -rem_code_phase_chips; tcode_chips = -rem_code_phase_chips;
// Alternative EPL code generation (40% of speed improvement!) // Alternative EPL code generation (40% of speed improvement!)
early_late_spc_samples=round(d_early_late_spc_chips/code_phase_step_chips); early_late_spc_samples = round(d_early_late_spc_chips/code_phase_step_chips);
epl_loop_length_samples=d_current_prn_length_samples+early_late_spc_samples*2; epl_loop_length_samples = d_current_prn_length_samples+early_late_spc_samples*2;
for (int i=0; i<epl_loop_length_samples; i++) for (int i = 0; i < epl_loop_length_samples; i++)
{ {
associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips)); associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index]; d_early_code[i] = d_ca_code[associated_chip_index];
tcode_chips = tcode_chips + d_code_phase_step_chips; tcode_chips = tcode_chips + d_code_phase_step_chips;
} }
memcpy(d_prompt_code,&d_early_code[early_late_spc_samples],d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_prompt_code,&d_early_code[early_late_spc_samples], d_current_prn_length_samples* sizeof(gr_complex));
memcpy(d_late_code,&d_early_code[early_late_spc_samples*2],d_current_prn_length_samples* sizeof(gr_complex)); memcpy(d_late_code,&d_early_code[early_late_spc_samples*2], d_current_prn_length_samples* sizeof(gr_complex));
} }
@@ -355,14 +346,11 @@ Gps_L1_Ca_Tcp_Connector_Tracking_cc::~Gps_L1_Ca_Tcp_Connector_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
*/
int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items, int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{ {
// process vars // process vars
float carr_error; float carr_error;
float carr_nco; float carr_nco;
@@ -385,13 +373,11 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
int acq_to_trk_delay_samples; int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; 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_next_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_next_prn_length_samples);
//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); 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 // /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_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false; d_pull_in = false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n";
consume_each(samples_offset); //shift input to perform alignement with local replica consume_each(samples_offset); //shift input to perform alignement with local replica
return 1; return 1;
} }
@@ -426,40 +412,48 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
// check for samples consistency (this should be done before in the receiver / here only if the source is a file) // check for samples consistency (this should be done before in the receiver / here only if the source is a file)
if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true) if (std::isnan((*d_Prompt).real()) == true or std::isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
{ {
const int samples_available = ninput_items[0]; const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available; d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter; LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available); consume_each(samples_available);
// make an output to not stop the rest of the processing blocks // make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0; current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q=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 = d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads=0.0; current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs=0.0; current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz=0.0; current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking=false; current_synchro_data.Flag_valid_tracking = false;
*out[0] =current_synchro_data; *out[0] =current_synchro_data;
return 1; return 1;
} }
//! Variable used for control //! Variable used for control
d_control_id++; d_control_id++;
//! Send and receive a TCP packet //! Send and receive a TCP packet
boost::array<float, NUM_TX_VARIABLES_GPS_L1_CA> tx_variables_array = {{d_control_id,(*d_Early).real(),(*d_Early).imag(),(*d_Late).real(),(*d_Late).imag(),(*d_Prompt).real(),(*d_Prompt).imag(),d_acq_carrier_doppler_hz,1}}; boost::array<float, NUM_TX_VARIABLES_GPS_L1_CA> tx_variables_array = {{d_control_id,
d_tcp_com.send_receive_tcp_packet_gps_l1_ca(tx_variables_array, &tcp_data); (*d_Early).real(),
(*d_Early).imag(),
(*d_Late).real(),
(*d_Late).imag(),
(*d_Prompt).real(),
(*d_Prompt).imag(),
d_acq_carrier_doppler_hz,
1}};
d_tcp_com.send_receive_tcp_packet_gps_l1_ca(tx_variables_array, &tcp_data);
//! Recover the tracking data //! Recover the tracking data
code_error = tcp_data.proc_pack_code_error; code_error = tcp_data.proc_pack_code_error;
carr_error = tcp_data.proc_pack_carr_error; carr_error = tcp_data.proc_pack_carr_error;
// Modify carrier freq based on NCO command // Modify carrier freq based on NCO command
d_carrier_doppler_hz = tcp_data.proc_pack_carrier_doppler_hz; d_carrier_doppler_hz = tcp_data.proc_pack_carrier_doppler_hz;
// Modify code freq based on NCO command // Modify code freq based on NCO command
code_nco=1/(1/GPS_L1_CA_CODE_RATE_HZ-code_error/GPS_L1_CA_CODE_LENGTH_CHIPS); code_nco = 1/(1/GPS_L1_CA_CODE_RATE_HZ - code_error/GPS_L1_CA_CODE_LENGTH_CHIPS);
d_code_freq_hz = code_nco; d_code_freq_hz = code_nco;
// Update the phasestep based on code freq (variable) and // Update the phasestep based on code freq (variable) and
@@ -517,11 +511,11 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
{ {
std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ; std::cout << "Channel " << d_channel << " loss of lock!" << std::endl ;
ControlMessageFactory* cmf = new ControlMessageFactory(); ControlMessageFactory* cmf = new ControlMessageFactory();
if (d_queue != gr::msg_queue::sptr()) { if (d_queue != gr::msg_queue::sptr()) {
d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
} }
delete cmf; delete cmf;
d_carrier_lock_fail_counter = 0; d_carrier_lock_fail_counter = 0;
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
@@ -551,7 +545,7 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
d_last_seg = floor(d_sample_counter / d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl; std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) 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; << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
} }
} }
else else
@@ -560,7 +554,7 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec
{ {
d_last_seg = floor(d_sample_counter / d_fs_in); d_last_seg = floor(d_sample_counter / d_fs_in);
std::cout << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) 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; << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
} }
} }
} }
@@ -622,7 +616,7 @@ 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)); d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes) // AUX vars (for debug purposes)
tmp_float=0; tmp_float = 0;
d_dump_file.write((char*)&tmp_float, sizeof(float)); d_dump_file.write((char*)&tmp_float, sizeof(float));
d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double)); d_dump_file.write((char*)&d_sample_counter_seconds, sizeof(double));
} }
@@ -632,7 +626,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 consume_each(d_current_prn_length_samples); // this is necessary 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 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 return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
@@ -645,7 +639,7 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::set_channel(unsigned int channel)
d_channel = channel; d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel; LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump==true) if (d_dump == true)
{ {
if (d_dump_file.is_open() == false) if (d_dump_file.is_open() == false)
{ {
@@ -664,12 +658,12 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::set_channel(unsigned int channel)
} }
} }
//! Listen for connections on a TCP port //! Listen for connections on a TCP port
if (d_listen_connection == true) if (d_listen_connection == true)
{ {
d_port = d_port_ch0 + d_channel; d_port = d_port_ch0 + d_channel;
d_listen_connection = d_tcp_com.listen_tcp_connection(d_port,d_port_ch0); d_listen_connection = d_tcp_com.listen_tcp_connection(d_port, d_port_ch0);
} }
} }
@@ -682,7 +676,6 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::set_channel_queue(concurrent_queue<int
void Gps_L1_Ca_Tcp_Connector_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) void Gps_L1_Ca_Tcp_Connector_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{ {
d_acquisition_gnss_synchro = p_gnss_synchro; d_acquisition_gnss_synchro = p_gnss_synchro;
// Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); // Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
//DLOG(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples; //DLOG(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
//DLOG(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz; //DLOG(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;

11
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h
View File

@@ -42,7 +42,6 @@
#include <boost/thread/thread.hpp> #include <boost/thread/thread.hpp>
#include <gnuradio/block.h> #include <gnuradio/block.h>
#include <gnuradio/msg_queue.h> #include <gnuradio/msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h" #include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h" #include "gps_sdr_signal_processing.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
@@ -54,8 +53,8 @@
class Gps_L1_Ca_Tcp_Connector_Tracking_cc; class Gps_L1_Ca_Tcp_Connector_Tracking_cc;
typedef boost::shared_ptr<Gps_L1_Ca_Tcp_Connector_Tracking_cc>
gps_l1_ca_tcp_connector_tracking_cc_sptr; typedef boost::shared_ptr<Gps_L1_Ca_Tcp_Connector_Tracking_cc> gps_l1_ca_tcp_connector_tracking_cc_sptr;
gps_l1_ca_tcp_connector_tracking_cc_sptr gps_l1_ca_tcp_connector_tracking_cc_sptr
gps_l1_ca_tcp_connector_make_tracking_cc(long if_freq, gps_l1_ca_tcp_connector_make_tracking_cc(long if_freq,
@@ -69,7 +68,6 @@ gps_l1_ca_tcp_connector_make_tracking_cc(long if_freq,
float early_late_space_chips, float early_late_space_chips,
size_t port_ch0); size_t port_ch0);
//class gps_l1_ca_tcp_connector_tracking_cc: public gr_sync_decimator
/*! /*!
* \brief This class implements a DLL + PLL tracking loop block * \brief This class implements a DLL + PLL tracking loop block
@@ -77,7 +75,6 @@ gps_l1_ca_tcp_connector_make_tracking_cc(long if_freq,
class Gps_L1_Ca_Tcp_Connector_Tracking_cc: public gr::block class Gps_L1_Ca_Tcp_Connector_Tracking_cc: public gr::block
{ {
public: public:
~Gps_L1_Ca_Tcp_Connector_Tracking_cc(); ~Gps_L1_Ca_Tcp_Connector_Tracking_cc();
void set_channel(unsigned int channel); void set_channel(unsigned int channel);
@@ -90,15 +87,11 @@ public:
* *
* The user must override work to define the signal processing code * The user must override work to define the signal processing code
*/ */
int general_work (int noutput_items, gr_vector_int &ninput_items, int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required); void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private: private:
friend gps_l1_ca_tcp_connector_tracking_cc_sptr friend gps_l1_ca_tcp_connector_tracking_cc_sptr
gps_l1_ca_tcp_connector_make_tracking_cc(long if_freq, gps_l1_ca_tcp_connector_make_tracking_cc(long if_freq,
long fs_in, unsigned long fs_in, unsigned

90
src/core/system_parameters/GPS_L1_CA.h
View File

@@ -73,14 +73,12 @@ const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this
const int GPS_CA_PREAMBLE_LENGTH_BITS = 8; const int GPS_CA_PREAMBLE_LENGTH_BITS = 8;
const int GPS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s] const int GPS_CA_TELEMETRY_RATE_BITS_SECOND = 50; //!< NAV message bit rate [bits/s]
const int GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND = GPS_CA_TELEMETRY_RATE_BITS_SECOND*20; //!< NAV message bit rate [symbols/s] const int GPS_CA_TELEMETRY_RATE_SYMBOLS_SECOND = GPS_CA_TELEMETRY_RATE_BITS_SECOND*20; //!< NAV message bit rate [symbols/s]
const int GPS_WORD_LENGTH = 4; // CRC + GPS WORD (-2 -1 0 ... 29) Bits = 4 bytes const int GPS_WORD_LENGTH = 4; //!< CRC + GPS WORD (-2 -1 0 ... 29) Bits = 4 bytes
const int GPS_SUBFRAME_LENGTH = 40; // GPS_WORD_LENGTH x 10 = 40 bytes const int GPS_SUBFRAME_LENGTH = 40; //!< GPS_WORD_LENGTH x 10 = 40 bytes
const int GPS_SUBFRAME_BITS = 300; //!< Number of bits per subframe in the NAV message [bits] const int GPS_SUBFRAME_BITS = 300; //!< Number of bits per subframe in the NAV message [bits]
const int GPS_SUBFRAME_SECONDS = 6; //!< Subframe duration [seconds] const int GPS_SUBFRAME_SECONDS = 6; //!< Subframe duration [seconds]
const int GPS_WORD_BITS = 30; //!< Number of bits per word in the NAV message [bits] const int GPS_WORD_BITS = 30; //!< Number of bits per word in the NAV message [bits]
// GPS NAVIGATION MESSAGE STRUCTURE // GPS NAVIGATION MESSAGE STRUCTURE
// NAVIGATION MESSAGE FIELDS POSITIONS (from IS-GPS-200E Appendix II) // NAVIGATION MESSAGE FIELDS POSITIONS (from IS-GPS-200E Appendix II)
@@ -92,7 +90,6 @@ const std::vector<std::pair<int,int> > ALERT_FLAG({{48,1}});
const std::vector<std::pair<int,int> > ANTI_SPOOFING_FLAG({{49,1}}); const std::vector<std::pair<int,int> > ANTI_SPOOFING_FLAG({{49,1}});
const std::vector<std::pair<int,int> > SUBFRAME_ID({{50,3}}); const std::vector<std::pair<int,int> > SUBFRAME_ID({{50,3}});
// SUBFRAME 1 // SUBFRAME 1
const std::vector<std::pair<int,int>> GPS_WEEK({{61,10}}); const std::vector<std::pair<int,int>> GPS_WEEK({{61,10}});
const std::vector<std::pair<int,int>> CA_OR_P_ON_L2({{71,2}}); //* const std::vector<std::pair<int,int>> CA_OR_P_ON_L2({{71,2}}); //*
@@ -101,11 +98,9 @@ const std::vector<std::pair<int,int>> SV_HEALTH ({{77,6}});
const std::vector<std::pair<int,int>> L2_P_DATA_FLAG ({{91,1}}); const std::vector<std::pair<int,int>> L2_P_DATA_FLAG ({{91,1}});
const std::vector<std::pair<int,int>> T_GD({{197,8}}); const std::vector<std::pair<int,int>> T_GD({{197,8}});
const double T_GD_LSB = TWO_N31; const double T_GD_LSB = TWO_N31;
const std::vector<std::pair<int,int>> IODC({{83,2},{211,8}}); const std::vector<std::pair<int,int>> IODC({{83,2},{211,8}});
const std::vector<std::pair<int,int>> T_OC({{219,16}}); const std::vector<std::pair<int,int>> T_OC({{219,16}});
const double T_OC_LSB = TWO_P4; const double T_OC_LSB = TWO_P4;
const std::vector<std::pair<int,int>> A_F2({{241,8}}); const std::vector<std::pair<int,int>> A_F2({{241,8}});
const double A_F2_LSB = TWO_N55; const double A_F2_LSB = TWO_N55;
const std::vector<std::pair<int,int>> A_F1({{249,16}}); const std::vector<std::pair<int,int>> A_F1({{249,16}});
@@ -114,7 +109,6 @@ const std::vector<std::pair<int,int>> A_F0({{271,22}});
const double A_F0_LSB = TWO_N31; const double A_F0_LSB = TWO_N31;
// SUBFRAME 2 // SUBFRAME 2
const std::vector<std::pair<int,int>> IODE_SF2({{61,8}}); const std::vector<std::pair<int,int>> IODE_SF2({{61,8}});
const std::vector<std::pair<int,int>> C_RS({{69,16}}); const std::vector<std::pair<int,int>> C_RS({{69,16}});
const double C_RS_LSB = TWO_N5; const double C_RS_LSB = TWO_N5;
@@ -137,7 +131,6 @@ const std::vector<std::pair<int,int>> AODO({{272,5}});
const int AODO_LSB = 900; const int AODO_LSB = 900;
// SUBFRAME 3 // SUBFRAME 3
const std::vector<std::pair<int,int>> C_IC({{61,16}}); const std::vector<std::pair<int,int>> C_IC({{61,16}});
const double C_IC_LSB = TWO_N29; const double C_IC_LSB = TWO_N29;
const std::vector<std::pair<int,int>> OMEGA_0({{77,8},{91,24}}); const std::vector<std::pair<int,int>> OMEGA_0({{77,8},{91,24}});
@@ -158,14 +151,11 @@ const double I_DOT_LSB = PI_TWO_N43;
// SUBFRAME 4-5 // SUBFRAME 4-5
const std::vector<std::pair<int,int>> SV_DATA_ID({{61,2}}); const std::vector<std::pair<int,int>> SV_DATA_ID({{61,2}});
const std::vector<std::pair<int,int>> SV_PAGE({{63,6}}); const std::vector<std::pair<int,int>> SV_PAGE({{63,6}});
// SUBFRAME 4 // SUBFRAME 4
//! \todo read all pages of subframe 4 //! \todo read all pages of subframe 4
// Page 18 - Ionospheric and UTC data // Page 18 - Ionospheric and UTC data
const std::vector<std::pair<int,int>> ALPHA_0({{69,8}}); const std::vector<std::pair<int,int>> ALPHA_0({{69,8}});
const double ALPHA_0_LSB = TWO_N30; const double ALPHA_0_LSB = TWO_N30;
@@ -211,13 +201,9 @@ const std::vector<std::pair<int,int>> HEALTH_SV31({{277,6}});
const std::vector<std::pair<int,int>> HEALTH_SV32({{283,6}}); const std::vector<std::pair<int,int>> HEALTH_SV32({{283,6}});
// SUBFRAME 5 // SUBFRAME 5
//! \todo read all pages of subframe 5 //! \todo read all pages of subframe 5
// page 25 - Health (PRN 1 - 24) // page 25 - Health (PRN 1 - 24)
const std::vector<std::pair<int,int>> T_OA({{69,8}}); const std::vector<std::pair<int,int>> T_OA({{69,8}});
const double T_OA_LSB = TWO_P12; const double T_OA_LSB = TWO_P12;
@@ -247,74 +233,4 @@ const std::vector<std::pair<int,int>> HEALTH_SV22({{247,6}});
const std::vector<std::pair<int,int>> HEALTH_SV23({{253,6}}); const std::vector<std::pair<int,int>> HEALTH_SV23({{253,6}});
const std::vector<std::pair<int,int>> HEALTH_SV24({{259,6}}); const std::vector<std::pair<int,int>> HEALTH_SV24({{259,6}});
/*
inline void ca_code_generator_complex(std::complex<float>* _dest, signed int _prn, unsigned int _chip_shift)
{
unsigned int G1[1023];
unsigned int G2[1023];
unsigned int G1_register[10], G2_register[10];
unsigned int feedback1, feedback2;
unsigned int lcv, lcv2;
unsigned int delay;
signed int prn = _prn-1; //Move the PRN code to fit an array indices
// G2 Delays as defined in IS-GPS-200E
signed int delays[32] = {5, 6, 7, 8, 17, 18, 139, 140, 141, 251,
252, 254, 255, 256, 257, 258, 469, 470, 471, 472,
473, 474, 509, 512, 513, 514, 515, 516, 859, 860,
861, 862};
// PRN sequences 33 through 37 are reserved for other uses (e.g. ground transmitters)
// A simple error check
if((prn < 0) || (prn > 32))
return;
for(lcv = 0; lcv < 10; lcv++)
{
G1_register[lcv] = 1;
G2_register[lcv] = 1;
}
// Generate G1 & G2 Register
for(lcv = 0; lcv < 1023; lcv++)
{
G1[lcv] = G1_register[0];
G2[lcv] = G2_register[0];
feedback1 = G1_register[7]^G1_register[0];
feedback2 = (G2_register[8] + G2_register[7] + G2_register[4] + G2_register[2] + G2_register[1] + G2_register[0]) & 0x1;
for(lcv2 = 0; lcv2 < 9; lcv2++)
{
G1_register[lcv2] = G1_register[lcv2 + 1];
G2_register[lcv2] = G2_register[lcv2 + 1];
}
G1_register[9] = feedback1;
G2_register[9] = feedback2;
}
// Set the delay
delay = 1023 - delays[prn];
delay += _chip_shift;
delay %= 1023;
// Generate PRN from G1 and G2 Registers
for(lcv = 0; lcv < 1023; lcv++)
{
_dest[lcv] = std::complex<float>(G1[(lcv + _chip_shift)%1023]^G2[delay], 0);
if(_dest[lcv].real() == 0.0) //javi
{
_dest[lcv].real(-1.0);
}
delay++;
delay %= 1023;
//std::cout<<_dest[lcv].real(); //OK
}
}
*/
#endif /* GNSS_SDR_GPS_L1_CA_H_ */ #endif /* GNSS_SDR_GPS_L1_CA_H_ */