diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index 9bcbbcf0c..a17833b3c 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -61,7 +61,7 @@ hybrid_make_pvt_cc(unsigned int nchannels, boost::shared_ptr queu hybrid_pvt_cc::hybrid_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("hybrid_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), + gr::block("hybrid_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(gr_complex))) { @@ -77,17 +77,18 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, boost::shared_ptr(); + d_kml_dump->set_headers(kml_dump_filename); //initialize nmea_printer - d_nmea_printer = new Nmea_Printer(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); + d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); d_dump_filename.append("_raw.dat"); dump_ls_pvt_filename.append("_ls_pvt.dat"); d_averaging_depth = averaging_depth; d_flag_averaging = flag_averaging; - d_ls_pvt = new hybrid_ls_pvt(nchannels, dump_ls_pvt_filename, d_dump); + d_ls_pvt = std::make_shared((int)nchannels, dump_ls_pvt_filename, d_dump); d_ls_pvt->set_averaging_depth(d_averaging_depth); d_sample_counter = 0; @@ -96,7 +97,7 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, boost::shared_ptr(); // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) @@ -120,12 +121,7 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, boost::shared_ptr(in[i][0].PRN, in[i][0])); // store valid pseudoranges in a map //d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges) - d_TOW_at_curr_symbol_constellation=in[i][0].d_TOW_at_current_symbol; // d_TOW_at_current_symbol not corrected by delta t (just for debug) + d_TOW_at_curr_symbol_constellation = in[i][0].d_TOW_at_current_symbol; // d_TOW_at_current_symbol not corrected by delta t (just for debug) d_rx_time = in[i][0].d_TOW_hybrid_at_current_symbol; // hybrid rx time, all the channels have the same RX timestamp (common RX time pseudoranges) //std::cout<<"CH PVT = "<< i << ", d_TOW = " << d_TOW_at_curr_symbol_constellation<<", rx_time_hybrid_PVT = " << d_rx_time << " same RX timestamp (common RX time pseudoranges)"<< std::endl; @@ -202,7 +198,7 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items, //if (gnss_pseudoranges_map.size() > 0 and d_ls_pvt->galileo_ephemeris_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() > 0) if (gnss_pseudoranges_map.size() > 0) { - //std::cout << "Both GPS and Galileo ephemeris map have been filled " << std::endl; + //std::cout << "Both GPS and Galileo ephemeris map have been filled " << std::endl; // compute on the fly PVT solution if ((d_sample_counter % d_output_rate_ms) == 0) { @@ -212,258 +208,258 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items, if (d_ls_pvt->b_valid_position == true) { - valid_solution_counter ++; + valid_solution_counter ++; - d_kml_dump.print_position_hybrid(d_ls_pvt, d_flag_averaging); + d_kml_dump->print_position_hybrid(d_ls_pvt, d_flag_averaging); - /* *********************************** COMPUTE STATISTICS OVER THE FIRST 1000 SOLUTOINS **********************************/ - if (valid_solution_counter<=1000) - //if (d_ls_pvt->d_valid_observations >= 14) - //if (d_ls_pvt->d_valid_GPS_obs >= 7 or d_ls_pvt->d_valid_GAL_obs >=7) - { - GDOP_vect.push_back (d_ls_pvt->d_GDOP); - //PDOP_vect.push_back (d_ls_pvt->d_PDOP); + /* *********************************** COMPUTE STATISTICS OVER THE FIRST 1000 SOLUTOINS **********************************/ + if (valid_solution_counter<=1000) + //if (d_ls_pvt->d_valid_observations >= 14) + //if (d_ls_pvt->d_valid_GPS_obs >= 7 or d_ls_pvt->d_valid_GAL_obs >=7) + { + GDOP_vect.push_back (d_ls_pvt->d_GDOP); + //PDOP_vect.push_back (d_ls_pvt->d_PDOP); - X_vect.push_back (d_ls_pvt->X); // add the last X value to X_vect - Y_vect.push_back (d_ls_pvt->Y); // add the last X value to X_vect - Z_vect.push_back (d_ls_pvt->Z); + X_vect.push_back (d_ls_pvt->X); // add the last X value to X_vect + Y_vect.push_back (d_ls_pvt->Y); // add the last X value to X_vect + Z_vect.push_back (d_ls_pvt->Z); - longitude_vect.push_back (d_ls_pvt->d_longitude_d); - latitude_vect.push_back (d_ls_pvt->d_latitude_d); - h_vect.push_back (d_ls_pvt->d_height_m); + longitude_vect.push_back (d_ls_pvt->d_longitude_d); + latitude_vect.push_back (d_ls_pvt->d_latitude_d); + h_vect.push_back (d_ls_pvt->d_height_m); - tot_obs_vect.push_back (d_ls_pvt->d_valid_observations); - Gal_obs_vect.push_back (d_ls_pvt->d_valid_GAL_obs); - GPS_obs_vect.push_back (d_ls_pvt->d_valid_GPS_obs); + tot_obs_vect.push_back (d_ls_pvt->d_valid_observations); + Gal_obs_vect.push_back (d_ls_pvt->d_valid_GAL_obs); + GPS_obs_vect.push_back (d_ls_pvt->d_valid_GPS_obs); - time_vect.push_back (d_ls_pvt->d_position_UTC_time); - //valid_solution_16_sat_counter ++; - } + time_vect.push_back (d_ls_pvt->d_position_UTC_time); + //valid_solution_16_sat_counter ++; + } - //if(valid_solution_16_sat_counter ==1000) + //if(valid_solution_16_sat_counter ==1000) - if (valid_solution_counter==1000) - { - //compute ECEF average and standard deviation over the first 1000 solutions - GDOP_sum=std::accumulate( GDOP_vect.begin(), GDOP_vect.end(), 0.0); - GDOP_mean = GDOP_sum / GDOP_vect.size(); + /* if (valid_solution_counter==1000) + { + //compute ECEF average and standard deviation over the first 1000 solutions + GDOP_sum = std::accumulate( GDOP_vect.begin(), GDOP_vect.end(), 0.0); + GDOP_mean = GDOP_sum / GDOP_vect.size(); - longitude_vect_sum = std::accumulate( longitude_vect.begin(), longitude_vect.end(), 0.0); - longitude_mean = longitude_vect_sum / longitude_vect.size(); + longitude_vect_sum = std::accumulate( longitude_vect.begin(), longitude_vect.end(), 0.0); + longitude_mean = longitude_vect_sum / longitude_vect.size(); - //IFEN true solutions - double ref_longitude= 11.80800563; - double ref_latitude= 48.17149767; + //IFEN true solutions + double ref_longitude = 11.80800563; + double ref_latitude = 48.17149767; - double ref_X=4171691.011; - double ref_Y=872120.003; - double ref_Z=4730005.761; + double ref_X = 4171691.011; + double ref_Y = 872120.003; + double ref_Z = 4730005.761; -// //REAL CAPTURE reference solutions (obtained with 4 Galileo) -// double ref_longitude= 1.987686994; -// double ref_latitude= 41.274786935; -// double ref_X=4797680.560; -// double ref_Y= 166506.414; -// double ref_Z=4185453.947; + // REAL CAPTURE reference solutions (obtained with 4 Galileo) + // double ref_longitude= 1.987686994; + // double ref_latitude= 41.274786935; + // double ref_X=4797680.560; + // double ref_Y= 166506.414; + // double ref_Z=4185453.947; - //compute mean value for precision - latitude_vect_sum = std::accumulate( latitude_vect.begin(), latitude_vect.end(), 0.0); - latitude_mean = latitude_vect_sum / latitude_vect.size(); + //compute mean value for precision + latitude_vect_sum = std::accumulate( latitude_vect.begin(), latitude_vect.end(), 0.0); + latitude_mean = latitude_vect_sum / latitude_vect.size(); - X_vect_sum = std::accumulate(X_vect.begin(), X_vect.end(), 0.0); - X_vect_mean = X_vect_sum / X_vect.size(); + X_vect_sum = std::accumulate(X_vect.begin(), X_vect.end(), 0.0); + X_vect_mean = X_vect_sum / X_vect.size(); - X_vect_sq_sum = std::inner_product(X_vect.begin(), X_vect.end(), X_vect.begin(), 0.0); - X_vect_stdev = std::sqrt(X_vect_sq_sum / X_vect.size() - X_vect_mean * X_vect_mean); + X_vect_sq_sum = std::inner_product(X_vect.begin(), X_vect.end(), X_vect.begin(), 0.0); + X_vect_stdev = std::sqrt(X_vect_sq_sum / X_vect.size() - X_vect_mean * X_vect_mean); - Y_vect_sum = std::accumulate(Y_vect.begin(), Y_vect.end(), 0.0); - Y_vect_mean = Y_vect_sum / Y_vect.size(); + Y_vect_sum = std::accumulate(Y_vect.begin(), Y_vect.end(), 0.0); + Y_vect_mean = Y_vect_sum / Y_vect.size(); - Y_vect_sq_sum = std::inner_product(Y_vect.begin(), Y_vect.end(), Y_vect.begin(), 0.0); - Y_vect_stdev = std::sqrt(Y_vect_sq_sum / Y_vect.size() - Y_vect_mean * Y_vect_mean); + Y_vect_sq_sum = std::inner_product(Y_vect.begin(), Y_vect.end(), Y_vect.begin(), 0.0); + Y_vect_stdev = std::sqrt(Y_vect_sq_sum / Y_vect.size() - Y_vect_mean * Y_vect_mean); - Z_vect_sum = std::accumulate(Z_vect.begin(), Z_vect.end(), 0.0); - Z_vect_mean = Z_vect_sum / Z_vect.size(); + Z_vect_sum = std::accumulate(Z_vect.begin(), Z_vect.end(), 0.0); + Z_vect_mean = Z_vect_sum / Z_vect.size(); - Z_vect_sq_sum = std::inner_product(Z_vect.begin(), Z_vect.end(), Z_vect.begin(), 0.0); - Z_vect_stdev = std::sqrt(Z_vect_sq_sum / Z_vect.size() - Z_vect_mean * Z_vect_mean); + Z_vect_sq_sum = std::inner_product(Z_vect.begin(), Z_vect.end(), Z_vect.begin(), 0.0); + Z_vect_stdev = std::sqrt(Z_vect_sq_sum / Z_vect.size() - Z_vect_mean * Z_vect_mean); - std::fstream file_solutions; - std::ofstream file_statitics; + std::fstream file_solutions; + std::ofstream file_statitics; - //std::fstream file_RES_X; - file_solutions.open ("GNSS_SDR_solutions.txt", std::fstream::out); - //file_statitics.open ("GNSS_SDR_statitics.txt", std::ofstream::out | std::ofstream::app); - file_statitics.open ("GNSS_SDR_statitics.txt", std::ofstream::out); + //std::fstream file_RES_X; + file_solutions.open ("GNSS_SDR_solutions.txt", std::fstream::out); + //file_statitics.open ("GNSS_SDR_statitics.txt", std::ofstream::out | std::ofstream::app); + file_statitics.open ("GNSS_SDR_statitics.txt", std::ofstream::out); - file_solutions.setf(file_solutions.fixed, file_solutions.floatfield); - file_statitics.setf(file_statitics.fixed, file_statitics.floatfield); + file_solutions.setf(file_solutions.fixed, file_solutions.floatfield); + file_statitics.setf(file_statitics.fixed, file_statitics.floatfield); - file_statitics << std::setprecision(9); + file_statitics << std::setprecision(9); - file_solutions << std::setw(13)<< "time" << std::setw(26)<<"X [m]" << std::setw(20)<< "Y [m]"<< std::setw(20)<< "Z [m]" << // X Y Z solutions - std::setw(20)<<"Long [deg]" << std::setw(15)<< "Lat [deg]"<< std::setw(15)<< "h [m]"<< // long, lat, h solutions - std::setw(18)<<"E(Acc) [m]" << std::setw(18)<< "N(Acc) [m]"<d_valid_GPS_obs << std::endl; - file_statitics << "Num of GALILEO observation " << d_ls_pvt->d_valid_GAL_obs << std::endl; + file_statitics << "Num of GPS observation " << d_ls_pvt->d_valid_GPS_obs << std::endl; + file_statitics << "Num of GALILEO observation " << d_ls_pvt->d_valid_GAL_obs << std::endl; - file_statitics << "GDOP mean= " << GDOP_mean << std::endl << std::endl; + file_statitics << "GDOP mean= " << GDOP_mean << std::endl << std::endl; - file_statitics << "ENU computed at (IFEN true coordinates): ref Longitude = " << ref_longitude << ", Ref Latitude = " << ref_latitude << " for Accuracy"<log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map); // } - // } + // } */ } } @@ -506,22 +502,17 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items, if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true) { std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " using "<d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; + << " using "<d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " using "<d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]"; + << " using "<d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]"; LOG(INFO) << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " using "<d_valid_observations<<" observations is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " - << d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP - << " GDOP = " << d_ls_pvt->d_GDOP; - - - - - + << " using "<d_valid_observations<<" observations is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " + << d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP + << " GDOP = " << d_ls_pvt->d_GDOP; } // MULTIPLEXED FILE RECORDING - Record results to file diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h index d05407bf8..98e0650eb 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h @@ -100,7 +100,7 @@ private: boost::shared_ptr d_queue; bool d_dump; bool b_rinex_header_writen; - Rinex_Printer *rp; + std::shared_ptr rp; unsigned int d_nchannels; std::string d_dump_filename; std::ofstream d_dump_file; @@ -112,8 +112,8 @@ private: long unsigned int valid_solution_counter; long unsigned int valid_solution_16_sat_counter; long unsigned int d_last_sample_nav_output; - Kml_Printer d_kml_dump; - Nmea_Printer *d_nmea_printer; + std::shared_ptr d_kml_dump; + std::shared_ptr d_nmea_printer; double d_rx_time; double d_TOW_at_curr_symbol_constellation; /* *********variable used for final statistics****/ @@ -209,8 +209,7 @@ private: double SEP; //3D-50% /* END variable used for final statistics */ - - hybrid_ls_pvt *d_ls_pvt; + std::shared_ptr d_ls_pvt; bool pseudoranges_pairCompare_min(std::pair a, std::pair b); public: diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h index 983ee9d68..afb6dded4 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.h @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include diff --git a/src/algorithms/PVT/libs/kml_printer.cc b/src/algorithms/PVT/libs/kml_printer.cc index ef1dd95fa..9532218a5 100644 --- a/src/algorithms/PVT/libs/kml_printer.cc +++ b/src/algorithms/PVT/libs/kml_printer.cc @@ -146,7 +146,7 @@ bool Kml_Printer::print_position_galileo(const std::shared_ptr& position, bool print_average_values) { double latitude; double longitude; diff --git a/src/algorithms/PVT/libs/kml_printer.h b/src/algorithms/PVT/libs/kml_printer.h index ab98765db..6a3c2fb7f 100644 --- a/src/algorithms/PVT/libs/kml_printer.h +++ b/src/algorithms/PVT/libs/kml_printer.h @@ -35,6 +35,7 @@ #include #include +#include #include #include "gps_l1_ca_ls_pvt.h" #include "galileo_e1_ls_pvt.h" @@ -53,7 +54,7 @@ public: bool set_headers(std::string filename); bool print_position(const std::shared_ptr& position, bool print_average_values); bool print_position_galileo(const std::shared_ptr& position, bool print_average_values); - bool print_position_hybrid(hybrid_ls_pvt* position, bool print_average_values); + bool print_position_hybrid(const std::shared_ptr& position, bool print_average_values); bool close_file(); Kml_Printer(); ~Kml_Printer();