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

Minor changes

Browse Source
This commit is contained in:
Antonio Ramos 2018-02-19 17:20:34 +01:00
parent 0cba5951a7
commit 82084dd867
8 changed files with 184 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/algorithms/libs/gnss_sdr_sample_counter.cc
View File

@ -40,7 +40,6 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs) : gr::sync_decimato
{
message_port_register_out(pmt::mp("sample_counter"));
set_max_noutput_items(1);
set_max_output_buffer(1);
current_T_rx_ms = 0;
report_interval_ms = 1000;//default reporting 1 second
flag_enable_send_msg = false; //enable it for reporting time with asynchronous message

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

@ -55,17 +55,22 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned
gr::io_signature::make(nchannels_in, nchannels_in, sizeof(Gnss_Synchro)),
gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro)))
{
//set_max_noutput_items(1);
//set_max_output_buffer(1);
set_max_noutput_items(1);
set_max_output_buffer(1);
d_dump = dump;
set_T_rx_s = false;
d_nchannels = nchannels_out;
d_dump_filename = dump_filename;
T_rx_s = 0.0;
T_rx_step_s = 0.001; // 1 ms
max_extrapol_time_s = 0.1; // 100 ms
max_delta = 0.05; // 50 ms
valid_channels.resize(d_nchannels, false);
d_num_valid_channels = 0;
for(unsigned int i = 0; i < d_nchannels; i++)
{
d_gnss_synchro_history.push_back(std::deque<Gnss_Synchro>());
}
// ############# ENABLE DATA FILE LOG #################
if (d_dump)
@ -285,7 +290,7 @@ int hybrid_observables_cc::save_matfile()
return 0;
}
double Hybrid_Interpolate_data(const std::pair<Gnss_Synchro, Gnss_Synchro>& a, const double& ti, int parameter)
double hybrid_observables_cc::interpolate_data(const std::pair<Gnss_Synchro, Gnss_Synchro>& a, const double& ti, int parameter)
{
// x(ti) = m * ti + c
// m = [x(t2) - x(t1)] / [t2 - t1]
@ -321,7 +326,7 @@ double Hybrid_Interpolate_data(const std::pair<Gnss_Synchro, Gnss_Synchro>& a, c
return(m * ti + c);
}
double Hybrid_Compute_T_rx_s(const Gnss_Synchro& a)
double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro& a)
{
if(a.Flag_valid_word)
{
@ -330,11 +335,6 @@ double Hybrid_Compute_T_rx_s(const Gnss_Synchro& a)
else { return 0.0; }
}
bool Hybrid_find_trx(const Gnss_Synchro&a)
{
return(std::fabs(T_rx_s - a.RX_time) > trx_comp_thres);
}
/*
bool Hybrid_pairCompare_gnss_synchro_T_rx(const std::pair<Gnss_Synchro, Gnss_Synchro>& a, const std::pair<Gnss_Synchro, Gnss_Synchro>& b)
{
@ -393,14 +393,80 @@ bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b)
void hybrid_observables_cc::forecast(int noutput_items __attribute__((unused)),
gr_vector_int &ninput_items_required)
{
bool available_items = false;
// bool available_items = false;
// for(unsigned int i = 0; i < d_nchannels; i++)
// {
// ninput_items_required[i] = 0;
// if(detail()->input(i)->items_available() > 0) { available_items = true; }
// }
// if(available_items) { ninput_items_required[d_nchannels] = 0; }
// else { ninput_items_required[d_nchannels] = 1; }
for(unsigned int i = 0; i < d_nchannels; i++)
{
ninput_items_required[i] = 0;
if(detail()->input(i)->items_available() > 0) { available_items = true; }
}
if(available_items) { ninput_items_required[d_nchannels] = 0; }
else { ninput_items_required[d_nchannels] = 1; }
ninput_items_required[d_nchannels] = 1;
}
void hybrid_observables_cc::clean_history(std::deque<Gnss_Synchro>& data)
{
while(data.size() > 0)
{
if((T_rx_s - data.front().RX_time) > max_delta) { data.pop_front(); }
else { return; }
}
}
unsigned int hybrid_observables_cc::find_closest(std::deque<Gnss_Synchro>& data)
{
unsigned int result = 0;
double delta_t = std::numeric_limits<double>::max();
std::deque<Gnss_Synchro>::iterator it;
unsigned int aux = 0;
for(it = data.begin(); it != data.end(); it++)
{
double instant_delta = T_rx_s - it->RX_time;
if((instant_delta > 0) and (instant_delta < delta_t))
{
delta_t = instant_delta;
result = aux;
}
aux++;
}
return result;
}
double hybrid_observables_cc::find_min_RX_time()
{
if(d_num_valid_channels == 0) { return 0.0; }
std::vector<std::deque<Gnss_Synchro>>::iterator it = d_gnss_synchro_history.begin();
double result = std::numeric_limits<double>::max();
for(unsigned int i = 0; i < d_nchannels; i++)
{
if(valid_channels[i])
{
if(it->front().RX_time < result) { result = it->front().RX_time; }
}
it++;
}
return(floor(result * 1000.0) / 1000.0);
}
void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data)
{
double TOW_ref = std::numeric_limits<double>::lowest();
std::vector<Gnss_Synchro>::iterator it;
for(it = data.begin(); it != data.end(); it++)
{
if(it->RX_time > TOW_ref) { TOW_ref = it->RX_time; }
}
for(it = data.begin(); it != data.end(); it++)
{
double traveltime_s = TOW_ref - it->RX_time + GPS_STARTOFFSET_ms / 1000.0;
it->RX_time = TOW_ref + GPS_STARTOFFSET_ms / 1000.0;
it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
}
}
@ -414,35 +480,14 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
unsigned int i;
int total_input_items = 0;
for(i = 0; i < d_nchannels; i++) { total_input_items += ninput_items[i]; }
bool compute_output = false;
consume(d_nchannels, 1);
//////////////////////////////////////////////////////////////////////////
if((total_input_items == 0) and (ninput_items[d_nchannels] == 0))
if((total_input_items == 0) and (d_num_valid_channels == 0))
{
return 0;
}
else if((total_input_items == 0) and (ninput_items[d_nchannels] > 0) and (d_num_valid_channels == 0))
{
T_rx_s += T_rx_step_s;
consume(d_nchannels, 1);
return 0;
}
else if((total_input_items == 0) and (ninput_items[d_nchannels] > 0) and (d_num_valid_channels > 0))
{
T_rx_s += T_rx_step_s;
compute_output = true;
consume(d_nchannels, 1);
}
else if((total_input_items > 0) and (ninput_items[d_nchannels] == 0))
{}
else if((total_input_items > 0) and (ninput_items[d_nchannels] > 0))
{
T_rx_s += T_rx_step_s;
compute_output = true;
consume(d_nchannels, 1);
}
else
{}
if(set_T_rx_s) { T_rx_s += T_rx_step_s; }
//////////////////////////////////////////////////////////////////////////
@ -456,39 +501,110 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
{
for(int aux = 0; aux < ninput_items[i]; aux++)
{
it->push_back(in[i][aux]);
it->at(it->size() - 1).RX_time = Hybrid_Compute_T_rx_s(in[i][aux]);
}
consume(ninput_items[i], 1);
}
while(std::find(d_gnss_synchro_history.begin(), d_gnss_synchro_history.end(), Hybrid_compare_trx) != d_gnss_synchro_history.end())
if(in[i][aux].Flag_valid_word)
{
it->push_back(in[i][aux]);
it->back().RX_time = compute_T_rx_s(in[i][aux]);
}
}
consume(i, ninput_items[i]);
}
if (it->size() > 2) { valid_channels[i] = true; }
else { valid_channels[i] = false; }
i++;
}
}
for(i = 0; i < d_nchannels; i++)
{
if(d_gnss_synchro_history.at(i).size() > 2) { valid_channels[i] = true; }
else { valid_channels[i] = false; }
}
d_num_valid_channels = valid_channels.count();
// Check if there is any valid channel after reading the new incoming Gnss_Synchro data
if(d_num_valid_channels == 0) { return 0; }
if(d_num_valid_channels == 0)
{
set_T_rx_s = false;
return 0;
}
for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the extrapolation threshold
if(!set_T_rx_s) //Find the lowest RX_time among the valid observables in the history
{
T_rx_s = find_min_RX_time();
set_T_rx_s = true;
}
for(i = 0; i < d_nchannels; i++) //Discard observables with T_rx higher than the threshold
{
if(valid_channels[i])
{
double delta_t = T_rx_s - d_gnss_synchro_history.at(i).second.RX_time;
//std::cout << "Sat " << d_gnss_synchro_history.at(i).second.PRN << ". Dt = " << delta_t * 1000.0 <<". Rx 2 "<< d_gnss_synchro_history.at(i).second.RX_time<<". Rx 1 "<< d_gnss_synchro_history.at(i).first.RX_time<<std::endl;
if(std::fabs(T_rx_s - d_gnss_synchro_history.at(i).second.RX_time) > max_extrapol_time_s)
{ valid_channels[i] = false; }
clean_history(d_gnss_synchro_history.at(i));
if(d_gnss_synchro_history.at(i).size() < 2) { valid_channels[i] = false; }
}
}
d_num_valid_channels = valid_channels.count();
// Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time
if((d_num_valid_channels == 0) or !compute_output) { return 0; }
d_num_valid_channels = valid_channels.count();
if(d_num_valid_channels == 0)
{
set_T_rx_s = false;
return 0;
}
std::vector<Gnss_Synchro> epoch_data;
i = 0;
for(it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++)
{
if(valid_channels[i])
{
unsigned int index_closest = find_closest(*it);
unsigned int index1;
unsigned int index2;
if(index_closest == (it->size() - 1))
{
index1 = index_closest - 1;
index2 = index_closest;
}
else
{
index1 = index_closest;
index2 = index_closest + 1;
}
Gnss_Synchro interpolated_gnss_synchro = it->at(index1);
interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(
std::pair<Gnss_Synchro, Gnss_Synchro>(it->at(index2), it->at(index1)), T_rx_s, 0);
interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(
std::pair<Gnss_Synchro, Gnss_Synchro>(it->at(index2), it->at(index1)), T_rx_s, 1);
interpolated_gnss_synchro.RX_time = interpolate_data(
std::pair<Gnss_Synchro, Gnss_Synchro>(it->at(index2), it->at(index1)), T_rx_s, 2);
//interpolated_gnss_synchro.Code_phase_samples = interpolate_data(
// std::pair<Gnss_Synchro, Gnss_Synchro>(it->at(index2), it->at(index1)), T_rx_s, 3);
epoch_data.push_back(interpolated_gnss_synchro);
}
i++;
}
correct_TOW_and_compute_prange(epoch_data);
std::vector<Gnss_Synchro>::iterator it2 = epoch_data.begin();
for(i = 0; i < d_nchannels; i++)
{
if(valid_channels[i])
{
out[i][0] = (*it2);
out[i][0].Flag_valid_pseudorange = true;
it2++;
}
else
{
out[i][0] = Gnss_Synchro();
out[i][0].Flag_valid_pseudorange = false;
}
}
return 1;
/* ANTONIO
it = d_gnss_synchro_history.begin();
double TOW_ref = std::numeric_limits<double>::max();
for(i = 0; i < d_nchannels; i++)
@ -520,6 +636,8 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
}
return 1;
*/
/******************************* OLD ALGORITHM ********************************/
// const Gnss_Synchro** in = reinterpret_cast<const Gnss_Synchro**>(&input_items[0]); // Get the input buffer pointer

11
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.h
View File

@ -35,7 +35,7 @@
#include <fstream>
#include <string>
//#include <utility> //std::pair
#include <utility> //std::pair
#include <vector> //std::vector
#include <deque>
#include <boost/dynamic_bitset.hpp>
@ -64,14 +64,21 @@ private:
friend hybrid_observables_cc_sptr
hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename);
hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename);
void clean_history(std::deque<Gnss_Synchro>& data);
double compute_T_rx_s(const Gnss_Synchro& a);
double interpolate_data(const std::pair<Gnss_Synchro, Gnss_Synchro>& a, const double& ti, int parameter);
double find_min_RX_time();
unsigned int find_closest(std::deque<Gnss_Synchro>& data);
void correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data);
//Tracking observable history
std::vector<std::deque<Gnss_Synchro>> d_gnss_synchro_history;
boost::dynamic_bitset<> valid_channels;
double T_rx_s;
double T_rx_step_s;
double max_extrapol_time_s;
double max_delta;
bool d_dump;
bool set_T_rx_s;
unsigned int d_nchannels;
unsigned int d_num_valid_channels;
std::string d_dump_filename;

1
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc
View File

@ -108,7 +108,6 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc(
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
set_max_noutput_items(1);
set_max_output_buffer(1);
// Telemetry Bit transition synchronization port out
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// Ephemeris data port out

1
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
View File

@ -184,7 +184,6 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
set_max_noutput_items(1);
set_max_output_buffer(1);
// Telemetry Bit transition synchronization port out
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// Ephemeris data port out

1
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -56,7 +56,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
set_max_noutput_items(1);
set_max_output_buffer(1);
// Telemetry Bit transition synchronization port out
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// Ephemeris data port out

1
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc
View File

@ -53,7 +53,6 @@ gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc(
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
set_max_noutput_items(1);
set_max_output_buffer(1);
// Telemetry Bit transition synchronization port out
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// Ephemeris data port out

1
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.cc
View File

@ -55,7 +55,6 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc(
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
set_max_noutput_items(1);
set_max_output_buffer(1);
// Telemetry Bit transition synchronization port out
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
// Ephemeris data port out