mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-15 07:13:17 +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
Show Stats 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
* \brief Interface of an adapter of a GALILEO E1 PVT solver block to a
* PvtInterface
* Position Velocity and Time
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* PvtInterface.
* \author Javier Arribas, 2013. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
@ -43,12 +41,12 @@
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
{
public:
GalileoE1Pvt(ConfigurationInterface* configuration,
GalileoE1Pvt(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams,
@ -61,7 +59,7 @@ public:
return role_;
}
//! Returns "GPS_L1_CA_PVT"
//! Returns "GALILEO_E1_PVT"
std::string implementation()
{
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
* \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
* 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) :
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::block("galileo_e1_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(gr_complex)))
{
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_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_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
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)
{
@ -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 ####
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)
if (global_galileo_ephemeris_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);
d_ls_pvt->galileo_ephemeris_map = global_galileo_ephemeris_map.get_map_copy();
}
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
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 ################################
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
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);
if (pvt_result==true)
if (pvt_result == true)
{
d_kml_dump.print_position_galileo(d_ls_pvt, d_flag_averaging);
//ToDo: Implement Galileo RINEX and Galileo NMEA outputs
// d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
//
// if (!b_rinex_header_writen) // & we have utc data in nav message!
// {
// std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
// 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_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
// }
// }
// 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
// // Notice that d_sample_counter period is 1ms (for GPS correlators)
// if ((d_sample_counter-d_last_sample_nav_output)>=6000)
// {
// rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map);
// d_last_sample_nav_output=d_sample_counter;
// }
// std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
// 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);
// }
// }
//ToDo: Implement Galileo RINEX and Galileo NMEA outputs
// d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging);
//
// if (!b_rinex_header_writen) // & we have utc data in nav message!
// {
// std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
// 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_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
// }
// }
// 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
// // Notice that d_sample_counter period is 1ms (for GPS correlators)
// if ((d_sample_counter-d_last_sample_nav_output)>=6000)
// {
// rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map);
// d_last_sample_nav_output=d_sample_counter;
// }
// std::map<int,Gps_Ephemeris>::iterator gps_ephemeris_iter;
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin();
// 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);
// }
// }
}
}
@ -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 <<
" 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)
{
try
{
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;
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 "Galileo_E1.h"
class galileo_e1_pvt_cc;
typedef boost::shared_ptr<galileo_e1_pvt_cc> galileo_e1_pvt_cc_sptr;
galileo_e1_pvt_cc_sptr
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);
galileo_e1_pvt_cc_sptr 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);
/*!
* \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
{
private:
friend galileo_e1_pvt_cc_sptr
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);
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);
friend galileo_e1_pvt_cc_sptr 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);
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;
bool d_dump;
bool b_rinex_header_writen;
@ -81,8 +107,8 @@ private:
Kml_Printer d_kml_dump;
Nmea_Printer *d_nmea_printer;
double d_rx_time;
galileo_e1_ls_pvt *d_ls_pvt; /*modify PVT/libs/galileo_e1_ls_pvt*/
bool pseudoranges_pairCompare_min( std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b);
galileo_e1_ls_pvt *d_ls_pvt;
bool pseudoranges_pairCompare_min(std::pair<int,Gnss_Synchro> a, std::pair<int,Gnss_Synchro> b);
public:
~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
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)
{
@ -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();
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
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
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
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);
}
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;
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);
}
@ -236,7 +236,7 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite
{
bool pvt_result;
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_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
{
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;
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;
gps_l1_ca_pvt_cc_sptr
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);
gps_l1_ca_pvt_cc_sptr 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);
/*!
* \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
{
private:
friend gps_l1_ca_pvt_cc_sptr
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);
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);
friend gps_l1_ca_pvt_cc_sptr 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);
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;
bool d_dump;
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
* (PVT) solver, based on K.Borre's Matlab receiver.
* \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
* Satellite Systems receiver
@ -30,7 +31,6 @@
#include <armadillo>
#include "galileo_e1_ls_pvt.h"
#include "Galileo_E1.h"
#include <glog/log_severity.h>
#include <glog/logging.h>
@ -39,7 +39,7 @@
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
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);
az = 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::vec Rot_X;
double rho2;
@ -178,22 +168,23 @@ arma::vec galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arm
{
//--- Update equations -----------------------------------------
rho2 = (X(0, i) - pos(0)) *
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
(X(1, i) - pos(1)) + (X(2,i) - pos(2)) *
(X(2,i) - pos(2));
(X(0, i) - pos(0)) + (X(1, i) - pos(1)) *
(X(1, i) - pos(1)) + (X(2, i) - pos(2)) *
(X(2, i) - pos(2));
traveltime = sqrt(rho2) / GALILEO_C_m_s;
//--- Correct satellite position (do to earth rotation) --------
Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo
//--- Find DOA and range of satellites
topocent(&d_visible_satellites_Az[i], &d_visible_satellites_El[i],
&d_visible_satellites_Distance[i], pos.subvec(0,2), Rot_X - pos.subvec(0,2));
//[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :));
topocent(&d_visible_satellites_Az[i],
&d_visible_satellites_El[i],
&d_visible_satellites_Distance[i],
pos.subvec(0,2),
Rot_X - pos.subvec(0, 2));
}
//--- 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 ---------------------------------------
//Armadillo
@ -208,7 +199,7 @@ arma::vec galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arm
//--- Apply position update --------------------------------------------
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)
}
@ -217,12 +208,11 @@ arma::vec galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arm
try
{
//-- 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)
{
d_Q=arma::zeros(4,4);
d_Q = arma::zeros(4,4);
}
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;
int valid_pseudoranges = gnss_pseudoranges_map.size();
arma::mat W = arma::eye(valid_pseudoranges,valid_pseudoranges); //channels weights matrix
arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
arma::mat satpos = arma::zeros(3,valid_pseudoranges); //satellite positions matrix
arma::mat W = arma::eye(valid_pseudoranges, valid_pseudoranges); //channels weights matrix
arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector
arma::mat satpos = arma::zeros(3, valid_pseudoranges); //satellite positions matrix
int Galileo_week_number = 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 SV_clock_bias_s = 0;
//double GST=0;
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++)
{
// 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())
{
/*!
@ -277,7 +265,6 @@ bool galileo_e1_ls_pvt::get_PVT(std::map<int,Gnss_Synchro> gnss_pseudoranges_map
//JAVIER VERSION:
double Rx_time = galileo_current_time;
//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;
@ -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(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;
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;
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
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);
// get time string gregorian calendar
//std::cout<<"UTC_raw="<<utc<<std::endl;
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)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
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
// SV ECEF DEBUG OUTPUT
DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(obs_counter) << " [m]" << std::endl;
<< " X=" << galileo_ephemeris_iter->second.d_satpos_X
<< " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y
<< " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z
<< " [m] PR_obs=" << obs(obs_counter) << " [m]";
}
else // the ephemeris are not available for this SV
{
// no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated
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++;
}
//
// // ********************************************************************************
// // ****** SOLVE LEAST SQUARES******************************************************
// // ********************************************************************************
// ********************************************************************************
// ****** SOLVE LEAST SQUARES******************************************************
// ********************************************************************************
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)
{
arma::vec mypos;
DLOG(INFO) << "satpos=" << satpos << std::endl;
DLOG(INFO) << "obs="<< obs << std::endl;
DLOG(INFO) << "W=" << W <<std::endl;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs="<< obs;
DLOG(INFO) << "W=" << W;
mypos = leastSquarePos(satpos, obs, W);
// 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);
// get time string gregorian calendar
// get time string Gregorian calendar
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)
boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t);
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);
//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)
{
b_valid_position=false;
return false; //erratic PVT
}
if (d_height_m > 50000)
{
b_valid_position = false;
return false;
}
DLOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time)
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]" << std::endl;
<< " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d
<< " [deg], Height= " << d_height_m << " [m]";
// ###### Compute DOPs ########
// 1- Rotation matrix from ECEF coordinates to 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(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));
F(1,0) = cos((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(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(1,0)=cos((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(1,2)=cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(2,0) = 0;
F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0));
F(2,0)=0;
F(2,1)=cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(2,2)=sin((GPS_TWO_PI*d_latitude_d/360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
arma::mat DOP_ENU = arma::zeros(3, 3);
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF=d_Q.submat( 0, 0, 2, 2);
arma::mat DOP_ENU=arma::zeros(3,3);
try
{
DOP_ENU=arma::htrans(F)*Q_ECEF*F;
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
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
d_TDOP = sqrt(d_Q(3,3)); // TDOP
}catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
try
{
DOP_ENU = arma::htrans(F)*Q_ECEF*F;
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
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
d_TDOP = sqrt(d_Q(3,3)); // TDOP
}
catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
// ######## LOG FILE #########
if(d_flag_dump_enabled == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
// PVT GPS time
// PVT GPS time
tmp_double = galileo_current_time;
d_dump_file.write((char*)&tmp_double, sizeof(double));
// 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_longitude_d = 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_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
{
//int current_depth=d_hist_longitude_d.size();
// int current_depth=d_hist_longitude_d.size();
// Push new values
d_hist_longitude_d.push_front(d_longitude_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;
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 f[5] = {1/297, 1/298.247, 1/298.26, 1/298.257222101, 1/298.257223563};
double lambda = atan2(Y,X);
double ex2 = (2 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1 - f[elipsoid_selection])*(1 -f[elipsoid_selection]));
double lambda = atan2(Y, X);
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 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;
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;
iterations = iterations + 1;
if (iterations > 100)
@ -579,7 +561,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
*h = 0;
double tolsq = 1.e-10; // tolerance to accept convergence
int maxit = 10; // max number of iterations
double rtd = 180/GPS_PI;
double rtd = 180/GPS_PI;
// compute square of eccentricity
double esq;
@ -593,22 +575,24 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
}
// first guess
double P = sqrt(X*X + Y*Y); // P is distance from spin axis
//direct calculation of longitude
if (P > 1.0E-20)
{
*dlambda = atan2(Y,X) * rtd;
*dlambda = atan2(Y, X) * rtd;
}
else
{
*dlambda = 0;
}
// correct longitude bound
if (*dlambda < 0)
{
*dlambda = *dlambda + 360.0;
}
double r = sqrt(P*P + Z*Z); // r is distance from origin (0,0,0)
double sinphi;
@ -630,7 +614,7 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
return;
}
*h = r - a*(1-sinphi*sinphi/finv);
*h = r - a*(1 - sinphi*sinphi/finv);
// iterate
double cosphi;
@ -639,28 +623,28 @@ void galileo_e1_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double
double dZ;
double oneesq = 1 - esq;
for (int i=0; i<maxit; i++)
for (int i = 0; i < maxit; i++)
{
sinphi = sin(*dphi);
cosphi = cos(*dphi);
sinphi = sin(*dphi);
cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq*sinphi*sinphi);
// compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq*sinphi*sinphi);
// compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
// update height and latitude
*h = *h + (sinphi*dZ + cosphi*dP);
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
*h = *h + (sinphi*dZ + cosphi*dP);
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
// test for convergence
if ((dP*dP + dZ*dZ) < tolsq)
{
break;
}
if (i == (maxit-1))
if (i == (maxit - 1))
{
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
*/
double lambda;
double phi;
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
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
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;
}
*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)
* solver, based on K.Borre's Matlab receiver.
* \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
* Satellite Systems receiver
@ -38,7 +39,6 @@
#include <stdio.h>
#include <sys/time.h>
#include <time.h>
//#include <math.h>
#include <cmath>
#include <map>
#include <algorithm>
@ -46,10 +46,10 @@
#include "galileo_navigation_message.h"
#include "armadillo"
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
#include "galileo_ephemeris.h"
#include "galileo_utc_model.h"
#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 togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
public:
int d_nchannels; //! Number of available channels for positioning
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
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_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
int d_nchannels; //!< Number of available channels for positioning
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
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_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
Galileo_Navigation_Message* d_ephemeris;
// new ephemeris storage
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map;
// new utc_model storage
std::map<int,Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
Galileo_Utc_Model galileo_utc_model;
// new iono storage
Galileo_Iono galileo_iono;
double d_galileo_current_time;
@ -85,26 +82,24 @@ public:
bool b_valid_position;
double d_latitude_d; //! Latitude in degrees
double d_longitude_d; //! Longitude in degrees
double d_height_m; //! Height [m]
double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //!< Longitude in degrees
double d_height_m; //!< Height [m]
//averaging
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
int d_averaging_depth; //! Length of averaging window
double d_avg_latitude_d; //! Averaged latitude in degrees
double d_avg_longitude_d; //! Averaged longitude in degrees
double d_avg_height_m; //! Averaged height [m]
int d_averaging_depth; //!< Length of averaging window
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
double d_x_m;
double d_y_m;
double d_z_m;
// DOP estimations
arma::mat d_Q;
double d_GDOP;
double d_PDOP;
@ -121,6 +116,7 @@ public:
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();
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);
az = 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::vec Rot_X;
double rho2;
@ -207,7 +197,7 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
//--- Apply position update --------------------------------------------
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)
}
@ -216,12 +206,11 @@ arma::vec gps_l1_ca_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma
try
{
//-- 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)
{
d_Q=arma::zeros(4,4);
d_Q = arma::zeros(4, 4);
}
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;
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::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;
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;
gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s);
satpos(0,obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1,obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2,obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
satpos(0, obs_counter) = gps_ephemeris_iter->second.d_satpos_X;
satpos(1, obs_counter) = gps_ephemeris_iter->second.d_satpos_Y;
satpos(2, obs_counter) = gps_ephemeris_iter->second.d_satpos_Z;
// 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;
@ -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
<< " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y
<< " [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)
GPS_week = gps_ephemeris_iter->second.i_GPS_week;
utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week);
}
else // the ephemeris are not available for this SV
{
// no valid pseudorange for the current SV
W(obs_counter, obs_counter) = 0; // SV de-activated
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first << std::endl;
obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero)
DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first;
}
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******************************************************
// ********************************************************************************
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)
{
arma::vec mypos;
DLOG(INFO) << "satpos=" << satpos << std::endl;
DLOG(INFO) << "obs="<< obs << std::endl;
DLOG(INFO) << "W=" << W <<std::endl;
DLOG(INFO) << "satpos=" << satpos;
DLOG(INFO) << "obs=" << obs;
DLOG(INFO) << "W=" << 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);
//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;
return false; //erratic PVT
b_valid_position = false;
return false;
}
// Compute UTC time and print PVT solution
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)
<< " 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 ########
// 1- Rotation matrix from ECEF coordinates to 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(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));
F(1,0) = cos((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(1,2) = cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(1,0)=cos((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(1,2)=cos((GPS_TWO_PI*d_latitude_d/360.0))*sin((GPS_TWO_PI*d_longitude_d)/360.0);
F(2,0) = 0;
F(2,1) = cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(2,2) = sin((GPS_TWO_PI*d_latitude_d/360.0));
F(2,0)=0;
F(2,1)=cos((GPS_TWO_PI*d_latitude_d)/360.0);
F(2,2)=sin((GPS_TWO_PI*d_latitude_d/360.0));
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2);
arma::mat DOP_ENU = arma::zeros(3, 3);
// 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS)
arma::mat Q_ECEF=d_Q.submat( 0, 0, 2, 2);
arma::mat DOP_ENU=arma::zeros(3,3);
try
{
DOP_ENU=arma::htrans(F)*Q_ECEF*F;
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
d_HDOP = sqrt(DOP_ENU(0,0) + DOP_ENU(1,1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2,2)); // VDOP
d_TDOP = sqrt(d_Q(3,3)); // TDOP
}catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
try
{
DOP_ENU = arma::htrans(F)*Q_ECEF*F;
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
d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1)); // HDOP
d_VDOP = sqrt(DOP_ENU(2, 2)); // VDOP
d_TDOP = sqrt(d_Q(3, 3)); // TDOP
}
catch(std::exception& ex)
{
d_GDOP = -1; // Geometric DOP
d_PDOP = -1; // PDOP
d_HDOP = -1; // HDOP
d_VDOP = -1; // VDOP
d_TDOP = -1; // TDOP
}
// ######## LOG FILE #########
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)
{
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_longitude_d = 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_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};
double lambda = atan2(Y,X);
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 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 phi = atan(Z / ((sqrt(X*X + Y*Y)*(1 - (2 - f[elipsoid_selection])) * f[elipsoid_selection])));
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;
double tolsq = 1.e-10; // tolerance to accept convergence
int maxit = 10; // max number of iterations
double rtd = 180/GPS_PI;
double rtd = 180/GPS_PI;
// compute square of eccentricity
double esq;
@ -611,28 +598,28 @@ void gps_l1_ca_ls_pvt::togeod(double *dphi, double *dlambda, double *h, double a
double dZ;
double oneesq = 1 - esq;
for (int i=0; i<maxit; i++)
for (int i = 0; i < maxit; i++)
{
sinphi = sin(*dphi);
cosphi = cos(*dphi);
sinphi = sin(*dphi);
cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq*sinphi*sinphi);
// compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq*sinphi*sinphi);
// compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
// compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi*oneesq + (*h)) * sinphi;
// update height and latitude
*h = *h + (sinphi*dZ + cosphi*dP);
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
// update height and latitude
*h = *h + (sinphi*dZ + cosphi*dP);
*dphi = *dphi + (cosphi*dZ - sinphi*dP)/(N_phi + (*h));
// test for convergence
if ((dP*dP + dZ*dZ) < tolsq)
{
break;
}
if (i == (maxit-1))
if (i == (maxit - 1))
{
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
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
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;
}
*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 "armadillo"
#include "boost/date_time/posix_time/posix_time.hpp"
#include "gnss_synchro.h"
#include "gps_ephemeris.h"
#include "gps_utc_model.h"
#include "sbas_telemetry_data.h"
#include "sbas_ionospheric_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 togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
public:
int d_nchannels; //! Number of available channels for positioning
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
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_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
int d_nchannels; //!< Number of available channels for positioning
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
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_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
Gps_Navigation_Message* d_ephemeris;
// new ephemeris storage
std::map<int,Gps_Ephemeris> gps_ephemeris_map;
// new utc_model storage
std::map<int,Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new Gps_Ephemeris
Gps_Utc_Model gps_utc_model;
// new iono storage
Gps_Iono gps_iono;
// new SBAS storage
Sbas_Ionosphere_Correction sbas_iono;
std::map<int,Sbas_Satellite_Correction> sbas_sat_corr_map;
std::map<int,Sbas_Ephemeris> sbas_ephemeris_map;
@ -94,26 +90,24 @@ public:
bool b_valid_position;
double d_latitude_d; //! Latitude in degrees
double d_longitude_d; //! Longitude in degrees
double d_height_m; //! Height [m]
double d_latitude_d; //!< Latitude in degrees
double d_longitude_d; //!< Longitude in degrees
double d_height_m; //!< Height [m]
//averaging
std::deque<double> d_hist_latitude_d;
std::deque<double> d_hist_longitude_d;
std::deque<double> d_hist_height_m;
int d_averaging_depth; //! Length of averaging window
double d_avg_latitude_d; //! Averaged latitude in degrees
double d_avg_longitude_d; //! Averaged longitude in degrees
double d_avg_height_m; //! Averaged height [m]
int d_averaging_depth; //!< Length of averaging window
double d_avg_latitude_d; //!< Averaged latitude in degrees
double d_avg_longitude_d; //!< Averaged longitude in degrees
double d_avg_height_m; //!< Averaged height [m]
double d_x_m;
double d_y_m;
double d_z_m;
// DOP estimations
arma::mat d_Q;
double d_GDOP;
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
{
public:
GpsL1CaPcpsAssistedAcquisition(ConfigurationInterface* configuration,
GpsL1CaPcpsAssistedAcquisition(ConfigurationInterface* configuration,
std::string role, unsigned int in_streams,
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:
friend pcps_acquisition_cc_sptr
pcps_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
pcps_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset);
long d_fs_in;
long d_freq;
int d_samples_per_ms;
long d_fs_in;
long d_freq;
int d_samples_per_ms;
int d_samples_per_code;
unsigned int d_doppler_resolution;
float d_threshold;
unsigned int d_doppler_resolution;
float d_threshold;
std::string d_satellite_str;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_max_dwells;
unsigned int d_well_count;
unsigned int d_fft_size;
unsigned long int d_sample_counter;
unsigned int d_fft_size;
unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins;
gr_complex* d_fft_codes;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft;
gr_complex* d_fft_codes;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
float d_input_power;
float d_test_statistics;
float d_input_power;
float d_test_statistics;
bool d_bit_transition_flag;
gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
int d_state;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
public:
/*!
* \brief Default destructor.
*/
~pcps_acquisition_cc();
~pcps_acquisition_cc();
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code.
*/
void set_local_code(std::complex<float> * code);
/*!
* \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code.
*/
void set_local_code(std::complex<float> * code);
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int doppler_step)
{
d_doppler_step = doppler_step;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int 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.
* \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 Parallel Code Phase Search 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);
/*!
* \brief Parallel Code Phase Search 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_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;
}
void pcps_assisted_acquisition_cc::set_doppler_step(unsigned int doppler_step)
{
d_doppler_step = doppler_step;
}
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_doppler_wipeoffs[i];
}
delete d_grid_data;
}
pcps_assisted_acquisition_cc::~pcps_assisted_acquisition_cc()
{
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_samplestamp_samples = 0;
d_input_power = 0.0;
d_state = 0;
d_fft_if->execute(); // We need the FFT of local code
//Conjugate the local code
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,
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()
{
Gps_Acq_Assist gps_acq_assisistance;
@ -175,7 +182,7 @@ void pcps_assisted_acquisition_cc::get_assistance()
}
this->d_disable_assist = false;
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
{
@ -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;
}
}
void pcps_assisted_acquisition_cc::reset_grid()
{
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;
}
}
}
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_min = d_config_doppler_min;
@ -204,8 +217,8 @@ void pcps_assisted_acquisition_cc::redefine_grid()
// Create the search grid array
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];
for (int i=0; i<d_num_doppler_points; i++)
d_grid_data = new float*[d_num_doppler_points];
for (int i = 0; i < d_num_doppler_points; i++)
{
d_grid_data[i] = new float[d_fft_size];
}
@ -214,18 +227,19 @@ void pcps_assisted_acquisition_cc::redefine_grid()
int doppler_hz;
float phase_step_rad;
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
// compute the carrier doppler wipe-off signal and store it
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];
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()
{
float magt = 0.0;
@ -274,6 +288,8 @@ double pcps_assisted_acquisition_cc::search_maximum()
return d_test_statistics;
}
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
@ -289,6 +305,8 @@ float pcps_assisted_acquisition_cc::estimate_input_power(gr_vector_const_void_st
return ( power / (float)d_fft_size);
}
int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_void_star &input_items)
{
// initialize acquisition algorithm
@ -305,7 +323,7 @@ int pcps_assisted_acquisition_cc::compute_and_accumulate_grid(gr_vector_const_vo
float* p_tmp_vector;
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
// 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);
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);
}
free(p_tmp_vector);
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_void_star &output_items)
{
/*!
* TODO: High sensitivity acquisition algorithm:
* State Mechine:
@ -355,7 +371,7 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items,
switch (d_state)
{
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
consume_each(ninput_items[0]);
break;
@ -371,7 +387,7 @@ int pcps_assisted_acquisition_cc::general_work(int noutput_items,
int consumed_samples;
consumed_samples = compute_and_accumulate_grid(input_items);
d_well_count++;
if (d_well_count>=d_max_dwells)
if (d_well_count >= d_max_dwells)
{
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) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL
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) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power;
d_active = false;
// Send message to channel queue //0=STOP_CHANNEL 1=ACQ_SUCCESS 2=ACQ_FAIL
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_
#define GNSS_SDR_PCPS_assisted_acquisition_cc_H_
#ifndef GNSS_SDR_PCPS_ASSISTED_ACQUISITION_CC_H_
#define GNSS_SDR_PCPS_ASSISTED_ACQUISITION_CC_H_
#include <fstream>
#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",
* Algorithm 1, for a pseudocode description of this implementation.
*/
class pcps_assisted_acquisition_cc: public gr::block
{
private:
@ -223,7 +222,6 @@ public:
*/
void set_doppler_step(unsigned int doppler_step);
/*!
* \brief Set tracking channel internal queue.
* \param channel_internal_queue - Channel's internal blocks information queue.
@ -241,7 +239,6 @@ public:
gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
};
#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:
friend pcps_cccwsr_acquisition_cc_sptr
pcps_cccwsr_make_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
pcps_cccwsr_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset);
long d_fs_in;
long d_freq;
int d_samples_per_ms;
long d_fs_in;
long d_freq;
int d_samples_per_ms;
int d_samples_per_code;
unsigned int d_doppler_resolution;
float d_threshold;
unsigned int d_doppler_resolution;
float d_threshold;
std::string d_satellite_str;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_max_dwells;
unsigned int d_well_count;
unsigned int d_fft_size;
unsigned long int d_sample_counter;
unsigned int d_fft_size;
unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins;
gr_complex* d_fft_code_data;
gr_complex* d_fft_code_pilot;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
gr_complex* d_data_correlation;
gr_complex* d_pilot_correlation;
gr_complex* d_correlation_plus;
gr_complex* d_correlation_minus;
float d_input_power;
float d_test_statistics;
float d_test_statistics;
gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
int d_state;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
public:
/*!
* \brief Default destructor.
*/
~pcps_cccwsr_acquisition_cc();
~pcps_cccwsr_acquisition_cc();
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Sets local code for CCCWSR acquisition algorithm.
* \param data_code - Pointer to the data 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);
/*!
* \brief Sets local code for CCCWSR acquisition algorithm.
* \param data_code - Pointer to the data 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);
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set statistics threshold of CCCWSR algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set statistics threshold of CCCWSR algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int doppler_step)
{
d_doppler_step = doppler_step;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int 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.
* \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 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);
/*!
* \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_*/

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()
{
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)
{
//get all platforms (drivers)
@ -252,11 +256,11 @@ int pcps_opencl_acquisition_cc::init_opencl_environment(std::string kernel_filen
d_cl_program = program;
// 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_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_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_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_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_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.
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;
}
void pcps_opencl_acquisition_cc::init()
{
d_gnss_synchro->Acq_delay_samples = 0.0;
@ -669,13 +675,13 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
d_core_working = false;
}
int pcps_opencl_acquisition_cc::general_work(int noutput_items,
gr_vector_int &ninput_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items)
{
int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
switch (d_state)
{
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
// consecutive signal blocks will be processed in multi-dwell operation. This is
// 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++)
{
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)
{
d_sample_counter += d_fft_size * (ninput_items[0]-num_dwells);
d_sample_counter += d_fft_size * (ninput_items[0] - num_dwells);
}
}
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()).
// 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!!
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;
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:
friend pcps_opencl_acquisition_cc_sptr
pcps_make_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
pcps_opencl_acquisition_cc(unsigned int sampled_ms, unsigned int max_dwells,
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
unsigned int doppler_max, long freq, long fs_in,
int samples_per_ms, int samples_per_code,
bool bit_transition_flag,
gr::msg_queue::sptr queue, bool dump,
std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset);
int init_opencl_environment(std::string kernel_filename);
long d_fs_in;
long d_freq;
int d_samples_per_ms;
long d_fs_in;
long d_freq;
int d_samples_per_ms;
int d_samples_per_code;
unsigned int d_doppler_resolution;
float d_threshold;
unsigned int d_doppler_resolution;
float d_threshold;
std::string d_satellite_str;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_max_dwells;
unsigned int d_well_count;
unsigned int d_fft_size;
unsigned int d_fft_size;
unsigned int d_fft_size_pow2;
int* d_max_doppler_indexs;
unsigned long int d_sample_counter;
unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins;
gr_complex* d_fft_codes;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft;
gr_complex* d_fft_codes;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
float d_input_power;
float d_test_statistics;
float d_input_power;
float d_test_statistics;
bool d_bit_transition_flag;
gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
int d_state;
bool d_core_working;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
unsigned int d_channel;
std::string d_dump_filename;
gr_complex* d_zero_vector;
gr_complex** d_in_buffer;
std::vector<unsigned long int> d_sample_counter_buffer;
@ -174,104 +173,104 @@ public:
/*!
* \brief Default destructor.
*/
~pcps_opencl_acquisition_cc();
~pcps_opencl_acquisition_cc();
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code.
*/
void set_local_code(std::complex<float> * code);
/*!
* \brief Sets local code for PCPS acquisition algorithm.
* \param code - Pointer to the PRN code.
*/
void set_local_code(std::complex<float> * code);
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set statistics threshold of PCPS algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int doppler_step)
{
d_doppler_step = doppler_step;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int 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.
* \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 Parallel Code Phase Search 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);
/*!
* \brief Parallel Code Phase Search 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);
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:
friend pcps_tong_acquisition_cc_sptr
pcps_tong_make_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, gr::msg_queue::sptr queue,
bool dump, std::string dump_filename);
long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, gr::msg_queue::sptr queue,
bool dump, std::string dump_filename);
pcps_tong_acquisition_cc(unsigned int sampled_ms, unsigned int doppler_max,
long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, gr::msg_queue::sptr queue,
bool dump, std::string dump_filename);
long freq, long fs_in, int samples_per_ms,
int samples_per_code, unsigned int tong_init_val,
unsigned int tong_max_val, gr::msg_queue::sptr queue,
bool dump, std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset);
long d_fs_in;
long d_freq;
int d_samples_per_ms;
long d_fs_in;
long d_freq;
int d_samples_per_ms;
int d_samples_per_code;
unsigned int d_doppler_resolution;
float d_threshold;
unsigned int d_doppler_resolution;
float d_threshold;
std::string d_satellite_str;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_doppler_max;
unsigned int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_well_count;
unsigned int d_tong_count;
unsigned int d_tong_init_val;
unsigned int d_tong_max_val;
unsigned int d_fft_size;
unsigned long int d_sample_counter;
unsigned int d_fft_size;
unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
unsigned int d_num_doppler_bins;
gr_complex* d_fft_codes;
float** d_grid_data;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft;
gr::fft::fft_complex* d_fft_if;
gr::fft::fft_complex* d_ifft;
Gnss_Synchro *d_gnss_synchro;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
unsigned int d_code_phase;
float d_doppler_freq;
float d_mag;
float* d_magnitude;
float d_input_power;
float d_test_statistics;
float d_input_power;
float d_test_statistics;
gr::msg_queue::sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
concurrent_queue<int> *d_channel_internal_queue;
std::ofstream d_dump_file;
bool d_active;
int d_state;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
public:
/*!
* \brief Default destructor.
*/
~pcps_tong_acquisition_cc();
~pcps_tong_acquisition_cc();
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to exchange synchronization data between acquisition and tracking blocks.
* \param p_gnss_synchro Satellite information shared by the processing blocks.
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_gnss_synchro = p_gnss_synchro;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Returns the maximum peak of grid search.
*/
unsigned int mag()
{
return d_mag;
}
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Initializes acquisition algorithm.
*/
void init();
/*!
* \brief Sets local code for TONG acquisition algorithm.
* \param code - Pointer to the PRN code.
*/
void set_local_code(std::complex<float> * code);
/*!
* \brief Sets local code for TONG acquisition algorithm.
* \param code - Pointer to the PRN code.
*/
void set_local_code(std::complex<float> * code);
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Starts acquisition algorithm, turning from standby mode to
* active mode
* \param active - bool that activates/deactivates the block.
*/
void set_active(bool active)
{
d_active = active;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set acquisition channel unique ID
* \param channel - receiver channel.
*/
void set_channel(unsigned int channel)
{
d_channel = channel;
}
/*!
* \brief Set statistics threshold of TONG algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set statistics threshold of TONG algorithm.
* \param threshold - Threshold for signal detection (check \ref Navitec2012,
* Algorithm 1, for a definition of this threshold).
*/
void set_threshold(float threshold)
{
d_threshold = threshold;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set maximum Doppler grid search
* \param doppler_max - Maximum Doppler shift considered in the grid search [Hz].
*/
void set_doppler_max(unsigned int doppler_max)
{
d_doppler_max = doppler_max;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int doppler_step)
{
d_doppler_step = doppler_step;
}
/*!
* \brief Set Doppler steps for the grid search
* \param doppler_step - Frequency bin of the search grid [Hz].
*/
void set_doppler_step(unsigned int 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.
* \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 Parallel Code Phase Search 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);
/*!
* \brief Parallel Code Phase Search 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_TONG_acquisition_cc_H_ */

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

@ -41,7 +41,7 @@
class ConfigurationInterface;
/*!
* \brief This class implements an ObservablesInterface for Galileo E1
* \brief This class implements an ObservablesInterface for Galileo E1B
*/
class GalileoE1Observables : public ObservablesInterface
{
@ -57,6 +57,7 @@ public:
return role_;
}
//! Returns "Galileo_E1B_Observables"
std::string implementation()
{
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
* \brief Implementation of the pseudorange computation block for GPS L1 C/A
* \file galileo_e1_observables_cc.cc
* \brief Implementation of the pseudorange computation block for Galileo E1
* \author Mara Branzanti 2013. mara.branzanti(at)gmail.com
* \author Javier Arribas 2013. jarribas(at)cttc.es
*
@ -92,12 +92,15 @@ galileo_e1_observables_cc::~galileo_e1_observables_cc()
d_dump_file.close();
}
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);
}
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);
@ -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,
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 **out = (Gnss_Synchro **) &output_items[0]; //Get the output 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 current_gnss_synchro[d_nchannels];
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
*/
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];
/*
* 1.2 Assume no valid pseudoranges
*/
current_gnss_synchro[i].Flag_valid_pseudorange = false;
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
{
//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]));
}
if (current_gnss_synchro[i].Flag_valid_word)
{
//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]));
}
}
/*
/*
* 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)
@ -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
* common RX time algorithm
*/
//;
// 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);
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;
//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;
}
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_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms;
//int reference_channel= gnss_synchro_iter->second.Channel_ID;
// 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)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
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;
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
for (unsigned int i=0; i<d_nchannels ; i++)
for (unsigned int i = 0; i < d_nchannels ; 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/thread.hpp>
#include "concurrent_queue.h"
#include "galileo_navigation_message.h"
#include "rinex_printer.h"
#include "Galileo_E1.h"
#include "gnss_synchro.h"
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();
}
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);
@ -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,
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 **out = (Gnss_Synchro **) &output_items[0]; //Get the output 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 current_gnss_synchro[d_nchannels];
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
*/
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
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]));
}
}
/*
* 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
* common RX time algorithm
*/
//;
// 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);
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;
//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 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="<<delta_rx_time_ms<<std::endl;
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 + GPS_STARTOFFSET_ms;
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)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
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;
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
for (unsigned int i=0; i<d_nchannels ; i++)
for (unsigned int i = 0; i < d_nchannels; 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)
{
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 g_encoder[2];
nn = 2; //Coding rate 1/n
KK = 7; //Constraint Length
nn = 2; // Coding rate 1/n
KK = 7; // Constraint Length
g_encoder[0] = 121; // Polynomial G1
g_encoder[1] = 91; // Polinomial G2
g_encoder[1] = 91; // Polynomial G2
mm = KK - 1;
max_states = 1 << mm; /* 2^mm */
max_states = 1 << mm; /* 2^mm */
DataLength = (CodeLength/nn) - mm;
/* 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( 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 );
/* Clean up memory */
free( out0 );
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
* \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,
* 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
* Satellite Systems receiver
@ -42,7 +42,6 @@
#include "gnuradio/trellis/interleaver.h"
#include "gnuradio/trellis/permutation.h"
#include "gnuradio/fec/viterbi.h"
#include "gnss_satellite.h"
#include "galileo_navigation_message.h"
#include "galileo_ephemeris.h"
@ -50,19 +49,13 @@
#include "galileo_iono.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;
typedef boost::shared_ptr<galileo_e1b_telemetry_decoder_cc> 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_telemetry_decoder_cc_sptr 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);
/*!

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)
{
int message_sample_offset =
(sample_alignment?0:-1)
(sample_alignment ? 0 : -1)
+ d_samples_per_symbol*(symbol_alignment ? -1 : 0)
+ d_samples_per_symbol * d_symbols_per_bit * it->first;
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();
/*
* 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);
private:
int d_n_smpls_in_history ;
double d_iir_par;
double d_corr_paired;
double d_corr_shifted;
bool d_aligned;
double d_past_sample;
} d_sample_aligner;
// helper class for symbol alignment and viterbi decoding
// helper class for symbol alignment and Viterbi decoding
class symbol_aligner_and_decoder
{
public:
@ -140,7 +139,6 @@ private:
Viterbi_Decoder * d_vd1;
Viterbi_Decoder * d_vd2;
double d_past_symbol;
} 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 <iostream>
#include <glog/log_severity.h>
#include <glog/logging.h>
@ -47,8 +46,8 @@
Viterbi_Decoder::Viterbi_Decoder(const int g_encoder[], const int KK, const int nn)
{
d_nn = nn; //Coding rate 1/n
d_KK = KK; //Constraint Length
d_nn = nn; // Coding rate 1/n
d_KK = KK; // Constraint Length
// derived code properties
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.
Input parameters:
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_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
state = do_traceback(traceback_depth);
// traceback and decode
decoding_length_mismatch = do_tb_and_decode(traceback_depth, nbits_requested, state, bits,
d_indicator_metric);
decoding_length_mismatch = do_tb_and_decode(traceback_depth, nbits_requested, state, bits, d_indicator_metric);
nbits_decoded = nbits_requested + decoding_length_mismatch;
VLOG(FLOW) << "decoding length mismatch (continuous decoding): "
<< decoding_length_mismatch;
VLOG(FLOW) << "decoding length mismatch (continuous decoding): " << decoding_length_mismatch;
return d_indicator_metric;
}
@ -332,8 +329,6 @@ int Viterbi_Decoder::do_tb_and_decode(int traceback_length, int requested_decodi
indicator_metric = 0;
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)
{
n_im++;

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

@ -35,6 +35,9 @@
#include <deque>
#include <iostream>
/*!
* \brief Class that implements a Viterbi decoder
*/
class Viterbi_Decoder
{
public:
@ -45,8 +48,8 @@ public:
/*!
* \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 LL The number of data bits to be decoded (does not include the mm zero-tail-bits)
* \param[in] input_c[] The received signal in LLR-form. For BPSK, must be in form r = 2*a*y/(sigma^2).
* \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)
*/

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

@ -44,12 +44,12 @@
class ConfigurationInterface;
/*!
* \brief This class implements a code DLL + carrier PLL/FLL Assisted tracking loop
*/
class GpsL1CaDllFllPllTracking : public TrackingInterface
{
public:
GpsL1CaDllFllPllTracking(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
@ -80,19 +80,13 @@ public:
void set_channel(unsigned int channel);
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
void start_tracking();
private:
gps_l1_ca_dll_fll_pll_tracking_cc_sptr tracking_;
size_t item_size_;
unsigned int channel_;
std::string role_;
unsigned int in_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 <sstream>
#include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h>
#include <glog/log_severity.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));
}
void galileo_e1_dll_pll_veml_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required)
{
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(
long if_freq,
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
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,
* having the sub-array boundaries unaligned to cache lines could lead
* 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_Very_Late, 16, sizeof(gr_complex)) == 0){};
//--- Initializations ------------------------------
// Initial code frequency basis of NCO
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;
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(); //initialize the carrier filter
d_code_loop_filter.initialize(); //initialize the code filter
d_carrier_loop_filter.initialize(); // initialize the carrier filter
d_code_loop_filter.initialize(); // initialize the code filter
// 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
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[(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[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 + 3)] = d_ca_code[3];
d_carrier_lock_fail_counter = 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;
std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0,1);
sys = sys_.substr(0, 1);
// DEBUG OUTPUT
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;
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";
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
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;
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));
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;
// 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);
//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;
//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*Galileo_E1_CODE_PERIOD;
d_rem_carr_phase_rad=fmod(d_rem_carr_phase_rad,GPS_TWO_PI);
//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;
//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 = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## DLL ##########################################################
// 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 phase accumulator
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]
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 #######################
// 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.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz;
*out[0] = current_synchro_data;
// ########## DEBUG OUTPUT
/*!
* \todo The stop timer has to be moved to the signal source!
*/
// stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream;
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{
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!
}
// stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream;
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{
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;
//if (d_channel == 0 || d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
}
}
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_L = std::abs<float>(*d_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
{

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
* \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)
@ -42,19 +37,15 @@
#include <boost/thread/thread.hpp>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
#include "concurrent_queue.h"
#include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h"
#include "correlator.h"
class 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;
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_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
* Minus Late) tracking block for Galileo E1 signals
*/
class galileo_e1_dll_pll_veml_tracking_cc: public gr::block
{
public:
~galileo_e1_dll_pll_veml_tracking_cc();
void set_channel(unsigned int channel);
@ -84,15 +73,17 @@ public:
void start_tracking();
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,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private:
friend galileo_e1_dll_pll_veml_tracking_cc_sptr
galileo_e1_dll_pll_veml_make_tracking_cc(long if_freq,
long fs_in, unsigned
@ -196,7 +187,6 @@ private:
std::map<std::string, std::string> systemName;
std::string sys;
};
#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 <sstream>
#include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
#include <boost/asio.hpp>
#include "tcp_packet_data.h"
@ -67,8 +65,7 @@
using google::LogMessage;
galileo_e1_tcp_connector_tracking_cc_sptr
galileo_e1_tcp_connector_make_tracking_cc(
galileo_e1_tcp_connector_tracking_cc_sptr galileo_e1_tcp_connector_make_tracking_cc(
long if_freq,
long fs_in,
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,
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(
long if_freq,
long fs_in,
@ -118,7 +116,7 @@ Galileo_E1_Tcp_Connector_Tracking_cc::Galileo_E1_Tcp_Connector_Tracking_cc(
// Initialize tracking ==========================================
//--- 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)
//--- 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_Very_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
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_carrier_lock_fail_counter = 0;
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["C"] = std::string("Compass");
}
void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking()
{
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_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
// 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
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)];
@ -215,14 +213,14 @@ void Galileo_E1_Tcp_Connector_Tracking_cc::start_tracking()
// DEBUG OUTPUT
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
d_pull_in = true;
d_enable_tracking = true;
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;
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));
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));
}
void Galileo_E1_Tcp_Connector_Tracking_cc::update_local_carrier()
{
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;
// 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;
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,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
float carr_error_filt_hz;
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_pull_in == true)
{
/*
* Signal alignment (skip samples until the incoming signal is aligned with local replica)
*/
int samples_offset;
float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_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);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica
return 1;
}
if (d_pull_in == true)
{
/*
* Signal alignment (skip samples until the incoming signal is aligned with local replica)
*/
int samples_offset;
float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_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);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica
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
Gnss_Synchro current_synchro_data;
// Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro;
// Block input data and block output stream pointers
const gr_complex* in = (gr_complex*) input_items[0];
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
// Block input data and block output stream pointers
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))
// Generate local code and carrier replicas (using \hat{f}_d(k-1))
update_local_code();
update_local_carrier();
@ -363,7 +360,19 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
d_control_id++;
//! 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);
// ################## PLL ##########################################################
@ -374,10 +383,10 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
// 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);
//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;
//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*Galileo_E1_CODE_PERIOD;
d_rem_carr_phase_rad=fmod(d_rem_carr_phase_rad,GPS_TWO_PI);
d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*Galileo_E1_CODE_PERIOD;
//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 = fmod(d_rem_carr_phase_rad, GPS_TWO_PI);
// ################## DLL ##########################################################
// 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;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer
float T_chip_seconds;
float T_prn_seconds;
float T_prn_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
T_chip_seconds = 1 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS;
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;
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
// keep alignment parameters for the next input buffer
float T_chip_seconds;
float T_prn_seconds;
float T_prn_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
T_chip_seconds = 1 / d_code_freq_chips;
T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS;
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;
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
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
d_cn0_estimation_counter++;
}
else
{
d_cn0_estimation_counter = 0;
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
d_cn0_estimation_counter++;
}
else
{
d_cn0_estimation_counter = 0;
// 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);
// 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);
// Carrier lock indicator
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
}
}
// Carrier lock indicator
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
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
// Tracking_timestamp_secs is aligned with the PRN start sample
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter+(double)d_next_prn_length_samples+(double)d_next_rem_code_phase_samples)/(double)d_fs_in;
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_next_prn_length_samples + (double)d_next_rem_code_phase_samples)/(double)d_fs_in;
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0
current_synchro_data.Code_phase_secs = 0;
current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad;
@ -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);
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)
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
}
}
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)
{
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)
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
std::cout << "Tracking CH " << d_channel
<< ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN)
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
}
}
}
@ -546,7 +554,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples;
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));
}
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;
}
}
// if(d_current_prn_length_samples!=d_vector_length)
// 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
consume_each(d_current_prn_length_samples); // this is needed in gr::block derivates
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
}
@ -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)
{
d_port = d_port_ch0 + d_channel;
d_listen_connection = d_tcp_com.listen_tcp_connection(d_port,d_port_ch0);
}
{
d_port = d_port_ch0 + d_channel;
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;
}
void Galileo_E1_Tcp_Connector_Tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
d_acquisition_gnss_synchro = p_gnss_synchro;
// Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
//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 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"
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_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
{
public:
~Galileo_E1_Tcp_Connector_Tracking_cc();
void set_channel(unsigned int channel);
@ -83,15 +81,11 @@ public:
void start_tracking();
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
int general_work (int noutput_items, gr_vector_int &ninput_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);
private:
friend galileo_e1_tcp_connector_tracking_cc_sptr
galileo_e1_tcp_connector_make_tracking_cc(long if_freq,
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 "gps_l1_ca_dll_fll_pll_tracking_cc.h"
//#include "gnss_signal_processing.h"
#include "gps_sdr_signal_processing.h"
#include "GPS_L1_CA.h"
#include "tracking_discriminators.h"
@ -48,7 +47,6 @@
#include <iostream>
#include <sstream>
#include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
@ -65,11 +63,10 @@
using google::LogMessage;
gps_l1_ca_dll_fll_pll_tracking_cc_sptr
gps_l1_ca_dll_fll_pll_make_tracking_cc(
long if_freq,
long fs_in,
unsigned
gps_l1_ca_dll_fll_pll_tracking_cc_sptr gps_l1_ca_dll_fll_pll_make_tracking_cc(
long if_freq,
long fs_in,
unsigned
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
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;
// 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.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){};
// space for carrier wipeoff and signal baseband vectors
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_Prompt, 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_acq_sample_stamp = 0;
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_current_prn_length_samples = (int)d_vector_length;
// 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
*/
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_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;
std::cout << "PULL-IN Doppler [Hz]= " << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
<< " PULL-IN Code Phase [samples]= " << d_acq_code_phase_samples << std::endl;
<< " Code Phase correction [samples]=" << delay_correction_samples
<< " 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
tcode_chips = -rem_code_phase_chips;
// Alternative EPL code generation (40% of speed improvement!)
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;
for (int i=0; i<epl_loop_length_samples; i++)
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;
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));
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;
}
@ -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()
{
double phase, phase_step;
double phase, phase_step;
phase_step = GPS_TWO_PI * d_carrier_doppler_hz / d_fs_in;
phase = d_rem_carr_phase;
for(int i = 0; i < d_current_prn_length_samples; i++)
@ -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,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
double code_error_chips = 0;
double code_error_filt_chips =0;
double code_error_filt_chips = 0;
double correlation_time_s = 0;
double PLL_discriminator_hz = 0;
double carr_nco_hz = 0;
// get the sample in and out pointers
const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
const gr_complex* in = (gr_complex*) input_items[0]; // block input samples pointer
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // block output streams pointer
d_Prompt_prev = *d_Prompt; // for the FLL discriminator
if (d_enable_tracking == true)
{
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data;
// Fill the acquisition data
current_synchro_data=*d_acquisition_gnss_synchro;
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data;
// Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro;
/*
* Receiver signal alignment
*/
@ -381,21 +364,20 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto
d_pull_in = false;
consume_each(samples_offset); //shift input to perform alignment with local replica
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0;
current_synchro_data.Prompt_Q=0.0;
current_synchro_data.Tracking_timestamp_secs=(double)d_sample_counter/d_fs_in;
current_synchro_data.Carrier_phase_rads=0.0;
current_synchro_data.Code_phase_secs=0.0;
current_synchro_data.CN0_dB_hz=0.0;
current_synchro_data.Flag_valid_tracking=false;
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in;
current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking = false;
*out[0] =current_synchro_data;
*out[0] = current_synchro_data;
return 1;
}
update_local_code();
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());
// 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)
{
const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available);
{
const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available);
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0;
current_synchro_data.Prompt_Q=0.0;
current_synchro_data.Tracking_timestamp_secs=(double)d_sample_counter/d_fs_in;
current_synchro_data.Carrier_phase_rads=0.0;
current_synchro_data.Code_phase_secs=0.0;
current_synchro_data.CN0_dB_hz=0.0;
current_synchro_data.Flag_valid_tracking=false;
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in;
current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking = false;
*out[0] =current_synchro_data;
*out[0] =current_synchro_data;
return 1;
}
return 1;
}
/*
* DLL, FLL, and PLL discriminators
*/
// 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
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
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);
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);
/*!
@ -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)
{
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;
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
}
@ -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_Late = gr_complex(0,0);
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;
prompt_I = (*d_Prompt).real();
prompt_Q = (*d_Prompt).imag();
tmp_E=std::abs<float>(*d_Early);
tmp_P=std::abs<float>(*d_Prompt);
tmp_L=std::abs<float>(*d_Late);
tmp_E = std::abs<float>(*d_Early);
tmp_P = std::abs<float>(*d_Prompt);
tmp_L = std::abs<float>(*d_Late);
try
{
// 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_Q, sizeof(float));
// PRN start sample stamp
//tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
// 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));
// 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));
tmp_float=(float)d_code_freq_hz;
tmp_float = (float)d_code_freq_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float));
//PLL commands
tmp_float=(float)PLL_discriminator_hz;
tmp_float = (float)PLL_discriminator_hz;
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));
//DLL commands
tmp_float=(float)code_error_chips;
tmp_float = (float)code_error_chips;
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));
// 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));
tmp_float=(float)d_carrier_lock_test;
tmp_float = (float)d_carrier_lock_test;
d_dump_file.write((char*)&tmp_float, sizeof(float));
// AUX vars (for debug purposes)
tmp_float = (float)d_rem_code_phase_samples;
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));
}
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;
}
}
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
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 <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h"
#include "tracking_FLL_PLL_filter.h"
#include "tracking_2nd_DLL_filter.h"
#include "gnss_synchro.h"
//#include "GPS_L1_CA.h"
#include "correlator.h"
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 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
@ -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
{
public:
~Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc();
void set_channel(unsigned int channel);
@ -103,10 +99,7 @@ public:
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private:
friend gps_l1_ca_dll_fll_pll_tracking_cc_sptr
gps_l1_ca_dll_fll_pll_make_tracking_cc(
long if_freq,
@ -163,7 +156,6 @@ private:
double d_early_late_spc_chips;
double d_carrier_doppler_hz;
double d_code_freq_hz;
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(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
d_queue = queue;
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;
// Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_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
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,
* having the sub-array boundaries unaligned to cache lines could lead
* to performance degradation. Here we allocate memory
* (gr_comlex array of size 2*d_vector_length) aligned to cache of 16 bytes
*/
// todo: do something if posix_memalign fails
// 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){};
@ -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){};
// space for carrier wipeoff and signal baseband vectors
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_Prompt, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
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");
}
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_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;
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_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * (float)d_fs_in;
d_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;
@ -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;
}
delay_correction_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;
// DLL/PLL filter initialization
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)
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[(int)GPS_L1_CA_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
//******************************************************************************
// Experimental: pre-sampled local signal replica at nominal code frequency.
// No code doppler correction
double tcode_chips;
int associated_chip_index;
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!)
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;
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));
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_late_code, &d_early_code[early_late_spc_samples*2], d_current_prn_length_samples* sizeof(gr_complex));
//******************************************************************************
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
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;
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));
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);
}
Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::~Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc()
{
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
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
*/
// Tracking signal processing
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)
{
// stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream;
// stream to collect cout calls to improve thread safety
std::stringstream tmp_str_stream;
float carr_error_hz;
float carr_error_filt_hz;
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)
{
int samples_offset;
// 28/11/2011 ACQ to TRK transition BUG CORRECTION
float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_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);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
//d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset) / (double)d_fs_in);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false;
//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;
}
// 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;
// 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];
// Generate local code and carrier replicas (using \hat{f}_d(k-1))
//update_local_code(); //disabled in the speed optimized tracking!
update_local_carrier();
// 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]
d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// keep alignment parameters for the next input buffer
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_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
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 ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
// Tracking_timestamp_secs is aligned with the PRN start sample
@ -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!
*/
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{
d_last_seg = floor(d_sample_counter / d_fs_in);
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
{
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;
}
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!
}
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;
LOG(INFO) << tmp_str_stream.rdbuf();
//if (d_channel == 0 || d_last_seg==5) d_carrier_lock_fail_counter=500; //DEBUG: force unlock!
}
}
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)
tmp_float = d_rem_code_phase_samples;
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));
}
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;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
if (d_dump == true)
{
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
#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 <queue>
@ -43,33 +43,27 @@
#include <boost/thread/thread.hpp>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h"
#include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h"
#include "correlator.h"
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
gps_l1_ca_dll_pll_make_optim_tracking_cc(long if_freq,
long fs_in, unsigned
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
bool dump,
std::string dump_filename,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
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 gps_l1_ca_dll_pll_make_optim_tracking_cc(long if_freq,
long fs_in,
unsigned int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
bool dump,
std::string dump_filename,
float pll_bw_hz,
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
@ -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
{
public:
~Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc();
void set_channel(unsigned int channel);
@ -85,20 +78,11 @@ public:
void start_tracking();
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,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private:
friend 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

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

@ -45,7 +45,6 @@
#include <iostream>
#include <sstream>
#include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h>
#include <glog/log_severity.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::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
d_queue = queue;
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;
// Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_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){};
// space for carrier wipeoff and signal baseband vectors
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_Prompt, 16, sizeof(gr_complex)) == 0){};
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
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");
}
void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
{
/*
* 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_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
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_carrier_doppler_hz = d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(); //initialize the carrier filter
d_code_loop_filter.initialize(); //initialize the code filter
d_carrier_loop_filter.initialize(); // initialize the carrier filter
d_code_loop_filter.initialize(); // initialize the code filter
// 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);
@ -244,7 +238,7 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking()
// DEBUG OUTPUT
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
d_pull_in = true;
@ -266,8 +260,8 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::update_local_code()
int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
double code_phase_step_chips;
int early_late_spc_samples;
int epl_loop_length_samples;
int early_late_spc_samples;
int epl_loop_length_samples;
// unified loop for E, P, L code vectors
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!)
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;
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));
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));
d_early_code[i] = d_ca_code[associated_chip_index];
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_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,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
float carr_error_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)
{
/*
* Receiver signal alignment
*/
// Receiver signal alignment
if (d_pull_in == true)
{
int samples_offset;
// 28/11/2011 ACQ to TRK transition BUG CORRECTION
float acq_trk_shif_correction_samples;
int acq_to_trk_delay_samples;
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
acq_trk_shif_correction_samples = d_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);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
//d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset) / (double)d_fs_in);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false;
//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;
}
@ -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;
// 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];
// 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());
// 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)
{
const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available);
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];
d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available);
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/(double)d_fs_in;
current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking = false;
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/(double)d_fs_in;
current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking = false;
*out[0] = current_synchro_data;
*out[0] = current_synchro_data;
return 1;
}
return 1;
}
// ################## PLL ##########################################################
// 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
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
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
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 = 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);
// ################## DLL ##########################################################
// 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_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
//Code phase accumulator
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]
d_acc_code_phase_secs=d_acc_code_phase_secs+code_error_filt_secs;
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;
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
// 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_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_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
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_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
d_cn0_estimation_counter++;
}
else
{
d_cn0_estimation_counter = 0;
// 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);
// Carrier lock indicator
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
}
}
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
d_cn0_estimation_counter++;
}
else
{
d_cn0_estimation_counter = 0;
// 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);
// Carrier lock indicator
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
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
// Tracking_timestamp_secs is aligned with the PRN start sample
@ -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);
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)
<< ", 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;
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
//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
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;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
if (d_dump == true)
{
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 <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h"
#include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h"
#include "correlator.h"
class 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 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
@ -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
{
public:
~Gps_L1_Ca_Dll_Pll_Tracking_cc();
void set_channel(unsigned int channel);
@ -85,20 +80,12 @@ public:
void start_tracking();
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,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private:
friend gps_l1_ca_dll_pll_tracking_cc_sptr
gps_l1_ca_dll_pll_make_tracking_cc(long if_freq,
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 <sstream>
#include <cmath>
//#include "math.h"
#include <gnuradio/io_signature.h>
#include <glog/log_severity.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::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
d_queue = queue;
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;
// Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_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
// 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_carr_sign = new gr_complex[d_vector_length*2];
/* 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_Late, 16, sizeof(gr_complex)) == 0){};
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
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
// 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.
// if (d_sample_counter<d_acq_sample_stamp)
// {
// acq_trk_diff_samples=0; //disable the correction
// }else{
// acq_trk_diff_samples = d_sample_counter - d_acq_sample_stamp;//-d_vector_length;
// }
// if (d_sample_counter<d_acq_sample_stamp)
// {
// acq_trk_diff_samples=0; //disable the correction
// }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;
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;
@ -293,8 +284,8 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::update_local_code()
int associated_chip_index;
int code_length_chips = (int)GPS_L1_CA_CODE_LENGTH_CHIPS;
double code_phase_step_chips;
int early_late_spc_samples;
int epl_loop_length_samples;
int early_late_spc_samples;
int epl_loop_length_samples;
// unified loop for E, P, L code vectors
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;
// Alternative EPL code generation (40% of speed improvement!)
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;
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));
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;
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));
d_early_code[i] = d_ca_code[associated_chip_index];
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_late_code,&d_early_code[early_late_spc_samples*2],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));
}
@ -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,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
float carr_error;
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;
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);
//std::cout<<"acq_trk_shif_correction="<<acq_trk_shif_correction_samples<<"\r\n";
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
d_sample_counter_seconds = d_sample_counter_seconds + (((double)samples_offset) / (double)d_fs_in);
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
d_pull_in = false;
//std::cout<<" samples_offset="<<samples_offset<<"\r\n";
consume_each(samples_offset); //shift input to perform alignement with local replica
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)
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];
d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available);
{
const int samples_available = ninput_items[0];
d_sample_counter = d_sample_counter + samples_available;
LOG_AT_LEVEL(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
consume_each(samples_available);
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I=0.0;
current_synchro_data.Prompt_Q=0.0;
current_synchro_data.Tracking_timestamp_secs=d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads=0.0;
current_synchro_data.Code_phase_secs=0.0;
current_synchro_data.CN0_dB_hz=0.0;
current_synchro_data.Flag_valid_tracking=false;
// make an output to not stop the rest of the processing blocks
current_synchro_data.Prompt_I = 0.0;
current_synchro_data.Prompt_Q = 0.0;
current_synchro_data.Tracking_timestamp_secs = d_sample_counter_seconds;
current_synchro_data.Carrier_phase_rads = 0.0;
current_synchro_data.Code_phase_secs = 0.0;
current_synchro_data.CN0_dB_hz = 0.0;
current_synchro_data.Flag_valid_tracking = false;
*out[0] =current_synchro_data;
*out[0] =current_synchro_data;
return 1;
}
return 1;
}
//! Variable used for control
d_control_id++;
//! 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}};
d_tcp_com.send_receive_tcp_packet_gps_l1_ca(tx_variables_array, &tcp_data);
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}};
d_tcp_com.send_receive_tcp_packet_gps_l1_ca(tx_variables_array, &tcp_data);
//! Recover the tracking data
code_error = tcp_data.proc_pack_code_error;
carr_error = tcp_data.proc_pack_carr_error;
// Modify carrier freq based on NCO command
d_carrier_doppler_hz = tcp_data.proc_pack_carrier_doppler_hz;
//! Recover the tracking data
code_error = tcp_data.proc_pack_code_error;
carr_error = tcp_data.proc_pack_carr_error;
// Modify carrier freq based on NCO command
d_carrier_doppler_hz = tcp_data.proc_pack_carrier_doppler_hz;
// 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;
// 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)
{
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;
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
@ -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);
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)
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
<< ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl;
}
}
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);
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));
// 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*)&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 += 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
@ -645,7 +639,7 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::set_channel(unsigned int channel)
d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
if (d_dump == true)
{
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)
{
d_port = d_port_ch0 + d_channel;
d_listen_connection = d_tcp_com.listen_tcp_connection(d_port,d_port_ch0);
}
{
d_port = d_port_ch0 + d_channel;
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)
{
d_acquisition_gnss_synchro = p_gnss_synchro;
// Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
//DLOG(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
//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 <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "concurrent_queue.h"
#include "gps_sdr_signal_processing.h"
#include "gnss_synchro.h"
@ -54,8 +53,8 @@
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_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,
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
@ -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
{
public:
~Gps_L1_Ca_Tcp_Connector_Tracking_cc();
void set_channel(unsigned int channel);
@ -90,15 +87,11 @@ public:
*
* The user must override work to define the signal processing code
*/
int general_work (int noutput_items, gr_vector_int &ninput_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);
private:
friend gps_l1_ca_tcp_connector_tracking_cc_sptr
gps_l1_ca_tcp_connector_make_tracking_cc(long if_freq,
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_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_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_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_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]
// GPS NAVIGATION MESSAGE STRUCTURE
// 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> > SUBFRAME_ID({{50,3}});
// SUBFRAME 1
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}}); //*
@ -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>> T_GD({{197,8}});
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>> T_OC({{219,16}});
const double T_OC_LSB = TWO_P4;
const std::vector<std::pair<int,int>> A_F2({{241,8}});
const double A_F2_LSB = TWO_N55;
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;
// SUBFRAME 2
const std::vector<std::pair<int,int>> IODE_SF2({{61,8}});
const std::vector<std::pair<int,int>> C_RS({{69,16}});
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;
// SUBFRAME 3
const std::vector<std::pair<int,int>> C_IC({{61,16}});
const double C_IC_LSB = TWO_N29;
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
const std::vector<std::pair<int,int>> SV_DATA_ID({{61,2}});
const std::vector<std::pair<int,int>> SV_PAGE({{63,6}});
// SUBFRAME 4
//! \todo read all pages of subframe 4
// Page 18 - Ionospheric and UTC data
const std::vector<std::pair<int,int>> ALPHA_0({{69,8}});
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}});
// SUBFRAME 5
//! \todo read all pages of subframe 5
// page 25 - Health (PRN 1 - 24)
const std::vector<std::pair<int,int>> T_OA({{69,8}});
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_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_ */