mirror of https://github.com/gnss-sdr/gnss-sdr
Remove set_max_noutput_items
This commit is contained in:
Antonio Ramos
parent
e964bf060f
commit
eed6ed1f5e
|
|
@ -39,6 +39,7 @@
|
|||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <limits>
|
||||
#include "display.h"
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
|
|
@ -55,7 +56,6 @@ 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);
|
||||
d_dump = dump;
|
||||
d_nchannels = nchannels_out;
|
||||
d_dump_filename = dump_filename;
|
||||
|
|
@ -78,7 +78,6 @@ hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels_in, unsigned
|
|||
try
|
||||
{
|
||||
d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit );
|
||||
d_dump_filename.append(".bin");
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
|
|
@ -224,6 +223,7 @@ int hybrid_observables_cc::save_matfile()
|
|||
mat_t *matfp;
|
||||
matvar_t *matvar;
|
||||
std::string filename = d_dump_filename;
|
||||
if(filename.size() > 4) { filename.erase(filename.end() - 4, filename.end()); }
|
||||
filename.append(".mat");
|
||||
matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
|
||||
if(reinterpret_cast<long*>(matfp) != NULL)
|
||||
|
|
@ -288,40 +288,36 @@ int hybrid_observables_cc::save_matfile()
|
|||
return 0;
|
||||
}
|
||||
|
||||
double hybrid_observables_cc::interpolate_data(const std::pair<Gnss_Synchro, Gnss_Synchro>& a, const double& ti, int parameter)
|
||||
bool hybrid_observables_cc::interpolate_data(Gnss_Synchro& out, std::deque<Gnss_Synchro>& data, const double& ti)
|
||||
{
|
||||
// x(ti) = m * ti + c
|
||||
// m = [x(t2) - x(t1)] / [t2 - t1]
|
||||
// c = x(t1) - m * t1
|
||||
std::deque<Gnss_Synchro>::iterator it;
|
||||
|
||||
double m = 0.0;
|
||||
double c = 0.0;
|
||||
arma::vec t = arma::vec(data.size());
|
||||
arma::vec dop = t;
|
||||
arma::vec cph = t;
|
||||
arma::vec tow = t;
|
||||
arma::vec tiv = arma::vec(1);
|
||||
arma::vec result;
|
||||
tiv(0) = ti;
|
||||
|
||||
if(!a.first.Flag_valid_word or !a.second.Flag_valid_word) { return 0.0; }
|
||||
|
||||
switch(parameter)
|
||||
unsigned int aux = 0;
|
||||
for(it = data.begin(); it != data.end(); it++)
|
||||
{
|
||||
case 0:// Doppler
|
||||
m = (a.first.Carrier_Doppler_hz - a.second.Carrier_Doppler_hz) / (a.first.RX_time - a.second.RX_time);
|
||||
c = a.second.Carrier_Doppler_hz - m * a.second.RX_time;
|
||||
break;
|
||||
t(aux) = it->RX_time;
|
||||
dop(aux) = it->Carrier_Doppler_hz;
|
||||
cph(aux) = it->Carrier_phase_rads;
|
||||
tow(aux) = it->TOW_at_current_symbol_s;
|
||||
|
||||
case 1:// Carrier phase
|
||||
m = (a.first.Carrier_phase_rads - a.second.Carrier_phase_rads) / (a.first.RX_time - a.second.RX_time);
|
||||
c = a.second.Carrier_phase_rads - m * a.second.RX_time;
|
||||
break;
|
||||
|
||||
case 2:// TOW
|
||||
m = (a.first.TOW_at_current_symbol_s - a.second.TOW_at_current_symbol_s) / (a.first.RX_time - a.second.RX_time);
|
||||
c = a.second.TOW_at_current_symbol_s - m * a.second.RX_time;
|
||||
break;
|
||||
|
||||
case 3:// Code phase samples
|
||||
m = (a.first.Code_phase_samples - a.second.Code_phase_samples) / (a.first.RX_time - a.second.RX_time);
|
||||
c = a.second.Code_phase_samples - m * a.second.RX_time;
|
||||
break;
|
||||
aux++;
|
||||
}
|
||||
return(m * ti + c);
|
||||
arma::interp1(t, dop, tiv, result);
|
||||
out.Carrier_Doppler_hz = result(0);
|
||||
arma::interp1(t, cph, tiv, result);
|
||||
out.Carrier_phase_rads = result(0);
|
||||
arma::interp1(t, tow, tiv, result);
|
||||
out.TOW_at_current_symbol_s = result(0);
|
||||
|
||||
return result.is_finite();
|
||||
}
|
||||
|
||||
double hybrid_observables_cc::compute_T_rx_s(const Gnss_Synchro& a)
|
||||
|
|
@ -350,49 +346,6 @@ void hybrid_observables_cc::clean_history(std::deque<Gnss_Synchro>& data)
|
|||
}
|
||||
}
|
||||
|
||||
std::pair<Gnss_Synchro, Gnss_Synchro> hybrid_observables_cc::find_closest(std::deque<Gnss_Synchro>& data, const double& ti)
|
||||
{
|
||||
std::pair<Gnss_Synchro, Gnss_Synchro> result;
|
||||
unsigned int index = 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 = std::fabs(ti - it->RX_time);
|
||||
if(instant_delta < delta_t)
|
||||
{
|
||||
delta_t = instant_delta;
|
||||
index = aux;
|
||||
}
|
||||
aux++;
|
||||
}
|
||||
delta_t = ti - data.at(index).RX_time;
|
||||
if( (index == 0) or (index == (data.size() - 1)) )
|
||||
{
|
||||
Gnss_Synchro invalid_data;
|
||||
invalid_data.Flag_valid_pseudorange = false;
|
||||
result.first = invalid_data;
|
||||
result.second = invalid_data;
|
||||
}
|
||||
else if(delta_t < 0.0)
|
||||
{
|
||||
result.first = data.at(index);
|
||||
result.first.Flag_valid_pseudorange = true;
|
||||
result.second = data.at(index - 1);
|
||||
result.second.Flag_valid_pseudorange = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
result.first = data.at(index + 1);
|
||||
result.first.Flag_valid_pseudorange = true;
|
||||
result.second = data.at(index);
|
||||
result.second.Flag_valid_pseudorange = true;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data)
|
||||
{
|
||||
std::vector<Gnss_Synchro>::iterator it;
|
||||
|
|
@ -417,6 +370,10 @@ void hybrid_observables_cc::correct_TOW_and_compute_prange(std::vector<Gnss_Sync
|
|||
<< ". 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -514,17 +471,15 @@ int hybrid_observables_cc::general_work(int noutput_items __attribute__((unused)
|
|||
{
|
||||
if(valid_channels[i])
|
||||
{
|
||||
std::pair<Gnss_Synchro, Gnss_Synchro> gnss_pair = find_closest(*it, T_rx_s_out);
|
||||
Gnss_Synchro interpolated_gnss_synchro = gnss_pair.second;
|
||||
if(interpolated_gnss_synchro.Flag_valid_pseudorange)
|
||||
Gnss_Synchro interpolated_gnss_synchro;
|
||||
if(interpolate_data(interpolated_gnss_synchro, *it, T_rx_s_out))
|
||||
{
|
||||
interpolated_gnss_synchro.Carrier_Doppler_hz = interpolate_data(gnss_pair, T_rx_s_out, 0);
|
||||
interpolated_gnss_synchro.Carrier_phase_rads = interpolate_data(gnss_pair, T_rx_s_out, 1);
|
||||
interpolated_gnss_synchro.TOW_at_current_symbol_s = interpolate_data(gnss_pair, T_rx_s_out, 2);
|
||||
|
||||
epoch_data.push_back(interpolated_gnss_synchro);
|
||||
}
|
||||
else { valid_channels[i] = false; }
|
||||
else
|
||||
{
|
||||
valid_channels[i] = false;
|
||||
}
|
||||
}
|
||||
i++;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,7 +38,6 @@
|
|||
#include <gnuradio/block.h>
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <utility> //std::pair
|
||||
#include <vector> //std::vector
|
||||
#include <deque>
|
||||
#include <boost/dynamic_bitset.hpp>
|
||||
|
|
@ -67,8 +66,7 @@ private:
|
|||
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);
|
||||
std::pair<Gnss_Synchro, Gnss_Synchro> find_closest(std::deque<Gnss_Synchro>& data, const double& ti);
|
||||
bool interpolate_data(Gnss_Synchro& out, std::deque<Gnss_Synchro>& data, const double& ti);
|
||||
void correct_TOW_and_compute_prange(std::vector<Gnss_Synchro>& data);
|
||||
int save_matfile();
|
||||
|
||||
|
|
|
|||
|
|
@ -107,7 +107,6 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc(
|
|||
gr::block("galileo_e1b_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry Bit transition synchronization port out
|
||||
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
|
||||
// Ephemeris data port out
|
||||
|
|
|
|||
|
|
@ -183,7 +183,6 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
|
|||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(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_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
|
|||
gr::block("gps_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry Bit transition synchronization port out
|
||||
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
|
||||
// Ephemeris data port out
|
||||
|
|
@ -95,8 +94,8 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
|
|||
d_GPS_frame_4bytes = 0;
|
||||
d_prev_GPS_frame_4bytes = 0;
|
||||
d_flag_parity = false;
|
||||
d_TOW_at_Preamble = 0;
|
||||
d_TOW_at_current_symbol = 0;
|
||||
d_TOW_at_Preamble = 0.0;
|
||||
d_TOW_at_current_symbol = 0.0;
|
||||
flag_TOW_set = false;
|
||||
d_average_count = 0;
|
||||
d_flag_preamble = false;
|
||||
|
|
@ -106,6 +105,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
|
|||
d_channel = 0;
|
||||
flag_PLL_180_deg_phase_locked = false;
|
||||
d_preamble_time_samples = 0;
|
||||
d_TOW_at_current_symbol_ms = 0;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -206,7 +206,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_
|
|||
else if (d_stat == 1) //check 6 seconds of preamble separation
|
||||
{
|
||||
preamble_diff_ms = round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
|
||||
if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1)
|
||||
if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) == 0)
|
||||
{
|
||||
DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite;
|
||||
d_GPS_FSM.Event_gps_word_preamble();
|
||||
|
|
@ -351,17 +351,21 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_
|
|||
// /(double)current_symbol.fs;
|
||||
// update TOW at the preamble instant (account with decoder latency)
|
||||
|
||||
d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S;
|
||||
d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0;
|
||||
d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 1.0 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S;
|
||||
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_GPS_FSM.d_nav.d_TOW) * 1000 + 161;
|
||||
//d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0;
|
||||
d_TOW_at_current_symbol = d_TOW_at_Preamble;
|
||||
flag_TOW_set = true;
|
||||
d_flag_new_tow_available = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD;
|
||||
d_TOW_at_current_symbol += GPS_L1_CA_CODE_PERIOD;
|
||||
d_TOW_at_current_symbol_ms++;
|
||||
}
|
||||
|
||||
current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
|
||||
current_symbol.TOW_at_current_symbol_s = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
|
||||
//current_symbol.TOW_at_current_symbol_s = d_TOW_at_current_symbol;
|
||||
current_symbol.Flag_valid_word = flag_TOW_set;
|
||||
|
||||
if (flag_PLL_180_deg_phase_locked == true)
|
||||
|
|
|
|||
|
|
@ -107,8 +107,9 @@ private:
|
|||
|
||||
unsigned long int d_preamble_time_samples;
|
||||
|
||||
long double d_TOW_at_Preamble;
|
||||
long double d_TOW_at_current_symbol;
|
||||
double d_TOW_at_Preamble;
|
||||
double d_TOW_at_current_symbol;
|
||||
unsigned int d_TOW_at_current_symbol_ms;
|
||||
|
||||
bool flag_TOW_set;
|
||||
bool flag_PLL_180_deg_phase_locked;
|
||||
|
|
|
|||
|
|
@ -32,6 +32,7 @@
|
|||
|
||||
#include "gps_l2c_telemetry_decoder_cc.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "display.h"
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <glog/logging.h>
|
||||
#include <gnuradio/io_signature.h>
|
||||
|
|
@ -54,7 +55,6 @@ gps_l2c_telemetry_decoder_cc::gps_l2c_telemetry_decoder_cc(
|
|||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry Bit transition synchronization port out
|
||||
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
|
||||
// Ephemeris data port out
|
||||
|
|
@ -133,21 +133,21 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__(
|
|||
{
|
||||
// get ephemeris object for this SV
|
||||
std::shared_ptr<Gps_CNAV_Ephemeris> tmp_obj = std::make_shared<Gps_CNAV_Ephemeris>(d_CNAV_Message.get_ephemeris());
|
||||
std::cout << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << std::endl;
|
||||
std::cout << TEXT_BLUE << "New GPS CNAV message received: ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl;
|
||||
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
|
||||
|
||||
}
|
||||
if (d_CNAV_Message.have_new_iono() == true)
|
||||
{
|
||||
std::shared_ptr<Gps_CNAV_Iono> tmp_obj = std::make_shared<Gps_CNAV_Iono>(d_CNAV_Message.get_iono());
|
||||
std::cout << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << std::endl;
|
||||
std::cout << TEXT_BLUE << "New GPS CNAV message received: iono model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
|
||||
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
|
||||
}
|
||||
|
||||
if (d_CNAV_Message.have_new_utc_model() == true)
|
||||
{
|
||||
std::shared_ptr<Gps_CNAV_Utc_Model> tmp_obj = std::make_shared<Gps_CNAV_Utc_Model>(d_CNAV_Message.get_utc_model());
|
||||
std::cout << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << std::endl;
|
||||
std::cout << TEXT_BLUE << "New GPS CNAV message received: UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
|
||||
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -56,7 +56,6 @@ gps_l5_telemetry_decoder_cc::gps_l5_telemetry_decoder_cc(
|
|||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry Bit transition synchronization port out
|
||||
this->message_port_register_out(pmt::mp("preamble_timestamp_s"));
|
||||
// Ephemeris data port out
|
||||
|
|
|
|||
|
|
@ -118,7 +118,6 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc(
|
|||
gr::block("galileo_e1_dll_pll_veml_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry bit synchronization message port input
|
||||
this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
|
||||
this->set_relative_rate(1.0 / vector_length);
|
||||
|
|
|
|||
|
|
@ -98,7 +98,6 @@ Galileo_E5a_Dll_Pll_Tracking_cc::Galileo_E5a_Dll_Pll_Tracking_cc(
|
|||
gr::block("Galileo_E5a_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry bit synchronization message port input
|
||||
this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
|
||||
this->message_port_register_out(pmt::mp("events"));
|
||||
|
|
|
|||
|
|
@ -94,7 +94,6 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc(
|
|||
gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry bit synchronization message port input
|
||||
this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
|
||||
this->message_port_register_out(pmt::mp("events"));
|
||||
|
|
|
|||
|
|
@ -91,7 +91,6 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc(
|
|||
gr::block("gps_l2_m_dll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry bit synchronization message port input
|
||||
this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
|
||||
this->message_port_register_out(pmt::mp("events"));
|
||||
|
|
|
|||
|
|
@ -92,7 +92,6 @@ gps_l5i_dll_pll_tracking_cc::gps_l5i_dll_pll_tracking_cc(
|
|||
gr::block("gps_l5i_dll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
set_max_noutput_items(1);
|
||||
// Telemetry bit synchronization message port input
|
||||
this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
|
||||
this->message_port_register_out(pmt::mp("events"));
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ m * \file gps_navigation_message.cc
|
|||
void Gps_Navigation_Message::reset()
|
||||
{
|
||||
b_valid_ephemeris_set_flag = false;
|
||||
d_TOW = 0;
|
||||
d_TOW = 0.0;
|
||||
d_TOW_SF1 = 0;
|
||||
d_TOW_SF2 = 0;
|
||||
d_TOW_SF3 = 0;
|
||||
|
|
@ -70,7 +70,7 @@ void Gps_Navigation_Message::reset()
|
|||
i_SV_accuracy = 0;
|
||||
i_SV_health = 0;
|
||||
d_TGD = 0;
|
||||
d_IODC = -1;
|
||||
d_IODC = -1.0;
|
||||
i_AODO = 0;
|
||||
|
||||
b_fit_interval_flag = false;
|
||||
|
|
@ -298,7 +298,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
|
|||
//TOW = bin2dec(subframe(31:47)) * 6;
|
||||
d_TOW_SF1 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
//we are in the first subframe (the transmitted TOW is the start time of the next subframe) !
|
||||
d_TOW_SF1 = d_TOW_SF1 * 6;
|
||||
d_TOW_SF1 = d_TOW_SF1 * 6.0;
|
||||
d_TOW = d_TOW_SF1; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
|
|
@ -324,7 +324,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
|
|||
|
||||
case 2: //--- It is subframe 2 -------------------
|
||||
d_TOW_SF2 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
d_TOW_SF2 = d_TOW_SF2 * 6;
|
||||
d_TOW_SF2 = d_TOW_SF2 * 6.0;
|
||||
d_TOW = d_TOW_SF2; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
|
|
@ -354,7 +354,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
|
|||
|
||||
case 3: // --- It is subframe 3 -------------------------------------
|
||||
d_TOW_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
d_TOW_SF3 = d_TOW_SF3 * 6;
|
||||
d_TOW_SF3 = d_TOW_SF3 * 6.0;
|
||||
d_TOW = d_TOW_SF3; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
|
|
@ -383,7 +383,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
|
|||
int SV_data_ID;
|
||||
int SV_page;
|
||||
d_TOW_SF4 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
d_TOW_SF4 = d_TOW_SF4 * 6;
|
||||
d_TOW_SF4 = d_TOW_SF4 * 6.0;
|
||||
d_TOW = d_TOW_SF4; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
|
|
@ -459,7 +459,7 @@ int Gps_Navigation_Message::subframe_decoder(char *subframe)
|
|||
int SV_data_ID_5;
|
||||
int SV_page_5;
|
||||
d_TOW_SF5 = static_cast<double>(read_navigation_unsigned(subframe_bits, TOW));
|
||||
d_TOW_SF5 = d_TOW_SF5 * 6;
|
||||
d_TOW_SF5 = d_TOW_SF5 * 6.0;
|
||||
d_TOW = d_TOW_SF5; // Set transmission time
|
||||
b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG);
|
||||
b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG);
|
||||
|
|
@ -679,9 +679,9 @@ bool Gps_Navigation_Message::satellite_validation()
|
|||
// First Step:
|
||||
// check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception
|
||||
// and check if the data have been filled (!=0)
|
||||
if (d_TOW_SF1 != 0 and d_TOW_SF2 != 0 and d_TOW_SF3 != 0)
|
||||
if (d_TOW_SF1 != 0.0 and d_TOW_SF2 != 0.0 and d_TOW_SF3 != 0.0)
|
||||
{
|
||||
if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!= -1)
|
||||
if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC != -1.0)
|
||||
{
|
||||
flag_data_valid = true;
|
||||
b_valid_ephemeris_set_flag = true;
|
||||
|
|
|
|||
|
|
@ -36,11 +36,8 @@
|
|||
#include <armadillo>
|
||||
#include <gnuradio/top_block.h>
|
||||
#include <gnuradio/blocks/file_source.h>
|
||||
#include <gnuradio/analog/sig_source_waveform.h>
|
||||
#include <gnuradio/analog/sig_source_c.h>
|
||||
#include <gnuradio/blocks/interleaved_char_to_complex.h>
|
||||
#include <gnuradio/blocks/null_sink.h>
|
||||
#include <gnuradio/blocks/skiphead.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include "GPS_L1_CA.h"
|
||||
#include "gnss_satellite.h"
|
||||
|
|
@ -57,9 +54,10 @@
|
|||
#include "observables_dump_reader.h"
|
||||
#include "tlm_dump_reader.h"
|
||||
#include "gps_l1_ca_dll_pll_tracking.h"
|
||||
#include "gps_l1_ca_dll_pll_c_aid_tracking.h"
|
||||
#include "hybrid_observables.h"
|
||||
#include "signal_generator_flags.h"
|
||||
#include "gnss_sdr_sample_counter.h"
|
||||
#include <matio.h>
|
||||
|
||||
|
||||
// ######## GNURADIO BLOCK MESSAGE RECEVER FOR TRACKING MESSAGES #########
|
||||
|
|
@ -189,18 +187,17 @@ public:
|
|||
int configure_generator();
|
||||
int generate_signal();
|
||||
void check_results_carrier_phase(
|
||||
arma::vec & true_ch0_phase_cycles,
|
||||
arma::vec & true_ch1_phase_cycles,
|
||||
arma::vec & true_ch0_tow_s,
|
||||
arma::vec & measuded_ch0_phase_cycles,
|
||||
arma::vec & measuded_ch1_phase_cycles,
|
||||
arma::vec & measuded_ch0_RX_time_s);
|
||||
arma::mat& true_ch0,
|
||||
arma::mat& true_ch1,
|
||||
arma::vec& true_tow_s,
|
||||
arma::mat& measured_ch0,
|
||||
arma::mat& measured_ch1);
|
||||
void check_results_code_psudorange(
|
||||
arma::vec & true_ch0_dist_m, arma::vec & true_ch1_dist_m,
|
||||
arma::vec & true_ch0_tow_s,
|
||||
arma::vec & measuded_ch0_Pseudorange_m,
|
||||
arma::vec & measuded_ch1_Pseudorange_m,
|
||||
arma::vec & measuded_ch0_RX_time_s);
|
||||
arma::mat& true_ch0,
|
||||
arma::mat& true_ch1,
|
||||
arma::vec& true_tow_s,
|
||||
arma::mat& measured_ch0,
|
||||
arma::mat& measured_ch1);
|
||||
|
||||
HybridObservablesTest()
|
||||
{
|
||||
|
|
@ -289,7 +286,7 @@ void HybridObservablesTest::configure_receiver()
|
|||
config->set_property("Tracking_1C.if", "0");
|
||||
config->set_property("Tracking_1C.dump", "true");
|
||||
config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");
|
||||
config->set_property("Tracking_1C.pll_bw_hz", "15.0");
|
||||
config->set_property("Tracking_1C.pll_bw_hz", "35.0");
|
||||
config->set_property("Tracking_1C.dll_bw_hz", "0.5");
|
||||
config->set_property("Tracking_1C.early_late_space_chips", "0.5");
|
||||
|
||||
|
|
@ -300,27 +297,37 @@ void HybridObservablesTest::configure_receiver()
|
|||
}
|
||||
|
||||
void HybridObservablesTest::check_results_carrier_phase(
|
||||
arma::vec & true_ch0_phase_cycles,
|
||||
arma::vec & true_ch1_phase_cycles,
|
||||
arma::vec & true_ch0_tow_s,
|
||||
arma::vec & measuded_ch0_phase_cycles,
|
||||
arma::vec & measuded_ch1_phase_cycles,
|
||||
arma::vec & measuded_ch0_RX_time_s)
|
||||
arma::mat& true_ch0,
|
||||
arma::mat& true_ch1,
|
||||
arma::vec& true_tow_s,
|
||||
arma::mat& measured_ch0,
|
||||
arma::mat& measured_ch1)
|
||||
{
|
||||
//1. True value interpolation to match the measurement times
|
||||
|
||||
double t0 = std::max(measured_ch0(0, 0), measured_ch1(0, 0));
|
||||
int size1 = measured_ch0.col(0).n_rows;
|
||||
int size2 = measured_ch1.col(0).n_rows;
|
||||
double t1 = std::min(measured_ch0(size1 - 1, 0), measured_ch1(size2 - 1, 0));
|
||||
arma::vec t = arma::linspace<arma::vec>(t0, t1, floor((t1 - t0) * 1e3));
|
||||
|
||||
arma::vec true_ch0_phase_interp;
|
||||
arma::vec true_ch1_phase_interp;
|
||||
arma::interp1(true_ch0_tow_s, true_ch0_phase_cycles, measuded_ch0_RX_time_s, true_ch0_phase_interp);
|
||||
arma::interp1(true_ch0_tow_s, true_ch1_phase_cycles, measuded_ch0_RX_time_s, true_ch1_phase_interp);
|
||||
arma::interp1(true_tow_s, true_ch0.col(3), t, true_ch0_phase_interp);
|
||||
arma::interp1(true_tow_s, true_ch1.col(3), t, true_ch1_phase_interp);
|
||||
|
||||
arma::vec meas_ch0_phase_interp;
|
||||
arma::vec meas_ch1_phase_interp;
|
||||
arma::interp1(measured_ch0.col(0), measured_ch0.col(3), t, meas_ch0_phase_interp);
|
||||
arma::interp1(measured_ch1.col(0), measured_ch1.col(3), t, meas_ch1_phase_interp);
|
||||
|
||||
//2. RMSE
|
||||
arma::vec err_ch0_cycles;
|
||||
arma::vec err_ch1_cycles;
|
||||
|
||||
//compute error without the accumulated carrier phase offsets (which depends on the receiver starting time)
|
||||
err_ch0_cycles = measuded_ch0_phase_cycles - true_ch0_phase_interp - measuded_ch0_phase_cycles(0) + true_ch0_phase_interp(0);
|
||||
err_ch1_cycles = measuded_ch1_phase_cycles - true_ch1_phase_interp - measuded_ch1_phase_cycles(0) + true_ch1_phase_interp(0);
|
||||
err_ch0_cycles = meas_ch0_phase_interp - true_ch0_phase_interp - meas_ch0_phase_interp(0) + true_ch0_phase_interp(0);
|
||||
err_ch1_cycles = meas_ch1_phase_interp - true_ch1_phase_interp - meas_ch1_phase_interp(0) + true_ch1_phase_interp(0);
|
||||
|
||||
arma::vec err2_ch0 = arma::square(err_ch0_cycles);
|
||||
double rmse_ch0 = sqrt(arma::mean(err2_ch0));
|
||||
|
|
@ -347,58 +354,68 @@ void HybridObservablesTest::check_results_carrier_phase(
|
|||
|
||||
//5. report
|
||||
std::streamsize ss = std::cout.precision();
|
||||
std::cout << std::setprecision(10) << "Channel 0 Carrier phase RMSE="
|
||||
<< rmse_ch0 << ", mean=" << error_mean_ch0
|
||||
<< ", stdev=" << sqrt(error_var_ch0)
|
||||
<< " (max,min)=" << max_error_ch0
|
||||
std::cout << std::setprecision(10) << "Channel 0 Carrier phase RMSE = "
|
||||
<< rmse_ch0 << ", mean = " << error_mean_ch0
|
||||
<< ", stdev = " << sqrt(error_var_ch0)
|
||||
<< " (max,min) = " << max_error_ch0
|
||||
<< "," << min_error_ch0
|
||||
<< " [cycles]" << std::endl;
|
||||
std::cout.precision (ss);
|
||||
|
||||
ASSERT_LT(rmse_ch0, 1e-2);
|
||||
ASSERT_LT(error_mean_ch0, 1e-2);
|
||||
ASSERT_GT(error_mean_ch0, -1e-2);
|
||||
ASSERT_LT(error_var_ch0, 1e-2);
|
||||
ASSERT_LT(max_error_ch0, 5e-2);
|
||||
ASSERT_GT(min_error_ch0, -5e-2);
|
||||
ASSERT_LT(rmse_ch0, 5e-2);
|
||||
ASSERT_LT(error_mean_ch0, 5e-2);
|
||||
ASSERT_GT(error_mean_ch0, -5e-2);
|
||||
ASSERT_LT(error_var_ch0, 5e-2);
|
||||
ASSERT_LT(max_error_ch0, 5e-2);
|
||||
ASSERT_GT(min_error_ch0, -5e-2);
|
||||
|
||||
//5. report
|
||||
ss = std::cout.precision();
|
||||
std::cout << std::setprecision(10) << "Channel 1 Carrier phase RMSE="
|
||||
<< rmse_ch1 << ", mean=" << error_mean_ch1
|
||||
<< ", stdev=" << sqrt(error_var_ch1)
|
||||
<< " (max,min)=" << max_error_ch1
|
||||
std::cout << std::setprecision(10) << "Channel 1 Carrier phase RMSE = "
|
||||
<< rmse_ch1 << ", mean = " << error_mean_ch1
|
||||
<< ", stdev = " << sqrt(error_var_ch1)
|
||||
<< " (max,min) = " << max_error_ch1
|
||||
<< "," << min_error_ch1
|
||||
<< " [cycles]" << std::endl;
|
||||
std::cout.precision (ss);
|
||||
|
||||
ASSERT_LT(rmse_ch1, 1e-2);
|
||||
ASSERT_LT(error_mean_ch1, 1e-2);
|
||||
ASSERT_GT(error_mean_ch1, -1e-2);
|
||||
ASSERT_LT(error_var_ch1, 1e-2);
|
||||
ASSERT_LT(max_error_ch1, 5e-2);
|
||||
ASSERT_GT(min_error_ch1, -5e-2);
|
||||
ASSERT_LT(rmse_ch1, 5e-2);
|
||||
ASSERT_LT(error_mean_ch1, 5e-2);
|
||||
ASSERT_GT(error_mean_ch1, -5e-2);
|
||||
ASSERT_LT(error_var_ch1, 5e-2);
|
||||
ASSERT_LT(max_error_ch1, 5e-2);
|
||||
ASSERT_GT(min_error_ch1, -5e-2);
|
||||
}
|
||||
|
||||
|
||||
void HybridObservablesTest::check_results_code_psudorange(
|
||||
arma::vec & true_ch0_dist_m,
|
||||
arma::vec & true_ch1_dist_m,
|
||||
arma::vec & true_ch0_tow_s,
|
||||
arma::vec & measuded_ch0_Pseudorange_m,
|
||||
arma::vec & measuded_ch1_Pseudorange_m,
|
||||
arma::vec & measuded_ch0_RX_time_s)
|
||||
arma::mat& true_ch0,
|
||||
arma::mat& true_ch1,
|
||||
arma::vec& true_tow_s,
|
||||
arma::mat& measured_ch0,
|
||||
arma::mat& measured_ch1)
|
||||
{
|
||||
//1. True value interpolation to match the measurement times
|
||||
|
||||
double t0 = std::max(measured_ch0(0, 0), measured_ch1(0, 0));
|
||||
int size1 = measured_ch0.col(0).n_rows;
|
||||
int size2 = measured_ch1.col(0).n_rows;
|
||||
double t1 = std::min(measured_ch0(size1 - 1, 0), measured_ch1(size2 - 1, 0));
|
||||
arma::vec t = arma::linspace<arma::vec>(t0, t1, floor((t1 - t0) * 1e3));
|
||||
|
||||
arma::vec true_ch0_dist_interp;
|
||||
arma::vec true_ch1_dist_interp;
|
||||
arma::interp1(true_ch0_tow_s, true_ch0_dist_m, measuded_ch0_RX_time_s, true_ch0_dist_interp);
|
||||
arma::interp1(true_ch0_tow_s, true_ch1_dist_m, measuded_ch0_RX_time_s, true_ch1_dist_interp);
|
||||
arma::interp1(true_tow_s, true_ch0.col(1), t, true_ch0_dist_interp);
|
||||
arma::interp1(true_tow_s, true_ch1.col(1), t, true_ch1_dist_interp);
|
||||
|
||||
arma::vec meas_ch0_dist_interp;
|
||||
arma::vec meas_ch1_dist_interp;
|
||||
arma::interp1(measured_ch0.col(0), measured_ch0.col(4), t, meas_ch0_dist_interp);
|
||||
arma::interp1(measured_ch1.col(0), measured_ch1.col(4), t, meas_ch1_dist_interp);
|
||||
|
||||
// generate delta pseudoranges
|
||||
arma::vec delta_true_dist_m = true_ch0_dist_interp-true_ch1_dist_interp;
|
||||
arma::vec delta_measured_dist_m = measuded_ch0_Pseudorange_m-measuded_ch1_Pseudorange_m;
|
||||
arma::vec delta_true_dist_m = true_ch0_dist_interp - true_ch1_dist_interp;
|
||||
arma::vec delta_measured_dist_m = meas_ch0_dist_interp - meas_ch1_dist_interp;
|
||||
|
||||
//2. RMSE
|
||||
arma::vec err;
|
||||
|
|
@ -417,20 +434,20 @@ void HybridObservablesTest::check_results_code_psudorange(
|
|||
|
||||
//5. report
|
||||
std::streamsize ss = std::cout.precision();
|
||||
std::cout << std::setprecision(10) << "Delta Observables RMSE="
|
||||
<< rmse << ", mean=" << error_mean
|
||||
<< ", stdev=" << sqrt(error_var)
|
||||
<< " (max,min)=" << max_error
|
||||
std::cout << std::setprecision(10) << "Delta Observables RMSE = "
|
||||
<< rmse << ", mean = " << error_mean
|
||||
<< ", stdev = " << sqrt(error_var)
|
||||
<< " (max,min) = " << max_error
|
||||
<< "," << min_error
|
||||
<< " [meters]" << std::endl;
|
||||
std::cout.precision (ss);
|
||||
|
||||
ASSERT_LT(rmse, 0.5);
|
||||
ASSERT_LT(error_mean, 0.5);
|
||||
ASSERT_LT(rmse, 0.5);
|
||||
ASSERT_LT(error_mean, 0.5);
|
||||
ASSERT_GT(error_mean, -0.5);
|
||||
ASSERT_LT(error_var, 0.5);
|
||||
ASSERT_LT(max_error, 2);
|
||||
ASSERT_GT(min_error, -2);
|
||||
ASSERT_LT(error_var, 0.5);
|
||||
ASSERT_LT(max_error, 2.0);
|
||||
ASSERT_GT(min_error, -2.0);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -440,7 +457,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
|||
configure_generator();
|
||||
|
||||
// Generate signal raw signal samples and observations RINEX file
|
||||
if (FLAGS_disable_generator==false)
|
||||
if (FLAGS_disable_generator == false)
|
||||
{
|
||||
generate_signal();
|
||||
}
|
||||
|
|
@ -478,9 +495,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
|||
|
||||
top_block = gr::make_top_block("Telemetry_Decoder test");
|
||||
std::shared_ptr<TrackingInterface> tracking_ch0 = std::make_shared<GpsL1CaDllPllTracking>(config.get(), "Tracking_1C", 1, 1);
|
||||
//std::shared_ptr<TrackingInterface> tracking_ch1 = std::make_shared<GpsL1CaDllPllCAidTracking>(config.get(), "Tracking_1C", 1, 1);
|
||||
std::shared_ptr<TrackingInterface> tracking_ch1 = std::make_shared<GpsL1CaDllPllTracking>(config.get(), "Tracking_1C", 1, 1);
|
||||
//std::shared_ptr<TrackingInterface> tracking_ch1 = std::make_shared<GpsL1CaDllPllCAidTracking>(config.get(), "Tracking_1C", 1, 1);
|
||||
|
||||
boost::shared_ptr<HybridObservablesTest_msg_rx> msg_rx_ch0 = HybridObservablesTest_msg_rx_make();
|
||||
boost::shared_ptr<HybridObservablesTest_msg_rx> msg_rx_ch1 = HybridObservablesTest_msg_rx_make();
|
||||
|
|
@ -524,15 +539,15 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
|||
tlm_ch0->set_channel(0);
|
||||
tlm_ch1->set_channel(1);
|
||||
|
||||
tlm_ch0->set_satellite(Gnss_Satellite(std::string("GPS"),gnss_synchro_ch0.PRN));
|
||||
tlm_ch1->set_satellite(Gnss_Satellite(std::string("GPS"),gnss_synchro_ch1.PRN));
|
||||
tlm_ch0->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_ch0.PRN));
|
||||
tlm_ch1->set_satellite(Gnss_Satellite(std::string("GPS"), gnss_synchro_ch1.PRN));
|
||||
}) << "Failure setting gnss_synchro.";
|
||||
|
||||
boost::shared_ptr<HybridObservablesTest_tlm_msg_rx> tlm_msg_rx_ch1 = HybridObservablesTest_tlm_msg_rx_make();
|
||||
boost::shared_ptr<HybridObservablesTest_tlm_msg_rx> tlm_msg_rx_ch2 = HybridObservablesTest_tlm_msg_rx_make();
|
||||
|
||||
//Observables
|
||||
std::shared_ptr<ObservablesInterface> observables(new HybridObservables(config.get(), "Observables",2, 2));
|
||||
std::shared_ptr<ObservablesInterface> observables(new HybridObservables(config.get(), "Observables",3, 2));
|
||||
|
||||
ASSERT_NO_THROW( {
|
||||
tracking_ch0->set_channel(gnss_synchro_ch0.Channel_ID);
|
||||
|
|
@ -556,7 +571,10 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
|||
gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make();
|
||||
gr::blocks::null_sink::sptr sink_ch0 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro));
|
||||
gr::blocks::null_sink::sptr sink_ch1 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro));
|
||||
gnss_sdr_sample_counter_sptr samp_counter = gnss_sdr_make_sample_counter(static_cast<double>(baseband_sampling_freq));
|
||||
top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0);
|
||||
top_block->connect(gr_interleaved_char_to_complex, 0, samp_counter, 0);
|
||||
|
||||
//ch0
|
||||
top_block->connect(gr_interleaved_char_to_complex, 0, tracking_ch0->get_left_block(), 0);
|
||||
top_block->connect(tracking_ch0->get_right_block(), 0, tlm_ch0->get_left_block(), 0);
|
||||
|
|
@ -570,6 +588,7 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
|||
|
||||
top_block->connect(observables->get_right_block(), 0, sink_ch0, 0);
|
||||
top_block->connect(observables->get_right_block(), 1, sink_ch1, 0);
|
||||
top_block->connect(samp_counter, 0, observables->get_left_block(), 2);
|
||||
|
||||
}) << "Failure connecting the blocks.";
|
||||
|
||||
|
|
@ -589,48 +608,43 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
|||
true_observables_reader true_observables;
|
||||
|
||||
ASSERT_NO_THROW({
|
||||
if ( true_observables.open_obs_file(std::string("./obs_out.bin")) == false)
|
||||
{
|
||||
throw std::exception();
|
||||
};
|
||||
if(true_observables.open_obs_file(std::string("./obs_out.bin")) == false)
|
||||
{
|
||||
throw std::exception();
|
||||
}
|
||||
}) << "Failure opening true observables file";
|
||||
|
||||
long int nepoch = true_observables.num_epochs();
|
||||
unsigned int nepoch = static_cast<unsigned int>(true_observables.num_epochs());
|
||||
|
||||
std::cout << "True observation epochs=" << nepoch << std::endl;
|
||||
arma::vec true_ch0_dist_m = arma::zeros(nepoch, 1);
|
||||
arma::vec true_ch0_acc_carrier_phase_cycles = arma::zeros(nepoch, 1);
|
||||
arma::vec true_ch0_Doppler_Hz = arma::zeros(nepoch, 1);
|
||||
arma::vec true_ch0_tow_s = arma::zeros(nepoch, 1);
|
||||
arma::vec true_ch1_dist_m = arma::zeros(nepoch, 1);
|
||||
arma::vec true_ch1_acc_carrier_phase_cycles = arma::zeros(nepoch, 1);
|
||||
arma::vec true_ch1_Doppler_Hz = arma::zeros(nepoch, 1);
|
||||
arma::vec true_ch1_tow_s = arma::zeros(nepoch, 1);
|
||||
std::cout << "True observation epochs = " << nepoch << std::endl;
|
||||
// Matrices for storing columnwise true GPS time, Range, Doppler and Carrier phase
|
||||
arma::mat true_ch0 = arma::zeros<arma::mat>(nepoch, 4);
|
||||
arma::mat true_ch1 = arma::zeros<arma::mat>(nepoch, 4);
|
||||
|
||||
true_observables.restart();
|
||||
long int epoch_counter = 0;
|
||||
ASSERT_NO_THROW({
|
||||
while(true_observables.read_binary_obs())
|
||||
{
|
||||
if (round(true_observables.prn[0])!=gnss_synchro_ch0.PRN)
|
||||
if(round(true_observables.prn[0]) != gnss_synchro_ch0.PRN)
|
||||
{
|
||||
std::cout<<"True observables SV PRN do not match"<<round(true_observables.prn[1])<<std::endl;
|
||||
std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl;
|
||||
throw std::exception();
|
||||
}
|
||||
if (round(true_observables.prn[1])!=gnss_synchro_ch1.PRN)
|
||||
if(round(true_observables.prn[1]) != gnss_synchro_ch1.PRN)
|
||||
{
|
||||
std::cout<<"True observables SV PRN do not match "<<round(true_observables.prn[1])<<std::endl;
|
||||
std::cout << "True observables SV PRN does not match " << round(true_observables.prn[1]) << std::endl;
|
||||
throw std::exception();
|
||||
}
|
||||
true_ch0_tow_s(epoch_counter) = true_observables.gps_time_sec[0];
|
||||
true_ch0_dist_m(epoch_counter) = true_observables.dist_m[0];
|
||||
true_ch0_Doppler_Hz(epoch_counter) = true_observables.doppler_l1_hz[0];
|
||||
true_ch0_acc_carrier_phase_cycles(epoch_counter) = true_observables.acc_carrier_phase_l1_cycles[0];
|
||||
true_ch0(epoch_counter, 0) = true_observables.gps_time_sec[0];
|
||||
true_ch0(epoch_counter, 1) = true_observables.dist_m[0];
|
||||
true_ch0(epoch_counter, 2) = true_observables.doppler_l1_hz[0];
|
||||
true_ch0(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[0];
|
||||
|
||||
true_ch1_tow_s(epoch_counter) = true_observables.gps_time_sec[1];
|
||||
true_ch1_dist_m(epoch_counter) = true_observables.dist_m[1];
|
||||
true_ch1_Doppler_Hz(epoch_counter) = true_observables.doppler_l1_hz[1];
|
||||
true_ch1_acc_carrier_phase_cycles(epoch_counter) = true_observables.acc_carrier_phase_l1_cycles[1];
|
||||
true_ch1(epoch_counter, 0) = true_observables.gps_time_sec[1];
|
||||
true_ch1(epoch_counter, 1) = true_observables.dist_m[1];
|
||||
true_ch1(epoch_counter, 2) = true_observables.doppler_l1_hz[1];
|
||||
true_ch1(epoch_counter, 3) = true_observables.acc_carrier_phase_l1_cycles[1];
|
||||
|
||||
epoch_counter++;
|
||||
}
|
||||
|
|
@ -639,87 +653,81 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
|
|||
//read measured values
|
||||
observables_dump_reader estimated_observables(2); //two channels
|
||||
ASSERT_NO_THROW({
|
||||
if (estimated_observables.open_obs_file(std::string("./observables.dat")) == false)
|
||||
{
|
||||
throw std::exception();
|
||||
};
|
||||
if(estimated_observables.open_obs_file(std::string("./observables.dat")) == false)
|
||||
{
|
||||
throw std::exception();
|
||||
}
|
||||
}) << "Failure opening dump observables file";
|
||||
|
||||
nepoch = estimated_observables.num_epochs();
|
||||
std::cout << "Measured observation epochs=" << nepoch << std::endl;
|
||||
nepoch = static_cast<unsigned int>(estimated_observables.num_epochs());
|
||||
std::cout << "Measured observation epochs = " << nepoch << std::endl;
|
||||
|
||||
arma::vec measuded_ch0_RX_time_s = arma::zeros(nepoch, 1);
|
||||
arma::vec measuded_ch0_TOW_at_current_symbol_s = arma::zeros(nepoch, 1);
|
||||
arma::vec measuded_ch0_Carrier_Doppler_hz = arma::zeros(nepoch, 1);
|
||||
arma::vec measuded_ch0_Acc_carrier_phase_hz = arma::zeros(nepoch, 1);
|
||||
arma::vec measuded_ch0_Pseudorange_m = arma::zeros(nepoch, 1);
|
||||
|
||||
arma::vec measuded_ch1_RX_time_s = arma::zeros(nepoch, 1);
|
||||
arma::vec measuded_ch1_TOW_at_current_symbol_s = arma::zeros(nepoch, 1);
|
||||
arma::vec measuded_ch1_Carrier_Doppler_hz = arma::zeros(nepoch, 1);
|
||||
arma::vec measuded_ch1_Acc_carrier_phase_hz = arma::zeros(nepoch, 1);
|
||||
arma::vec measuded_ch1_Pseudorange_m = arma::zeros(nepoch, 1);
|
||||
// Matrices for storing columnwise measured RX_time, TOW, Doppler, Carrier phase and Pseudorange
|
||||
arma::mat measured_ch0 = arma::zeros<arma::mat>(nepoch, 5);
|
||||
arma::mat measured_ch1 = arma::zeros<arma::mat>(nepoch, 5);
|
||||
|
||||
estimated_observables.restart();
|
||||
|
||||
epoch_counter = 0;
|
||||
long int epoch_counter2 = 0;
|
||||
while(estimated_observables.read_binary_obs())
|
||||
{
|
||||
measuded_ch0_RX_time_s(epoch_counter) = estimated_observables.RX_time[0];
|
||||
measuded_ch0_TOW_at_current_symbol_s(epoch_counter) =estimated_observables.TOW_at_current_symbol_s[0];
|
||||
measuded_ch0_Carrier_Doppler_hz(epoch_counter) = estimated_observables.Carrier_Doppler_hz[0];
|
||||
measuded_ch0_Acc_carrier_phase_hz(epoch_counter) = estimated_observables.Acc_carrier_phase_hz[0];
|
||||
measuded_ch0_Pseudorange_m(epoch_counter) = estimated_observables.Pseudorange_m[0];
|
||||
|
||||
measuded_ch1_RX_time_s(epoch_counter) = estimated_observables.RX_time[1];
|
||||
measuded_ch1_TOW_at_current_symbol_s(epoch_counter) =estimated_observables.TOW_at_current_symbol_s[1];
|
||||
measuded_ch1_Carrier_Doppler_hz(epoch_counter) = estimated_observables.Carrier_Doppler_hz[1];
|
||||
measuded_ch1_Acc_carrier_phase_hz(epoch_counter) = estimated_observables.Acc_carrier_phase_hz[1];
|
||||
measuded_ch1_Pseudorange_m(epoch_counter) = estimated_observables.Pseudorange_m[1];
|
||||
|
||||
epoch_counter++;
|
||||
if(static_cast<bool>(estimated_observables.valid[0]))
|
||||
{
|
||||
measured_ch0(epoch_counter, 0) = estimated_observables.RX_time[0];
|
||||
measured_ch0(epoch_counter, 1) = estimated_observables.TOW_at_current_symbol_s[0];
|
||||
measured_ch0(epoch_counter, 2) = estimated_observables.Carrier_Doppler_hz[0];
|
||||
measured_ch0(epoch_counter, 3) = estimated_observables.Acc_carrier_phase_hz[0];
|
||||
measured_ch0(epoch_counter, 4) = estimated_observables.Pseudorange_m[0];
|
||||
epoch_counter++;
|
||||
}
|
||||
if(static_cast<bool>(estimated_observables.valid[1]))
|
||||
{
|
||||
measured_ch1(epoch_counter2, 0) = estimated_observables.RX_time[1];
|
||||
measured_ch1(epoch_counter2, 1) = estimated_observables.TOW_at_current_symbol_s[1];
|
||||
measured_ch1(epoch_counter2, 2) = estimated_observables.Carrier_Doppler_hz[1];
|
||||
measured_ch1(epoch_counter2, 3) = estimated_observables.Acc_carrier_phase_hz[1];
|
||||
measured_ch1(epoch_counter2, 4) = estimated_observables.Pseudorange_m[1];
|
||||
epoch_counter2++;
|
||||
}
|
||||
}
|
||||
|
||||
//Cut measurement tail zeros
|
||||
arma::uvec index = arma::find(measured_ch0.col(0) > 0.0, 1, "last");
|
||||
if((index.size() > 0) and index(0) < (nepoch - 1))
|
||||
{ measured_ch0.shed_rows(index(0) + 1, nepoch - 1); }
|
||||
index = arma::find(measured_ch1.col(0) > 0.0, 1, "last");
|
||||
if((index.size() > 0) and index(0) < (nepoch - 1))
|
||||
{ measured_ch1.shed_rows(index(0) + 1, nepoch - 1); }
|
||||
|
||||
//Cut measurement initial transitory of the measurements
|
||||
arma::uvec initial_meas_point = arma::find(measuded_ch0_RX_time_s >= true_ch0_tow_s(0), 1, "first");
|
||||
index = arma::find(measured_ch0.col(0) >= true_ch0(0, 0), 1, "first");
|
||||
if((index.size() > 0) and (index(0) > 0))
|
||||
{ measured_ch0.shed_rows(0, index(0)); }
|
||||
index = arma::find(measured_ch1.col(0) >= true_ch1(0, 0), 1, "first");
|
||||
if((index.size() > 0) and (index(0) > 0))
|
||||
{ measured_ch1.shed_rows(0, index(0)); }
|
||||
|
||||
measuded_ch0_RX_time_s = measuded_ch0_RX_time_s.subvec(initial_meas_point(0), measuded_ch0_RX_time_s.size() - 1);
|
||||
measuded_ch0_Pseudorange_m = measuded_ch0_Pseudorange_m.subvec(initial_meas_point(0), measuded_ch0_Pseudorange_m.size() - 1);
|
||||
measuded_ch0_Acc_carrier_phase_hz = measuded_ch0_Acc_carrier_phase_hz.subvec(initial_meas_point(0), measuded_ch0_Acc_carrier_phase_hz.size() - 1);
|
||||
|
||||
measuded_ch1_RX_time_s = measuded_ch1_RX_time_s.subvec(initial_meas_point(0), measuded_ch1_RX_time_s.size() - 1);
|
||||
measuded_ch1_Pseudorange_m = measuded_ch1_Pseudorange_m.subvec(initial_meas_point(0), measuded_ch1_Pseudorange_m.size() - 1);
|
||||
measuded_ch1_Acc_carrier_phase_hz = measuded_ch1_Acc_carrier_phase_hz.subvec(initial_meas_point(0), measuded_ch1_Acc_carrier_phase_hz.size() - 1);
|
||||
|
||||
//correct the clock error using true values (it is not possible for a receiver to correct
|
||||
//Correct the clock error using true values (it is not possible for a receiver to correct
|
||||
//the receiver clock offset error at the observables level because it is required the
|
||||
//decoding of the ephemeris data and solve the PVT equations)
|
||||
|
||||
//find the reference satellite and compute the receiver time offset at obsevable level
|
||||
//Find the reference satellite (the nearest) and compute the receiver time offset at observable level
|
||||
arma::vec receiver_time_offset_s;
|
||||
if (measuded_ch0_Pseudorange_m(0)<measuded_ch1_Pseudorange_m(0))
|
||||
{
|
||||
receiver_time_offset_s = true_ch0_dist_m / GPS_C_m_s - GPS_STARTOFFSET_ms/1000.0;
|
||||
}
|
||||
if(measured_ch0(0, 4) < measured_ch1(0, 4))
|
||||
{
|
||||
receiver_time_offset_s = true_ch0.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
receiver_time_offset_s = true_ch1_dist_m / GPS_C_m_s - GPS_STARTOFFSET_ms/1000.0;
|
||||
}
|
||||
arma::vec corrected_reference_TOW_s = true_ch0_tow_s - receiver_time_offset_s;
|
||||
{
|
||||
receiver_time_offset_s = true_ch1.col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0;
|
||||
}
|
||||
arma::vec corrected_reference_TOW_s = true_ch0.col(0) - receiver_time_offset_s;
|
||||
std::cout << "Receiver time offset: " << receiver_time_offset_s(0) * 1e3 << " [ms]" << std::endl;
|
||||
|
||||
std::cout <<" receiver_time_offset_s [0]: " << receiver_time_offset_s(0) << std::endl;
|
||||
//Compare measured observables
|
||||
check_results_code_psudorange(true_ch0, true_ch1, corrected_reference_TOW_s, measured_ch0, measured_ch1);
|
||||
check_results_carrier_phase(true_ch0, true_ch1, corrected_reference_TOW_s, measured_ch0, measured_ch1);
|
||||
|
||||
//compare measured observables
|
||||
check_results_code_psudorange(true_ch0_dist_m, true_ch1_dist_m, corrected_reference_TOW_s,
|
||||
measuded_ch0_Pseudorange_m,measuded_ch1_Pseudorange_m, measuded_ch0_RX_time_s);
|
||||
|
||||
check_results_carrier_phase(true_ch0_acc_carrier_phase_cycles,
|
||||
true_ch1_acc_carrier_phase_cycles,
|
||||
corrected_reference_TOW_s,
|
||||
measuded_ch0_Acc_carrier_phase_hz,
|
||||
measuded_ch1_Acc_carrier_phase_hz,
|
||||
measuded_ch0_RX_time_s);
|
||||
|
||||
std::cout << "Test completed in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl;
|
||||
std::cout << "Test completed in " << elapsed_seconds.count() << " [s]" << std::endl;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue