diff --git a/src/algorithms/PVT/adapters/CMakeLists.txt b/src/algorithms/PVT/adapters/CMakeLists.txt index f2f8e4c0d..e9c5682cb 100644 --- a/src/algorithms/PVT/adapters/CMakeLists.txt +++ b/src/algorithms/PVT/adapters/CMakeLists.txt @@ -16,7 +16,10 @@ # along with GNSS-SDR. If not, see . # -set(PVT_ADAPTER_SOURCES gps_l1_ca_pvt.cc) +set(PVT_ADAPTER_SOURCES + gps_l1_ca_pvt.cc + galileo_e1_pvt.cc +) include_directories( $(CMAKE_CURRENT_SOURCE_DIR) diff --git a/src/algorithms/PVT/adapters/galileo_e1_pvt.cc b/src/algorithms/PVT/adapters/galileo_e1_pvt.cc new file mode 100644 index 000000000..39e449d58 --- /dev/null +++ b/src/algorithms/PVT/adapters/galileo_e1_pvt.cc @@ -0,0 +1,109 @@ +/*! + * \file galileo_e1_pvt.cc + * \brief Implementation of an adapter of a GALILEO E1 PVT solver block to a + * PvtInterface + * \author Javier Arribas, 2011. jarribas(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2012 (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 "galileo_e1_pvt.h" +#include "configuration_interface.h" +#include "galileo_e1_pvt_cc.h" +#include +#include + +using google::LogMessage; + +GalileoE1Pvt::GalileoE1Pvt(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams, + boost::shared_ptr queue) : + role_(role), + in_streams_(in_streams), + out_streams_(out_streams), + queue_(queue) +{ + // dump parameters + std::string default_dump_filename = "./pvt.dat"; + std::string default_nmea_dump_filename = "./nmea_pvt.nmea"; + std::string default_nmea_dump_devname = "/dev/tty1"; + DLOG(INFO) << "role " << role; + dump_ = configuration->property(role + ".dump", false); + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); + // moving average depth parameters + int averaging_depth; + averaging_depth = configuration->property(role + ".averaging_depth", 10); + bool flag_averaging; + flag_averaging = configuration->property(role + ".flag_averaging", false); + // output rate + int output_rate_ms; + output_rate_ms = configuration->property(role + ".output_rate_ms", 500); + // display rate + int display_rate_ms; + display_rate_ms = configuration->property(role + ".display_rate_ms", 500); + // NMEA Printer settings + bool flag_nmea_tty_port; + flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); + std::string nmea_dump_filename; + nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); + std::string nmea_dump_devname; + nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname); + // make PVT object + pvt_ = galileo_e1_make_pvt_cc(in_streams_, queue_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname); + DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; +} + + +GalileoE1Pvt::~GalileoE1Pvt() +{} + + +void GalileoE1Pvt::connect(gr::top_block_sptr top_block) +{ + // Nothing to connect internally + DLOG(INFO) << "nothing to connect internally"; +} + + +void GalileoE1Pvt::disconnect(gr::top_block_sptr top_block) +{ + // Nothing to disconnect +} + +gr::basic_block_sptr GalileoE1Pvt::get_left_block() +{ + return pvt_; +} + + +gr::basic_block_sptr GalileoE1Pvt::get_right_block() +{ + return pvt_; +} + diff --git a/src/algorithms/PVT/adapters/galileo_e1_pvt.h b/src/algorithms/PVT/adapters/galileo_e1_pvt.h new file mode 100644 index 000000000..db6296a6c --- /dev/null +++ b/src/algorithms/PVT/adapters/galileo_e1_pvt.h @@ -0,0 +1,97 @@ +/*! + * \file galileo_e1_pvt.h + * \brief Interface of an adapter of a GALILEO E1 PVT solver block to a + * PvtInterface + * Position Velocity and Time + * \author Javier Arribas, 2011. jarribas(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2013 (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_GALILEO_E1_PVT_H_ +#define GNSS_SDR_GALILEO_E1_PVT_H_ + +#include "pvt_interface.h" +#include "galileo_e1_pvt_cc.h" +#include + +class ConfigurationInterface; + +/*! + * \brief This class implements a PvtInterface for GPS L1 C/A + */ +class GalileoE1Pvt : public PvtInterface +{ +public: + GalileoE1Pvt(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams, + boost::shared_ptr queue); + + virtual ~GalileoE1Pvt(); + + std::string role() + { + return role_; + } + + //! Returns "GPS_L1_CA_PVT" + std::string implementation() + { + return "GALILEO_E1_PVT"; + } + + void connect(gr::top_block_sptr top_block); + void disconnect(gr::top_block_sptr top_block); + gr::basic_block_sptr get_left_block(); + gr::basic_block_sptr get_right_block(); + + void reset() + { + return; + } + + //! All blocks must have an item_size() function implementation. Returns sizeof(gr_complex) + size_t item_size() + { + return sizeof(gr_complex); + } + +private: + galileo_e1_pvt_cc_sptr pvt_; + bool dump_; + unsigned int fs_in_; + std::string dump_filename_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; + boost::shared_ptr queue_; +}; + +#endif diff --git a/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt b/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt index fa0484b9c..131fddc55 100644 --- a/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt @@ -16,7 +16,10 @@ # along with GNSS-SDR. If not, see . # -set(PVT_GR_BLOCKS_SOURCES gps_l1_ca_pvt_cc.cc) +set(PVT_GR_BLOCKS_SOURCES + gps_l1_ca_pvt_cc.cc + galileo_e1_pvt_cc.cc +) include_directories( $(CMAKE_CURRENT_SOURCE_DIR) diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc new file mode 100644 index 000000000..9047582fa --- /dev/null +++ b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc @@ -0,0 +1,250 @@ +/*! + * \file galileo_e1_pvt_cc.cc + * \brief Implementation of a Position Velocity and Time computation block for GPS L1 C/A + * \author Javier Arribas, 2011. jarribas(at)cttc.es + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2012 (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 "galileo_e1_pvt_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "control_message_factory.h" +#include "boost/date_time/posix_time/posix_time.hpp" +#include "gnss_synchro.h" +#include "concurrent_map.h" + +using google::LogMessage; + +extern concurrent_map global_galileo_ephemeris_map; +extern concurrent_map global_galileo_iono_map; +extern concurrent_map global_galileo_utc_model_map; + +galileo_e1_pvt_cc_sptr +galileo_e1_make_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname) +{ + return galileo_e1_pvt_cc_sptr(new galileo_e1_pvt_cc(nchannels, queue, dump, dump_filename, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname)); +} + + +galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname) : + gr::block("galileo_e1_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), + gr::io_signature::make(1, 1, sizeof(gr_complex))) +{ + + d_output_rate_ms = output_rate_ms; + d_display_rate_ms = display_rate_ms; + d_queue = queue; + d_dump = dump; + d_nchannels = nchannels; + d_dump_filename = dump_filename; + std::string dump_ls_pvt_filename = dump_filename; + + //initialize kml_printer + std::string kml_dump_filename; + kml_dump_filename = d_dump_filename; + kml_dump_filename.append(".kml"); + 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_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 galileo_e1_ls_pvt(nchannels,dump_ls_pvt_filename,d_dump); + d_ls_pvt->set_averaging_depth(d_averaging_depth); + + d_sample_counter = 0; + d_last_sample_nav_output = 0; + d_rx_time = 0.0; + + b_rinex_header_writen = false; + rp = new Rinex_Printer(); + + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + std::cout << "PVT dump enabled Log file: " << d_dump_filename.c_str() << std::endl; + } + catch (std::ifstream::failure e) + { + std::cout << "Exception opening PVT dump file " << e.what() << std::endl; + } + } + } +} + + + +galileo_e1_pvt_cc::~galileo_e1_pvt_cc() +{ + d_kml_dump.close_file(); + delete d_ls_pvt; + delete rp; + delete d_nmea_printer; +} + + + +bool galileo_e1_pvt_cc::pseudoranges_pairCompare_min( std::pair a, std::pair b) +{ + return (a.second.Pseudorange_m) < (b.second.Pseudorange_m); +} + + + +int galileo_e1_pvt_cc::general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + d_sample_counter++; + + std::map gnss_pseudoranges_map; + + Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer + + for (unsigned int i=0; i(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) + } + } + + // ############ 1. READ EPHEMERIS/UTC_MODE/IONO FROM GLOBAL MAPS #### + + d_ls_pvt->galileo_ephemeris_map = global_galileo_ephemeris_map.get_map_copy(); + + if (global_galileo_utc_model_map.size()>0) + { + // UTC MODEL data is shared for all the Galileo satellites. Read always at ID=0 + global_galileo_utc_model_map.read(0,d_ls_pvt->galileo_utc_model); + } + + if (global_galileo_iono_map.size()>0) + { + // IONO data is shared for all the Galileo satellites. Read always at ID=0 + global_galileo_iono_map.read(0,d_ls_pvt->galileo_iono); + } + + // ############ 2 COMPUTE THE PVT ################################ +// if (gnss_pseudoranges_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() >0) +// { +// // compute on the fly PVT solution +// //mod 8/4/2012 Set the PVT computation rate in this block +// if ((d_sample_counter % d_output_rate_ms) == 0) +// { +// bool pvt_result; +// 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_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); +// +// if (!b_rinex_header_writen) // & we have utc data in nav message! +// { +// std::map::iterator gps_ephemeris_iter; +// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); +// if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) +// { +// rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second,d_rx_time); +// rp->rinex_nav_header(rp->navFile,d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); +// b_rinex_header_writen = true; // do not write header anymore +// } +// } +// if(b_rinex_header_writen) // Put here another condition to separate annotations (e.g 30 s) +// { +// // Limit the RINEX navigation output rate to 1/6 seg +// // Notice that d_sample_counter period is 1ms (for GPS correlators) +// if ((d_sample_counter-d_last_sample_nav_output)>=6000) +// { +// rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map); +// d_last_sample_nav_output=d_sample_counter; +// } +// std::map::iterator gps_ephemeris_iter; +// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); +// if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) +// { +// rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_pseudoranges_map); +// } +// } +// } +// } +// +// // DEBUG MESSAGE: Display position in console output +// 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) +// << " 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; +// +// std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) +// << " 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 <. + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GALILEO_E1_PVT_CC_H +#define GNSS_SDR_GALILEO_E1_PVT_CC_H + +#include +#include +#include +#include +#include +#include +#include "galileo_navigation_message.h" +#include "galileo_ephemeris.h" +#include "galileo_utc_model.h" +#include "galileo_iono.h" +#include "nmea_printer.h" +#include "kml_printer.h" +#include "rinex_printer.h" +#include "galileo_e1_ls_pvt.h" //this file is just a copy of gps +#include "GPS_L1_CA.h" +#include "Galileo_E1.h" + + +class galileo_e1_pvt_cc; + +typedef boost::shared_ptr galileo_e1_pvt_cc_sptr; + +galileo_e1_pvt_cc_sptr +galileo_e1_make_pvt_cc(unsigned int n_channels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); + +/*! + * \brief This class implements a block that computes the PVT solution + */ +class galileo_e1_pvt_cc : public gr::block +{ +private: + friend galileo_e1_pvt_cc_sptr + galileo_e1_make_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); + galileo_e1_pvt_cc(unsigned int nchannels, boost::shared_ptr queue, bool dump, std::string dump_filename, int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname); + boost::shared_ptr d_queue; + bool d_dump; + bool b_rinex_header_writen; + Rinex_Printer *rp; + unsigned int d_nchannels; + std::string d_dump_filename; + std::ofstream d_dump_file; + int d_averaging_depth; + bool d_flag_averaging; + int d_output_rate_ms; + int d_display_rate_ms; + long unsigned int d_sample_counter; + long unsigned int d_last_sample_nav_output; + Kml_Printer d_kml_dump; + Nmea_Printer *d_nmea_printer; + double d_rx_time; + galileo_e1_ls_pvt *d_ls_pvt; /*modify PVT/libs/galileo_e1_ls_pvt*/ + + bool pseudoranges_pairCompare_min( std::pair a, std::pair b); +public: + ~galileo_e1_pvt_cc (); //!< Default destructor + /*! + * \brief Set the queue for getting navigation messages from the GpsL1CaTelemetryDecoder + */ + + int general_work (int noutput_items, gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing +}; + +#endif diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 6ca2b89df..cf6ea2404 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -18,6 +18,7 @@ set(PVT_LIB_SOURCES gps_l1_ca_ls_pvt.cc + galileo_e1_ls_pvt.cc kml_printer.cc rinex_printer.cc nmea_printer.cc diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc new file mode 100644 index 000000000..d5d585523 --- /dev/null +++ b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc @@ -0,0 +1,719 @@ +/*! + * \file galileo_e1_ls_pvt.cc + * \brief Implementation of a Least Squares Position, Velocity, and Time + * (PVT) solver, based on K.Borre's Matlab receiver. + * \author Javier Arribas, 2011. jarribas(at)cttc.es + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) + * + * 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 +#include "galileo_e1_ls_pvt.h" + +#include "GPS_L1_CA.h" +#include +#include +#include "boost/date_time/posix_time/posix_time.hpp" +#include "gnss_synchro.h" + +using google::LogMessage; + +galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file) +{ + // init empty ephemeris for all the available GNSS channels + d_nchannels = nchannels; + d_ephemeris = new Galileo_Navigation_Message[nchannels]; + d_dump_filename = dump_filename; + d_flag_dump_enabled = flag_dump_to_file; + d_averaging_depth = 0; + d_galileo_current_time = 0; + b_valid_position = false; + // ############# ENABLE DATA FILE LOG ################# + if (d_flag_dump_enabled == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + std::cout << "PVT lib dump enabled Log file: " << d_dump_filename.c_str() << std::endl; + } + catch (std::ifstream::failure e) + { + std::cout << "Exception opening PVT lib dump file " << e.what() << std::endl; + } + } + } +} + + +void galileo_e1_ls_pvt::set_averaging_depth(int depth) +{ + d_averaging_depth = depth; +} + + +galileo_e1_ls_pvt::~galileo_e1_ls_pvt() +{ + d_dump_file.close(); + delete[] d_ephemeris; +} + + +arma::vec galileo_e1_ls_pvt::rotateSatellite(double traveltime, 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; +} + + +arma::vec galileo_e1_ls_pvt::leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w) +{ + /* Computes the Least Squares Solution. + * Inputs: + * satpos - Satellites positions in ECEF system: [X; Y; Z;] + * obs - Observations - the pseudorange measurements to each satellite + * w - weigths vector + * + * Returns: + * pos - receiver position and receiver clock error + * (in ECEF system: [X, Y, Z, dt]) + */ + + //=== Initialization ======================================================= + 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 A; + arma::mat omc; + arma::mat az; + arma::mat el; + A = arma::zeros(nmbOfSatellites, 4); + omc = arma::zeros(nmbOfSatellites, 1); + az = arma::zeros(1, nmbOfSatellites); + el = arma::zeros(1, nmbOfSatellites); + for (int i = 0; i < nmbOfSatellites; i++) + { + for (int j = 0; j < 4; j++) + { + A(i, j) = 0.0; //Armadillo + } + omc(i, 0) = 0.0; + az(0, i) = 0.0; + } + el = az; + arma::mat X = satpos; + arma::vec Rot_X; + double rho2; + double traveltime; + double trop; + arma::mat mat_tmp; + arma::vec x; + + //=== Iteratively find receiver position =================================== + for (int iter = 0; iter < nmbOfIterations; iter++) + { + for (int i = 0; i < nmbOfSatellites; i++) + { + if (iter == 0) + { + //--- Initialize variables at the first iteration -------------- + Rot_X = X.col(i); //Armadillo + trop = 0.0; + } + else + { + //--- Update equations ----------------------------------------- + rho2 = (X(0, i) - pos(0)) * + (X(0, i) - pos(0)) + (X(1, i) - pos(1)) * + (X(1, i) - pos(1)) + (X(2,i) - pos(2)) * + (X(2,i) - pos(2)); + traveltime = sqrt(rho2) / GPS_C_m_s; + + //--- Correct satellite position (do to earth rotation) -------- + Rot_X = rotateSatellite(traveltime, X.col(i)); //armadillo + + //--- Find DOA and range of satellites + topocent(&d_visible_satellites_Az[i], &d_visible_satellites_El[i], + &d_visible_satellites_Distance[i], pos.subvec(0,2), Rot_X - pos.subvec(0,2)); + //[az(i), el(i), dist] = topocent(pos(1:3, :), Rot_X - pos(1:3, :)); + + } + //--- Apply the corrections ---------------------------------------- + omc(i) = (obs(i) - norm(Rot_X - pos.subvec(0,2),2) - pos(3) - trop); // Armadillo + + //--- Construct the A matrix --------------------------------------- + //Armadillo + A(i,0) = (-(Rot_X(0) - pos(0))) / obs(i); + A(i,1) = (-(Rot_X(1) - pos(1))) / obs(i); + A(i,2) = (-(Rot_X(2) - pos(2))) / obs(i); + A(i,3) = 1.0; + } + + //--- Find position update --------------------------------------------- + x = arma::solve(w*A, w*omc); // Armadillo + + //--- Apply position update -------------------------------------------- + pos = pos + x; + if (arma::norm(x,2)<1e-4) + { + break; // exit the loop because we assume that the LS algorithm has converged (err < 0.1 cm) + } + } + + try + { + //-- compute the Dilution Of Precision values + //arma::mat Q; + d_Q = arma::inv(arma::htrans(A)*A); + } + catch(std::exception& e) + { + d_Q=arma::zeros(4,4); + } + return pos; +} + + +bool galileo_e1_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging) +{ +// std::map::iterator gnss_pseudoranges_iter; +// std::map::iterator galileo_ephemeris_iter; +// int valid_pseudoranges = gnss_pseudoranges_map.size(); +// +// arma::mat W = arma::eye(valid_pseudoranges,valid_pseudoranges); //channels weights matrix +// arma::vec obs = arma::zeros(valid_pseudoranges); // pseudoranges observation vector +// arma::mat satpos = arma::zeros(3,valid_pseudoranges); //satellite positions matrix +// +// int GPS_week = 0; +// double utc = 0; +// double SV_clock_drift_s = 0; +// double SV_relativistic_clock_corr_s = 0; +// double TX_time_corrected_s; +// double SV_clock_bias_s = 0; +// +// d_flag_averaging = flag_averaging; +// +// // ******************************************************************************** +// // ****** 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++) +// { +// // 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key +// gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first); +// if (gps_ephemeris_iter != gps_ephemeris_map.end()) +// { +// /*! +// * \todo Place here the satellite CN0 (power level, or weight factor) +// */ +// W(obs_counter, obs_counter) = 1; +// +// // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) +// // first estimate of transmit time +// double Rx_time = GPS_current_time; +// double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m/GPS_C_m_s; +// +// // 2- compute the clock drift using the clock model (broadcast) for this SV +// SV_clock_drift_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); +// +// // 3- compute the relativistic clock drift using the clock model (broadcast) for this SV +// SV_relativistic_clock_corr_s = gps_ephemeris_iter->second.sv_clock_relativistic_term(Tx_time); +// +// // 4- compute the current ECEF position for this SV using corrected TX time +// SV_clock_bias_s = SV_clock_drift_s + SV_relativistic_clock_corr_s - gps_ephemeris_iter->second.d_TGD; +// 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; +// +// // 5- fill the observations vector with the corrected pseudorranges +// obs(obs_counter) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s*GPS_C_m_s; +// 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++; +// +// // SV ECEF DEBUG OUTPUT +// DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN +// << " X=" << gps_ephemeris_iter->second.d_satpos_X +// << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y +// << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z +// << " [m] PR_obs=" << obs(obs_counter) << " [m]" << std::endl; +// +// // compute the UTC time for this SV (just to print the asociated UTC timestamp) +// GPS_week = gps_ephemeris_iter->second.i_GPS_week; +// utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week); +// +// } +// else // the ephemeris are not available for this SV +// { +// // no valid pseudorange for the current SV +// W(obs_counter, obs_counter) = 0; // SV de-activated +// obs(obs_counter) = 1; // to avoid algorithm problems (divide by zero) +// DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first << std::endl; +// } +// obs_counter++; +// } +// +// // ******************************************************************************** +// // ****** SOLVE LEAST SQUARES****************************************************** +// // ******************************************************************************** +// d_valid_observations = valid_obs; +// DLOG(INFO) << "(new)PVT: valid observations=" << valid_obs << std::endl; +// +// if (valid_obs >= 4) +// { +// arma::vec mypos; +// DLOG(INFO) << "satpos=" << satpos << std::endl; +// DLOG(INFO) << "obs="<< obs << std::endl; +// DLOG(INFO) << "W=" << W < 100) + { + std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh << std::endl; + break; + } + } + while (abs(h - oldh) > 1.0e-12); + d_latitude_d = phi * 180.0 / GPS_PI; + d_longitude_d = lambda * 180 / GPS_PI; + d_height_m = h; +} + + +void galileo_e1_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/GPS_PI; + + // compute square of eccentricity + double esq; + if (finv < 1.0E-20) + { + esq = 0; + } + else + { + esq = (2 - 1/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; + } + // 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; + } + *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 - esq; + + for (int i=0; i. + * + * ------------------------------------------------------------------------- + */ +#ifndef GNSS_SDR_GALILEO_E1_LS_PVT_H_ +#define GNSS_SDR_GALILEO_E1_LS_PVT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +//#include +#include +#include +#include +#include "GPS_L1_CA.h" +#include "galileo_navigation_message.h" +#include "armadillo" +#include "boost/date_time/posix_time/posix_time.hpp" + +#include "gnss_synchro.h" +#include "galileo_ephemeris.h" +#include "galileo_utc_model.h" +#define PVT_MAX_CHANNELS 24 + +/*! + * \brief This class implements a simple PVT Least Squares solution + */ +class galileo_e1_ls_pvt +{ +private: + arma::vec leastSquarePos(arma::mat satpos, arma::vec obs, arma::mat w); + arma::vec rotateSatellite(double traveltime, arma::vec X_sat); + void topocent(double *Az, double *El, double *D, arma::vec x, arma::vec dx); + void togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z); +public: + int d_nchannels; //! Number of available channels for positioning + int d_valid_observations; //! Number of valid pseudorange observations (valid satellites) + int d_visible_satellites_IDs[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites + double d_visible_satellites_El[PVT_MAX_CHANNELS]; //! Array with the LOS Elevation of the valid satellites + double d_visible_satellites_Az[PVT_MAX_CHANNELS]; //! Array with the LOS Azimuth of the valid satellites + double d_visible_satellites_Distance[PVT_MAX_CHANNELS]; //! Array with the LOS Distance of the valid satellites + double d_visible_satellites_CN0_dB[PVT_MAX_CHANNELS]; //! Array with the IDs of the valid satellites + + Galileo_Navigation_Message* d_ephemeris; + + // new ephemeris storage + std::map galileo_ephemeris_map; + // new utc_model storage + Galileo_Utc_Model galileo_utc_model; + // new iono storage + Galileo_Iono galileo_iono; + + //double d_GPS_current_time; + double d_galileo_current_time; + 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 + std::deque d_hist_latitude_d; + std::deque d_hist_longitude_d; + std::deque d_hist_height_m; + int d_averaging_depth; //! Length of averaging window + + double d_avg_latitude_d; //! Averaged latitude in degrees + double d_avg_longitude_d; //! Averaged longitude in degrees + double d_avg_height_m; //! Averaged height [m] + + 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; + + bool d_flag_dump_enabled; + bool d_flag_averaging; + + std::string d_dump_filename; + std::ofstream d_dump_file; + + void set_averaging_depth(int depth); + + galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file); + ~galileo_e1_ls_pvt(); + + bool get_PVT(std::map gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging); + + /*! + * \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); +}; + +#endif diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 71b9ab4ac..6d83544d3 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -72,6 +72,7 @@ #include "gps_l1_ca_observables.h" #include "galileo_e1_observables.h" #include "gps_l1_ca_pvt.h" +#include "galileo_e1_pvt.h" #if GN3S_DRIVER #include "gn3s_signal_source.h" @@ -436,7 +437,11 @@ GNSSBlockInterface* GNSSBlockFactory::GetBlock( block = new GpsL1CaPvt(configuration, role, in_streams, out_streams, queue); } - + else if (implementation.compare("GALILEO_E1_PVT") == 0) + { + block = new GalileoE1Pvt(configuration, role, in_streams, + out_streams, queue); + } // OUTPUT FILTERS -------------------------------------------------------------- else if (implementation.compare("Null_Sink_Output_Filter") == 0) {