mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-15 03:35:46 +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
Show Stats 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(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
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& 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
@ -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>
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();
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)
{
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 ((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;
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;
}
@ -386,63 +386,63 @@ bool hybrid_observables_gs::interp_trk_obs(Gnss_Synchro &interpolated_obs, const
{
int32_t t1_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
// << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter << std::endl;
// std::cout << "S1= " << d_gnss_synchro_history->get(ch, nearest_element).Tracking_sample_counter
// << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->get(ch, neighbor_element).Tracking_sample_counter << std::endl;
t1_idx = nearest_element;
t2_idx = neighbor_element;
}
else
{
// std::cout << "inv S1= " << d_gnss_synchro_history->at(ch, neighbor_element).Tracking_sample_counter
// << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter << std::endl;
// std::cout << "inv S1= " << d_gnss_synchro_history->get(ch, neighbor_element).Tracking_sample_counter
// << " Si=" << rx_clock << " S2=" << d_gnss_synchro_history->get(ch, nearest_element).Tracking_sample_counter << std::endl;
t1_idx = neighbor_element;
t2_idx = nearest_element;
}
// 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)
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) /
(d_gnss_synchro_history->at(ch, t2_idx).RX_time -
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->get(ch, t2_idx).RX_time -
d_gnss_synchro_history->get(ch, t1_idx).RX_time);
// 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
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
// 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
{
// 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
// << " 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: "
// << 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: "
// << 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: "
// << 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
// << " ,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 false;
}
// 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);
}
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++)
{
out[n][0] = epoch_data.at(n);
out[n][0] = epoch_data[n];
}
// 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++)
{
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
this->message_port_pub(pmt::mp("status"), pmt::make_any(gnss_synchro_sptr));
}