mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-29 22:42:59 +00:00
Code Issues Releases Wiki Activity

Use unique_ptr instead of shared_ptr for d_gnss_synchro_history, and code cleaning

Browse Source
This commit is contained in:
Carles Fernandez
2020-06-19 00:32:19 +02:00
parent 633d27b5fa
commit 45c20f08ac
2 changed files with 56 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

71
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc
View File

@@ -23,6 +23,7 @@
#include "MATH_CONSTANTS.h" // for SPEED_OF_LIGHT #include "MATH_CONSTANTS.h" // for SPEED_OF_LIGHT
#include "gnss_circular_deque.h" #include "gnss_circular_deque.h"
#include "gnss_sdr_create_directory.h" #include "gnss_sdr_create_directory.h"
#include "gnss_sdr_make_unique.h"
#include "gnss_synchro.h" #include "gnss_synchro.h"
#include <glog/logging.h> #include <glog/logging.h>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
@@ -90,7 +91,7 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block(
d_dump_filename = conf_.dump_filename; d_dump_filename = conf_.dump_filename;
d_nchannels_out = conf_.nchannels_out; d_nchannels_out = conf_.nchannels_out;
d_nchannels_in = conf_.nchannels_in; d_nchannels_in = conf_.nchannels_in;
d_gnss_synchro_history = std::make_shared<Gnss_circular_deque<Gnss_Synchro>>(1000, d_nchannels_out); d_gnss_synchro_history = std::make_unique<Gnss_circular_deque<Gnss_Synchro>>(1000, d_nchannels_out);
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump) if (d_dump)
@@ -136,10 +137,10 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block(
d_dump = false; d_dump = false;
} }
} }
T_rx_TOW_ms = 0U; d_T_rx_TOW_ms = 0U;
T_rx_step_ms = 20; // read from config at the adapter GNSS-SDR.observable_interval_ms!! d_T_rx_step_ms = 20; // read from config at the adapter GNSS-SDR.observable_interval_ms!!
T_rx_TOW_set = false; d_T_rx_TOW_set = false;
T_status_report_timer_ms = 0; d_T_status_report_timer_ms = 0;
// rework // rework
d_Rx_clock_buffer.set_capacity(10); // 10*20 ms = 200 ms of data in buffer d_Rx_clock_buffer.set_capacity(10); // 10*20 ms = 200 ms of data in buffer
d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
@@ -198,11 +199,11 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
{ {
double new_rx_clock_offset_s; double new_rx_clock_offset_s;
new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg)); new_rx_clock_offset_s = boost::any_cast<double>(pmt::any_ref(msg));
T_rx_TOW_ms = T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0)); d_T_rx_TOW_ms = d_T_rx_TOW_ms - static_cast<int>(round(new_rx_clock_offset_s * 1000.0));
// align the receiver clock to integer multiple of 20 ms // align the receiver clock to integer multiple of 20 ms
if (T_rx_TOW_ms % 20) if (d_T_rx_TOW_ms % 20)
{ {
T_rx_TOW_ms += 20 - T_rx_TOW_ms % 20; d_T_rx_TOW_ms += 20 - d_T_rx_TOW_ms % 20;
} }
// d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer // d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
for (uint32_t n = 0; n < d_nchannels_out; n++) for (uint32_t n = 0; n < d_nchannels_out; n++)
@@ -220,7 +221,7 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
} }
int32_t hybrid_observables_gs::save_matfile() int32_t hybrid_observables_gs::save_matfile() const
{ {
// READ DUMP FILE // READ DUMP FILE
std::string dump_filename = d_dump_filename; std::string dump_filename = d_dump_filename;
@@ -357,13 +358,13 @@ int32_t hybrid_observables_gs::save_matfile()
} }
double hybrid_observables_gs::compute_T_rx_s(const Gnss_Synchro &a) double hybrid_observables_gs::compute_T_rx_s(const Gnss_Synchro &a) const
{ {
return ((static_cast<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(a.fs)); return ((static_cast<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(a.fs));
} }
bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const uint32_t &ch, const uint64_t &rx_clock) bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, uint32_t ch, uint64_t rx_clock) const
{ {
int32_t nearest_element = -1; int32_t nearest_element = -1;
int64_t abs_diff; int64_t abs_diff;
@@ -476,7 +477,7 @@ void hybrid_observables_gs::update_TOW(const std::vector<Gnss_Synchro> &data)
// 2. If the TOW is set, it must be incremented by the desired receiver time step. // 2. If the TOW is set, it must be incremented by the desired receiver time step.
// the time step must match the observables timer block (connected to the las input channel) // the time step must match the observables timer block (connected to the las input channel)
std::vector<Gnss_Synchro>::const_iterator it; std::vector<Gnss_Synchro>::const_iterator it;
if (!T_rx_TOW_set) if (!d_T_rx_TOW_set)
{ {
// int32_t TOW_ref = std::numeric_limits<uint32_t>::max(); // int32_t TOW_ref = std::numeric_limits<uint32_t>::max();
uint32_t TOW_ref = 0U; uint32_t TOW_ref = 0U;
@@ -487,35 +488,35 @@ void hybrid_observables_gs::update_TOW(const std::vector<Gnss_Synchro> &data)
if (it->TOW_at_current_symbol_ms > TOW_ref) if (it->TOW_at_current_symbol_ms > TOW_ref)
{ {
TOW_ref = it->TOW_at_current_symbol_ms; TOW_ref = it->TOW_at_current_symbol_ms;
T_rx_TOW_set = true; d_T_rx_TOW_set = true;
} }
} }
} }
T_rx_TOW_ms = TOW_ref; d_T_rx_TOW_ms = TOW_ref;
// align the receiver clock to integer multiple of 20 ms // align the receiver clock to integer multiple of 20 ms
if (T_rx_TOW_ms % 20) if (d_T_rx_TOW_ms % 20)
{ {
T_rx_TOW_ms += 20 - T_rx_TOW_ms % 20; d_T_rx_TOW_ms += 20 - d_T_rx_TOW_ms % 20;
} }
} }
else else
{ {
T_rx_TOW_ms += T_rx_step_ms; // the tow time step increment must match the ref time channel step d_T_rx_TOW_ms += d_T_rx_step_ms; // the tow time step increment must match the ref time channel step
if (T_rx_TOW_ms >= 604800000) if (d_T_rx_TOW_ms >= 604800000)
{ {
DLOG(INFO) << "TOW RX TIME rollover!"; DLOG(INFO) << "TOW RX TIME rollover!";
T_rx_TOW_ms = T_rx_TOW_ms % 604800000; d_T_rx_TOW_ms = d_T_rx_TOW_ms % 604800000;
} }
} }
} }
void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data) void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data) const
{ {
// std::cout.precision(17); // std::cout.precision(17);
// std::cout << " T_rx_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) << std::endl; // std::cout << " d_T_rx_TOW_ms: " << static_cast<double>(d_T_rx_TOW_ms) << std::endl;
std::vector<Gnss_Synchro>::iterator it; std::vector<Gnss_Synchro>::iterator it;
auto current_T_rx_TOW_ms = static_cast<double>(T_rx_TOW_ms); auto current_T_rx_TOW_ms = static_cast<double>(d_T_rx_TOW_ms);
double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0; double current_T_rx_TOW_s = current_T_rx_TOW_ms / 1000.0;
for (it = data.begin(); it != data.end(); it++) for (it = data.begin(); it != data.end(); it++)
{ {
@@ -531,7 +532,7 @@ void hybrid_observables_gs::compute_pranges(std::vector<Gnss_Synchro> &data)
it->Flag_valid_pseudorange = true; it->Flag_valid_pseudorange = true;
// debug code // debug code
// std::cout << "[" << it->Channel_ID << "] interp_TOW_ms: " << it->interp_TOW_ms << std::endl; // 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 << "] Diff d_T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(d_T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
} }
else else
{ {
@@ -550,7 +551,7 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
{ {
// 0. get wavelength for the current signal // 0. get wavelength for the current signal
double wavelength_m = 0; double wavelength_m = 0;
switch (mapStringValues_[it->Signal]) switch (d_mapStringValues[it->Signal])
{ {
case evGPS_1C: case evGPS_1C:
wavelength_m = SPEED_OF_LIGHT / FREQ1; wavelength_m = SPEED_OF_LIGHT / FREQ1;
@@ -591,21 +592,21 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
// todo: propagate the PLL lock status in Gnss_Synchro // todo: propagate the PLL lock status in Gnss_Synchro
// 1. check if last PLL lock status was false and initialize last d_channel_last_pseudorange_smooth // 1. check if last PLL lock status was false and initialize last d_channel_last_pseudorange_smooth
if (d_channel_last_pll_lock.at(it->Channel_ID) == true) if (d_channel_last_pll_lock[it->Channel_ID] == true)
{ {
// 2. Compute the smoothed pseudorange for this channel // 2. Compute the smoothed pseudorange for this channel
// Hatch filter algorithm (https://insidegnss.com/can-you-list-all-the-properties-of-the-carrier-smoothing-filter/) // Hatch filter algorithm (https://insidegnss.com/can-you-list-all-the-properties-of-the-carrier-smoothing-filter/)
double r_sm = d_channel_last_pseudorange_smooth.at(it->Channel_ID); double r_sm = d_channel_last_pseudorange_smooth[it->Channel_ID];
double factor = ((d_smooth_filter_M - 1.0) / d_smooth_filter_M); double factor = ((d_smooth_filter_M - 1.0) / d_smooth_filter_M);
it->Pseudorange_m = factor * r_sm + (1.0 / d_smooth_filter_M) * it->Pseudorange_m + wavelength_m * (factor / PI_2) * (it->Carrier_phase_rads - d_channel_last_carrier_phase_rads.at(it->Channel_ID)); it->Pseudorange_m = factor * r_sm + (1.0 / d_smooth_filter_M) * it->Pseudorange_m + wavelength_m * (factor / PI_2) * (it->Carrier_phase_rads - d_channel_last_carrier_phase_rads[it->Channel_ID]);
} }
d_channel_last_pseudorange_smooth.at(it->Channel_ID) = it->Pseudorange_m; d_channel_last_pseudorange_smooth[it->Channel_ID] = it->Pseudorange_m;
d_channel_last_carrier_phase_rads.at(it->Channel_ID) = it->Carrier_phase_rads; d_channel_last_carrier_phase_rads[it->Channel_ID] = it->Carrier_phase_rads;
d_channel_last_pll_lock.at(it->Channel_ID) = it->Flag_valid_pseudorange; d_channel_last_pll_lock[it->Channel_ID] = it->Flag_valid_pseudorange;
} }
else else
{ {
d_channel_last_pll_lock.at(it->Channel_ID) = false; d_channel_last_pll_lock[it->Channel_ID] = false;
} }
} }
} }
@@ -675,7 +676,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
epoch_data[n] = interpolated_gnss_synchro; epoch_data[n] = interpolated_gnss_synchro;
} }
if (T_rx_TOW_set) if (d_T_rx_TOW_set)
{ {
update_TOW(epoch_data); update_TOW(epoch_data);
} }
@@ -703,8 +704,8 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
out[n][0] = epoch_data[n]; out[n][0] = epoch_data[n];
} }
// report channel status every second // report channel status every second
T_status_report_timer_ms += T_rx_step_ms; d_T_status_report_timer_ms += d_T_rx_step_ms;
if (T_status_report_timer_ms >= 1000) if (d_T_status_report_timer_ms >= 1000)
{ {
for (uint32_t n = 0; n < d_nchannels_out; n++) for (uint32_t n = 0; n < d_nchannels_out; n++)
{ {
@@ -712,7 +713,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
// publish valid gnss_synchro to the gnss_flowgraph channel status monitor // publish valid gnss_synchro to the gnss_flowgraph channel status monitor
this->message_port_pub(pmt::mp("status"), pmt::make_any(gnss_synchro_sptr)); this->message_port_pub(pmt::mp("status"), pmt::make_any(gnss_synchro_sptr));
} }
T_status_report_timer_ms = 0; d_T_status_report_timer_ms = 0;
} }
if (d_dump) if (d_dump)

40
src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h
View File

@@ -30,11 +30,10 @@
#include <cstdint> // for int32_t #include <cstdint> // for int32_t
#include <fstream> // for std::ofstream #include <fstream> // for std::ofstream
#include <map> // for std::map #include <map> // for std::map
#include <memory> // for std:shared_ptr #include <memory> // for std::shared, std:unique_ptr
#include <string> // for std::string #include <string> // for std::string
#include <vector> // for std::vector #include <vector> // for std::vector
#if GNURADIO_USES_STD_POINTERS #if GNURADIO_USES_STD_POINTERS
#include <memory>
#else #else
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#endif #endif
@@ -71,7 +70,7 @@ private:
Obs_Conf d_conf; Obs_Conf d_conf;
enum StringValue enum StringValue_
{ {
evGPS_1C, evGPS_1C,
evGPS_2S, evGPS_2S,
@@ -86,31 +85,32 @@ private:
evBDS_B3 evBDS_B3
}; };
std::map<std::string, StringValue> mapStringValues_; bool d_T_rx_TOW_set; // rx time follow GPST
bool d_dump;
bool d_dump_mat;
uint32_t d_T_rx_TOW_ms;
uint32_t d_T_rx_step_ms;
uint32_t d_T_status_report_timer_ms;
uint32_t d_nchannels_in;
uint32_t d_nchannels_out;
double d_smooth_filter_M;
std::map<std::string, StringValue_> d_mapStringValues;
std::vector<bool> d_channel_last_pll_lock; std::vector<bool> d_channel_last_pll_lock;
std::vector<double> d_channel_last_pseudorange_smooth; std::vector<double> d_channel_last_pseudorange_smooth;
std::vector<double> d_channel_last_carrier_phase_rads; std::vector<double> d_channel_last_carrier_phase_rads;
double d_smooth_filter_M;
void smooth_pseudoranges(std::vector<Gnss_Synchro>& data);
bool T_rx_TOW_set; // rx time follow GPST
bool d_dump;
bool d_dump_mat;
uint32_t T_rx_TOW_ms;
uint32_t T_rx_step_ms;
uint32_t T_status_report_timer_ms;
uint32_t d_nchannels_in;
uint32_t d_nchannels_out;
std::string d_dump_filename; std::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;
std::unique_ptr<Gnss_circular_deque<Gnss_Synchro>> d_gnss_synchro_history; // Tracking observable history
boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history boost::circular_buffer<uint64_t> d_Rx_clock_buffer; // time history
std::shared_ptr<Gnss_circular_deque<Gnss_Synchro>> d_gnss_synchro_history; // Tracking observable history
void msg_handler_pvt_to_observables(const pmt::pmt_t& msg); void msg_handler_pvt_to_observables(const pmt::pmt_t& msg);
double compute_T_rx_s(const Gnss_Synchro& a); double compute_T_rx_s(const Gnss_Synchro& a) const;
bool interp_trk_obs(Gnss_Synchro& interpolated_obs, const uint32_t& ch, const uint64_t& rx_clock); bool interp_trk_obs(Gnss_Synchro& interpolated_obs, uint32_t ch, uint64_t rx_clock) const;
void update_TOW(const std::vector<Gnss_Synchro>& data); void update_TOW(const std::vector<Gnss_Synchro>& data);
void compute_pranges(std::vector<Gnss_Synchro>& data); void compute_pranges(std::vector<Gnss_Synchro>& data) const;
int32_t save_matfile(); void smooth_pseudoranges(std::vector<Gnss_Synchro>& data);
int32_t save_matfile() const;
}; };
#endif // GNSS_SDR_HYBRID_OBSERVABLES_GS_H #endif // GNSS_SDR_HYBRID_OBSERVABLES_GS_H