From f635580e314512877131195e1971120af5f730cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C3=81lvaro=20Cebri=C3=A1n=20Juan?= Date: Mon, 21 Jan 2019 11:59:29 +0100 Subject: [PATCH] Add PVT Monitor --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 5 + .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 34 +++++ .../PVT/gnuradio_blocks/rtklib_pvt_cc.h | 4 + src/algorithms/PVT/libs/CMakeLists.txt | 3 + src/algorithms/PVT/libs/monitor_pvt.h | 140 ++++++++++++++++++ .../PVT/libs/monitor_pvt_udp_sink.cc | 69 +++++++++ .../PVT/libs/monitor_pvt_udp_sink.h | 53 +++++++ src/algorithms/PVT/libs/pvt_conf.h | 4 + src/algorithms/PVT/libs/rtklib_solver.cc | 56 +++++++ src/algorithms/PVT/libs/rtklib_solver.h | 3 + 10 files changed, 371 insertions(+) create mode 100644 src/algorithms/PVT/libs/monitor_pvt.h create mode 100644 src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc create mode 100644 src/algorithms/PVT/libs/monitor_pvt_udp_sink.h diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index bf8dfc9dc..175967413 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -520,6 +520,11 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, pvt_output_parameters.nmea_output_file_path = configuration->property(role + ".nmea_output_file_path", default_output_path); pvt_output_parameters.rtcm_output_file_path = configuration->property(role + ".rtcm_output_file_path", default_output_path); + // Read PVT MONITOR Configuration + pvt_output_parameters.monitor_enabled = configuration->property(role + ".enable_monitor", false); + pvt_output_parameters.udp_addresses = configuration->property(role + ".monitor_client_addresses", std::string("127.0.0.1")); + pvt_output_parameters.udp_port = configuration->property(role + ".monitor_udp_port", 1234); + // make PVT object pvt_ = rtklib_make_pvt_cc(in_streams_, pvt_output_parameters, rtk); DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 23f3d8c1e..8982300ab 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -559,6 +559,19 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels, d_last_status_print_seg = 0; + // PVT MONITOR + flag_monitor_pvt_enabled = conf_.monitor_enabled; + if (flag_monitor_pvt_enabled) + { + std::string address_string = conf_.udp_addresses; + std::vector udp_addr_vec = split_string(address_string, '_'); + std::sort(udp_addr_vec.begin(), udp_addr_vec.end()); + udp_addr_vec.erase(std::unique(udp_addr_vec.begin(), udp_addr_vec.end()), udp_addr_vec.end()); + + udp_sink_ptr = std::unique_ptr(new Monitor_Pvt_Udp_Sink(udp_addr_vec, conf_.udp_port)); + } + + // Create Sys V message queue first_fix = true; sysv_msg_key = 1101; @@ -3086,8 +3099,29 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item << d_pvt_solver->get_vdop() << " GDOP = " << d_pvt_solver->get_gdop() << std::endl; */ } + + // PVT MONITOR + if (d_pvt_solver->is_valid_position() and flag_monitor_pvt_enabled) + { + Monitor_Pvt monitor_pvt = d_pvt_solver->get_monitor_pvt(); + udp_sink_ptr->write_monitor_pvt(monitor_pvt); + } } } return noutput_items; } + +std::vector rtklib_pvt_cc::split_string(const std::string& s, char delim) +{ + std::vector v; + std::stringstream ss(s); + std::string item; + + while (std::getline(ss, item, delim)) + { + *(std::back_inserter(v)++) = item; + } + + return v; +} diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index df79836c7..1c4109c17 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -35,6 +35,7 @@ #include "gps_ephemeris.h" #include "gpx_printer.h" #include "kml_printer.h" +#include "monitor_pvt_udp_sink.h" #include "nmea_printer.h" #include "pvt_conf.h" #include "rinex_printer.h" @@ -143,6 +144,9 @@ private: return (pt - boost::posix_time::ptime(boost::gregorian::date(1970, 1, 1))).total_seconds(); } + bool flag_monitor_pvt_enabled; + std::unique_ptr udp_sink_ptr; + std::vector split_string(const std::string& s, char delim); public: rtklib_pvt_cc(uint32_t nchannels, diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index 25a4c592a..0220ae0a5 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -30,6 +30,7 @@ set(PVT_LIB_SOURCES geojson_printer.cc rtklib_solver.cc pvt_conf.cc + monitor_pvt_udp_sink.cc ) set(PVT_LIB_HEADERS @@ -44,6 +45,8 @@ set(PVT_LIB_HEADERS geojson_printer.h rtklib_solver.h pvt_conf.h + monitor_pvt_udp_sink.h + monitor_pvt.h ) include_directories( diff --git a/src/algorithms/PVT/libs/monitor_pvt.h b/src/algorithms/PVT/libs/monitor_pvt.h new file mode 100644 index 000000000..4cfc8f991 --- /dev/null +++ b/src/algorithms/PVT/libs/monitor_pvt.h @@ -0,0 +1,140 @@ +/*! + * \file monitor_pvt.h + * \brief Interface of the Monitor_Pvt class + * \author + * Álvaro Cebrián Juan, 2019. acebrianjuan(at)gmail.com + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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 MONITOR_PVT_H_ +#define MONITOR_PVT_H_ + +#include +#include + +/*! + * \brief This class contains parameters and outputs of the PVT block + */ +class Monitor_Pvt +{ +public: + // TOW + uint32_t TOW_at_current_symbol_ms; + // WEEK + uint32_t week; + // PVT GPS time + double RX_time; + // User clock offset [s] + double user_clk_offset; + + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) + double pos_x; + double pos_y; + double pos_z; + double vel_x; + double vel_y; + double vel_z; + + // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) + double cov_xx; + double cov_yy; + double cov_zz; + double cov_xy; + double cov_yz; + double cov_zx; + + // GEO user position Latitude [deg] + double latitude; + // GEO user position Longitude [deg] + double longitude; + // GEO user position Height [m] + double height; + + // NUMBER OF VALID SATS + uint8_t valid_sats; + // RTKLIB solution status + uint8_t solution_status; + // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) + uint8_t solution_type; + // AR ratio factor for validation + float AR_ratio_factor; + // AR ratio threshold for validation + float AR_ratio_threshold; + + // GDOP / PDOP/ HDOP/ VDOP + double gdop; + double pdop; + double hdop; + double vdop; + + /*! + * \brief This member function serializes and restores + * Monitor_Pvt objects from a byte stream. + */ + template + + void serialize(Archive& ar, const unsigned int version) + { + if (version) + { + }; + + ar& BOOST_SERIALIZATION_NVP(TOW_at_current_symbol_ms); + ar& BOOST_SERIALIZATION_NVP(week); + ar& BOOST_SERIALIZATION_NVP(RX_time); + ar& BOOST_SERIALIZATION_NVP(user_clk_offset); + + ar& BOOST_SERIALIZATION_NVP(pos_x); + ar& BOOST_SERIALIZATION_NVP(pos_y); + ar& BOOST_SERIALIZATION_NVP(pos_z); + ar& BOOST_SERIALIZATION_NVP(vel_x); + ar& BOOST_SERIALIZATION_NVP(vel_y); + ar& BOOST_SERIALIZATION_NVP(vel_z); + + ar& BOOST_SERIALIZATION_NVP(cov_xx); + ar& BOOST_SERIALIZATION_NVP(cov_yy); + ar& BOOST_SERIALIZATION_NVP(cov_zz); + ar& BOOST_SERIALIZATION_NVP(cov_xy); + ar& BOOST_SERIALIZATION_NVP(cov_yz); + ar& BOOST_SERIALIZATION_NVP(cov_zx); + + ar& BOOST_SERIALIZATION_NVP(latitude); + ar& BOOST_SERIALIZATION_NVP(longitude); + ar& BOOST_SERIALIZATION_NVP(height); + + ar& BOOST_SERIALIZATION_NVP(valid_sats); + ar& BOOST_SERIALIZATION_NVP(solution_status); + ar& BOOST_SERIALIZATION_NVP(solution_type); + ar& BOOST_SERIALIZATION_NVP(AR_ratio_factor); + ar& BOOST_SERIALIZATION_NVP(AR_ratio_threshold); + + ar& BOOST_SERIALIZATION_NVP(gdop); + ar& BOOST_SERIALIZATION_NVP(pdop); + ar& BOOST_SERIALIZATION_NVP(hdop); + ar& BOOST_SERIALIZATION_NVP(vdop); + } +}; + +#endif /* MONITOR_PVT_H_ */ diff --git a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc new file mode 100644 index 000000000..0aaf14e2c --- /dev/null +++ b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc @@ -0,0 +1,69 @@ +/*! + * \file monitor_pvt_udp_sink.cc + * \brief Implementation of a class that sends serialized Monitor_Pvt + * objects over udp to one or multiple endpoints + * \author Álvaro Cebrián Juan, 2019. acebrianjuan(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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 "monitor_pvt_udp_sink.h" +#include +//#include +#include +#include + +Monitor_Pvt_Udp_Sink::Monitor_Pvt_Udp_Sink(std::vector addresses, const uint16_t& port) : socket{io_service} +{ + for (auto address : addresses) + { + boost::asio::ip::udp::endpoint endpoint(boost::asio::ip::address::from_string(address, error), port); + endpoints.push_back(endpoint); + } +} + +bool Monitor_Pvt_Udp_Sink::write_monitor_pvt(Monitor_Pvt monitor_pvt) +{ + std::ostringstream archive_stream; + boost::archive::binary_oarchive oa{archive_stream}; + oa << monitor_pvt; + std::string outbound_data = archive_stream.str(); + + for (auto endpoint : endpoints) + { + socket.open(endpoint.protocol(), error); + socket.connect(endpoint, error); + + try + { + socket.send(boost::asio::buffer(outbound_data)); + } + catch (boost::system::system_error const& e) + { + return false; + } + } + return true; +} diff --git a/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h new file mode 100644 index 000000000..713e9eff3 --- /dev/null +++ b/src/algorithms/PVT/libs/monitor_pvt_udp_sink.h @@ -0,0 +1,53 @@ +/*! + * \file monitor_pvt_udp_sink.h + * \brief Interface of a class that sends serialized Monitor_Pvt objects + * over udp to one or multiple endpoints + * \author Álvaro Cebrián Juan, 2019. acebrianjuan(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2018 (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 MONITOR_PVT_UDP_SINK_H_ +#define MONITOR_PVT_UDP_SINK_H_ + +#include "monitor_pvt.h" +#include + +class Monitor_Pvt_Udp_Sink +{ +public: + Monitor_Pvt_Udp_Sink(std::vector addresses, const uint16_t &port); + bool write_monitor_pvt(Monitor_Pvt monitor_pvt); + +private: + boost::asio::io_service io_service; + boost::asio::ip::udp::socket socket; + boost::system::error_code error; + std::vector endpoints; + Monitor_Pvt monitor_pvt; +}; + + +#endif /* MONITOR_PVT_UDP_SINK_H_ */ diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index f5e9af10e..45c960c2f 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -80,6 +80,10 @@ public: std::string xml_output_path; std::string rtcm_output_file_path; + bool monitor_enabled; + std::string udp_addresses; + int udp_port; + Pvt_Conf(); }; diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index c1b984804..a6bb03c4a 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -440,6 +440,10 @@ double rtklib_solver::get_vdop() const return dop_[3]; } +Monitor_Pvt rtklib_solver::get_monitor_pvt() const +{ + return monitor_pvt; +} bool rtklib_solver::get_PVT(const std::map &gnss_observables_map, bool flag_averaging) { @@ -907,6 +911,58 @@ bool rtklib_solver::get_PVT(const std::map &gnss_observables_ << " [deg], Height= " << this->get_height() << " [m]" << " RX time offset= " << this->get_time_offset_s() << " [s]"; + // PVT MONITOR + + // TOW + monitor_pvt.TOW_at_current_symbol_ms = gnss_observables_map.begin()->second.TOW_at_current_symbol_ms; + // WEEK + monitor_pvt.week = adjgpsweek(nav_data.eph[0].week); + // PVT GPS time + monitor_pvt.RX_time = gnss_observables_map.begin()->second.RX_time; + // User clock offset [s] + monitor_pvt.user_clk_offset = rx_position_and_time(3); + + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double) + monitor_pvt.pos_x = pvt_sol.rr[0]; + monitor_pvt.pos_y = pvt_sol.rr[1]; + monitor_pvt.pos_z = pvt_sol.rr[2]; + monitor_pvt.vel_x = pvt_sol.rr[3]; + monitor_pvt.vel_y = pvt_sol.rr[4]; + monitor_pvt.vel_z = pvt_sol.rr[5]; + + // position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double) + monitor_pvt.cov_xx = pvt_sol.qr[0]; + monitor_pvt.cov_yy = pvt_sol.qr[1]; + monitor_pvt.cov_zz = pvt_sol.qr[2]; + monitor_pvt.cov_xy = pvt_sol.qr[3]; + monitor_pvt.cov_yz = pvt_sol.qr[4]; + monitor_pvt.cov_zx = pvt_sol.qr[5]; + + // GEO user position Latitude [deg] + monitor_pvt.latitude = get_latitude(); + // GEO user position Longitude [deg] + monitor_pvt.longitude = get_longitude(); + // GEO user position Height [m] + monitor_pvt.height = get_height(); + + // NUMBER OF VALID SATS + monitor_pvt.valid_sats = pvt_sol.ns; + // RTKLIB solution status + monitor_pvt.solution_status = pvt_sol.stat; + // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) + monitor_pvt.solution_type = pvt_sol.type; + // AR ratio factor for validation + monitor_pvt.AR_ratio_factor = pvt_sol.ratio; + // AR ratio threshold for validation + monitor_pvt.AR_ratio_threshold = pvt_sol.thres; + + // GDOP / PDOP/ HDOP/ VDOP + monitor_pvt.gdop = dop_[0]; + monitor_pvt.pdop = dop_[1]; + monitor_pvt.hdop = dop_[2]; + monitor_pvt.vdop = dop_[3]; + + // ######## LOG FILE ######### if (d_flag_dump_enabled == true) { diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index db0a47d1a..a3504c796 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -62,6 +62,7 @@ #include "gnss_synchro.h" #include "gps_cnav_navigation_message.h" #include "gps_navigation_message.h" +#include "monitor_pvt.h" #include "pvt_solution.h" #include "rtklib_rtkpos.h" #include @@ -85,6 +86,7 @@ private: bool d_flag_dump_mat_enabled; int d_nchannels; // Number of available channels for positioning std::array dop_; + Monitor_Pvt monitor_pvt; public: sol_t pvt_sol; @@ -97,6 +99,7 @@ public: double get_vdop() const; double get_pdop() const; double get_gdop() const; + Monitor_Pvt get_monitor_pvt() const; std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris