mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 15:23:04 +00:00
Code Issues Releases Wiki Activity

Get elements of the circular deque without expensive bound checking

Browse Source
This commit is contained in:
Carles Fernandez
2019-09-09 12:10:40 +02:00
parent 476a2a73cf
commit 5f7f6366b6
2 changed files with 34 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/algorithms/libs/gnss_circular_deque.h
View File

@@ -44,7 +44,8 @@ public:
Gnss_circular_deque(); //!< Default constructor Gnss_circular_deque(); //!< Default constructor
Gnss_circular_deque(const unsigned int max_size, const unsigned int nchann); //!< nchann = number of channels; max_size = channel capacity Gnss_circular_deque(const unsigned int max_size, const unsigned int nchann); //!< nchann = number of channels; max_size = channel capacity
unsigned int size(const unsigned int ch); //!< Returns the number of available elements in a channel unsigned int size(const unsigned int ch); //!< Returns the number of available elements in a channel
T& at(const unsigned int ch, const unsigned int pos); //!< Returns a reference to an element T& at(const unsigned int ch, const unsigned int pos); //!< Returns a reference to an element with bount checking
T& get(const unsigned int ch, const unsigned int pos); //!< Returns a reference to an element without bound checking
T& front(const unsigned int ch); //!< Returns a reference to the first element in the deque T& front(const unsigned int ch); //!< Returns a reference to the first element in the deque
T& back(const unsigned int ch); //!< Returns a reference to the last element in the deque T& back(const unsigned int ch); //!< Returns a reference to the last element in the deque
void push_back(const unsigned int ch, const T& new_data); //!< Inserts an element at the end of the deque void push_back(const unsigned int ch, const T& new_data); //!< Inserts an element at the end of the deque
@@ -100,6 +101,13 @@ T& Gnss_circular_deque<T>::at(const unsigned int ch, const unsigned int pos)
} }
template <class T>
T& Gnss_circular_deque<T>::get(const unsigned int ch, const unsigned int pos)
{
return d_data[ch][pos];
}
template <class T> template <class T>
void Gnss_circular_deque<T>::clear(const unsigned int ch) void Gnss_circular_deque<T>::clear(const unsigned int ch)
{ {

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

@@ -361,7 +361,7 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
int64_t old_abs_diff = std::numeric_limits<int64_t>::max(); int64_t old_abs_diff = std::numeric_limits<int64_t>::max();
for (uint32_t 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 = llabs(static_cast<int64_t>(rx_clock) - static_cast<int64_t>(d_gnss_synchro_history->at(ch, i).Tracking_sample_counter)); abs_diff = llabs(static_cast<int64_t>(rx_clock) - static_cast<int64_t>(d_gnss_synchro_history->get(ch, i).Tracking_sample_counter));
if (old_abs_diff > abs_diff) if (old_abs_diff > abs_diff)
{ {
old_abs_diff = abs_diff; old_abs_diff = abs_diff;
@@ -371,10 +371,10 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
if (nearest_element != -1 and nearest_element != static_cast<int32_t>(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) if ((static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->get(ch, nearest_element).fs)) < 0.02)
{ {
int32_t neighbor_element; int32_t neighbor_element;
if (rx_clock > d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter) if (rx_clock > d_gnss_synchro_history->get(ch, nearest_element).Tracking_sample_counter)
{ {
neighbor_element = nearest_element + 1; neighbor_element = nearest_element + 1;
} }
@@ -386,63 +386,63 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
{ {
int32_t t1_idx; int32_t t1_idx;
int32_t t2_idx; int32_t t2_idx;
if (rx_clock > d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter) if (rx_clock > d_gnss_synchro_history->get(ch, nearest_element).Tracking_sample_counter)
{ {
// std::cout << "S1= " << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter // std::cout << "S1= " << d_gnss_synchro_history->get(ch, nearest_element).Tracking_sample_counter
// << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter << std::endl; // << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->get(ch, neighbor_element).Tracking_sample_counter << std::endl;
t1_idx = nearest_element; t1_idx = nearest_element;
t2_idx = neighbor_element; t2_idx = neighbor_element;
} }
else else
{ {
// std::cout << "inv S1= " << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter // std::cout << "inv S1= " << d_gnss_synchro_history->get(ch, neighbor_element).Tracking_sample_counter
// << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter << std::endl; // << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->get(ch, nearest_element).Tracking_sample_counter << std::endl;
t1_idx = neighbor_element; t1_idx = neighbor_element;
t2_idx = nearest_element; t2_idx = nearest_element;
} }
// 1st: copy the nearest gnss_synchro data for that channel // 1st: copy the nearest gnss_synchro data for that channel
interpolated_obs = d_gnss_synchro_history->at(ch, nearest_element); interpolated_obs = d_gnss_synchro_history->get(ch, nearest_element);
// 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1) // 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1)
double T_rx_s = static_cast<double>(rx_clock) / static_cast<double>(interpolated_obs.fs); double T_rx_s = static_cast<double>(rx_clock) / static_cast<double>(interpolated_obs.fs);
double time_factor = (T_rx_s - d_gnss_synchro_history->at(ch, t1_idx).RX_time) / double time_factor = (T_rx_s - d_gnss_synchro_history->get(ch, t1_idx).RX_time) /
(d_gnss_synchro_history->at(ch, t2_idx).RX_time - (d_gnss_synchro_history->get(ch, t2_idx).RX_time -
d_gnss_synchro_history->at(ch, t1_idx).RX_time); d_gnss_synchro_history->get(ch, t1_idx).RX_time);
// CARRIER PHASE INTERPOLATION // CARRIER PHASE INTERPOLATION
interpolated_obs.Carrier_phase_rads = d_gnss_synchro_history->at(ch, t1_idx).Carrier_phase_rads + (d_gnss_synchro_history->at(ch, t2_idx).Carrier_phase_rads - d_gnss_synchro_history->at(ch, t1_idx).Carrier_phase_rads) * time_factor; interpolated_obs.Carrier_phase_rads = d_gnss_synchro_history->get(ch, t1_idx).Carrier_phase_rads + (d_gnss_synchro_history->get(ch, t2_idx).Carrier_phase_rads - d_gnss_synchro_history->get(ch, t1_idx).Carrier_phase_rads) * time_factor;
// CARRIER DOPPLER INTERPOLATION // CARRIER DOPPLER INTERPOLATION
interpolated_obs.Carrier_Doppler_hz = d_gnss_synchro_history->at(ch, t1_idx).Carrier_Doppler_hz + (d_gnss_synchro_history->at(ch, t2_idx).Carrier_Doppler_hz - d_gnss_synchro_history->at(ch, t1_idx).Carrier_Doppler_hz) * time_factor; interpolated_obs.Carrier_Doppler_hz = d_gnss_synchro_history->get(ch, t1_idx).Carrier_Doppler_hz + (d_gnss_synchro_history->get(ch, t2_idx).Carrier_Doppler_hz - d_gnss_synchro_history->get(ch, t1_idx).Carrier_Doppler_hz) * time_factor;
// TOW INTERPOLATION // TOW INTERPOLATION
// check TOW rollover // check TOW rollover
if ((d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms - d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms) > 0) if ((d_gnss_synchro_history->get(ch, t2_idx).TOW_at_current_symbol_ms - d_gnss_synchro_history->get(ch, t1_idx).TOW_at_current_symbol_ms) > 0)
{ {
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; interpolated_obs.interp_TOW_ms = static_cast<double>(d_gnss_synchro_history->get(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->get(ch, t2_idx).TOW_at_current_symbol_ms) - static_cast<double>(d_gnss_synchro_history->get(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor;
} }
else else
{ {
// TOW rollover situation // TOW rollover situation
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 + 604800000) - static_cast<double>(d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor; interpolated_obs.interp_TOW_ms = static_cast<double>(d_gnss_synchro_history->get(ch, t1_idx).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->get(ch, t2_idx).TOW_at_current_symbol_ms + 604800000) - static_cast<double>(d_gnss_synchro_history->get(ch, t1_idx).TOW_at_current_symbol_ms)) * time_factor;
} }
// LOG(INFO) << "Channel " << ch << " int idx: " << t1_idx << " TOW Int: " << interpolated_obs.interp_TOW_ms // LOG(INFO) << "Channel " << ch << " int idx: " << t1_idx << " TOW Int: " << interpolated_obs.interp_TOW_ms
// << " TOW p1 : " << d_gnss_synchro_history->at(ch, t1_idx).TOW_at_current_symbol_ms // << " TOW p1 : " << d_gnss_synchro_history->get(ch, t1_idx).TOW_at_current_symbol_ms
// << " TOW p2: " // << " TOW p2: "
// << d_gnss_synchro_history->at(ch, t2_idx).TOW_at_current_symbol_ms // << d_gnss_synchro_history->get(ch, t2_idx).TOW_at_current_symbol_ms
// << " t2-t1: " // << " t2-t1: "
// << d_gnss_synchro_history->at(ch, t2_idx).RX_time - d_gnss_synchro_history->at(ch, t1_idx).RX_time // << d_gnss_synchro_history->get(ch, t2_idx).RX_time - d_gnss_synchro_history->get(ch, t1_idx).RX_time
// << " trx - t1: " // << " trx - t1: "
// << T_rx_s - d_gnss_synchro_history->at(ch, t1_idx).RX_time; // << T_rx_s - d_gnss_synchro_history->get(ch, t1_idx).RX_time;
// std::cout << "Rx samplestamp: " << T_rx_s << " Channel " << ch << " interp buff idx " << nearest_element // 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"; // << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->get(ch, nearest_element).fs) << " s)\n";
return true; return true;
} }
return false; return false;
} }
// std::cout << "ALERT: Channel " << ch << " interp buff idx " << nearest_element // 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"; // << " ,diff: " << old_abs_diff << " samples (" << static_cast<double>(old_abs_diff) / static_cast<double>(d_gnss_synchro_history->get(ch, nearest_element).fs) << " s)\n";
// usleep(1000); // usleep(1000);
} }
return false; return false;
@@ -610,7 +610,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
for (uint32_t n = 0; n < d_nchannels_out; n++) for (uint32_t n = 0; n < d_nchannels_out; n++)
{ {
out[n][0] = epoch_data.at(n); out[n][0] = epoch_data[n];
} }
// report channel status every second // report channel status every second
@@ -619,7 +619,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)
{ {
for (uint32_t n = 0; n < d_nchannels_out; n++) for (uint32_t n = 0; n < d_nchannels_out; n++)
{ {
std::shared_ptr<Gnss_Synchro> gnss_synchro_sptr = std::make_shared<Gnss_Synchro>(epoch_data.at(n)); std::shared_ptr<Gnss_Synchro> gnss_synchro_sptr = std::make_shared<Gnss_Synchro>(epoch_data[n]);
// 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));
} }