mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-15 20:50:33 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into next

Browse Source
This commit is contained in:
Carles Fernandez 2018-08-07 20:21:51 +02:00
commit f01a590c8f
No known key found for this signature in database
GPG Key ID: 4C583C52B0C3877D
24 changed files with 1499 additions and 704 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -176,6 +176,7 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
band2 = true; band2 = true;
} }
} }
break;
default: default:
{ {
} }

19
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc
View File

@ -76,16 +76,13 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
acq_parameters.max_dwells = max_dwells_; acq_parameters.max_dwells = max_dwells_;
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
acq_parameters.dump_filename = dump_filename_; acq_parameters.dump_filename = dump_filename_;
acq_parameters.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1);
//--- Find number of samples per spreading code ------------------------- //--- Find number of samples per spreading code -------------------------
code_length_ = static_cast<unsigned int>(std::round(static_cast<double>(fs_in_) / (GPS_L5i_CODE_RATE_HZ / static_cast<double>(GPS_L5i_CODE_LENGTH_CHIPS)))); code_length_ = static_cast<unsigned int>(std::floor(static_cast<double>(fs_in_) / (GPS_L5i_CODE_RATE_HZ / GPS_L5i_CODE_LENGTH_CHIPS)));
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
vector_length_ = code_length_; acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GPS_L5i_PERIOD * 1000.0);
if (bit_transition_flag_)
{
vector_length_ *= 2;
}
vector_length_ = std::floor(acq_parameters.sampled_ms * acq_parameters.samples_per_ms) * (acq_parameters.bit_transition_flag ? 2 : 1);
code_ = new gr_complex[vector_length_]; code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0) if (item_type_.compare("cshort") == 0)
@ -96,11 +93,9 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
{ {
item_size_ = sizeof(gr_complex); item_size_ = sizeof(gr_complex);
} }
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GPS_L5i_PERIOD * 1000.0);
acq_parameters.ms_per_code = 1; acq_parameters.ms_per_code = 1;
acq_parameters.it_size = item_size_; acq_parameters.it_size = item_size_;
acq_parameters.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1);
num_codes_ = acq_parameters.sampled_ms; num_codes_ = acq_parameters.sampled_ms;
acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4);
acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0);
@ -108,7 +103,6 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
acq_parameters.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false); acq_parameters.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false);
acquisition_ = pcps_make_acquisition(acq_parameters); acquisition_ = pcps_make_acquisition(acq_parameters);
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")";
@ -117,7 +111,6 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
cbyte_to_float_x2_ = make_complex_byte_to_float_x2(); cbyte_to_float_x2_ = make_complex_byte_to_float_x2();
float_to_complex_ = gr::blocks::float_to_complex::make(); float_to_complex_ = gr::blocks::float_to_complex::make();
} }
channel_ = 0; channel_ = 0;
threshold_ = 0.0; threshold_ = 0.0;
doppler_step_ = 0; doppler_step_ = 0;

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

@ -36,14 +36,16 @@
#include <iostream> #include <iostream>
#include <string> #include <string>
gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, size_t _size) : gr::sync_decimator("sample_counter", gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, int _interval_ms, size_t _size) : gr::sync_decimator("sample_counter",
gr::io_signature::make(1, 1, _size), gr::io_signature::make(1, 1, _size),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
static_cast<unsigned int>(std::round(_fs * 0.001))) static_cast<unsigned int>(std::round(_fs * static_cast<double>(_interval_ms) / 1e3)))
{ {
message_port_register_out(pmt::mp("sample_counter")); message_port_register_out(pmt::mp("sample_counter"));
set_max_noutput_items(1); set_max_noutput_items(1);
samples_per_output = std::round(_fs * 0.001); interval_ms = _interval_ms;
fs = _fs;
samples_per_output = std::round(fs * static_cast<double>(interval_ms) / 1e3);
sample_counter = 0; sample_counter = 0;
current_T_rx_ms = 0; current_T_rx_ms = 0;
current_s = 0; current_s = 0;
@ -58,9 +60,9 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter(double _fs, size_t _size) : gr:
} }
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size) gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size)
{ {
gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _size)); gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter(_fs, _interval_ms, _size));
return sample_counter_; return sample_counter_;
} }
@ -74,6 +76,7 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)),
out[0].Flag_valid_symbol_output = false; out[0].Flag_valid_symbol_output = false;
out[0].Flag_valid_word = false; out[0].Flag_valid_word = false;
out[0].Channel_ID = -1; out[0].Channel_ID = -1;
out[0].fs = fs;
if ((current_T_rx_ms % report_interval_ms) == 0) if ((current_T_rx_ms % report_interval_ms) == 0)
{ {
current_s++; current_s++;
@ -134,6 +137,6 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)),
} }
sample_counter += samples_per_output; sample_counter += samples_per_output;
out[0].Tracking_sample_counter = sample_counter; out[0].Tracking_sample_counter = sample_counter;
current_T_rx_ms++; current_T_rx_ms += interval_ms;
return 1; return 1;
} }

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

@ -39,14 +39,16 @@ class gnss_sdr_sample_counter;
typedef boost::shared_ptr<gnss_sdr_sample_counter> gnss_sdr_sample_counter_sptr; typedef boost::shared_ptr<gnss_sdr_sample_counter> gnss_sdr_sample_counter_sptr;
gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size); gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size);
class gnss_sdr_sample_counter : public gr::sync_decimator class gnss_sdr_sample_counter : public gr::sync_decimator
{ {
private: private:
gnss_sdr_sample_counter(double _fs, size_t _size); gnss_sdr_sample_counter(double _fs, int _interval_ms, size_t _size);
unsigned int samples_per_output; unsigned int samples_per_output;
unsigned long int sample_counter; double fs;
unsigned long long int sample_counter;
int interval_ms;
long long int current_T_rx_ms; // Receiver time in ms since the beginning of the run long long int current_T_rx_ms; // Receiver time in ms since the beginning of the run
unsigned int current_s; // Receiver time in seconds, modulo 60 unsigned int current_s; // Receiver time in seconds, modulo 60
bool flag_m; // True if the receiver has been running for at least 1 minute bool flag_m; // True if the receiver has been running for at least 1 minute
@ -59,7 +61,7 @@ private:
bool flag_enable_send_msg; bool flag_enable_send_msg;
public: public:
friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, size_t _size); friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int _interval_ms, size_t _size);
int work(int noutput_items, int work(int noutput_items,
gr_vector_const_void_star &input_items, gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items); gr_vector_void_star &output_items);

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

@ -32,7 +32,6 @@
#include "hybrid_observables_cc.h" #include "hybrid_observables_cc.h"
#include "display.h" #include "display.h"
#include "GPS_L1_CA.h" #include "GPS_L1_CA.h"
#include <armadillo>
#include <glog/logging.h> #include <glog/logging.h>
#include <gnuradio/io_signature.h> #include <gnuradio/io_signature.h>
#include <matio.h> #include <matio.h>
@ -59,15 +58,11 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro))) gr::io_signature::make(nchannels_out, nchannels_out, sizeof(Gnss_Synchro)))
{ {
d_dump = dump; d_dump = dump;
d_nchannels = nchannels_out; d_nchannels_out = nchannels_out;
d_nchannels_in = nchannels_in;
d_dump_filename = dump_filename; d_dump_filename = dump_filename;
T_rx_s = 0.0; T_rx_clock_step_samples = 0;
T_rx_step_ms = 1; // 1 ms d_gnss_synchro_history = new Gnss_circular_deque<Gnss_Synchro>(500, d_nchannels_out);
max_delta = 1.5; // 1.5 s
d_latency = 0.5; // 300 ms
valid_channels.resize(d_nchannels, false);
d_num_valid_channels = 0;
d_gnss_synchro_history = new Gnss_circular_deque<Gnss_Synchro>(static_cast<unsigned int>(max_delta * 1000.0), d_nchannels);
// ############# ENABLE DATA FILE LOG ################# // ############# ENABLE DATA FILE LOG #################
if (d_dump) if (d_dump)
@ -88,7 +83,12 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in,
} }
} }
T_rx_TOW_ms = 0; T_rx_TOW_ms = 0;
T_rx_TOW_offset_ms = 0;
T_rx_TOW_set = false; T_rx_TOW_set = false;
//rework
d_Rx_clock_buffer.resize(10); //10*20ms= 200 ms of data in buffer
d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer
} }
@ -120,7 +120,7 @@ int hybrid_observables_cc::save_matfile()
// READ DUMP FILE // READ DUMP FILE
std::ifstream::pos_type size; std::ifstream::pos_type size;
int number_of_double_vars = 7; int number_of_double_vars = 7;
int epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels; int epoch_size_bytes = sizeof(double) * number_of_double_vars * d_nchannels_out;
std::ifstream dump_file; std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try try
@ -144,15 +144,15 @@ int hybrid_observables_cc::save_matfile()
{ {
return 1; return 1;
} }
double **RX_time = new double *[d_nchannels]; double **RX_time = new double *[d_nchannels_out];
double **TOW_at_current_symbol_s = new double *[d_nchannels]; double **TOW_at_current_symbol_s = new double *[d_nchannels_out];
double **Carrier_Doppler_hz = new double *[d_nchannels]; double **Carrier_Doppler_hz = new double *[d_nchannels_out];
double **Carrier_phase_cycles = new double *[d_nchannels]; double **Carrier_phase_cycles = new double *[d_nchannels_out];
double **Pseudorange_m = new double *[d_nchannels]; double **Pseudorange_m = new double *[d_nchannels_out];
double **PRN = new double *[d_nchannels]; double **PRN = new double *[d_nchannels_out];
double **Flag_valid_pseudorange = new double *[d_nchannels]; double **Flag_valid_pseudorange = new double *[d_nchannels_out];
for (unsigned int i = 0; i < d_nchannels; i++) for (unsigned int i = 0; i < d_nchannels_out; i++)
{ {
RX_time[i] = new double[num_epoch]; RX_time[i] = new double[num_epoch];
TOW_at_current_symbol_s[i] = new double[num_epoch]; TOW_at_current_symbol_s[i] = new double[num_epoch];
@ -169,7 +169,7 @@ int hybrid_observables_cc::save_matfile()
{ {
for (long int i = 0; i < num_epoch; i++) for (long int i = 0; i < num_epoch; i++)
{ {
for (unsigned int chan = 0; chan < d_nchannels; chan++) for (unsigned int chan = 0; chan < d_nchannels_out; chan++)
{ {
dump_file.read(reinterpret_cast<char *>(&RX_time[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast<char *>(&RX_time[chan][i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_s[chan][i]), sizeof(double)); dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_s[chan][i]), sizeof(double));
@ -186,7 +186,7 @@ int hybrid_observables_cc::save_matfile()
catch (const std::ifstream::failure &e) catch (const std::ifstream::failure &e)
{ {
std::cerr << "Problem reading dump file:" << e.what() << std::endl; std::cerr << "Problem reading dump file:" << e.what() << std::endl;
for (unsigned int i = 0; i < d_nchannels; i++) for (unsigned int i = 0; i < d_nchannels_out; i++)
{ {
delete[] RX_time[i]; delete[] RX_time[i];
delete[] TOW_at_current_symbol_s[i]; delete[] TOW_at_current_symbol_s[i];
@ -207,17 +207,17 @@ int hybrid_observables_cc::save_matfile()
return 1; return 1;
} }
double *RX_time_aux = new double[d_nchannels * num_epoch]; double *RX_time_aux = new double[d_nchannels_out * num_epoch];
double *TOW_at_current_symbol_s_aux = new double[d_nchannels * num_epoch]; double *TOW_at_current_symbol_s_aux = new double[d_nchannels_out * num_epoch];
double *Carrier_Doppler_hz_aux = new double[d_nchannels * num_epoch]; double *Carrier_Doppler_hz_aux = new double[d_nchannels_out * num_epoch];
double *Carrier_phase_cycles_aux = new double[d_nchannels * num_epoch]; double *Carrier_phase_cycles_aux = new double[d_nchannels_out * num_epoch];
double *Pseudorange_m_aux = new double[d_nchannels * num_epoch]; double *Pseudorange_m_aux = new double[d_nchannels_out * num_epoch];
double *PRN_aux = new double[d_nchannels * num_epoch]; double *PRN_aux = new double[d_nchannels_out * num_epoch];
double *Flag_valid_pseudorange_aux = new double[d_nchannels * num_epoch]; double *Flag_valid_pseudorange_aux = new double[d_nchannels_out * num_epoch];
unsigned int k = 0; unsigned int k = 0;
for (long int j = 0; j < num_epoch; j++) for (long int j = 0; j < num_epoch; j++)
{ {
for (unsigned int i = 0; i < d_nchannels; i++) for (unsigned int i = 0; i < d_nchannels_out; i++)
{ {
RX_time_aux[k] = RX_time[i][j]; RX_time_aux[k] = RX_time[i][j];
TOW_at_current_symbol_s_aux[k] = TOW_at_current_symbol_s[i][j]; TOW_at_current_symbol_s_aux[k] = TOW_at_current_symbol_s[i][j];
@ -242,7 +242,7 @@ int hybrid_observables_cc::save_matfile()
matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73); matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
if (reinterpret_cast<long *>(matfp) != NULL) if (reinterpret_cast<long *>(matfp) != NULL)
{ {
size_t dims[2] = {static_cast<size_t>(d_nchannels), static_cast<size_t>(num_epoch)}; size_t dims[2] = {static_cast<size_t>(d_nchannels_out), static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux, MAT_F_DONT_COPY_DATA); matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, RX_time_aux, MAT_F_DONT_COPY_DATA);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
@ -273,7 +273,7 @@ int hybrid_observables_cc::save_matfile()
} }
Mat_Close(matfp); Mat_Close(matfp);
for (unsigned int i = 0; i < d_nchannels; i++) for (unsigned int i = 0; i < d_nchannels_out; i++)
{ {
delete[] RX_time[i]; delete[] RX_time[i];
delete[] TOW_at_current_symbol_s[i]; delete[] TOW_at_current_symbol_s[i];
@ -302,175 +302,165 @@ int hybrid_observables_cc::save_matfile()
} }
bool hybrid_observables_cc::interpolate_data(Gnss_Synchro &out, const unsigned int &ch, const double &ti)
{
if ((ti < d_gnss_synchro_history->front(ch).RX_time) or (ti > d_gnss_synchro_history->back(ch).RX_time))
{
return false;
}
find_interp_elements(ch, ti);
// 1st: copy the nearest gnss_synchro data for that channel
out = d_gnss_synchro_history->at(ch, 0);
// 2nd: Linear interpolation: y(t) = y(t1) + (y(t2) - y(t1)) * (t - t1) / (t2 - t1)
// CARRIER PHASE INTERPOLATION
out.Carrier_phase_rads = d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads + (d_gnss_synchro_history->at(ch, 1).Carrier_phase_rads - d_gnss_synchro_history->at(ch, 0).Carrier_phase_rads) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
// CARRIER DOPPLER INTERPOLATION
out.Carrier_Doppler_hz = d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz + (d_gnss_synchro_history->at(ch, 1).Carrier_Doppler_hz - d_gnss_synchro_history->at(ch, 0).Carrier_Doppler_hz) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
// TOW INTERPOLATION
out.interp_TOW_ms = static_cast<double>(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms) + (static_cast<double>(d_gnss_synchro_history->at(ch, 1).TOW_at_current_symbol_ms) - static_cast<double>(d_gnss_synchro_history->at(ch, 0).TOW_at_current_symbol_ms)) * (ti - d_gnss_synchro_history->at(ch, 0).RX_time) / (d_gnss_synchro_history->at(ch, 1).RX_time - d_gnss_synchro_history->at(ch, 0).RX_time);
return true;
}
double hybrid_observables_cc::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)
{ {
return ((static_cast<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(a.fs)); return ((static_cast<double>(a.Tracking_sample_counter) + a.Code_phase_samples) / static_cast<double>(a.fs));
} }
else
{
return 0.0;
}
}
bool hybrid_observables_cc::interp_trk_obs(Gnss_Synchro &interpolated_obs, const unsigned int &ch, const unsigned long int &rx_clock)
void hybrid_observables_cc::find_interp_elements(const unsigned int &ch, const double &ti)
{ {
unsigned int closest = 0; int nearest_element = -1;
double dif = std::numeric_limits<double>::max(); long int abs_diff;
double dt = 0.0; long int old_abs_diff = std::numeric_limits<long int>::max();
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); abs_diff = abs(rx_clock - d_gnss_synchro_history->at(ch, i).Tracking_sample_counter);
if (dt < dif) if (old_abs_diff > abs_diff)
{ {
closest = i; old_abs_diff = abs_diff;
dif = dt; nearest_element = i;
}
}
if (nearest_element != -1 and nearest_element != 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)
{
int neighbor_element;
if (rx_clock > d_gnss_synchro_history->at(ch, nearest_element).Tracking_sample_counter)
{
neighbor_element = nearest_element + 1;
} }
else else
{ {
break; neighbor_element = nearest_element - 1;
} }
if (neighbor_element < d_gnss_synchro_history->size(ch) and neighbor_element >= 0)
{
int t1_idx;
int t2_idx;
if (rx_clock > d_gnss_synchro_history->at(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;
t1_idx = nearest_element;
t2_idx = neighbor_element;
} }
if (ti > d_gnss_synchro_history->at(ch, closest).RX_time) else
{ {
while (closest > 0) //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;
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);
// 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);
// 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;
// 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;
// TOW INTERPOLATION
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;
//
// 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";
return true;
}
else
{ {
d_gnss_synchro_history->pop_front(ch); return false;
closest--;
} }
} }
else else
{ {
while (closest > 1) // 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";
// usleep(1000);
return false;
}
}
else
{ {
d_gnss_synchro_history->pop_front(ch); return false;
closest--;
} }
} }
}
void hybrid_observables_cc::forecast(int noutput_items, gr_vector_int &ninput_items_required) void hybrid_observables_cc::forecast(int noutput_items, gr_vector_int &ninput_items_required)
{ {
for (unsigned int i = 0; i < d_nchannels; i++) for (int n = 0; n < d_nchannels_in - 1; n++)
{ {
ninput_items_required[i] = 0; ninput_items_required[n] = 0;
} }
ninput_items_required[d_nchannels] = noutput_items; //last input channel is the sample counter, triggered each ms
ninput_items_required[d_nchannels_in - 1] = 1;
} }
void hybrid_observables_cc::clean_history(unsigned int pos) void hybrid_observables_cc::update_TOW(std::vector<Gnss_Synchro> &data)
{
while (d_gnss_synchro_history->size(pos) > 0)
{
if ((T_rx_s - d_gnss_synchro_history->front(pos).RX_time) > max_delta)
{
d_gnss_synchro_history->pop_front(pos);
}
else
{
return;
}
}
}
void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Synchro> &data)
{ {
//1. Set the TOW using the minimum TOW in the observables.
// this will be the receiver time.
//2. If the TOW is set, it must be incremented by the desired receiver time step.
// the time step must match the observables timer block (connected to the las input channel)
std::vector<Gnss_Synchro>::iterator it; std::vector<Gnss_Synchro>::iterator it;
// if (!T_rx_TOW_set)
/////////////////////// DEBUG ////////////////////////// // {
// Logs if there is a pseudorange difference between //unsigned int TOW_ref = std::numeric_limits<unsigned int>::max();
// signals of the same satellite higher than a threshold unsigned int TOW_ref = 0;
////////////////////////////////////////////////////////
#ifndef NDEBUG
std::vector<Gnss_Synchro>::iterator it2;
double thr_ = 250.0 / SPEED_OF_LIGHT; // Maximum pseudorange difference = 250 meters
for (it = data.begin(); it != (data.end() - 1); it++)
{
for (it2 = it + 1; it2 != data.end(); it2++)
{
if (it->PRN == it2->PRN and it->System == it2->System)
{
double tow_dif_ = std::fabs(it->TOW_at_current_symbol_ms - it2->TOW_at_current_symbol_ms);
if (tow_dif_ > thr_ * 1000.0)
{
DLOG(INFO) << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal
<< ". TOW difference in PRN " << it->PRN
<< " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT
<< " meters in pseudorange";
std::cout << TEXT_RED << "System " << it->System << ". Signals " << it->Signal << " and " << it2->Signal
<< ". TOW difference in PRN " << it->PRN
<< " = " << tow_dif_ * 1e3 << "[ms]. Equivalent to " << tow_dif_ * SPEED_OF_LIGHT
<< " meters in pseudorange" << TEXT_RESET << std::endl;
}
}
}
}
#endif
if (!T_rx_TOW_set)
{
unsigned int TOW_ref = std::numeric_limits<unsigned int>::lowest();
for (it = data.begin(); it != data.end(); it++) for (it = data.begin(); it != data.end(); it++)
{
if (it->Flag_valid_word)
{ {
if (it->TOW_at_current_symbol_ms > TOW_ref) if (it->TOW_at_current_symbol_ms > TOW_ref)
{ {
TOW_ref = it->TOW_at_current_symbol_ms; TOW_ref = it->TOW_at_current_symbol_ms;
T_rx_TOW_set = true;
}
} }
} }
T_rx_TOW_ms = TOW_ref; T_rx_TOW_ms = TOW_ref;
T_rx_TOW_set = true; //}
// else
// {
// T_rx_TOW_ms += T_rx_step_ms;
// //todo: check what happens during the week rollover
// if (T_rx_TOW_ms >= 604800000)
// {
// T_rx_TOW_ms = T_rx_TOW_ms % 604800000;
// }
// }
// std::cout << "T_rx_TOW_ms: " << T_rx_TOW_ms << std::endl;
} }
else void hybrid_observables_cc::compute_pranges(std::vector<Gnss_Synchro> &data)
{ {
T_rx_TOW_ms += T_rx_step_ms; std::vector<Gnss_Synchro>::iterator it;
//todo: check what happens during the week rollover
if (T_rx_TOW_ms >= 604800000)
{
T_rx_TOW_ms = T_rx_TOW_ms % 604800000;
}
}
for (it = data.begin(); it != data.end(); it++) for (it = data.begin(); it != data.end(); it++)
{
if (it->Flag_valid_word)
{ {
double traveltime_s = (static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0; double traveltime_s = (static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
//todo: check what happens during the week rollover (TOW rollover at 604800000s)
//std::cout.precision(17); it->RX_time = (static_cast<double>(T_rx_TOW_ms) + GPS_STARTOFFSET_ms) / 1000.0;
//std::cout << "Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
it->RX_time = (T_rx_TOW_ms + GPS_STARTOFFSET_ms) / 1000.0;
it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT; it->Pseudorange_m = traveltime_s * SPEED_OF_LIGHT;
it->Flag_valid_pseudorange = true;
//debug code
// std::cout.precision(17);
// std::cout << "[" << it->Channel_ID << "] interp_TOW_ms: " << it->interp_TOW_ms << std::endl;
// std::cout << "[" << it->Channel_ID << "] Diff T_rx_TOW_ms - interp_TOW_ms: " << static_cast<double>(T_rx_TOW_ms) - it->interp_TOW_ms << std::endl;
// std::cout << "[" << it->Channel_ID << "] Pseudorange_m: " << it->Pseudorange_m << std::endl;
} }
} }
// usleep(1000);
}
int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)), int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)),
@ -480,156 +470,105 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]); const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]);
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]);
unsigned int i; //push receiver clock into history buffer (connected to the last of the input channels)
unsigned int returned_elements = 0; //The clock buffer gives time to the channels to compute the tracking observables
int total_input_items = 0; if (ninput_items[d_nchannels_in - 1] > 0)
for (i = 0; i < d_nchannels; i++)
{ {
total_input_items += ninput_items[i]; d_Rx_clock_buffer.push_back(in[d_nchannels_in - 1][0].Tracking_sample_counter);
if (T_rx_clock_step_samples == 0)
{
T_rx_clock_step_samples = std::round(static_cast<double>(in[d_nchannels_in - 1][0].fs) * 1e-3); // 1 ms
std::cout << "Observables clock step samples set to " << T_rx_clock_step_samples << std::endl;
usleep(1000000);
} }
for (int epoch = 0; epoch < ninput_items[d_nchannels]; epoch++)
{
T_rx_s += (static_cast<double>(T_rx_step_ms) / 1000.0);
////////////////////////////////////////////////////////////////////////// //consume one item from the clock channel (last of the input channels)
if ((total_input_items == 0) and (d_num_valid_channels == 0)) consume(d_nchannels_in - 1, 1);
{
consume(d_nchannels, epoch + 1);
return returned_elements;
} }
////////////////////////////////////////////////////////////////////////// //push the tracking observables into buffers to allow the observable interpolation at the desired Rx clock
for (int n = 0; n < d_nchannels_out; n++)
if (total_input_items > 0 and epoch == 0)
{ {
for (i = 0; i < d_nchannels; i++) // push the valid tracking Gnss_Synchros to their corresponding deque
for (int m = 0; m < ninput_items[n]; m++)
{ {
if (ninput_items[i] > 0) if (in[n][m].Flag_valid_word)
{ {
// Add the new Gnss_Synchros to their corresponding deque if (d_gnss_synchro_history->size(n) > 0)
for (int aux = 0; aux < ninput_items[i]; aux++)
{ {
if (in[i][aux].Flag_valid_word)
{
d_gnss_synchro_history->push_back(i, in[i][aux]);
d_gnss_synchro_history->back(i).RX_time = compute_T_rx_s(in[i][aux]);
// Check if the last Gnss_Synchro comes from the same satellite as the previous ones // Check if the last Gnss_Synchro comes from the same satellite as the previous ones
if (d_gnss_synchro_history->size(i) > 1) if (d_gnss_synchro_history->front(n).PRN != in[n][m].PRN)
{ {
if (d_gnss_synchro_history->front(i).PRN != d_gnss_synchro_history->back(i).PRN) d_gnss_synchro_history->clear(n);
{
d_gnss_synchro_history->clear(i);
} }
} }
d_gnss_synchro_history->push_back(n, in[n][m]);
d_gnss_synchro_history->back(n).RX_time = compute_T_rx_s(in[n][m]);
} }
} }
consume(i, ninput_items[i]); consume(n, ninput_items[n]);
}
}
} }
for (i = 0; i < d_nchannels; i++) if (d_Rx_clock_buffer.size() == d_Rx_clock_buffer.capacity())
{ {
if (d_gnss_synchro_history->size(i) > 2) std::vector<Gnss_Synchro> epoch_data;
int n_valid = 0;
for (int n = 0; n < d_nchannels_out; n++)
{ {
valid_channels[i] = true; Gnss_Synchro interpolated_gnss_synchro;
if (!interp_trk_obs(interpolated_gnss_synchro, n, d_Rx_clock_buffer.front() + T_rx_TOW_offset_ms * T_rx_clock_step_samples))
{
//produce an empty observation
interpolated_gnss_synchro = Gnss_Synchro();
interpolated_gnss_synchro.Flag_valid_pseudorange = false;
interpolated_gnss_synchro.Flag_valid_word = false;
interpolated_gnss_synchro.Flag_valid_acquisition = false;
interpolated_gnss_synchro.fs = 0;
interpolated_gnss_synchro.Channel_ID = n;
} }
else else
{ {
valid_channels[i] = false; n_valid++;
} }
}
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)
{
consume(d_nchannels, epoch + 1);
return returned_elements;
}
for (i = 0; i < d_nchannels; i++) // Discard observables with T_rx higher than the threshold
{
if (valid_channels[i])
{
clean_history(i);
if (d_gnss_synchro_history->size(i) < 2)
{
valid_channels[i] = false;
}
}
}
// Check if there is any valid channel after computing the time distance between the Gnss_Synchro data and the receiver time
d_num_valid_channels = valid_channels.count();
double T_rx_s_out = T_rx_s - d_latency;
if ((d_num_valid_channels == 0) or (T_rx_s_out < 0.0))
{
consume(d_nchannels, epoch + 1);
return returned_elements;
}
std::vector<Gnss_Synchro> epoch_data;
for (i = 0; i < d_nchannels; i++)
{
if (valid_channels[i])
{
Gnss_Synchro interpolated_gnss_synchro; // empty set, it is required to COPY the nearest in the interpolation history = d_gnss_synchro_history->back(i);
if (interpolate_data(interpolated_gnss_synchro, i, T_rx_s_out))
{
epoch_data.push_back(interpolated_gnss_synchro); epoch_data.push_back(interpolated_gnss_synchro);
} }
else if (n_valid > 0)
{ {
valid_channels[i] = false; update_TOW(epoch_data);
} if (T_rx_TOW_ms % 20 != 0)
}
}
d_num_valid_channels = valid_channels.count();
if (d_num_valid_channels == 0)
{ {
consume(d_nchannels, epoch + 1); T_rx_TOW_offset_ms = T_rx_TOW_ms % 20;
return returned_elements; }
} }
correct_TOW_and_compute_prange(epoch_data); if (n_valid > 0) compute_pranges(epoch_data);
std::vector<Gnss_Synchro>::iterator it = epoch_data.begin();
for (i = 0; i < d_nchannels; i++) for (int n = 0; n < d_nchannels_out; n++)
{ {
if (valid_channels[i]) out[n][0] = epoch_data.at(n);
{
out[i][epoch] = (*it);
out[i][epoch].Flag_valid_pseudorange = true;
it++;
}
else
{
out[i][epoch] = Gnss_Synchro();
out[i][epoch].Flag_valid_pseudorange = false;
}
} }
if (d_dump) if (d_dump)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
try try
{ {
double tmp_double; double tmp_double;
for (i = 0; i < d_nchannels; i++) for (int i = 0; i < d_nchannels_out; i++)
{ {
tmp_double = out[i][epoch].RX_time; tmp_double = out[i][0].RX_time;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].interp_TOW_ms / 1000.0; tmp_double = out[i][0].interp_TOW_ms / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].Carrier_Doppler_hz; tmp_double = out[i][0].Carrier_Doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].Carrier_phase_rads / GPS_TWO_PI; tmp_double = out[i][0].Carrier_phase_rads / GPS_TWO_PI;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = out[i][epoch].Pseudorange_m; tmp_double = out[i][0].Pseudorange_m;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(out[i][epoch].PRN); tmp_double = static_cast<double>(out[i][0].PRN);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_double = static_cast<double>(out[i][epoch].Flag_valid_pseudorange); tmp_double = static_cast<double>(out[i][0].Flag_valid_pseudorange);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
} }
} }
@ -639,9 +578,10 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
d_dump = false; d_dump = false;
} }
} }
return 1;
returned_elements++; }
else
{
return 0;
} }
consume(d_nchannels, ninput_items[d_nchannels]);
return returned_elements;
} }

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

@ -65,26 +65,25 @@ private:
friend hybrid_observables_cc_sptr friend hybrid_observables_cc_sptr
hybrid_make_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename); 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); hybrid_observables_cc(unsigned int nchannels_in, unsigned int nchannels_out, bool dump, std::string dump_filename);
void clean_history(unsigned int pos);
double compute_T_rx_s(const Gnss_Synchro& a);
bool interpolate_data(Gnss_Synchro& out, const unsigned int& ch, const double& ti); bool interpolate_data(Gnss_Synchro& out, const unsigned int& ch, const double& ti);
void find_interp_elements(const unsigned int& ch, const double& ti); bool interp_trk_obs(Gnss_Synchro& interpolated_obs, const unsigned int& ch, const unsigned long int& rx_clock);
void correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data); double compute_T_rx_s(const Gnss_Synchro& a);
void compute_pranges(std::vector<Gnss_Synchro>& data);
void update_TOW(std::vector<Gnss_Synchro>& data);
int save_matfile(); int save_matfile();
//time history
boost::circular_buffer<unsigned long int> d_Rx_clock_buffer;
//Tracking observable history //Tracking observable history
Gnss_circular_deque<Gnss_Synchro>* d_gnss_synchro_history; Gnss_circular_deque<Gnss_Synchro>* d_gnss_synchro_history;
boost::dynamic_bitset<> valid_channels; unsigned int T_rx_clock_step_samples;
double T_rx_s;
unsigned int T_rx_step_ms;
//rx time follow GPST //rx time follow GPST
bool T_rx_TOW_set; bool T_rx_TOW_set;
unsigned int T_rx_TOW_ms; unsigned int T_rx_TOW_ms;
double max_delta; unsigned int T_rx_TOW_offset_ms;
double d_latency;
bool d_dump; bool d_dump;
unsigned int d_nchannels; unsigned int d_nchannels_in;
unsigned int d_num_valid_channels; unsigned int d_nchannels_out;
std::string d_dump_filename; std::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;
}; };

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

@ -117,7 +117,8 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc(
d_flag_frame_sync = false; d_flag_frame_sync = false;
d_flag_parity = false; d_flag_parity = false;
d_TOW_at_current_symbol = 0; d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
delta_t = 0; delta_t = 0;
d_CRC_error_counter = 0; d_CRC_error_counter = 0;
flag_even_word_arrived = 0; flag_even_word_arrived = 0;
@ -251,9 +252,9 @@ void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols, in
DLOG(INFO) << "T0G=" << tmp_obj->t_0G_10; DLOG(INFO) << "T0G=" << tmp_obj->t_0G_10;
DLOG(INFO) << "WN_0G_10=" << tmp_obj->WN_0G_10; DLOG(INFO) << "WN_0G_10=" << tmp_obj->WN_0G_10;
DLOG(INFO) << "Current parameters:"; DLOG(INFO) << "Current parameters:";
DLOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol; DLOG(INFO) << "d_TOW_at_current_symbol_ms=" << d_TOW_at_current_symbol_ms;
DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0; DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0;
delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (d_TOW_at_current_symbol - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64))); delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64)));
DLOG(INFO) << "delta_t=" << delta_t << "[s]"; DLOG(INFO) << "delta_t=" << delta_t << "[s]";
} }
} }
@ -406,6 +407,9 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
d_flag_frame_sync = false; d_flag_frame_sync = false;
d_stat = 0; d_stat = 0;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
d_nav.flag_TOW_set = false;
} }
} }
} }
@ -419,44 +423,48 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
if (d_nav.flag_TOW_5 == true) // page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) if (d_nav.flag_TOW_5 == true) // page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{ {
// TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay // TOW_5 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_current_symbol = d_nav.TOW_5 + static_cast<double>(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast<double>(required_symbols + 1) * GALILEO_E1_CODE_PERIOD; d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.TOW_5 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS;
d_nav.flag_TOW_5 = false; d_nav.flag_TOW_5 = false;
} }
else if (d_nav.flag_TOW_6 == true) // page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) else if (d_nav.flag_TOW_6 == true) // page 6 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec)
{ {
// TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay // TOW_6 refers to the even preamble, but when we decode it we are in the odd part, so 1 second later plus the decoding delay
d_TOW_at_current_symbol = d_nav.TOW_6 + static_cast<double>(GALILEO_INAV_PAGE_PART_SECONDS) + static_cast<double>(required_symbols + 1) * GALILEO_E1_CODE_PERIOD; d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.TOW_6 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS;
d_nav.flag_TOW_6 = false; d_nav.flag_TOW_6 = false;
} }
else else
{ {
// this page has no timing information // this page has no timing information
d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD; d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS; // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD;
} }
} }
else // if there is not a new preamble, we define the TOW of the current symbol else // if there is not a new preamble, we define the TOW of the current symbol
{ {
d_TOW_at_current_symbol += GALILEO_E1_CODE_PERIOD; if (d_nav.flag_TOW_set == true)
{
d_TOW_at_current_symbol_ms += GALILEO_E1_CODE_PERIOD_MS;
}
} }
// if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) // remove used symbols from history
// todo: Use circular buffer here
if (d_symbol_history.size() > required_symbols)
{
d_symbol_history.pop_front();
}
if (d_nav.flag_TOW_set)
{
if (d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived if (d_nav.flag_GGTO_1 == true and d_nav.flag_GGTO_2 == true and d_nav.flag_GGTO_3 == true and d_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
{ {
delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (d_TOW_at_current_symbol - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64.0))); delta_t = d_nav.A_0G_10 + d_nav.A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - d_nav.t_0G_10 + 604800.0 * (fmod((d_nav.WN_0 - d_nav.WN_0G_10), 64.0)));
} }
if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) current_symbol.Flag_valid_word = d_nav.flag_TOW_set;
{ current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_symbol.Flag_valid_word = true;
}
else
{
current_symbol.Flag_valid_word = false;
}
current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
// todo: Galileo to GPS time conversion should be moved to observable block. // todo: Galileo to GPS time conversion should be moved to observable block.
// current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW // current_symbol.TOW_at_current_symbol_ms -= delta_t; //Galileo to GPS TOW
@ -467,11 +475,11 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
{ {
double tmp_double; double tmp_double;
unsigned long int tmp_ulong_int; unsigned long int tmp_ulong_int;
tmp_double = d_TOW_at_current_symbol; tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter; tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int)); d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = 0; tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
} }
catch (const std::ifstream::failure &e) catch (const std::ifstream::failure &e)
@ -479,13 +487,12 @@ int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute
LOG(WARNING) << "Exception writing observables dump file " << e.what(); LOG(WARNING) << "Exception writing observables dump file " << e.what();
} }
} }
// remove used symbols from history
if (d_symbol_history.size() > required_symbols)
{
d_symbol_history.pop_front();
}
// 3. Make the output (copy the object contents to the GNURadio reserved memory) // 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol; *out[0] = current_symbol;
return 1; return 1;
} }
else
{
return 0;
}
}

3
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h
View File

@ -105,7 +105,8 @@ private:
Gnss_Satellite d_satellite; Gnss_Satellite d_satellite;
int d_channel; int d_channel;
double d_TOW_at_current_symbol; unsigned int d_TOW_at_Preamble_ms;
unsigned int d_TOW_at_current_symbol_ms;
bool flag_TOW_set; bool flag_TOW_set;
double delta_t; //GPS-GALILEO time offset double delta_t; //GPS-GALILEO time offset

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

@ -154,7 +154,6 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
// initialize internal vars // initialize internal vars
d_dump = dump; d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN()); d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "GALILEO E5A TELEMETRY PROCESSING: satellite " << d_satellite;
// set the preamble // set the preamble
for (int i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++) for (int i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
@ -182,7 +181,8 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
d_flag_preamble = false; d_flag_preamble = false;
d_preamble_index = 0; d_preamble_index = 0;
d_flag_frame_sync = false; d_flag_frame_sync = false;
d_TOW_at_current_symbol = 0.0; d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
flag_TOW_set = false; flag_TOW_set = false;
d_CRC_error_counter = 0; d_CRC_error_counter = 0;
d_channel = 0; d_channel = 0;
@ -345,7 +345,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
// ****************** Frame sync ****************** // ****************** Frame sync ******************
if ((d_stat == 0) && new_symbol) // no preamble information if ((d_stat == 0) && new_symbol) // no preamble information
{ {
if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS) if (abs(corr_value) == GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
{ {
d_preamble_index = d_sample_counter; // record the preamble sample stamp d_preamble_index = d_sample_counter; // record the preamble sample stamp
LOG(INFO) << "Preamble detection for Galileo E5a satellite " << d_satellite; LOG(INFO) << "Preamble detection for Galileo E5a satellite " << d_satellite;
@ -354,7 +354,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
} }
else if ((d_stat == 1) && new_symbol) // possible preamble lock else if ((d_stat == 1) && new_symbol) // possible preamble lock
{ {
if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS) if (abs(corr_value) == GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
{ {
// check preamble separation // check preamble separation
preamble_diff = d_sample_counter - d_preamble_index; preamble_diff = d_sample_counter - d_preamble_index;
@ -418,6 +418,9 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
d_flag_frame_sync = false; d_flag_frame_sync = false;
d_stat = 0; d_stat = 0;
flag_bit_start = false; flag_bit_start = false;
d_nav.flag_TOW_set = false;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
} }
} }
} }
@ -432,46 +435,52 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
{ {
if (d_nav.flag_TOW_1 == true) if (d_nav.flag_TOW_1 == true)
{ {
d_TOW_at_current_symbol = d_nav.FNAV_TOW_1 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.FNAV_TOW_1 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
d_nav.flag_TOW_1 = false; d_nav.flag_TOW_1 = false;
} }
else if (d_nav.flag_TOW_2 == true) else if (d_nav.flag_TOW_2 == true)
{ {
d_TOW_at_current_symbol = d_nav.FNAV_TOW_2 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.FNAV_TOW_2 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
d_nav.flag_TOW_2 = false; d_nav.flag_TOW_2 = false;
} }
else if (d_nav.flag_TOW_3 == true) else if (d_nav.flag_TOW_3 == true)
{ {
d_TOW_at_current_symbol = d_nav.FNAV_TOW_3 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.FNAV_TOW_3 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
d_nav.flag_TOW_3 = false; d_nav.flag_TOW_3 = false;
} }
else if (d_nav.flag_TOW_4 == true) else if (d_nav.flag_TOW_4 == true)
{ {
d_TOW_at_current_symbol = d_nav.FNAV_TOW_4 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.FNAV_TOW_4 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + (GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS;
d_nav.flag_TOW_4 = false; d_nav.flag_TOW_4 = false;
} }
else else
{ {
d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD; d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS;
} }
} }
else // if there is not a new preamble, we define the TOW of the current symbol else // if there is not a new preamble, we define the TOW of the current symbol
{ {
d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD; if (d_nav.flag_TOW_set == true)
{
d_TOW_at_current_symbol_ms += GALILEO_E5a_CODE_PERIOD_MS;
}
} }
//if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) // remove used symbols from history
if (d_flag_frame_sync and d_nav.flag_TOW_set) // todo: Use circular buffer here
while (d_symbol_history.size() > required_symbols)
{
d_symbol_history.pop_front();
}
if (d_nav.flag_TOW_set)
{ {
current_sample.Flag_valid_word = true; current_sample.Flag_valid_word = true;
} current_sample.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
else
{
current_sample.Flag_valid_word = false;
}
current_sample.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
if (d_dump) if (d_dump)
{ {
// MULTIPLEXED FILE RECORDING - Record results to file // MULTIPLEXED FILE RECORDING - Record results to file
@ -479,11 +488,11 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
{ {
double tmp_double; double tmp_double;
unsigned long int tmp_ulong_int; unsigned long int tmp_ulong_int;
tmp_double = d_TOW_at_current_symbol; tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_sample.Tracking_sample_counter; tmp_ulong_int = current_sample.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int)); d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = 0.0; tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
} }
catch (const std::ifstream::failure &e) catch (const std::ifstream::failure &e)
@ -491,14 +500,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute
LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what(); LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what();
} }
} }
// remove used symbols from history
while (d_symbol_history.size() > required_symbols)
{
d_symbol_history.pop_front();
}
// 3. Make the output // 3. Make the output
if (current_sample.Flag_valid_word)
{
out[0] = current_sample; out[0] = current_sample;
return 1; return 1;
} }

3
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h
View File

@ -104,7 +104,8 @@ private:
bool new_symbol; bool new_symbol;
double d_prompt_acum; double d_prompt_acum;
double page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS]; double page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
double d_TOW_at_current_symbol; unsigned int d_TOW_at_Preamble_ms;
unsigned int d_TOW_at_current_symbol_ms;
double delta_t; //GPS-GALILEO time offset double delta_t; //GPS-GALILEO time offset
std::string d_dump_filename; std::string d_dump_filename;
std::ofstream d_dump_file; std::ofstream d_dump_file;

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

@ -419,6 +419,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
flag_TOW_set = false; flag_TOW_set = false;
d_current_subframe_symbol = 0; d_current_subframe_symbol = 0;
d_crc_error_synchronization_counter = 0; d_crc_error_synchronization_counter = 0;
d_TOW_at_current_symbol_ms = 0;
} }
} }
} }
@ -426,16 +427,21 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
//2. Add the telemetry decoder information //2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_flag_new_tow_available == true) if (this->d_flag_preamble == true and d_flag_new_tow_available == true)
{ {
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_nav.d_TOW) * 1000 + GPS_CA_PREAMBLE_DURATION_MS; d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_nav.d_TOW * 1000.0) + GPS_CA_PREAMBLE_DURATION_MS;
d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms; d_TOW_at_Preamble_ms = static_cast<unsigned int>(d_nav.d_TOW * 1000.0);
flag_TOW_set = true; flag_TOW_set = true;
d_flag_new_tow_available = false; d_flag_new_tow_available = false;
} }
else else
{
if (flag_TOW_set == true)
{ {
d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS; d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS;
} }
}
if (flag_TOW_set == true)
{
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms; current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_symbol.Flag_valid_word = flag_TOW_set; current_symbol.Flag_valid_word = flag_TOW_set;
@ -456,7 +462,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter; tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int)); d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) * 1000.0; tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
} }
catch (const std::ifstream::failure &e) catch (const std::ifstream::failure &e)
@ -470,3 +476,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
return 1; return 1;
} }
else
{
return 0;
}
}

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

@ -64,8 +64,8 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc(
DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite; DLOG(INFO) << "GPS L5 TELEMETRY PROCESSING: satellite " << d_satellite;
d_channel = 0; d_channel = 0;
d_flag_valid_word = false; d_flag_valid_word = false;
d_TOW_at_current_symbol = 0.0; d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble = 0.0; d_TOW_at_Preamble_ms = 0;
//initialize the CNAV frame decoder (libswiftcnav) //initialize the CNAV frame decoder (libswiftcnav)
cnav_msg_decoder_init(&d_cnav_decoder); cnav_msg_decoder_init(&d_cnav_decoder);
for (int aux = 0; aux < GPS_L5i_NH_CODE_LENGTH; aux++) for (int aux = 0; aux < GPS_L5i_NH_CODE_LENGTH; aux++)
@ -236,24 +236,28 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u
} }
//update TOW at the preamble instant //update TOW at the preamble instant
d_TOW_at_Preamble = static_cast<double>(msg.tow) * 6.0; d_TOW_at_Preamble_ms = msg.tow * 6000;
//* The time of the last input symbol can be computed from the message ToW and //* The time of the last input symbol can be computed from the message ToW and
//* delay by the formulae: //* delay by the formulae:
//* \code //* \code
//* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory //* symbolTime_ms = msg->tow * 6000 + *pdelay * 10 + (12 * 10); 12 symbols of the encoder's transitory
d_TOW_at_current_symbol = (static_cast<double>(msg.tow) * 6.0) + (static_cast<double>(delay) + 12.0) * GPS_L5i_SYMBOL_PERIOD; //d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5i_SYMBOL_PERIOD_MS;
d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0;
d_TOW_at_current_symbol_ms = msg.tow * 6000 + (delay + 12) * GPS_L5i_SYMBOL_PERIOD_MS;
d_flag_valid_word = true; d_flag_valid_word = true;
} }
else else
{ {
d_TOW_at_current_symbol += GPS_L5i_PERIOD; d_TOW_at_current_symbol_ms += GPS_L5i_PERIOD_MS;
if (current_synchro_data.Flag_valid_symbol_output == false) if (current_synchro_data.Flag_valid_symbol_output == false)
{ {
d_flag_valid_word = false; d_flag_valid_word = false;
} }
} }
current_synchro_data.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
if (d_flag_valid_word == true)
{
current_synchro_data.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_synchro_data.Flag_valid_word = d_flag_valid_word; current_synchro_data.Flag_valid_word = d_flag_valid_word;
if (d_dump == true) if (d_dump == true)
@ -263,11 +267,11 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u
{ {
double tmp_double; double tmp_double;
unsigned long int tmp_ulong_int; unsigned long int tmp_ulong_int;
tmp_double = d_TOW_at_current_symbol; tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_synchro_data.Tracking_sample_counter; tmp_ulong_int = current_synchro_data.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int)); d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = d_TOW_at_Preamble; tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
} }
catch (const std::ifstream::failure &e) catch (const std::ifstream::failure &e)
@ -280,3 +284,8 @@ int gps_l5_telemetry_decoder_cc::general_work(int noutput_items __attribute__((u
out[0] = current_synchro_data; out[0] = current_synchro_data;
return 1; return 1;
} }
else
{
return 0;
}
}

7
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_cc.h
View File

@ -41,8 +41,7 @@
#include <utility> #include <utility>
#include <vector> #include <vector>
extern "C" extern "C" {
{
#include "cnav_msg.h" #include "cnav_msg.h"
#include "edc.h" #include "edc.h"
#include "bits.h" #include "bits.h"
@ -85,8 +84,8 @@ private:
cnav_msg_decoder_t d_cnav_decoder; cnav_msg_decoder_t d_cnav_decoder;
double d_TOW_at_current_symbol; unsigned int d_TOW_at_current_symbol_ms;
double d_TOW_at_Preamble; unsigned int d_TOW_at_Preamble_ms;
bool d_flag_valid_word; bool d_flag_valid_word;
Gps_CNAV_Navigation_Message d_CNAV_Message; Gps_CNAV_Navigation_Message d_CNAV_Message;

4
src/algorithms/tracking/libs/dll_pll_conf.cc
View File

@ -41,7 +41,9 @@ Dll_Pll_Conf::Dll_Pll_Conf()
vector_length = 0; vector_length = 0;
dump = false; dump = false;
dump_filename = "./dll_pll_dump.dat"; dump_filename = "./dll_pll_dump.dat";
pll_bw_hz = 40.0; pll_pull_in_bw_hz = 50.0;
dll_pull_in_bw_hz = 3.0;
pll_bw_hz = 35.0;
dll_bw_hz = 2.0; dll_bw_hz = 2.0;
pll_bw_narrow_hz = 5.0; pll_bw_narrow_hz = 5.0;
dll_bw_narrow_hz = 0.75; dll_bw_narrow_hz = 0.75;

2
src/algorithms/tracking/libs/dll_pll_conf.h
View File

@ -44,6 +44,8 @@ public:
unsigned int vector_length; unsigned int vector_length;
bool dump; bool dump;
std::string dump_filename; std::string dump_filename;
float pll_pull_in_bw_hz;
float dll_pull_in_bw_hz;
float pll_bw_hz; float pll_bw_hz;
float dll_bw_hz; float dll_bw_hz;
float pll_bw_narrow_hz; float pll_bw_narrow_hz;

12
src/core/receiver/gnss_flowgraph.cc
View File

@ -277,7 +277,8 @@ void GNSSFlowgraph::connect()
std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl; std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl;
throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration")); throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration"));
} }
ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0)); int observable_interval_ms = static_cast<double>(configuration_->property("GNSS-SDR.observable_interval_ms", 20));
ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, observable_interval_ms, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0));
top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0);
top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse
} }
@ -296,8 +297,9 @@ void GNSSFlowgraph::connect()
{ {
//null source //null source
null_source_ = gr::blocks::null_source::make(sizeof(Gnss_Synchro)); null_source_ = gr::blocks::null_source::make(sizeof(Gnss_Synchro));
//throttle 1kHz //throttle to observable interval
throttle_ = gr::blocks::throttle::make(sizeof(Gnss_Synchro), 1000); // 1000 samples per second (1kHz) int observable_interval_ms = static_cast<double>(configuration_->property("GNSS-SDR.observable_interval_ms", 20));
throttle_ = gr::blocks::throttle::make(sizeof(Gnss_Synchro), std::round(1.0 / static_cast<double>(observable_interval_ms))); // 1000 samples per second (1kHz)
time_counter_ = gnss_sdr_make_time_counter(); time_counter_ = gnss_sdr_make_time_counter();
top_block_->connect(null_source_, 0, throttle_, 0); top_block_->connect(null_source_, 0, throttle_, 0);
top_block_->connect(throttle_, 0, time_counter_, 0); top_block_->connect(throttle_, 0, time_counter_, 0);
@ -323,7 +325,9 @@ void GNSSFlowgraph::connect()
std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl; std::cout << "Set GNSS-SDR.internal_fs_sps in configuration file" << std::endl;
throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration")); throw(std::invalid_argument("Set GNSS-SDR.internal_fs_sps in configuration"));
} }
ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0));
int observable_interval_ms = static_cast<double>(configuration_->property("GNSS-SDR.observable_interval_ms", 20));
ch_out_sample_counter = gnss_sdr_make_sample_counter(fs, observable_interval_ms, sig_conditioner_.at(0)->get_right_block()->output_signature()->sizeof_stream_item(0));
top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0); top_block_->connect(sig_conditioner_.at(0)->get_right_block(), 0, ch_out_sample_counter, 0);
top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse top_block_->connect(ch_out_sample_counter, 0, observables_->get_left_block(), channels_count_); //extra port for the sample counter pulse
} }

2
src/core/system_parameters/GPS_L1_CA.h
View File

@ -68,7 +68,7 @@ const double MAX_TOA_DELAY_MS = 20;
//#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here //#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here
//const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here) //const double GPS_STARTOFFSET_ms = 68.802; //[ms] Initial sign. travel time (this cannot go here)
const double GPS_STARTOFFSET_ms = 69.0; const double GPS_STARTOFFSET_ms = 60.0;
// OBSERVABLE HISTORY DEEP FOR INTERPOLATION // OBSERVABLE HISTORY DEEP FOR INTERPOLATION
const int GPS_L1_CA_HISTORY_DEEP = 100; const int GPS_L1_CA_HISTORY_DEEP = 100;

2
src/core/system_parameters/GPS_L5.h
View File

@ -55,7 +55,9 @@ const double GPS_L5_FREQ_HZ = FREQ5; //!< L5 [Hz]
const double GPS_L5i_CODE_RATE_HZ = 10.23e6; //!< GPS L5i code rate [chips/s] const double GPS_L5i_CODE_RATE_HZ = 10.23e6; //!< GPS L5i code rate [chips/s]
const int GPS_L5i_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips] const int GPS_L5i_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips]
const double GPS_L5i_PERIOD = 0.001; //!< GPS L5 code period [seconds] const double GPS_L5i_PERIOD = 0.001; //!< GPS L5 code period [seconds]
const int GPS_L5i_PERIOD_MS = 1; //!< GPS L5 code period [ms]
const double GPS_L5i_SYMBOL_PERIOD = 0.01; //!< GPS L5 symbol period [seconds] const double GPS_L5i_SYMBOL_PERIOD = 0.01; //!< GPS L5 symbol period [seconds]
const int GPS_L5i_SYMBOL_PERIOD_MS = 10; //!< GPS L5 symbol period [ms]
const double GPS_L5q_CODE_RATE_HZ = 10.23e6; //!< GPS L5i code rate [chips/s] const double GPS_L5q_CODE_RATE_HZ = 10.23e6; //!< GPS L5i code rate [chips/s]
const int GPS_L5q_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips] const int GPS_L5q_CODE_LENGTH_CHIPS = 10230; //!< GPS L5i code length [chips]

2
src/core/system_parameters/Galileo_E1.h
View File

@ -80,6 +80,7 @@ const int GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS = 250;
const int GALILEO_INAV_PAGE_PART_SYMBOLS = 250; //!< Each Galileo INAV pages are composed of two parts (even and odd) each of 250 symbols, including preamble. See Galileo ICD 4.3.2 const int GALILEO_INAV_PAGE_PART_SYMBOLS = 250; //!< Each Galileo INAV pages are composed of two parts (even and odd) each of 250 symbols, including preamble. See Galileo ICD 4.3.2
const int GALILEO_INAV_PAGE_SYMBOLS = 500; //!< The complete Galileo INAV page length const int GALILEO_INAV_PAGE_SYMBOLS = 500; //!< The complete Galileo INAV page length
const int GALILEO_INAV_PAGE_PART_SECONDS = 1; // a page part last 1 sec const int GALILEO_INAV_PAGE_PART_SECONDS = 1; // a page part last 1 sec
const int GALILEO_INAV_PAGE_PART_MS = 1000; // a page part last 1 sec
const int GALILEO_INAV_PAGE_SECONDS = 2; // a full page last 2 sec const int GALILEO_INAV_PAGE_SECONDS = 2; // a full page last 2 sec
const int GALILEO_INAV_INTERLEAVER_ROWS = 8; const int GALILEO_INAV_INTERLEAVER_ROWS = 8;
const int GALILEO_INAV_INTERLEAVER_COLS = 30; const int GALILEO_INAV_INTERLEAVER_COLS = 30;
@ -89,6 +90,7 @@ const int GALILEO_DATA_JK_BITS = 128;
const int GALILEO_DATA_FRAME_BITS = 196; const int GALILEO_DATA_FRAME_BITS = 196;
const int GALILEO_DATA_FRAME_BYTES = 25; const int GALILEO_DATA_FRAME_BYTES = 25;
const double GALILEO_E1_CODE_PERIOD = 0.004; const double GALILEO_E1_CODE_PERIOD = 0.004;
const int GALILEO_E1_CODE_PERIOD_MS = 4;
const std::vector<std::pair<int, int>> type({{1, 6}}); const std::vector<std::pair<int, int>> type({{1, 6}});
const std::vector<std::pair<int, int>> PAGE_TYPE_bit({{1, 6}}); const std::vector<std::pair<int, int>> PAGE_TYPE_bit({{1, 6}});

5
src/tests/CMakeLists.txt
View File

@ -123,6 +123,7 @@ if(ENABLE_CUDA)
set(GNSS_SDR_TEST_OPTIONAL_LIBS ${GNSS_SDR_TEST_OPTIONAL_LIBS} ${CUDA_LIBRARIES}) set(GNSS_SDR_TEST_OPTIONAL_LIBS ${GNSS_SDR_TEST_OPTIONAL_LIBS} ${CUDA_LIBRARIES})
endif(ENABLE_CUDA) endif(ENABLE_CUDA)
if(ENABLE_GPERFTOOLS) if(ENABLE_GPERFTOOLS)
if(GPERFTOOLS_FOUND) if(GPERFTOOLS_FOUND)
set(GNSS_SDR_TEST_OPTIONAL_LIBS "${GNSS_SDR_TEST_OPTIONAL_LIBS};${GPERFTOOLS_LIBRARIES}") set(GNSS_SDR_TEST_OPTIONAL_LIBS "${GNSS_SDR_TEST_OPTIONAL_LIBS};${GPERFTOOLS_LIBRARIES}")
@ -257,6 +258,10 @@ if(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
endif(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA) endif(ENABLE_UNIT_TESTING_EXTRA OR ENABLE_SYSTEM_TESTING_EXTRA OR ENABLE_FPGA)
if (ENABLE_UNIT_TESTING_EXTRA)
set(GNSS_SDR_TEST_OPTIONAL_LIBS ${GNSS_SDR_TEST_OPTIONAL_LIBS} ${gpstk_libs})
endif (ENABLE_UNIT_TESTING_EXTRA)
if(ENABLE_UNIT_TESTING_EXTRA) if(ENABLE_UNIT_TESTING_EXTRA)
add_definitions(-DEXTRA_TESTS) add_definitions(-DEXTRA_TESTS)
if(NOT EXISTS ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples/gps_l2c_m_prn7_5msps.dat) if(NOT EXISTS ${CMAKE_SOURCE_DIR}/thirdparty/signal_samples/gps_l2c_m_prn7_5msps.dat)

1
src/tests/common-files/observable_tests_flags.h
View File

@ -35,6 +35,7 @@
#include <limits> #include <limits>
DEFINE_double(skip_obs_transitory_s, 30.0, "Skip the initial observable outputs to avoid transitory results [s]"); DEFINE_double(skip_obs_transitory_s, 30.0, "Skip the initial observable outputs to avoid transitory results [s]");
DEFINE_bool(compute_single_diffs, false, "Compute also the signel difference errors for Accumulated Carrier Phase and Carrier Doppler (requires LO synchronization between receivers)");
#endif #endif

8
src/tests/common-files/tracking_tests_flags.h
View File

@ -48,12 +48,12 @@ DEFINE_double(CN0_dBHz_start, std::numeric_limits<double>::infinity(), "Enable n
DEFINE_double(CN0_dBHz_stop, std::numeric_limits<double>::infinity(), "Enable noise generator and set the CN0 stop sweep value [dB-Hz]"); DEFINE_double(CN0_dBHz_stop, std::numeric_limits<double>::infinity(), "Enable noise generator and set the CN0 stop sweep value [dB-Hz]");
DEFINE_double(CN0_dB_step, 3.0, "Noise generator CN0 sweep step value [dB]"); DEFINE_double(CN0_dB_step, 3.0, "Noise generator CN0 sweep step value [dB]");
DEFINE_double(PLL_bw_hz_start, 40.0, "PLL Wide configuration start sweep value [Hz]"); DEFINE_double(PLL_bw_hz_start, 20.0, "PLL Wide configuration start sweep value [Hz]");
DEFINE_double(PLL_bw_hz_stop, 40.0, "PLL Wide configuration stop sweep value [Hz]"); DEFINE_double(PLL_bw_hz_stop, 20.0, "PLL Wide configuration stop sweep value [Hz]");
DEFINE_double(PLL_bw_hz_step, 5.0, "PLL Wide configuration sweep step value [Hz]"); DEFINE_double(PLL_bw_hz_step, 5.0, "PLL Wide configuration sweep step value [Hz]");
DEFINE_double(DLL_bw_hz_start, 1.5, "DLL Wide configuration start sweep value [Hz]"); DEFINE_double(DLL_bw_hz_start, 1.0, "DLL Wide configuration start sweep value [Hz]");
DEFINE_double(DLL_bw_hz_stop, 1.5, "DLL Wide configuration stop sweep value [Hz]"); DEFINE_double(DLL_bw_hz_stop, 1.0, "DLL Wide configuration stop sweep value [Hz]");
DEFINE_double(DLL_bw_hz_step, 0.25, "DLL Wide configuration sweep step value [Hz]"); DEFINE_double(DLL_bw_hz_step, 0.25, "DLL Wide configuration sweep step value [Hz]");
DEFINE_double(PLL_narrow_bw_hz, 5.0, "PLL Narrow configuration value [Hz]"); DEFINE_double(PLL_narrow_bw_hz, 5.0, "PLL Narrow configuration value [Hz]");

961
src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc
View File

File diff suppressed because it is too large Load Diff

28
src/tests/unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc
View File

@ -411,7 +411,8 @@ bool TrackingPullInTest::acquire_signal(int SV_ID)
tmp_gnss_synchro.PRN = SV_ID; tmp_gnss_synchro.PRN = SV_ID;
System_and_Signal = "GPS L1 CA"; System_and_Signal = "GPS L1 CA";
config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells)); config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells));
acquisition = std::make_shared<GpsL1CaPcpsAcquisitionFineDoppler>(config.get(), "Acquisition", 1, 0); //acquisition = std::make_shared<GpsL1CaPcpsAcquisitionFineDoppler>(config.get(), "Acquisition", 1, 0);
acquisition = std::make_shared<GpsL1CaPcpsAcquisition>(config.get(), "Acquisition", 1, 0);
} }
else if (implementation.compare("Galileo_E1_DLL_PLL_VEML_Tracking") == 0) else if (implementation.compare("Galileo_E1_DLL_PLL_VEML_Tracking") == 0)
{ {
@ -809,6 +810,7 @@ TEST_F(TrackingPullInTest, ValidationOfResults)
std::vector<double> promptI; std::vector<double> promptI;
std::vector<double> promptQ; std::vector<double> promptQ;
std::vector<double> CN0_dBHz; std::vector<double> CN0_dBHz;
std::vector<double> Doppler;
long int epoch_counter = 0; long int epoch_counter = 0;
while (trk_dump.read_binary_obs()) while (trk_dump.read_binary_obs())
{ {
@ -828,7 +830,7 @@ TEST_F(TrackingPullInTest, ValidationOfResults)
promptI.push_back(trk_dump.prompt_I); promptI.push_back(trk_dump.prompt_I);
promptQ.push_back(trk_dump.prompt_Q); promptQ.push_back(trk_dump.prompt_Q);
CN0_dBHz.push_back(trk_dump.CN0_SNV_dB_Hz); CN0_dBHz.push_back(trk_dump.CN0_SNV_dB_Hz);
Doppler.push_back(trk_dump.carrier_doppler_hz);
epoch_counter++; epoch_counter++;
} }
@ -917,6 +919,28 @@ TEST_F(TrackingPullInTest, ValidationOfResults)
g3.savetops("CN0_output"); g3.savetops("CN0_output");
g3.showonscreen(); // window output g3.showonscreen(); // window output
Gnuplot g4("linespoints");
if (!FLAGS_enable_external_signal_file)
{
g4.set_title(std::to_string(generator_CN0_values.at(current_cn0_idx)) + " dB-Hz, GPS L1 C/A tracking CN0 output (PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")");
}
else
{
g4.set_title("D_e=" + std::to_string(acq_doppler_error_hz_values.at(current_acq_doppler_error_idx)) + " [Hz] " + "T_e= " + std::to_string(acq_delay_error_chips_values.at(current_acq_doppler_error_idx).at(current_acq_code_error_idx)) + " [Chips] PLL/DLL BW: " + std::to_string(FLAGS_PLL_bw_hz_start) + "," + std::to_string(FLAGS_DLL_bw_hz_start) + " [Hz], (PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")");
}
g4.set_grid();
g4.set_xlabel("Time [s]");
g4.set_ylabel("Estimated Doppler [Hz]");
g4.cmd("set key box opaque");
g4.plot_xy(trk_timestamp_s, Doppler,
std::to_string(static_cast<int>(round(generator_CN0_values.at(current_cn0_idx)))) + "[dB-Hz]", decimate);
g4.set_legend();
g4.savetops("Doppler");
g4.showonscreen(); // window output
} }
} }
catch (const GnuplotException& ge) catch (const GnuplotException& ge)