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 28be90a22..35e1478d8 100644 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc @@ -123,9 +123,6 @@ galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, bool dump, std::str this->message_port_register_in(pmt::mp("telemetry")); this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&galileo_e1_pvt_cc::msg_handler_telemetry, this, _1)); - // Receiver time feedback to observables block - this->message_port_register_out(pmt::mp("rx_dt_s")); - //initialize kml_printer std::string kml_dump_filename; kml_dump_filename = d_dump_filename; 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 c16124ef3..8c774fa87 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 @@ -227,9 +227,6 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&gps_l1_ca_pvt_cc::msg_handler_telemetry, this, _1)); - // Receiver time feedback to observables block - this->message_port_register_out(pmt::mp("rx_dt_s")); - //initialize kml_printer std::string kml_dump_filename; kml_dump_filename = d_dump_filename; @@ -376,11 +373,16 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), g pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); if (pvt_result == true) { - //feedback the receiver time offset estimation to observables block + //correct the observable to account for the receiver clock offset + + for (std::map::iterator it=gnss_observables_map.begin(); it!=gnss_observables_map.end(); ++it) + { + it->second.Pseudorange_m=it->second.Pseudorange_m-d_ls_pvt->d_rx_dt_s*GPS_C_m_s; + } // send asynchronous message to observables block // time offset is expressed as the equivalent travel distance [m] - pmt::pmt_t value = pmt::from_double(d_ls_pvt->d_rx_dt_s); - this->message_port_pub(pmt::mp("rx_dt_s"), value); + //pmt::pmt_t value = pmt::from_double(d_ls_pvt->d_rx_dt_s); + //this->message_port_pub(pmt::mp("rx_dt_s"), value); //std::cout<<"d_rx_dt_s*GPS_C_m_s="<d_rx_dt_s*GPS_C_m_s<message_port_register_in(pmt::mp("telemetry")); this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&hybrid_pvt_cc::msg_handler_telemetry, this, _1)); - // Receiver time feedback to observables block - this->message_port_register_out(pmt::mp("rx_dt_s")); - //initialize kml_printer std::string kml_dump_filename; kml_dump_filename = d_dump_filename; 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 b5fe755fb..4f5cc1247 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -80,11 +80,10 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, { std::map::iterator gnss_pseudoranges_iter; 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::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector - arma::mat satpos = arma::zeros(3, valid_pseudoranges); //satellite positions matrix + arma::vec 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 GPS_week = 0; double utc = 0; @@ -97,7 +96,6 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, // ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) **** // ******************************************************************************** int valid_obs = 0; //valid observations counter - int obs_counter = 0; for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin(); gnss_pseudoranges_iter != gnss_pseudoranges_map.end(); gnss_pseudoranges_iter++) @@ -109,7 +107,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, /*! * \todo Place here the satellite CN0 (power level, or weight factor) */ - W(obs_counter, obs_counter) = 1; + W.resize(valid_obs+1,1); + W(valid_obs)=1; // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) // first estimate of transmit time @@ -122,13 +121,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, // 3- compute the current ECEF position for this SV using corrected TX time 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.resize(3,valid_obs+1); + satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X; + satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y; + satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z; // 4- fill the observations vector with the corrected pseudoranges - obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s; + obs.resize(valid_obs+1,1); + obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GPS_C_m_s-d_rx_dt_s*GPS_C_m_s; d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN; d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz; valid_obs++; @@ -138,7 +137,7 @@ 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]"; + << " [m] PR_obs=" << obs(valid_obs) << " [m]"; // compute the UTC time for this SV (just to print the associated UTC timestamp) GPS_week = gps_ephemeris_iter->second.i_GPS_week; @@ -146,12 +145,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, } 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; } - obs_counter++; } // ******************************************************************************** @@ -162,32 +157,44 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, if (valid_obs >= 4) { - arma::vec mypos; + arma::vec rx_position_and_time; DLOG(INFO) << "satpos=" << satpos; DLOG(INFO) << "obs=" << obs; DLOG(INFO) << "W=" << W; - mypos = leastSquarePos(satpos, obs, W); - DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z) = " << mypos; + //check if this is the initial position computation + if (d_rx_dt_s==0) + { + //execute Bancroft's algorithm to estimate initial receiver position and time + std::cout<<"Executing Bancroft algorithm...\n"; + rx_position_and_time =bancroftPos(satpos.t(), obs); + d_rx_pos=rx_position_and_time.rows(0,2); //save ECEF position for the next iteration + d_rx_dt_s=rx_position_and_time(3)/GPS_C_m_s; //save time for the next iteration [meters]->[seconds] + } - cart2geo(static_cast(mypos(0)), static_cast(mypos(1)), static_cast(mypos(2)), 4); + //Execute WLS using previos position as the initialization point + rx_position_and_time = leastSquarePos(satpos, obs, W); + d_rx_pos=rx_position_and_time.rows(0,2); //save ECEF position for the next iteration + d_rx_dt_s+=rx_position_and_time(3)/GPS_C_m_s; //accumulate the rx time error for the next iteration [meters]->[seconds] - d_rx_dt_s = mypos(3)/GPS_C_m_s; // Convert RX time offset from meters to seconds + DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; + DLOG(INFO) <<"Accumulated rx clock error="<(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); // Compute UTC time and print PVT solution double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60) boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast(GPS_week)); // 22 August 1999 last GPS time roll over boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); d_position_UTC_time = p_time; - DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time) << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]"; // ###### Compute DOPs ######## + std::cout<<"c\r"; compute_DOP(); - + std::cout<<"d\r"; // ######## LOG FILE ######### if(d_flag_dump_enabled == true) { @@ -199,16 +206,16 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, tmp_double = GPS_current_time; d_dump_file.write((char*)&tmp_double, sizeof(double)); // ECEF User Position East [m] - tmp_double = mypos(0); + tmp_double = d_rx_pos(0); d_dump_file.write((char*)&tmp_double, sizeof(double)); // ECEF User Position North [m] - tmp_double = mypos(1); + tmp_double = d_rx_pos(1); d_dump_file.write((char*)&tmp_double, sizeof(double)); // ECEF User Position Up [m] - tmp_double = mypos(2); + tmp_double = d_rx_pos(2); d_dump_file.write((char*)&tmp_double, sizeof(double)); // User clock offset [s] - tmp_double = mypos(3); + tmp_double = d_rx_dt_s; d_dump_file.write((char*)&tmp_double, sizeof(double)); // GEO user position Latitude [deg] tmp_double = d_latitude_d; diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index 1c92d4def..a4b315f1d 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -41,12 +41,139 @@ using google::LogMessage; Ls_Pvt::Ls_Pvt() : Pvt_Solution() { - d_x_m = 0.0; - d_y_m = 0.0; - d_z_m = 0.0; + } -arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w) +arma::vec Ls_Pvt::bancroftPos(const arma::mat& satpos, const arma::vec& obs) { + +// %BANCROFT Calculation of preliminary coordinates +// % for a GPS receiver based on pseudoranges +// % to 4 or more satellites. The ECEF +// % coordinates are stored in satpos. The observed pseudoranges are stored in obs +// %Reference: Bancroft, S. (1985) An Algebraic Solution +// % of the GPS Equations, IEEE Trans. Aerosp. +// % and Elec. Systems, AES-21, 56--59 +// %Kai Borre 04-30-95, improved by C.C. Goad 11-24-96 +// %Copyright (c) by Kai Borre +// %$Revision: 1.0 $ $Date: 1997/09/26 $ +// +// % Test values to use in debugging +// % B_pass =[ -11716227.778 -10118754.628 21741083.973 22163882.029; +// % -12082643.974 -20428242.179 11741374.154 21492579.823; +// % 14373286.650 -10448439.349 19596404.858 21492492.771; +// % 10278432.244 -21116508.618 -12689101.970 25284588.982]; +// % Solution: 595025.053 -4856501.221 4078329.981 +// +// % Test values to use in debugging +// % B_pass = [14177509.188 -18814750.650 12243944.449 21119263.116; +// % 15097198.146 -4636098.555 21326705.426 22527063.486; +// % 23460341.997 -9433577.991 8174873.599 23674159.579; +// % -8206498.071 -18217989.839 17605227.065 20951643.862; +// % 1399135.830 -17563786.820 19705534.862 20155386.649; +// % 6995655.459 -23537808.269 -9927906.485 24222112.972]; +// % Solution: 596902.683 -4847843.316 4088216.740 + + arma::vec pos = arma::zeros(4,1); + arma::mat B_pass=arma::zeros(obs.size(),4); + B_pass.submat(0,0,obs.size()-1,2)=satpos; + B_pass.col(3)=obs; + + arma::mat B; + arma::mat BBB; + double traveltime=0; + for (int iter = 0; iter<2; iter++) + { + B = B_pass; + int m=arma::size(B,0); + for (int i=0;i 3) + { + BBB = arma::inv(B.t()*B)*B.t(); + } + else + { + BBB = arma::inv(B); + } + arma::vec e = arma::ones(m,1); + arma::vec alpha = arma::zeros(m,1); + for (int i =0; i abs_omc(1)) + { + pos = possible_pos.col(1); + } + else + { + pos = possible_pos.col(0); + } + }// % iter loop + return pos; +} + +double Ls_Pvt::lorentz(const arma::vec& x, const arma::vec& y) { +// %LORENTZ Calculates the Lorentz inner product of the two +// % 4 by 1 vectors x and y +// +// %Kai Borre 04-22-95 +// %Copyright (c) by Kai Borre +// %$Revision: 1.0 $ $Date: 1997/09/26 $ +// +// % M = diag([1 1 1 -1]); +// % p = x'*M*y; + + return(x(0)*y(0) + x(1)*y(1) + x(2)*y(2) - x(3)*y(3)); +} + +arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec) { /* Computes the Least Squares Solution. * Inputs: @@ -63,7 +190,10 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs int nmbOfIterations = 10; // TODO: include in config int nmbOfSatellites; nmbOfSatellites = satpos.n_cols; //Armadillo - arma::vec pos = "0.0 0.0 0.0 0.0"; + arma::mat w=arma::zeros(nmbOfSatellites,nmbOfSatellites); + w.diag()=w_vec; //diagonal weight matrix + + arma::vec pos = {{d_rx_pos(0)},{d_rx_pos(0)},{d_rx_pos(0)},0}; //time error in METERS (time x speed) arma::mat A; arma::mat omc; arma::mat az; @@ -160,5 +290,8 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs { d_Q = arma::zeros(4,4); } + return pos; } + + diff --git a/src/algorithms/PVT/libs/ls_pvt.h b/src/algorithms/PVT/libs/ls_pvt.h index 385d6e437..9332c198c 100644 --- a/src/algorithms/PVT/libs/ls_pvt.h +++ b/src/algorithms/PVT/libs/ls_pvt.h @@ -41,13 +41,24 @@ */ class Ls_Pvt : public Pvt_Solution { +private: + /*! + * \brief Computes the Lorentz inner product between two vectors + */ + double lorentz(const arma::vec & x,const arma::vec & y); public: Ls_Pvt(); - arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w); - double d_x_m; - double d_y_m; - double d_z_m; + /*! + * \brief Computes the initial position solution based on the Bancroft algorithm + */ + arma::vec bancroftPos(const arma::mat & satpos, const arma::vec & obs); + + /*! + * \brief Computes the Weighted Least Squares position solution + */ + arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::vec & w_vec); + }; #endif diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index d4312e082..a3913dc1c 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -57,6 +57,7 @@ Pvt_Solution::Pvt_Solution() b_valid_position = false; d_averaging_depth = 0; d_valid_observations = 0; + d_rx_pos=arma::zeros(3,1); d_rx_dt_s = 0.0; } diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h index 5f6d3a9d5..3b0681391 100644 --- a/src/algorithms/PVT/libs/pvt_solution.h +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -51,6 +51,8 @@ public: double d_latitude_d; //!< RX position Latitude WGS84 [deg] double d_longitude_d; //!< RX position Longitude WGS84 [deg] double d_height_m; //!< RX position height WGS84 [m] + + arma::vec d_rx_pos; double d_rx_dt_s; //!< RX time offset [s] boost::posix_time::ptime d_position_UTC_time; 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 faed78b6e..ed75b4c46 100644 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc @@ -56,24 +56,10 @@ galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string du } -void galileo_e1_observables_cc::msg_handler_rx_dt_s(pmt::pmt_t msg) -{ - //pmt::print(msg); - - d_rx_dt_s = pmt::to_double(msg); -} - galileo_e1_observables_cc::galileo_e1_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) : gr::block("galileo_e1_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) { - - // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("rx_dt_s")); - - this->set_msg_handler(pmt::mp("rx_dt_s"), - boost::bind(&galileo_e1_observables_cc::msg_handler_rx_dt_s, this, _1)); - // initialize internal vars d_dump = dump; d_nchannels = nchannels; diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h index 4779099ff..1cb3ef4c4 100644 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h @@ -61,7 +61,6 @@ private: friend galileo_e1_observables_cc_sptr galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); galileo_e1_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); - void msg_handler_rx_dt_s(pmt::pmt_t msg); //Tracking observable history std::vector> d_acc_carrier_phase_queue_rads; @@ -69,7 +68,6 @@ private: std::vector> d_symbol_TOW_queue_s; // class private vars - double d_rx_dt_s; bool d_dump; unsigned int d_nchannels; unsigned int history_deep; 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 36afa7606..f1ce131b0 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 @@ -58,13 +58,6 @@ gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, bool gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) { - // Telemetry bit synchronization message port input - this->message_port_register_in(pmt::mp("rx_dt_s")); - - this->set_msg_handler(pmt::mp("rx_dt_s"), - boost::bind(&gps_l1_ca_observables_cc::msg_handler_rx_dt_s, this, _1)); - - d_rx_dt_s=0; // initialize internal vars d_dump = dump; d_nchannels = nchannels; @@ -104,15 +97,6 @@ gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc() d_dump_file.close(); } - -void gps_l1_ca_observables_cc::msg_handler_rx_dt_s(pmt::pmt_t msg) -{ - //pmt::print(msg); - //accumulate the receiver time offset - d_rx_dt_s = d_rx_dt_s+pmt::to_double(msg); -} - - bool pairCompare_gnss_synchro_Prn_delay_ms(const std::pair& a, const std::pair& b) { return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms); @@ -220,7 +204,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni //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; //convert to meters and remove the receiver time offset in meters - pseudorange_m = traveltime_ms * GPS_C_m_ms-d_rx_dt_s*GPS_C_m_s; // [m] + pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] // update the pseudorange object current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; @@ -237,7 +221,6 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0; // arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz); // arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads); - // Curve fitting to cuadratic function arma::mat A = arma::ones (history_deep, 2); A.col(1) = symbol_TOW_vec_s; @@ -249,10 +232,8 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni coef_doppler = pinv_A * dopper_vec_hz; arma::vec acc_phase_lin; arma::vec carrier_doppler_lin; - acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0]; // +coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0]; - carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0]; // +coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0]; - //std::cout<<"acc_phase_vec_interp_rads="<message_port_register_in(pmt::mp("rx_dt_s")); - - this->set_msg_handler(pmt::mp("rx_dt_s"), - boost::bind(&hybrid_observables_cc::msg_handler_rx_dt_s, this, _1)); - // initialize internal vars d_dump = dump; d_nchannels = nchannels; diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h index ea916afcf..20154c78a 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h @@ -59,7 +59,6 @@ private: friend hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); - void msg_handler_rx_dt_s(pmt::pmt_t msg); //Tracking observable history std::vector> d_acc_carrier_phase_queue_rads; @@ -67,7 +66,6 @@ private: std::vector> d_symbol_TOW_queue_s; // class private vars - double d_rx_dt_s; bool d_dump; unsigned int d_nchannels; unsigned int history_deep; diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index ecdaf4e24..138140d34 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -324,8 +324,6 @@ void GNSSFlowgraph::connect() top_block_->connect(observables_->get_right_block(), i, pvt_->get_left_block(), i); top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("telemetry"), pvt_->get_left_block(), pmt::mp("telemetry")); } - //asynchronous feedback of receiver time estimation from PVT to observables - top_block_->msg_connect(pvt_->get_left_block(), pmt::mp("rx_dt_s"), observables_->get_right_block(), pmt::mp("rx_dt_s")); } catch (std::exception& e) {