mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-09-28 15:08:51 +00:00
Minor changes
This commit is contained in:
Antonio Ramos
parent
0cba5951a7
commit
82084dd867
@ -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
|
||||
|
||||
@ -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; }
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
@ -450,45 +495,116 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
|
||||
if (total_input_items > 0)
|
||||
{
|
||||
i = 0;
|
||||
for (it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++)
|
||||
for(it = d_gnss_synchro_history.begin(); it != d_gnss_synchro_history.end(); it++)
|
||||
{
|
||||
if (ninput_items[i] > 0)
|
||||
if(ninput_items[i] > 0)
|
||||
{
|
||||
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]);
|
||||
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(ninput_items[i], 1);
|
||||
consume(i, ninput_items[i]);
|
||||
}
|
||||
while(std::find(d_gnss_synchro_history.begin(), d_gnss_synchro_history.end(), Hybrid_compare_trx) != d_gnss_synchro_history.end())
|
||||
{
|
||||
|
||||
}
|
||||
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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user