mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-16 05:00:35 +00:00
Code Issues Releases Wiki Activity

Fix bugs

Browse Source
This commit is contained in:
Antonio Ramos 2018-04-10 15:37:07 +02:00
parent 2a62d868ab
commit f350174fd8
2 changed files with 44 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,6 +33,7 @@
#ifndef GNSS_SDR_CIRCULAR_DEQUE_H_ #ifndef GNSS_SDR_CIRCULAR_DEQUE_H_
#define GNSS_SDR_CIRCULAR_DEQUE_H_ #define GNSS_SDR_CIRCULAR_DEQUE_H_
#include <exception>
template <class T> template <class T>
class Gnss_circular_deque class Gnss_circular_deque
@ -89,6 +90,10 @@ Gnss_circular_deque<T>::Gnss_circular_deque(const unsigned int max_size, const u
d_index_pop[i] = 0; d_index_pop[i] = 0;
d_index_push[i] = 0; d_index_push[i] = 0;
d_history[i] = new T[d_max_size]; d_history[i] = new T[d_max_size];
for (unsigned int ii = 0; ii < d_max_size; ii++)
{
d_history[i][ii] = d_return_void;
}
} }
} }
} }
@ -127,12 +132,14 @@ T& Gnss_circular_deque<T>::back(const unsigned int ch)
} }
else else
{ {
index = d_max_size; index = d_max_size - 1;
} }
return d_history[ch][index]; return d_history[ch][index];
} }
else else
{ {
std::exception ex;
throw ex;
return d_return_void; return d_return_void;
} }
} }
@ -146,6 +153,8 @@ T& Gnss_circular_deque<T>::front(const unsigned int ch)
} }
else else
{ {
std::exception ex;
throw ex;
return d_return_void; return d_return_void;
} }
} }
@ -155,11 +164,13 @@ T& Gnss_circular_deque<T>::at(const unsigned int ch, const unsigned int pos)
{ {
if (d_size[ch] > 0 and pos < d_size[ch]) if (d_size[ch] > 0 and pos < d_size[ch])
{ {
unsigned int index = (d_index_pop[ch] + pos) % d_max_size; unsigned int index = ((d_index_pop[ch] + pos) % d_max_size);
return d_history[ch][index]; return d_history[ch][index];
} }
else else
{ {
std::exception ex;
throw ex;
return d_return_void; return d_return_void;
} }
} }
@ -175,31 +186,45 @@ void Gnss_circular_deque<T>::clear(const unsigned int ch)
template <class T> template <class T>
T Gnss_circular_deque<T>::pop_front(const unsigned int ch) T Gnss_circular_deque<T>::pop_front(const unsigned int ch)
{ {
T result;
if (d_size[ch] > 0) if (d_size[ch] > 0)
{ {
d_size[ch]--; d_size[ch]--;
result = d_history[ch][d_index_pop[ch]]; T result = d_history[ch][d_index_pop[ch]];
if (d_size[ch] > 0)
{
d_index_pop[ch]++; d_index_pop[ch]++;
d_index_pop[ch] %= d_max_size; d_index_pop[ch] %= d_max_size;
} }
else
{
clear(ch);
}
return result; return result;
}
else
{
std::exception ex;
throw ex;
return d_return_void;
}
} }
template <class T> template <class T>
void Gnss_circular_deque<T>::push_back(const unsigned int ch, const T& new_data) void Gnss_circular_deque<T>::push_back(const unsigned int ch, const T& new_data)
{ {
bool increment_pop = true;
if (d_size[ch] < d_max_size)
{
increment_pop = false;
d_size[ch]++;
}
d_history[ch][d_index_push[ch]] = new_data; d_history[ch][d_index_push[ch]] = new_data;
d_index_push[ch]++; d_index_push[ch]++;
d_index_push[ch] %= d_max_size; d_index_push[ch] %= d_max_size;
if (d_size[ch] < d_max_size) if (increment_pop)
{ {
d_size[ch]++; d_index_pop[ch] = d_index_push[ch];
}
else
{
d_index_pop[ch]++;
d_index_pop[ch] %= d_max_size;
} }
} }

13
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -307,6 +307,7 @@ bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned i
} }
std::pair<unsigned int, unsigned int> ind = find_interp_elements(ch, ti); std::pair<unsigned int, unsigned int> ind = find_interp_elements(ch, ti);
//Linear interpolation parameters: y(t) = m * t + c
double m = 0.0; double m = 0.0;
double c = 0.0; double c = 0.0;
@ -378,8 +379,8 @@ std::pair<unsigned int, unsigned int> hybrid_observables_cc::find_interp_element
double dt = 0.0; double dt = 0.0;
for (unsigned int i = 0; i < d_gnss_synchro_history->size(ch); i++) for (unsigned int i = 0; i < d_gnss_synchro_history->size(ch); i++)
{ {
dt = std::fabs(ti - d_gnss_synchro_history->at(ch, i).RX_time); dt = ti - d_gnss_synchro_history->at(ch, i).RX_time;
if (dt < dif) if (dt < dif and dt > 0.0)
{ {
dif = dt; dif = dt;
closest = i; closest = i;
@ -398,18 +399,10 @@ std::pair<unsigned int, unsigned int> hybrid_observables_cc::find_interp_element
index2 = d_gnss_synchro_history->size(ch) - 1; index2 = d_gnss_synchro_history->size(ch) - 1;
} }
else else
{
if (d_gnss_synchro_history->at(ch, closest).RX_time < ti)
{ {
index1 = closest; index1 = closest;
index2 = closest + 1; index2 = closest + 1;
} }
else
{
index1 = closest - 1;
index2 = closest;
}
}
return std::pair<unsigned int, unsigned int>(index1, index2); return std::pair<unsigned int, unsigned int>(index1, index2);
} }