mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-27 21:23:18 +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
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
2 changed files with 56 additions and 55 deletions
Show Stats 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 "gnss_circular_deque.h"
#include "gnss_sdr_create_directory.h"
#include "gnss_sdr_make_unique.h"
#include "gnss_synchro.h"
#include <glog/logging.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_nchannels_out = conf_.nchannels_out;
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 #################
if (d_dump)
@ -136,10 +137,10 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) : gr::block(
d_dump = false;
}
}
T_rx_TOW_ms = 0U;
T_rx_step_ms = 20; // read from config at the adapter GNSS-SDR.observable_interval_ms!!
T_rx_TOW_set = false;
T_status_report_timer_ms = 0;
d_T_rx_TOW_ms = 0U;
d_T_rx_step_ms = 20; // read from config at the adapter GNSS-SDR.observable_interval_ms!!
d_T_rx_TOW_set = false;
d_T_status_report_timer_ms = 0;
// rework
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
@ -198,11 +199,11 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg
{
double new_rx_clock_offset_s;
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
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
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
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));
}
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;
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.
// the time step must match the observables timer block (connected to the las input channel)
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();
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)
{
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
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
{
T_rx_TOW_ms += T_rx_step_ms; // the tow time step increment must match the ref time channel step
if (T_rx_TOW_ms >= 604800000)
d_T_rx_TOW_ms += d_T_rx_step_ms; // the tow time step increment must match the ref time channel step
if (d_T_rx_TOW_ms >= 604800000)
{
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 << " 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;
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;
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;
// debug code
// 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
{
@ -550,7 +551,7 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector<Gnss_Synchro> &data)
{
// 0. get wavelength for the current signal
double wavelength_m = 0;
switch (mapStringValues_[it->Signal])
switch (d_mapStringValues[it->Signal])
{
case evGPS_1C:
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
// 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
// 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);
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_carrier_phase_rads.at(it->Channel_ID) = it->Carrier_phase_rads;
d_channel_last_pll_lock.at(it->Channel_ID) = it->Flag_valid_pseudorange;
d_channel_last_pseudorange_smooth[it->Channel_ID] = it->Pseudorange_m;
d_channel_last_carrier_phase_rads[it->Channel_ID] = it->Carrier_phase_rads;
d_channel_last_pll_lock[it->Channel_ID] = it->Flag_valid_pseudorange;
}
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;
}
if (T_rx_TOW_set)
if (d_T_rx_TOW_set)
{
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];
}
// report channel status every second
T_status_report_timer_ms += T_rx_step_ms;
if (T_status_report_timer_ms >= 1000)
d_T_status_report_timer_ms += d_T_rx_step_ms;
if (d_T_status_report_timer_ms >= 1000)
{
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
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)

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

@ -30,11 +30,10 @@
#include <cstdint> // for int32_t
#include <fstream> // for std::ofstream
#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 <vector> // for std::vector
#if GNURADIO_USES_STD_POINTERS
#include <memory>
#else
#include <boost/shared_ptr.hpp>
#endif
@ -71,7 +70,7 @@ private:
Obs_Conf d_conf;
enum StringValue
enum StringValue_
{
evGPS_1C,
evGPS_2S,
@ -86,31 +85,32 @@ private:
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<double> d_channel_last_pseudorange_smooth;
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::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
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);
double compute_T_rx_s(const Gnss_Synchro& a);
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) const;
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 compute_pranges(std::vector<Gnss_Synchro>& data);
int32_t save_matfile();
void compute_pranges(std::vector<Gnss_Synchro>& data) const;
void smooth_pseudoranges(std::vector<Gnss_Synchro>& data);
int32_t save_matfile() const;
};
#endif // GNSS_SDR_HYBRID_OBSERVABLES_GS_H