diff --git a/src/algorithms/PVT/adapters/galileo_e1_pvt.h b/src/algorithms/PVT/adapters/galileo_e1_pvt.h index db6296a6c..4863d21e1 100644 --- a/src/algorithms/PVT/adapters/galileo_e1_pvt.h +++ b/src/algorithms/PVT/adapters/galileo_e1_pvt.h @@ -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"; diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc index 845b01c80..743caa372 100644 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc @@ -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 galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr 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_ptrset_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; i0) - { - 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::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::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::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::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 < galileo_e1_pvt_cc_sptr; -galileo_e1_pvt_cc_sptr -galileo_e1_make_pvt_cc(unsigned int n_channels, boost::shared_ptr 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 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 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 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 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 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 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 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 a, std::pair b); + galileo_e1_ls_pvt *d_ls_pvt; + bool pseudoranges_pairCompare_min(std::pair a, std::pair b); public: ~galileo_e1_pvt_cc (); //!< Default destructor diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc index 1b0a23bc7..d342cfa91 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc @@ -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; igps_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 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 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 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 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 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 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 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 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 d_queue; bool d_dump; bool b_rinex_header_writen; diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc index 3a0b81ac5..95e277e26 100644 --- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc +++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc @@ -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 #include "galileo_e1_ls_pvt.h" - #include "Galileo_E1.h" #include #include @@ -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 gnss_pseudoranges_map std::map::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 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 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 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 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="<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 < gnss_pseudoranges_map d_avg_latitude_d = 0; d_avg_longitude_d = 0; d_avg_height_m = 0; - for (unsigned int i=0; i 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 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 #include #include -//#include #include #include #include @@ -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 galileo_ephemeris_map; - // new utc_model storage + std::map 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 d_hist_latitude_d; std::deque d_hist_longitude_d; std::deque 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 gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging); diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc index 9f8ca450d..ac2bac027 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -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 gnss_pseudoranges_map, std::map::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 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 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 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 < 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 gnss_pseudoranges_map, d_avg_latitude_d = 0; d_avg_longitude_d = 0; d_avg_height_m = 0; - for (unsigned int i=0; i gps_ephemeris_map; - // new utc_model storage + std::map 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 sbas_sat_corr_map; std::map 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 d_hist_latitude_d; std::deque d_hist_longitude_d; std::deque 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; diff --git a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h index 1f8e17e3b..283fadef0 100644 --- a/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h +++ b/src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h @@ -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 queue); diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index 2d8a2a87d..6278700da 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -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 *d_channel_internal_queue; - std::ofstream d_dump_file; - bool d_active; + concurrent_queue *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 * code); + /*! + * \brief Sets local code for PCPS acquisition algorithm. + * \param code - Pointer to the PRN code. + */ + void set_local_code(std::complex * 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 *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 *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_*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc index 8b8b9266d..4220e8030 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc @@ -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;iAcq_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;id_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; iget_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 diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h index 7d875aeb2..31600393a 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.h @@ -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 #include @@ -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_*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h index 8cc769ea6..416f1935a 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.h @@ -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 *d_channel_internal_queue; - std::ofstream d_dump_file; - bool d_active; + concurrent_queue *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 * code_data, std::complex * 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 * code_data, std::complex * 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 *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 *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_*/ diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc index b7a607e27..381b68e02 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc @@ -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) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h index 37356ad09..aa176137c 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.h @@ -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 *d_channel_internal_queue; - std::ofstream d_dump_file; - bool d_active; + concurrent_queue *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 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 * code); + /*! + * \brief Sets local code for PCPS acquisition algorithm. + * \param code - Pointer to the PRN code. + */ + void set_local_code(std::complex * 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 *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 *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 diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h index 50a4b43ba..10d2f7d78 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.h @@ -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 *d_channel_internal_queue; - std::ofstream d_dump_file; - bool d_active; + concurrent_queue *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 * code); + /*! + * \brief Sets local code for TONG acquisition algorithm. + * \param code - Pointer to the PRN code. + */ + void set_local_code(std::complex * 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 *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 *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_ */ diff --git a/src/algorithms/observables/adapters/galileo_e1_observables.h b/src/algorithms/observables/adapters/galileo_e1_observables.h index 029e8c846..53f4c1d1d 100644 --- a/src/algorithms/observables/adapters/galileo_e1_observables.h +++ b/src/algorithms/observables/adapters/galileo_e1_observables.h @@ -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"; diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc index d72173c98..3f3d71511 100644 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc @@ -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 a, std::pair b) { return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms); } + bool Galileo_pairCompare_gnss_synchro_d_TOW_at_current_symbol( std::pair a, std::pair 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 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(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(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["<second.Channel_ID<<"]="<second.Channel_ID<<"]="<second.d_TOW_at_current_symbol<second.d_TOW_at_current_symbol)*1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms; - //std::cout<<"traveltime_ms="<second.Channel_ID<<"]="<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 #include #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; diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc index 6e911776e..ad9e41217 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc @@ -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 a, std::pair 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 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(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="<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 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 queue, bool dump); /*! diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc index f9402718e..f91b78e8b 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc @@ -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; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h index 030653aba..3d85705ec 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h @@ -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 samples, std::vector &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; diff --git a/src/algorithms/telemetry_decoder/libs/viterbi_decoder.cc b/src/algorithms/telemetry_decoder/libs/viterbi_decoder.cc index 5f7869c83..217091b5e 100644 --- a/src/algorithms/telemetry_decoder/libs/viterbi_decoder.cc +++ b/src/algorithms/telemetry_decoder/libs/viterbi_decoder.cc @@ -31,7 +31,6 @@ #include "viterbi_decoder.h" #include - #include #include @@ -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++; diff --git a/src/algorithms/telemetry_decoder/libs/viterbi_decoder.h b/src/algorithms/telemetry_decoder/libs/viterbi_decoder.h index 3956bb001..7cde33506 100644 --- a/src/algorithms/telemetry_decoder/libs/viterbi_decoder.h +++ b/src/algorithms/telemetry_decoder/libs/viterbi_decoder.h @@ -35,6 +35,9 @@ #include #include +/*! + * \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) */ diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h index bc0bbe23f..6c6e568ee 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h @@ -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 *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_; diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index 1d7532301..f3f1a3d96 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -45,7 +45,6 @@ #include #include #include -//#include "math.h" #include #include #include @@ -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; iPRN) - << ", Doppler="<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(*d_Prompt); tmp_L = std::abs(*d_Late); tmp_VL = std::abs(*d_Very_Late); - //std::cout<<"VE="< #include #include - #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_sptr; +typedef boost::shared_ptr 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 *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 systemName; std::string sys; - }; #endif //GNSS_SDR_GALILEO_E1_DLL_PLL_VEML_TRACKING_CC_H diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index b5bf084d9..09323d247 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -49,11 +49,9 @@ #include #include #include -//#include "math.h" #include #include #include - #include #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; itelemetry_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 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 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 - galileo_e1_tcp_connector_tracking_cc_sptr; + +typedef boost::shared_ptr 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 *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 diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc index 2ca631a41..c58ad1d6a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc @@ -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 #include #include -//#include "math.h" #include #include #include @@ -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 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; itelemetry_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(*d_Early); - tmp_P=std::abs(*d_Prompt); - tmp_L=std::abs(*d_Late); + tmp_E = std::abs(*d_Early); + tmp_P = std::abs(*d_Prompt); + tmp_L = std::abs(*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 } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h index 8775f21e6..94398dc5c 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h @@ -43,13 +43,11 @@ #include #include #include -//#include #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; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc index ffadba75e..032344b95 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc @@ -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; itelemetry_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="<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) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h index 3cbcd2dcf..72e40427e 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h @@ -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 #include @@ -43,33 +43,27 @@ #include #include #include -//#include #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_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 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_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 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 *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 diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index d5e6d364b..a5cbc3cd2 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -45,7 +45,6 @@ #include #include #include -//#include "math.h" #include #include #include @@ -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 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 "< #include #include -//#include #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 @@ -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 *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 diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index 7c30c96bd..0b29e0aec 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -47,7 +47,6 @@ #include #include #include -//#include "math.h" #include #include #include @@ -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 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 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 #include #include -//#include #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_sptr; + +typedef boost::shared_ptr 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 diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index ab25a3888..47b643539 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -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 > ALERT_FLAG({{48,1}}); const std::vector > ANTI_SPOOFING_FLAG({{49,1}}); const std::vector > SUBFRAME_ID({{50,3}}); - // SUBFRAME 1 const std::vector> GPS_WEEK({{61,10}}); const std::vector> CA_OR_P_ON_L2({{71,2}}); //* @@ -101,11 +98,9 @@ const std::vector> SV_HEALTH ({{77,6}}); const std::vector> L2_P_DATA_FLAG ({{91,1}}); const std::vector> T_GD({{197,8}}); const double T_GD_LSB = TWO_N31; - const std::vector> IODC({{83,2},{211,8}}); const std::vector> T_OC({{219,16}}); const double T_OC_LSB = TWO_P4; - const std::vector> A_F2({{241,8}}); const double A_F2_LSB = TWO_N55; const std::vector> A_F1({{249,16}}); @@ -114,7 +109,6 @@ const std::vector> A_F0({{271,22}}); const double A_F0_LSB = TWO_N31; // SUBFRAME 2 - const std::vector> IODE_SF2({{61,8}}); const std::vector> C_RS({{69,16}}); const double C_RS_LSB = TWO_N5; @@ -137,7 +131,6 @@ const std::vector> AODO({{272,5}}); const int AODO_LSB = 900; // SUBFRAME 3 - const std::vector> C_IC({{61,16}}); const double C_IC_LSB = TWO_N29; const std::vector> OMEGA_0({{77,8},{91,24}}); @@ -158,14 +151,11 @@ const double I_DOT_LSB = PI_TWO_N43; // SUBFRAME 4-5 - const std::vector> SV_DATA_ID({{61,2}}); const std::vector> SV_PAGE({{63,6}}); // SUBFRAME 4 - //! \todo read all pages of subframe 4 - // Page 18 - Ionospheric and UTC data const std::vector> ALPHA_0({{69,8}}); const double ALPHA_0_LSB = TWO_N30; @@ -211,13 +201,9 @@ const std::vector> HEALTH_SV31({{277,6}}); const std::vector> HEALTH_SV32({{283,6}}); - - - // SUBFRAME 5 //! \todo read all pages of subframe 5 - // page 25 - Health (PRN 1 - 24) const std::vector> T_OA({{69,8}}); const double T_OA_LSB = TWO_P12; @@ -247,74 +233,4 @@ const std::vector> HEALTH_SV22({{247,6}}); const std::vector> HEALTH_SV23({{253,6}}); const std::vector> HEALTH_SV24({{259,6}}); -/* - -inline void ca_code_generator_complex(std::complex* _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(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_ */