mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-10-05 18:30:51 +00:00
Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next
This commit is contained in:
Carles Fernandez
commit
9b9bededae
@ -54,24 +54,24 @@ namespace bc = boost::integer;
|
||||
using google::LogMessage;
|
||||
|
||||
|
||||
rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels,
|
||||
rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t nchannels,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
int output_rate_ms,
|
||||
int display_rate_ms,
|
||||
int32_t output_rate_ms,
|
||||
int32_t display_rate_ms,
|
||||
bool flag_nmea_tty_port,
|
||||
std::string nmea_dump_filename,
|
||||
std::string nmea_dump_devname,
|
||||
int rinex_version,
|
||||
int rinexobs_rate_ms,
|
||||
int rinexnav_rate_ms,
|
||||
int32_t rinex_version,
|
||||
int32_t rinexobs_rate_ms,
|
||||
int32_t rinexnav_rate_ms,
|
||||
bool flag_rtcm_server,
|
||||
bool flag_rtcm_tty_port,
|
||||
unsigned short rtcm_tcp_port,
|
||||
unsigned short rtcm_station_id,
|
||||
uint16_t rtcm_tcp_port,
|
||||
uint16_t rtcm_station_id,
|
||||
std::map<int, int> rtcm_msg_rate_ms,
|
||||
std::string rtcm_dump_devname,
|
||||
const unsigned int type_of_receiver,
|
||||
const uint32_t type_of_receiver,
|
||||
rtk_t& rtk)
|
||||
{
|
||||
return rtklib_pvt_cc_sptr(new rtklib_pvt_cc(nchannels,
|
||||
@ -245,24 +245,24 @@ std::map<int, Gps_Ephemeris> rtklib_pvt_cc::get_GPS_L1_ephemeris_map()
|
||||
}
|
||||
|
||||
|
||||
rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels,
|
||||
rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
int output_rate_ms,
|
||||
int display_rate_ms,
|
||||
int32_t output_rate_ms,
|
||||
int32_t display_rate_ms,
|
||||
bool flag_nmea_tty_port,
|
||||
std::string nmea_dump_filename,
|
||||
std::string nmea_dump_devname,
|
||||
int rinex_version,
|
||||
int rinexobs_rate_ms,
|
||||
int rinexnav_rate_ms,
|
||||
int32_t rinex_version,
|
||||
int32_t rinexobs_rate_ms,
|
||||
int32_t rinexnav_rate_ms,
|
||||
bool flag_rtcm_server,
|
||||
bool flag_rtcm_tty_port,
|
||||
unsigned short rtcm_tcp_port,
|
||||
unsigned short rtcm_station_id,
|
||||
uint16_t rtcm_tcp_port,
|
||||
uint16_t rtcm_station_id,
|
||||
std::map<int, int> rtcm_msg_rate_ms,
|
||||
std::string rtcm_dump_devname,
|
||||
const unsigned int type_of_receiver,
|
||||
const uint32_t type_of_receiver,
|
||||
rtk_t& rtk) : gr::sync_block("rtklib_pvt_cc",
|
||||
gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)),
|
||||
gr::io_signature::make(0, 0, 0))
|
||||
@ -367,7 +367,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels,
|
||||
d_dump_filename.append("_raw.dat");
|
||||
dump_ls_pvt_filename.append("_ls_pvt.dat");
|
||||
|
||||
d_ls_pvt = std::make_shared<rtklib_solver>(static_cast<int>(nchannels), dump_ls_pvt_filename, d_dump, rtk);
|
||||
d_ls_pvt = std::make_shared<rtklib_solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, rtk);
|
||||
d_ls_pvt->set_averaging_depth(1);
|
||||
|
||||
d_rx_time = 0.0;
|
||||
@ -540,7 +540,7 @@ bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff)
|
||||
int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_items,
|
||||
gr_vector_void_star& output_items __attribute__((unused)))
|
||||
{
|
||||
for (int epoch = 0; epoch < noutput_items; epoch++)
|
||||
for (int32_t epoch = 0; epoch < noutput_items; epoch++)
|
||||
{
|
||||
bool flag_display_pvt = false;
|
||||
bool flag_compute_pvt_output = false;
|
||||
@ -550,15 +550,15 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
bool flag_write_RTCM_MSM_output = false;
|
||||
bool flag_write_RINEX_obs_output = false;
|
||||
bool flag_write_RINEX_nav_output = false;
|
||||
unsigned int gps_channel = 0;
|
||||
unsigned int gal_channel = 0;
|
||||
unsigned int glo_channel = 0;
|
||||
uint32_t gps_channel = 0;
|
||||
uint32_t gal_channel = 0;
|
||||
uint32_t glo_channel = 0;
|
||||
|
||||
gnss_observables_map.clear();
|
||||
const Gnss_Synchro** in = reinterpret_cast<const Gnss_Synchro**>(&input_items[0]); // Get the input buffer pointer
|
||||
|
||||
// ############ 1. READ PSEUDORANGES ####
|
||||
for (unsigned int i = 0; i < d_nchannels; i++)
|
||||
for (uint32_t i = 0; i < d_nchannels; i++)
|
||||
{
|
||||
if (in[i][epoch].Flag_valid_pseudorange)
|
||||
{
|
||||
@ -619,7 +619,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
if (gnss_observables_map.size() > 0)
|
||||
{
|
||||
double current_RX_time = gnss_observables_map.begin()->second.RX_time;
|
||||
unsigned int current_RX_time_ms = static_cast<unsigned int>(current_RX_time * 1000.0);
|
||||
uint32_t current_RX_time_ms = static_cast<uint32_t>(current_RX_time * 1000.0);
|
||||
if (current_RX_time_ms % d_output_rate_ms == 0)
|
||||
{
|
||||
flag_compute_pvt_output = true;
|
||||
@ -676,12 +676,12 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
{
|
||||
flag_write_RTCM_MSM_output = true;
|
||||
}
|
||||
if (current_RX_time_ms % static_cast<unsigned int>(d_rinexobs_rate_ms) == 0)
|
||||
if (current_RX_time_ms % static_cast<uint32_t>(d_rinexobs_rate_ms) == 0)
|
||||
{
|
||||
flag_write_RINEX_obs_output = true;
|
||||
}
|
||||
|
||||
if (current_RX_time_ms % static_cast<unsigned int>(d_rinexnav_rate_ms) == 0)
|
||||
if (current_RX_time_ms % static_cast<uint32_t>(d_rinexnav_rate_ms) == 0)
|
||||
{
|
||||
flag_write_RINEX_nav_output = true;
|
||||
}
|
||||
@ -1393,7 +1393,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
{
|
||||
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
|
||||
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -1476,7 +1476,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
{
|
||||
//gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
|
||||
//galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -1541,7 +1541,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
{
|
||||
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
|
||||
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -1606,7 +1606,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
{
|
||||
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
|
||||
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -1671,7 +1671,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
{
|
||||
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
|
||||
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -1783,7 +1783,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -1858,7 +1858,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
|
||||
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
|
||||
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -1916,7 +1916,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -1973,7 +1973,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
|
||||
// gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end();
|
||||
// galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end();
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -2031,7 +2031,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int i = 0;
|
||||
uint32_t i = 0;
|
||||
for (gnss_observables_iter = gnss_observables_map.cbegin(); gnss_observables_iter != gnss_observables_map.cend(); gnss_observables_iter++)
|
||||
{
|
||||
std::string system(&gnss_observables_iter->second.System, 1);
|
||||
@ -2126,7 +2126,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
for (unsigned int i = 0; i < d_nchannels; i++)
|
||||
for (uint32_t i = 0; i < d_nchannels; i++)
|
||||
{
|
||||
tmp_double = in[i][epoch].Pseudorange_m;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
|
||||
@ -44,6 +44,7 @@
|
||||
#include <sys/ipc.h>
|
||||
#include <sys/msg.h>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <fstream>
|
||||
#include <utility>
|
||||
#include <string>
|
||||
@ -53,24 +54,24 @@ class rtklib_pvt_cc;
|
||||
|
||||
typedef boost::shared_ptr<rtklib_pvt_cc> rtklib_pvt_cc_sptr;
|
||||
|
||||
rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int n_channels,
|
||||
rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t n_channels,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
int output_rate_ms,
|
||||
int display_rate_ms,
|
||||
int32_t output_rate_ms,
|
||||
int32_t display_rate_ms,
|
||||
bool flag_nmea_tty_port,
|
||||
std::string nmea_dump_filename,
|
||||
std::string nmea_dump_devname,
|
||||
int rinex_version,
|
||||
int rinexobs_rate_ms,
|
||||
int rinexnav_rate_ms,
|
||||
int32_t rinex_version,
|
||||
int32_t rinexobs_rate_ms,
|
||||
int32_t rinexnav_rate_ms,
|
||||
bool flag_rtcm_server,
|
||||
bool flag_rtcm_tty_port,
|
||||
unsigned short rtcm_tcp_port,
|
||||
unsigned short rtcm_station_id,
|
||||
uint16_t rtcm_tcp_port,
|
||||
uint16_t rtcm_station_id,
|
||||
std::map<int, int> rtcm_msg_rate_ms,
|
||||
std::string rtcm_dump_devname,
|
||||
const unsigned int type_of_receiver,
|
||||
const uint32_t type_of_receiver,
|
||||
rtk_t& rtk);
|
||||
|
||||
/*!
|
||||
@ -79,24 +80,24 @@ rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int n_channels,
|
||||
class rtklib_pvt_cc : public gr::sync_block
|
||||
{
|
||||
private:
|
||||
friend rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels,
|
||||
friend rtklib_pvt_cc_sptr rtklib_make_pvt_cc(uint32_t nchannels,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
int output_rate_ms,
|
||||
int display_rate_ms,
|
||||
int32_t output_rate_ms,
|
||||
int32_t display_rate_ms,
|
||||
bool flag_nmea_tty_port,
|
||||
std::string nmea_dump_filename,
|
||||
std::string nmea_dump_devname,
|
||||
int rinex_version,
|
||||
int rinexobs_rate_ms,
|
||||
int rinexnav_rate_ms,
|
||||
int32_t rinex_version,
|
||||
int32_t rinexobs_rate_ms,
|
||||
int32_t rinexnav_rate_ms,
|
||||
bool flag_rtcm_server,
|
||||
bool flag_rtcm_tty_port,
|
||||
unsigned short rtcm_tcp_port,
|
||||
unsigned short rtcm_station_id,
|
||||
uint16_t rtcm_tcp_port,
|
||||
uint16_t rtcm_station_id,
|
||||
std::map<int, int> rtcm_msg_rate_ms,
|
||||
std::string rtcm_dump_devname,
|
||||
const unsigned int type_of_receiver,
|
||||
const uint32_t type_of_receiver,
|
||||
rtk_t& rtk);
|
||||
|
||||
void msg_handler_telemetry(pmt::pmt_t msg);
|
||||
@ -105,26 +106,26 @@ private:
|
||||
bool b_rinex_header_written;
|
||||
bool b_rinex_header_updated;
|
||||
double d_rinex_version;
|
||||
int d_rinexobs_rate_ms;
|
||||
int d_rinexnav_rate_ms;
|
||||
int32_t d_rinexobs_rate_ms;
|
||||
int32_t d_rinexnav_rate_ms;
|
||||
|
||||
bool b_rtcm_writing_started;
|
||||
int d_rtcm_MT1045_rate_ms; //!< Galileo Broadcast Ephemeris
|
||||
int d_rtcm_MT1019_rate_ms; //!< GPS Broadcast Ephemeris (orbits)
|
||||
int d_rtcm_MT1020_rate_ms; //!< GLONASS Broadcast Ephemeris (orbits)
|
||||
int d_rtcm_MT1077_rate_ms; //!< The type 7 Multiple Signal Message format for the USA’s GPS system, popular
|
||||
int d_rtcm_MT1087_rate_ms; //!< GLONASS MSM7. The type 7 Multiple Signal Message format for the Russian GLONASS system
|
||||
int d_rtcm_MT1097_rate_ms; //!< Galileo MSM7. The type 7 Multiple Signal Message format for Europe’s Galileo system
|
||||
int d_rtcm_MSM_rate_ms;
|
||||
int32_t d_rtcm_MT1045_rate_ms; //!< Galileo Broadcast Ephemeris
|
||||
int32_t d_rtcm_MT1019_rate_ms; //!< GPS Broadcast Ephemeris (orbits)
|
||||
int32_t d_rtcm_MT1020_rate_ms; //!< GLONASS Broadcast Ephemeris (orbits)
|
||||
int32_t d_rtcm_MT1077_rate_ms; //!< The type 7 Multiple Signal Message format for the USA’s GPS system, popular
|
||||
int32_t d_rtcm_MT1087_rate_ms; //!< GLONASS MSM7. The type 7 Multiple Signal Message format for the Russian GLONASS system
|
||||
int32_t d_rtcm_MT1097_rate_ms; //!< Galileo MSM7. The type 7 Multiple Signal Message format for Europe’s Galileo system
|
||||
int32_t d_rtcm_MSM_rate_ms;
|
||||
|
||||
int d_last_status_print_seg; //for status printer
|
||||
int32_t d_last_status_print_seg; //for status printer
|
||||
|
||||
unsigned int d_nchannels;
|
||||
uint32_t d_nchannels;
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
|
||||
int d_output_rate_ms;
|
||||
int d_display_rate_ms;
|
||||
int32_t d_output_rate_ms;
|
||||
int32_t d_display_rate_ms;
|
||||
|
||||
std::shared_ptr<Rinex_Printer> rp;
|
||||
std::shared_ptr<Kml_Printer> d_kml_dump;
|
||||
@ -139,7 +140,7 @@ private:
|
||||
std::map<int, Gnss_Synchro> gnss_observables_map;
|
||||
bool observables_pairCompare_min(const std::pair<int, Gnss_Synchro>& a, const std::pair<int, Gnss_Synchro>& b);
|
||||
|
||||
unsigned int type_of_rx;
|
||||
uint32_t type_of_rx;
|
||||
|
||||
bool first_fix;
|
||||
key_t sysv_msg_key;
|
||||
@ -153,23 +154,23 @@ private:
|
||||
std::chrono::time_point<std::chrono::system_clock> start, end;
|
||||
|
||||
public:
|
||||
rtklib_pvt_cc(unsigned int nchannels,
|
||||
rtklib_pvt_cc(uint32_t nchannels,
|
||||
bool dump, std::string dump_filename,
|
||||
int output_rate_ms,
|
||||
int display_rate_ms,
|
||||
int32_t output_rate_ms,
|
||||
int32_t display_rate_ms,
|
||||
bool flag_nmea_tty_port,
|
||||
std::string nmea_dump_filename,
|
||||
std::string nmea_dump_devname,
|
||||
int rinex_version,
|
||||
int rinexobs_rate_ms,
|
||||
int rinexnav_rate_ms,
|
||||
int32_t rinex_version,
|
||||
int32_t rinexobs_rate_ms,
|
||||
int32_t rinexnav_rate_ms,
|
||||
bool flag_rtcm_server,
|
||||
bool flag_rtcm_tty_port,
|
||||
unsigned short rtcm_tcp_port,
|
||||
unsigned short rtcm_station_id,
|
||||
uint16_t rtcm_tcp_port,
|
||||
uint16_t rtcm_station_id,
|
||||
std::map<int, int> rtcm_msg_rate_ms,
|
||||
std::string rtcm_dump_devname,
|
||||
const unsigned int type_of_receiver,
|
||||
const uint32_t type_of_receiver,
|
||||
rtk_t& rtk);
|
||||
|
||||
/*!
|
||||
|
||||
@ -51,8 +51,8 @@ hybrid_observables_cc_sptr hybrid_make_observables_cc(unsigned int nchannels_in,
|
||||
}
|
||||
|
||||
|
||||
hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
|
||||
unsigned int nchannels_out,
|
||||
hybrid_observables_cc::hybrid_observables_cc(uint32_t nchannels_in,
|
||||
uint32_t nchannels_out,
|
||||
bool dump,
|
||||
std::string dump_filename) : gr::block("hybrid_observables_cc",
|
||||
gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)),
|
||||
@ -87,8 +87,8 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
|
||||
T_rx_TOW_offset_ms = 0;
|
||||
T_rx_TOW_set = false;
|
||||
|
||||
//rework
|
||||
d_Rx_clock_buffer.resize(10); //10*20ms= 200 ms of data in buffer
|
||||
// rework
|
||||
d_Rx_clock_buffer.resize(10); // 10*20 ms = 200 ms of data in buffer
|
||||
d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
|
||||
}
|
||||
|
||||
@ -116,12 +116,12 @@ hybrid_observables_cc::~hybrid_observables_cc()
|
||||
}
|
||||
|
||||
|
||||
int hybrid_observables_cc::save_matfile()
|
||||
int32_t hybrid_observables_cc::save_matfile()
|
||||
{
|
||||
// READ DUMP FILE
|
||||
std::ifstream::pos_type size;
|
||||
int number_of_double_vars = 7;
|
||||
int epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels_out;
|
||||
int32_t number_of_double_vars = 7;
|
||||
int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels_out;
|
||||
std::ifstream dump_file;
|
||||
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
|
||||
try
|
||||
@ -153,7 +153,7 @@ int hybrid_observables_cc::save_matfile()
|
||||
double **PRN = new double *[d_nchannels_out];
|
||||
double **Flag_valid_pseudorange = new double *[d_nchannels_out];
|
||||
|
||||
for (unsigned int i = 0; i < d_nchannels_out; i++)
|
||||
for (uint32_t i = 0; i < d_nchannels_out; i++)
|
||||
{
|
||||
RX_time[i] = new double[num_epoch];
|
||||
TOW_at_current_symbol_s[i] = new double[num_epoch];
|
||||
@ -170,7 +170,7 @@ int hybrid_observables_cc::save_matfile()
|
||||
{
|
||||
for (int64_t i = 0; i < num_epoch; i++)
|
||||
{
|
||||
for (unsigned int chan = 0; chan < d_nchannels_out; chan++)
|
||||
for (uint32_t chan = 0; chan < d_nchannels_out; chan++)
|
||||
{
|
||||
dump_file.read(reinterpret_cast<char *>(&RX_time[chan][i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_s[chan][i]), sizeof(double));
|
||||
@ -187,7 +187,7 @@ int hybrid_observables_cc::save_matfile()
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
|
||||
for (unsigned int i = 0; i < d_nchannels_out; i++)
|
||||
for (uint32_t i = 0; i < d_nchannels_out; i++)
|
||||
{
|
||||
delete[] RX_time[i];
|
||||
delete[] TOW_at_current_symbol_s[i];
|
||||
@ -215,10 +215,10 @@ int hybrid_observables_cc::save_matfile()
|
||||
double *Pseudorange_m_aux = new double[d_nchannels_out * num_epoch];
|
||||
double *PRN_aux = new double[d_nchannels_out * num_epoch];
|
||||
double *Flag_valid_pseudorange_aux = new double[d_nchannels_out * num_epoch];
|
||||
unsigned int k = 0;
|
||||
uint32_t k = 0;
|
||||
for (int64_t j = 0; j < num_epoch; j++)
|
||||
{
|
||||
for (unsigned int i = 0; i < d_nchannels_out; i++)
|
||||
for (uint32_t i = 0; i < d_nchannels_out; i++)
|
||||
{
|
||||
RX_time_aux[k] = RX_time[i][j];
|
||||
TOW_at_current_symbol_s_aux[k] = TOW_at_current_symbol_s[i][j];
|
||||
@ -274,7 +274,7 @@ int hybrid_observables_cc::save_matfile()
|
||||
}
|
||||
Mat_Close(matfp);
|
||||
|
||||
for (unsigned int i = 0; i < d_nchannels_out; i++)
|
||||
for (uint32_t i = 0; i < d_nchannels_out; i++)
|
||||
{
|
||||
delete[] RX_time[i];
|
||||
delete[] TOW_at_current_symbol_s[i];
|
||||
@ -308,12 +308,13 @@ double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro &a)
|
||||
return ((static_cast<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(a.fs));
|
||||
}
|
||||
|
||||
bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const unsigned int &ch, const uint64_t &rx_clock)
|
||||
|
||||
bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const uint32_t &ch, const uint64_t &rx_clock)
|
||||
{
|
||||
int nearest_element = -1;
|
||||
int32_t nearest_element = -1;
|
||||
int64_t abs_diff;
|
||||
int64_t old_abs_diff = std::numeric_limits<int64_t>::max();
|
||||
for (unsigned int i = 0; i < d_gnss_synchro_history->size(ch); i++)
|
||||
for (uint32_t i = 0; i < d_gnss_synchro_history->size(ch); i++)
|
||||
{
|
||||
abs_diff = labs(static_cast<int64_t>(rx_clock) - static_cast<int64_t>(d_gnss_synchro_history->at(ch, i).Tracking_sample_counter));
|
||||
if (old_abs_diff > abs_diff)
|
||||
@ -323,11 +324,11 @@ bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
|
||||
}
|
||||
}
|
||||
|
||||
if (nearest_element != -1 and nearest_element != static_cast<int>(d_gnss_synchro_history->size(ch)))
|
||||
if (nearest_element != -1 and nearest_element != static_cast<int32_t>(d_gnss_synchro_history->size(ch)))
|
||||
{
|
||||
if ((static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs)) < 0.02)
|
||||
{
|
||||
int neighbor_element;
|
||||
int32_t neighbor_element;
|
||||
if (rx_clock > d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter)
|
||||
{
|
||||
neighbor_element = nearest_element + 1;
|
||||
@ -336,10 +337,10 @@ bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
|
||||
{
|
||||
neighbor_element = nearest_element - 1;
|
||||
}
|
||||
if (neighbor_element < static_cast<int>(d_gnss_synchro_history->size(ch)) and neighbor_element >= 0)
|
||||
if (neighbor_element < static_cast<int32_t>(d_gnss_synchro_history->size(ch)) and neighbor_element >= 0)
|
||||
{
|
||||
int t1_idx;
|
||||
int t2_idx;
|
||||
int32_t t1_idx;
|
||||
int32_t t2_idx;
|
||||
if (rx_clock > d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter)
|
||||
{
|
||||
//std::cout << "S1= " << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter
|
||||
@ -373,8 +374,8 @@ bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
|
||||
// TOW INTERPOLATION
|
||||
interpolated_obs.interp_TOW_ms = static_cast<double>(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms) - static_cast<double>(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor;
|
||||
//
|
||||
// std::cout << "Rx samplestamp: " << T_rx_s << " Channel " << ch << " interp buff idx " << nearest_element
|
||||
// << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs) << " s)\n";
|
||||
// std::cout << "Rx samplestamp: " << T_rx_s << " Channel " << ch << " interp buff idx " << nearest_element
|
||||
// << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs) << " s)\n";
|
||||
return true;
|
||||
}
|
||||
else
|
||||
@ -384,9 +385,9 @@ bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
|
||||
}
|
||||
else
|
||||
{
|
||||
// std::cout << "ALERT: Channel " << ch << " interp buff idx " << nearest_element
|
||||
// << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs) << " s)\n";
|
||||
// usleep(1000);
|
||||
// std::cout << "ALERT: Channel " << ch << " interp buff idx " << nearest_element
|
||||
// << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->at(ch, nearest_element).fs) << " s)\n";
|
||||
// usleep(1000);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@ -395,13 +396,15 @@ bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items_required)
|
||||
{
|
||||
for (int n = 0; n < static_cast<int>(d_nchannels_in) - 1; n++)
|
||||
for (int32_t n = 0; n < static_cast<int32_t>(d_nchannels_in) - 1; n++)
|
||||
{
|
||||
ninput_items_required[n] = 0;
|
||||
}
|
||||
//last input channel is the sample counter, triggered each ms
|
||||
// last input channel is the sample counter, triggered each ms
|
||||
ninput_items_required[d_nchannels_in - 1] = 1;
|
||||
}
|
||||
|
||||
@ -415,8 +418,8 @@ void hybrid_observables_cc::update_TOW(std::vector<Gnss_Synchro> &data)
|
||||
std::vector<Gnss_Synchro>::iterator it;
|
||||
// if (!T_rx_TOW_set)
|
||||
// {
|
||||
//unsigned int TOW_ref = std::numeric_limits<unsigned int>::max();
|
||||
unsigned int TOW_ref = 0;
|
||||
//uint32_t TOW_ref = std::numeric_limits<uint32_t>::max();
|
||||
uint32_t TOW_ref = 0;
|
||||
for (it = data.begin(); it != data.end(); it++)
|
||||
{
|
||||
if (it->Flag_valid_word)
|
||||
@ -455,14 +458,14 @@ void hybrid_observables_cc::compute_pranges(std::vector<Gnss_Synchro> &data)
|
||||
it->RX_time = (static_cast<double>(T_rx_TOW_ms) + GPS_STARTOFFSET_ms) / 1000.0;
|
||||
it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
|
||||
it->Flag_valid_pseudorange = true;
|
||||
//debug code
|
||||
// std::cout.precision(17);
|
||||
// std::cout << "[" << it->Channel_ID << "] interp_TOW_ms: " << it->interp_TOW_ms << std::endl;
|
||||
// std::cout << "[" << it->Channel_ID << "] Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
|
||||
// std::cout << "[" << it->Channel_ID << "] Pseudorange_m: " << it->Pseudorange_m << std::endl;
|
||||
// debug code
|
||||
// std::cout.precision(17);
|
||||
// std::cout << "[" << it->Channel_ID << "] interp_TOW_ms: " << it->interp_TOW_ms << std::endl;
|
||||
// std::cout << "[" << it->Channel_ID << "] Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
|
||||
// std::cout << "[" << it->Channel_ID << "] Pseudorange_m: " << it->Pseudorange_m << std::endl;
|
||||
}
|
||||
}
|
||||
// usleep(1000);
|
||||
// usleep(1000);
|
||||
}
|
||||
|
||||
|
||||
@ -473,8 +476,8 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
|
||||
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]);
|
||||
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]);
|
||||
|
||||
//push receiver clock into history buffer (connected to the last of the input channels)
|
||||
//The clock buffer gives time to the channels to compute the tracking observables
|
||||
// Push receiver clock into history buffer (connected to the last of the input channels)
|
||||
// The clock buffer gives time to the channels to compute the tracking observables
|
||||
if (ninput_items[d_nchannels_in - 1] > 0)
|
||||
{
|
||||
d_Rx_clock_buffer.push_back(in[d_nchannels_in - 1][0].Tracking_sample_counter);
|
||||
@ -485,14 +488,15 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
|
||||
usleep(1000000);
|
||||
}
|
||||
|
||||
//consume one item from the clock channel (last of the input channels)
|
||||
// Consume one item from the clock channel (last of the input channels)
|
||||
consume(d_nchannels_in - 1, 1);
|
||||
}
|
||||
//push the tracking observables into buffers to allow the observable interpolation at the desired Rx clock
|
||||
for (unsigned int n = 0; n < d_nchannels_out; n++)
|
||||
|
||||
// Push the tracking observables into buffers to allow the observable interpolation at the desired Rx clock
|
||||
for (uint32_t n = 0; n < d_nchannels_out; n++)
|
||||
{
|
||||
// push the valid tracking Gnss_Synchros to their corresponding deque
|
||||
for (int m = 0; m < ninput_items[n]; m++)
|
||||
// Push the valid tracking Gnss_Synchros to their corresponding deque
|
||||
for (int32_t m = 0; m < ninput_items[n]; m++)
|
||||
{
|
||||
if (in[n][m].Flag_valid_word)
|
||||
{
|
||||
@ -514,13 +518,13 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
|
||||
if (d_Rx_clock_buffer.size() == d_Rx_clock_buffer.capacity())
|
||||
{
|
||||
std::vector<Gnss_Synchro> epoch_data;
|
||||
int n_valid = 0;
|
||||
for (unsigned int n = 0; n < d_nchannels_out; n++)
|
||||
int32_t n_valid = 0;
|
||||
for (uint32_t n = 0; n < d_nchannels_out; n++)
|
||||
{
|
||||
Gnss_Synchro interpolated_gnss_synchro;
|
||||
if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front() + T_rx_TOW_offset_ms * T_rx_clock_step_samples))
|
||||
{
|
||||
//produce an empty observation
|
||||
// Produce an empty observation
|
||||
interpolated_gnss_synchro = Gnss_Synchro();
|
||||
interpolated_gnss_synchro.Flag_valid_pseudorange = false;
|
||||
interpolated_gnss_synchro.Flag_valid_word = false;
|
||||
@ -545,19 +549,18 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
|
||||
|
||||
if (n_valid > 0) compute_pranges(epoch_data);
|
||||
|
||||
for (unsigned int n = 0; n < d_nchannels_out; n++)
|
||||
for (uint32_t n = 0; n < d_nchannels_out; n++)
|
||||
{
|
||||
out[n][0] = epoch_data.at(n);
|
||||
}
|
||||
|
||||
|
||||
if (d_dump)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
for (unsigned int i = 0; i < d_nchannels_out; i++)
|
||||
for (uint32_t i = 0; i < d_nchannels_out; i++)
|
||||
{
|
||||
tmp_double = out[i][0].RX_time;
|
||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
|
||||
|
||||
@ -63,27 +63,27 @@ public:
|
||||
|
||||
private:
|
||||
friend hybrid_observables_cc_sptr
|
||||
hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename);
|
||||
hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename);
|
||||
bool interpolate_data(Gnss_Synchro& out, const unsigned int& ch, const double& ti);
|
||||
bool interp_trk_obs(Gnss_Synchro& interpolated_obs, const unsigned int& ch, const uint64_t& rx_clock);
|
||||
hybrid_make_observables_cc(uint32_t nchannels_in, uint32_t nchannels_out, bool dump, std::string dump_filename);
|
||||
hybrid_observables_cc(uint32_t nchannels_in, uint32_t nchannels_out, bool dump, std::string dump_filename);
|
||||
bool interpolate_data(Gnss_Synchro& out, const uint32_t& ch, const double& ti);
|
||||
bool interp_trk_obs(Gnss_Synchro& interpolated_obs, const uint32_t& ch, const uint64_t& rx_clock);
|
||||
double compute_T_rx_s(const Gnss_Synchro& a);
|
||||
void compute_pranges(std::vector<Gnss_Synchro>& data);
|
||||
void update_TOW(std::vector<Gnss_Synchro>& data);
|
||||
int save_matfile();
|
||||
int32_t save_matfile();
|
||||
|
||||
//time history
|
||||
boost::circular_buffer<uint64_t> d_Rx_clock_buffer;
|
||||
//Tracking observable history
|
||||
Gnss_circular_deque<Gnss_Synchro>* d_gnss_synchro_history;
|
||||
unsigned int T_rx_clock_step_samples;
|
||||
uint32_t T_rx_clock_step_samples;
|
||||
//rx time follow GPST
|
||||
bool T_rx_TOW_set;
|
||||
unsigned int T_rx_TOW_ms;
|
||||
unsigned int T_rx_TOW_offset_ms;
|
||||
uint32_t T_rx_TOW_ms;
|
||||
uint32_t T_rx_TOW_offset_ms;
|
||||
bool d_dump;
|
||||
unsigned int d_nchannels_in;
|
||||
unsigned int d_nchannels_out;
|
||||
uint32_t d_nchannels_in;
|
||||
uint32_t d_nchannels_out;
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
};
|
||||
|
||||
@ -1249,7 +1249,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
|
||||
//printf("333333 current_synchro_data.Acq_samplestamp_samples = %d\n", current_synchro_data.Acq_samplestamp_samples);
|
||||
//printf("333333 current_synchro_data.Acq_delay_samples = %f\n", current_synchro_data.Acq_delay_samples);
|
||||
//printf("333333 d_correlation_length_samples = %d\n", d_correlation_length_samples);
|
||||
unsigned num_frames = ceil((counter_value - current_synchro_data.Acq_samplestamp_samples - current_synchro_data.Acq_delay_samples) / d_correlation_length_samples);
|
||||
uint32_t num_frames = ceil((counter_value - current_synchro_data.Acq_samplestamp_samples - current_synchro_data.Acq_delay_samples) / d_correlation_length_samples);
|
||||
//printf("333333 num_frames = %d\n", num_frames);
|
||||
uint64_t absolute_samples_offset = current_synchro_data.Acq_delay_samples + current_synchro_data.Acq_samplestamp_samples + num_frames * d_correlation_length_samples;
|
||||
//printf("333333 absolute_samples_offset = %llu\n", absolute_samples_offset);
|
||||
@ -1480,7 +1480,7 @@ int dll_pll_veml_tracking_fpga::general_work(int noutput_items __attribute__((un
|
||||
case 3:
|
||||
{
|
||||
d_sample_counter = d_sample_counter_next;
|
||||
d_sample_counter_next = d_sample_counter + d_current_prn_length_samples;
|
||||
d_sample_counter_next = d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples);
|
||||
|
||||
// Fill the acquisition data
|
||||
current_synchro_data = *d_acquisition_gnss_synchro;
|
||||
|
||||
@ -448,7 +448,7 @@ int main(int argc, char** argv)
|
||||
}
|
||||
catch (const boost::exception& e)
|
||||
{
|
||||
LOG(INFO) << "Exception caught while pushing to he internal queue.";
|
||||
LOG(INFO) << "Exception caught while pushing to the internal queue.";
|
||||
}
|
||||
try
|
||||
{
|
||||
@ -481,12 +481,12 @@ int main(int argc, char** argv)
|
||||
Eph_map = global_gps_ephemeris_map.get_map_copy();
|
||||
current_TOW = Eph_map.begin()->second.d_TOW;
|
||||
|
||||
time_t t = utc_time(Eph_map.begin()->second.i_GPS_week, (int64_t)current_TOW);
|
||||
time_t t = utc_time(Eph_map.begin()->second.i_GPS_week, static_cast<int64_t>(current_TOW));
|
||||
|
||||
fprintf(stdout, "Reference Time:\n");
|
||||
fprintf(stdout, " GPS Week: %d\n", Eph_map.begin()->second.i_GPS_week);
|
||||
fprintf(stdout, " GPS TOW: %lld %lf\n", (int64_t)current_TOW, (int64_t)current_TOW * 0.08);
|
||||
fprintf(stdout, " ~ UTC: %s", ctime(&t));
|
||||
std::cout << "Reference Time:" << std::endl;
|
||||
std::cout << " GPS Week: " << Eph_map.begin()->second.i_GPS_week << std::endl;
|
||||
std::cout << " GPS TOW: " << static_cast<int64_t>(current_TOW) << " " << static_cast<int64_t>(current_TOW) * 0.08 << std::endl;
|
||||
std::cout << " ~ UTC: " << ctime(&t) << std::endl;
|
||||
std::cout << "Current TOW obtained from SUPL assistance = " << current_TOW << std::endl;
|
||||
}
|
||||
else
|
||||
|
||||
Loading…
Reference in New Issue
Block a user