mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Merge branch 'acebrianjuan-monitor-pvt' into next

This commit is contained in:
Carles Fernandez 2019-02-05 01:02:11 +01:00
parent 471038d064 a8b1432c1b
commit 41b8cc7b1a
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
10 changed files with 371 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -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() << ")";

34
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

@ -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<std::string> 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<Monitor_Pvt_Udp_Sink>(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<std::string> rtklib_pvt_cc::split_string(const std::string& s, char delim)
{
std::vector<std::string> v;
std::stringstream ss(s);
std::string item;
while (std::getline(ss, item, delim))
{
*(std::back_inserter(v)++) = item;
}
return v;
}

4
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@ -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<Monitor_Pvt_Udp_Sink> udp_sink_ptr;
std::vector<std::string> split_string(const std::string& s, char delim);
public:
rtklib_pvt_cc(uint32_t nchannels,

3
src/algorithms/PVT/libs/CMakeLists.txt
View File

@ -29,6 +29,7 @@ set(PVT_LIB_SOURCES
geojson_printer.cc
rtklib_solver.cc
pvt_conf.cc
monitor_pvt_udp_sink.cc
)
set(PVT_LIB_HEADERS
@ -43,6 +44,8 @@ set(PVT_LIB_HEADERS
geojson_printer.h
rtklib_solver.h
pvt_conf.h
monitor_pvt_udp_sink.h
monitor_pvt.h
)
list(SORT PVT_LIB_HEADERS)

140
src/algorithms/PVT/libs/monitor_pvt.h Normal file
View File

@ -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 <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef MONITOR_PVT_H_
#define MONITOR_PVT_H_
#include <boost/serialization/nvp.hpp>
#include <cstdint>
/*!
* \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 <class Archive>
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_ */

69
src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc Normal file
View File

@ -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 <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "monitor_pvt_udp_sink.h"
#include <boost/archive/binary_oarchive.hpp>
//#include <boost/serialization/vector.hpp>
#include <iostream>
#include <sstream>
Monitor_Pvt_Udp_Sink::Monitor_Pvt_Udp_Sink(std::vector<std::string> 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;
}

53
src/algorithms/PVT/libs/monitor_pvt_udp_sink.h Normal file
View File

@ -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 <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef MONITOR_PVT_UDP_SINK_H_
#define MONITOR_PVT_UDP_SINK_H_
#include "monitor_pvt.h"
#include <boost/asio.hpp>
class Monitor_Pvt_Udp_Sink
{
public:
Monitor_Pvt_Udp_Sink(std::vector<std::string> 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<boost::asio::ip::udp::endpoint> endpoints;
Monitor_Pvt monitor_pvt;
};
#endif /* MONITOR_PVT_UDP_SINK_H_ */

4
src/algorithms/PVT/libs/pvt_conf.h
View File

@ -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();
};

56
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -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<int, Gnss_Synchro> &gnss_observables_map, bool flag_averaging)
{
@ -907,6 +911,58 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro> &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)
{

3
src/algorithms/PVT/libs/rtklib_solver.h
View File

@ -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 <array>
@ -85,6 +86,7 @@ private:
bool d_flag_dump_mat_enabled;
int d_nchannels; // Number of available channels for positioning
std::array<double, 4> 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<int, Galileo_Ephemeris> galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris
std::map<int, Gps_Ephemeris> gps_ephemeris_map; //!< Map storing new GPS_Ephemeris