mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-10-31 23:26:22 +00:00
Merge branch 'acebrianjuan-monitor-pvt' into next
This commit is contained in:
commit
41b8cc7b1a
@ -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() << ")";
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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,
|
||||
|
@ -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
140
src/algorithms/PVT/libs/monitor_pvt.h
Normal 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
69
src/algorithms/PVT/libs/monitor_pvt_udp_sink.cc
Normal 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
53
src/algorithms/PVT/libs/monitor_pvt_udp_sink.h
Normal 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_ */
|
@ -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();
|
||||
};
|
||||
|
||||
|
@ -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)
|
||||
{
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user