mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-28 09:54:51 +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:
Damian Miralles 2018-08-29 17:57:13 -06:00
commit 483e5ece35
83 changed files with 5890 additions and 3358 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
AUTHORS
View File

@ -50,6 +50,7 @@ Daniel Fehr daniel.co@bluewin.ch Contributor
David Pubill david.pubill@cttc.cat Contributor
Fran Fabra fabra@ice.csic.es Contributor
Gabriel Araujo gabriel.araujo.5000@gmail.com Contributor
Gerald LaMountain gerald@gece.neu.edu Contributor
Leonardo Tonetto tonetto.dev@gmail.com Contributor
Mara Branzanti mara.branzanti@gmail.com Contributor
Marc Molina marc.molina.pena@gmail.com Contributor

1
CMakeLists.txt
View File

@ -523,7 +523,6 @@ if(NOT GNURADIO_RUNTIME_FOUND)
message("You can install it easily via Macports:")
message(" sudo port install gnuradio ")
message("Alternatively, you can use homebrew:")
message(" brew tap odrisci/gnuradio")
message(" brew install gnuradio" )
message(FATAL_ERROR "GNU Radio ${GNSSSDR_GNURADIO_MIN_VERSION} or later is required to build gnss-sdr")
endif(OS_IS_MACOSX)

10
MANIFEST.md
View File

@ -12,11 +12,17 @@ author:
- et altri (see AUTHORS file for a list of contributors)
copyright_owner:
- The Authors
dependencies: gnuradio (>= 3.7.3), armadillo, gflags, glog, gnutls, matio
dependencies:
- gnuradio (>= 3.7.3)
- armadillo
- gflags
- glog
- gnutls
- matio
license: GPLv3+
repo: https://github.com/gnss-sdr/gnss-sdr
website: https://gnss-sdr.org
icon: https://raw.githubusercontent.com/gnss-sdr/gnss-sdr/master/docs/doxygen/images/gnss-sdr_logo.png
icon: https://gnss-sdr.org/assets/images/logo400x400.jpg
---
Global Navigation Satellite Systems receiver defined by software. It performs all the signal
processing from raw signal samples up to the computation of the Position-Velocity-Time solution,

63
conf/gnss-sdr-kalman-bayes.conf Normal file
View File

@ -0,0 +1,63 @@
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second].
GNSS-SDR.internal_fs_sps=2000000
GNSS-SDR.internal_fs_hz=2000000
;######### SIGNAL_SOURCE CONFIG ############
SignalSource.implementation=File_Signal_Source
SignalSource.filename=/home/glamountain/gnss-sdr/data/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN.dat
SignalSource.item_type=ishort
SignalSource.sampling_frequency=4000000
SignalSource.freq=1575420000
SignalSource.samples=0
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Signal_Conditioner
DataTypeAdapter.implementation=Ishort_To_Complex
InputFilter.implementation=Pass_Through
InputFilter.item_type=gr_complex
Resampler.implementation=Direct_Resampler
Resampler.sample_freq_in=4000000
Resampler.sample_freq_out=2000000
Resampler.item_type=gr_complex
;######### CHANNELS GLOBAL CONFIG ############
Channels_1C.count=8
Channels.in_acquisition=1
Channel.signal=1C
;######### ACQUISITION GLOBAL CONFIG ############
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition_1C.item_type=gr_complex
Acquisition_1C.threshold=0.008
Acquisition_1C.doppler_max=10000
Acquisition_1C.doppler_step=250
Acquisition_1C.dump=false
Acquisition_1C.dump_filename=../data/kalman/acq_dump
;######### TRACKING GLOBAL CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_KF_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.pll_bw_hz=40.0;
Tracking_1C.dll_bw_hz=4.0;
Tracking_1C.order=3;
Tracking_1C.dump=true
Tracking_1C.dump_filename=../data/kalman/epl_tracking_ch_
Tracking_1C.bce_run = true;
Tracking_1C.p_transient = 0;
Tracking_1C.s_transient = 100;
;######### TELEMETRY DECODER GPS CONFIG ############
TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
;######### OBSERVABLES CONFIG ############
Observables.implementation=GPS_L1_CA_Observables
;######### PVT CONFIG ############
PVT.implementation=GPS_L1_CA_PVT
PVT.averaging_depth=100
PVT.flag_averaging=true
PVT.output_rate_ms=10
PVT.display_rate_ms=500

211
conf/gnss-sdr_GPS_L1_nsr_kf.conf Normal file
View File

@ -0,0 +1,211 @@
; Default configuration file
; You can define your own receiver and invoke it by doing
; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf
;
[GNSS-SDR]
;######### GLOBAL OPTIONS ##################
;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second].
;GNSS-SDR.internal_fs_sps=6826700
GNSS-SDR.internal_fs_sps=2560000
;GNSS-SDR.internal_fs_sps=4096000
;GNSS-SDR.internal_fs_sps=5120000
;######### SIGNAL_SOURCE CONFIG ############
;#implementation: Use [File_Signal_Source] [Nsr_File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] (experimental)
SignalSource.implementation=Nsr_File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=/home/javier/signals/ifen/E1L1_FE0_Band0.stream ; <- PUT YOUR FILE HERE
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
SignalSource.item_type=byte
;#sampling_frequency: Original Signal sampling frequency in [Hz]
SignalSource.sampling_frequency=20480000
;#freq: RF front-end center frequency in [Hz]
SignalSource.freq=1575420000
;#samples: Number of samples to be processed. Notice that 0 indicates the entire file.
SignalSource.samples=0
;#repeat: Repeat the processing file. Disable this option in this version
SignalSource.repeat=false
;#dump: Dump the Signal source data to a file. Disable this option in this version
SignalSource.dump=false
SignalSource.dump_filename=../data/signal_source.dat
;#enable_throttle_control: Enabling this option tells the signal source to keep the delay between samples in post processing.
; it helps to not overload the CPU, but the processing time will be longer.
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
;## It holds blocks to change data type, filter and resample input data.
;#implementation: Use [Pass_Through] or [Signal_Conditioner]
;#[Pass_Through] disables this block and the [DataTypeAdapter], [InputFilter] and [Resampler] blocks
;#[Signal_Conditioner] enables this block. Then you have to configure [DataTypeAdapter], [InputFilter] and [Resampler] blocks
SignalConditioner.implementation=Signal_Conditioner
;######### DATA_TYPE_ADAPTER CONFIG ############
;## Changes the type of input data.
;#implementation: [Pass_Through] disables this block
DataTypeAdapter.implementation=Pass_Through
DataTypeAdapter.item_type=float
;######### INPUT_FILTER CONFIG ############
;## Filter the input data. Can be combined with frequency translation for IF signals
;#implementation: Use [Pass_Through] or [Fir_Filter] or [Freq_Xlating_Fir_Filter]
;#[Freq_Xlating_Fir_Filter] enables FIR filter and a composite frequency translation
;# that shifts IF down to zero Hz.
InputFilter.implementation=Freq_Xlating_Fir_Filter
;#dump: Dump the filtered data to a file.
InputFilter.dump=false
;#dump_filename: Log path and filename.
InputFilter.dump_filename=../data/input_filter.dat
;#The following options are used in the filter design of Fir_Filter and Freq_Xlating_Fir_Filter implementation.
;#These options are based on parameters of gnuradio's function: gr_remez.
;#These function calculates the optimal (in the Chebyshev/minimax sense) FIR filter inpulse
;#reponse given a set of band edges, the desired reponse on those bands,
;#and the weight given to the error in those bands.
;#input_item_type: Type and resolution for input signal samples. Use only gr_complex in this version.
InputFilter.input_item_type=float
;#outut_item_type: Type and resolution for output filtered signal samples. Use only gr_complex in this version.
InputFilter.output_item_type=gr_complex
;#taps_item_type: Type and resolution for the taps of the filter. Use only float in this version.
InputFilter.taps_item_type=float
;#number_of_taps: Number of taps in the filter. Increasing this parameter increases the processing time
InputFilter.number_of_taps=5
;#number_of _bands: Number of frequency bands in the filter.
InputFilter.number_of_bands=2
;#bands: frequency at the band edges [ b1 e1 b2 e2 b3 e3 ...].
;#Frequency is in the range [0, 1], with 1 being the Nyquist frequency (Fs/2)
;#The number of band_begin and band_end elements must match the number of bands
InputFilter.band1_begin=0.0
InputFilter.band1_end=0.45
InputFilter.band2_begin=0.55
InputFilter.band2_end=1.0
;#ampl: desired amplitude at the band edges [ a(b1) a(e1) a(b2) a(e2) ...].
;#The number of ampl_begin and ampl_end elements must match the number of bands
InputFilter.ampl1_begin=1.0
InputFilter.ampl1_end=1.0
InputFilter.ampl2_begin=0.0
InputFilter.ampl2_end=0.0
;#band_error: weighting applied to each band (usually 1).
;#The number of band_error elements must match the number of bands
InputFilter.band1_error=1.0
InputFilter.band2_error=1.0
;#filter_type: one of "bandpass", "hilbert" or "differentiator"
InputFilter.filter_type=bandpass
;#grid_density: determines how accurately the filter will be constructed.
;The minimum value is 16; higher values are slower to compute the filter.
InputFilter.grid_density=16
;# Original sampling frequency stored in the signal file
InputFilter.sampling_frequency=20480000
;#The following options are used only in Freq_Xlating_Fir_Filter implementation.
;#InputFilter.IF is the intermediate frequency (in Hz) shifted down to zero Hz
InputFilter.IF=5499998.47412109
;# Decimation factor after the frequency tranaslating block
InputFilter.decimation_factor=8
;######### RESAMPLER CONFIG ############
;## Resamples the input data.
;#implementation: Use [Pass_Through] or [Direct_Resampler]
;#[Pass_Through] disables this block
;#[Direct_Resampler] enables a resampler that implements a nearest neigbourhood interpolation
Resampler.implementation=Pass_Through
;######### CHANNELS GLOBAL CONFIG ############
;#count: Number of available GPS satellite channels.
Channels_1C.count=8
Channels.in_acquisition=1
#Channel.signal=1C
;######### ACQUISITION GLOBAL CONFIG ############
Acquisition_1C.dump=false
Acquisition_1C.dump_filename=./acq_dump.dat
Acquisition_1C.item_type=gr_complex
Acquisition_1C.if=0
Acquisition_1C.sampled_ms=1
Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition
;#use_CFAR_algorithm: If enabled, acquisition estimates the input signal power to implement CFAR detection algorithms
;#notice that this affects the Acquisition threshold range!
Acquisition_1C.use_CFAR_algorithm=false;
;#threshold: Acquisition threshold
Acquisition_1C.threshold=10
;Acquisition_1C.pfa=0.01
Acquisition_1C.doppler_max=5000
Acquisition_1C.doppler_step=100
;######### TRACKING GPS CONFIG ############
Tracking_1C.implementation=GPS_L1_CA_KF_Tracking
Tracking_1C.item_type=gr_complex
Tracking_1C.if=0
Tracking_1C.dump=true
Tracking_1C.dump_filename=../data/epl_tracking_ch_
Tracking_1C.pll_bw_hz=15.0;
Tracking_1C.dll_bw_hz=2.0;
Tracking_1C.order=3;
;######### TELEMETRY DECODER GPS CONFIG ############
TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder
TelemetryDecoder_1C.dump=false
TelemetryDecoder_1C.decimation_factor=1;
;######### OBSERVABLES CONFIG ############
;#implementation:
Observables.implementation=Hybrid_Observables
;#dump: Enable or disable the Observables internal binary data file logging [true] or [false]
Observables.dump=false
;#dump_filename: Log path and filename.
Observables.dump_filename=./observables.dat
;######### PVT CONFIG ############
PVT.implementation=RTKLIB_PVT
PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic
PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX
PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad
PVT.output_rate_ms=100
PVT.display_rate_ms=500
PVT.dump_filename=./PVT
PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea;
PVT.flag_nmea_tty_port=false;
PVT.nmea_dump_devname=/dev/pts/4
PVT.flag_rtcm_server=false
PVT.flag_rtcm_tty_port=false
PVT.rtcm_dump_devname=/dev/pts/1
PVT.dump=true

61
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc
View File

@ -364,8 +364,7 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels,
d_rinexobs_rate_ms = rinexobs_rate_ms;
d_rinexnav_rate_ms = rinexnav_rate_ms;
d_dump_filename.append("_raw.dat");
dump_ls_pvt_filename.append("_ls_pvt.dat");
dump_ls_pvt_filename.append("_pvt.dat");
d_ls_pvt = std::make_shared<rtklib_solver>(static_cast<int32_t>(nchannels), dump_ls_pvt_filename, d_dump, rtk);
d_ls_pvt->set_averaging_depth(1);
@ -374,23 +373,6 @@ rtklib_pvt_cc::rtklib_pvt_cc(uint32_t nchannels,
d_last_status_print_seg = 0;
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "PVT dump enabled Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception opening PVT dump file " << e.what();
}
}
}
// Create Sys V message queue
first_fix = true;
@ -500,18 +482,6 @@ rtklib_pvt_cc::~rtklib_pvt_cc()
{
LOG(WARNING) << "Failed to save GLONASS GNAV Ephemeris, map is empty";
}
if (d_dump_file.is_open() == true)
{
try
{
d_dump_file.close();
}
catch (const std::exception& ex)
{
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
}
}
}
@ -2102,7 +2072,7 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
<< std::fixed << std::setprecision(3)
<< " [deg], Height = " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl;
std::cout << std::setprecision(ss);
LOG(INFO) << "RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]";
DLOG(INFO) << "RX clock offset: " << d_ls_pvt->get_time_offset_s() << "[s]";
// boost::posix_time::ptime p_time;
// gtime_t rtklib_utc_time = gpst2time(adjgpsweek(d_ls_pvt->gps_ephemeris_map.cbegin()->second.i_GPS_week), d_rx_time);
@ -2110,36 +2080,15 @@ int rtklib_pvt_cc::work(int noutput_items, gr_vector_const_void_star& input_item
// p_time += boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6));
// std::cout << TEXT_MAGENTA << "Observable RX time (GPST) " << boost::posix_time::to_simple_string(p_time) << TEXT_RESET << std::endl;
LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
<< " [deg], Height = " << d_ls_pvt->get_height() << " [m]";
DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude()
<< " [deg], Height = " << d_ls_pvt->get_height() << " [m]";
/* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time())
<< " UTC using "<< d_ls_pvt->get_num_valid_observations() <<" observations is HDOP = " << d_ls_pvt->get_hdop() << " VDOP = "
<< d_ls_pvt->get_vdop()
<< " GDOP = " << d_ls_pvt->get_gdop() << std::endl; */
}
// MULTIPLEXED FILE RECORDING - Record results to file
if (d_dump == true)
{
try
{
double tmp_double;
for (uint32_t i = 0; i < d_nchannels; i++)
{
tmp_double = in[i][epoch].Pseudorange_m;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
tmp_double = 0;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
d_dump_file.write(reinterpret_cast<char*>(&d_rx_time), sizeof(double));
}
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
}
}
}

1
src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h
View File

@ -122,7 +122,6 @@ private:
uint32_t d_nchannels;
std::string d_dump_filename;
std::ofstream d_dump_file;
int32_t d_output_rate_ms;
int32_t d_display_rate_ms;

5
src/algorithms/PVT/libs/gpx_printer.cc
View File

@ -123,8 +123,9 @@ bool Gpx_Printer::print_position(const std::shared_ptr<rtklib_solver>& position,
double vdop = position_->get_vdop();
double pdop = position_->get_pdop();
std::string utc_time = to_iso_extended_string(position_->get_position_UTC_time());
utc_time.resize(23); // time up to ms
utc_time.append("Z"); // UTC time zone
if (utc_time.length() < 23) utc_time += ".";
utc_time.resize(23, '0'); // time up to ms
utc_time.append("Z"); // UTC time zone
if (print_average_values == false)
{

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

@ -86,7 +86,7 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception opening PVT lib dump file " << e.what();
LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what();
}
}
}
@ -103,7 +103,7 @@ rtklib_solver::~rtklib_solver()
}
catch (const std::exception& ex)
{
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
LOG(WARNING) << "Exception in destructor closing the RTKLIB dump file " << ex.what();
}
}
}
@ -556,34 +556,55 @@ bool rtklib_solver::get_PVT(const std::map<int, Gnss_Synchro>& gnss_observables_
try
{
double tmp_double;
uint32_t tmp_uint32;
// TOW
tmp_uint32 = gnss_observables_map.begin()->second.TOW_at_current_symbol_ms;
d_dump_file.write(reinterpret_cast<char*>(&tmp_uint32), sizeof(uint32_t));
// WEEK
tmp_uint32 = adjgpsweek(nav_data.eph[0].week);
d_dump_file.write(reinterpret_cast<char*>(&tmp_uint32), sizeof(uint32_t));
// PVT GPS time
tmp_double = gnss_observables_map.begin()->second.RX_time;
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF User Position East [m]
tmp_double = rx_position_and_time(0);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF User Position North [m]
tmp_double = rx_position_and_time(1);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF User Position Up [m]
tmp_double = rx_position_and_time(2);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// User clock offset [s]
tmp_double = rx_position_and_time(3);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double)
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.rr[0]), sizeof(pvt_sol.rr));
// position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double)
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.qr[0]), sizeof(pvt_sol.qr));
// GEO user position Latitude [deg]
tmp_double = this->get_latitude();
tmp_double = get_latitude();
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// GEO user position Longitude [deg]
tmp_double = this->get_longitude();
tmp_double = get_longitude();
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// GEO user position Height [m]
tmp_double = this->get_height();
tmp_double = get_height();
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
// NUMBER OF VALID SATS
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.ns), sizeof(uint8_t));
// RTKLIB solution status
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.stat), sizeof(uint8_t));
// RTKLIB solution type (0:xyz-ecef,1:enu-baseline)
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.type), sizeof(uint8_t));
//AR ratio factor for validation
tmp_double = pvt_sol.ratio;
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.ratio), sizeof(float));
//AR ratio threshold for validation
tmp_double = pvt_sol.thres;
d_dump_file.write(reinterpret_cast<char*>(&pvt_sol.thres), sizeof(float));
//GDOP//PDOP//HDOP//VDOP
d_dump_file.write(reinterpret_cast<char*>(&dop_[0]), sizeof(dop_));
}
catch (const std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing PVT LS dump file " << e.what();
LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what();
}
}
}

11
src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc
View File

@ -280,7 +280,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init()
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_doppler_step = 0U;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_mag = 0.0;
d_input_power = 0.0;
const double GALILEO_TWO_PI = 6.283185307179600;
@ -328,7 +329,8 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::set_state(int state)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -376,7 +378,8 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
//restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -633,7 +636,7 @@ int galileo_e5a_noncoherentIQ_acquisition_caf_cc::general_work(int noutput_items
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
// 5- Compute the test statistics and compare to the threshold
d_test_statistics = d_mag / d_input_power;
}

11
src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc
View File

@ -151,10 +151,10 @@ void galileo_pcps_8ms_acquisition_cc::init()
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
d_gnss_synchro->Acq_doppler_step = 0U;
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_mag = 0.0;
d_input_power = 0.0;
const double GALILEO_TWO_PI = 6.283185307179600;
@ -188,7 +188,8 @@ void galileo_pcps_8ms_acquisition_cc::set_state(int state)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -219,7 +220,8 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
//restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -328,6 +330,7 @@ int galileo_pcps_8ms_acquisition_cc::general_work(int noutput_items,
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
}
// Record results to file if required

5
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@ -261,7 +261,7 @@ void pcps_acquisition::init()
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
d_gnss_synchro->Acq_doppler_step = 0U;
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
@ -334,6 +334,7 @@ void pcps_acquisition::set_state(int32_t state)
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;
@ -725,6 +726,7 @@ void pcps_acquisition::acquisition_core(uint64_t samp_count)
d_gnss_synchro->Acq_delay_samples = static_cast<double>(std::fmod(static_cast<float>(indext), acq_parameters.samples_per_code));
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samp_count;
d_gnss_synchro->Acq_doppler_step = acq_parameters.doppler_step2;
}
lk.lock();
@ -865,6 +867,7 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)),
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;

8
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc
View File

@ -180,10 +180,10 @@ void pcps_acquisition_fine_doppler_cc::init()
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
d_gnss_synchro->Acq_doppler_step = 0U;
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_state = 0;
}
@ -295,6 +295,7 @@ double pcps_acquisition_fine_doppler_cc::compute_CAF()
d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step - d_config_doppler_max);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
return d_test_statistics;
}
@ -461,7 +462,8 @@ void pcps_acquisition_fine_doppler_cc::set_state(int state)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_test_statistics = 0.0;
d_active = true;

5
src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc
View File

@ -150,10 +150,10 @@ void pcps_assisted_acquisition_cc::init()
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
d_gnss_synchro->Acq_doppler_step = 0U;
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_input_power = 0.0;
d_state = 0;
@ -279,6 +279,7 @@ double pcps_assisted_acquisition_cc::search_maximum()
d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step + d_doppler_min);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
// Record results to file if required
if (d_dump)

11
src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc
View File

@ -165,10 +165,10 @@ void pcps_cccwsr_acquisition_cc::init()
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
d_gnss_synchro->Acq_doppler_step = 0U;
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_mag = 0.0;
d_input_power = 0.0;
@ -203,7 +203,8 @@ void pcps_cccwsr_acquisition_cc::set_state(int state)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -234,7 +235,8 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
//restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -354,6 +356,7 @@ int pcps_cccwsr_acquisition_cc::general_work(int noutput_items,
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
}
// Record results to file if required

12
src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc
View File

@ -290,10 +290,10 @@ void pcps_opencl_acquisition_cc::init()
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
d_gnss_synchro->Acq_doppler_step = 0U;
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_mag = 0.0;
d_input_power = 0.0;
@ -450,6 +450,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_volk()
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
// 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
@ -613,6 +614,7 @@ void pcps_opencl_acquisition_cc::acquisition_core_opencl()
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samplestamp;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
// 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
@ -676,7 +678,8 @@ void pcps_opencl_acquisition_cc::set_state(int state)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -708,7 +711,8 @@ int pcps_opencl_acquisition_cc::general_work(int noutput_items,
//restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;

10
src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc
View File

@ -199,7 +199,8 @@ void pcps_quicksync_acquisition_cc::init()
//DLOG(INFO) << "START init";
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_mag = 0.0;
d_input_power = 0.0;
@ -236,7 +237,8 @@ void pcps_quicksync_acquisition_cc::set_state(int state)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -279,7 +281,8 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
//restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
@ -456,6 +459,7 @@ int pcps_quicksync_acquisition_cc::general_work(int noutput_items,
d_gnss_synchro->Acq_delay_samples = static_cast<double>(d_possible_delay[indext]);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
/* 5- Compute the test statistics and compare to the threshold d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;*/
d_test_statistics = d_mag / d_input_power;

11
src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc
View File

@ -166,10 +166,10 @@ void pcps_tong_acquisition_cc::init()
d_gnss_synchro->Flag_valid_symbol_output = false;
d_gnss_synchro->Flag_valid_pseudorange = false;
d_gnss_synchro->Flag_valid_word = false;
d_gnss_synchro->Acq_doppler_step = 0U;
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_mag = 0.0;
d_input_power = 0.0;
@ -211,7 +211,8 @@ void pcps_tong_acquisition_cc::set_state(int state)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_dwell_count = 0;
d_tong_count = d_tong_init_val;
d_mag = 0.0;
@ -250,7 +251,8 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
//restart acquisition variables
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0ULL;
d_gnss_synchro->Acq_doppler_step = 0U;
d_dwell_count = 0;
d_tong_count = d_tong_init_val;
d_mag = 0.0;
@ -345,6 +347,7 @@ int pcps_tong_acquisition_cc::general_work(int noutput_items,
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % d_samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
d_gnss_synchro->Acq_doppler_step = d_doppler_step;
}
// Record results to file if required

5
src/algorithms/libs/rtklib/rtklib.h
View File

@ -71,7 +71,6 @@
#define socket_t int
#define closesocket close
#define lock_t pthread_mutex_t
#define thread_t pthread_t
#define initlock(f) pthread_mutex_init(f, NULL)
#define rtk_lock(f) pthread_mutex_lock(f)
#define rtk_unlock(f) pthread_mutex_unlock(f)
@ -1211,7 +1210,7 @@ typedef struct
char local[1024]; /* local file path */
int topts[4]; /* time options {poff,tint,toff,tretry} (s) */
gtime_t tnext; /* next retry time (gpst) */
thread_t thread; /* download thread */
pthread_t thread; /* download thread */
} ftp_t;
@ -1284,7 +1283,7 @@ typedef struct
stream_t stream[8]; /* streams {rov,base,corr,sol1,sol2,logr,logb,logc} */
stream_t *moni; /* monitor stream */
unsigned int tick; /* start tick */
thread_t thread; /* server thread */
pthread_t thread; /* server thread */
int cputime; /* CPU time (ms) for a processing cycle */
int prcout; /* missing observation data count */
lock_t lock; /* lock flag */

32
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

@ -89,38 +89,6 @@ double leaps[MAXLEAPS + 1][7] = {/* leap seconds (y,m,d,h,m,s,utc-gpst) */
{}};
const prcopt_t prcopt_default = { /* defaults processing options */
PMODE_SINGLE, 0, 2, SYS_GPS, /* mode, soltype, nf, navsys */
15.0 * D2R, {{}, {{}, {}}}, /* elmin, snrmask */
0, 1, 1, 1, /* sateph, modear, glomodear, bdsmodear */
5, 0, 10, 1, /* maxout, minlock, minfix, armaxiter */
0, 0, 0, 0, /* estion, esttrop, dynamics, tidecorr */
1, 0, 0, 0, 0, /* niter, codesmooth, intpref, sbascorr, sbassatsel */
0, 0, /* rovpos, refpos */
{100.0, 100.0, 100.0}, /* eratio[] */
{100.0, 0.003, 0.003, 0.0, 1.0}, /* err[] */
{30.0, 0.03, 0.3}, /* std[] */
{1e-4, 1e-3, 1e-4, 1e-1, 1e-2, 0.0}, /* prn[] */
5E-12, /* sclkstab */
{3.0, 0.9999, 0.25, 0.1, 0.05, 0, 0, 0}, /* thresar */
0.0, 0.0, 0.05, /* elmaskar, almaskhold, thresslip */
30.0, 30.0, 30.0, /* maxtdif, maxinno, maxgdop */
{}, {}, {}, /* baseline, ru, rb */
{"", ""}, /* anttype */
{}, {}, {}, /* antdel, pcv, exsats */
0, 0, 0, {"", ""}, {}, 0, {{}, {}}, {{}, {{}, {}}, {{}, {}}, {}, {}}, 0, {}};
const solopt_t solopt_default = {
/* defaults solution output options */
SOLF_LLH, TIMES_GPST, 1, 3, /* posf, times, timef, timeu */
0, 1, 0, 0, 0, 0, /* degf, outhead, outopt, datum, height, geoid */
0, 0, 0, /* solstatic, sstat, trace */
{0.0, 0.0}, /* nmeaintv */
" ", "", 0 /* separator/program name */
};
const char *formatstrs[32] = {/* stream format strings */
"RTCM 2", /* 0 */
"RTCM 3", /* 1 */

40
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h → src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f.h
View File

@ -1,6 +1,6 @@
/*!
* \file volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h
* \brief VOLK_GNSSSDR puppet for the multiple 32-bit float vector fast resampler kernel.
* \file volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f.h
* \brief VOLK_GNSSSDR puppet for the multiple 32-bit float vector high dynamics resampler kernel.
* \authors <ul>
* <li> Cillian O'Driscoll 2017 cillian.odriscoll at gmail dot com
* <li> Javier Arribas, 2018. javiarribas(at)gmail.com
@ -33,10 +33,10 @@
* -------------------------------------------------------------------------
*/
#ifndef INCLUDED_volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_H
#define INCLUDED_volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_H
#ifndef INCLUDED_volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_H
#define INCLUDED_volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_H
#include "volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h"
#include "volk_gnsssdr/volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn.h"
#include <volk_gnsssdr/volk_gnsssdr_malloc.h>
#include <volk_gnsssdr/volk_gnsssdr_complex.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
@ -44,7 +44,7 @@
#ifdef LV_HAVE_GENERIC
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_generic(float* result, const float* local_code, unsigned int num_points)
static inline void volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_generic(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
@ -60,7 +60,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_generic(float* re
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_generic(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_generic(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
@ -75,7 +75,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_generic(float* re
#ifdef LV_HAVE_SSE3
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse3(float* result, const float* local_code, unsigned int num_points)
static inline void volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_a_sse3(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
@ -91,7 +91,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse3(float* res
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse3(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_a_sse3(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
@ -106,7 +106,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse3(float* res
#ifdef LV_HAVE_SSE3
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse3(float* result, const float* local_code, unsigned int num_points)
static inline void volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_u_sse3(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
@ -122,7 +122,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse3(float* res
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse3(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_u_sse3(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
@ -137,7 +137,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse3(float* res
#ifdef LV_HAVE_SSE4_1
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse4_1(float* result, const float* local_code, unsigned int num_points)
static inline void volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_u_sse4_1(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
@ -153,7 +153,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse4_1(float* r
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse4_1(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_u_sse4_1(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
@ -168,7 +168,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_sse4_1(float* r
#ifdef LV_HAVE_SSE4_1
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse4_1(float* result, const float* local_code, unsigned int num_points)
static inline void volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_a_sse4_1(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
@ -184,7 +184,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse4_1(float* r
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse4_1(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_a_sse4_1(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
@ -199,7 +199,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_sse4_1(float* r
#ifdef LV_HAVE_AVX
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_avx(float* result, const float* local_code, unsigned int num_points)
static inline void volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_a_avx(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
@ -215,7 +215,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_avx(float* resu
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_a_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
@ -229,7 +229,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_a_avx(float* resu
#ifdef LV_HAVE_AVX
static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_avx(float* result, const float* local_code, unsigned int num_points)
static inline void volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f_u_avx(float* result, const float* local_code, unsigned int num_points)
{
int code_length_chips = 2046;
float code_phase_step_chips = ((float)(code_length_chips) + 0.1) / ((float)num_points);
@ -245,7 +245,7 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_avx(float* resu
result_aux[n] = (float*)volk_gnsssdr_malloc(sizeof(float) * num_points, volk_gnsssdr_get_alignment());
}
volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_u_avx(result_aux, local_code, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips, shifts_chips, code_length_chips, num_out_vectors, num_points);
memcpy((float*)result, (float*)result_aux[0], sizeof(float) * num_points);
@ -285,4 +285,4 @@ static inline void volk_gnsssdr_32f_fast_resamplerxnpuppet_32f_u_avx(float* resu
//}
//#endif
#endif // INCLUDED_volk_gnsssdr_32f_fast_resamplerpuppet_32f_H
#endif // INCLUDED_volk_gnsssdr_32f_high_dynamics_resamplerpuppet_32f_H

28
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h → src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn.h
View File

@ -1,5 +1,5 @@
/*!
* \file volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h
* \file volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn.h
* \brief VOLK_GNSSSDR kernel: Resamples 1 complex 32-bit float vectors using zero hold resample algorithm
* and produces the delayed replicas by copying and rotating the resulting resampled signal.
* \authors <ul>
@ -38,7 +38,7 @@
*/
/*!
* \page volk_gnsssdr_32f_xn_fast_resampler_32f_xn
* \page volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn
*
* \b Overview
*
@ -46,7 +46,7 @@
*
* <b>Dispatcher Prototype</b>
* \code
* void volk_gnsssdr_32f_xn_fast_resampler_32f_xn(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
* void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
* \endcode
*
* \b Inputs
@ -64,8 +64,8 @@
*
*/
#ifndef INCLUDED_volk_gnsssdr_32f_xn_fast_resampler_32f_xn_H
#define INCLUDED_volk_gnsssdr_32f_xn_fast_resampler_32f_xn_H
#ifndef INCLUDED_volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_H
#define INCLUDED_volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_H
#include <assert.h>
#include <math.h>
@ -78,7 +78,7 @@
#ifdef LV_HAVE_GENERIC
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_generic(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
static inline void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_generic(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
int local_code_chip_index;
int current_correlator_tap;
@ -109,7 +109,7 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_generic(float** res
#ifdef LV_HAVE_SSE3
#include <pmmintrin.h>
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse3(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
static inline void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_a_sse3(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
float** _result = result;
const unsigned int quarterPoints = num_points / 4;
@ -194,7 +194,7 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse3(float** resu
#ifdef LV_HAVE_SSE3
#include <pmmintrin.h>
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse3(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
static inline void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_u_sse3(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
float** _result = result;
const unsigned int quarterPoints = num_points / 4;
@ -280,7 +280,7 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse3(float** resu
#ifdef LV_HAVE_SSE4_1
#include <smmintrin.h>
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse4_1(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
static inline void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_a_sse4_1(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
float** _result = result;
const unsigned int quarterPoints = num_points / 4;
@ -362,7 +362,7 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_sse4_1(float** re
#ifdef LV_HAVE_SSE4_1
#include <smmintrin.h>
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse4_1(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
static inline void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_u_sse4_1(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
float** _result = result;
const unsigned int quarterPoints = num_points / 4;
@ -444,7 +444,7 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_sse4_1(float** re
#ifdef LV_HAVE_AVX
#include <immintrin.h>
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
static inline void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_a_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
float** _result = result;
const unsigned int avx_iters = num_points / 8;
@ -532,7 +532,7 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_a_avx(float** resul
#ifdef LV_HAVE_AVX
#include <immintrin.h>
static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
static inline void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_u_avx(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
{
float** _result = result;
const unsigned int avx_iters = num_points / 8;
@ -621,7 +621,7 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_avx(float** resul
//#ifdef LV_HAVE_NEONV7
//#include <arm_neon.h>
//
//static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_neon(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
//static inline void volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_neon(float** result, const float* local_code, float rem_code_phase_chips, float code_phase_step_chips, float* shifts_chips, unsigned int code_length_chips, int num_out_vectors, unsigned int num_points)
//{
// float** _result = result;
// const unsigned int neon_iters = num_points / 4;
@ -704,4 +704,4 @@ static inline void volk_gnsssdr_32f_xn_fast_resampler_32f_xn_u_avx(float** resul
//
//#endif
#endif /*INCLUDED_volk_gnsssdr_32f_xn_fast_resampler_32f_xn_H*/
#endif /*INCLUDED_volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn_H*/

2
src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h
View File

@ -93,7 +93,7 @@ std::vector<volk_gnsssdr_test_case_t> init_test_list(volk_gnsssdr_test_params_t
QA(VOLK_INIT_PUPP(volk_gnsssdr_16i_resamplerxnpuppet_16i, volk_gnsssdr_16i_xn_resampler_16i_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_32fc_resamplerxnpuppet_32fc, volk_gnsssdr_32fc_xn_resampler_32fc_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_32f_resamplerxnpuppet_32f, volk_gnsssdr_32f_xn_resampler_32f_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_32f_fast_resamplerxnpuppet_32f, volk_gnsssdr_32f_xn_fast_resampler_32f_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f, volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_16ic_x2_dotprodxnpuppet_16ic, volk_gnsssdr_16ic_x2_dot_prod_16ic_xn, test_params))
QA(VOLK_INIT_PUPP(volk_gnsssdr_16ic_x2_rotator_dotprodxnpuppet_16ic, volk_gnsssdr_16ic_x2_rotator_dot_prod_16ic_xn, test_params_int16))
QA(VOLK_INIT_PUPP(volk_gnsssdr_16ic_16i_rotator_dotprodxnpuppet_16ic, volk_gnsssdr_16ic_16i_rotator_dot_prod_16ic_xn, test_params_int16))

2
src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc
View File

@ -55,7 +55,7 @@ GalileoE1BTelemetryDecoder::GalileoE1BTelemetryDecoder(ConfigurationInterface* c
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
// make telemetry decoder object
telemetry_decoder_ = galileo_e1b_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me
telemetry_decoder_ = galileo_make_telemetry_decoder_cc(satellite_, 1, dump_); //unified galileo decoder set to INAV (frame_type=1)
DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")";
channel_ = 0;
if (in_streams_ > 1)

5
src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h
View File

@ -36,7 +36,7 @@
#include "telemetry_decoder_interface.h"
#include "galileo_e1b_telemetry_decoder_cc.h"
#include "galileo_telemetry_decoder_cc.h"
#include "gnss_satellite.h"
#include <string>
@ -76,7 +76,6 @@ public:
void set_satellite(const Gnss_Satellite& satellite) override;
inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); }
inline void reset() override
{
return;
@ -88,7 +87,7 @@ public:
}
private:
galileo_e1b_telemetry_decoder_cc_sptr telemetry_decoder_;
galileo_telemetry_decoder_cc_sptr telemetry_decoder_;
Gnss_Satellite satellite_;
int channel_;
bool dump_;

2
src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.cc
View File

@ -58,7 +58,7 @@ GalileoE5aTelemetryDecoder::GalileoE5aTelemetryDecoder(ConfigurationInterface* c
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
// make telemetry decoder object
telemetry_decoder_ = galileo_e5a_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me
telemetry_decoder_ = galileo_make_telemetry_decoder_cc(satellite_, 2, dump_); //unified galileo decoder set to FNAV (frame_type=2)
DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")";
channel_ = 0;
if (in_streams_ > 1)

5
src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h
View File

@ -37,7 +37,7 @@
#ifndef GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_H_
#define GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_H_
#include "galileo_e5a_telemetry_decoder_cc.h"
#include "galileo_telemetry_decoder_cc.h"
#include "telemetry_decoder_interface.h"
#include <string>
@ -76,7 +76,6 @@ public:
void set_satellite(const Gnss_Satellite& satellite) override;
inline void set_channel(int channel) override { telemetry_decoder_->set_channel(channel); }
inline void reset() override
{
return;
@ -88,7 +87,7 @@ public:
}
private:
galileo_e5a_telemetry_decoder_cc_sptr telemetry_decoder_;
galileo_telemetry_decoder_cc_sptr telemetry_decoder_;
Gnss_Satellite satellite_;
int channel_;
bool dump_;

3
src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt
View File

@ -20,11 +20,10 @@ set(TELEMETRY_DECODER_GR_BLOCKS_SOURCES
gps_l1_ca_telemetry_decoder_cc.cc
gps_l2c_telemetry_decoder_cc.cc
gps_l5_telemetry_decoder_cc.cc
galileo_e1b_telemetry_decoder_cc.cc
sbas_l1_telemetry_decoder_cc.cc
galileo_e5a_telemetry_decoder_cc.cc
glonass_l1_ca_telemetry_decoder_cc.cc
glonass_l2_ca_telemetry_decoder_cc.cc
galileo_telemetry_decoder_cc.cc
)
include_directories(

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

@ -1,501 +0,0 @@
/*!
* \file galileo_e1b_telemetry_decoder_cc.cc
* \brief Implementation of a Galileo INAV message demodulator block
* \author Mara Branzanti 2013. mara.branzanti(at)gmail.com
* \author Javier Arribas 2013. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "galileo_e1b_telemetry_decoder_cc.h"
#include "control_message_factory.h"
#include "convolutional.h"
#include "gnss_synchro.h"
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <iostream>
#define CRC_ERROR_LIMIT 6
using google::LogMessage;
galileo_e1b_telemetry_decoder_cc_sptr
galileo_e1b_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump)
{
return galileo_e1b_telemetry_decoder_cc_sptr(new galileo_e1b_telemetry_decoder_cc(satellite, dump));
}
void galileo_e1b_telemetry_decoder_cc::viterbi_decoder(double *page_part_symbols, int32_t *page_part_bits)
{
Viterbi(page_part_bits, out0, state0, out1, state1,
page_part_symbols, KK, nn, DataLength);
}
void galileo_e1b_telemetry_decoder_cc::deinterleaver(int32_t rows, int32_t cols, double *in, double *out)
{
for (int32_t r = 0; r < rows; r++)
{
for (int32_t c = 0; c < cols; c++)
{
out[c * rows + r] = in[r * cols + c];
}
}
}
galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc(
const Gnss_Satellite &satellite,
bool dump) : 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)))
{
// Ephemeris data port out
this->message_port_register_out(pmt::mp("telemetry"));
// initialize internal vars
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "Initializing GALILEO E1B TELEMETRY PROCESSING";
d_samples_per_symbol = (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS) / Galileo_E1_B_SYMBOL_RATE_BPS;
// set the preamble
uint16_t preambles_bits[GALILEO_INAV_PREAMBLE_LENGTH_BITS] = GALILEO_INAV_PREAMBLE;
d_symbols_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol;
memcpy(static_cast<uint16_t *>(this->d_preambles_bits), static_cast<uint16_t *>(preambles_bits), GALILEO_INAV_PREAMBLE_LENGTH_BITS * sizeof(uint16_t));
// preamble bits to sampled symbols
d_preambles_symbols = static_cast<int32_t *>(volk_gnsssdr_malloc(d_symbols_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
int32_t n = 0;
for (int32_t i = 0; i < GALILEO_INAV_PREAMBLE_LENGTH_BITS; i++)
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
if (d_preambles_bits[i] == 1)
{
d_preambles_symbols[n] = 1;
}
else
{
d_preambles_symbols[n] = -1;
}
n++;
}
}
d_sample_counter = 0ULL;
d_stat = 0;
d_preamble_index = 0ULL;
d_flag_frame_sync = false;
d_flag_parity = false;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
delta_t = 0;
d_CRC_error_counter = 0;
flag_even_word_arrived = 0;
d_flag_preamble = false;
d_channel = 0;
flag_TOW_set = false;
// vars for Viterbi decoder
int32_t max_states = 1 << mm; // 2^mm
g_encoder[0] = 121; // Polynomial G1
g_encoder[1] = 91; // Polynomial G2
out0 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
out1 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
state0 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
state1 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
// create appropriate transition matrices
nsc_transit(out0, state0, 0, g_encoder, KK, nn);
nsc_transit(out1, state1, 1, g_encoder, KK, nn);
}
galileo_e1b_telemetry_decoder_cc::~galileo_e1b_telemetry_decoder_cc()
{
volk_gnsssdr_free(d_preambles_symbols);
volk_gnsssdr_free(out0);
volk_gnsssdr_free(out1);
volk_gnsssdr_free(state0);
volk_gnsssdr_free(state1);
if (d_dump_file.is_open() == true)
{
try
{
d_dump_file.close();
}
catch (const std::exception &ex)
{
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
}
}
}
void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols, int32_t frame_length)
{
// 1. De-interleave
double *page_part_symbols_deint = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
deinterleaver(GALILEO_INAV_INTERLEAVER_ROWS, GALILEO_INAV_INTERLEAVER_COLS, page_part_symbols, page_part_symbols_deint);
// 2. Viterbi decoder
// 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder)
// 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180º
for (int32_t i = 0; i < frame_length; i++)
{
if ((i + 1) % 2 == 0)
{
page_part_symbols_deint[i] = -page_part_symbols_deint[i];
}
}
int32_t *page_part_bits = static_cast<int32_t *>(volk_gnsssdr_malloc((frame_length / 2) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
viterbi_decoder(page_part_symbols_deint, page_part_bits);
volk_gnsssdr_free(page_part_symbols_deint);
// 3. Call the Galileo page decoder
std::string page_String;
for (int32_t i = 0; i < (frame_length / 2); i++)
{
if (page_part_bits[i] > 0)
{
page_String.push_back('1');
}
else
{
page_String.push_back('0');
}
}
if (page_part_bits[0] == 1)
{
// DECODE COMPLETE WORD (even + odd) and TEST CRC
d_nav.split_page(page_String, flag_even_word_arrived);
if (d_nav.flag_CRC_test == true)
{
LOG(INFO) << "Galileo E1 CRC correct in channel " << d_channel << " from satellite " << d_satellite;
//std::cout << "Galileo E1 CRC correct on channel " << d_channel << " from satellite " << d_satellite << std::endl;
}
else
{
std::cout << "Galileo E1 CRC error in channel " << d_channel << " from satellite " << d_satellite << std::endl;
LOG(INFO) << "Galileo E1 CRC error in channel " << d_channel << " from satellite " << d_satellite;
}
flag_even_word_arrived = 0;
}
else
{
// STORE HALF WORD (even page)
d_nav.split_page(page_String.c_str(), flag_even_word_arrived);
flag_even_word_arrived = 1;
}
volk_gnsssdr_free(page_part_bits);
// 4. Push the new navigation data to the queues
if (d_nav.have_new_ephemeris() == true)
{
// get object for this SV (mandatory)
std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_nav.get_ephemeris());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_nav.have_new_iono_and_GST() == true)
{
// get object for this SV (mandatory)
std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(d_nav.get_iono());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_nav.have_new_utc_model() == true)
{
// get object for this SV (mandatory)
std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_nav.get_utc_model());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_nav.have_new_almanac() == true)
{
std::shared_ptr<Galileo_Almanac> tmp_obj = std::make_shared<Galileo_Almanac>(d_nav.get_almanac());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
//debug
std::cout << "Galileo E1 I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << std::endl;
DLOG(INFO) << "GPS_to_Galileo time conversion:";
DLOG(INFO) << "A0G=" << tmp_obj->A_0G_10;
DLOG(INFO) << "A1G=" << tmp_obj->A_1G_10;
DLOG(INFO) << "T0G=" << tmp_obj->t_0G_10;
DLOG(INFO) << "WN_0G_10=" << tmp_obj->WN_0G_10;
DLOG(INFO) << "Current parameters:";
DLOG(INFO) << "d_TOW_at_current_symbol_ms=" << d_TOW_at_current_symbol_ms;
DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0;
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]";
}
}
void galileo_e1b_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite)
{
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
}
void galileo_e1b_telemetry_decoder_cc::set_channel(int32_t channel)
{
d_channel = channel;
LOG(INFO) << "Navigation channel set to " << channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename = "telemetry";
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
}
}
}
}
int galileo_e1b_telemetry_decoder_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
int32_t corr_value = 0;
int32_t preamble_diff = 0;
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); // Get the output buffer pointer
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]); // Get the input buffer pointer
Gnss_Synchro current_symbol; // structure to save the synchronization information and send the output object to the next block
// 1. Copy the current tracking output
current_symbol = in[0][0];
d_symbol_history.push_back(current_symbol); // add new symbol to the symbol queue
d_sample_counter++; // count for the processed samples
consume_each(1);
d_flag_preamble = false;
uint32_t required_symbols = static_cast<uint32_t>(GALILEO_INAV_PAGE_SYMBOLS) + static_cast<uint32_t>(d_symbols_per_preamble);
if (d_symbol_history.size() > required_symbols)
{
// TODO Optimize me!
// ******* preamble correlation ********
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
{
if (d_symbol_history.at(i).Prompt_I < 0) // symbols clipping
{
corr_value -= d_preambles_symbols[i];
}
else
{
corr_value += d_preambles_symbols[i];
}
}
}
// ******* frame sync ******************
if (d_stat == 0) // no preamble information
{
if (abs(corr_value) >= d_symbols_per_preamble)
{
d_preamble_index = d_sample_counter; // record the preamble sample stamp
LOG(INFO) << "Preamble detection for Galileo satellite " << this->d_satellite;
d_stat = 1; // enter into frame pre-detection status
}
}
else if (d_stat == 1) // possible preamble lock
{
if (abs(corr_value) >= d_symbols_per_preamble)
{
// check preamble separation
preamble_diff = static_cast<int32_t>(d_sample_counter - d_preamble_index);
if (abs(preamble_diff - GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS) == 0)
{
// try to decode frame
LOG(INFO) << "Starting page decoder for Galileo satellite " << this->d_satellite;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
d_stat = 2;
}
else
{
if (preamble_diff > GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS)
{
d_stat = 0; // start again
}
}
}
}
else if (d_stat == 2)
{
if (d_sample_counter == d_preamble_index + static_cast<uint64_t>(GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS))
{
// NEW Galileo page part is received
// 0. fetch the symbols into an array
int32_t frame_length = GALILEO_INAV_PAGE_PART_SYMBOLS - d_symbols_per_preamble;
double *page_part_symbols = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
for (int32_t i = 0; i < frame_length; i++)
{
if (corr_value > 0)
{
page_part_symbols[i] = d_symbol_history.at(i + d_symbols_per_preamble).Prompt_I; // because last symbol of the preamble is just received now!
}
else
{
page_part_symbols[i] = -d_symbol_history.at(i + d_symbols_per_preamble).Prompt_I; // because last symbol of the preamble is just received now!
}
}
// call the decoder
decode_word(page_part_symbols, frame_length);
if (d_nav.flag_CRC_test == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
<< d_symbol_history.at(0).Tracking_sample_counter << " [samples]";
}
}
else
{
d_CRC_error_counter++;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
{
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
d_flag_frame_sync = false;
d_stat = 0;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
d_nav.flag_TOW_set = false;
}
}
volk_gnsssdr_free(page_part_symbols);
}
}
// UPDATE GNSS SYNCHRO DATA
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true)
// update TOW at the preamble instant
{
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
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.TOW_5 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS);
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)
{
// 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_Preamble_ms = static_cast<uint32_t>(d_nav.TOW_6 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS);
d_nav.flag_TOW_6 = false;
}
else
{
// this page has no timing information
d_TOW_at_current_symbol_ms += static_cast<uint32_t>(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
{
if (d_nav.flag_TOW_set == true)
{
d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E1_CODE_PERIOD_MS);
}
}
// 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
{
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)));
}
current_symbol.Flag_valid_word = d_nav.flag_TOW_set;
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// 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
if (d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
uint64_t tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
}
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
}
else
{
return 0;
}
}

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

@ -1,513 +0,0 @@
/*!
* \file galileo_e5a_telemetry_decoder_cc.cc
* \brief Implementation of a Galileo FNAV message demodulator block
* \author Marc Sales, 2014. marcsales92(at)gmail.com
* Javier Arribas, 2017. jarribas(at)cttc.es
* \based on work from:
* <ul>
* <li> Javier Arribas, 2011. jarribas(at)cttc.es
* </ul>
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "galileo_e5a_telemetry_decoder_cc.h"
#include "control_message_factory.h"
#include "convolutional.h"
#include "display.h"
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <cmath>
#include <iostream>
#define GALILEO_E5a_CRC_ERROR_LIMIT 6
using google::LogMessage;
galileo_e5a_telemetry_decoder_cc_sptr
galileo_e5a_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump)
{
return galileo_e5a_telemetry_decoder_cc_sptr(new galileo_e5a_telemetry_decoder_cc(satellite, dump));
}
void galileo_e5a_telemetry_decoder_cc::viterbi_decoder(double *page_part_symbols, int32_t *page_part_bits)
{
Viterbi(page_part_bits, out0, state0, out1, state1,
page_part_symbols, KK, nn, DataLength);
}
void galileo_e5a_telemetry_decoder_cc::deinterleaver(int32_t rows, int32_t cols, double *in, double *out)
{
for (int32_t r = 0; r < rows; r++)
{
for (int32_t c = 0; c < cols; c++)
{
out[c * rows + r] = in[r * cols + c];
}
}
}
void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols, int32_t frame_length)
{
// 1. De-interleave
double *page_symbols_deint = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
deinterleaver(GALILEO_FNAV_INTERLEAVER_ROWS, GALILEO_FNAV_INTERLEAVER_COLS, page_symbols, page_symbols_deint);
// 2. Viterbi decoder
// 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder)
// 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180<38>
for (int32_t i = 0; i < frame_length; i++)
{
if ((i + 1) % 2 == 0)
{
page_symbols_deint[i] = -page_symbols_deint[i];
}
}
int32_t *page_bits = static_cast<int32_t *>(volk_gnsssdr_malloc((frame_length / 2) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
viterbi_decoder(page_symbols_deint, page_bits);
volk_gnsssdr_free(page_symbols_deint);
// 3. Call the Galileo page decoder
std::string page_String;
for (int32_t i = 0; i < frame_length; i++)
{
if (page_bits[i] > 0)
{
page_String.push_back('1');
}
else
{
page_String.push_back('0');
}
}
volk_gnsssdr_free(page_bits);
// DECODE COMPLETE WORD (even + odd) and TEST CRC
d_nav.split_page(page_String);
if (d_nav.flag_CRC_test == true)
{
LOG(INFO) << "Galileo E5a CRC correct in channel " << d_channel << " from satellite " << d_satellite;
}
else
{
std::cout << "Galileo E5a CRC error in channel " << d_channel << " from satellite " << d_satellite << std::endl;
LOG(INFO) << "Galileo E5a CRC error in channel " << d_channel << " from satellite " << d_satellite;
}
// 4. Push the new navigation data to the queues
if (d_nav.have_new_ephemeris() == true)
{
std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_nav.get_ephemeris());
std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_nav.have_new_iono_and_GST() == true)
{
std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(d_nav.get_iono());
std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_nav.have_new_utc_model() == true)
{
std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_nav.get_utc_model());
std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
}
galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
const Gnss_Satellite &satellite, bool dump) : gr::block("galileo_e5a_telemetry_decoder_cc",
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
// Ephemeris data port out
this->message_port_register_out(pmt::mp("telemetry"));
// initialize internal vars
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
// set the preamble
for (int32_t i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
{
if (GALILEO_FNAV_PREAMBLE.at(i) == '0')
{
d_preambles_bits[i] = 1;
}
else
{
d_preambles_bits[i] = -1;
}
}
for (int32_t i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
{
for (int32_t k = 0; k < GALILEO_FNAV_CODES_PER_SYMBOL; k++)
{
d_preamble_samples[(i * GALILEO_FNAV_CODES_PER_SYMBOL) + k] = d_preambles_bits[i];
}
}
d_sample_counter = 0ULL;
d_stat = 0;
corr_value = 0;
d_flag_preamble = false;
d_preamble_index = 0ULL;
d_flag_frame_sync = false;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
flag_TOW_set = false;
d_CRC_error_counter = 0;
d_channel = 0;
delta_t = 0.0;
d_symbol_counter = 0;
d_prompt_acum = 0.0;
flag_bit_start = true;
new_symbol = false;
required_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE + GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
// vars for Viterbi decoder
int32_t max_states = 1 << mm; // 2^mm
g_encoder[0] = 121; // Polynomial G1
g_encoder[1] = 91; // Polynomial G2
out0 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
out1 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
state0 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
state1 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
// create appropriate transition matrices
nsc_transit(out0, state0, 0, g_encoder, KK, nn);
nsc_transit(out1, state1, 1, g_encoder, KK, nn);
}
galileo_e5a_telemetry_decoder_cc::~galileo_e5a_telemetry_decoder_cc()
{
volk_gnsssdr_free(out0);
volk_gnsssdr_free(out1);
volk_gnsssdr_free(state0);
volk_gnsssdr_free(state1);
if (d_dump_file.is_open() == true)
{
try
{
d_dump_file.close();
}
catch (const std::exception &ex)
{
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
}
}
}
void galileo_e5a_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite)
{
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
}
void galileo_e5a_telemetry_decoder_cc::set_channel(int32_t channel)
{
d_channel = channel;
LOG(INFO) << "Navigation channel set to " << channel;
// Enable data file logging
if (d_dump == true)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename = "telemetry";
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
}
}
}
}
int galileo_e5a_telemetry_decoder_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
int32_t preamble_diff = 0;
Gnss_Synchro *out = reinterpret_cast<Gnss_Synchro *>(output_items[0]); // Get the output buffer pointer
const Gnss_Synchro *in = reinterpret_cast<const Gnss_Synchro *>(input_items[0]); // Get the input buffer pointer
// 1. Copy the current tracking output
Gnss_Synchro current_sample = in[0];
d_symbol_counter++;
if (flag_bit_start)
{
d_prompt_acum += current_sample.Prompt_I;
if (d_symbol_counter == GALILEO_FNAV_CODES_PER_SYMBOL)
{
current_sample.Prompt_I = d_prompt_acum / static_cast<double>(GALILEO_FNAV_CODES_PER_SYMBOL);
d_symbol_history.push_back(current_sample); // add new symbol to the symbol queue
d_prompt_acum = 0.0;
d_symbol_counter = 0;
new_symbol = true;
}
}
else
{
if (current_sample.Prompt_I < 0.0)
{
d_preamble_init.push_back(1);
}
else
{
d_preamble_init.push_back(-1);
}
if (d_preamble_init.size() == GALILEO_FNAV_CODES_PER_PREAMBLE)
{
std::deque<int32_t>::iterator iter;
int32_t k = 0;
corr_value = 0;
for (iter = d_preamble_init.begin(); iter != d_preamble_init.end(); iter++)
{
corr_value += *iter * d_preamble_samples[k];
k++;
}
if (abs(corr_value) == GALILEO_FNAV_CODES_PER_PREAMBLE)
{
d_symbol_counter = 0;
flag_bit_start = true;
corr_value = 0;
d_preamble_init.clear();
d_symbol_history.clear();
LOG(INFO) << "Bit start sync for Galileo E5a satellite " << d_satellite;
}
else
{
d_preamble_init.pop_front();
}
}
}
d_sample_counter++; // count for the processed samples
consume_each(1);
d_flag_preamble = false;
if ((d_symbol_history.size() > required_symbols) && new_symbol)
{
// ****************** Preamble orrelation ******************
corr_value = 0;
for (int32_t i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
{
if (d_symbol_history.at(i).Prompt_I < 0.0) // symbols clipping
{
corr_value -= d_preambles_bits[i];
}
else
{
corr_value += d_preambles_bits[i];
}
}
}
// ****************** Frame sync ******************
if ((d_stat == 0) && new_symbol) // no preamble information
{
if (abs(corr_value) == GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
{
d_preamble_index = d_sample_counter; // record the preamble sample stamp
LOG(INFO) << "Preamble detection for Galileo E5a satellite " << d_satellite;
d_stat = 1; // enter into frame pre-detection status
}
}
else if ((d_stat == 1) && new_symbol) // possible preamble lock
{
if (abs(corr_value) == GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
{
// check preamble separation
preamble_diff = static_cast<int32_t>(d_sample_counter - d_preamble_index);
if (preamble_diff == GALILEO_FNAV_CODES_PER_PAGE)
{
// try to decode frame
LOG(INFO) << "Starting page decoder for Galileo E5a satellite " << d_satellite;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
d_stat = 2;
}
else if (preamble_diff > GALILEO_FNAV_CODES_PER_PAGE)
{
d_stat = 0; // start again
flag_bit_start = false;
LOG(INFO) << "Preamble diff = " << preamble_diff;
}
}
}
else if ((d_stat == 2) && new_symbol)
{
if (d_sample_counter == (d_preamble_index + static_cast<uint64_t>(GALILEO_FNAV_CODES_PER_PAGE)))
{
// NEW Galileo page part is received
// 0. fetch the symbols into an array
int32_t frame_length = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
double corr_sign = 0.0;
if (corr_value > 0)
{
corr_sign = -1.0;
}
else
{
corr_sign = 1.0;
}
for (int32_t i = 0; i < frame_length; i++)
{
page_symbols[i] = corr_sign * d_symbol_history.at(i + GALILEO_FNAV_PREAMBLE_LENGTH_BITS).Prompt_I; // because last symbol of the preamble is just received now!
}
// call the decoder
decode_word(page_symbols, frame_length);
if (d_nav.flag_CRC_test == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
<< d_symbol_history.at(0).Tracking_sample_counter << " [samples]";
}
}
else
{
d_CRC_error_counter++;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > GALILEO_E5A_CRC_ERROR_LIMIT)
{
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
d_flag_frame_sync = false;
d_stat = 0;
flag_bit_start = false;
d_nav.flag_TOW_set = false;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
}
}
}
}
new_symbol = false;
// UPDATE GNSS SYNCHRO DATA
// Add the telemetry decoder information
if (d_flag_preamble and d_nav.flag_TOW_set)
// update TOW at the preamble instant
// We expect a preamble each 10 seconds (FNAV page period)
{
if (d_nav.flag_TOW_1 == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.FNAV_TOW_1 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_nav.flag_TOW_1 = false;
}
else if (d_nav.flag_TOW_2 == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.FNAV_TOW_2 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_nav.flag_TOW_2 = false;
}
else if (d_nav.flag_TOW_3 == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.FNAV_TOW_3 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_nav.flag_TOW_3 = false;
}
else if (d_nav.flag_TOW_4 == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_nav.FNAV_TOW_4 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_nav.flag_TOW_4 = false;
}
else
{
d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E5a_CODE_PERIOD_MS);
}
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
if (d_nav.flag_TOW_set == true)
{
d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E5a_CODE_PERIOD_MS);
}
}
// remove used symbols from history
// 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.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
if (d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
uint64_t tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_sample.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what();
}
}
// 3. Make the output
out[0] = current_sample;
return 1;
}
else
{
return 0;
}
}

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

@ -1,127 +0,0 @@
/*!
* \file galileo_e5a_telemetry_decoder_cc.cc
* \brief Implementation of a Galileo FNAV message demodulator block
* \author Marc Sales, 2014. marcsales92(at)gmail.com
* Javier Arribas, 2017. jarribas(at)cttc.es
* \based on work from:
* <ul>
* <li> Javier Arribas, 2011. jarribas(at)cttc.es
* </ul>
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_
#define GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_
#include "Galileo_E5a.h"
#include "gnss_satellite.h"
#include "galileo_fnav_message.h"
#include "galileo_ephemeris.h"
#include "galileo_almanac.h"
#include "galileo_iono.h"
#include "galileo_utc_model.h"
#include "gnss_synchro.h"
#include <gnuradio/block.h>
#include <deque>
#include <fstream>
#include <string>
class galileo_e5a_telemetry_decoder_cc;
typedef boost::shared_ptr<galileo_e5a_telemetry_decoder_cc> galileo_e5a_telemetry_decoder_cc_sptr;
galileo_e5a_telemetry_decoder_cc_sptr galileo_e5a_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
/*!
* \brief This class implements a block that decodes the FNAV data defined in Galileo ICD
*
*/
class galileo_e5a_telemetry_decoder_cc : public gr::block
{
public:
~galileo_e5a_telemetry_decoder_cc();
void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN
void set_channel(int32_t channel); //!< Set receiver's channel
/*!
* \brief This is where all signal processing takes place
*/
int general_work(int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
private:
friend galileo_e5a_telemetry_decoder_cc_sptr
galileo_e5a_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
galileo_e5a_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
void viterbi_decoder(double *page_part_symbols, int32_t *page_part_bits);
void deinterleaver(int32_t rows, int32_t cols, double *in, double *out);
void decode_word(double *page_symbols, int32_t frame_length);
int32_t d_preambles_bits[GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
int32_t d_preamble_samples[GALILEO_FNAV_CODES_PER_PREAMBLE];
std::deque<int> d_preamble_init;
int32_t d_stat;
int32_t d_CRC_error_counter;
int32_t d_channel;
int32_t d_symbol_counter;
int32_t corr_value;
uint32_t required_symbols;
uint64_t d_sample_counter;
uint64_t d_preamble_index;
bool d_flag_frame_sync;
bool d_flag_preamble;
bool d_dump;
bool flag_TOW_set;
bool flag_bit_start;
bool new_symbol;
double d_prompt_acum;
double page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
uint32_t d_TOW_at_Preamble_ms;
uint32_t d_TOW_at_current_symbol_ms;
double delta_t; //GPS-GALILEO time offset
std::string d_dump_filename;
std::ofstream d_dump_file;
std::deque<Gnss_Synchro> d_symbol_history;
Gnss_Satellite d_satellite;
// navigation message vars
Galileo_Fnav_Message d_nav;
// vars for Viterbi decoder
int32_t *out0, *out1, *state0, *state1;
int32_t g_encoder[2];
const int32_t nn = 2; // Coding rate 1/n
const int32_t KK = 7; // Constraint Length
int32_t mm = KK - 1;
const int32_t CodeLength = 488;
int32_t DataLength = (CodeLength / nn) - mm;
};
#endif /* GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_ */

780
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc Normal file
View File

@ -0,0 +1,780 @@
/*!
* \file galileo_telemetry_decoder_cc.cc
* \brief Implementation of a Galileo unified INAV and FNAV message demodulator block
* \author Javier Arribas 2018. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "galileo_telemetry_decoder_cc.h"
#include "control_message_factory.h"
#include "convolutional.h"
#include "display.h"
#include "gnss_synchro.h"
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <iostream>
#define CRC_ERROR_LIMIT 6
using google::LogMessage;
galileo_telemetry_decoder_cc_sptr
galileo_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, int frame_type, bool dump)
{
return galileo_telemetry_decoder_cc_sptr(new galileo_telemetry_decoder_cc(satellite, frame_type, dump));
}
void galileo_telemetry_decoder_cc::viterbi_decoder(double *page_part_symbols, int32_t *page_part_bits)
{
Viterbi(page_part_bits, out0, state0, out1, state1,
page_part_symbols, KK, nn, DataLength);
}
void galileo_telemetry_decoder_cc::deinterleaver(int32_t rows, int32_t cols, double *in, double *out)
{
for (int32_t r = 0; r < rows; r++)
{
for (int32_t c = 0; c < cols; c++)
{
out[c * rows + r] = in[r * cols + c];
}
}
}
galileo_telemetry_decoder_cc::galileo_telemetry_decoder_cc(
const Gnss_Satellite &satellite, int frame_type,
bool dump) : gr::block("galileo_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
// Ephemeris data port out
this->message_port_register_out(pmt::mp("telemetry"));
// initialize internal vars
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
d_frame_type = frame_type;
LOG(INFO) << "Initializing GALILEO UNIFIED TELEMETRY DECODER";
switch (d_frame_type)
{
case 1: //INAV
{
d_PRN_code_period_ms = static_cast<uint32_t>(GALILEO_E1_CODE_PERIOD_MS);
d_samples_per_symbol = Galileo_E1_B_SAMPLES_PER_SYMBOL;
d_bits_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS;
// set the preamble
d_samples_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol;
d_preamble_period_symbols = GALILEO_INAV_PREAMBLE_PERIOD_SYMBOLS;
d_required_symbols = static_cast<uint32_t>(GALILEO_INAV_PAGE_SYMBOLS) + d_samples_per_preamble;
// preamble bits to sampled symbols
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_frame_length_symbols = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS;
CodeLength = GALILEO_INAV_PAGE_PART_SYMBOLS - GALILEO_INAV_PREAMBLE_LENGTH_BITS;
DataLength = (CodeLength / nn) - mm;
break;
}
case 2: //FNAV
{
d_PRN_code_period_ms = static_cast<uint32_t>(GALILEO_E5a_CODE_PERIOD_MS);
d_samples_per_symbol = GALILEO_FNAV_CODES_PER_SYMBOL;
d_bits_per_preamble = GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
// set the preamble
d_samples_per_preamble = GALILEO_FNAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol;
d_preamble_period_symbols = GALILEO_FNAV_CODES_PER_PAGE;
d_required_symbols = static_cast<uint32_t>(GALILEO_FNAV_SYMBOLS_PER_PAGE) * d_samples_per_symbol + d_samples_per_preamble;
// preamble bits to sampled symbols
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_secondary_code_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(Galileo_E5a_I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_frame_length_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
CodeLength = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
DataLength = (CodeLength / nn) - mm;
for (int32_t i = 0; i < Galileo_E5a_I_SECONDARY_CODE_LENGTH; i++)
{
if (Galileo_E5a_I_SECONDARY_CODE.at(i) == '1')
{
d_secondary_code_samples[i] = 1;
}
else
{
d_secondary_code_samples[i] = -1;
}
}
break;
}
default:
std::cout << "Galileo unified telemetry decoder error: Unknown frame type " << std::endl;
}
d_page_part_symbols = static_cast<double *>(volk_gnsssdr_malloc(d_frame_length_symbols * sizeof(double), volk_gnsssdr_get_alignment()));
int32_t n = 0;
for (int32_t i = 0; i < d_bits_per_preamble; i++)
{
switch (d_frame_type)
{
case 1: //INAV
{
if (GALILEO_INAV_PREAMBLE.at(i) == '1')
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = 1;
n++;
}
}
else
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = -1;
n++;
}
}
break;
}
case 2: //FNAV for E5a-I
{
// Galileo E5a data channel (E5a-I) still has a secondary code
int m = 0;
if (GALILEO_FNAV_PREAMBLE.at(i) == '1')
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = d_secondary_code_samples[m];
n++;
m++;
m = m % Galileo_E5a_I_SECONDARY_CODE_LENGTH;
}
}
else
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = -d_secondary_code_samples[m];
n++;
m++;
m = m % Galileo_E5a_I_SECONDARY_CODE_LENGTH;
}
}
break;
}
}
}
d_sample_counter = 0ULL;
d_stat = 0;
d_preamble_index = 0ULL;
d_flag_frame_sync = false;
d_flag_parity = false;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
delta_t = 0;
d_CRC_error_counter = 0;
flag_even_word_arrived = 0;
d_flag_preamble = false;
d_channel = 0;
flag_TOW_set = false;
// vars for Viterbi decoder
int32_t max_states = 1 << mm; // 2^mm
g_encoder[0] = 121; // Polynomial G1
g_encoder[1] = 91; // Polynomial G2
out0 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
out1 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
state0 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
state1 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
// create appropriate transition matrices
nsc_transit(out0, state0, 0, g_encoder, KK, nn);
nsc_transit(out1, state1, 1, g_encoder, KK, nn);
}
galileo_telemetry_decoder_cc::~galileo_telemetry_decoder_cc()
{
volk_gnsssdr_free(d_preamble_samples);
if (d_frame_type == 2)
{
volk_gnsssdr_free(d_secondary_code_samples);
}
volk_gnsssdr_free(d_page_part_symbols);
volk_gnsssdr_free(out0);
volk_gnsssdr_free(out1);
volk_gnsssdr_free(state0);
volk_gnsssdr_free(state1);
if (d_dump_file.is_open() == true)
{
try
{
d_dump_file.close();
}
catch (const std::exception &ex)
{
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
}
}
}
void galileo_telemetry_decoder_cc::decode_INAV_word(double *page_part_symbols, int32_t frame_length)
{
// 1. De-interleave
double *page_part_symbols_deint = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
deinterleaver(GALILEO_INAV_INTERLEAVER_ROWS, GALILEO_INAV_INTERLEAVER_COLS, page_part_symbols, page_part_symbols_deint);
// 2. Viterbi decoder
// 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder)
// 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180º
for (int32_t i = 0; i < frame_length; i++)
{
if ((i + 1) % 2 == 0)
{
page_part_symbols_deint[i] = -page_part_symbols_deint[i];
}
}
int32_t *page_part_bits = static_cast<int32_t *>(volk_gnsssdr_malloc((frame_length / 2) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
viterbi_decoder(page_part_symbols_deint, page_part_bits);
volk_gnsssdr_free(page_part_symbols_deint);
// 3. Call the Galileo page decoder
std::string page_String;
for (int32_t i = 0; i < (frame_length / 2); i++)
{
if (page_part_bits[i] > 0)
{
page_String.push_back('1');
}
else
{
page_String.push_back('0');
}
}
if (page_part_bits[0] == 1)
{
// DECODE COMPLETE WORD (even + odd) and TEST CRC
d_inav_nav.split_page(page_String, flag_even_word_arrived);
if (d_inav_nav.flag_CRC_test == true)
{
LOG(INFO) << "Galileo E1 CRC correct in channel " << d_channel << " from satellite " << d_satellite;
}
else
{
LOG(INFO) << "Galileo E1 CRC error in channel " << d_channel << " from satellite " << d_satellite;
}
flag_even_word_arrived = 0;
}
else
{
// STORE HALF WORD (even page)
d_inav_nav.split_page(page_String.c_str(), flag_even_word_arrived);
flag_even_word_arrived = 1;
}
volk_gnsssdr_free(page_part_bits);
// 4. Push the new navigation data to the queues
if (d_inav_nav.have_new_ephemeris() == true)
{
// get object for this SV (mandatory)
std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_inav_nav.get_ephemeris());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_inav_nav.have_new_iono_and_GST() == true)
{
// get object for this SV (mandatory)
std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(d_inav_nav.get_iono());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_inav_nav.have_new_utc_model() == true)
{
// get object for this SV (mandatory)
std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_inav_nav.get_utc_model());
std::cout << "New Galileo E1 I/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_inav_nav.have_new_almanac() == true)
{
std::shared_ptr<Galileo_Almanac> tmp_obj = std::make_shared<Galileo_Almanac>(d_inav_nav.get_almanac());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
//debug
std::cout << "Galileo E1 I/NAV almanac received in channel " << d_channel << " from satellite " << d_satellite << std::endl;
DLOG(INFO) << "GPS_to_Galileo time conversion:";
DLOG(INFO) << "A0G=" << tmp_obj->A_0G_10;
DLOG(INFO) << "A1G=" << tmp_obj->A_1G_10;
DLOG(INFO) << "T0G=" << tmp_obj->t_0G_10;
DLOG(INFO) << "WN_0G_10=" << tmp_obj->WN_0G_10;
DLOG(INFO) << "Current parameters:";
DLOG(INFO) << "d_TOW_at_current_symbol_ms=" << d_TOW_at_current_symbol_ms;
DLOG(INFO) << "d_nav.WN_0=" << d_inav_nav.WN_0;
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_inav_nav.WN_0 - tmp_obj->WN_0G_10), 64)));
DLOG(INFO) << "delta_t=" << delta_t << "[s]";
}
}
void galileo_telemetry_decoder_cc::decode_FNAV_word(double *page_symbols, int32_t frame_length)
{
// 1. De-interleave
double *page_symbols_deint = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
deinterleaver(GALILEO_FNAV_INTERLEAVER_ROWS, GALILEO_FNAV_INTERLEAVER_COLS, page_symbols, page_symbols_deint);
// 2. Viterbi decoder
// 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder)
// 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180<38>
for (int32_t i = 0; i < frame_length; i++)
{
if ((i + 1) % 2 == 0)
{
page_symbols_deint[i] = -page_symbols_deint[i];
}
}
int32_t *page_bits = static_cast<int32_t *>(volk_gnsssdr_malloc((frame_length / 2) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
viterbi_decoder(page_symbols_deint, page_bits);
volk_gnsssdr_free(page_symbols_deint);
// 3. Call the Galileo page decoder
std::string page_String;
for (int32_t i = 0; i < frame_length; i++)
{
if (page_bits[i] > 0)
{
page_String.push_back('1');
}
else
{
page_String.push_back('0');
}
}
volk_gnsssdr_free(page_bits);
// DECODE COMPLETE WORD (even + odd) and TEST CRC
d_fnav_nav.split_page(page_String);
if (d_fnav_nav.flag_CRC_test == true)
{
LOG(INFO) << "Galileo E5a CRC correct in channel " << d_channel << " from satellite " << d_satellite;
}
else
{
LOG(INFO) << "Galileo E5a CRC error in channel " << d_channel << " from satellite " << d_satellite;
}
// 4. Push the new navigation data to the queues
if (d_fnav_nav.have_new_ephemeris() == true)
{
std::shared_ptr<Galileo_Ephemeris> tmp_obj = std::make_shared<Galileo_Ephemeris>(d_fnav_nav.get_ephemeris());
std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": ephemeris from satellite " << d_satellite << TEXT_RESET << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_fnav_nav.have_new_iono_and_GST() == true)
{
std::shared_ptr<Galileo_Iono> tmp_obj = std::make_shared<Galileo_Iono>(d_fnav_nav.get_iono());
std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": iono/GST model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_fnav_nav.have_new_utc_model() == true)
{
std::shared_ptr<Galileo_Utc_Model> tmp_obj = std::make_shared<Galileo_Utc_Model>(d_fnav_nav.get_utc_model());
std::cout << TEXT_MAGENTA << "New Galileo E5a F/NAV message received in channel " << d_channel << ": UTC model parameters from satellite " << d_satellite << TEXT_RESET << std::endl;
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
}
void galileo_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite)
{
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
}
void galileo_telemetry_decoder_cc::set_channel(int32_t channel)
{
d_channel = channel;
LOG(INFO) << "Navigation channel set to " << channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename = "telemetry";
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
}
}
}
}
int galileo_telemetry_decoder_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
int32_t corr_value = 0;
int32_t preamble_diff = 0;
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); // Get the output buffer pointer
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]); // Get the input buffer pointer
Gnss_Synchro current_symbol; // structure to save the synchronization information and send the output object to the next block
// 1. Copy the current tracking output
current_symbol = in[0][0];
// add new symbol to the symbol queue
d_symbol_history.push_back(current_symbol.Prompt_I);
d_sample_counter++; // count for the processed samples
consume_each(1);
d_flag_preamble = false;
if (d_symbol_history.size() > d_required_symbols)
{
// TODO Optimize me!
// ******* preamble correlation ********
for (int32_t i = 0; i < d_samples_per_preamble; i++)
{
if (d_symbol_history.at(i) < 0.0) // symbols clipping
{
corr_value -= d_preamble_samples[i];
}
else
{
corr_value += d_preamble_samples[i];
}
}
}
// ******* frame sync ******************
switch (d_stat)
{
case 0: // no preamble information
{
if (abs(corr_value) >= d_samples_per_preamble)
{
d_preamble_index = d_sample_counter; // record the preamble sample stamp
LOG(INFO) << "Preamble detection for Galileo satellite " << this->d_satellite;
d_stat = 1; // enter into frame pre-detection status
}
break;
}
case 1: // possible preamble lock
{
if (abs(corr_value) >= d_samples_per_preamble)
{
// check preamble separation
preamble_diff = static_cast<int32_t>(d_sample_counter - d_preamble_index);
if (abs(preamble_diff - d_preamble_period_symbols) == 0)
{
// try to decode frame
LOG(INFO) << "Starting page decoder for Galileo satellite " << this->d_satellite;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
d_stat = 2;
}
else
{
if (preamble_diff > d_preamble_period_symbols)
{
d_stat = 0; // start again
}
}
}
break;
}
case 2: //preamble acquired
{
if (d_sample_counter == d_preamble_index + static_cast<uint64_t>(d_preamble_period_symbols))
{
// call the decoder
switch (d_frame_type)
{
case 1: //INAV
// NEW Galileo page part is received
// 0. fetch the symbols into an array
if (corr_value > 0) //normal PLL lock
{
for (uint32_t i = 0; i < d_frame_length_symbols; i++)
{
d_page_part_symbols[i] = d_symbol_history.at(i + d_samples_per_preamble); // because last symbol of the preamble is just received now!
}
}
else //180 deg. inverted carrier phase PLL lock
{
for (uint32_t i = 0; i < d_frame_length_symbols; i++)
{
d_page_part_symbols[i] = d_symbol_history.at(i + d_samples_per_preamble); // because last symbol of the preamble is just received now!
}
}
decode_INAV_word(d_page_part_symbols, d_frame_length_symbols);
break;
case 2: //FNAV
// NEW Galileo page part is received
// 0. fetch the symbols into an array
if (corr_value > 0) //normal PLL lock
{
int k = 0;
for (uint32_t i = 0; i < d_frame_length_symbols; i++)
{
d_page_part_symbols[i] = 0;
for (uint32_t m = 0; m < d_samples_per_symbol; m++)
{
d_page_part_symbols[i] += static_cast<float>(d_secondary_code_samples[k]) * d_symbol_history.at(i * d_samples_per_symbol + d_samples_per_preamble + m); // because last symbol of the preamble is just received now!
k++;
k = k % Galileo_E5a_I_SECONDARY_CODE_LENGTH;
}
}
}
else //180 deg. inverted carrier phase PLL lock
{
int k = 0;
for (uint32_t i = 0; i < d_frame_length_symbols; i++)
{
d_page_part_symbols[i] = 0;
for (uint32_t m = 0; m < d_samples_per_symbol; m++) //integrate samples into symbols
{
d_page_part_symbols[i] -= static_cast<float>(d_secondary_code_samples[k]) * d_symbol_history.at(i * d_samples_per_symbol + d_samples_per_preamble + m); // because last symbol of the preamble is just received now!
k++;
k = k % Galileo_E5a_I_SECONDARY_CODE_LENGTH;
}
}
}
decode_FNAV_word(d_page_part_symbols, d_frame_length_symbols);
break;
default:
return -1;
break;
}
if (d_inav_nav.flag_CRC_test == true or d_fnav_nav.flag_CRC_test == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
DLOG(INFO) << " Frame sync SAT " << this->d_satellite;
}
}
else
{
d_CRC_error_counter++;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
{
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
d_flag_frame_sync = false;
d_stat = 0;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0;
d_fnav_nav.flag_TOW_set = false;
d_inav_nav.flag_TOW_set = false;
}
}
}
break;
}
}
// UPDATE GNSS SYNCHRO DATA
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true)
// update TOW at the preamble instant
{
switch (d_frame_type)
{
case 1: //INAV
{
if (d_inav_nav.flag_TOW_set == true)
{
if (d_inav_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
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.TOW_5 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS);
d_inav_nav.flag_TOW_5 = false;
}
else if (d_inav_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
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_inav_nav.TOW_6 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * GALILEO_E1_CODE_PERIOD_MS);
d_inav_nav.flag_TOW_6 = false;
}
else
{
// this page has no timing information
d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E1_CODE_PERIOD_MS); // + GALILEO_INAV_PAGE_PART_SYMBOLS*GALILEO_E1_CODE_PERIOD;
}
}
break;
}
case 2: //FNAV
{
if (d_fnav_nav.flag_TOW_set == true)
{
if (d_fnav_nav.flag_TOW_1 == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.FNAV_TOW_1 * 1000.0);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_E5a_CODE_PERIOD_MS);
//d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_fnav_nav.flag_TOW_1 = false;
}
else if (d_fnav_nav.flag_TOW_2 == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.FNAV_TOW_2 * 1000.0);
//d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_E5a_CODE_PERIOD_MS);
d_fnav_nav.flag_TOW_2 = false;
}
else if (d_fnav_nav.flag_TOW_3 == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.FNAV_TOW_3 * 1000.0);
//d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_E5a_CODE_PERIOD_MS);
d_fnav_nav.flag_TOW_3 = false;
}
else if (d_fnav_nav.flag_TOW_4 == true)
{
d_TOW_at_Preamble_ms = static_cast<uint32_t>(d_fnav_nav.FNAV_TOW_4 * 1000.0);
//d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD_MS);
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * GALILEO_E5a_CODE_PERIOD_MS);
d_fnav_nav.flag_TOW_4 = false;
}
else
{
d_TOW_at_current_symbol_ms += static_cast<uint32_t>(GALILEO_E5a_CODE_PERIOD_MS);
}
break;
}
}
}
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
switch (d_frame_type)
{
case 1: //INAV
{
if (d_inav_nav.flag_TOW_set == true)
{
d_TOW_at_current_symbol_ms += d_PRN_code_period_ms;
}
break;
}
case 2: //FNAV
{
if (d_fnav_nav.flag_TOW_set == true)
{
d_TOW_at_current_symbol_ms += d_PRN_code_period_ms;
}
break;
}
}
}
// remove used symbols from history
// todo: Use circular buffer here
if (d_symbol_history.size() > d_required_symbols)
{
d_symbol_history.pop_front();
}
switch (d_frame_type)
{
case 1: //INAV
{
if (d_inav_nav.flag_TOW_set)
{
if (d_inav_nav.flag_GGTO_1 == true and d_inav_nav.flag_GGTO_2 == true and d_inav_nav.flag_GGTO_3 == true and d_inav_nav.flag_GGTO_4 == true) // all GGTO parameters arrived
{
delta_t = d_inav_nav.A_0G_10 + d_inav_nav.A_1G_10 * (static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0 - d_inav_nav.t_0G_10 + 604800.0 * (fmod((d_inav_nav.WN_0 - d_inav_nav.WN_0G_10), 64.0)));
}
current_symbol.Flag_valid_word = true;
}
break;
}
case 2: //FNAV
{
if (d_fnav_nav.flag_TOW_set)
{
current_symbol.Flag_valid_word = true;
}
break;
}
}
if (d_inav_nav.flag_TOW_set or d_fnav_nav.flag_TOW_set)
{
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
// 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
if (d_dump == true)
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
uint64_t tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
}
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
}
else
{
return 0;
}
}

58
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h → src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.h
View File

@ -1,8 +1,7 @@
/*!
* \file galileo_e1b_telemetry_decoder_cc.h
* \brief Interface of a Galileo INAV message demodulator block
* \author Javier Arribas 2013 jarribas(at)cttc.es,
* Mara Branzanti 2013 mara.branzanti(at)gmail.com
* \file galileo_telemetry_decoder_cc.h
* \brief Implementation of a Galileo unified INAV and FNAV message demodulator block
* \author Javier Arribas 2018. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
@ -29,12 +28,15 @@
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GALILEO_E1B_TELEMETRY_DECODER_CC_H
#define GNSS_SDR_GALILEO_E1B_TELEMETRY_DECODER_CC_H
#ifndef GNSS_SDR_galileo_telemetry_decoder_cc_H
#define GNSS_SDR_galileo_telemetry_decoder_cc_H
#include "Galileo_E1.h"
#include "Galileo_E5a.h"
#include "galileo_navigation_message.h"
#include "galileo_fnav_message.h"
#include "galileo_ephemeris.h"
#include "galileo_almanac.h"
#include "galileo_iono.h"
@ -46,20 +48,20 @@
#include <string>
class galileo_e1b_telemetry_decoder_cc;
class galileo_telemetry_decoder_cc;
typedef boost::shared_ptr<galileo_e1b_telemetry_decoder_cc> galileo_e1b_telemetry_decoder_cc_sptr;
typedef boost::shared_ptr<galileo_telemetry_decoder_cc> galileo_telemetry_decoder_cc_sptr;
galileo_e1b_telemetry_decoder_cc_sptr galileo_e1b_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
galileo_telemetry_decoder_cc_sptr galileo_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, int frame_type, bool dump);
/*!
* \brief This class implements a block that decodes the INAV data defined in Galileo ICD
* \brief This class implements a block that decodes the INAV and FNAV data defined in Galileo ICD
*
*/
class galileo_e1b_telemetry_decoder_cc : public gr::block
class galileo_telemetry_decoder_cc : public gr::block
{
public:
~galileo_e1b_telemetry_decoder_cc();
~galileo_telemetry_decoder_cc();
void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN
void set_channel(int32_t channel); //!< Set receiver's channel
int32_t flag_even_word_arrived;
@ -71,23 +73,30 @@ public:
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
private:
friend galileo_e1b_telemetry_decoder_cc_sptr
galileo_e1b_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
galileo_e1b_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
friend galileo_telemetry_decoder_cc_sptr
galileo_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, int frame_type, bool dump);
galileo_telemetry_decoder_cc(const Gnss_Satellite &satellite, int frame_type, bool dump);
void viterbi_decoder(double *page_part_symbols, int32_t *page_part_bits);
void deinterleaver(int32_t rows, int32_t cols, double *in, double *out);
void decode_word(double *symbols, int32_t frame_length);
void decode_INAV_word(double *symbols, int32_t frame_length);
void decode_FNAV_word(double *page_symbols, int32_t frame_length);
uint16_t d_preambles_bits[GALILEO_INAV_PREAMBLE_LENGTH_BITS];
int32_t *d_preambles_symbols;
int d_frame_type;
int32_t d_bits_per_preamble;
int32_t d_samples_per_preamble;
int32_t d_preamble_period_symbols;
int32_t *d_preamble_samples;
int32_t *d_secondary_code_samples;
uint32_t d_samples_per_symbol;
int32_t d_symbols_per_preamble;
uint32_t d_PRN_code_period_ms;
uint32_t d_required_symbols;
uint32_t d_frame_length_symbols;
double *d_page_part_symbols;
std::deque<Gnss_Synchro> d_symbol_history;
std::deque<float> d_symbol_history;
uint64_t d_sample_counter;
uint64_t d_preamble_index;
@ -99,7 +108,8 @@ private:
int32_t d_CRC_error_counter;
// navigation message vars
Galileo_Navigation_Message d_nav;
Galileo_Navigation_Message d_inav_nav;
Galileo_Fnav_Message d_fnav_nav;
bool d_dump;
Gnss_Satellite d_satellite;
@ -120,8 +130,8 @@ private:
const int32_t nn = 2; // Coding rate 1/n
const int32_t KK = 7; // Constraint Length
int32_t mm = KK - 1;
const int32_t CodeLength = 240;
int32_t DataLength = (CodeLength / nn) - mm;
int32_t CodeLength;
int32_t DataLength;
};
#endif

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

@ -321,7 +321,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
// record the oldest subframe symbol before inserting a new symbol into the circular buffer
if (d_current_subframe_symbol < GPS_SUBFRAME_MS and d_symbol_history.size() > 0)
{
d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history.at(0).Prompt_I;
d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history[0].Prompt_I;
d_current_subframe_symbol++;
}
@ -337,9 +337,9 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
// std::cout << "-------\n";
for (uint32_t i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++)
{
if (d_symbol_history.at(i).Flag_valid_symbol_output == true)
if (d_symbol_history[i].Flag_valid_symbol_output == true)
{
if (d_symbol_history.at(i).Prompt_I < 0) // symbols clipping
if (d_symbol_history[i].Prompt_I < 0) // symbols clipping
{
corr_value -= d_preambles_symbols[i];
}
@ -358,18 +358,18 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
if (d_stat == 0)
{
// record the preamble sample stamp
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the preamble sample stamp
DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history.at(0).Tracking_sample_counter=" << d_symbol_history.at(0).Tracking_sample_counter;
d_preamble_time_samples = d_symbol_history[0].Tracking_sample_counter; // record the preamble sample stamp
DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history[0].Tracking_sample_counter=" << d_symbol_history[0].Tracking_sample_counter;
d_stat = 1; // enter into frame pre-detection status
}
else if (d_stat == 1) // check 6 seconds of preamble separation
{
preamble_diff_ms = std::round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
preamble_diff_ms = std::round(((static_cast<double>(d_symbol_history[0].Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history[0].fs)) * 1000.0);
if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) % GPS_SUBFRAME_MS == 0)
{
DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite;
d_flag_preamble = true;
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the PRN start sample index associated to the preamble
d_preamble_time_samples = d_symbol_history[0].Tracking_sample_counter; // record the PRN start sample index associated to the preamble
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
@ -383,7 +383,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
flag_PLL_180_deg_phase_locked = false;
}
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
<< static_cast<double>(d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs) << " [s]";
<< static_cast<double>(d_preamble_time_samples) / static_cast<double>(d_symbol_history[0].fs) << " [s]";
}
// try to decode the subframe:
@ -407,7 +407,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
{
if (d_stat == 1)
{
preamble_diff_ms = round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
preamble_diff_ms = round(((static_cast<double>(d_symbol_history[0].Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history[0].fs)) * 1000.0);
if (preamble_diff_ms > GPS_SUBFRAME_MS)
{
DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms;

2
src/algorithms/tracking/adapters/CMakeLists.txt
View File

@ -35,6 +35,7 @@ set(TRACKING_ADAPTER_SOURCES
gps_l2_m_dll_pll_tracking.cc
glonass_l1_ca_dll_pll_tracking.cc
glonass_l1_ca_dll_pll_c_aid_tracking.cc
gps_l1_ca_kf_tracking.cc
gps_l5_dll_pll_tracking.cc
glonass_l2_ca_dll_pll_tracking.cc
glonass_l2_ca_dll_pll_c_aid_tracking.cc
@ -49,6 +50,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/algorithms/tracking/gnuradio_blocks
${CMAKE_SOURCE_DIR}/src/algorithms/tracking/libs
${CMAKE_SOURCE_DIR}/src/algorithms/libs
${ARMADILLO_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
${GNURADIO_RUNTIME_INCLUDE_DIRS}

174
src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc Normal file
View File

@ -0,0 +1,174 @@
/*!
* \file gps_l1_ca_kf_tracking.cc
* \brief Implementation of an adapter of a DLL + Kalman carrier
* tracking loop block for GPS L1 C/A signals
* \author Javier Arribas, 2018. jarribas(at)cttc.es
* \author Jordi Vila-Valls 2018. jvila(at)cttc.es
* \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
*
* Reference:
* J. Vila-Valls, P. Closas, M. Navarro and C. Fernández-Prades,
* "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
* Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "gps_l1_ca_kf_tracking.h"
#include "gnss_sdr_flags.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include <glog/logging.h>
using google::LogMessage;
GpsL1CaKfTracking::GpsL1CaKfTracking(
ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams)
{
DLOG(INFO) << "role " << role;
//################# CONFIGURATION PARAMETERS ########################
int order;
int fs_in;
int vector_length;
int f_if;
bool dump;
std::string dump_filename;
std::string item_type;
std::string default_item_type = "gr_complex";
float dll_bw_hz;
float early_late_space_chips;
bool bce_run;
unsigned int bce_ptrans;
unsigned int bce_strans;
int bce_nu;
int bce_kappa;
item_type = configuration->property(role + ".item_type", default_item_type);
order = configuration->property(role + ".order", 2);
int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
f_if = configuration->property(role + ".if", 0);
dump = configuration->property(role + ".dump", false);
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
if (FLAGS_dll_bw_hz != 0.0) dll_bw_hz = static_cast<float>(FLAGS_dll_bw_hz);
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./track_ch";
dump_filename = configuration->property(role + ".dump_filename", default_dump_filename);
vector_length = std::round(fs_in / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
bce_run = configuration->property(role + ".bce_run", false);
bce_ptrans = configuration->property(role + ".p_transient", 0);
bce_strans = configuration->property(role + ".s_transient", 0);
bce_nu = configuration->property(role + ".bce_nu", 0);
bce_kappa = configuration->property(role + ".bce_kappa", 0);
//################# MAKE TRACKING GNURadio object ###################
if (item_type.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
tracking_ = gps_l1_ca_kf_make_tracking_cc(
order,
f_if,
fs_in,
vector_length,
dump,
dump_filename,
dll_bw_hz,
early_late_space_chips,
bce_run,
bce_ptrans,
bce_strans,
bce_nu,
bce_kappa);
}
else
{
item_size_ = sizeof(gr_complex);
LOG(WARNING) << item_type << " unknown tracking item type.";
}
channel_ = 0;
DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
}
GpsL1CaKfTracking::~GpsL1CaKfTracking()
{
}
void GpsL1CaKfTracking::start_tracking()
{
tracking_->start_tracking();
}
/*
* Set tracking channel unique ID
*/
void GpsL1CaKfTracking::set_channel(unsigned int channel)
{
channel_ = channel;
tracking_->set_channel(channel);
}
void GpsL1CaKfTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
tracking_->set_gnss_synchro(p_gnss_synchro);
}
void GpsL1CaKfTracking::connect(gr::top_block_sptr top_block)
{
if (top_block)
{ /* top_block is not null */
};
//nothing to connect, now the tracking uses gr_sync_decimator
}
void GpsL1CaKfTracking::disconnect(gr::top_block_sptr top_block)
{
if (top_block)
{ /* top_block is not null */
};
//nothing to disconnect, now the tracking uses gr_sync_decimator
}
gr::basic_block_sptr GpsL1CaKfTracking::get_left_block()
{
return tracking_;
}
gr::basic_block_sptr GpsL1CaKfTracking::get_right_block()
{
return tracking_;
}

105
src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h Normal file
View File

@ -0,0 +1,105 @@
/*!
* \file GPS_L1_CA_KF_Tracking.h
* \brief Interface of an adapter of a DLL + Kalman carrier
* tracking loop block for GPS L1 C/A signals
* \author Javier Arribas, 2018. jarribas(at)cttc.es
* \author Jordi Vila-Valls 2018. jvila(at)cttc.es
* \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
*
* Reference:
* J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
* "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
* Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_
#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_
#include "gps_l1_ca_kf_tracking_cc.h"
#include "tracking_interface.h"
#include <string>
class ConfigurationInterface;
/*!
* \brief This class implements a code DLL + carrier PLL tracking loop
*/
class GpsL1CaKfTracking : public TrackingInterface
{
public:
GpsL1CaKfTracking(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams);
virtual ~GpsL1CaKfTracking();
inline std::string role() override
{
return role_;
}
//! Returns "GPS_L1_CA_KF_Tracking"
inline std::string implementation() override
{
return "GPS_L1_CA_KF_Tracking";
}
inline size_t item_size() override
{
return item_size_;
}
void connect(gr::top_block_sptr top_block) override;
void disconnect(gr::top_block_sptr top_block) override;
gr::basic_block_sptr get_left_block() override;
gr::basic_block_sptr get_right_block() override;
/*!
* \brief Set tracking channel unique ID
*/
void set_channel(unsigned int channel) override;
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to efficiently exchange synchronization data between acquisition and tracking blocks
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override;
void start_tracking() override;
private:
gps_l1_ca_kf_tracking_cc_sptr tracking_;
size_t item_size_;
unsigned int channel_;
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
};
#endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_H_

2
src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc
View File

@ -110,7 +110,7 @@ GpsL5DllPllTracking::GpsL5DllPllTracking(
int max_lock_fail = configuration->property(role + ".max_lock_fail", 50);
if (FLAGS_max_lock_fail != 50) max_lock_fail = FLAGS_max_lock_fail;
trk_param.max_lock_fail = max_lock_fail;
double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.85);
double carrier_lock_th = configuration->property(role + ".carrier_lock_th", 0.75);
if (FLAGS_carrier_lock_th != 0.85) carrier_lock_th = FLAGS_carrier_lock_th;
trk_param.carrier_lock_th = carrier_lock_th;

2
src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt
View File

@ -34,6 +34,7 @@ set(TRACKING_GR_BLOCKS_SOURCES
glonass_l1_ca_dll_pll_tracking_cc.cc
glonass_l1_ca_dll_pll_c_aid_tracking_cc.cc
glonass_l1_ca_dll_pll_c_aid_tracking_sc.cc
gps_l1_ca_kf_tracking_cc.cc
glonass_l2_ca_dll_pll_tracking_cc.cc
glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc
glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc
@ -48,6 +49,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/core/receiver
${CMAKE_SOURCE_DIR}/src/algorithms/tracking/libs
${CMAKE_SOURCE_DIR}/src/algorithms/libs
${ARMADILLO_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}

14
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@ -238,17 +238,18 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_code_length_chips = static_cast<uint32_t>(Galileo_E5a_CODE_LENGTH_CHIPS);
d_secondary = true;
if (trk_parameters.track_pilot)
{
d_secondary = true;
d_secondary_code_length = static_cast<uint32_t>(Galileo_E5a_Q_SECONDARY_CODE_LENGTH);
signal_pretty_name = signal_pretty_name + "Q";
interchange_iq = true;
}
else
{
d_secondary_code_length = static_cast<uint32_t>(Galileo_E5a_I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&Galileo_E5a_I_SECONDARY_CODE);
//Do not acquire secondary code in data component. It is done in telemetry decoder
d_secondary = false;
signal_pretty_name = signal_pretty_name + "I";
interchange_iq = false;
}
@ -362,7 +363,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
}
// --- Initializations ---
multicorrelator_cpu.set_fast_resampler(trk_parameters.use_fast_resampler);
multicorrelator_cpu.set_high_dynamics_resampler(trk_parameters.use_high_dynamics_resampler);
// Initial code frequency basis of NCO
d_code_freq_chips = d_code_chip_rate;
// Residual code phase (in chips)
@ -394,6 +395,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
d_extend_correlation_symbols_count = 0;
d_code_phase_step_chips = 0.0;
d_code_phase_rate_step_chips = 0.0;
d_carrier_phase_step_rad = 0.0;
d_rem_code_phase_chips = 0.0;
d_K_blk_samples = 0.0;
@ -496,7 +498,7 @@ void dll_pll_veml_tracking::start_tracking()
for (uint32_t i = 0; i < d_code_length_chips; i++)
{
d_tracking_code[i] = aux_code[i].imag();
d_data_code[i] = aux_code[i].real();
d_data_code[i] = aux_code[i].real(); //the same because it is generated the full signal (E5aI + E5aQ)
}
d_Prompt_Data[0] = gr_complex(0.0, 0.0);
correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code, d_prompt_data_shift);
@ -707,6 +709,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples)
d_carrier_phase_step_rad,
static_cast<float>(d_rem_code_phase_chips) * static_cast<float>(d_code_samples_per_chip),
static_cast<float>(d_code_phase_step_chips) * static_cast<float>(d_code_samples_per_chip),
static_cast<float>(d_code_phase_rate_step_chips) * static_cast<float>(d_code_samples_per_chip),
trk_parameters.vector_length);
// DATA CORRELATOR (if tracking tracks the pilot signal)
@ -718,6 +721,7 @@ void dll_pll_veml_tracking::do_correlation_step(const gr_complex *input_samples)
d_carrier_phase_step_rad,
static_cast<float>(d_rem_code_phase_chips) * static_cast<float>(d_code_samples_per_chip),
static_cast<float>(d_code_phase_step_chips) * static_cast<float>(d_code_samples_per_chip),
static_cast<float>(d_code_phase_rate_step_chips) * static_cast<float>(d_code_samples_per_chip),
trk_parameters.vector_length);
}
}

1
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h
View File

@ -145,6 +145,7 @@ private:
gr_complex *d_Prompt_Data;
double d_code_phase_step_chips;
double d_code_phase_rate_step_chips;
double d_carrier_phase_step_rad;
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;

2
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h
View File

@ -233,7 +233,7 @@ private:
int32_t d_correlation_length_samples;
int32_t d_next_prn_length_samples;
uint64_t d_sample_counter_next;
uint32_t d_pull_in = 0;
uint32_t d_pull_in = 0U;
};
#endif //GNSS_SDR_DLL_PLL_VEML_TRACKING_FPGA_H

958
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc Normal file
View File

@ -0,0 +1,958 @@
/*!
* \file gps_l1_ca_kf_tracking_cc.cc
* \brief Implementation of a processing block of a DLL + Kalman carrier
* tracking loop for GPS L1 C/A signals
* \author Javier Arribas, 2018. jarribas(at)cttc.es
* \author Jordi Vila-Valls 2018. jvila(at)cttc.es
* \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
*
* Reference:
* J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
* "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
* Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "gps_l1_ca_kf_tracking_cc.h"
#include "gps_sdr_signal_processing.h"
#include "tracking_discriminators.h"
#include "lock_detectors.h"
#include "gnss_sdr_flags.h"
#include "GPS_L1_CA.h"
#include "control_message_factory.h"
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <matio.h>
#include <cmath>
#include <iostream>
#include <memory>
#include <sstream>
using google::LogMessage;
gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(
uint32_t order,
int64_t if_freq,
int64_t fs_in,
uint32_t vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips,
bool bce_run,
uint32_t bce_ptrans,
uint32_t bce_strans,
int32_t bce_nu,
int32_t bce_kappa)
{
return gps_l1_ca_kf_tracking_cc_sptr(new Gps_L1_Ca_Kf_Tracking_cc(order, if_freq,
fs_in, vector_length, dump, dump_filename, dll_bw_hz, early_late_space_chips,
bce_run, bce_ptrans, bce_strans, bce_nu, bce_kappa));
}
void Gps_L1_Ca_Kf_Tracking_cc::forecast(int noutput_items,
gr_vector_int &ninput_items_required)
{
if (noutput_items != 0)
{
ninput_items_required[0] = static_cast<int>(d_vector_length) * 2; // set the required available samples in each call
}
}
Gps_L1_Ca_Kf_Tracking_cc::Gps_L1_Ca_Kf_Tracking_cc(
uint32_t order,
int64_t if_freq,
int64_t fs_in,
uint32_t vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips,
bool bce_run,
uint32_t bce_ptrans,
uint32_t bce_strans,
int32_t bce_nu,
int32_t bce_kappa) : gr::block("Gps_L1_Ca_Kf_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
// Telemetry bit synchronization message port input
this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
this->message_port_register_out(pmt::mp("events"));
// initialize internal vars
d_order = order;
d_dump = dump;
d_if_freq = if_freq;
d_fs_in = fs_in;
d_vector_length = vector_length;
d_dump_filename = dump_filename;
d_current_prn_length_samples = static_cast<int>(d_vector_length);
// Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
// --- DLL variables --------------------------------------------------------
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
// Initialization of local code replica
// Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code = static_cast<float *>(volk_gnsssdr_malloc(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS) * sizeof(float), volk_gnsssdr_get_alignment()));
// correlator outputs (scalar)
d_n_correlator_taps = 3; // Early, Prompt, and Late
d_correlator_outs = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
for (int32_t n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs[n] = gr_complex(0, 0);
}
d_local_code_shift_chips = static_cast<float *>(volk_gnsssdr_malloc(d_n_correlator_taps * sizeof(float), volk_gnsssdr_get_alignment()));
// Set TAPs delay values [chips]
d_local_code_shift_chips[0] = -d_early_late_spc_chips;
d_local_code_shift_chips[1] = 0.0;
d_local_code_shift_chips[2] = d_early_late_spc_chips;
multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps);
// --- Perform initializations ------------------------------
// define initial code frequency basis of NCO
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ;
// define residual code phase (in chips)
d_rem_code_phase_samples = 0.0;
// define residual carrier phase
d_rem_carr_phase_rad = 0.0;
// define residual carrier phase covariance
d_carr_phase_sigma2 = 0.0;
// sample synchronization
d_sample_counter = 0;
d_acq_sample_stamp = 0;
d_enable_tracking = false;
d_pull_in = false;
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter = 0;
d_Prompt_buffer = new gr_complex[FLAGS_cn0_samples];
d_carrier_lock_test = 1;
d_CN0_SNV_dB_Hz = 0;
d_carrier_lock_fail_counter = 0;
d_carrier_lock_threshold = FLAGS_carrier_lock_th;
systemName["G"] = std::string("GPS");
systemName["S"] = std::string("SBAS");
d_acquisition_gnss_synchro = 0;
d_channel = 0;
d_acq_code_phase_samples = 0.0;
d_acq_carrier_doppler_hz = 0.0;
d_carrier_doppler_hz = 0.0;
d_carrier_dopplerrate_hz2 = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_code_phase_samples = 0.0;
d_rem_code_phase_chips = 0.0;
d_code_phase_step_chips = 0.0;
d_code_phase_rate_step_chips = 0.0;
d_carrier_phase_step_rad = 0.0;
code_error_chips = 0.0;
code_error_filt_chips = 0.0;
set_relative_rate(1.0 / static_cast<double>(d_vector_length));
// Kalman filter initialization (receiver initialization)
double CN_dB_Hz = 30;
double CN_lin = pow(10, CN_dB_Hz / 10.0);
double sigma2_phase_detector_cycles2;
sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD));
// covariances (static)
double sigma2_carrier_phase = GPS_TWO_PI / 4;
double sigma2_doppler = 450;
double sigma2_doppler_rate = pow(4.0 * GPS_TWO_PI, 2) / 12.0;
kf_P_x_ini = arma::zeros(2, 2);
kf_P_x_ini(0, 0) = sigma2_carrier_phase;
kf_P_x_ini(1, 1) = sigma2_doppler;
kf_R = arma::zeros(1, 1);
kf_R(0, 0) = sigma2_phase_detector_cycles2;
kf_Q = arma::zeros(2, 2);
kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4);
kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD;
kf_F = arma::zeros(2, 2);
kf_F(0, 0) = 1.0;
kf_F(0, 1) = GPS_TWO_PI * GPS_L1_CA_CODE_PERIOD;
kf_F(1, 0) = 0.0;
kf_F(1, 1) = 1.0;
kf_H = arma::zeros(1, 2);
kf_H(0, 0) = 1.0;
kf_x = arma::zeros(2, 1);
kf_y = arma::zeros(1, 1);
kf_P_y = arma::zeros(1, 1);
// order three
if (d_order == 3)
{
kf_P_x_ini = arma::resize(kf_P_x_ini, 3, 3);
kf_P_x_ini(2, 2) = sigma2_doppler_rate;
kf_Q = arma::zeros(3, 3);
kf_Q(0, 0) = pow(GPS_L1_CA_CODE_PERIOD, 4);
kf_Q(1, 1) = GPS_L1_CA_CODE_PERIOD;
kf_Q(2, 2) = GPS_L1_CA_CODE_PERIOD;
kf_F = arma::resize(kf_F, 3, 3);
kf_F(0, 2) = 0.5 * GPS_TWO_PI * pow(GPS_L1_CA_CODE_PERIOD, 2);
kf_F(1, 2) = GPS_L1_CA_CODE_PERIOD;
kf_F(2, 0) = 0.0;
kf_F(2, 1) = 0.0;
kf_F(2, 2) = 1.0;
kf_H = arma::resize(kf_H, 1, 3);
kf_H(0, 2) = 0.0;
kf_x = arma::resize(kf_x, 3, 1);
kf_x(2, 0) = 0.0;
}
// Bayesian covariance estimator initialization
kf_iter = 0;
bayes_run = bce_run;
bayes_ptrans = bce_ptrans;
bayes_strans = bce_strans;
bayes_kappa = bce_kappa;
bayes_nu = bce_nu;
kf_R_est = kf_R;
bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2));
}
void Gps_L1_Ca_Kf_Tracking_cc::start_tracking()
{
/*
* correct the code phase according to the delay between acq and trk
*/
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
d_acq_carrier_doppler_step_hz = static_cast<double>(d_acquisition_gnss_synchro->Acq_doppler_step);
// Correct Kalman filter covariance according to acq doppler step size (3 sigma)
if (d_acquisition_gnss_synchro->Acq_doppler_step > 0)
{
kf_P_x_ini(1, 1) = pow(d_acq_carrier_doppler_step_hz / 3.0, 2);
bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2));
}
int64_t acq_trk_diff_samples;
double acq_trk_diff_seconds;
acq_trk_diff_samples = static_cast<int64_t>(d_sample_counter) - static_cast<int64_t>(d_acq_sample_stamp); //-d_vector_length;
DLOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples;
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
// Doppler effect Fd = (C / (C + Vr)) * F
double radial_velocity = (GPS_L1_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
double T_chip_mod_seconds;
double T_prn_mod_seconds;
double T_prn_mod_samples;
d_code_freq_chips = radial_velocity * GPS_L1_CA_CODE_RATE_HZ;
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
T_chip_mod_seconds = 1 / d_code_freq_chips;
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
d_current_prn_length_samples = round(T_prn_mod_samples);
double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ;
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
double corrected_acq_phase_samples, delay_correction_samples;
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
if (corrected_acq_phase_samples < 0)
{
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
}
delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
d_acq_code_phase_samples = corrected_acq_phase_samples;
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
d_carrier_dopplerrate_hz2 = 0;
d_carrier_phase_step_rad = GPS_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// DLL filter initialization
d_code_loop_filter.initialize(); // initialize the code filter
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
gps_l1_ca_code_gen_float(d_ca_code, d_acquisition_gnss_synchro->PRN, 0);
multicorrelator_cpu.set_local_code_and_taps(static_cast<int>(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
for (int32_t n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs[n] = gr_complex(0, 0);
}
d_carrier_lock_fail_counter = 0;
d_rem_code_phase_samples = 0;
d_rem_carr_phase_rad = 0.0;
d_rem_code_phase_chips = 0.0;
d_acc_carrier_phase_rad = 0.0;
d_carr_phase_sigma2 = 0.0;
d_code_phase_samples = d_acq_code_phase_samples;
std::string sys_ = &d_acquisition_gnss_synchro->System;
sys = sys_.substr(0, 1);
// DEBUG OUTPUT
std::cout << "Tracking of GPS L1 C/A signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
// enable tracking
d_pull_in = true;
d_enable_tracking = true;
LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
<< " Code Phase correction [samples]=" << delay_correction_samples
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
}
Gps_L1_Ca_Kf_Tracking_cc::~Gps_L1_Ca_Kf_Tracking_cc()
{
if (d_dump_file.is_open())
{
try
{
d_dump_file.close();
}
catch (const std::exception &ex)
{
LOG(WARNING) << "Exception in destructor " << ex.what();
}
}
if (d_dump)
{
if (d_channel == 0)
{
std::cout << "Writing .mat files ...";
}
Gps_L1_Ca_Kf_Tracking_cc::save_matfile();
if (d_channel == 0)
{
std::cout << " done." << std::endl;
}
}
try
{
volk_gnsssdr_free(d_local_code_shift_chips);
volk_gnsssdr_free(d_correlator_outs);
volk_gnsssdr_free(d_ca_code);
delete[] d_Prompt_buffer;
multicorrelator_cpu.free();
}
catch (const std::exception &ex)
{
LOG(WARNING) << "Exception in destructor " << ex.what();
}
}
int32_t Gps_L1_Ca_Kf_Tracking_cc::save_matfile()
{
// READ DUMP FILE
std::ifstream::pos_type size;
int32_t number_of_double_vars = 1;
int32_t number_of_float_vars = 19;
int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
sizeof(float) * number_of_float_vars + sizeof(uint32_t);
std::ifstream dump_file;
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
try
{
dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem opening dump file:" << e.what() << std::endl;
return 1;
}
// count number of epochs and rewind
int64_t num_epoch = 0;
if (dump_file.is_open())
{
size = dump_file.tellg();
num_epoch = static_cast<int64_t>(size) / static_cast<int64_t>(epoch_size_bytes);
dump_file.seekg(0, std::ios::beg);
}
else
{
return 1;
}
float *abs_VE = new float[num_epoch];
float *abs_E = new float[num_epoch];
float *abs_P = new float[num_epoch];
float *abs_L = new float[num_epoch];
float *abs_VL = new float[num_epoch];
float *Prompt_I = new float[num_epoch];
float *Prompt_Q = new float[num_epoch];
uint64_t *PRN_start_sample_count = new uint64_t[num_epoch];
float *acc_carrier_phase_rad = new float[num_epoch];
float *carrier_doppler_hz = new float[num_epoch];
float *carrier_dopplerrate_hz2 = new float[num_epoch];
float *code_freq_chips = new float[num_epoch];
float *carr_error_hz = new float[num_epoch];
float *carr_noise_sigma2 = new float[num_epoch];
float *carr_error_filt_hz = new float[num_epoch];
float *code_error_chips = new float[num_epoch];
float *code_error_filt_chips = new float[num_epoch];
float *CN0_SNV_dB_Hz = new float[num_epoch];
float *carrier_lock_test = new float[num_epoch];
float *aux1 = new float[num_epoch];
double *aux2 = new double[num_epoch];
uint32_t *PRN = new uint32_t[num_epoch];
try
{
if (dump_file.is_open())
{
for (int64_t i = 0; i < num_epoch; i++)
{
dump_file.read(reinterpret_cast<char *>(&abs_VE[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&abs_E[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&abs_P[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&abs_L[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&abs_VL[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&Prompt_I[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&Prompt_Q[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count[i]), sizeof(uint64_t));
dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carrier_dopplerrate_hz2[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&code_freq_chips[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carr_error_hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carr_noise_sigma2[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&code_error_chips[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&code_error_filt_chips[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&CN0_SNV_dB_Hz[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&carrier_lock_test[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&aux1[i]), sizeof(float));
dump_file.read(reinterpret_cast<char *>(&aux2[i]), sizeof(double));
dump_file.read(reinterpret_cast<char *>(&PRN[i]), sizeof(uint32_t));
}
}
dump_file.close();
}
catch (const std::ifstream::failure &e)
{
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
delete[] abs_VE;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] abs_VL;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] carrier_dopplerrate_hz2;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_noise_sigma2;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 1;
}
// WRITE MAT FILE
mat_t *matfp;
matvar_t *matvar;
std::string filename = d_dump_filename;
filename.erase(filename.length() - 4, 4);
filename.append(".mat");
matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
if (reinterpret_cast<long *>(matfp) != NULL)
{
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VE, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("abs_VL", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_VL, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, acc_carrier_phase_rad, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_doppler_hz, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_dopplerrate_hz2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_dopplerrate_hz2, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_freq_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_freq_chips, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_hz, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_noise_sigma2", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_noise_sigma2, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carr_error_filt_hz, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_chips, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, code_error_filt_chips, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, CN0_SNV_dB_Hz, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, carrier_lock_test, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux1", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, aux1, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
}
Mat_Close(matfp);
delete[] abs_VE;
delete[] abs_E;
delete[] abs_P;
delete[] abs_L;
delete[] abs_VL;
delete[] Prompt_I;
delete[] Prompt_Q;
delete[] PRN_start_sample_count;
delete[] acc_carrier_phase_rad;
delete[] carrier_doppler_hz;
delete[] carrier_dopplerrate_hz2;
delete[] code_freq_chips;
delete[] carr_error_hz;
delete[] carr_noise_sigma2;
delete[] carr_error_filt_hz;
delete[] code_error_chips;
delete[] code_error_filt_chips;
delete[] CN0_SNV_dB_Hz;
delete[] carrier_lock_test;
delete[] aux1;
delete[] aux2;
delete[] PRN;
return 0;
}
void Gps_L1_Ca_Kf_Tracking_cc::set_channel(uint32_t channel)
{
gr::thread::scoped_lock l(d_setlock);
d_channel = channel;
LOG(INFO) << "Tracking Channel set to " << d_channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump)
{
if (!d_dump_file.is_open())
{
try
{
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
}
}
}
}
void Gps_L1_Ca_Kf_Tracking_cc::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro)
{
d_acquisition_gnss_synchro = p_gnss_synchro;
}
int Gps_L1_Ca_Kf_Tracking_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
// process vars
d_carr_phase_error_rad = 0.0;
double code_error_chips = 0.0;
double code_error_filt_chips = 0.0;
// Block input data and block output stream pointers
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]);
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]);
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
Gnss_Synchro current_synchro_data = Gnss_Synchro();
if (d_enable_tracking == true)
{
// Fill the acquisition data
current_synchro_data = *d_acquisition_gnss_synchro;
// Receiver signal alignment
if (d_pull_in == true)
{
// Signal alignment (skip samples until the incoming signal is aligned with local replica)
uint64_t acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
double acq_trk_shif_correction_samples = static_cast<double>(d_current_prn_length_samples) - std::fmod(static_cast<double>(acq_to_trk_delay_samples), static_cast<double>(d_current_prn_length_samples));
int32_t samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
if (samples_offset < 0)
{
samples_offset = 0;
}
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_acq_code_phase_samples;
d_sample_counter += samples_offset; // count for the processed samples
d_pull_in = false;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.fs = d_fs_in;
current_synchro_data.correlation_length_ms = 1;
*out[0] = current_synchro_data;
// Kalman filter initialization reset
kf_P_x = kf_P_x_ini;
// Update Kalman states based on acquisition information
kf_x(0) = d_carrier_phase_step_rad * samples_offset;
kf_x(1) = d_carrier_doppler_hz;
if (kf_x.n_elem > 2)
{
kf_x(2) = d_carrier_dopplerrate_hz2;
}
// Covariance estimation initialization reset
kf_iter = 0;
bayes_estimator.init(arma::zeros(1, 1), bayes_kappa, bayes_nu, (kf_H * kf_P_x_ini * kf_H.t() + kf_R) * (bayes_nu + 2));
consume_each(samples_offset); // shift input to perform alignment with local replica
return 1;
}
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
// Perform carrier wipe-off and compute Early, Prompt and Late correlation
multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in);
multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad,
d_carrier_phase_step_rad,
d_rem_code_phase_chips,
d_code_phase_step_chips,
d_code_phase_rate_step_chips,
d_current_prn_length_samples);
// ################## Kalman Carrier Tracking ######################################
// Kalman state prediction (time update)
kf_x_pre = kf_F * kf_x; //state prediction
kf_P_x_pre = kf_F * kf_P_x * kf_F.t() + kf_Q; //state error covariance prediction
// Update discriminator [rads/Ti]
d_carr_phase_error_rad = pll_cloop_two_quadrant_atan(d_correlator_outs[1]); // prompt output
// Kalman estimation (measurement update)
double sigma2_phase_detector_cycles2;
double CN_lin = pow(10, d_CN0_SNV_dB_Hz / 10.0);
sigma2_phase_detector_cycles2 = (1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD)) * (1.0 + 1.0 / (2.0 * CN_lin * GPS_L1_CA_CODE_PERIOD));
kf_y(0) = d_carr_phase_error_rad; // measurement vector
kf_R(0, 0) = sigma2_phase_detector_cycles2;
if (bayes_run && (kf_iter >= bayes_ptrans))
{
bayes_estimator.update_sequential(kf_y);
}
if (bayes_run && (kf_iter >= (bayes_ptrans + bayes_strans)))
{
// TODO: Resolve segmentation fault
kf_P_y = bayes_estimator.get_Psi_est();
kf_R_est = kf_P_y - kf_H * kf_P_x_pre * kf_H.t();
}
else
{
kf_P_y = kf_H * kf_P_x_pre * kf_H.t() + kf_R; // innovation covariance matrix
kf_R_est = kf_R;
}
// Kalman filter update step
kf_K = (kf_P_x_pre * kf_H.t()) * arma::inv(kf_P_y); // Kalman gain
kf_x = kf_x_pre + kf_K * kf_y; // updated state estimation
kf_P_x = (arma::eye(size(kf_P_x_pre)) - kf_K * kf_H) * kf_P_x_pre; // update state estimation error covariance matrix
// Store Kalman filter results
d_rem_carr_phase_rad = kf_x(0); // set a new carrier Phase estimation to the NCO
d_carrier_doppler_hz = kf_x(1); // set a new carrier Doppler estimation to the NCO
if (kf_x.n_elem > 2)
{
d_carrier_dopplerrate_hz2 = kf_x(2);
}
else
{
d_carrier_dopplerrate_hz2 = 0;
}
d_carr_phase_sigma2 = kf_R_est(0, 0);
// ################## DLL ##########################################################
// New code Doppler frequency estimation based on carrier frequency estimation
d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ);
// DLL discriminator
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] early and late
// Code discriminator filter
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second]
double T_chip_seconds = 1.0 / static_cast<double>(d_code_freq_chips);
double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS;
double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips * T_chip_seconds); // [seconds]
// ################## CARRIER AND CODE NCO BUFFER ALIGNMENT #######################
// keep alignment parameters for the next input buffer
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
d_current_prn_length_samples = static_cast<int>(round(K_blk_samples)); // round to a discrete number of samples
//################### NCO COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
d_carrier_phase_step_rad = PI_2 * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
// carrier phase accumulator
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
//################### DLL COMMANDS #################################################
// code phase step (Code resampler phase increment per sample) [chips/sample]
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
// remnant code phase [chips]
d_rem_code_phase_samples = K_blk_samples - static_cast<double>(d_current_prn_length_samples); // rounding error < 1 sample
d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast<double>(d_fs_in));
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter < FLAGS_cn0_samples)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt
d_cn0_estimation_counter++;
}
else
{
d_cn0_estimation_counter = 0;
// Code lock indicator
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, FLAGS_cn0_samples, GPS_L1_CA_CODE_PERIOD);
// Carrier lock indicator
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, FLAGS_cn0_samples);
// Loss of lock detection
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < FLAGS_cn0_min)
{
//if (d_channel == 1)
//std::cout << "Carrier Lock Test Fail in channel " << d_channel << ": " << d_carrier_lock_test << " < " << d_carrier_lock_threshold << "," << nfail++ << std::endl;
d_carrier_lock_fail_counter++;
//nfail++;
}
else
{
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter > FLAGS_max_lock_fail)
{
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock
d_carrier_lock_fail_counter = 0;
d_enable_tracking = false;
}
}
// ########### Output the tracking data to navigation and PVT ##########
current_synchro_data.Prompt_I = static_cast<double>((d_correlator_outs[1]).real());
current_synchro_data.Prompt_Q = static_cast<double>((d_correlator_outs[1]).imag());
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples);
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
current_synchro_data.Flag_valid_symbol_output = true;
current_synchro_data.correlation_length_ms = 1;
kf_iter++;
}
else
{
for (int32_t n = 0; n < d_n_correlator_taps; n++)
{
d_correlator_outs[n] = gr_complex(0, 0);
}
current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples);
current_synchro_data.System = {'G'};
current_synchro_data.correlation_length_ms = 1;
}
// assign the GNU Radio block output data
current_synchro_data.fs = d_fs_in;
*out[0] = current_synchro_data;
if (d_dump)
{
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I;
float prompt_Q;
float tmp_E, tmp_P, tmp_L;
float tmp_VE = 0.0;
float tmp_VL = 0.0;
float tmp_float;
double tmp_double;
prompt_I = d_correlator_outs[1].real();
prompt_Q = d_correlator_outs[1].imag();
tmp_E = std::abs<float>(d_correlator_outs[0]);
tmp_P = std::abs<float>(d_correlator_outs[1]);
tmp_L = std::abs<float>(d_correlator_outs[2]);
try
{
// EPR
d_dump_file.write(reinterpret_cast<char *>(&tmp_VE), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&tmp_E), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&tmp_P), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&tmp_L), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&tmp_VL), sizeof(float));
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write(reinterpret_cast<char *>(&prompt_I), sizeof(float));
d_dump_file.write(reinterpret_cast<char *>(&prompt_Q), sizeof(float));
// PRN start sample stamp
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(uint64_t));
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_rad;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// carrier and code frequency
tmp_float = d_carrier_doppler_hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_carrier_dopplerrate_hz2;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_code_freq_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// Kalman commands
tmp_float = static_cast<float>(d_carr_phase_error_rad * GPS_TWO_PI);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = static_cast<float>(d_carr_phase_sigma2);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = static_cast<float>(d_rem_carr_phase_rad * GPS_TWO_PI);
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// DLL commands
tmp_float = code_error_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = code_error_filt_chips;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// CN0 and carrier lock test
tmp_float = d_CN0_SNV_dB_Hz;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_float = d_carrier_lock_test;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
// AUX vars (for debug purposes)
tmp_float = d_rem_code_phase_samples;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
tmp_double = static_cast<double>(d_sample_counter + static_cast<uint64_t>(d_current_prn_length_samples));
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
// PRN
uint32_t prn_ = d_acquisition_gnss_synchro->PRN;
d_dump_file.write(reinterpret_cast<char *>(&prn_), sizeof(uint32_t));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing trk dump file " << e.what();
}
}
consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates
d_sample_counter += d_current_prn_length_samples; // count for the processed samples
return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false
}

222
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h Normal file
View File

@ -0,0 +1,222 @@
/*!
* \file gps_l1_ca_kf_tracking_cc.cc
* \brief Interface of a processing block of a DLL + Kalman carrier
* tracking loop for GPS L1 C/A signals
* \author Javier Arribas, 2018. jarribas(at)cttc.es
* \author Jordi Vila-Valls 2018. jvila(at)cttc.es
* \author Carles Fernandez-Prades 2018. cfernandez(at)cttc.es
*
* Reference:
* J. Vila-Valls, P. Closas, M. Navarro and C. Fernandez-Prades,
* "Are PLLs Dead? A Tutorial on Kalman Filter-based Techniques for Digital
* Carrier Synchronization", IEEE Aerospace and Electronic Systems Magazine,
* Vol. 32, No. 7, pp. 28–45, July 2017. DOI: 10.1109/MAES.2017.150260
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H
#define GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H
#include "gnss_synchro.h"
#include "tracking_2nd_DLL_filter.h"
#include "tracking_2nd_PLL_filter.h"
#include "cpu_multicorrelator_real_codes.h"
#include "bayesian_estimation.h"
#include <armadillo>
#include <gnuradio/block.h>
#include <fstream>
#include <map>
#include <string>
class Gps_L1_Ca_Kf_Tracking_cc;
typedef boost::shared_ptr<Gps_L1_Ca_Kf_Tracking_cc>
gps_l1_ca_kf_tracking_cc_sptr;
gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(uint32_t order,
int64_t if_freq,
int64_t fs_in, uint32_t vector_length,
bool dump,
std::string dump_filename,
float pll_bw_hz,
float early_late_space_chips,
bool bce_run,
uint32_t bce_ptrans,
uint32_t bce_strans,
int32_t bce_nu,
int32_t bce_kappa);
/*!
* \brief This class implements a DLL + PLL tracking loop block
*/
class Gps_L1_Ca_Kf_Tracking_cc : public gr::block
{
public:
~Gps_L1_Ca_Kf_Tracking_cc();
void set_channel(uint32_t channel);
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
void start_tracking();
int general_work(int noutput_items, gr_vector_int& ninput_items,
gr_vector_const_void_star& input_items, gr_vector_void_star& output_items);
void forecast(int noutput_items, gr_vector_int& ninput_items_required);
private:
friend gps_l1_ca_kf_tracking_cc_sptr
gps_l1_ca_kf_make_tracking_cc(uint32_t order,
int64_t if_freq,
int64_t fs_in, uint32_t vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips,
bool bce_run,
uint32_t bce_ptrans,
uint32_t bce_strans,
int32_t bce_nu,
int32_t bce_kappa);
Gps_L1_Ca_Kf_Tracking_cc(uint32_t order,
int64_t if_freq,
int64_t fs_in, uint32_t vector_length,
bool dump,
std::string dump_filename,
float dll_bw_hz,
float early_late_space_chips,
bool bce_run,
uint32_t bce_ptrans,
uint32_t bce_strans,
int32_t bce_nu,
int32_t bce_kappa);
// tracking configuration vars
uint32_t d_order;
uint32_t d_vector_length;
bool d_dump;
Gnss_Synchro* d_acquisition_gnss_synchro;
uint32_t d_channel;
int64_t d_if_freq;
int64_t d_fs_in;
double d_early_late_spc_chips;
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
double d_rem_code_phase_chips;
double d_rem_carr_phase_rad;
// Kalman filter variables
arma::mat kf_P_x_ini; // initial state error covariance matrix
arma::mat kf_P_x; // state error covariance matrix
arma::mat kf_P_x_pre; // Predicted state error covariance matrix
arma::mat kf_P_y; // innovation covariance matrix
arma::mat kf_F; // state transition matrix
arma::mat kf_H; // system matrix
arma::mat kf_R; // measurement error covariance matrix
arma::mat kf_Q; // system error covariance matrix
arma::colvec kf_x; // state vector
arma::colvec kf_x_pre; // predicted state vector
arma::colvec kf_y; // measurement vector
arma::mat kf_K; // Kalman gain matrix
// Bayesian estimator
Bayesian_estimator bayes_estimator;
arma::mat kf_R_est; // measurement error covariance
uint32_t bayes_ptrans;
uint32_t bayes_strans;
int32_t bayes_nu;
int32_t bayes_kappa;
bool bayes_run;
uint32_t kf_iter;
// PLL and DLL filter library
Tracking_2nd_DLL_filter d_code_loop_filter;
// Tracking_2nd_PLL_filter d_carrier_loop_filter;
// acquisition
double d_acq_carrier_doppler_step_hz;
double d_acq_code_phase_samples;
double d_acq_carrier_doppler_hz;
// correlator
int32_t d_n_correlator_taps;
float* d_ca_code;
float* d_local_code_shift_chips;
gr_complex* d_correlator_outs;
cpu_multicorrelator_real_codes multicorrelator_cpu;
// tracking vars
double d_code_freq_chips;
double d_code_phase_step_chips;
double d_code_phase_rate_step_chips;
double d_carrier_doppler_hz;
double d_carrier_dopplerrate_hz2;
double d_carrier_phase_step_rad;
double d_acc_carrier_phase_rad;
double d_carr_phase_error_rad;
double d_carr_phase_sigma2;
double d_code_phase_samples;
double code_error_chips;
double code_error_filt_chips;
// PRN period in samples
int32_t d_current_prn_length_samples;
// processing samples counters
uint64_t d_sample_counter;
uint64_t d_acq_sample_stamp;
// CN0 estimation and lock detector
int32_t d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
double d_carrier_lock_test;
double d_CN0_SNV_dB_Hz;
double d_carrier_lock_threshold;
int32_t d_carrier_lock_fail_counter;
// control vars
bool d_enable_tracking;
bool d_pull_in;
// file dump
std::string d_dump_filename;
std::ofstream d_dump_file;
std::map<std::string, std::string> systemName;
std::string sys;
int32_t save_matfile();
};
#endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H

2
src/algorithms/tracking/libs/CMakeLists.txt
View File

@ -44,6 +44,7 @@ set(TRACKING_LIB_SOURCES
tracking_FLL_PLL_filter.cc
tracking_loop_filter.cc
dll_pll_conf.cc
bayesian_estimation.cc
)
if(ENABLE_FPGA)
@ -56,6 +57,7 @@ include_directories(
${CMAKE_SOURCE_DIR}/src/core/interfaces
${CMAKE_SOURCE_DIR}/src/core/receiver
${CMAKE_SOURCE_DIR}/src/algorithms/libs
${ARMADILLO_INCLUDE_DIRS}
${VOLK_INCLUDE_DIRS}
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}

187
src/algorithms/tracking/libs/bayesian_estimation.cc Normal file
View File

@ -0,0 +1,187 @@
/*!
* \file bayesian_estimation.cc
* \brief Interface of a library with Bayesian noise statistic estimation
*
* Bayesian_estimator is a Bayesian estimator which attempts to estimate
* the properties of a stochastic process based on a sequence of
* discrete samples of the sequence.
*
* [1] TODO: Refs
*
* \authors <ul>
* <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
* <li> Jordi Vila-Valls 2018. jvila(at)cttc.es
* </ul>
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "bayesian_estimation.h"
Bayesian_estimator::Bayesian_estimator()
{
int ny = 1;
mu_prior = arma::zeros(ny, 1);
kappa_prior = 0;
nu_prior = 0;
Psi_prior = arma::eye(ny, ny) * (nu_prior + ny + 1);
mu_est = mu_prior;
Psi_est = Psi_prior;
}
Bayesian_estimator::Bayesian_estimator(int ny)
{
mu_prior = arma::zeros(ny, 1);
kappa_prior = 0;
nu_prior = 0;
Psi_prior = arma::eye(ny, ny) * (nu_prior + ny + 1);
mu_est = mu_prior;
Psi_est = Psi_prior;
}
Bayesian_estimator::Bayesian_estimator(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
{
mu_prior = mu_prior_0;
kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0;
mu_est = mu_prior;
Psi_est = Psi_prior;
}
Bayesian_estimator::~Bayesian_estimator()
{
}
void Bayesian_estimator::init(const arma::mat& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
{
mu_prior = mu_prior_0;
kappa_prior = kappa_prior_0;
nu_prior = nu_prior_0;
Psi_prior = Psi_prior_0;
mu_est = mu_prior;
Psi_est = Psi_prior;
}
/*
* Perform Bayesian noise estimation using the normal-inverse-Wishart priors stored in
* the class structure, and update the priors according to the computed posteriors
*/
void Bayesian_estimator::update_sequential(const arma::vec& data)
{
int K = data.n_cols;
int ny = data.n_rows;
if (mu_prior.is_empty())
{
mu_prior = arma::zeros(ny, 1);
}
if (Psi_prior.is_empty())
{
Psi_prior = arma::zeros(ny, ny);
}
arma::vec y_mean = arma::mean(data, 1);
arma::mat Psi_N = arma::zeros(ny, ny);
for (int kk = 0; kk < K; kk++)
{
Psi_N = Psi_N + (data.col(kk) - y_mean) * ((data.col(kk) - y_mean).t());
}
arma::vec mu_posterior = (kappa_prior * mu_prior + K * y_mean) / (kappa_prior + K);
int kappa_posterior = kappa_prior + K;
int nu_posterior = nu_prior + K;
arma::mat Psi_posterior = Psi_prior + Psi_N + (kappa_prior * K) / (kappa_prior + K) * (y_mean - mu_prior) * ((y_mean - mu_prior).t());
mu_est = mu_posterior;
if ((nu_posterior - ny - 1) > 0)
{
Psi_est = Psi_posterior / (nu_posterior - ny - 1);
}
else
{
Psi_est = Psi_posterior / (nu_posterior + ny + 1);
}
mu_prior = mu_posterior;
kappa_prior = kappa_posterior;
nu_prior = nu_posterior;
Psi_prior = Psi_posterior;
}
/*
* Perform Bayesian noise estimation using a new set of normal-inverse-Wishart priors
* and update the priors according to the computed posteriors
*/
void Bayesian_estimator::update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0)
{
int K = data.n_cols;
int ny = data.n_rows;
arma::vec y_mean = arma::mean(data, 1);
arma::mat Psi_N = arma::zeros(ny, ny);
for (int kk = 0; kk < K; kk++)
{
Psi_N = Psi_N + (data.col(kk) - y_mean) * ((data.col(kk) - y_mean).t());
}
arma::vec mu_posterior = (kappa_prior_0 * mu_prior_0 + K * y_mean) / (kappa_prior_0 + K);
int kappa_posterior = kappa_prior_0 + K;
int nu_posterior = nu_prior_0 + K;
arma::mat Psi_posterior = Psi_prior_0 + Psi_N + (kappa_prior_0 * K) / (kappa_prior_0 + K) * (y_mean - mu_prior_0) * ((y_mean - mu_prior_0).t());
mu_est = mu_posterior;
if ((nu_posterior - ny - 1) > 0)
{
Psi_est = Psi_posterior / (nu_posterior - ny - 1);
}
else
{
Psi_est = Psi_posterior / (nu_posterior + ny + 1);
}
mu_prior = mu_posterior;
kappa_prior = kappa_posterior;
nu_prior = nu_posterior;
Psi_prior = Psi_posterior;
}
arma::mat Bayesian_estimator::get_mu_est() const
{
return mu_est;
}
arma::mat Bayesian_estimator::get_Psi_est() const
{
return Psi_est;
}

85
src/algorithms/tracking/libs/bayesian_estimation.h Normal file
View File

@ -0,0 +1,85 @@
/*!
* \file bayesian_estimation.h
* \brief Interface of a library with Bayesian noise statistic estimation
*
* Bayesian_estimator is a Bayesian estimator which attempts to estimate
* the properties of a stochastic process based on a sequence of
* discrete samples of the sequence.
*
* [1] TODO: Refs
*
* \authors <ul>
* <li> Gerald LaMountain, 2018. gerald(at)ece.neu.edu
* <li> Jordi Vila-Valls 2018. jvila(at)cttc.es
* </ul>
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_BAYESIAN_ESTIMATION_H_
#define GNSS_SDR_BAYESIAN_ESTIMATION_H_
#include <gnuradio/gr_complex.h>
#include <armadillo>
/*! \brief Bayesian_estimator is an estimator of noise characteristics (i.e. mean, covariance)
*
* Bayesian_estimator is an estimator which performs estimation of noise characteristics from
* a sequence of identically and independently distributed (IID) samples of a stationary
* stochastic process by way of Bayesian inference using conjugate priors. The posterior
* distribution is assumed to be Gaussian with mean \mathbf{\mu} and covariance \hat{\mathbf{C}},
* which has a conjugate prior given by a normal-inverse-Wishart distribution with paramemters
* \mathbf{\mu}_{0}, \kappa_{0}, \nu_{0}, and \mathbf{\Psi}.
*
* [1] TODO: Ref1
*
*/
class Bayesian_estimator
{
public:
Bayesian_estimator();
Bayesian_estimator(int ny);
Bayesian_estimator(const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
~Bayesian_estimator();
void init(const arma::mat& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
void update_sequential(const arma::vec& data);
void update_sequential(const arma::vec& data, const arma::vec& mu_prior_0, int kappa_prior_0, int nu_prior_0, const arma::mat& Psi_prior_0);
arma::mat get_mu_est() const;
arma::mat get_Psi_est() const;
private:
arma::vec mu_est;
arma::mat Psi_est;
arma::vec mu_prior;
int kappa_prior;
int nu_prior;
arma::mat Psi_prior;
};
#endif

18
src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc
View File

@ -1,6 +1,6 @@
/*!
* \file cpu_multicorrelator_real_codes.cc
* \brief High optimized CPU vector multiTAP correlator class with real-valued local codes
* \brief Highly optimized CPU vector multiTAP correlator class with real-valued local codes
* \authors <ul>
* <li> Javier Arribas, 2015. jarribas(at)cttc.es
* <li> Cillian O'Driscoll, 2017. cillian.odriscoll(at)gmail.com
@ -46,7 +46,7 @@ cpu_multicorrelator_real_codes::cpu_multicorrelator_real_codes()
d_local_codes_resampled = nullptr;
d_code_length_chips = 0;
d_n_correlators = 0;
d_use_fast_resampler = true;
d_use_high_dynamics_resampler = true;
}
@ -100,9 +100,9 @@ bool cpu_multicorrelator_real_codes::set_input_output_vectors(std::complex<float
void cpu_multicorrelator_real_codes::update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips)
{
if (d_use_fast_resampler)
if (d_use_high_dynamics_resampler)
{
volk_gnsssdr_32f_xn_fast_resampler_32f_xn(d_local_codes_resampled,
volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn(d_local_codes_resampled,
d_local_code_in,
rem_code_phase_chips,
code_phase_step_chips,
@ -131,9 +131,10 @@ bool cpu_multicorrelator_real_codes::Carrier_wipeoff_multicorrelator_resampler(
float phase_step_rad,
float rem_code_phase_chips,
float code_phase_step_chips,
float code_phase_rate_step_chips,
int signal_length_samples)
{
update_local_code(signal_length_samples, rem_code_phase_chips, code_phase_step_chips);
update_local_code(signal_length_samples, rem_code_phase_chips, code_phase_step_chips, code_phase_rate_step_chips);
// Regenerate phase at each call in order to avoid numerical issues
lv_32fc_t phase_offset_as_complex[1];
phase_offset_as_complex[0] = lv_cmake(std::cos(rem_carrier_phase_in_rad), -std::sin(rem_carrier_phase_in_rad));
@ -158,8 +159,9 @@ bool cpu_multicorrelator_real_codes::free()
return true;
}
void cpu_multicorrelator_real_codes::set_fast_resampler(
bool use_fast_resampler)
void cpu_multicorrelator_real_codes::set_high_dynamics_resampler(
bool use_high_dynamics_resampler)
{
d_use_fast_resampler = use_fast_resampler;
d_use_high_dynamics_resampler = use_high_dynamics_resampler;
}

8
src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.h
View File

@ -1,6 +1,6 @@
/*!
* \file cpu_multicorrelator_real_codes.h
* \brief High optimized CPU vector multiTAP correlator class using real-valued local codes
* \brief Highly optimized CPU vector multiTAP correlator class using real-valued local codes
* \authors <ul>
* <li> Javier Arribas, 2015. jarribas(at)cttc.es
* <li> Cillian O'Driscoll, 2017, cillian.odriscoll(at)gmail.com
@ -46,13 +46,13 @@ class cpu_multicorrelator_real_codes
{
public:
cpu_multicorrelator_real_codes();
void set_fast_resampler(bool use_fast_resampler);
void set_high_dynamics_resampler(bool use_high_dynamics_resampler);
~cpu_multicorrelator_real_codes();
bool init(int max_signal_length_samples, int n_correlators);
bool set_local_code_and_taps(int code_length_chips, const float *local_code_in, float *shifts_chips);
bool set_input_output_vectors(std::complex<float> *corr_out, const std::complex<float> *sig_in);
void update_local_code(int correlator_length_samples, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips = 0.0);
bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float rem_code_phase_chips, float code_phase_step_chips, int signal_length_samples);
bool Carrier_wipeoff_multicorrelator_resampler(float rem_carrier_phase_in_rad, float phase_step_rad, float rem_code_phase_chips, float code_phase_step_chips, float code_phase_rate_step_chips, int signal_length_samples);
bool free();
private:
@ -62,7 +62,7 @@ private:
const float *d_local_code_in;
std::complex<float> *d_corr_out;
float *d_shifts_chips;
bool d_use_fast_resampler;
bool d_use_high_dynamics_resampler;
int d_code_length_chips;
int d_n_correlators;
};

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

@ -36,7 +36,7 @@
Dll_Pll_Conf::Dll_Pll_Conf()
{
/* DLL/PLL tracking configuration */
use_fast_resampler = true;
use_high_dynamics_resampler = true;
fs_in = 0.0;
vector_length = 0U;
dump = false;

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

@ -56,7 +56,7 @@ public:
float early_late_space_narrow_chips;
float very_early_late_space_narrow_chips;
int32_t extend_correlation_symbols;
bool use_fast_resampler;
bool use_high_dynamics_resampler;
int32_t cn0_samples;
int32_t carrier_lock_det_mav_samples;
int32_t cn0_min;

26
src/core/monitor/gnss_synchro_monitor.cc
View File

@ -1,6 +1,9 @@
/*!
* \file gnss_synchro_monitor.cc
* \brief Interface of a Position Velocity and Time computation block
* \brief Implementation of a receiver monitoring block which allows sending
* a data stream with the receiver internal parameters (Gnss_Synchro objects)
* to local or remote clients over UDP.
*
* \author Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
*
* -------------------------------------------------------------------------
@ -61,6 +64,8 @@ gnss_synchro_monitor::gnss_synchro_monitor(unsigned int n_channels,
d_nchannels = n_channels;
udp_sink_ptr = std::unique_ptr<Gnss_Synchro_Udp_Sink>(new Gnss_Synchro_Udp_Sink(udp_addresses, udp_port));
count = 0;
}
@ -75,17 +80,16 @@ int gnss_synchro_monitor::work(int noutput_items, gr_vector_const_void_star& inp
const Gnss_Synchro** in = reinterpret_cast<const Gnss_Synchro**>(&input_items[0]); // Get the input buffer pointer
for (int epoch = 0; epoch < noutput_items; epoch++)
{
// ############ 1. READ PSEUDORANGES ####
for (unsigned int i = 0; i < d_nchannels; i++)
count++;
if (count >= d_output_rate_ms)
{
//if (in[i][epoch].Flag_valid_pseudorange)
// {
// }
//todo: send the gnss_synchro objects
std::vector<Gnss_Synchro> stocks;
stocks.push_back(in[i][epoch]);
udp_sink_ptr->write_gnss_synchro(stocks);
for (unsigned int i = 0; i < d_nchannels; i++)
{
std::vector<Gnss_Synchro> stocks;
stocks.push_back(in[i][epoch]);
udp_sink_ptr->write_gnss_synchro(stocks);
}
count = 0;
}
}
return noutput_items;

7
src/core/monitor/gnss_synchro_monitor.h
View File

@ -1,6 +1,9 @@
/*!
* \file gnss_synchro_monitor.h
* \brief Interface of a Position Velocity and Time computation block
* \brief Interface of a receiver monitoring block which allows sending
* a data stream with the receiver internal parameters (Gnss_Synchro objects)
* to local or remote clients over UDP.
*
* \author Álvaro Cebrián Juan, 2018. acebrianjuan(at)gmail.com
*
* -------------------------------------------------------------------------
@ -65,6 +68,8 @@ private:
std::unique_ptr<Gnss_Synchro_Udp_Sink> udp_sink_ptr;
int count;
public:
gnss_synchro_monitor(unsigned int nchannels,

13
src/core/receiver/gnss_block_factory.cc
View File

@ -104,6 +104,7 @@
#include "sbas_l1_telemetry_decoder.h"
#include "hybrid_observables.h"
#include "rtklib_pvt.h"
#include "gps_l1_ca_kf_tracking.h"
#if RAW_UDP
#include "custom_udp_signal_source.h"
@ -1501,6 +1502,12 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
out_streams));
block = std::move(block_);
}
else if (implementation.compare("GPS_L1_CA_KF_Tracking") == 0)
{
std::unique_ptr<GNSSBlockInterface> block_(new GpsL1CaKfTracking(configuration.get(), role, in_streams,
out_streams));
block = std::move(block_);
}
else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0)
{
std::unique_ptr<TrackingInterface> block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams,
@ -1876,6 +1883,12 @@ std::unique_ptr<TrackingInterface> GNSSBlockFactory::GetTrkBlock(
out_streams));
block = std::move(block_);
}
else if (implementation.compare("GPS_L1_CA_KF_Tracking") == 0)
{
std::unique_ptr<TrackingInterface> block_(new GpsL1CaKfTracking(configuration.get(), role, in_streams,
out_streams));
block = std::move(block_);
}
else if (implementation.compare("GPS_L1_CA_DLL_PLL_C_Aid_Tracking") == 0)
{
std::unique_ptr<TrackingInterface> block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams,

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

@ -1157,11 +1157,10 @@ void GNSSFlowgraph::init()
*/
enable_monitor_ = configuration_->property("Monitor.enable_monitor", false);
std::vector<std::string> udp_addr_vec;
std::string address_string = configuration_->property("Monitor.client_addresses", std::string("127.0.0.1"));
//todo: split the string in substrings using the separator and fill the address vector.
udp_addr_vec.push_back(address_string);
std::vector<std::string> udp_addr_vec = split_string(address_string, '_');
std::sort(udp_addr_vec.begin(), udp_addr_vec.end());
udp_addr_vec.erase(std::unique(udp_addr_vec.begin(), udp_addr_vec.end()), udp_addr_vec.end());
if (enable_monitor_)
{
@ -1183,7 +1182,7 @@ void GNSSFlowgraph::set_signals_list()
11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28,
29, 30, 31, 32};
std::set<unsigned int> available_sbas_prn = {120, 124, 126};
std::set<unsigned int> available_sbas_prn = {123, 131, 135, 136, 138};
std::set<unsigned int> available_galileo_prn = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10,
11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28,
@ -1599,3 +1598,17 @@ Gnss_Signal GNSSFlowgraph::search_next_signal(std::string searched_signal, bool
}
return result;
}
std::vector<std::string> GNSSFlowgraph::split_string(const std::string &s, char delim)
{
std::vector<std::string> v;
std::stringstream ss(s);
std::string item;
while (std::getline(ss, item, delim))
{
*(std::back_inserter(v)++) = item;
}
return v;
}

1
src/core/receiver/gnss_flowgraph.h
View File

@ -186,6 +186,7 @@ private:
bool enable_monitor_;
gr::basic_block_sptr GnssSynchroMonitor_;
std::vector<std::string> split_string(const std::string &s, char delim);
};
#endif /*GNSS_SDR_GNSS_FLOWGRAPH_H_*/

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

@ -59,6 +59,7 @@ const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-ca
const double Galileo_E1_SUB_CARRIER_B_RATE_HZ = 6.138e6; //!< Galileo E1 sub-carrier 'b' rate [Hz]
const double Galileo_E1_B_CODE_LENGTH_CHIPS = 4092.0; //!< Galileo E1-B code length [chips]
const double Galileo_E1_B_SYMBOL_RATE_BPS = 250.0; //!< Galileo E1-B symbol rate [bits/second]
const int32_t Galileo_E1_B_SAMPLES_PER_SYMBOL = 1; //!< (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS) / Galileo_E1_B_SYMBOL_RATE_BPS
const int32_t Galileo_E1_C_SECONDARY_CODE_LENGTH = 25; //!< Galileo E1-C secondary code length [chips]
const int32_t Galileo_E1_NUMBER_OF_CODES = 50;
@ -70,10 +71,7 @@ const int32_t GALILEO_E1_HISTORY_DEEP = 100;
// Galileo INAV Telemetry structure
#define GALILEO_INAV_PREAMBLE \
{ \
0, 1, 0, 1, 1, 0, 0, 0, 0, 0 \
}
const std::string GALILEO_INAV_PREAMBLE = {"0101100000"};
const int32_t GALILEO_INAV_PREAMBLE_LENGTH_BITS = 10;
const double GALILEO_INAV_PAGE_PART_WITH_PREABLE_SECONDS = 2.0 + GALILEO_INAV_PREAMBLE_LENGTH_BITS * Galileo_E1_CODE_PERIOD;

49
src/core/system_parameters/gnss_satellite.cc
View File

@ -184,26 +184,30 @@ void Gnss_Satellite::set_PRN(uint32_t PRN_)
}
else if (system.compare("SBAS") == 0)
{
if (PRN_ == 122)
if (PRN_ == 120)
{
PRN = PRN_;
} // WAAS Inmarsat 3F4 (AOR-W)
else if (PRN_ == 134)
} // EGNOS Test Platform.Inmarsat 3-F2 (Atlantic Ocean Region-East)
else if (PRN_ == 123)
{
PRN = PRN_;
} // WAAS Inmarsat 3F3 (POR)
else if (PRN_ == 120)
} // EGNOS Operational Platform. Astra 5B
else if (PRN_ == 131)
{
PRN = PRN_;
} // EGNOS AOR-E Broadcast satellite http://www.egnos-pro.esa.int/index.html
else if (PRN_ == 124)
} // WAAS Eutelsat 117 West B
else if (PRN_ == 135)
{
PRN = PRN_;
} // EGNOS ESA ARTEMIS used for EGNOS Operations
else if (PRN_ == 126)
} // WAAS Galaxy 15
else if (PRN_ == 136)
{
PRN = PRN_;
} // EGNOS IOR-W currently used by Industry to perform various tests on the system.
} // EGNOS Operational Platform. SES-5 (a.k.a. Sirius 5 or Astra 4B)
else if (PRN_ == 138)
{
PRN = PRN_;
} // WAAS Anik F1R
else
{
DLOG(INFO) << "This PRN is not defined";
@ -492,20 +496,23 @@ std::string Gnss_Satellite::what_block(const std::string& system_, uint32_t PRN_
{
switch (PRN_)
{
case 122:
block_ = std::string("WAAS"); // WAAS Inmarsat 3F4 (AOR-W)
break;
case 134:
block_ = std::string("WAAS"); // WAAS Inmarsat 3F3 (POR)
break;
case 120:
block_ = std::string("EGNOS"); // EGNOS AOR-E Broadcast satellite http://www.egnos-pro.esa.int/index.html
block_ = std::string("EGNOS Test Platform"); // Inmarsat 3-F2 (Atlantic Ocean Region-East)
break;
case 124:
block_ = std::string("EGNOS"); // EGNOS ESA ARTEMIS used for EGNOS Operations
case 123:
block_ = std::string("EGNOS"); // EGNOS Operational Platform. Astra 5B
break;
case 126:
block_ = std::string("EGNOS"); // EGNOS IOR-W currently used by Industry to perform various tests on the system.
case 131:
block_ = std::string("WAAS"); // WAAS Eutelsat 117 West B
break;
case 135:
block_ = std::string("WAAS"); // WAAS Galaxy 15
break;
case 136:
block_ = std::string("EGNOS"); // EGNOS Operational Platform. SES-5 (a.k.a. Sirius 5 or Astra 4B)
break;
case 138:
block_ = std::string("WAAS"); // WAAS Anik F1R
break;
default:
block_ = std::string("Unknown");

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

@ -53,6 +53,7 @@ public:
double Acq_delay_samples; //!< Set by Acquisition processing block
double Acq_doppler_hz; //!< Set by Acquisition processing block
uint64_t Acq_samplestamp_samples; //!< Set by Acquisition processing block
uint32_t Acq_doppler_step; //!< Set by Acquisition processing block
bool Flag_valid_acquisition; //!< Set by Acquisition processing block
// Tracking
@ -96,6 +97,7 @@ public:
ar& Acq_delay_samples;
ar& Acq_doppler_hz;
ar& Acq_samplestamp_samples;
ar& Acq_doppler_step;
ar& Flag_valid_acquisition;
// Tracking
ar& fs;

34
src/tests/CMakeLists.txt
View File

@ -18,6 +18,7 @@
add_subdirectory(unit-tests/signal-processing-blocks/libs)
add_subdirectory(system-tests/libs)
################################################################################
# Google Test - https://github.com/google/googletest
@ -342,6 +343,7 @@ set(LIST_INCLUDE_DIRS
${CMAKE_SOURCE_DIR}/src/algorithms/acquisition/gnuradio_blocks
${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs
${CMAKE_SOURCE_DIR}/src/tests/unit-tests/signal-processing-blocks/libs
${CMAKE_SOURCE_DIR}/src/tests/system-tests/libs
${CMAKE_SOURCE_DIR}/src/tests/common-files
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
@ -489,29 +491,24 @@ if(ENABLE_SYSTEM_TESTING)
${GTEST_LIBRARIES} ${GNURADIO_RUNTIME_LIBRARIES}
${GNURADIO_BLOCKS_LIBRARIES} ${GNURADIO_FILTER_LIBRARIES}
${GNURADIO_ANALOG_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES}
gnss_sp_libs gnss_rx gnss_system_parameters )
gnss_sp_libs gnss_rx gnss_system_parameters
system_testing_lib)
add_system_test(position_test)
if(GPSTK_FOUND OR OWN_GPSTK)
#if(GPSTK_FOUND OR OWN_GPSTK)
## OBS_SYSTEM_TEST and OBS_GPS_L1_SYSTEM_TEST
set(OPT_LIBS_ ${GFlags_LIBS} ${GLOG_LIBRARIES} ${GTEST_LIBRARIES}
gnss_sp_libs gnss_rx ${gpstk_libs} )
set(OPT_INCLUDES_ ${GPSTK_INCLUDE_DIRS} ${GPSTK_INCLUDE_DIRS}/gpstk)
add_system_test(obs_gps_l1_system_test)
add_system_test(obs_system_test)
endif(GPSTK_FOUND OR OWN_GPSTK)
# set(OPT_LIBS_ ${GFlags_LIBS} ${GLOG_LIBRARIES} ${GTEST_LIBRARIES}
# gnss_sp_libs gnss_rx ${gpstk_libs} )
# set(OPT_INCLUDES_ ${GPSTK_INCLUDE_DIRS} ${GPSTK_INCLUDE_DIRS}/gpstk)
# add_system_test(obs_gps_l1_system_test)
# add_system_test(obs_system_test)
#endif(GPSTK_FOUND OR OWN_GPSTK)
else(ENABLE_SYSTEM_TESTING_EXTRA)
# Avoid working with old executables if they were switched ON and then OFF
if(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test)
file(REMOVE ${CMAKE_SOURCE_DIR}/install/position_test)
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test)
if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test)
file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_system_test)
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test)
endif(ENABLE_SYSTEM_TESTING_EXTRA)
else(ENABLE_SYSTEM_TESTING)
# Avoid working with old executables if they were switched ON and then OFF
@ -521,12 +518,6 @@ else(ENABLE_SYSTEM_TESTING)
if(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test)
file(REMOVE ${CMAKE_SOURCE_DIR}/install/position_test)
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/position_test)
if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_gps_l1_system_test)
if(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test)
file(REMOVE ${CMAKE_SOURCE_DIR}/install/obs_system_test)
endif(EXISTS ${CMAKE_SOURCE_DIR}/install/obs_system_test)
endif(ENABLE_SYSTEM_TESTING)
@ -666,7 +657,8 @@ endif(NOT ${GTEST_DIR_LOCAL})
add_executable(trk_test ${CMAKE_CURRENT_SOURCE_DIR}/single_test_main.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/galileo_e1_dll_pll_veml_tracking_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc )
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc
${CMAKE_CURRENT_SOURCE_DIR}/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc )
target_link_libraries(trk_test ${Boost_LIBRARIES}
${GFlags_LIBS}

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

@ -35,7 +35,7 @@
#include <limits>
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)");
DEFINE_bool(compute_single_diffs, false, "Compute also the single difference errors for Accumulated Carrier Phase and Carrier Doppler (requires LO synchronization between receivers)");
DEFINE_bool(compare_with_5X, false, "Compare the E5a Doppler and Carrier Phases with the E5 full bw in RINEX (expect discrepancy due to the center frequencies differences");
#endif

2
src/tests/common-files/signal_generator_flags.h
View File

@ -38,7 +38,7 @@ DEFINE_bool(disable_generator, false, "Disable the signal generator (a external
DEFINE_string(generator_binary, std::string(SW_GENERATOR_BIN), "Path of software-defined signal generator binary");
DEFINE_string(rinex_nav_file, std::string(DEFAULT_RINEX_NAV), "Input RINEX navigation file");
DEFINE_int32(duration, 100, "Duration of the experiment [in seconds, max = 300]");
DEFINE_string(static_position, "30.286502,120.032669,100", "Static receiver position [log,lat,height]");
DEFINE_string(static_position, "30.286502,120.032669,100", "Static receiver position [latitude,longitude,height]");
DEFINE_string(dynamic_position, "", "Observer positions file, in .csv or .nmea format");
DEFINE_string(filename_rinex_obs, "sim.16o", "Filename of output RINEX navigation file");
DEFINE_string(filename_raw_data, "signal_out.bin", "Filename of output raw data file");

41
src/tests/system-tests/libs/CMakeLists.txt Normal file
View File

@ -0,0 +1,41 @@
# Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors)
#
# This file is part of GNSS-SDR.
#
# GNSS-SDR is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# GNSS-SDR is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
#
set(SYSTEM_TESTING_LIB_SOURCES
geofunctions.cc
spirent_motion_csv_dump_reader.cc
rtklib_solver_dump_reader.cc
)
include_directories(
${CMAKE_CURRENT_SOURCE_DIR}
${GLOG_INCLUDE_DIRS}
${GFlags_INCLUDE_DIRS}
${MATIO_INCLUDE_DIRS}
)
file(GLOB SYSTEM_TESTING_LIB_HEADERS "*.h")
list(SORT SYSTEM_TESTING_LIB_HEADERS)
add_library(system_testing_lib ${SYSTEM_TESTING_LIB_SOURCES} ${SYSTEM_TESTING_LIB_HEADERS})
source_group(Headers FILES ${SYSTEM_TESTING_LIB_HEADERS})
if(NOT MATIO_FOUND)
add_dependencies(system_testing_lib matio-${GNSSSDR_MATIO_LOCAL_VERSION})
endif(NOT MATIO_FOUND)

473
src/tests/system-tests/libs/geofunctions.cc Normal file
View File

@ -0,0 +1,473 @@
/*!
* \file geofunctions.h
* \brief A set of coordinate transformations functions and helpers,
* some of them migrated from MATLAB, for geographic information systems.
* \author Javier Arribas, 2018. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "geofunctions.h"
#define STRP_G_SI 9.80665
#define STRP_PI 3.1415926535898 //!< Pi as defined in IS-GPS-200E
arma::mat Skew_symmetric(arma::vec a)
{
arma::mat A = arma::zeros(3, 3);
A << 0.0 << -a(2) << a(1) << arma::endr
<< a(2) << 0.0 << -a(0) << arma::endr
<< -a(1) << a(0) << 0 << arma::endr;
// {{0, -a(2), a(1)},
// {a(2), 0, -a(0)},
// {-a(1), a(0), 0}};
return A;
}
double WGS84_g0(double Lat_rad)
{
double k = 0.001931853; //normal gravity constant
double e2 = 0.00669438002290; //the square of the first numerical eccentricity
double nge = 9.7803253359; //normal gravity value on the equator (m/sec^2)
double b = sin(Lat_rad); //Lat in degrees
b = b * b;
double g0 = nge * (1 + k * b) / (sqrt(1 - e2 * b));
return g0;
}
double WGS84_geocentric_radius(double Lat_geodetic_rad)
{
//WGS84 earth model Geocentric radius (Eq. 2.88)
double WGS84_A = 6378137.0; //Semi-major axis of the Earth, a [m]
double WGS84_IF = 298.257223563; //Inverse flattening of the Earth
double WGS84_F = (1 / WGS84_IF); //The flattening of the Earth
//double WGS84_B=(WGS84_A*(1-WGS84_F)); // Semi-minor axis of the Earth [m]
double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); //Eccentricity of the Earth
//transverse radius of curvature
double R_E = WGS84_A / sqrt(1 -
WGS84_E * WGS84_E *
sin(Lat_geodetic_rad) *
sin(Lat_geodetic_rad)); // (Eq. 2.66)
//gocentric radius at the Earth surface
double r_eS = R_E * sqrt(cos(Lat_geodetic_rad) * cos(Lat_geodetic_rad) +
(1 - WGS84_E * WGS84_E) * (1 - WGS84_E * WGS84_E) * sin(Lat_geodetic_rad) * sin(Lat_geodetic_rad)); // (Eq. 2.88)
return r_eS;
}
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx)
{
double lambda;
double phi;
double h;
double dtr = STRP_PI / 180.0;
double a = 6378137.0; // semi-major axis of the reference ellipsoid WGS-84
double finv = 298.257223563; // inverse of flattening of the reference ellipsoid WGS-84
// Transform x into geodetic coordinates
togeod(&phi, &lambda, &h, a, finv, x(0), x(1), x(2));
double cl = cos(lambda * dtr);
double sl = sin(lambda * dtr);
double cb = cos(phi * dtr);
double sb = sin(phi * dtr);
arma::mat F = arma::zeros(3, 3);
F(0, 0) = -sl;
F(0, 1) = -sb * cl;
F(0, 2) = cb * cl;
F(1, 0) = cl;
F(1, 1) = -sb * sl;
F(1, 2) = cb * sl;
F(2, 0) = 0;
F(2, 1) = cb;
F(2, 2) = sb;
arma::vec local_vector;
local_vector = arma::htrans(F) * dx;
double E = local_vector(0);
double N = local_vector(1);
double U = local_vector(2);
double hor_dis;
hor_dis = sqrt(E * E + N * N);
if (hor_dis < 1.0E-20)
{
*Az = 0;
*El = 90;
}
else
{
*Az = atan2(E, N) / dtr;
*El = atan2(U, hor_dis) / dtr;
}
if (*Az < 0)
{
*Az = *Az + 360.0;
}
*D = sqrt(dx(0) * dx(0) + dx(1) * dx(1) + dx(2) * dx(2));
return 0;
}
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z)
{
*h = 0;
double tolsq = 1.e-10; // tolerance to accept convergence
int maxit = 10; // max number of iterations
double rtd = 180.0 / STRP_PI;
// compute square of eccentricity
double esq;
if (finv < 1.0E-20)
{
esq = 0.0;
}
else
{
esq = (2.0 - 1.0 / finv) / finv;
}
// first guess
double P = sqrt(X * X + Y * Y); // P is distance from spin axis
//direct calculation of longitude
if (P > 1.0E-20)
{
*dlambda = atan2(Y, X) * rtd;
}
else
{
*dlambda = 0.0;
}
// correct longitude bound
if (*dlambda < 0)
{
*dlambda = *dlambda + 360.0;
}
double r = sqrt(P * P + Z * Z); // r is distance from origin (0,0,0)
double sinphi;
if (r > 1.0E-20)
{
sinphi = Z / r;
}
else
{
sinphi = 0.0;
}
*dphi = asin(sinphi);
// initial value of height = distance from origin minus
// approximate distance from origin to surface of ellipsoid
if (r < 1.0E-20)
{
*h = 0;
return 1;
}
*h = r - a * (1 - sinphi * sinphi / finv);
// iterate
double cosphi;
double N_phi;
double dP;
double dZ;
double oneesq = 1.0 - esq;
for (int i = 0; i < maxit; i++)
{
sinphi = sin(*dphi);
cosphi = cos(*dphi);
// compute radius of curvature in prime vertical direction
N_phi = a / sqrt(1 - esq * sinphi * sinphi);
// compute residuals in P and Z
dP = P - (N_phi + (*h)) * cosphi;
dZ = Z - (N_phi * oneesq + (*h)) * sinphi;
// update height and latitude
*h = *h + (sinphi * dZ + cosphi * dP);
*dphi = *dphi + (cosphi * dZ - sinphi * dP) / (N_phi + (*h));
// test for convergence
if ((dP * dP + dZ * dZ) < tolsq)
{
break;
}
if (i == (maxit - 1))
{
// LOG(WARNING) << "The computation of geodetic coordinates did not converge";
}
}
*dphi = (*dphi) * rtd;
return 0;
}
arma::mat Gravity_ECEF(arma::vec r_eb_e)
{
//Parameters
double R_0 = 6378137; //WGS84 Equatorial radius in meters
double mu = 3.986004418E14; //WGS84 Earth gravitational constant (m^3 s^-2)
double J_2 = 1.082627E-3; //WGS84 Earth's second gravitational constant
double omega_ie = 7.292115E-5; // Earth rotation rate (rad/s)
// Calculate distance from center of the Earth
double mag_r = sqrt(arma::as_scalar(r_eb_e.t() * r_eb_e));
// If the input position is 0,0,0, produce a dummy output
arma::vec g = arma::zeros(3, 1);
if (mag_r != 0)
{
//Calculate gravitational acceleration using (2.142)
double z_scale = 5 * pow((r_eb_e(2) / mag_r), 2);
arma::vec tmp_vec = {(1 - z_scale) * r_eb_e(0),
(1 - z_scale) * r_eb_e(1),
(3 - z_scale) * r_eb_e(2)};
arma::vec gamma_ = (-mu / pow(mag_r, 3)) * (r_eb_e + 1.5 * J_2 * pow(R_0 / mag_r, 2) * tmp_vec);
//Add centripetal acceleration using (2.133)
g(0) = gamma_(0) + pow(omega_ie, 2) * r_eb_e(0);
g(1) = gamma_(1) + pow(omega_ie, 2) * r_eb_e(1);
g(2) = gamma_(2);
}
return g;
}
arma::vec LLH_to_deg(arma::vec LLH)
{
double rtd = 180.0 / STRP_PI;
LLH(0) = LLH(0) * rtd;
LLH(1) = LLH(1) * rtd;
return LLH;
}
double degtorad(double angleInDegrees)
{
double angleInRadians = (STRP_PI / 180.0) * angleInDegrees;
return angleInRadians;
}
double radtodeg(double angleInRadians)
{
double angleInDegrees = (180.0 / STRP_PI) * angleInRadians;
return angleInDegrees;
}
double mstoknotsh(double MetersPerSeconds)
{
double knots = mstokph(MetersPerSeconds) * 0.539957;
return knots;
}
double mstokph(double MetersPerSeconds)
{
double kph = 3600.0 * MetersPerSeconds / 1e3;
return kph;
}
arma::vec CTM_to_Euler(arma::mat C)
{
//Calculate Euler angles using (2.23)
arma::vec eul = arma::zeros(3, 1);
eul(0) = atan2(C(1, 2), C(2, 2)); // roll
if (C(0, 2) < -1.0) C(0, 2) = -1.0;
if (C(0, 2) > 1.0) C(0, 2) = 1.0;
eul(1) = -asin(C(0, 2)); // pitch
eul(2) = atan2(C(0, 1), C(0, 0)); // yaw
return eul;
}
arma::mat Euler_to_CTM(arma::vec eul)
{
//Eq.2.15
//Euler angles to Attitude matrix is equivalent to rotate the body
//in the three axes:
// arma::mat Ax= {{1,0,0}, {0,cos(Att_phi),sin(Att_phi)} ,{0,-sin(Att_phi),cos(Att_phi)}};
// arma::mat Ay= {{cos(Att_theta), 0, -sin(Att_theta)}, {0,1,0} , {sin(Att_theta), 0, cos(Att_theta)}};
// arma::mat Az= {{cos(Att_psi), sin(Att_psi), 0}, {-sin(Att_psi), cos(Att_psi), 0},{0,0,1}};
// arma::mat C_b_n=Ax*Ay*Az; // Attitude expressed in the LOCAL FRAME (NED)
// C_b_n=C_b_n.t();
//Precalculate sines and cosines of the Euler angles
double sin_phi = sin(eul(0));
double cos_phi = cos(eul(0));
double sin_theta = sin(eul(1));
double cos_theta = cos(eul(1));
double sin_psi = sin(eul(2));
double cos_psi = cos(eul(2));
arma::mat C = arma::zeros(3, 3);
//Calculate coordinate transformation matrix using (2.22)
C(0, 0) = cos_theta * cos_psi;
C(0, 1) = cos_theta * sin_psi;
C(0, 2) = -sin_theta;
C(1, 0) = -cos_phi * sin_psi + sin_phi * sin_theta * cos_psi;
C(1, 1) = cos_phi * cos_psi + sin_phi * sin_theta * sin_psi;
C(1, 2) = sin_phi * cos_theta;
C(2, 0) = sin_phi * sin_psi + cos_phi * sin_theta * cos_psi;
C(2, 1) = -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi;
C(2, 2) = cos_phi * cos_theta;
return C;
}
arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection)
{
const double a[5] = {6378388.0, 6378160.0, 6378135.0, 6378137.0, 6378137.0};
const double f[5] = {1.0 / 297.0, 1.0 / 298.247, 1.0 / 298.26, 1.0 / 298.257222101, 1.0 / 298.257223563};
double lambda = atan2(XYZ[1], XYZ[0]);
double ex2 = (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] / ((1.0 - f[elipsoid_selection]) * (1.0 - f[elipsoid_selection]));
double c = a[elipsoid_selection] * sqrt(1.0 + ex2);
double phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection])) * f[elipsoid_selection])));
double h = 0.1;
double oldh = 0.0;
double N;
int iterations = 0;
do
{
oldh = h;
N = c / sqrt(1 + ex2 * (cos(phi) * cos(phi)));
phi = atan(XYZ[2] / ((sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) * (1.0 - (2.0 - f[elipsoid_selection]) * f[elipsoid_selection] * N / (N + h)))));
h = sqrt(XYZ[0] * XYZ[0] + XYZ[1] * XYZ[1]) / cos(phi) - N;
iterations = iterations + 1;
if (iterations > 100)
{
//std::cout << "Failed to approximate h with desired precision. h-oldh= " << h - oldh;
break;
}
}
while (std::abs(h - oldh) > 1.0e-12);
arma::vec LLH = {{phi, lambda, h}}; //radians
return LLH;
}
void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n)
{
//Compute the Latitude of the ECEF position
LLH = cart2geo(r_eb_e, 4); //ECEF -> WGS84 geographical
// Calculate ECEF to Geographical coordinate transformation matrix using (2.150)
double cos_lat = cos(LLH(0));
double sin_lat = sin(LLH(0));
double cos_long = cos(LLH(1));
double sin_long = sin(LLH(1));
//C++11 and arma >= 5.2
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
// {-sin_long, cos_long, 0},
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}}; //ECEF to Geo
//C++98 arma <5.2
arma::mat C_e_n = arma::zeros(3, 3);
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
<< -sin_long << cos_long << 0 << arma::endr
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr; //ECEF to Geo
// Transform velocity using (2.73)
v_eb_n = C_e_n * v_eb_e;
C_b_n = C_e_n * C_b_e; // Attitude conversion from ECEF to NED
}
void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e)
{
// Parameters
double R_0 = 6378137; //WGS84 Equatorial radius in meters
double e = 0.0818191908425; //WGS84 eccentricity
// Calculate transverse radius of curvature using (2.105)
double R_E = R_0 / sqrt(1 - (e * sin(LLH(0))) * (e * sin(LLH(0))));
// Convert position using (2.112)
double cos_lat = cos(LLH(0));
double sin_lat = sin(LLH(0));
double cos_long = cos(LLH(1));
double sin_long = sin(LLH(1));
r_eb_e = {(R_E + LLH(2)) * cos_lat * cos_long,
(R_E + LLH(2)) * cos_lat * sin_long,
((1 - e * e) * R_E + LLH(2)) * sin_lat};
//Calculate ECEF to Geo coordinate transformation matrix using (2.150)
//C++11 and arma>=5.2
// arma::mat C_e_n = {{-sin_lat * cos_long, -sin_lat * sin_long, cos_lat},
// {-sin_long, cos_long, 0},
// {-cos_lat * cos_long, -cos_lat * sin_long, -sin_lat}};
//C++98 arma <5.2
//Calculate ECEF to Geo coordinate transformation matrix using (2.150)
arma::mat C_e_n = arma::zeros(3, 3);
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
<< -sin_long << cos_long << 0 << arma::endr
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr;
// Transform velocity using (2.73)
v_eb_e = C_e_n.t() * v_eb_n;
// Transform attitude using (2.15)
C_b_e = C_e_n.t() * C_b_n;
}
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e)
{
//% Parameters
double R_0 = 6378137; //WGS84 Equatorial radius in meters
double e = 0.0818191908425; //WGS84 eccentricity
// Calculate transverse radius of curvature using (2.105)
double R_E = R_0 / sqrt(1 - pow(e * sin(L_b), 2));
// Convert position using (2.112)
double cos_lat = cos(L_b);
double sin_lat = sin(L_b);
double cos_long = cos(lambda_b);
double sin_long = sin(lambda_b);
r_eb_e = {(R_E + h_b) * cos_lat * cos_long,
(R_E + h_b) * cos_lat * sin_long,
((1 - pow(e, 2)) * R_E + h_b) * sin_lat};
// Calculate ECEF to Geo coordinate transformation matrix using (2.150)
arma::mat C_e_n = arma::zeros(3, 3);
C_e_n << -sin_lat * cos_long << -sin_lat * sin_long << cos_lat << arma::endr
<< -sin_long << cos_long << 0 << arma::endr
<< -cos_lat * cos_long << -cos_lat * sin_long << -sin_lat << arma::endr;
// Transform velocity using (2.73)
v_eb_e = C_e_n.t() * v_eb_n;
}

156
src/tests/system-tests/libs/geofunctions.h Normal file
View File

@ -0,0 +1,156 @@
/*!
* \file geofunctions.h
* \brief A set of coordinate transformations functions and helpers,
* some of them migrated from MATLAB, for geographic information systems.
* \author Javier Arribas, 2018. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GEOFUNCTIONS_H
#define GEOFUNCTIONS_H
#include <armadillo>
// %Skew_symmetric - Calculates skew-symmetric matrix
arma::mat Skew_symmetric(arma::vec a);
double WGS84_g0(double Lat_rad);
double WGS84_geocentric_radius(double Lat_geodetic_rad);
/* Transformation of vector dx into topocentric coordinate
system with origin at x
Inputs:
x - vector origin coordinates (in ECEF system [X; Y; Z;])
dx - vector ([dX; dY; dZ;]).
Outputs:
D - vector length. Units like the input
Az - azimuth from north positive clockwise, degrees
El - elevation angle, degrees
Based on a Matlab function by Kai Borre
*/
int topocent(double *Az, double *El, double *D, const arma::vec &x, const arma::vec &dx);
/* Subroutine to calculate geodetic coordinates latitude, longitude,
height given Cartesian coordinates X,Y,Z, and reference ellipsoid
values semi-major axis (a) and the inverse of flattening (finv).
The output units of angular quantities will be in decimal degrees
(15.5 degrees not 15 deg 30 min). The output units of h will be the
same as the units of X,Y,Z,a.
Inputs:
a - semi-major axis of the reference ellipsoid
finv - inverse of flattening of the reference ellipsoid
X,Y,Z - Cartesian coordinates
Outputs:
dphi - latitude
dlambda - longitude
h - height above reference ellipsoid
Based in a Matlab function by Kai Borre
*/
int togeod(double *dphi, double *dlambda, double *h, double a, double finv, double X, double Y, double Z);
//Gravitation_ECI - Calculates acceleration due to gravity resolved about
//ECEF-frame
arma::mat Gravity_ECEF(arma::vec r_eb_e);
/* Conversion of Cartesian coordinates (X,Y,Z) to geographical
coordinates (latitude, longitude, h) on a selected reference ellipsoid.
Choices of Reference Ellipsoid for Geographical Coordinates
0. International Ellipsoid 1924
1. International Ellipsoid 1967
2. World Geodetic System 1972
3. Geodetic Reference System 1980
4. World Geodetic System 1984
*/
arma::vec cart2geo(arma::vec XYZ, int elipsoid_selection);
arma::vec LLH_to_deg(arma::vec LLH);
double degtorad(double angleInDegrees);
double radtodeg(double angleInRadians);
double mstoknotsh(double MetersPerSeconds);
double mstokph(double Kph);
arma::vec CTM_to_Euler(arma::mat C);
arma::mat Euler_to_CTM(arma::vec eul);
void ECEF_to_Geo(arma::vec r_eb_e, arma::vec v_eb_e, arma::mat C_b_e, arma::vec &LLH, arma::vec &v_eb_n, arma::mat &C_b_n);
// %
// % Inputs:
// % L_b latitude (rad)
// % lambda_b longitude (rad)
// % h_b height (m)
// % v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
// % north, east, and down (m/s)
// % C_b_n body-to-NED coordinate transformation matrix
// %
// % Outputs:
// % r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
// % along ECEF-frame axes (m)
// % v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
// % ECEF-frame axes (m/s)
// % C_b_e body-to-ECEF-frame coordinate transformation matrix
//
// % Copyright 2012, Paul Groves
// % License: BSD; see license.txt for details
void Geo_to_ECEF(arma::vec LLH, arma::vec v_eb_n, arma::mat C_b_n, arma::vec &r_eb_e, arma::vec &v_eb_e, arma::mat &C_b_e);
//pv_Geo_to_ECEF - Converts curvilinear to Cartesian position and velocity
//resolving axes from NED to ECEF
//This function created 11/4/2012 by Paul Groves
//%
//% Inputs:
//% L_b latitude (rad)
//% lambda_b longitude (rad)
//% h_b height (m)
//% v_eb_n velocity of body frame w.r.t. ECEF frame, resolved along
//% north, east, and down (m/s)
//%
//% Outputs:
//% r_eb_e Cartesian position of body frame w.r.t. ECEF frame, resolved
//% along ECEF-frame axes (m)
//% v_eb_e velocity of body frame w.r.t. ECEF frame, resolved along
//% ECEF-frame axes (m/s)
void pv_Geo_to_ECEF(double L_b, double lambda_b, double h_b, arma::vec v_eb_n, arma::vec &r_eb_e, arma::vec &v_eb_e);
#endif

46
src/tests/system-tests/libs/position_test_flags.h Normal file
View File

@ -0,0 +1,46 @@
/*!
* \file signal_generator_flags.h
* \brief Helper file for unit testing
* \author Javier Arribas, 2018. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_POSITION_TEST_FLAGS_H_
#define GNSS_SDR_POSITION_TEST_FLAGS_H_
#include <gflags/gflags.h>
#include <limits>
DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test.");
DEFINE_bool(plot_position_test, false, "Plots results of with gnuplot");
DEFINE_bool(static_scenario, true, "Compute figures of merit for static user position (DRMS, CEP, etc..)");
DEFINE_bool(use_pvt_solver_dump, false, "Use PVT solver binary dump or fall back to KML PVT file (contains only position information)");
DEFINE_bool(use_ref_motion_file, false, "Enable or disable the use of a reference file containing the true receiver position, velocity and acceleration.");
DEFINE_int32(ref_motion_file_type, 1, "Type of reference motion file: 1- Spirent CSV motion file");
DEFINE_string(ref_motion_filename, std::string("motion.csv"), "Path and filename for the reference motion file");
DEFINE_string(pvt_solver_dump_filename, std::string("PVT_pvt.dat"), "Path and filename for the PVT solver binary dump file");
#endif

155
src/tests/system-tests/libs/rtklib_solver_dump_reader.cc Normal file
View File

@ -0,0 +1,155 @@
/*!
* \file rtklib_solver_dump_reader.cc
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "rtklib_solver_dump_reader.h"
#include <iostream>
bool rtklib_solver_dump_reader::read_binary_obs()
{
try
{
d_dump_file.read(reinterpret_cast<char *>(&TOW_at_current_symbol_ms), sizeof(TOW_at_current_symbol_ms));
// std::cout << "TOW_at_current_symbol_ms: " << TOW_at_current_symbol_ms << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&week), sizeof(week));
// std::cout << "week: " << week << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&RX_time), sizeof(RX_time));
// std::cout << "RX_time: " << RX_time << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&clk_offset_s), sizeof(clk_offset_s));
// std::cout << "clk_offset_s: " << clk_offset_s << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&rr[0]), sizeof(rr));
// for (int n = 0; n < 6; n++)
// {
// std::cout << "rr: " << rr[n] << std::endl;
// }
d_dump_file.read(reinterpret_cast<char *>(&qr[0]), sizeof(qr));
// for (int n = 0; n < 6; n++)
// {
// std::cout << "qr" << qr[n] << std::endl;
// }
d_dump_file.read(reinterpret_cast<char *>(&latitude), sizeof(latitude));
std::cout << "latitude: " << latitude << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&longitude), sizeof(longitude));
std::cout << "longitude: " << longitude << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&height), sizeof(height));
std::cout << "height: " << height << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&ns), sizeof(ns));
// std::cout << "ns: " << (int)ns << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&status), sizeof(status));
// std::cout << "status: " << (int)status << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&type), sizeof(type));
// std::cout << "type: " << (int)type << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&AR_ratio), sizeof(AR_ratio));
// std::cout << "AR_ratio: " << AR_ratio << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&AR_thres), sizeof(AR_thres));
// std::cout << "AR_thres: " << AR_thres << std::endl;
d_dump_file.read(reinterpret_cast<char *>(&dop[0]), sizeof(dop));
// for (int n = 0; n < 4; n++)
// {
// std::cout << "dop" << dop[n] << std::endl;
// }
// getchar();
}
catch (const std::ifstream::failure &e)
{
return false;
}
return true;
}
bool rtklib_solver_dump_reader::restart()
{
if (d_dump_file.is_open())
{
d_dump_file.clear();
d_dump_file.seekg(0, std::ios::beg);
return true;
}
else
{
return false;
}
}
int64_t rtklib_solver_dump_reader::num_epochs()
{
// std::ifstream::pos_type size;
// int number_of_double_vars = 1;
// int number_of_float_vars = 17;
// int epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars +
// sizeof(float) * number_of_float_vars + sizeof(unsigned int);
// std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
// if (tmpfile.is_open())
// {
// size = tmpfile.tellg();
// int64_t nepoch = size / epoch_size_bytes;
// return nepoch;
// }
// else
// {
return 0;
// }
}
bool rtklib_solver_dump_reader::open_obs_file(std::string out_file)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename = out_file;
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str(), std::ios::in | std::ios::binary);
return true;
}
catch (const std::ifstream::failure &e)
{
std::cout << "Problem opening rtklib_solver dump Log file: " << d_dump_filename.c_str() << std::endl;
return false;
}
}
else
{
return false;
}
}
rtklib_solver_dump_reader::~rtklib_solver_dump_reader()
{
if (d_dump_file.is_open() == true)
{
d_dump_file.close();
}
}

88
src/tests/system-tests/libs/rtklib_solver_dump_reader.h Normal file
View File

@ -0,0 +1,88 @@
/*!
* \file rtklib_solver_dump_reader.h
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H
#define GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H
#include <cstdint>
#include <fstream>
#include <string>
#include <vector>
class rtklib_solver_dump_reader
{
public:
~rtklib_solver_dump_reader();
bool read_binary_obs();
bool restart();
int64_t num_epochs();
bool open_obs_file(std::string out_file);
//rtklib_solver dump variables
// TOW
uint32_t TOW_at_current_symbol_ms;
// WEEK
uint32_t week;
// PVT GPS time
double RX_time;
// User clock offset [s]
double clk_offset_s;
// ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] (6 x double)
double rr[6];
// position variance/covariance (m^2) {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} (6 x double)
float qr[6];
// GEO user position Latitude [deg]
double latitude;
// GEO user position Longitude [deg]
double longitude;
// GEO user position Height [m]
double height;
// NUMBER OF VALID SATS
uint8_t ns;
// RTKLIB solution status
uint8_t status;
// RTKLIB solution type (0:xyz-ecef,1:enu-baseline)
uint8_t type;
//AR ratio factor for validation
float AR_ratio;
//AR ratio threshold for validation
float AR_thres;
//GDOP//PDOP//HDOP//VDOP
double dop[4];
private:
std::string d_dump_filename;
std::ifstream d_dump_file;
};
#endif //GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H

202
src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc Normal file
View File

@ -0,0 +1,202 @@
/*!
* \file spirent_motion_csv_dump_reader.cc
* \brief Helper file for unit testing
* \author Javier Arribas, 2017. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "spirent_motion_csv_dump_reader.h"
#include <iostream>
bool spirent_motion_csv_dump_reader::read_csv_obs()
{
try
{
std::vector<double> vec;
std::string line;
if (getline(d_dump_file, line))
{
boost::tokenizer<boost::escaped_list_separator<char>> tk(
line, boost::escaped_list_separator<char>('\\', ',', '\"'));
for (boost::tokenizer<boost::escaped_list_separator<char>>::iterator i(
tk.begin());
i != tk.end(); ++i)
{
try
{
vec.push_back(std::stod(*i));
}
catch (const std::exception &ex)
{
vec.push_back(0.0);
}
}
}
}
catch (const std::ifstream::failure &e)
{
return false;
}
return true;
}
bool spirent_motion_csv_dump_reader::parse_vector(std::vector<double> &vec)
{
try
{
int n = 0;
TOW_ms = vec.at(n++);
Pos_X = vec.at(n++);
Pos_Y = vec.at(n++);
Pos_Z = vec.at(n++);
Vel_X = vec.at(n++);
Vel_Y = vec.at(n++);
Vel_Z = vec.at(n++);
Acc_X = vec.at(n++);
Acc_Y = vec.at(n++);
Acc_Z = vec.at(n++);
Jerk_X = vec.at(n++);
Jerk_Y = vec.at(n++);
Jerk_Z = vec.at(n++);
Lat = vec.at(n++);
Long = vec.at(n++);
Height = vec.at(n++);
Heading = vec.at(n++);
Elevation = vec.at(n++);
Bank = vec.at(n++);
Ang_vel_X = vec.at(n++);
Ang_vel_Y = vec.at(n++);
Ang_vel_Z = vec.at(n++);
Ang_acc_X = vec.at(n++);
Ang_acc_Y = vec.at(n++);
Ang_acc_Z = vec.at(n++);
Ant1_Pos_X = vec.at(n++);
Ant1_Pos_Y = vec.at(n++);
Ant1_Pos_Z = vec.at(n++);
Ant1_Vel_X = vec.at(n++);
Ant1_Vel_Y = vec.at(n++);
Ant1_Vel_Z = vec.at(n++);
Ant1_Acc_X = vec.at(n++);
Ant1_Acc_Y = vec.at(n++);
Ant1_Acc_Z = vec.at(n++);
Ant1_Lat = vec.at(n++);
Ant1_Long = vec.at(n++);
Ant1_Height = vec.at(n++);
Ant1_DOP = vec.at(n++);
return true;
}
catch (const std::exception &ex)
{
return false;
}
}
bool spirent_motion_csv_dump_reader::restart()
{
if (d_dump_file.is_open())
{
d_dump_file.clear();
d_dump_file.seekg(0, std::ios::beg);
std::string line;
for (int n = 0; n < header_lines; n++)
{
getline(d_dump_file, line);
}
return true;
}
else
{
return false;
}
}
int64_t spirent_motion_csv_dump_reader::num_epochs()
{
int64_t nepoch = 0;
std::string line;
std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
if (tmpfile.is_open())
{
while (std::getline(tmpfile, line))
++nepoch;
return nepoch - header_lines;
}
else
{
return 0;
}
}
bool spirent_motion_csv_dump_reader::open_obs_file(std::string out_file)
{
if (d_dump_file.is_open() == false)
{
try
{
d_dump_filename = out_file;
d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
d_dump_file.open(d_dump_filename.c_str());
std::string line;
for (int n = 0; n < header_lines; n++)
{
getline(d_dump_file, line);
}
return true;
}
catch (const std::ifstream::failure &e)
{
std::cout << "Problem opening Spirent CSV dump Log file: " << d_dump_filename.c_str() << std::endl;
return false;
}
}
else
{
return false;
}
}
void spirent_motion_csv_dump_reader::close_obs_file()
{
if (d_dump_file.is_open() == false)
{
d_dump_file.close();
}
}
spirent_motion_csv_dump_reader::spirent_motion_csv_dump_reader()
{
header_lines = 2;
}
spirent_motion_csv_dump_reader::~spirent_motion_csv_dump_reader()
{
if (d_dump_file.is_open() == true)
{
d_dump_file.close();
}
}

98
src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h Normal file
View File

@ -0,0 +1,98 @@
/*!
* \file spirent_motion_csv_dump_reader.h
* \brief Helper file for unit testing
* \author Javier Arribas, 2018. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_spirent_motion_csv_dump_READER_H
#define GNSS_SDR_spirent_motion_csv_dump_READER_H
#include <boost/tokenizer.hpp>
#include <cstdint>
#include <fstream>
#include <string>
#include <vector>
class spirent_motion_csv_dump_reader
{
public:
spirent_motion_csv_dump_reader();
~spirent_motion_csv_dump_reader();
bool read_csv_obs();
bool restart();
int64_t num_epochs();
bool open_obs_file(std::string out_file);
void close_obs_file();
int header_lines;
//dump variables
double TOW_ms;
double Pos_X;
double Pos_Y;
double Pos_Z;
double Vel_X;
double Vel_Y;
double Vel_Z;
double Acc_X;
double Acc_Y;
double Acc_Z;
double Jerk_X;
double Jerk_Y;
double Jerk_Z;
double Lat;
double Long;
double Height;
double Heading;
double Elevation;
double Bank;
double Ang_vel_X;
double Ang_vel_Y;
double Ang_vel_Z;
double Ang_acc_X;
double Ang_acc_Y;
double Ang_acc_Z;
double Ant1_Pos_X;
double Ant1_Pos_Y;
double Ant1_Pos_Z;
double Ant1_Vel_X;
double Ant1_Vel_Y;
double Ant1_Vel_Z;
double Ant1_Acc_X;
double Ant1_Acc_Y;
double Ant1_Acc_Z;
double Ant1_Lat;
double Ant1_Long;
double Ant1_Height;
double Ant1_DOP;
private:
std::string d_dump_filename;
std::ifstream d_dump_file;
bool parse_vector(std::vector<double> &vec);
};
#endif //GNSS_SDR_spirent_motion_csv_dump_READER_H

719
src/tests/system-tests/obs_gps_l1_system_test.cc
View File

@ -1,719 +0,0 @@
/*!
* \file obs_gps_l1_system_test.cc
* \brief This class implements a test for the validation of generated observables.
* \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "concurrent_map.h"
#include "concurrent_queue.h"
#include "control_thread.h"
#include "in_memory_configuration.h"
#include "signal_generator_flags.h"
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <gpstk/RinexUtilities.hpp>
#include <gpstk/Rinex3ObsBase.hpp>
#include <gpstk/Rinex3ObsData.hpp>
#include <gpstk/Rinex3ObsHeader.hpp>
#include <gpstk/Rinex3ObsStream.hpp>
#include <algorithm>
#include <chrono>
#include <cstdlib>
#include <exception>
#include <iostream>
#include <numeric>
#include <string>
#include <thread>
#include <unistd.h>
// For GPS NAVIGATION (L1)
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;
class ObsGpsL1SystemTest : public ::testing::Test
{
public:
std::string generator_binary;
std::string p1;
std::string p2;
std::string p3;
std::string p4;
std::string p5;
const double baseband_sampling_freq = 2.6e6;
std::string filename_rinex_obs = FLAGS_filename_rinex_obs;
std::string filename_raw_data = FLAGS_filename_raw_data;
std::string generated_rinex_obs;
int configure_generator();
int generate_signal();
int configure_receiver();
int run_receiver();
void check_results();
bool check_valid_rinex_nav(std::string filename); // return true if the file is a valid Rinex navigation file.
bool check_valid_rinex_obs(std::string filename); // return true if the file is a valid Rinex observation file.
double compute_stdev(const std::vector<double>& vec);
std::shared_ptr<InMemoryConfiguration> config;
};
bool ObsGpsL1SystemTest::check_valid_rinex_nav(std::string filename)
{
bool res = false;
res = gpstk::isRinexNavFile(filename);
return res;
}
double ObsGpsL1SystemTest::compute_stdev(const std::vector<double>& vec)
{
double sum__ = std::accumulate(vec.begin(), vec.end(), 0.0);
double mean__ = sum__ / vec.size();
double accum__ = 0.0;
std::for_each(std::begin(vec), std::end(vec), [&](const double d) {
accum__ += (d - mean__) * (d - mean__);
});
double stdev__ = std::sqrt(accum__ / (vec.size() - 1));
return stdev__;
}
bool ObsGpsL1SystemTest::check_valid_rinex_obs(std::string filename)
{
bool res = false;
res = gpstk::isRinexObsFile(filename);
return res;
}
int ObsGpsL1SystemTest::configure_generator()
{
// Configure signal generator
generator_binary = FLAGS_generator_binary;
p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file;
if (FLAGS_dynamic_position.empty())
{
p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(std::min(FLAGS_duration * 10, 3000));
if (FLAGS_duration > 300) std::cout << "WARNING: Duration has been set to its maximum value of 300 s" << std::endl;
}
else
{
p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position);
}
p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output
p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples
p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps]
return 0;
}
int ObsGpsL1SystemTest::generate_signal()
{
pid_t wait_result;
int child_status;
char* const parmList[] = {&generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL};
int pid;
if ((pid = fork()) == -1)
perror("fork error");
else if (pid == 0)
{
execv(&generator_binary[0], parmList);
std::cout << "Return not expected. Must be an execv error." << std::endl;
std::terminate();
}
wait_result = waitpid(pid, &child_status, 0);
if (wait_result == -1) perror("waitpid error");
EXPECT_EQ(true, check_valid_rinex_obs(filename_rinex_obs));
std::cout << "Signal and Observables RINEX files created." << std::endl;
return 0;
}
int ObsGpsL1SystemTest::configure_receiver()
{
config = std::make_shared<InMemoryConfiguration>();
const int sampling_rate_internal = baseband_sampling_freq;
const int number_of_taps = 11;
const int number_of_bands = 2;
const float band1_begin = 0.0;
const float band1_end = 0.48;
const float band2_begin = 0.52;
const float band2_end = 1.0;
const float ampl1_begin = 1.0;
const float ampl1_end = 1.0;
const float ampl2_begin = 0.0;
const float ampl2_end = 0.0;
const float band1_error = 1.0;
const float band2_error = 1.0;
const int grid_density = 16;
const float zero = 0.0;
const int number_of_channels = 8;
const int in_acquisition = 1;
const float threshold = 0.01;
const float doppler_max = 8000.0;
const float doppler_step = 500.0;
const int max_dwells = 1;
const int tong_init_val = 2;
const int tong_max_val = 10;
const int tong_max_dwells = 30;
const int coherent_integration_time_ms = 1;
const float pll_bw_hz = 30.0;
const float dll_bw_hz = 4.0;
const float early_late_space_chips = 0.5;
const float pll_bw_narrow_hz = 20.0;
const float dll_bw_narrow_hz = 2.0;
const int extend_correlation_ms = 1;
const int display_rate_ms = 500;
const int output_rate_ms = 10;
config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(sampling_rate_internal));
// Set the assistance system parameters
config->set_property("GNSS-SDR.SUPL_read_gps_assistance_xml", "false");
config->set_property("GNSS-SDR.SUPL_gps_enabled", "false");
config->set_property("GNSS-SDR.SUPL_gps_ephemeris_server", "supl.google.com");
config->set_property("GNSS-SDR.SUPL_gps_ephemeris_port", std::to_string(7275));
config->set_property("GNSS-SDR.SUPL_gps_acquisition_server", "supl.google.com");
config->set_property("GNSS-SDR.SUPL_gps_acquisition_port", std::to_string(7275));
config->set_property("GNSS-SDR.SUPL_MCC", std::to_string(244));
config->set_property("GNSS-SDR.SUPL_MNS", std::to_string(5));
config->set_property("GNSS-SDR.SUPL_LAC", "0x59e2");
config->set_property("GNSS-SDR.SUPL_CI", "0x31b0");
// Set the Signal Source
config->set_property("SignalSource.implementation", "File_Signal_Source");
config->set_property("SignalSource.filename", "./" + filename_raw_data);
config->set_property("SignalSource.sampling_frequency", std::to_string(sampling_rate_internal));
config->set_property("SignalSource.item_type", "ibyte");
config->set_property("SignalSource.samples", std::to_string(zero));
// Set the Signal Conditioner
config->set_property("SignalConditioner.implementation", "Signal_Conditioner");
config->set_property("DataTypeAdapter.implementation", "Ibyte_To_Complex");
config->set_property("InputFilter.implementation", "Fir_Filter");
config->set_property("InputFilter.dump", "false");
config->set_property("InputFilter.input_item_type", "gr_complex");
config->set_property("InputFilter.output_item_type", "gr_complex");
config->set_property("InputFilter.taps_item_type", "float");
config->set_property("InputFilter.number_of_taps", std::to_string(number_of_taps));
config->set_property("InputFilter.number_of_bands", std::to_string(number_of_bands));
config->set_property("InputFilter.band1_begin", std::to_string(band1_begin));
config->set_property("InputFilter.band1_end", std::to_string(band1_end));
config->set_property("InputFilter.band2_begin", std::to_string(band2_begin));
config->set_property("InputFilter.band2_end", std::to_string(band2_end));
config->set_property("InputFilter.ampl1_begin", std::to_string(ampl1_begin));
config->set_property("InputFilter.ampl1_end", std::to_string(ampl1_end));
config->set_property("InputFilter.ampl2_begin", std::to_string(ampl2_begin));
config->set_property("InputFilter.ampl2_end", std::to_string(ampl2_end));
config->set_property("InputFilter.band1_error", std::to_string(band1_error));
config->set_property("InputFilter.band2_error", std::to_string(band2_error));
config->set_property("InputFilter.filter_type", "bandpass");
config->set_property("InputFilter.grid_density", std::to_string(grid_density));
config->set_property("InputFilter.sampling_frequency", std::to_string(sampling_rate_internal));
config->set_property("InputFilter.IF", std::to_string(zero));
config->set_property("Resampler.implementation", "Pass_Through");
config->set_property("Resampler.dump", "false");
config->set_property("Resampler.item_type", "gr_complex");
config->set_property("Resampler.sample_freq_in", std::to_string(sampling_rate_internal));
config->set_property("Resampler.sample_freq_out", std::to_string(sampling_rate_internal));
// Set the number of Channels
config->set_property("Channels_1C.count", std::to_string(number_of_channels));
config->set_property("Channels.in_acquisition", std::to_string(in_acquisition));
config->set_property("Channel.signal", "1C");
// Set Acquisition
config->set_property("Acquisition_1C.implementation", "GPS_L1_CA_PCPS_Tong_Acquisition");
config->set_property("Acquisition_1C.item_type", "gr_complex");
config->set_property("Acquisition_1C.coherent_integration_time_ms", std::to_string(coherent_integration_time_ms));
config->set_property("Acquisition_1C.threshold", std::to_string(threshold));
config->set_property("Acquisition_1C.doppler_max", std::to_string(doppler_max));
config->set_property("Acquisition_1C.doppler_step", std::to_string(doppler_step));
config->set_property("Acquisition_1C.bit_transition_flag", "false");
config->set_property("Acquisition_1C.max_dwells", std::to_string(max_dwells));
config->set_property("Acquisition_1C.tong_init_val", std::to_string(tong_init_val));
config->set_property("Acquisition_1C.tong_max_val", std::to_string(tong_max_val));
config->set_property("Acquisition_1C.tong_max_dwells", std::to_string(tong_max_dwells));
// Set Tracking
config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking");
//config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_C_Aid_Tracking");
config->set_property("Tracking_1C.item_type", "gr_complex");
config->set_property("Tracking_1C.dump", "false");
config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");
config->set_property("Tracking_1C.pll_bw_hz", std::to_string(pll_bw_hz));
config->set_property("Tracking_1C.dll_bw_hz", std::to_string(dll_bw_hz));
config->set_property("Tracking_1C.early_late_space_chips", std::to_string(early_late_space_chips));
config->set_property("Tracking_1C.pll_bw_narrow_hz", std::to_string(pll_bw_narrow_hz));
config->set_property("Tracking_1C.dll_bw_narrow_hz", std::to_string(dll_bw_narrow_hz));
config->set_property("Tracking_1C.extend_correlation_ms", std::to_string(extend_correlation_ms));
// Set Telemetry
config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder");
config->set_property("TelemetryDecoder_1C.dump", "false");
// Set Observables
config->set_property("Observables.implementation", "Hybrid_Observables");
config->set_property("Observables.dump", "false");
config->set_property("Observables.dump_filename", "./observables.dat");
config->set_property("Observables.averaging_depth", std::to_string(100));
// Set PVT
config->set_property("PVT.implementation", "RTKLIB_PVT");
config->set_property("PVT.positioning_mode", "Single");
config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms));
config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms));
config->set_property("PVT.dump_filename", "./PVT");
config->set_property("PVT.nmea_dump_filename", "./gnss_sdr_pvt.nmea");
config->set_property("PVT.flag_nmea_tty_port", "false");
config->set_property("PVT.nmea_dump_devname", "/dev/pts/4");
config->set_property("PVT.flag_rtcm_server", "false");
config->set_property("PVT.flag_rtcm_tty_port", "false");
config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1");
config->set_property("PVT.dump", "false");
config->set_property("PVT.rinex_version", std::to_string(2));
return 0;
}
int ObsGpsL1SystemTest::run_receiver()
{
std::shared_ptr<ControlThread> control_thread;
control_thread = std::make_shared<ControlThread>(config);
// start receiver
try
{
control_thread->run();
}
catch (const boost::exception& e)
{
std::cout << "Boost exception: " << boost::diagnostic_information(e);
}
catch (const std::exception& ex)
{
std::cout << "STD exception: " << ex.what();
}
// Get the name of the RINEX obs file generated by the receiver
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
FILE* fp;
std::string argum2 = std::string("/bin/ls *O | grep GSDR | tail -1");
char buffer[1035];
fp = popen(&argum2[0], "r");
if (fp == NULL)
{
std::cout << "Failed to run command: " << argum2 << std::endl;
return -1;
}
while (fgets(buffer, sizeof(buffer), fp) != NULL)
{
std::string aux = std::string(buffer);
ObsGpsL1SystemTest::generated_rinex_obs = aux.erase(aux.length() - 1, 1);
}
pclose(fp);
return 0;
}
void ObsGpsL1SystemTest::check_results()
{
std::vector<std::vector<std::pair<double, double>>> pseudorange_ref(33);
std::vector<std::vector<std::pair<double, double>>> carrierphase_ref(33);
std::vector<std::vector<std::pair<double, double>>> doppler_ref(33);
std::vector<std::vector<std::pair<double, double>>> pseudorange_meas(33);
std::vector<std::vector<std::pair<double, double>>> carrierphase_meas(33);
std::vector<std::vector<std::pair<double, double>>> doppler_meas(33);
// Open and read reference RINEX observables file
try
{
gpstk::Rinex3ObsStream r_ref(FLAGS_filename_rinex_obs);
r_ref.exceptions(std::ios::failbit);
gpstk::Rinex3ObsData r_ref_data;
gpstk::Rinex3ObsHeader r_ref_header;
gpstk::RinexDatum dataobj;
r_ref >> r_ref_header;
while (r_ref >> r_ref_data)
{
for (int myprn = 1; myprn < 33; myprn++)
{
gpstk::SatID prn(myprn, gpstk::SatID::systemGPS);
gpstk::CommonTime time = r_ref_data.time;
double sow(static_cast<gpstk::GPSWeekSecond>(time).sow);
gpstk::Rinex3ObsData::DataMap::iterator pointer = r_ref_data.obs.find(prn);
if (pointer == r_ref_data.obs.end())
{
// PRN not present; do nothing
}
else
{
dataobj = r_ref_data.getObs(prn, "C1C", r_ref_header);
double P1 = dataobj.data;
std::pair<double, double> pseudo(sow, P1);
pseudorange_ref.at(myprn).push_back(pseudo);
dataobj = r_ref_data.getObs(prn, "L1C", r_ref_header);
double L1 = dataobj.data;
std::pair<double, double> carrier(sow, L1);
carrierphase_ref.at(myprn).push_back(carrier);
dataobj = r_ref_data.getObs(prn, "D1C", r_ref_header);
double D1 = dataobj.data;
std::pair<double, double> doppler(sow, D1);
doppler_ref.at(myprn).push_back(doppler);
} // End of 'if( pointer == roe.obs.end() )'
} // end for
} // end while
} // End of 'try' block
catch (const gpstk::FFStreamError& e)
{
std::cout << e;
exit(1);
}
catch (const gpstk::Exception& e)
{
std::cout << e;
exit(1);
}
catch (...)
{
std::cout << "unknown error. I don't feel so well..." << std::endl;
exit(1);
}
try
{
std::string arg2_gen = std::string("./") + ObsGpsL1SystemTest::generated_rinex_obs;
gpstk::Rinex3ObsStream r_meas(arg2_gen);
r_meas.exceptions(std::ios::failbit);
gpstk::Rinex3ObsData r_meas_data;
gpstk::Rinex3ObsHeader r_meas_header;
gpstk::RinexDatum dataobj;
r_meas >> r_meas_header;
while (r_meas >> r_meas_data)
{
for (int myprn = 1; myprn < 33; myprn++)
{
gpstk::SatID prn(myprn, gpstk::SatID::systemGPS);
gpstk::CommonTime time = r_meas_data.time;
double sow(static_cast<gpstk::GPSWeekSecond>(time).sow);
gpstk::Rinex3ObsData::DataMap::iterator pointer = r_meas_data.obs.find(prn);
if (pointer == r_meas_data.obs.end())
{
// PRN not present; do nothing
}
else
{
dataobj = r_meas_data.getObs(prn, "C1C", r_meas_header);
double P1 = dataobj.data;
std::pair<double, double> pseudo(sow, P1);
pseudorange_meas.at(myprn).push_back(pseudo);
dataobj = r_meas_data.getObs(prn, "L1C", r_meas_header);
double L1 = dataobj.data;
std::pair<double, double> carrier(sow, L1);
carrierphase_meas.at(myprn).push_back(carrier);
dataobj = r_meas_data.getObs(prn, "D1C", r_meas_header);
double D1 = dataobj.data;
std::pair<double, double> doppler(sow, D1);
doppler_meas.at(myprn).push_back(doppler);
} // End of 'if( pointer == roe.obs.end() )'
} // end for
} // end while
} // End of 'try' block
catch (const gpstk::FFStreamError& e)
{
std::cout << e;
exit(1);
}
catch (const gpstk::Exception& e)
{
std::cout << e;
exit(1);
}
catch (...)
{
std::cout << "unknown error. I don't feel so well..." << std::endl;
exit(1);
}
// Time alignment
std::vector<std::vector<std::pair<double, double>>> pseudorange_ref_aligned(33);
std::vector<std::vector<std::pair<double, double>>> carrierphase_ref_aligned(33);
std::vector<std::vector<std::pair<double, double>>> doppler_ref_aligned(33);
std::vector<std::vector<std::pair<double, double>>>::iterator iter;
std::vector<std::pair<double, double>>::iterator it;
std::vector<std::pair<double, double>>::iterator it2;
std::vector<std::vector<double>> pr_diff(33);
std::vector<std::vector<double>> cp_diff(33);
std::vector<std::vector<double>> doppler_diff(33);
std::vector<std::vector<double>>::iterator iter_diff;
std::vector<double>::iterator iter_v;
int prn_id = 0;
for (iter = pseudorange_ref.begin(); iter != pseudorange_ref.end(); iter++)
{
for (it = iter->begin(); it != iter->end(); it++)
{
// If a measure exists for this sow, store it
for (it2 = pseudorange_meas.at(prn_id).begin(); it2 != pseudorange_meas.at(prn_id).end(); it2++)
{
if (std::abs(it->first - it2->first) < 0.1) // store measures closer than 10 ms.
{
pseudorange_ref_aligned.at(prn_id).push_back(*it);
pr_diff.at(prn_id).push_back(it->second - it2->second);
//std::cout << "Sat " << prn_id << ": " << "PR_ref=" << it->second << " PR_meas=" << it2->second << " Diff:" << it->second - it2->second << std::endl;
}
}
}
prn_id++;
}
prn_id = 0;
for (iter = carrierphase_ref.begin(); iter != carrierphase_ref.end(); iter++)
{
for (it = iter->begin(); it != iter->end(); it++)
{
// If a measure exists for this sow, store it
for (it2 = carrierphase_meas.at(prn_id).begin(); it2 != carrierphase_meas.at(prn_id).end(); it2++)
{
if (std::abs(it->first - it2->first) < 0.1) // store measures closer than 10 ms.
{
carrierphase_ref_aligned.at(prn_id).push_back(*it);
cp_diff.at(prn_id).push_back(it->second - it2->second);
// std::cout << "Sat " << prn_id << ": " << "Carrier_ref=" << it->second << " Carrier_meas=" << it2->second << " Diff:" << it->second - it2->second << std::endl;
}
}
}
prn_id++;
}
prn_id = 0;
for (iter = doppler_ref.begin(); iter != doppler_ref.end(); iter++)
{
for (it = iter->begin(); it != iter->end(); it++)
{
// If a measure exists for this sow, store it
for (it2 = doppler_meas.at(prn_id).begin(); it2 != doppler_meas.at(prn_id).end(); it2++)
{
if (std::abs(it->first - it2->first) < 0.01) // store measures closer than 10 ms.
{
doppler_ref_aligned.at(prn_id).push_back(*it);
doppler_diff.at(prn_id).push_back(it->second - it2->second);
}
}
}
prn_id++;
}
// Compute pseudorange error
prn_id = 0;
std::vector<double> mean_pr_diff_v;
for (iter_diff = pr_diff.begin(); iter_diff != pr_diff.end(); iter_diff++)
{
// For each satellite with reference and measurements aligned in time
int number_obs = 0;
double mean_diff = 0.0;
for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++)
{
mean_diff = mean_diff + *iter_v;
number_obs = number_obs + 1;
}
if (number_obs > 0)
{
mean_diff = mean_diff / number_obs;
mean_pr_diff_v.push_back(mean_diff);
std::cout << "-- Mean pseudorange difference for sat " << prn_id << ": " << mean_diff;
double stdev_ = compute_stdev(*iter_diff);
std::cout << " +/- " << stdev_;
std::cout << " [m]" << std::endl;
}
else
{
mean_diff = 0.0;
}
prn_id++;
}
double stdev_pr = compute_stdev(mean_pr_diff_v);
std::cout << "Pseudorange diff error stdev = " << stdev_pr << " [m]" << std::endl;
ASSERT_LT(stdev_pr, 10.0);
// Compute carrier phase error
prn_id = 0;
std::vector<double> mean_cp_diff_v;
for (iter_diff = cp_diff.begin(); iter_diff != cp_diff.end(); iter_diff++)
{
// For each satellite with reference and measurements aligned in time
int number_obs = 0;
double mean_diff = 0.0;
for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++)
{
mean_diff = mean_diff + *iter_v;
number_obs = number_obs + 1;
}
if (number_obs > 0)
{
mean_diff = mean_diff / number_obs;
mean_cp_diff_v.push_back(mean_diff);
std::cout << "-- Mean carrier phase difference for sat " << prn_id << ": " << mean_diff;
double stdev_pr_ = compute_stdev(*iter_diff);
std::cout << " +/- " << stdev_pr_ << " whole cycles (19 cm)" << std::endl;
}
else
{
mean_diff = 0.0;
}
prn_id++;
}
// Compute Doppler error
prn_id = 0;
std::vector<double> mean_doppler_v;
for (iter_diff = doppler_diff.begin(); iter_diff != doppler_diff.end(); iter_diff++)
{
// For each satellite with reference and measurements aligned in time
int number_obs = 0;
double mean_diff = 0.0;
for (iter_v = iter_diff->begin(); iter_v != iter_diff->end(); iter_v++)
{
//std::cout << *iter_v << std::endl;
mean_diff = mean_diff + *iter_v;
number_obs = number_obs + 1;
}
if (number_obs > 0)
{
mean_diff = mean_diff / number_obs;
mean_doppler_v.push_back(mean_diff);
std::cout << "-- Mean Doppler difference for sat " << prn_id << ": " << mean_diff << " [Hz]" << std::endl;
}
else
{
mean_diff = 0.0;
}
prn_id++;
}
double stdev_dp = compute_stdev(mean_doppler_v);
std::cout << "Doppler error stdev = " << stdev_dp << " [Hz]" << std::endl;
ASSERT_LT(stdev_dp, 10.0);
}
TEST_F(ObsGpsL1SystemTest, Observables_system_test)
{
std::cout << "Validating input RINEX nav file: " << FLAGS_rinex_nav_file << " ..." << std::endl;
bool is_rinex_nav_valid = check_valid_rinex_nav(FLAGS_rinex_nav_file);
EXPECT_EQ(true, is_rinex_nav_valid) << "The RINEX navigation file " << FLAGS_rinex_nav_file << " is not well formed.";
std::cout << "The file is valid." << std::endl;
// Configure the signal generator
configure_generator();
// Generate signal raw signal samples and observations RINEX file
if (!FLAGS_disable_generator)
{
generate_signal();
}
std::cout << "Validating generated reference RINEX obs file: " << FLAGS_filename_rinex_obs << " ..." << std::endl;
bool is_gen_rinex_obs_valid = check_valid_rinex_obs("./" + FLAGS_filename_rinex_obs);
EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << FLAGS_filename_rinex_obs << ", generated by gnss-sim, is not well formed.";
std::cout << "The file is valid." << std::endl;
// Configure receiver
configure_receiver();
// Run the receiver
EXPECT_EQ(run_receiver(), 0) << "Problem executing the software-defined signal generator";
std::cout << "Validating RINEX obs file obtained by GNSS-SDR: " << ObsGpsL1SystemTest::generated_rinex_obs << " ..." << std::endl;
is_gen_rinex_obs_valid = check_valid_rinex_obs("./" + ObsGpsL1SystemTest::generated_rinex_obs);
EXPECT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << ObsGpsL1SystemTest::generated_rinex_obs << ", generated by GNSS-SDR, is not well formed.";
std::cout << "The file is valid." << std::endl;
// Check results
check_results();
}
int main(int argc, char** argv)
{
std::cout << "Running Observables validation test..." << std::endl;
int res = 0;
try
{
testing::InitGoogleTest(&argc, argv);
}
catch (...)
{
} // catch the "testing::internal::<unnamed>::ClassUniqueToAlwaysTrue" from gtest
google::ParseCommandLineFlags(&argc, &argv, true);
google::InitGoogleLogging(argv[0]);
// Run the Tests
try
{
res = RUN_ALL_TESTS();
}
catch (...)
{
LOG(WARNING) << "Unexpected catch";
}
google::ShutDownCommandLineFlags();
return res;
}

1051
src/tests/system-tests/obs_system_test.cc
View File

File diff suppressed because it is too large Load Diff

260
src/tests/system-tests/position_test.cc
View File

@ -1,7 +1,10 @@
/*!
* \file position_test.cc
* \brief This class implements a test for the validation of computed position.
* \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es
* \authors <ul>
* <li> Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es
* <li> Javier Arribas, 2018. jarribas(at)cttc.es
* </ul>
*
*
* -------------------------------------------------------------------------
@ -29,6 +32,9 @@
* -------------------------------------------------------------------------
*/
#include "position_test_flags.h"
#include "rtklib_solver_dump_reader.h"
#include "spirent_motion_csv_dump_reader.h"
#include "concurrent_map.h"
#include "concurrent_queue.h"
#include "control_thread.h"
@ -39,6 +45,7 @@
#include "test_flags.h"
#include "signal_generator_flags.h"
#include <boost/filesystem.hpp>
#include <armadillo>
#include <glog/logging.h>
#include <gtest/gtest.h>
#include <algorithm>
@ -48,10 +55,6 @@
#include <numeric>
#include <thread>
DEFINE_string(config_file_ptest, std::string(""), "File containing the configuration parameters for the position test.");
DEFINE_bool(plot_position_test, false, "Plots results of FFTLengthTest with gnuplot");
// For GPS NAVIGATION (L1)
concurrent_queue<Gps_Acq_Assist> global_gps_acq_assist_queue;
concurrent_map<Gps_Acq_Assist> global_gps_acq_assist_map;
@ -144,9 +147,9 @@ void StaticPositionSystemTest::geodetic2Enu(double latitude, double longitude, d
geodetic2Ecef(ref_lat * d2r, ref_long * d2r, ref_h, &ref_x, &ref_y, &ref_z);
double aux_x = x - ref_x;
double aux_y = y - ref_y;
double aux_z = z - ref_z;
double aux_x = x; // - ref_x;
double aux_y = y; // - ref_y;
double aux_z = z; // - ref_z;
// ECEF to NED matrix
double phiP = atan2(ref_z, sqrt(std::pow(ref_x, 2.0) + std::pow(ref_y, 2.0)));
@ -386,7 +389,7 @@ int StaticPositionSystemTest::configure_receiver()
config->set_property("PVT.flag_rtcm_server", "false");
config->set_property("PVT.flag_rtcm_tty_port", "false");
config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1");
config->set_property("PVT.dump", "false");
config->set_property("PVT.dump", "true");
config->set_property("PVT.rinex_version", std::to_string(2));
config->set_property("PVT.iono_model", "OFF");
config->set_property("PVT.trop_model", "OFF");
@ -455,123 +458,166 @@ int StaticPositionSystemTest::run_receiver()
void StaticPositionSystemTest::check_results()
{
std::fstream myfile(StaticPositionSystemTest::generated_kml_file, std::ios_base::in);
ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened";
std::string line;
std::vector<double> pos_e;
std::vector<double> pos_n;
std::vector<double> pos_u;
// Skip header
std::getline(myfile, line);
bool is_header = true;
while (is_header)
std::istringstream iss2(FLAGS_static_position);
std::string str_aux;
std::getline(iss2, str_aux, ',');
double ref_lat = std::stod(str_aux);
std::getline(iss2, str_aux, ',');
double ref_long = std::stod(str_aux);
std::getline(iss2, str_aux, '\n');
double ref_h = std::stod(str_aux);
double ref_e, ref_n, ref_u;
geodetic2Enu(ref_lat, ref_long, ref_h,
&ref_e, &ref_n, &ref_u);
if (!FLAGS_use_pvt_solver_dump)
{
//fall back to read receiver KML output (position only)
std::fstream myfile(StaticPositionSystemTest::generated_kml_file, std::ios_base::in);
ASSERT_TRUE(myfile.is_open()) << "No valid kml file could be opened";
std::string line;
// Skip header
std::getline(myfile, line);
ASSERT_FALSE(myfile.eof()) << "No valid kml file found.";
std::size_t found = line.find("<coordinates>");
if (found != std::string::npos) is_header = false;
}
bool is_data = true;
//read data
while (is_data)
{
if (!std::getline(myfile, line))
bool is_header = true;
while (is_header)
{
is_data = false;
break;
std::getline(myfile, line);
ASSERT_FALSE(myfile.eof()) << "No valid kml file found.";
std::size_t found = line.find("<coordinates>");
if (found != std::string::npos) is_header = false;
}
std::size_t found = line.find("</coordinates>");
if (found != std::string::npos)
is_data = false;
else
{
std::string str2;
std::istringstream iss(line);
double value;
double lat = 0.0;
double longitude = 0.0;
double h = 0.0;
for (int i = 0; i < 3; i++)
{
std::getline(iss, str2, ',');
value = std::stod(str2);
if (i == 0) lat = value;
if (i == 1) longitude = value;
if (i == 2) h = value;
}
bool is_data = true;
//read data
while (is_data)
{
if (!std::getline(myfile, line))
{
is_data = false;
break;
}
std::size_t found = line.find("</coordinates>");
if (found != std::string::npos)
is_data = false;
else
{
std::string str2;
std::istringstream iss(line);
double value;
double lat = 0.0;
double longitude = 0.0;
double h = 0.0;
for (int i = 0; i < 3; i++)
{
std::getline(iss, str2, ',');
value = std::stod(str2);
if (i == 0) longitude = value;
if (i == 1) lat = value;
if (i == 2) h = value;
}
double north, east, up;
geodetic2Enu(lat, longitude, h, &east, &north, &up);
// std::cout << "lat = " << lat << ", longitude = " << longitude << " h = " << h << std::endl;
// std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
pos_e.push_back(east);
pos_n.push_back(north);
pos_u.push_back(up);
// getchar();
}
}
myfile.close();
ASSERT_FALSE(pos_e.size() == 0) << "KML file is empty";
}
else
{
//use complete binary dump from pvt solver
rtklib_solver_dump_reader pvt_reader;
pvt_reader.open_obs_file(FLAGS_pvt_solver_dump_filename);
while (pvt_reader.read_binary_obs())
{
double north, east, up;
geodetic2Enu(lat, longitude, h, &east, &north, &up);
//std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
geodetic2Enu(pvt_reader.latitude, pvt_reader.longitude, pvt_reader.height, &east, &north, &up);
// std::cout << "lat = " << pvt_reader.latitude << ", longitude = " << pvt_reader.longitude << " h = " << pvt_reader.height << std::endl;
// std::cout << "E = " << east << ", N = " << north << " U = " << up << std::endl;
pos_e.push_back(east);
pos_n.push_back(north);
pos_u.push_back(up);
// getchar();
}
}
myfile.close();
ASSERT_FALSE(pos_e.size() == 0) << "KML file is empty";
double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0);
double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0);
double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0);
// compute results
double sigma_E_2_accuracy = std::pow(compute_stdev_accuracy(pos_e, 0.0), 2.0);
double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, 0.0), 2.0);
double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, 0.0), 2.0);
double sum__e = std::accumulate(pos_e.begin(), pos_e.end(), 0.0);
double mean__e = sum__e / pos_e.size();
double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0);
double mean__n = sum__n / pos_n.size();
double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0);
double mean__u = sum__u / pos_u.size();
std::stringstream stm;
std::ofstream position_test_file;
if (FLAGS_config_file_ptest.empty())
if (FLAGS_static_scenario)
{
stm << "---- ACCURACY ----" << std::endl;
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
stm << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl;
stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "Bias 2D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl;
stm << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl;
stm << std::endl;
double sigma_E_2_precision = std::pow(compute_stdev_precision(pos_e), 2.0);
double sigma_N_2_precision = std::pow(compute_stdev_precision(pos_n), 2.0);
double sigma_U_2_precision = std::pow(compute_stdev_precision(pos_u), 2.0);
double sigma_E_2_accuracy = std::pow(compute_stdev_accuracy(pos_e, ref_e), 2.0);
double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, ref_n), 2.0);
double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, ref_u), 2.0);
double sum__e = std::accumulate(pos_e.begin(), pos_e.end(), 0.0);
double mean__e = sum__e / pos_e.size();
double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0);
double mean__n = sum__n / pos_n.size();
double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0);
double mean__u = sum__u / pos_u.size();
std::stringstream stm;
std::ofstream position_test_file;
if (FLAGS_config_file_ptest.empty())
{
stm << "---- ACCURACY ----" << std::endl;
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
stm << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl;
stm << "CEP = " << 0.62 * compute_stdev_accuracy(pos_n, 0.0) + 0.56 * compute_stdev_accuracy(pos_e, 0.0) << " [m]" << std::endl;
stm << "99% SAS = " << 1.122 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl;
stm << "Bias 2D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl;
stm << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl;
stm << std::endl;
}
stm << "---- PRECISION ----" << std::endl;
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl;
stm << "DRMS = " << sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl;
stm << "CEP = " << 0.62 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl;
stm << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
stm << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
stm << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
std::cout << stm.rdbuf();
std::string output_filename = "position_test_output_" + StaticPositionSystemTest::generated_kml_file.erase(StaticPositionSystemTest::generated_kml_file.length() - 3, 3) + "txt";
position_test_file.open(output_filename.c_str());
if (position_test_file.is_open())
{
position_test_file << stm.str();
position_test_file.close();
}
// Sanity Check
double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision);
ASSERT_LT(precision_SEP, 20.0);
if (FLAGS_plot_position_test == true)
{
print_results(pos_e, pos_n, pos_u);
}
}
stm << "---- PRECISION ----" << std::endl;
stm << "2DRMS = " << 2 * sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl;
stm << "DRMS = " << sqrt(sigma_E_2_precision + sigma_N_2_precision) << " [m]" << std::endl;
stm << "CEP = " << 0.62 * compute_stdev_precision(pos_n) + 0.56 * compute_stdev_precision(pos_e) << " [m]" << std::endl;
stm << "99% SAS = " << 1.122 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
stm << "90% SAS = " << 0.833 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
stm << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
stm << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl;
std::cout << stm.rdbuf();
std::string output_filename = "position_test_output_" + StaticPositionSystemTest::generated_kml_file.erase(StaticPositionSystemTest::generated_kml_file.length() - 3, 3) + "txt";
position_test_file.open(output_filename.c_str());
if (position_test_file.is_open())
else
{
position_test_file << stm.str();
position_test_file.close();
}
// Sanity Check
double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision);
ASSERT_LT(precision_SEP, 20.0);
if (FLAGS_plot_position_test == true)
{
print_results(pos_e, pos_n, pos_u);
//dynamic position
}
}
@ -698,7 +744,7 @@ TEST_F(StaticPositionSystemTest, Position_system_test)
configure_receiver();
// Run the receiver
EXPECT_EQ(run_receiver(), 0) << "Problem executing the software-defined signal generator";
EXPECT_EQ(run_receiver(), 0) << "Problem executing GNSS-SDR";
// Check results
check_results();

3
src/tests/test_main.cc
View File

@ -127,6 +127,7 @@ DECLARE_string(log_dir);
#include "unit-tests/signal-processing-blocks/tracking/tracking_loop_filter_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc"
#if CUDA_BLOCKS_TEST
#include "unit-tests/signal-processing-blocks/tracking/gpu_multicorrelator_test.cc"
@ -148,6 +149,7 @@ DECLARE_string(log_dir);
#include "unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/gps_l2_m_dll_pll_tracking_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc"
#include "unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test.cc"
#if ENABLE_FPGA
#include "unit-tests/signal-processing-blocks/tracking/tracking_pull-in_test_fpga.cc"
@ -156,6 +158,7 @@ DECLARE_string(log_dir);
#include "unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc"
#endif
#include "unit-tests/signal-processing-blocks/telemetry_decoder/galileo_fnav_inav_decoder_test.cc"
#include "unit-tests/system-parameters/glonass_gnav_ephemeris_test.cc"
#include "unit-tests/system-parameters/glonass_gnav_nav_message_test.cc"

2
src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc
View File

@ -64,7 +64,7 @@ DEFINE_int32(acq_test_second_doppler_step, 10, "If --acq_test_make_two_steps is
DEFINE_int32(acq_test_signal_duration_s, 2, "Generated signal duration, in s");
DEFINE_int32(acq_test_num_meas, 0, "Number of measurements per run. 0 means the complete file.");
DEFINE_double(acq_test_cn0_init, 33.0, "Initial CN0, in dBHz.");
DEFINE_double(acq_test_cn0_init, 30.0, "Initial CN0, in dBHz.");
DEFINE_double(acq_test_cn0_final, 45.0, "Final CN0, in dBHz.");
DEFINE_double(acq_test_cn0_step, 3.0, "CN0 step, in dB.");

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

@ -53,7 +53,6 @@
#include "telemetry_decoder_interface.h"
#include "in_memory_configuration.h"
#include "gnss_synchro.h"
#include "gps_l1_ca_telemetry_decoder.h"
#include "tracking_true_obs_reader.h"
#include "true_observables_reader.h"
#include "tracking_dump_reader.h"
@ -326,7 +325,8 @@ bool HybridObservablesTest::acquire_signal()
{
tmp_gnss_synchro.System = 'G';
std::string signal = "1C";
signal.copy(tmp_gnss_synchro.Signal, 2, 0);
const char* str = signal.c_str(); // get a C style null terminated string
std::memcpy(static_cast<void*>(tmp_gnss_synchro.Signal), str, 3); // copy string into synchro char array: 2 char + null
tmp_gnss_synchro.PRN = SV_ID;
System_and_Signal = "GPS L1 CA";
config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells));
@ -337,7 +337,8 @@ bool HybridObservablesTest::acquire_signal()
{
tmp_gnss_synchro.System = 'E';
std::string signal = "1B";
signal.copy(tmp_gnss_synchro.Signal, 2, 0);
const char* str = signal.c_str(); // get a C style null terminated string
std::memcpy(static_cast<void*>(tmp_gnss_synchro.Signal), str, 3); // copy string into synchro char array: 2 char + null
tmp_gnss_synchro.PRN = SV_ID;
System_and_Signal = "Galileo E1B";
config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells));
@ -347,7 +348,8 @@ bool HybridObservablesTest::acquire_signal()
{
tmp_gnss_synchro.System = 'G';
std::string signal = "2S";
signal.copy(tmp_gnss_synchro.Signal, 2, 0);
const char* str = signal.c_str(); // get a C style null terminated string
std::memcpy(static_cast<void*>(tmp_gnss_synchro.Signal), str, 3); // copy string into synchro char array: 2 char + null
tmp_gnss_synchro.PRN = SV_ID;
System_and_Signal = "GPS L2CM";
config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells));
@ -357,7 +359,8 @@ bool HybridObservablesTest::acquire_signal()
{
tmp_gnss_synchro.System = 'E';
std::string signal = "5X";
signal.copy(tmp_gnss_synchro.Signal, 2, 0);
const char* str = signal.c_str(); // get a C style null terminated string
std::memcpy(static_cast<void*>(tmp_gnss_synchro.Signal), str, 3); // copy string into synchro char array: 2 char + null
tmp_gnss_synchro.PRN = SV_ID;
System_and_Signal = "Galileo E5a";
config->set_property("Acquisition_5X.coherent_integration_time_ms", "1");
@ -372,7 +375,8 @@ bool HybridObservablesTest::acquire_signal()
{
tmp_gnss_synchro.System = 'E';
std::string signal = "5X";
signal.copy(tmp_gnss_synchro.Signal, 2, 0);
const char* str = signal.c_str(); // get a C style null terminated string
std::memcpy(static_cast<void*>(tmp_gnss_synchro.Signal), str, 3); // copy string into synchro char array: 2 char + null
tmp_gnss_synchro.PRN = SV_ID;
System_and_Signal = "Galileo E5a";
config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells));
@ -382,7 +386,8 @@ bool HybridObservablesTest::acquire_signal()
{
tmp_gnss_synchro.System = 'G';
std::string signal = "L5";
signal.copy(tmp_gnss_synchro.Signal, 2, 0);
const char* str = signal.c_str(); // get a C style null terminated string
std::memcpy(static_cast<void*>(tmp_gnss_synchro.Signal), str, 3); // copy string into synchro char array: 2 char + null
tmp_gnss_synchro.PRN = SV_ID;
System_and_Signal = "GPS L5I";
config->set_property("Acquisition.max_dwells", std::to_string(FLAGS_external_signal_acquisition_dwells));
@ -579,7 +584,7 @@ void HybridObservablesTest::configure_receiver(
std::memcpy(static_cast<void*>(gnss_synchro_master.Signal), str, 3); // copy string into synchro char array: 2 char + null
config->set_property("Tracking.early_late_space_chips", "0.5");
config->set_property("Tracking.track_pilot", "false");
config->set_property("Tracking.track_pilot", "true");
config->set_property("TelemetryDecoder.implementation", "GPS_L2C_Telemetry_Decoder");
}
@ -596,7 +601,7 @@ void HybridObservablesTest::configure_receiver(
config->supersede_property("Tracking.implementation", std::string("Galileo_E5a_DLL_PLL_Tracking"));
}
config->set_property("Tracking.early_late_space_chips", "0.5");
config->set_property("Tracking.track_pilot", "false");
config->set_property("Tracking.track_pilot", "true");
config->set_property("Tracking.order", "2");
config->set_property("TelemetryDecoder.implementation", "Galileo_E5a_Telemetry_Decoder");
@ -610,7 +615,7 @@ void HybridObservablesTest::configure_receiver(
std::memcpy(static_cast<void*>(gnss_synchro_master.Signal), str, 3); // copy string into synchro char array: 2 char + null
config->set_property("Tracking.early_late_space_chips", "0.5");
config->set_property("Tracking.track_pilot", "false");
config->set_property("Tracking.track_pilot", "true");
config->set_property("Tracking.order", "2");
config->set_property("TelemetryDecoder.implementation", "GPS_L5_Telemetry_Decoder");
@ -888,9 +893,9 @@ void HybridObservablesTest::check_results_carrier_doppler_double_diff(
ASSERT_LT(error_mean, 5);
ASSERT_GT(error_mean, -5);
//assuming PLL BW=35
ASSERT_LT(error_var, 200);
ASSERT_LT(max_error, 70);
ASSERT_GT(min_error, -70);
ASSERT_LT(error_var, 250);
ASSERT_LT(max_error, 100);
ASSERT_GT(min_error, -100);
ASSERT_LT(rmse, 30);
}
@ -967,9 +972,9 @@ void HybridObservablesTest::check_results_carrier_doppler(
ASSERT_LT(error_mean_ch0, 5);
ASSERT_GT(error_mean_ch0, -5);
//assuming PLL BW=35
ASSERT_LT(error_var_ch0, 200);
ASSERT_LT(max_error_ch0, 70);
ASSERT_GT(min_error_ch0, -70);
ASSERT_LT(error_var_ch0, 250);
ASSERT_LT(max_error_ch0, 100);
ASSERT_GT(min_error_ch0, -100);
ASSERT_LT(rmse_ch0, 30);
}
@ -1195,7 +1200,7 @@ bool HybridObservablesTest::ReadRinexObs(std::vector<arma::mat>* obs_vec, Gnss_S
dataobj = r_ref_data.getObs(prn, "L5I", r_ref_header);
obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 3) = dataobj.data;
}
else if (strcmp("5X\0", gnss.Signal) == 0) //Simulator gives RINEX with E5a+E5b
else if (strcmp("5X\0", gnss.Signal) == 0) //Simulator gives RINEX with E5a+E5b. Doppler and accumulated Carrier phase WILL differ
{
obs_vec->at(n)(obs_vec->at(n).n_rows - 1, 0) = sow;
dataobj = r_ref_data.getObs(prn, "C8I", r_ref_header);
@ -1579,8 +1584,8 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
}
arma::vec receiver_time_offset_ref_channel_s;
receiver_time_offset_ref_channel_s = true_obs_vec.at(min_pr_ch_id).col(1) / GPS_C_m_s - GPS_STARTOFFSET_ms / 1000.0;
std::cout << "Ref channel initial Receiver time offset " << receiver_time_offset_ref_channel_s(0) * 1e3 << " [ms]" << std::endl;
receiver_time_offset_ref_channel_s = (true_obs_vec.at(min_pr_ch_id).col(1)(0) - measured_obs_vec.at(min_pr_ch_id).col(4)(0)) / GPS_C_m_s;
std::cout << "Ref. channel initial Receiver time offset " << receiver_time_offset_ref_channel_s(0) * 1e3 << " [ms]" << std::endl;
for (unsigned int n = 0; n < measured_obs_vec.size(); n++)
{
@ -1624,21 +1629,27 @@ TEST_F(HybridObservablesTest, ValidationOfResults)
measured_obs_vec.at(n),
measured_obs_vec.at(min_pr_ch_id),
"[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " ");
check_results_carrier_phase_double_diff(true_obs_vec.at(n),
true_obs_vec.at(min_pr_ch_id),
true_TOW_ch_s,
true_TOW_ref_ch_s,
measured_obs_vec.at(n),
measured_obs_vec.at(min_pr_ch_id),
"[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " ");
check_results_carrier_doppler_double_diff(true_obs_vec.at(n),
true_obs_vec.at(min_pr_ch_id),
true_TOW_ch_s,
true_TOW_ref_ch_s,
measured_obs_vec.at(n),
measured_obs_vec.at(min_pr_ch_id),
"[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " ");
//Do not compare E5a with E5 RINEX due to the Doppler frequency discrepancy caused by the different center frequencies
//E5a_fc=1176.45e6, E5b_fc=1207.14e6, E5_fc=1191.795e6;
if (strcmp("5X\0", gnss_synchro_vec.at(n).Signal) != 0 or FLAGS_compare_with_5X)
{
check_results_carrier_phase_double_diff(true_obs_vec.at(n),
true_obs_vec.at(min_pr_ch_id),
true_TOW_ch_s,
true_TOW_ref_ch_s,
measured_obs_vec.at(n),
measured_obs_vec.at(min_pr_ch_id),
"[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " ");
check_results_carrier_doppler_double_diff(true_obs_vec.at(n),
true_obs_vec.at(min_pr_ch_id),
true_TOW_ch_s,
true_TOW_ref_ch_s,
measured_obs_vec.at(n),
measured_obs_vec.at(min_pr_ch_id),
"[CH " + std::to_string(n) + "] PRN " + std::to_string(gnss_synchro_vec.at(n).PRN) + " ");
}
}
else
{

288
src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/galileo_fnav_inav_decoder_test.cc Normal file
View File

@ -0,0 +1,288 @@
/*!
* \file galileo_fnav_inav_decoder_test.cc
* \brief This class implements the unit test for the Galileo FNAV and INAV frames
* according to the Galileo ICD
* \author Javier Arribas, 2018. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "galileo_navigation_message.h"
#include "galileo_fnav_message.h"
#include "convolutional.h"
#include <unistd.h>
#include <chrono>
#include <exception>
#include <string>
#include <armadillo>
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <gtest/gtest.h>
class Galileo_FNAV_INAV_test : public ::testing::Test
{
public:
Galileo_Navigation_Message INAV_decoder;
Galileo_Fnav_Message FNAV_decoder;
// vars for Viterbi decoder
int32_t *out0, *out1, *state0, *state1;
int32_t g_encoder[2];
const int32_t nn = 2; // Coding rate 1/n
const int32_t KK = 7; // Constraint Length
int32_t mm = KK - 1;
int32_t flag_even_word_arrived;
void viterbi_decoder(double *page_part_symbols, int32_t *page_part_bits, int32_t _datalength)
{
Viterbi(page_part_bits, out0, state0, out1, state1,
page_part_symbols, KK, nn, _datalength);
}
void deinterleaver(int32_t rows, int32_t cols, double *in, double *out)
{
for (int32_t r = 0; r < rows; r++)
{
for (int32_t c = 0; c < cols; c++)
{
out[c * rows + r] = in[r * cols + c];
}
}
}
bool decode_INAV_word(double *page_part_symbols, int32_t frame_length)
{
// 1. De-interleave
double *page_part_symbols_deint = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
deinterleaver(GALILEO_INAV_INTERLEAVER_ROWS, GALILEO_INAV_INTERLEAVER_COLS, page_part_symbols, page_part_symbols_deint);
// 2. Viterbi decoder
// 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder)
// 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180º
for (int32_t i = 0; i < frame_length; i++)
{
if ((i + 1) % 2 == 0)
{
page_part_symbols_deint[i] = -page_part_symbols_deint[i];
}
}
int32_t *page_part_bits = static_cast<int32_t *>(volk_gnsssdr_malloc((frame_length / 2) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
const int32_t CodeLength = 240;
int32_t DataLength = (CodeLength / nn) - mm;
viterbi_decoder(page_part_symbols_deint, page_part_bits, DataLength);
volk_gnsssdr_free(page_part_symbols_deint);
// 3. Call the Galileo page decoder
std::string page_String;
for (int32_t i = 0; i < (frame_length / 2); i++)
{
if (page_part_bits[i] > 0)
{
page_String.push_back('1');
}
else
{
page_String.push_back('0');
}
}
bool crc_ok = false;
if (page_part_bits[0] == 1)
{
// DECODE COMPLETE WORD (even + odd) and TEST CRC
INAV_decoder.split_page(page_String, flag_even_word_arrived);
if (INAV_decoder.flag_CRC_test == true)
{
std::cout << "Galileo E1 INAV PAGE CRC correct \n";
//std::cout << "Galileo E1 CRC correct on channel " << d_channel << " from satellite " << d_satellite << std::endl;
crc_ok = true;
}
flag_even_word_arrived = 0;
}
else
{
// STORE HALF WORD (even page)
INAV_decoder.split_page(page_String.c_str(), flag_even_word_arrived);
flag_even_word_arrived = 1;
}
volk_gnsssdr_free(page_part_bits);
return crc_ok;
}
bool decode_FNAV_word(double *page_symbols, int32_t frame_length)
{
// 1. De-interleave
double *page_symbols_deint = static_cast<double *>(volk_gnsssdr_malloc(frame_length * sizeof(double), volk_gnsssdr_get_alignment()));
deinterleaver(GALILEO_FNAV_INTERLEAVER_ROWS, GALILEO_FNAV_INTERLEAVER_COLS, page_symbols, page_symbols_deint);
// 2. Viterbi decoder
// 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder)
// 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180
for (int32_t i = 0; i < frame_length; i++)
{
if ((i + 1) % 2 == 0)
{
page_symbols_deint[i] = -page_symbols_deint[i];
}
}
int32_t *page_bits = static_cast<int32_t *>(volk_gnsssdr_malloc((frame_length / 2) * sizeof(int32_t), volk_gnsssdr_get_alignment()));
const int32_t CodeLength = 488;
int32_t DataLength = (CodeLength / nn) - mm;
viterbi_decoder(page_symbols_deint, page_bits, DataLength);
volk_gnsssdr_free(page_symbols_deint);
// 3. Call the Galileo page decoder
std::string page_String;
for (int32_t i = 0; i < frame_length; i++)
{
if (page_bits[i] > 0)
{
page_String.push_back('1');
}
else
{
page_String.push_back('0');
}
}
volk_gnsssdr_free(page_bits);
// DECODE COMPLETE WORD (even + odd) and TEST CRC
FNAV_decoder.split_page(page_String);
if (FNAV_decoder.flag_CRC_test == true)
{
std::cout << "Galileo E5a FNAV PAGE CRC correct \n";
return true;
}
else
{
return false;
}
}
Galileo_FNAV_INAV_test()
{
// vars for Viterbi decoder
int32_t max_states = 1 << mm; // 2^mm
g_encoder[0] = 121; // Polynomial G1
g_encoder[1] = 91; // Polynomial G2
out0 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
out1 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
state0 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
state1 = static_cast<int32_t *>(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment()));
// create appropriate transition matrices
nsc_transit(out0, state0, 0, g_encoder, KK, nn);
nsc_transit(out1, state1, 1, g_encoder, KK, nn);
flag_even_word_arrived = 0;
}
~Galileo_FNAV_INAV_test()
{
volk_gnsssdr_free(out0);
volk_gnsssdr_free(out1);
volk_gnsssdr_free(state0);
volk_gnsssdr_free(state1);
}
};
TEST_F(Galileo_FNAV_INAV_test, ValidationOfResults)
{
std::chrono::time_point<std::chrono::system_clock> start, end;
std::chrono::duration<double> elapsed_seconds(0);
int repetitions = 10;
// FNAV FULLY ENCODED FRAME
double FNAV_frame[488] = {-1, 1, -1, -1, 1, -1, 1, 1, 1, -1, -1, 1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
1, -1, -1, 1, -1, -1, 1, 1, 1, -1, 1, -1, 1, 1, -1, 1, -1, -1, -1, -1, 1, -1, -1, 1, -1, -1, -1, -1, 1, 1, 1, 1, 1, 1,
-1, 1, -1, 1, -1, 1, 1, -1, -1, 1, -1, 1, -1, 1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -1, 1, 1, -1, 1, -1, 1, -1,
-1, 1, 1, -1, 1, 1, 1, 1, -1, 1, 1, 1, -1, 1, -1, 1, 1, -1, 1, -1, 1, 1, -1, 1, 1, 1, -1, -1, 1, 1, -1, -1, -1, -1,
-1, -1, -1, -1, 1, 1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 1, -1, 1, 1, 1, 1, -1, -1, -1, -1, -1, 1, 1,
-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, 1, 1, 1, 1, 1, -1, -1, 1, -1, -1, 1, 1, 1, 1, -1, -1, -1, -1, 1, -1, -1, -1,
-1, 1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -1, 1, 1, -1, -1, -1, -1, -1, 1, -1, -1, 1, 1, 1, 1, 1, 1, -1,
-1, 1, -1, 1, -1, -1, 1, -1, 1, -1, -1, -1, -1, 1, -1, 1, 1, -1, 1, -1, -1, -1, 1, -1, 1, 1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, 1, 1, 1, -1, 1, -1, -1, 1, 1, 1, 1, 1, -1, -1, 1, 1, -1, -1, -1, 1, -1, 1, -1, 1, 1, -1,
1, -1, 1, 1, -1, -1, -1, 1, 1, -1, 1, 1, 1, -1, -1, -1, -1, 1, -1, -1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -1,
-1, 1, -1, 1, -1, -1, -1, -1, -1, 1, -1, 1, 1, -1, -1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, -1, -1, -1, 1, -1, -1, -1, 1,
1, -1, 1, -1, -1, 1, 1, -1, -1, -1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 1, -1, 1, 1, 1, -1,
-1, 1, -1, -1, -1, -1, 1, -1, -1, -1, -1, 1, 1, 1, -1, 1, -1, 1, -1, 1, 1, -1, -1, 1, -1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1,
1, -1, -1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -1, 1, -1, -1, -1, 1, 1, 1, -1, 1, 1, 1, 1, -1, -1, -1, 1, 1, 1, 1, 1,
1, -1, 1, -1, -1, 1, 1, 1, -1, -1, 1, -1, 1, 1};
ASSERT_NO_THROW({
for (int n = 0; n < repetitions; n++)
{
EXPECT_EQ(decode_FNAV_word(&FNAV_frame[0], 488), true);
}
}) << "Exception during FNAV frame decoding";
// INAV FULLY ENCODED FRAME
double INAV_frame_even[240] = {-1, -1, 1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, 1, 1, -1, -1, -1, 1, 1,
-1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, -1, -1, 1, -1, -1, -1, 1, -1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, -1, 1, 1, -1, 1, 1, 1, 1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, 1, -1, 1, -1, -1, -1, -1, 1, 1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, 1, -1, -1, -1, -1, 1, 1, -1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -1,
1, -1, 1, 1, -1, -1, -1, -1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -1, -1, -1, 1,
-1, -1, 1, 1, 1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 1, 1, -1, 1, 1, 1};
double INAV_frame_odd[240] = {1, -1, 1, 1, 1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
1, 1, 1, 1, -1, 1, -1, -1, 1, -1, 1, -1, -1, 1, -1, 1,
1, 1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -1, -1,
-1, -1, 1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, -1, -1, 1,
1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
-1, 1, -1, 1, -1, 1, 1, -1, -1, 1, 1, 1, 1, -1, -1, -1,
1, 1, 1, 1, 1, 1, 1, 1, 1, -1, 1, 1, 1, 1, -1, -1,
1, -1, 1, -1, -1, -1, 1, 1, -1, -1, 1, -1, 1, 1, -1, -1,
-1, -1, -1, -1, -1, -1, 1, -1, 1, 1, 1, 1, -1, 1, -1, 1,
1, 1, -1, 1, 1, 1, 1, 1, -1, -1, -1, 1, 1, 1, 1, 1,
1, 1, 1, 1, -1, 1, -1, -1, -1, -1, -1, 1, -1, 1, -1, 1,
-1, -1, -1, -1, -1, -1, 1, 1, 1, 1, -1, -1, -1, -1, -1, -1,
-1, -1, 1, -1, -1, -1, -1, -1, -1, 1, -1, 1, 1, 1, 1, 1,
1, -1, -1, -1, -1, 1, 1, -1, 1, 1, 1, 1, 1, 1, 1, 1,
1, 1, 1, 1, 1, 1, -1, -1, -1, -1, 1, 1, -1, -1, 1, 1};
ASSERT_NO_THROW({
for (int n = 0; n < repetitions; n++)
{
decode_INAV_word(&INAV_frame_even[0], 240);
EXPECT_EQ(decode_INAV_word(&INAV_frame_odd[0], 240), true);
}
}) << "Exception during INAV frame decoding";
std::cout << "Galileo FNAV/INAV Test completed in " << elapsed_seconds.count() * 1e6 << " microseconds" << std::endl;
}

69
src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc Normal file
View File

@ -0,0 +1,69 @@
/*!
* \file bayesian_estimation_positivity_test.cc
* \brief This file implements timing tests for the Bayesian covariance estimator
* \author Gerald LaMountain, 20168. gerald(at)ece.neu.edu
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <random>
#include <armadillo>
#include <gtest/gtest.h>
#include "bayesian_estimation.h"
#define BAYESIAN_TEST_N_TRIALS 100
#define BAYESIAN_TEST_ITER 10000
TEST(BayesianEstimationPositivityTest, BayesianPositivityTest)
{
Bayesian_estimator bayes;
arma::vec bayes_mu = arma::zeros(1, 1);
int bayes_nu = 0;
int bayes_kappa = 0;
arma::mat bayes_Psi = arma::ones(1, 1);
arma::vec input = arma::zeros(1, 1);
//--- Perform initializations ------------------------------
std::random_device r;
std::default_random_engine e1(r());
std::normal_distribution<float> normal_dist(0, 5);
for (int k = 0; k < BAYESIAN_TEST_N_TRIALS; k++)
{
bayes.init(bayes_mu, bayes_kappa, bayes_nu, bayes_Psi);
for (int n = 0; n < BAYESIAN_TEST_ITER; n++)
{
input(0) = static_cast<double>(normal_dist(e1));
bayes.update_sequential(input);
arma::mat output = bayes.get_Psi_est();
double output0 = output(0, 0);
ASSERT_EQ(output0 > 0.0, true);
}
}
}

4
src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc
View File

@ -49,6 +49,7 @@ void run_correlator_cpu_real_codes(cpu_multicorrelator_real_codes* correlator,
float d_rem_carrier_phase_rad,
float d_carrier_phase_step_rad,
float d_code_phase_step_chips,
float d_code_phase_rate_step_chips,
float d_rem_code_phase_chips,
int correlation_size)
{
@ -58,6 +59,7 @@ void run_correlator_cpu_real_codes(cpu_multicorrelator_real_codes* correlator,
d_carrier_phase_step_rad,
d_code_phase_step_chips,
d_rem_code_phase_chips,
d_code_phase_rate_step_chips,
correlation_size);
}
}
@ -125,6 +127,7 @@ TEST(CpuMulticorrelatorRealCodesTest, MeasureExecutionTime)
float d_rem_carrier_phase_rad = 0.0;
float d_carrier_phase_step_rad = 0.1;
float d_code_phase_step_chips = 0.3;
float d_code_phase_rate_step_chips = 0.00001;
float d_rem_code_phase_chips = 0.4;
EXPECT_NO_THROW(
@ -141,6 +144,7 @@ TEST(CpuMulticorrelatorRealCodesTest, MeasureExecutionTime)
d_rem_carrier_phase_rad,
d_carrier_phase_step_rad,
d_code_phase_step_chips,
d_code_phase_rate_step_chips,
d_rem_code_phase_chips,
correlation_sizes[correlation_sizes_idx]));
}

568
src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc Normal file
View File

@ -0,0 +1,568 @@
/*!
* \file gps_l1_ca_kf_tracking_test.cc
* \brief This class implements a tracking test for GPS_L1_CA_KF_Tracking
* implementation based on some input parameters.
* \author Carles Fernandez, 2018
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2012-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include <chrono>
#include <unistd.h>
#include <vector>
#include <armadillo>
#include <boost/filesystem.hpp>
#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_block_factory.h"
#include "tracking_interface.h"
#include "in_memory_configuration.h"
#include "tracking_true_obs_reader.h"
#include "tracking_dump_reader.h"
#include "signal_generator_flags.h"
#include "gnuplot_i.h"
#include "test_flags.h"
#include "gnss_sdr_flags.h"
DEFINE_bool(plot_gps_l1_kf_tracking_test, false, "Plots results of GpsL1CAKfTrackingTest with gnuplot");
// ######## GNURADIO BLOCK MESSAGE RECEVER #########
class GpsL1CAKfTrackingTest_msg_rx;
typedef boost::shared_ptr<GpsL1CAKfTrackingTest_msg_rx> GpsL1CAKfTrackingTest_msg_rx_sptr;
GpsL1CAKfTrackingTest_msg_rx_sptr GpsL1CAKfTrackingTest_msg_rx_make();
class GpsL1CAKfTrackingTest_msg_rx : public gr::block
{
private:
friend GpsL1CAKfTrackingTest_msg_rx_sptr GpsL1CAKfTrackingTest_msg_rx_make();
void msg_handler_events(pmt::pmt_t msg);
GpsL1CAKfTrackingTest_msg_rx();
public:
int rx_message;
~GpsL1CAKfTrackingTest_msg_rx(); //!< Default destructor
};
GpsL1CAKfTrackingTest_msg_rx_sptr GpsL1CAKfTrackingTest_msg_rx_make()
{
return GpsL1CAKfTrackingTest_msg_rx_sptr(new GpsL1CAKfTrackingTest_msg_rx());
}
void GpsL1CAKfTrackingTest_msg_rx::msg_handler_events(pmt::pmt_t msg)
{
try
{
long int message = pmt::to_long(msg);
rx_message = message;
}
catch (boost::bad_any_cast& e)
{
LOG(WARNING) << "msg_handler_telemetry Bad any cast!";
rx_message = 0;
}
}
GpsL1CAKfTrackingTest_msg_rx::GpsL1CAKfTrackingTest_msg_rx() : gr::block("GpsL1CAKfTrackingTest_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0))
{
this->message_port_register_in(pmt::mp("events"));
this->set_msg_handler(pmt::mp("events"), boost::bind(&GpsL1CAKfTrackingTest_msg_rx::msg_handler_events, this, _1));
rx_message = 0;
}
GpsL1CAKfTrackingTest_msg_rx::~GpsL1CAKfTrackingTest_msg_rx()
{
}
// ###########################################################
class GpsL1CAKfTrackingTest : public ::testing::Test
{
public:
std::string generator_binary;
std::string p1;
std::string p2;
std::string p3;
std::string p4;
std::string p5;
std::string implementation = "GPS_L1_CA_KF_Tracking";
const int baseband_sampling_freq = FLAGS_fs_gen_sps;
std::string filename_rinex_obs = FLAGS_filename_rinex_obs;
std::string filename_raw_data = FLAGS_filename_raw_data;
int configure_generator();
int generate_signal();
void check_results_doppler(arma::vec& true_time_s,
arma::vec& true_value,
arma::vec& meas_time_s,
arma::vec& meas_value);
void check_results_acc_carrier_phase(arma::vec& true_time_s,
arma::vec& true_value,
arma::vec& meas_time_s,
arma::vec& meas_value);
void check_results_codephase(arma::vec& true_time_s,
arma::vec& true_value,
arma::vec& meas_time_s,
arma::vec& meas_value);
GpsL1CAKfTrackingTest()
{
factory = std::make_shared<GNSSBlockFactory>();
config = std::make_shared<InMemoryConfiguration>();
item_size = sizeof(gr_complex);
gnss_synchro = Gnss_Synchro();
}
~GpsL1CAKfTrackingTest()
{
}
void configure_receiver();
gr::top_block_sptr top_block;
std::shared_ptr<GNSSBlockFactory> factory;
std::shared_ptr<InMemoryConfiguration> config;
Gnss_Synchro gnss_synchro;
size_t item_size;
};
int GpsL1CAKfTrackingTest::configure_generator()
{
// Configure signal generator
generator_binary = FLAGS_generator_binary;
p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file;
if (FLAGS_dynamic_position.empty())
{
p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(FLAGS_duration * 10);
}
else
{
p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position);
}
p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output
p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples
p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); // Baseband sampling frequency [MSps]
return 0;
}
int GpsL1CAKfTrackingTest::generate_signal()
{
int child_status;
char* const parmList[] = {&generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL};
int pid;
if ((pid = fork()) == -1)
perror("fork err");
else if (pid == 0)
{
execv(&generator_binary[0], parmList);
std::cout << "Return not expected. Must be an execv err." << std::endl;
std::terminate();
}
waitpid(pid, &child_status, 0);
std::cout << "Signal and Observables RINEX and RAW files created." << std::endl;
return 0;
}
void GpsL1CAKfTrackingTest::configure_receiver()
{
gnss_synchro.Channel_ID = 0;
gnss_synchro.System = 'G';
std::string signal = "1C";
signal.copy(gnss_synchro.Signal, 2, 0);
gnss_synchro.PRN = FLAGS_test_satellite_PRN;
config->set_property("GNSS-SDR.internal_fs_sps", std::to_string(baseband_sampling_freq));
// Set Tracking
config->set_property("Tracking_1C.implementation", implementation);
config->set_property("Tracking_1C.item_type", "gr_complex");
if (FLAGS_dll_bw_hz != 0.0)
{
config->set_property("Tracking_1C.dll_bw_hz", std::to_string(FLAGS_dll_bw_hz));
}
else
{
config->set_property("Tracking_1C.dll_bw_hz", "2.0");
}
config->set_property("Tracking_1C.early_late_space_chips", "0.5");
config->set_property("Tracking_1C.extend_correlation_ms", "1");
config->set_property("Tracking_1C.dump", "true");
config->set_property("Tracking_1C.dump_filename", "./tracking_ch_");
}
void GpsL1CAKfTrackingTest::check_results_doppler(arma::vec& true_time_s,
arma::vec& true_value,
arma::vec& meas_time_s,
arma::vec& meas_value)
{
// 1. True value interpolation to match the measurement times
arma::vec true_value_interp;
arma::uvec true_time_s_valid = find(true_time_s > 0);
true_time_s = true_time_s(true_time_s_valid);
true_value = true_value(true_time_s_valid);
arma::uvec meas_time_s_valid = find(meas_time_s > 0);
meas_time_s = meas_time_s(meas_time_s_valid);
meas_value = meas_value(meas_time_s_valid);
arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp);
// 2. RMSE
arma::vec err;
err = meas_value - true_value_interp;
arma::vec err2 = arma::square(err);
double rmse = sqrt(arma::mean(err2));
// 3. Mean err and variance
double error_mean = arma::mean(err);
double error_var = arma::var(err);
// 4. Peaks
double max_error = arma::max(err);
double min_error = arma::min(err);
// 5. report
std::streamsize ss = std::cout.precision();
std::cout << std::setprecision(10) << "TRK Doppler RMSE=" << rmse
<< ", mean=" << error_mean
<< ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Hz]" << std::endl;
std::cout.precision(ss);
}
void GpsL1CAKfTrackingTest::check_results_acc_carrier_phase(arma::vec& true_time_s,
arma::vec& true_value,
arma::vec& meas_time_s,
arma::vec& meas_value)
{
// 1. True value interpolation to match the measurement times
arma::vec true_value_interp;
arma::uvec true_time_s_valid = find(true_time_s > 0);
true_time_s = true_time_s(true_time_s_valid);
true_value = true_value(true_time_s_valid);
arma::uvec meas_time_s_valid = find(meas_time_s > 0);
meas_time_s = meas_time_s(meas_time_s_valid);
meas_value = meas_value(meas_time_s_valid);
arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp);
// 2. RMSE
arma::vec err;
err = meas_value - true_value_interp;
arma::vec err2 = arma::square(err);
double rmse = sqrt(arma::mean(err2));
// 3. Mean err and variance
double error_mean = arma::mean(err);
double error_var = arma::var(err);
// 4. Peaks
double max_error = arma::max(err);
double min_error = arma::min(err);
// 5. report
std::streamsize ss = std::cout.precision();
std::cout << std::setprecision(10) << "TRK acc carrier phase RMSE=" << rmse
<< ", mean=" << error_mean
<< ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Hz]" << std::endl;
std::cout.precision(ss);
}
void GpsL1CAKfTrackingTest::check_results_codephase(arma::vec& true_time_s,
arma::vec& true_value,
arma::vec& meas_time_s,
arma::vec& meas_value)
{
// 1. True value interpolation to match the measurement times
arma::vec true_value_interp;
arma::uvec true_time_s_valid = find(true_time_s > 0);
true_time_s = true_time_s(true_time_s_valid);
true_value = true_value(true_time_s_valid);
arma::uvec meas_time_s_valid = find(meas_time_s > 0);
meas_time_s = meas_time_s(meas_time_s_valid);
meas_value = meas_value(meas_time_s_valid);
arma::interp1(true_time_s, true_value, meas_time_s, true_value_interp);
// 2. RMSE
arma::vec err;
err = meas_value - true_value_interp;
arma::vec err2 = arma::square(err);
double rmse = sqrt(arma::mean(err2));
// 3. Mean err and variance
double error_mean = arma::mean(err);
double error_var = arma::var(err);
// 4. Peaks
double max_error = arma::max(err);
double min_error = arma::min(err);
// 5. report
std::streamsize ss = std::cout.precision();
std::cout << std::setprecision(10) << "TRK code phase RMSE=" << rmse
<< ", mean=" << error_mean
<< ", stdev=" << sqrt(error_var) << " (max,min)=" << max_error << "," << min_error << " [Chips]" << std::endl;
std::cout.precision(ss);
}
TEST_F(GpsL1CAKfTrackingTest, ValidationOfResults)
{
// Configure the signal generator
configure_generator();
// Generate signal raw signal samples and observations RINEX file
if (FLAGS_disable_generator == false)
{
generate_signal();
}
std::chrono::time_point<std::chrono::system_clock> start, end;
configure_receiver();
// open true observables log file written by the simulator
tracking_true_obs_reader true_obs_data;
int test_satellite_PRN = FLAGS_test_satellite_PRN;
std::cout << "Testing satellite PRN=" << test_satellite_PRN << std::endl;
std::string true_obs_file = std::string("./gps_l1_ca_obs_prn");
true_obs_file.append(std::to_string(test_satellite_PRN));
true_obs_file.append(".dat");
ASSERT_EQ(true_obs_data.open_obs_file(true_obs_file), true) << "Failure opening true observables file";
top_block = gr::make_top_block("Tracking test");
std::shared_ptr<GNSSBlockInterface> trk_ = factory->GetBlock(config, "Tracking_1C", implementation, 1, 1);
std::shared_ptr<TrackingInterface> tracking = std::dynamic_pointer_cast<TrackingInterface>(trk_); //std::make_shared<GpsL1CaDllPllCAidTracking>(config.get(), "Tracking_1C", 1, 1);
boost::shared_ptr<GpsL1CAKfTrackingTest_msg_rx> msg_rx = GpsL1CAKfTrackingTest_msg_rx_make();
// load acquisition data based on the first epoch of the true observations
ASSERT_EQ(true_obs_data.read_binary_obs(), true)
<< "Failure reading true tracking dump file." << std::endl
<< "Maybe sat PRN #" + std::to_string(FLAGS_test_satellite_PRN) +
" is not available?";
// restart the epoch counter
true_obs_data.restart();
std::cout << "Initial Doppler [Hz]=" << true_obs_data.doppler_l1_hz << " Initial code delay [Chips]=" << true_obs_data.prn_delay_chips << std::endl;
gnss_synchro.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD;
gnss_synchro.Acq_doppler_hz = true_obs_data.doppler_l1_hz;
gnss_synchro.Acq_samplestamp_samples = 0;
ASSERT_NO_THROW({
tracking->set_channel(gnss_synchro.Channel_ID);
}) << "Failure setting channel.";
ASSERT_NO_THROW({
tracking->set_gnss_synchro(&gnss_synchro);
}) << "Failure setting gnss_synchro.";
ASSERT_NO_THROW({
tracking->connect(top_block);
}) << "Failure connecting tracking to the top_block.";
ASSERT_NO_THROW({
std::string file = "./" + filename_raw_data;
const char* file_name = file.c_str();
gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false);
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 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro));
top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0);
top_block->connect(gr_interleaved_char_to_complex, 0, tracking->get_left_block(), 0);
top_block->connect(tracking->get_right_block(), 0, sink, 0);
top_block->msg_connect(tracking->get_right_block(), pmt::mp("events"), msg_rx, pmt::mp("events"));
}) << "Failure connecting the blocks of tracking test.";
tracking->start_tracking();
EXPECT_NO_THROW({
start = std::chrono::system_clock::now();
top_block->run(); // Start threads and wait
end = std::chrono::system_clock::now();
}) << "Failure running the top_block.";
// check results
// load the true values
long int nepoch = true_obs_data.num_epochs();
std::cout << "True observation epochs=" << nepoch << std::endl;
arma::vec true_timestamp_s = arma::zeros(nepoch, 1);
arma::vec true_acc_carrier_phase_cycles = arma::zeros(nepoch, 1);
arma::vec true_Doppler_Hz = arma::zeros(nepoch, 1);
arma::vec true_prn_delay_chips = arma::zeros(nepoch, 1);
arma::vec true_tow_s = arma::zeros(nepoch, 1);
long int epoch_counter = 0;
while (true_obs_data.read_binary_obs())
{
true_timestamp_s(epoch_counter) = true_obs_data.signal_timestamp_s;
true_acc_carrier_phase_cycles(epoch_counter) = true_obs_data.acc_carrier_phase_cycles;
true_Doppler_Hz(epoch_counter) = true_obs_data.doppler_l1_hz;
true_prn_delay_chips(epoch_counter) = true_obs_data.prn_delay_chips;
true_tow_s(epoch_counter) = true_obs_data.tow;
epoch_counter++;
}
//load the measured values
tracking_dump_reader trk_dump;
ASSERT_EQ(trk_dump.open_obs_file(std::string("./tracking_ch_0.dat")), true)
<< "Failure opening tracking dump file";
nepoch = trk_dump.num_epochs();
std::cout << "Measured observation epochs=" << nepoch << std::endl;
//trk_dump.restart();
arma::vec trk_timestamp_s = arma::zeros(nepoch, 1);
arma::vec trk_acc_carrier_phase_cycles = arma::zeros(nepoch, 1);
arma::vec trk_Doppler_Hz = arma::zeros(nepoch, 1);
arma::vec trk_prn_delay_chips = arma::zeros(nepoch, 1);
std::vector<double> prompt;
std::vector<double> early;
std::vector<double> late;
std::vector<double> promptI;
std::vector<double> promptQ;
epoch_counter = 0;
while (trk_dump.read_binary_obs())
{
trk_timestamp_s(epoch_counter) = static_cast<double>(trk_dump.PRN_start_sample_count) / static_cast<double>(baseband_sampling_freq);
trk_acc_carrier_phase_cycles(epoch_counter) = trk_dump.acc_carrier_phase_rad / GPS_TWO_PI;
trk_Doppler_Hz(epoch_counter) = trk_dump.carrier_doppler_hz;
double delay_chips = GPS_L1_CA_CODE_LENGTH_CHIPS - GPS_L1_CA_CODE_LENGTH_CHIPS * (fmod((static_cast<double>(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast<double>(baseband_sampling_freq), 1.0e-3) / 1.0e-3);
trk_prn_delay_chips(epoch_counter) = delay_chips;
epoch_counter++;
prompt.push_back(trk_dump.abs_P);
early.push_back(trk_dump.abs_E);
late.push_back(trk_dump.abs_L);
promptI.push_back(trk_dump.prompt_I);
promptQ.push_back(trk_dump.prompt_Q);
}
// Align initial measurements and cut the tracking pull-in transitory
double pull_in_offset_s = 1.0;
arma::uvec initial_meas_point = arma::find(trk_timestamp_s >= (true_timestamp_s(0) + pull_in_offset_s), 1, "first");
trk_timestamp_s = trk_timestamp_s.subvec(initial_meas_point(0), trk_timestamp_s.size() - 1);
trk_acc_carrier_phase_cycles = trk_acc_carrier_phase_cycles.subvec(initial_meas_point(0), trk_acc_carrier_phase_cycles.size() - 1);
trk_Doppler_Hz = trk_Doppler_Hz.subvec(initial_meas_point(0), trk_Doppler_Hz.size() - 1);
trk_prn_delay_chips = trk_prn_delay_chips.subvec(initial_meas_point(0), trk_prn_delay_chips.size() - 1);
check_results_doppler(true_timestamp_s, true_Doppler_Hz, trk_timestamp_s, trk_Doppler_Hz);
check_results_codephase(true_timestamp_s, true_prn_delay_chips, trk_timestamp_s, trk_prn_delay_chips);
check_results_acc_carrier_phase(true_timestamp_s, true_acc_carrier_phase_cycles, trk_timestamp_s, trk_acc_carrier_phase_cycles);
std::chrono::duration<double> elapsed_seconds = end - start;
std::cout << "Signal tracking completed in " << elapsed_seconds.count() << " seconds." << std::endl;
if (FLAGS_plot_gps_l1_kf_tracking_test == true)
{
const std::string gnuplot_executable(FLAGS_gnuplot_executable);
if (gnuplot_executable.empty())
{
std::cout << "WARNING: Although the flag plot_gps_l1_tracking_test has been set to TRUE," << std::endl;
std::cout << "gnuplot has not been found in your system." << std::endl;
std::cout << "Test results will not be plotted." << std::endl;
}
else
{
try
{
boost::filesystem::path p(gnuplot_executable);
boost::filesystem::path dir = p.parent_path();
std::string gnuplot_path = dir.native();
Gnuplot::set_GNUPlotPath(gnuplot_path);
std::vector<double> timevec;
double t = 0.0;
for (auto it = prompt.begin(); it != prompt.end(); it++)
{
timevec.push_back(t);
t = t + GPS_L1_CA_CODE_PERIOD;
}
Gnuplot g1("linespoints");
g1.set_title("GPS L1 C/A signal tracking correlators' output (satellite PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")");
g1.set_grid();
g1.set_xlabel("Time [s]");
g1.set_ylabel("Correlators' output");
g1.cmd("set key box opaque");
unsigned int decimate = static_cast<unsigned int>(FLAGS_plot_decimate);
g1.plot_xy(timevec, prompt, "Prompt", decimate);
g1.plot_xy(timevec, early, "Early", decimate);
g1.plot_xy(timevec, late, "Late", decimate);
g1.savetops("Correlators_outputs");
g1.savetopdf("Correlators_outputs", 18);
g1.showonscreen(); // window output
Gnuplot g2("points");
g2.set_title("Constellation diagram (satellite PRN #" + std::to_string(FLAGS_test_satellite_PRN) + ")");
g2.set_grid();
g2.set_xlabel("Inphase");
g2.set_ylabel("Quadrature");
g2.cmd("set size ratio -1");
g2.plot_xy(promptI, promptQ);
g2.savetops("Constellation");
g2.savetopdf("Constellation", 18);
g2.showonscreen(); // window output
}
catch (const GnuplotException& ge)
{
std::cout << ge.what() << std::endl;
}
}
}
}

93
src/utils/matlab/gps_l1_ca_kf_plot_sample.m Normal file
View File

@ -0,0 +1,93 @@
% Reads GNSS-SDR Tracking dump binary file using the provided
% function and plots some internal variables
% Javier Arribas, 2011. jarribas(at)cttc.es
% -------------------------------------------------------------------------
%
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
%
% GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver
%
% This file is part of GNSS-SDR.
%
% GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% at your option) any later version.
%
% GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
%
% -------------------------------------------------------------------------
%
close all;
clear all;
if ~exist('dll_pll_veml_read_tracking_dump.m', 'file')
addpath('./libs')
end
samplingFreq = 6625000; %[Hz]
channels = 8;
first_channel = 0;
code_period = 0.001;
path = '/archive/'; %% CHANGE THIS PATH
figpath = [path];
for N=1:1:channels
tracking_log_path = [path 'epl_tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE epl_tracking_ch_ BY YOUR dump_filename
GNSS_tracking(N) = gps_l1_ca_kf_read_tracking_dump(tracking_log_path);
end
% GNSS-SDR format conversion to MATLAB GPS receiver
for N=1:1:channels
trackResults(N).status = 'T'; %fake track
trackResults(N).codeFreq = GNSS_tracking(N).code_freq_hz.';
trackResults(N).carrFreq = GNSS_tracking(N).carrier_doppler_hz.';
trackResults(N).carrFreqRate = GNSS_tracking(N).carrier_dopplerrate_hz2.';
trackResults(N).dllDiscr = GNSS_tracking(N).code_error.';
trackResults(N).dllDiscrFilt = GNSS_tracking(N).code_nco.';
trackResults(N).pllDiscr = GNSS_tracking(N).carr_error.';
trackResults(N).pllDiscrFilt = GNSS_tracking(N).carr_nco.';
trackResults(N).I_P = GNSS_tracking(N).prompt_I.';
trackResults(N).Q_P = GNSS_tracking(N).prompt_Q.';
trackResults(N).I_E = GNSS_tracking(N).E.';
trackResults(N).I_L = GNSS_tracking(N).L.';
trackResults(N).Q_E = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).Q_L = zeros(1,length(GNSS_tracking(N).E));
trackResults(N).PRN = GNSS_tracking(N).PRN.';
trackResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
kalmanResults(N).PRN = GNSS_tracking(N).PRN.';
kalmanResults(N).innovation = GNSS_tracking(N).carr_error.';
kalmanResults(N).state1 = GNSS_tracking(N).carr_nco.';
kalmanResults(N).state2 = GNSS_tracking(N).carrier_doppler_hz.';
kalmanResults(N).state3 = GNSS_tracking(N).carrier_dopplerrate_hz2.';
kalmanResults(N).r_noise_cov = GNSS_tracking(N).carr_noise_sigma2.';
kalmanResults(N).CNo = GNSS_tracking(N).CN0_SNV_dB_Hz.';
% Use original MATLAB tracking plot function
settings.numberOfChannels = channels;
settings.msToProcess = length(GNSS_tracking(N).E);
settings.codePeriod = code_period;
settings.timeStartInSeconds = 20;
%plotTracking(N, trackResults, settings)
plotKalman(N, kalmanResults, settings)
saveas(gcf, [figpath 'epl_tracking_ch_' num2str(N) '_PRN_' num2str(trackResults(N).PRN(end)) '.png'], 'png')
end

158
src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m Normal file
View File

@ -0,0 +1,158 @@
% Usage: gps_l1_ca_kf_read_tracking_dump (filename, [count])
%
% Opens GNSS-SDR tracking binary log file .dat and returns the contents
% Read GNSS-SDR Tracking dump binary file into MATLAB.
% Javier Arribas, 2011. jarribas(at)cttc.es
% -------------------------------------------------------------------------
%
% Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
%
% GNSS-SDR is a software defined Global Navigation
% Satellite Systems receiver
%
% This file is part of GNSS-SDR.
%
% GNSS-SDR is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% at your option) any later version.
%
% GNSS-SDR is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
%
% -------------------------------------------------------------------------
%
function [GNSS_tracking] = gps_l1_ca_kf_read_tracking_dump (filename, count)
m = nargchk (1,2,nargin);
num_float_vars = 19;
num_unsigned_long_int_vars = 1;
num_double_vars = 1;
num_unsigned_int_vars = 1;
if(~isempty(strfind(computer('arch'), '64')))
% 64-bit computer
double_size_bytes = 8;
unsigned_long_int_size_bytes = 8;
float_size_bytes = 4;
unsigned_int_size_bytes = 4;
else
double_size_bytes = 8;
unsigned_long_int_size_bytes = 4;
float_size_bytes = 4;
unsigned_int_size_bytes = 4;
end
skip_bytes_each_read = float_size_bytes * num_float_vars + unsigned_long_int_size_bytes * num_unsigned_long_int_vars + ...
double_size_bytes * num_double_vars + num_unsigned_int_vars*unsigned_int_size_bytes;
bytes_shift = 0;
if (m)
usage (m);
end
if (nargin < 2)
count = Inf;
end
%loops_counter = fread (f, count, 'uint32',4*12);
f = fopen (filename, 'rb');
if (f < 0)
else
v1 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v2 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v3 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v4 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v5 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v6 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v7 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v8 = fread (f, count, 'long', skip_bytes_each_read - unsigned_long_int_size_bytes);
bytes_shift = bytes_shift + unsigned_long_int_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v9 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v10 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v11 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v12 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v13 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v14 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v15 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v16 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v17 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v18 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next interleaved float
v19 = fread (f, count, 'float', skip_bytes_each_read - float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next float
v20 = fread (f, count, 'float', skip_bytes_each_read-float_size_bytes);
bytes_shift = bytes_shift + float_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next double
v21 = fread (f, count, 'double', skip_bytes_each_read - double_size_bytes);
bytes_shift = bytes_shift + double_size_bytes;
fseek(f,bytes_shift,'bof'); % move to next unsigned int
v22 = fread (f, count, 'uint', skip_bytes_each_read - unsigned_int_size_bytes);
fclose (f);
GNSS_tracking.VE = v1;
GNSS_tracking.E = v2;
GNSS_tracking.P = v3;
GNSS_tracking.L = v4;
GNSS_tracking.VL = v5;
GNSS_tracking.prompt_I = v6;
GNSS_tracking.prompt_Q = v7;
GNSS_tracking.PRN_start_sample = v8;
GNSS_tracking.acc_carrier_phase_rad = v9;
GNSS_tracking.carrier_doppler_hz = v10;
GNSS_tracking.carrier_dopplerrate_hz2 = v11;
GNSS_tracking.code_freq_hz = v12;
GNSS_tracking.carr_error = v13;
GNSS_tracking.carr_noise_sigma2 = v14;
GNSS_tracking.carr_nco = v15;
GNSS_tracking.code_error = v16;
GNSS_tracking.code_nco = v17;
GNSS_tracking.CN0_SNV_dB_Hz = v18;
GNSS_tracking.carrier_lock_test = v19;
GNSS_tracking.var1 = v20;
GNSS_tracking.var2 = v21;
GNSS_tracking.PRN = v22;
end

135
src/utils/matlab/libs/plotKalman.m Normal file
View File

@ -0,0 +1,135 @@
function plotKalman(channelList, trackResults, settings)
% This function plots the tracking results for the given channel list.
%
% plotTracking(channelList, trackResults, settings)
%
% Inputs:
% channelList - list of channels to be plotted.
% trackResults - tracking results from the tracking function.
% settings - receiver settings.
%--------------------------------------------------------------------------
% SoftGNSS v3.0
%
% Copyright (C) Darius Plausinaitis
% Written by Darius Plausinaitis
%--------------------------------------------------------------------------
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301,
%USA.
%--------------------------------------------------------------------------
% Protection - if the list contains incorrect channel numbers
channelList = intersect(channelList, 1:settings.numberOfChannels);
%=== For all listed channels ==============================================
for channelNr = channelList
%% Select (or create) and clear the figure ================================
% The number 200 is added just for more convenient handling of the open
% figure windows, when many figures are closed and reopened.
% Figures drawn or opened by the user, will not be "overwritten" by
% this function.
figure(channelNr +200);
clf(channelNr +200);
set(channelNr +200, 'Name', ['Channel ', num2str(channelNr), ...
' (PRN ', ...
num2str(trackResults(channelNr).PRN(end-1)), ...
') results']);
timeStart = settings.timeStartInSeconds;
%% Draw axes ==============================================================
% Row 1
handles(1, 1) = subplot(4, 2, 1);
handles(1, 2) = subplot(4, 2, 2);
% Row 2
handles(2, 1) = subplot(4, 2, 3);
handles(2, 2) = subplot(4, 2, 4);
% Row 3
handles(3, 1) = subplot(4, 2, [5 6]);
% Row 4
handles(4, 1) = subplot(4, 2, [7 8]);
%% Plot all figures =======================================================
timeAxisInSeconds = (1:settings.msToProcess)/1000;
%----- CNo for signal----------------------------------
plot (handles(1, 1), timeAxisInSeconds, ...
trackResults(channelNr).CNo(1:settings.msToProcess), 'b');
grid (handles(1, 1));
axis (handles(1, 1), 'tight');
xlabel(handles(1, 1), 'Time (s)');
ylabel(handles(1, 1), 'CNo (dB-Hz)');
title (handles(1, 1), 'Carrier to Noise Ratio');
%----- PLL discriminator filtered----------------------------------
plot (handles(1, 2), timeAxisInSeconds, ...
trackResults(channelNr).state1(1:settings.msToProcess), 'b');
grid (handles(1, 2));
axis (handles(1, 2), 'tight');
xlim (handles(1, 2), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(1, 2), 'Time (s)');
ylabel(handles(1, 2), 'Phase Amplitude');
title (handles(1, 2), 'Filtered Carrier Phase');
%----- Carrier Frequency --------------------------------
plot (handles(2, 1), timeAxisInSeconds(2:end), ...
trackResults(channelNr).state2(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]);
grid (handles(2, 1));
axis (handles(2, 1));
xlim (handles(2, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(2, 1), 'Time (s)');
ylabel(handles(2, 1), 'Freq (hz)');
title (handles(2, 1), 'Filtered Doppler Frequency');
%----- Carrier Frequency Rate --------------------------------
plot (handles(2, 2), timeAxisInSeconds(2:end), ...
trackResults(channelNr).state3(2:settings.msToProcess), 'Color',[0.42 0.25 0.39]);
grid (handles(2, 2));
axis (handles(2, 2));
xlim (handles(2, 2), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(2, 2), 'Time (s)');
ylabel(handles(2, 2), 'Freq (hz)');
title (handles(2, 2), 'Filtered Doppler Frequency Rate');
%----- PLL discriminator unfiltered--------------------------------
plot (handles(3, 1), timeAxisInSeconds, ...
trackResults(channelNr).innovation, 'r');
grid (handles(3, 1));
axis (handles(3, 1), 'auto');
xlim (handles(3, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(3, 1), 'Time (s)');
ylabel(handles(3, 1), 'Amplitude');
title (handles(3, 1), 'Raw PLL discriminator (Innovation)');
%----- PLL discriminator covariance --------------------------------
plot (handles(4, 1), timeAxisInSeconds, ...
trackResults(channelNr).r_noise_cov, 'r');
grid (handles(4, 1));
axis (handles(4, 1), 'auto');
xlim (handles(4, 1), [timeStart, timeAxisInSeconds(end)]);
xlabel(handles(4, 1), 'Time (s)');
ylabel(handles(4, 1), 'Variance');
title (handles(4, 1), 'Estimated Noise Variance');
end % for channelNr = channelList