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 bd136846d..310866d4d 100644 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc @@ -77,6 +77,13 @@ galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr(); d_kml_dump->set_headers(kml_dump_filename); + //initialize geojson_printer + std::string geojson_dump_filename; + geojson_dump_filename = d_dump_filename; + geojson_dump_filename.append(".geojson"); + d_geojson_printer = std::make_shared(); + d_geojson_printer->set_headers(geojson_dump_filename); + //initialize nmea_printer d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); d_dump_filename.append("_raw.dat"); @@ -223,9 +230,9 @@ int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_it if (pvt_result == true) { d_kml_dump->print_position(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); - // + d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging); + d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); + if (!b_rinex_header_writen) { std::map::iterator galileo_ephemeris_iter; diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h index 02b910f81..993c5ad05 100644 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h @@ -46,6 +46,7 @@ #include "nmea_printer.h" #include "kml_printer.h" #include "rinex_printer.h" +#include "geojson_printer.h" #include "galileo_e1_ls_pvt.h" #include "Galileo_E1.h" @@ -108,6 +109,8 @@ private: long unsigned int d_last_sample_nav_output; std::shared_ptr d_kml_dump; std::shared_ptr d_nmea_printer; + std::shared_ptr d_geojson_printer; + double d_rx_time; std::shared_ptr d_ls_pvt; bool pseudoranges_pairCompare_min(const std::pair& a, const std::pair& b); 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 7ad3e2dea..9fa67eb3e 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 @@ -87,8 +87,15 @@ gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, std::string kml_dump_filename; kml_dump_filename = d_dump_filename; kml_dump_filename.append(".kml"); - d_kml_dump = std::make_shared(); - d_kml_dump->set_headers(kml_dump_filename); + d_kml_printer = std::make_shared(); + d_kml_printer->set_headers(kml_dump_filename); + + //initialize geojson_printer + std::string geojson_dump_filename; + geojson_dump_filename = d_dump_filename; + geojson_dump_filename.append(".geojson"); + d_geojson_printer = std::make_shared(); + d_geojson_printer->set_headers(geojson_dump_filename); //initialize nmea_printer d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); @@ -261,7 +268,8 @@ int gps_l1_ca_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_ite pvt_result = d_ls_pvt->get_PVT(gnss_pseudoranges_map, d_rx_time, d_flag_averaging); if (pvt_result == true) { - d_kml_dump->print_position(d_ls_pvt, d_flag_averaging); + d_kml_printer->print_position(d_ls_pvt, d_flag_averaging); + d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging); d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); if (!b_rinex_header_writen) diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h index 645760ee3..976404699 100644 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h @@ -45,6 +45,7 @@ #include "nmea_printer.h" #include "kml_printer.h" #include "rinex_printer.h" +#include "geojson_printer.h" #include "gps_l1_ca_ls_pvt.h" #include "GPS_L1_CA.h" @@ -107,8 +108,9 @@ private: int d_display_rate_ms; long unsigned int d_sample_counter; long unsigned int d_last_sample_nav_output; - std::shared_ptr d_kml_dump; + std::shared_ptr d_kml_printer; std::shared_ptr d_nmea_printer; + std::shared_ptr d_geojson_printer; double d_rx_time; std::shared_ptr d_ls_pvt; diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc index 601919e74..cfa5fa404 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc @@ -81,6 +81,13 @@ hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, boost::shared_ptr(); d_kml_dump->set_headers(kml_dump_filename); + //initialize geojson_printer + std::string geojson_dump_filename; + geojson_dump_filename = d_dump_filename; + geojson_dump_filename.append(".geojson"); + d_geojson_printer = std::make_shared(); + d_geojson_printer->set_headers(geojson_dump_filename); + //initialize nmea_printer d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); @@ -279,9 +286,9 @@ int hybrid_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items, if (pvt_result == true) { d_kml_dump->print_position(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); - // + d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging); + d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); + if (!b_rinex_header_writen) // & we have utc data in nav message! { std::map::iterator galileo_ephemeris_iter; diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h index 2ac6b1eb0..777c38829 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h @@ -49,7 +49,9 @@ #include "gps_iono.h" #include "nmea_printer.h" #include "kml_printer.h" +#include "geojson_printer.h" #include "rinex_printer.h" +#include "nmea_printer.h" #include "hybrid_ls_pvt.h" #include "GPS_L1_CA.h" #include "Galileo_E1.h" @@ -113,6 +115,7 @@ private: long unsigned int d_last_sample_nav_output; std::shared_ptr d_kml_dump; std::shared_ptr d_nmea_printer; + std::shared_ptr d_geojson_printer; double d_rx_time; double d_TOW_at_curr_symbol_constellation; std::shared_ptr d_ls_pvt; diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 47b2b162c..e64975e1e 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -19,6 +19,7 @@ add_definitions( -DGNSS_SDR_VERSION="${VERSION}" ) set(PVT_LIB_SOURCES + pvt_solution.cc ls_pvt.cc gps_l1_ca_ls_pvt.cc galileo_e1_ls_pvt.cc diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc index 9dde4c9e7..157c034ab 100644 --- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc +++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc @@ -251,60 +251,12 @@ bool galileo_e1_ls_pvt::get_PVT(std::map gnss_pseudoranges_map } // MOVING AVERAGE PVT - if (flag_averaging == true) - { - if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth) - { - // Pop oldest value - d_hist_longitude_d.pop_back(); - d_hist_latitude_d.pop_back(); - d_hist_height_m.pop_back(); - // Push new values - d_hist_longitude_d.push_front(d_longitude_d); - d_hist_latitude_d.push_front(d_latitude_d); - d_hist_height_m.push_front(d_height_m); - - d_avg_latitude_d = 0; - d_avg_longitude_d = 0; - d_avg_height_m = 0; - for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++) - { - d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); - d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); - d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); - } - d_avg_latitude_d = d_avg_latitude_d / static_cast(d_averaging_depth); - d_avg_longitude_d = d_avg_longitude_d / static_cast(d_averaging_depth); - d_avg_height_m = d_avg_height_m / static_cast(d_averaging_depth); - b_valid_position = true; - return true; //indicates that the returned position is valid - } - else - { - // 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); - d_hist_height_m.push_front(d_height_m); - - d_avg_latitude_d = d_latitude_d; - d_avg_longitude_d = d_longitude_d; - d_avg_height_m = d_height_m; - b_valid_position = false; - return false; //indicates that the returned position is not valid yet - } - } - else - { - b_valid_position = true; - return true; //indicates that the returned position is valid - } + galileo_e1_ls_pvt::pos_averaging(flag_averaging); } else { b_valid_position = false; - return false; } - return false; + return b_valid_position; } diff --git a/src/algorithms/PVT/libs/geojson_printer.cc b/src/algorithms/PVT/libs/geojson_printer.cc index eaef9ea7c..8668d1c0c 100644 --- a/src/algorithms/PVT/libs/geojson_printer.cc +++ b/src/algorithms/PVT/libs/geojson_printer.cc @@ -48,16 +48,26 @@ GeoJSON_Printer::~GeoJSON_Printer () bool GeoJSON_Printer::set_headers(std::string filename) { geojson_file.open(filename.c_str()); + filename_ = filename; + first_pos = true; if (geojson_file.is_open()) { DLOG(INFO) << "GeoJSON printer writing on " << filename.c_str(); + // Set iostream numeric format and precision geojson_file.setf(geojson_file.fixed, geojson_file.floatfield); geojson_file << std::setprecision(14); + + // Writing the header geojson_file << "{" << std::endl; geojson_file << " \"type\": \"Feature\"," << std::endl; + geojson_file << " \"properties\": {" << std::endl; + geojson_file << " \"name\": \"Locations generated by GNSS-SDR\" " << std::endl; + geojson_file << " }," << std::endl; + + geojson_file << " \"geometry\": {" << std::endl; - geojson_file << " \"type\": \"MultiPoint\"," << std::endl; + geojson_file << " \"type\": \"MultiPoint\"," << std::endl; geojson_file << " \"coordinates\": [" << std::endl; return true; @@ -70,13 +80,13 @@ bool GeoJSON_Printer::set_headers(std::string filename) -bool GeoJSON_Printer::print_position(const std::shared_ptr& position, bool print_average_values) +bool GeoJSON_Printer::print_position(const std::shared_ptr& position, bool print_average_values) { double latitude; double longitude; double height; - std::shared_ptr position_ = position; + std::shared_ptr position_ = position; if (print_average_values == false) { @@ -93,7 +103,16 @@ bool GeoJSON_Printer::print_position(const std::shared_ptr& position, bo if (geojson_file.is_open()) { - geojson_file << " [" << longitude << ", " << latitude << ", " << height << "]," << std::endl; + if (first_pos == true) + { + geojson_file << " [" << longitude << ", " << latitude << ", " << height << "]"; + first_pos = false; + } + else + { + geojson_file << "," << std::endl; + geojson_file << " [" << longitude << ", " << latitude << ", " << height << "]"; + } return true; } else @@ -108,10 +127,18 @@ bool GeoJSON_Printer::close_file() { if (geojson_file.is_open()) { + geojson_file << std::endl; geojson_file << " ]" << std::endl; - geojson_file << " }" << std::endl; + geojson_file << " }" << std::endl; geojson_file << "}" << std::endl; geojson_file.close(); + + // if nothing is written, erase the file + if (first_pos == true) + { + if(remove(filename_.c_str()) != 0) LOG(INFO) << "Error deleting temporary file"; + } + return true; } else diff --git a/src/algorithms/PVT/libs/geojson_printer.h b/src/algorithms/PVT/libs/geojson_printer.h index aabc9941c..3c15883e0 100644 --- a/src/algorithms/PVT/libs/geojson_printer.h +++ b/src/algorithms/PVT/libs/geojson_printer.h @@ -37,7 +37,7 @@ #include #include #include -#include "ls_pvt.h" +#include "pvt_solution.h" /*! @@ -49,11 +49,13 @@ class GeoJSON_Printer { private: std::ofstream geojson_file; + bool first_pos; + std::string filename_; public: GeoJSON_Printer(); ~GeoJSON_Printer(); bool set_headers(std::string filename); - bool print_position(const std::shared_ptr& position, bool print_average_values); + bool print_position(const std::shared_ptr& position, bool print_average_values); bool close_file(); }; 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 e0e83bec2..5598f9626 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -171,10 +171,10 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, gps_l1_ca_ls_pvt::cart2geo(mypos(0), mypos(1), mypos(2), 4); //ToDo: Find an Observables/PVT random bug with some satellite configurations that gives an erratic PVT solution (i.e. height>50 km) if (d_height_m > 50000) - { - b_valid_position = false; - return false; - } + { + b_valid_position = false; + return false; + } // Compute UTC time and print PVT solution double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60) boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast(GPS_week)); @@ -183,8 +183,8 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, d_position_UTC_time = p_time; LOG(INFO) << "(new)Position at " << boost::posix_time::to_simple_string(p_time) - << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d - << " [deg], Height= " << d_height_m << " [m]"; + << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d + << " [deg], Height= " << d_height_m << " [m]"; // ###### Compute DOPs ######## gps_l1_ca_ls_pvt::compute_DOP(); @@ -228,60 +228,13 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, } // MOVING AVERAGE PVT - if (flag_averaging == true) - { - if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth) - { - // Pop oldest value - d_hist_longitude_d.pop_back(); - d_hist_latitude_d.pop_back(); - d_hist_height_m.pop_back(); - // Push new values - d_hist_longitude_d.push_front(d_longitude_d); - d_hist_latitude_d.push_front(d_latitude_d); - d_hist_height_m.push_front(d_height_m); - - d_avg_latitude_d = 0; - d_avg_longitude_d = 0; - d_avg_height_m = 0; - for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++) - { - d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); - d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); - d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); - } - d_avg_latitude_d = d_avg_latitude_d / static_cast(d_averaging_depth); - d_avg_longitude_d = d_avg_longitude_d / static_cast(d_averaging_depth); - d_avg_height_m = d_avg_height_m / static_cast(d_averaging_depth); - b_valid_position = true; - return true; //indicates that the returned position is valid - } - else - { - //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); - d_hist_height_m.push_front(d_height_m); - - d_avg_latitude_d = d_latitude_d; - d_avg_longitude_d = d_longitude_d; - d_avg_height_m = d_height_m; - b_valid_position = false; - return false; //indicates that the returned position is not valid yet - } - } - else - { - b_valid_position = true; - return true; //indicates that the returned position is valid - } + gps_l1_ca_ls_pvt::pos_averaging(flag_averaging); } else { b_valid_position = false; - return false; } + return b_valid_position; } diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc index 41d280144..e387a180d 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.cc +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.cc @@ -172,10 +172,10 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, do // SV ECEF DEBUG OUTPUT DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN - << " X=" << galileo_ephemeris_iter->second.d_satpos_X - << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y - << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z - << " [m] PR_obs=" << obs(obs_counter) << " [m]"; + << " X=" << galileo_ephemeris_iter->second.d_satpos_X + << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y + << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z + << " [m] PR_obs=" << obs(obs_counter) << " [m]"; } else // the ephemeris are not available for this SV @@ -275,8 +275,8 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, do { b_valid_position = false; LOG(INFO) << "Hybrid 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]"; + << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d + << " [deg], Height= " << d_height_m << " [m]"; //std::cout << "Hybrid Position at " << boost::posix_time::to_simple_string(p_time) // << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d @@ -285,8 +285,8 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, do } LOG(INFO) << "Hybrid 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]"; + << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d + << " [deg], Height= " << d_height_m << " [m]"; // ###### Compute DOPs ######## @@ -331,60 +331,12 @@ bool hybrid_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, do } // MOVING AVERAGE PVT - if (flag_averaging == true) - { - if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth) - { - // Pop oldest value - d_hist_longitude_d.pop_back(); - d_hist_latitude_d.pop_back(); - d_hist_height_m.pop_back(); - // Push new values - d_hist_longitude_d.push_front(d_longitude_d); - d_hist_latitude_d.push_front(d_latitude_d); - d_hist_height_m.push_front(d_height_m); - - d_avg_latitude_d = 0; - d_avg_longitude_d = 0; - d_avg_height_m = 0; - for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++) - { - d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); - d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); - d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); - } - d_avg_latitude_d = d_avg_latitude_d / static_cast(d_averaging_depth); - d_avg_longitude_d = d_avg_longitude_d / static_cast(d_averaging_depth); - d_avg_height_m = d_avg_height_m / static_cast(d_averaging_depth); - b_valid_position = true; - return true; //indicates that the returned position is valid - } - else - { - // 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); - d_hist_height_m.push_front(d_height_m); - - d_avg_latitude_d = d_latitude_d; - d_avg_longitude_d = d_longitude_d; - d_avg_height_m = d_height_m; - b_valid_position = false; - return false; //indicates that the returned position is not valid yet - } - } - else - { - b_valid_position = true; - return true; //indicates that the returned position is valid - } + hybrid_ls_pvt::pos_averaging(flag_averaging); } else { b_valid_position = false; - return false; } - return false; + return b_valid_position; } diff --git a/src/algorithms/PVT/libs/kml_printer.cc b/src/algorithms/PVT/libs/kml_printer.cc index c74ba2450..2482dde4d 100644 --- a/src/algorithms/PVT/libs/kml_printer.cc +++ b/src/algorithms/PVT/libs/kml_printer.cc @@ -82,13 +82,13 @@ bool Kml_Printer::set_headers(std::string filename) -bool Kml_Printer::print_position(const std::shared_ptr& position, bool print_average_values) +bool Kml_Printer::print_position(const std::shared_ptr& position, bool print_average_values) { double latitude; double longitude; double height; - std::shared_ptr position_ = position; + std::shared_ptr position_ = position; if (print_average_values == false) { diff --git a/src/algorithms/PVT/libs/kml_printer.h b/src/algorithms/PVT/libs/kml_printer.h index 6f90d7321..5fb852221 100644 --- a/src/algorithms/PVT/libs/kml_printer.h +++ b/src/algorithms/PVT/libs/kml_printer.h @@ -37,7 +37,7 @@ #include #include #include -#include "ls_pvt.h" +#include "pvt_solution.h" /*! * \brief Prints PVT information to OGC KML format file (can be viewed with Google Earth) @@ -52,7 +52,7 @@ public: Kml_Printer(); ~Kml_Printer(); bool set_headers(std::string filename); - bool print_position(const std::shared_ptr& position, bool print_average_values); + bool print_position(const std::shared_ptr& position, bool print_average_values); bool close_file(); }; diff --git a/src/algorithms/PVT/libs/ls_pvt.cc b/src/algorithms/PVT/libs/ls_pvt.cc index b78d198fb..a618692aa 100644 --- a/src/algorithms/PVT/libs/ls_pvt.cc +++ b/src/algorithms/PVT/libs/ls_pvt.cc @@ -1,5 +1,5 @@ /*! - * \file ls_pvt.h + * \file ls_pvt.cc * \brief Implementation of a base class for Least Squares PVT solutions * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es * @@ -38,30 +38,15 @@ using google::LogMessage; -DEFINE_bool(tropo, true, "Apply tropospheric correction"); +//DEFINE_bool(tropo, true, "Apply tropospheric correction"); -Ls_Pvt::Ls_Pvt() +Ls_Pvt::Ls_Pvt() : Pvt_Solution() { - d_valid_observations = 0; - d_latitude_d = 0.0; - d_longitude_d = 0.0; - d_height_m = 0.0; - d_avg_latitude_d = 0.0; - d_avg_longitude_d = 0.0; - d_avg_height_m = 0.0; d_x_m = 0.0; d_y_m = 0.0; d_z_m = 0.0; - d_GDOP = 0.0; - d_PDOP = 0.0; - d_HDOP = 0.0; - d_VDOP = 0.0; - d_TDOP = 0.0; - d_flag_averaging = false; - b_valid_position = false; - d_averaging_depth = 0; } arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w) @@ -173,435 +158,3 @@ arma::vec Ls_Pvt::leastSquarePos(const arma::mat & satpos, const arma::vec & obs } return pos; } - - - -arma::vec Ls_Pvt::rotateSatellite(double const traveltime, const arma::vec & X_sat) -{ - /* - * Returns rotated satellite ECEF coordinates due to Earth - * rotation during signal travel time - * - * Inputs: - * travelTime - signal travel time - * X_sat - satellite's ECEF coordinates - * - * Returns: - * X_sat_rot - rotated satellite's coordinates (ECEF) - */ - - //--- Find rotation angle -------------------------------------------------- - double omegatau; - omegatau = OMEGA_EARTH_DOT * traveltime; - - //--- Build a rotation matrix ---------------------------------------------- - arma::mat R3 = arma::zeros(3,3); - R3(0, 0) = cos(omegatau); - R3(0, 1) = sin(omegatau); - R3(0, 2) = 0.0; - R3(1, 0) = -sin(omegatau); - R3(1, 1) = cos(omegatau); - R3(1, 2) = 0.0; - R3(2, 0) = 0.0; - R3(2, 1) = 0.0; - R3(2, 2) = 1; - - //--- Do the rotation ------------------------------------------------------ - arma::vec X_sat_rot; - X_sat_rot = R3 * X_sat; - return X_sat_rot; -} - - -void Ls_Pvt::cart2geo(double X, double Y, double Z, int elipsoid_selection) -{ - /* Conversion of Cartesian coordinates (X,Y,Z) to geographical - coordinates (latitude, longitude, h) on a selected reference ellipsoid. - - Choices of Reference Ellipsoid for Geographical Coordinates - 0. International Ellipsoid 1924 - 1. International Ellipsoid 1967 - 2. World Geodetic System 1972 - 3. Geodetic Reference System 1980 - 4. World Geodetic System 1984 - */ - - const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0}; - const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563}; - - double lambda = atan2(Y, X); - double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection])); - double c = a[elipsoid_selection] * sqrt(1.0 + ex2); - double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection]))); - - double h = 0.1; - double oldh = 0.0; - double N; - int iterations = 0; - do - { - oldh = h; - N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); - phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) )))); - h = sqrt(X * X + Y * Y) / cos(phi) - N; - iterations = iterations + 1; - if (iterations > 100) - { - LOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh; - break; - } - } - while (std::abs(h - oldh) > 1.0e-12); - d_latitude_d = phi * 180.0 / GPS_PI; - d_longitude_d = lambda * 180.0 / GPS_PI; - d_height_m = h; -} - - -void Ls_Pvt::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z) -{ - /* Subroutine to calculate geodetic coordinates latitude, longitude, - height given Cartesian coordinates X,Y,Z, and reference ellipsoid - values semi-major axis (a) and the inverse of flattening (finv). - - The output units of angular quantities will be in decimal degrees - (15.5 degrees not 15 deg 30 min). The output units of h will be the - same as the units of X,Y,Z,a. - - Inputs: - a - semi-major axis of the reference ellipsoid - finv - inverse of flattening of the reference ellipsoid - X,Y,Z - Cartesian coordinates - - Outputs: - dphi - latitude - dlambda - longitude - h - height above reference ellipsoid - - Based in a Matlab function by Kai Borre - */ - - *h = 0; - double tolsq = 1.e-10; // tolerance to accept convergence - int maxit = 10; // max number of iterations - double rtd = 180.0 / GPS_PI; - - // compute square of eccentricity - double esq; - if (finv < 1.0E-20) - { - esq = 0.0; - } - else - { - esq = (2.0 - 1.0 / finv) / finv; - } - - // 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; - } - else - { - *dlambda = 0.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; - if (r > 1.0E-20) - { - sinphi = Z/r; - } - else - { - sinphi = 0.0; - } - *dphi = asin(sinphi); - - // initial value of height = distance from origin minus - // approximate distance from origin to surface of ellipsoid - if (r < 1.0E-20) - { - *h = 0; - return; - } - - *h = r - a * (1 - sinphi * sinphi/finv); - - // iterate - double cosphi; - double N_phi; - double dP; - double dZ; - double oneesq = 1.0 - esq; - - for (int i = 0; i < maxit; i++) - { - sinphi = sin(*dphi); - cosphi = cos(*dphi); - - // compute radius of curvature in prime vertical direction - N_phi = a / sqrt(1 - esq * sinphi * sinphi); - - // compute residuals in P and Z - dP = P - (N_phi + (*h)) * cosphi; - dZ = Z - (N_phi * oneesq + (*h)) * sinphi; - - // update height and latitude - *h = *h + (sinphi * dZ + cosphi * dP); - *dphi = *dphi + (cosphi * dZ - sinphi * dP)/(N_phi + (*h)); - - // test for convergence - if ((dP * dP + dZ * dZ) < tolsq) - { - break; - } - if (i == (maxit - 1)) - { - LOG(WARNING) << "The computation of geodetic coordinates did not converge"; - } - } - *dphi = (*dphi) * rtd; -} - - -void Ls_Pvt::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km) -{ - /* Inputs: - sinel - sin of elevation angle of satellite - hsta_km - height of station in km - p_mb - atmospheric pressure in mb at height hp_km - t_kel - surface temperature in degrees Kelvin at height htkel_km - hum - humidity in % at height hhum_km - hp_km - height of pressure measurement in km - htkel_km - height of temperature measurement in km - hhum_km - height of humidity measurement in km - - Outputs: - ddr_m - range correction (meters) - - Reference - Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric - Refraction Correction Model. Paper presented at the - American Geophysical Union Annual Fall Meeting, San - Francisco, December 12-17 - - Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre - */ - - const double a_e = 6378.137; // semi-major axis of earth ellipsoid - const double b0 = 7.839257e-5; - const double tlapse = -6.5; - const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5); - - double tkhum = t_kel + tlapse * (hhum_km - htkel_km); - double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15); - double e0 = 0.0611 * hum * pow(10, atkel); - double tksea = t_kel - tlapse * htkel_km; - double tkelh = tksea + tlapse * hhum_km; - double e0sea = e0 * pow((tksea / tkelh), (4 * em)); - double tkelp = tksea + tlapse * hp_km; - double psea = p_mb * pow((tksea / tkelp), em); - - if(sinel < 0) { sinel = 0.0; } - - double tropo_delay = 0.0; - bool done = false; - double refsea = 77.624e-6 / tksea; - double htop = 1.1385e-5 / refsea; - refsea = refsea * psea; - double ref = refsea * pow(((htop - hsta_km) / htop), 4); - - double a; - double b; - double rtop; - - while(1) - { - rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2)); - - // check to see if geometry is crazy - if(rtop < 0) { rtop = 0; } - - rtop = sqrt(rtop) - (a_e + hsta_km) * sinel; - - a = -sinel / (htop - hsta_km); - b = -b0 * (1 - pow(sinel,2)) / (htop - hsta_km); - - arma::vec rn = arma::vec(8); - rn.zeros(); - - for(int i = 0; i<8; i++) - { - rn(i) = pow(rtop, (i+1+1)); - - } - - arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b /3, a * (pow(a, 2) + 3 * b), - pow(a, 4)/5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b)/3, - pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0}; - - if(pow(b, 2) > 1.0e-35) - { - alpha(6) = a * pow(b, 3) /2; - alpha(7) = pow(b, 4) / 9; - } - - double dr = rtop; - arma::mat aux_ = alpha * rn; - dr = dr + aux_(0, 0); - tropo_delay = tropo_delay + dr * ref * 1000; - - if(done == true) - { - *ddr_m = tropo_delay; - break; - } - - done = true; - refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea; - htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea; - ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4); - } -} - -void Ls_Pvt::topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx) -{ - /* Transformation of vector dx into topocentric coordinate - system with origin at x - Inputs: - x - vector origin coordinates (in ECEF system [X; Y; Z;]) - dx - vector ([dX; dY; dZ;]). - - Outputs: - D - vector length. Units like the input - Az - azimuth from north positive clockwise, degrees - El - elevation angle, degrees - - Based on a Matlab function by Kai Borre - */ - - double lambda; - double phi; - double h; - double dtr = GPS_PI / 180.0; - double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84 - double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84 - - // Transform x into geodetic coordinates - Ls_Pvt::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2)); - - double cl = cos(lambda * dtr); - double sl = sin(lambda * dtr); - double cb = cos(phi * dtr); - double sb = sin(phi * dtr); - - arma::mat F = arma::zeros(3,3); - - F(0,0) = -sl; - F(0,1) = -sb * cl; - F(0,2) = cb * cl; - - F(1,0) = cl; - F(1,1) = -sb * sl; - F(1,2) = cb * sl; - - F(2,0) = 0; - F(2,1) = cb; - F(2,2) = sb; - - arma::vec local_vector; - - local_vector = arma::htrans(F) * dx; - - double E = local_vector(0); - double N = local_vector(1); - double U = local_vector(2); - - double hor_dis; - hor_dis = sqrt(E * E + N * N); - - if (hor_dis < 1.0E-20) - { - *Az = 0; - *El = 90; - } - else - { - *Az = atan2(E, N) / dtr; - *El = atan2(U, hor_dis) / dtr; - } - - if (*Az < 0) - { - *Az = *Az + 360.0; - } - - *D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); -} - - - -int Ls_Pvt::compute_DOP() -{ - // ###### Compute DOPs ######## - - // 1- Rotation matrix from ECEF coordinates to ENU coordinates - // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates - arma::mat F = arma::zeros(3,3); - F(0,0) = -sin(GPS_TWO_PI * (d_longitude_d/360.0)); - F(0,1) = -sin(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0)); - F(0,2) = cos(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0)); - - F(1,0) = cos((GPS_TWO_PI * d_longitude_d)/360.0); - F(1,1) = -sin((GPS_TWO_PI * d_latitude_d)/360.0) * sin((GPS_TWO_PI * d_longitude_d)/360.0); - F(1,2) = cos((GPS_TWO_PI * d_latitude_d/360.0)) * sin((GPS_TWO_PI * d_longitude_d)/360.0); - - F(2,0) = 0; - F(2,1) = cos((GPS_TWO_PI * d_latitude_d)/360.0); - F(2,2) = sin((GPS_TWO_PI * d_latitude_d/360.0)); - - // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS) - arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2); - arma::mat DOP_ENU = arma::zeros(3, 3); - - try - { - DOP_ENU = arma::htrans(F) * Q_ECEF * F; - d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP - d_PDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1) + DOP_ENU(2, 2));// PDOP - d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1)); // HDOP - d_VDOP = sqrt(DOP_ENU(2, 2)); // VDOP - d_TDOP = sqrt(d_Q(3, 3)); // TDOP - } - catch(std::exception& ex) - { - d_GDOP = -1; // Geometric DOP - d_PDOP = -1; // PDOP - d_HDOP = -1; // HDOP - d_VDOP = -1; // VDOP - d_TDOP = -1; // TDOP - } - return 0; - -} - - - - -int Ls_Pvt::set_averaging_depth(int depth) -{ - d_averaging_depth = depth; - return 0; -} diff --git a/src/algorithms/PVT/libs/ls_pvt.h b/src/algorithms/PVT/libs/ls_pvt.h index eb923f2f8..385d6e437 100644 --- a/src/algorithms/PVT/libs/ls_pvt.h +++ b/src/algorithms/PVT/libs/ls_pvt.h @@ -33,144 +33,21 @@ #define GNSS_SDR_LS_PVT_H_ -#include -#include -#include - -#define PVT_MAX_CHANNELS 24 +#include "pvt_solution.h" /*! * \brief Base class for the Least Squares PVT solution * */ -class Ls_Pvt +class Ls_Pvt : public Pvt_Solution { public: Ls_Pvt(); arma::vec leastSquarePos(const arma::mat & satpos, const arma::vec & obs, const arma::mat & w); - arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat); - - 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 - - boost::posix_time::ptime d_position_UTC_time; - - bool b_valid_position; - - double d_latitude_d; //!< Latitude in degrees - double d_longitude_d; //!< Longitude in degrees - double d_height_m; //!< Height [m] - - //averaging - int d_averaging_depth; //!< Length of averaging window - std::deque d_hist_latitude_d; - std::deque d_hist_longitude_d; - std::deque d_hist_height_m; - - 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; - double d_HDOP; - double d_VDOP; - double d_TDOP; - int compute_DOP(); //!< Compute Dilution Of Precision - - bool d_flag_averaging; - - int set_averaging_depth(int depth); - - - /*! - * \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical - * coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid. - * - * \param[in] X [m] Cartesian coordinate - * \param[in] Y [m] Cartesian coordinate - * \param[in] Z [m] Cartesian coordinate - * \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates: - * 0 - International Ellipsoid 1924. - * 1 - International Ellipsoid 1967. - * 2 - World Geodetic System 1972. - * 3 - Geodetic Reference System 1980. - * 4 - World Geodetic System 1984. - * - */ - void cart2geo(double X, double Y, double Z, int elipsoid_selection); - - /*! - * \brief Transformation of vector dx into topocentric coordinate system with origin at x - * - * \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;]) - * \param[in] dx Vector ([dX; dY; dZ;]). - * - * \param[out] D Vector length. Units like the input - * \param[out] Az Azimuth from north positive clockwise, degrees - * \param[out] El Elevation angle, degrees - * - * Based on a Matlab function by Kai Borre - */ - void topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx); - - /*! - * \brief Subroutine to calculate geodetic coordinates latitude, longitude, - * height given Cartesian coordinates X,Y,Z, and reference ellipsoid - * values semi-major axis (a) and the inverse of flattening (finv). - * - * The output units of angular quantities will be in decimal degrees - * (15.5 degrees not 15 deg 30 min). The output units of h will be the - * same as the units of X,Y,Z,a. - * - * \param[in] a - semi-major axis of the reference ellipsoid - * \param[in] finv - inverse of flattening of the reference ellipsoid - * \param[in] X,Y,Z - Cartesian coordinates - *: - * \param[out] dphi - latitude - * \param[out] dlambda - longitude - * \param[out] h - height above reference ellipsoid - * - * Based in a Matlab function by Kai Borre - */ - void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); - - /*! - * \brief Tropospheric correction - * - * \param[in] sinel - sin of elevation angle of satellite - * \param[in] hsta_km - height of station in km - * \param[in] p_mb - atmospheric pressure in mb at height hp_km - * \param[in] t_kel - surface temperature in degrees Kelvin at height htkel_km - * \param[in] hum - humidity in % at height hhum_km - * \param[in] hp_km - height of pressure measurement in km - * \param[in] htkel_km - height of temperature measurement in km - * \param[in] hhum_km - height of humidity measurement in km - * - * \param[out] ddr_m - range correction (meters) - * - * - * Reference: - * Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric - * Refraction Correction Model. Paper presented at the - * American Geophysical Union Annual Fall Meeting, San - * Francisco, December 12-17 - * - * Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre - */ - void tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km); - }; #endif diff --git a/src/algorithms/PVT/libs/nmea_printer.cc b/src/algorithms/PVT/libs/nmea_printer.cc index ceda216d7..c3213b90e 100644 --- a/src/algorithms/PVT/libs/nmea_printer.cc +++ b/src/algorithms/PVT/libs/nmea_printer.cc @@ -39,7 +39,7 @@ #include #include #include -#include "GPS_L1_CA.h" + using google::LogMessage; @@ -132,7 +132,7 @@ void Nmea_Printer::close_serial () } -bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr& pvt_data, bool print_average_values) +bool Nmea_Printer::Print_Nmea_Line(const std::shared_ptr& pvt_data, bool print_average_values) { std::string GPRMC; std::string GPGGA; diff --git a/src/algorithms/PVT/libs/nmea_printer.h b/src/algorithms/PVT/libs/nmea_printer.h index b613d45c4..115ab07c3 100644 --- a/src/algorithms/PVT/libs/nmea_printer.h +++ b/src/algorithms/PVT/libs/nmea_printer.h @@ -40,7 +40,7 @@ #include #include #include -#include "ls_pvt.h" +#include "pvt_solution.h" /*! @@ -60,7 +60,7 @@ public: /*! * \brief Print NMEA PVT and satellite info to the initialized device */ - bool Print_Nmea_Line(const std::shared_ptr& position, bool print_average_values); + bool Print_Nmea_Line(const std::shared_ptr& position, bool print_average_values); /*! * \brief Default destructor. @@ -72,7 +72,7 @@ private: std::ofstream nmea_file_descriptor; // Output file stream for NMEA log file std::string nmea_devname; int nmea_dev_descriptor; // NMEA serial device descriptor (i.e. COM port) - std::shared_ptr d_PVT_data; + std::shared_ptr d_PVT_data; int init_serial(std::string serial_device); //serial port control void close_serial(); std::string get_GPGGA(); // fix data diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc new file mode 100644 index 000000000..fd2edc4fd --- /dev/null +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -0,0 +1,548 @@ +/*! + * \file pvt_solution.cc + * \brief Implementation of a base class for a PVT solution + * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "pvt_solution.h" +#include +#include "GPS_L1_CA.h" +#include +#include + + +using google::LogMessage; + +DEFINE_bool(tropo, true, "Apply tropospheric correction"); + +Pvt_Solution::Pvt_Solution() +{ + d_latitude_d = 0.0; + d_longitude_d = 0.0; + d_height_m = 0.0; + d_avg_latitude_d = 0.0; + d_avg_longitude_d = 0.0; + d_avg_height_m = 0.0; + d_GDOP = 0.0; + d_PDOP = 0.0; + d_HDOP = 0.0; + d_VDOP = 0.0; + d_TDOP = 0.0; + d_flag_averaging = false; + b_valid_position = false; + d_averaging_depth = 0; + d_valid_observations = 0; +} + +arma::vec Pvt_Solution::rotateSatellite(double const traveltime, const arma::vec & X_sat) +{ + /* + * Returns rotated satellite ECEF coordinates due to Earth + * rotation during signal travel time + * + * Inputs: + * travelTime - signal travel time + * X_sat - satellite's ECEF coordinates + * + * Returns: + * X_sat_rot - rotated satellite's coordinates (ECEF) + */ + + //--- Find rotation angle -------------------------------------------------- + double omegatau; + omegatau = OMEGA_EARTH_DOT * traveltime; + + //--- Build a rotation matrix ---------------------------------------------- + arma::mat R3 = arma::zeros(3,3); + R3(0, 0) = cos(omegatau); + R3(0, 1) = sin(omegatau); + R3(0, 2) = 0.0; + R3(1, 0) = -sin(omegatau); + R3(1, 1) = cos(omegatau); + R3(1, 2) = 0.0; + R3(2, 0) = 0.0; + R3(2, 1) = 0.0; + R3(2, 2) = 1; + + //--- Do the rotation ------------------------------------------------------ + arma::vec X_sat_rot; + X_sat_rot = R3 * X_sat; + return X_sat_rot; +} + + +int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) +{ + /* Conversion of Cartesian coordinates (X,Y,Z) to geographical + coordinates (latitude, longitude, h) on a selected reference ellipsoid. + + Choices of Reference Ellipsoid for Geographical Coordinates + 0. International Ellipsoid 1924 + 1. International Ellipsoid 1967 + 2. World Geodetic System 1972 + 3. Geodetic Reference System 1980 + 4. World Geodetic System 1984 + */ + + const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0}; + const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563}; + + double lambda = atan2(Y, X); + double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection])); + double c = a[elipsoid_selection] * sqrt(1.0 + ex2); + double phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection]))); + + double h = 0.1; + double oldh = 0.0; + double N; + int iterations = 0; + do + { + oldh = h; + N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi))); + phi = atan(Z / ((sqrt(X * X + Y * Y) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h) )))); + h = sqrt(X * X + Y * Y) / cos(phi) - N; + iterations = iterations + 1; + if (iterations > 100) + { + LOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh; + break; + } + } + while (std::abs(h - oldh) > 1.0e-12); + d_latitude_d = phi * 180.0 / GPS_PI; + d_longitude_d = lambda * 180.0 / GPS_PI; + d_height_m = h; + return 0; +} + + +int Pvt_Solution::togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z) +{ + /* Subroutine to calculate geodetic coordinates latitude, longitude, + height given Cartesian coordinates X,Y,Z, and reference ellipsoid + values semi-major axis (a) and the inverse of flattening (finv). + + The output units of angular quantities will be in decimal degrees + (15.5 degrees not 15 deg 30 min). The output units of h will be the + same as the units of X,Y,Z,a. + + Inputs: + a - semi-major axis of the reference ellipsoid + finv - inverse of flattening of the reference ellipsoid + X,Y,Z - Cartesian coordinates + + Outputs: + dphi - latitude + dlambda - longitude + h - height above reference ellipsoid + + Based in a Matlab function by Kai Borre + */ + + *h = 0; + double tolsq = 1.e-10; // tolerance to accept convergence + int maxit = 10; // max number of iterations + double rtd = 180.0 / GPS_PI; + + // compute square of eccentricity + double esq; + if (finv < 1.0E-20) + { + esq = 0.0; + } + else + { + esq = (2.0 - 1.0 / finv) / finv; + } + + // 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; + } + else + { + *dlambda = 0.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; + if (r > 1.0E-20) + { + sinphi = Z/r; + } + else + { + sinphi = 0.0; + } + *dphi = asin(sinphi); + + // initial value of height = distance from origin minus + // approximate distance from origin to surface of ellipsoid + if (r < 1.0E-20) + { + *h = 0; + return 1; + } + + *h = r - a * (1 - sinphi * sinphi/finv); + + // iterate + double cosphi; + double N_phi; + double dP; + double dZ; + double oneesq = 1.0 - esq; + + for (int i = 0; i < maxit; i++) + { + sinphi = sin(*dphi); + cosphi = cos(*dphi); + + // compute radius of curvature in prime vertical direction + N_phi = a / sqrt(1 - esq * sinphi * sinphi); + + // compute residuals in P and Z + dP = P - (N_phi + (*h)) * cosphi; + dZ = Z - (N_phi * oneesq + (*h)) * sinphi; + + // update height and latitude + *h = *h + (sinphi * dZ + cosphi * dP); + *dphi = *dphi + (cosphi * dZ - sinphi * dP)/(N_phi + (*h)); + + // test for convergence + if ((dP * dP + dZ * dZ) < tolsq) + { + break; + } + if (i == (maxit - 1)) + { + LOG(WARNING) << "The computation of geodetic coordinates did not converge"; + } + } + *dphi = (*dphi) * rtd; + return 0; +} + + +int Pvt_Solution::tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km) +{ + /* Inputs: + sinel - sin of elevation angle of satellite + hsta_km - height of station in km + p_mb - atmospheric pressure in mb at height hp_km + t_kel - surface temperature in degrees Kelvin at height htkel_km + hum - humidity in % at height hhum_km + hp_km - height of pressure measurement in km + htkel_km - height of temperature measurement in km + hhum_km - height of humidity measurement in km + + Outputs: + ddr_m - range correction (meters) + + Reference + Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric + Refraction Correction Model. Paper presented at the + American Geophysical Union Annual Fall Meeting, San + Francisco, December 12-17 + + Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre + */ + + const double a_e = 6378.137; // semi-major axis of earth ellipsoid + const double b0 = 7.839257e-5; + const double tlapse = -6.5; + const double em = -978.77 / (2.8704e6 * tlapse * 1.0e-5); + + double tkhum = t_kel + tlapse * (hhum_km - htkel_km); + double atkel = 7.5 * (tkhum - 273.15) / (237.3 + tkhum - 273.15); + double e0 = 0.0611 * hum * pow(10, atkel); + double tksea = t_kel - tlapse * htkel_km; + double tkelh = tksea + tlapse * hhum_km; + double e0sea = e0 * pow((tksea / tkelh), (4 * em)); + double tkelp = tksea + tlapse * hp_km; + double psea = p_mb * pow((tksea / tkelp), em); + + if(sinel < 0) { sinel = 0.0; } + + double tropo_delay = 0.0; + bool done = false; + double refsea = 77.624e-6 / tksea; + double htop = 1.1385e-5 / refsea; + refsea = refsea * psea; + double ref = refsea * pow(((htop - hsta_km) / htop), 4); + + double a; + double b; + double rtop; + + while(1) + { + rtop = pow((a_e + htop), 2) - pow((a_e + hsta_km), 2) * (1 - pow(sinel, 2)); + + // check to see if geometry is crazy + if(rtop < 0) { rtop = 0; } + + rtop = sqrt(rtop) - (a_e + hsta_km) * sinel; + + a = -sinel / (htop - hsta_km); + b = -b0 * (1 - pow(sinel,2)) / (htop - hsta_km); + + arma::vec rn = arma::vec(8); + rn.zeros(); + + for(int i = 0; i<8; i++) + { + rn(i) = pow(rtop, (i+1+1)); + + } + + arma::rowvec alpha = {2 * a, 2 * pow(a, 2) + 4 * b /3, a * (pow(a, 2) + 3 * b), + pow(a, 4)/5 + 2.4 * pow(a, 2) * b + 1.2 * pow(b, 2), 2 * a * b * (pow(a, 2) + 3 * b)/3, + pow(b, 2) * (6 * pow(a, 2) + 4 * b) * 1.428571e-1, 0, 0}; + + if(pow(b, 2) > 1.0e-35) + { + alpha(6) = a * pow(b, 3) /2; + alpha(7) = pow(b, 4) / 9; + } + + double dr = rtop; + arma::mat aux_ = alpha * rn; + dr = dr + aux_(0, 0); + tropo_delay = tropo_delay + dr * ref * 1000; + + if(done == true) + { + *ddr_m = tropo_delay; + break; + } + + done = true; + refsea = (371900.0e-6 / tksea - 12.92e-6) / tksea; + htop = 1.1385e-5 * (1255 / tksea + 0.05) / refsea; + ref = refsea * e0sea * pow(((htop - hsta_km) / htop), 4); + } + return 0; +} + +int Pvt_Solution::topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx) +{ + /* Transformation of vector dx into topocentric coordinate + system with origin at x + Inputs: + x - vector origin coordinates (in ECEF system [X; Y; Z;]) + dx - vector ([dX; dY; dZ;]). + + Outputs: + D - vector length. Units like the input + Az - azimuth from north positive clockwise, degrees + El - elevation angle, degrees + + Based on a Matlab function by Kai Borre + */ + + double lambda; + double phi; + double h; + double dtr = GPS_PI / 180.0; + double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84 + double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84 + + // Transform x into geodetic coordinates + Pvt_Solution::togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2)); + + double cl = cos(lambda * dtr); + double sl = sin(lambda * dtr); + double cb = cos(phi * dtr); + double sb = sin(phi * dtr); + + arma::mat F = arma::zeros(3,3); + + F(0,0) = -sl; + F(0,1) = -sb * cl; + F(0,2) = cb * cl; + + F(1,0) = cl; + F(1,1) = -sb * sl; + F(1,2) = cb * sl; + + F(2,0) = 0; + F(2,1) = cb; + F(2,2) = sb; + + arma::vec local_vector; + + local_vector = arma::htrans(F) * dx; + + double E = local_vector(0); + double N = local_vector(1); + double U = local_vector(2); + + double hor_dis; + hor_dis = sqrt(E * E + N * N); + + if (hor_dis < 1.0E-20) + { + *Az = 0; + *El = 90; + } + else + { + *Az = atan2(E, N) / dtr; + *El = atan2(U, hor_dis) / dtr; + } + + if (*Az < 0) + { + *Az = *Az + 360.0; + } + + *D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2)); + return 0; +} + + + +int Pvt_Solution::compute_DOP() +{ + // ###### Compute DOPs ######## + + // 1- Rotation matrix from ECEF coordinates to ENU coordinates + // ref: http://www.navipedia.net/index.php/Transformations_between_ECEF_and_ENU_coordinates + arma::mat F = arma::zeros(3,3); + F(0,0) = -sin(GPS_TWO_PI * (d_longitude_d/360.0)); + F(0,1) = -sin(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0)); + F(0,2) = cos(GPS_TWO_PI * (d_latitude_d/360.0)) * cos(GPS_TWO_PI * (d_longitude_d/360.0)); + + F(1,0) = cos((GPS_TWO_PI * d_longitude_d)/360.0); + F(1,1) = -sin((GPS_TWO_PI * d_latitude_d)/360.0) * sin((GPS_TWO_PI * d_longitude_d)/360.0); + F(1,2) = cos((GPS_TWO_PI * d_latitude_d/360.0)) * sin((GPS_TWO_PI * d_longitude_d)/360.0); + + F(2,0) = 0; + F(2,1) = cos((GPS_TWO_PI * d_latitude_d)/360.0); + F(2,2) = sin((GPS_TWO_PI * d_latitude_d/360.0)); + + // 2- Apply the rotation to the latest covariance matrix (available in ECEF from LS) + arma::mat Q_ECEF = d_Q.submat(0, 0, 2, 2); + arma::mat DOP_ENU = arma::zeros(3, 3); + + try + { + DOP_ENU = arma::htrans(F) * Q_ECEF * F; + d_GDOP = sqrt(arma::trace(DOP_ENU)); // Geometric DOP + d_PDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1) + DOP_ENU(2, 2));// PDOP + d_HDOP = sqrt(DOP_ENU(0, 0) + DOP_ENU(1, 1)); // HDOP + d_VDOP = sqrt(DOP_ENU(2, 2)); // VDOP + d_TDOP = sqrt(d_Q(3, 3)); // TDOP + } + catch(std::exception& ex) + { + d_GDOP = -1; // Geometric DOP + d_PDOP = -1; // PDOP + d_HDOP = -1; // HDOP + d_VDOP = -1; // VDOP + d_TDOP = -1; // TDOP + } + return 0; + +} + + + + +int Pvt_Solution::set_averaging_depth(int depth) +{ + d_averaging_depth = depth; + return 0; +} + + +int Pvt_Solution::pos_averaging(bool flag_averaring) +{ + // MOVING AVERAGE PVT + bool avg = flag_averaring; + if (avg == true) + { + if (d_hist_longitude_d.size() == (unsigned int)d_averaging_depth) + { + // Pop oldest value + d_hist_longitude_d.pop_back(); + d_hist_latitude_d.pop_back(); + d_hist_height_m.pop_back(); + // Push new values + d_hist_longitude_d.push_front(d_longitude_d); + d_hist_latitude_d.push_front(d_latitude_d); + d_hist_height_m.push_front(d_height_m); + + d_avg_latitude_d = 0; + d_avg_longitude_d = 0; + d_avg_height_m = 0; + for (unsigned int i = 0; i < d_hist_longitude_d.size(); i++) + { + d_avg_latitude_d = d_avg_latitude_d + d_hist_latitude_d.at(i); + d_avg_longitude_d = d_avg_longitude_d + d_hist_longitude_d.at(i); + d_avg_height_m = d_avg_height_m + d_hist_height_m.at(i); + } + d_avg_latitude_d = d_avg_latitude_d / static_cast(d_averaging_depth); + d_avg_longitude_d = d_avg_longitude_d / static_cast(d_averaging_depth); + d_avg_height_m = d_avg_height_m / static_cast(d_averaging_depth); + b_valid_position = true; + } + else + { + //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); + d_hist_height_m.push_front(d_height_m); + + d_avg_latitude_d = d_latitude_d; + d_avg_longitude_d = d_longitude_d; + d_avg_height_m = d_height_m; + b_valid_position = false; + } + } + else + { + b_valid_position = true; + } + return 0; +} + diff --git a/src/algorithms/PVT/libs/pvt_solution.h b/src/algorithms/PVT/libs/pvt_solution.h new file mode 100644 index 000000000..7adec71f1 --- /dev/null +++ b/src/algorithms/PVT/libs/pvt_solution.h @@ -0,0 +1,170 @@ +/*! + * \file pvt_solution.h + * \brief Interface of a base class for a PVT solution + * \author Carles Fernandez-Prades, 2015. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_PVT_SOLUTION_H_ +#define GNSS_SDR_PVT_SOLUTION_H_ + + +#include +#include +#include + +#define PVT_MAX_CHANNELS 24 + +/*! + * \brief Base class for a PVT solution + * + */ +class Pvt_Solution +{ +public: + Pvt_Solution(); + + double d_latitude_d; + double d_longitude_d; + double d_height_m; + + boost::posix_time::ptime d_position_UTC_time; + + bool b_valid_position; + + 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 + + //averaging + int d_averaging_depth; //!< Length of averaging window + std::deque d_hist_latitude_d; + std::deque d_hist_longitude_d; + std::deque d_hist_height_m; + + 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 pos_averaging(bool flag_averaging); + + // DOP estimations + arma::mat d_Q; + double d_GDOP; + double d_PDOP; + double d_HDOP; + double d_VDOP; + double d_TDOP; + int compute_DOP(); //!< Compute Dilution Of Precision + + bool d_flag_averaging; + + int set_averaging_depth(int depth); + + arma::vec rotateSatellite(double traveltime, const arma::vec & X_sat); + + /*! + * \brief Conversion of Cartesian coordinates (X,Y,Z) to geographical + * coordinates (d_latitude_d, d_longitude_d, d_height_m) on a selected reference ellipsoid. + * + * \param[in] X [m] Cartesian coordinate + * \param[in] Y [m] Cartesian coordinate + * \param[in] Z [m] Cartesian coordinate + * \param[in] elipsoid_selection. Choices of Reference Ellipsoid for Geographical Coordinates: + * 0 - International Ellipsoid 1924. + * 1 - International Ellipsoid 1967. + * 2 - World Geodetic System 1972. + * 3 - Geodetic Reference System 1980. + * 4 - World Geodetic System 1984. + * + */ + int cart2geo(double X, double Y, double Z, int elipsoid_selection); + + /*! + * \brief Transformation of vector dx into topocentric coordinate system with origin at x + * + * \param[in] x Vector origin coordinates (in ECEF system [X; Y; Z;]) + * \param[in] dx Vector ([dX; dY; dZ;]). + * + * \param[out] D Vector length. Units like the input + * \param[out] Az Azimuth from north positive clockwise, degrees + * \param[out] El Elevation angle, degrees + * + * Based on a Matlab function by Kai Borre + */ + int topocent(double *Az, double *El, double *D, const arma::vec & x, const arma::vec & dx); + + /*! + * \brief Subroutine to calculate geodetic coordinates latitude, longitude, + * height given Cartesian coordinates X,Y,Z, and reference ellipsoid + * values semi-major axis (a) and the inverse of flattening (finv). + * + * The output units of angular quantities will be in decimal degrees + * (15.5 degrees not 15 deg 30 min). The output units of h will be the + * same as the units of X,Y,Z,a. + * + * \param[in] a - semi-major axis of the reference ellipsoid + * \param[in] finv - inverse of flattening of the reference ellipsoid + * \param[in] X,Y,Z - Cartesian coordinates + * + * \param[out] dphi - latitude + * \param[out] dlambda - longitude + * \param[out] h - height above reference ellipsoid + * + * Based in a Matlab function by Kai Borre + */ + int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); + + /*! + * \brief Tropospheric correction + * + * \param[in] sinel - sin of elevation angle of satellite + * \param[in] hsta_km - height of station in km + * \param[in] p_mb - atmospheric pressure in mb at height hp_km + * \param[in] t_kel - surface temperature in degrees Kelvin at height htkel_km + * \param[in] hum - humidity in % at height hhum_km + * \param[in] hp_km - height of pressure measurement in km + * \param[in] htkel_km - height of temperature measurement in km + * \param[in] hhum_km - height of humidity measurement in km + * + * \param[out] ddr_m - range correction (meters) + * + * + * Reference: + * Goad, C.C. & Goodman, L. (1974) A Modified Hopfield Tropospheric + * Refraction Correction Model. Paper presented at the + * American Geophysical Union Annual Fall Meeting, San + * Francisco, December 12-17 + * + * Translated to C++ by Carles Fernandez from a Matlab implementation by Kai Borre + */ + int tropo(double *ddr_m, double sinel, double hsta_km, double p_mb, double t_kel, double hum, double hp_km, double htkel_km, double hhum_km); +}; + +#endif