diff --git a/AUTHORS b/AUTHORS index e6508f5ce..6f98d2038 100644 --- a/AUTHORS +++ b/AUTHORS @@ -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 diff --git a/CMakeLists.txt b/CMakeLists.txt index 9be84be8d..4d8d7cc94 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/MANIFEST.md b/MANIFEST.md index d98411e9f..ea64ec678 100644 --- a/MANIFEST.md +++ b/MANIFEST.md @@ -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, diff --git a/conf/gnss-sdr-kalman-bayes.conf b/conf/gnss-sdr-kalman-bayes.conf new file mode 100644 index 000000000..051d080de --- /dev/null +++ b/conf/gnss-sdr-kalman-bayes.conf @@ -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 diff --git a/conf/gnss-sdr_GPS_L1_nsr_kf.conf b/conf/gnss-sdr_GPS_L1_nsr_kf.conf new file mode 100644 index 000000000..d2a84cd9f --- /dev/null +++ b/conf/gnss-sdr_GPS_L1_nsr_kf.conf @@ -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 diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 6bbf91c2d..9a5335c8a 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -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(static_cast(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(&tmp_double), sizeof(double)); - tmp_double = 0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_rx_time), sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } - } } } diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index 97e581f86..36d09fab4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -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; diff --git a/src/algorithms/PVT/libs/gpx_printer.cc b/src/algorithms/PVT/libs/gpx_printer.cc index 1949bb7dc..51fa6760f 100644 --- a/src/algorithms/PVT/libs/gpx_printer.cc +++ b/src/algorithms/PVT/libs/gpx_printer.cc @@ -123,8 +123,9 @@ bool Gpx_Printer::print_position(const std::shared_ptr& 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) { diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 472ffc9d4..1f87b8797 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -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& 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(&tmp_uint32), sizeof(uint32_t)); + // WEEK + tmp_uint32 = adjgpsweek(nav_data.eph[0].week); + d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); // PVT GPS time tmp_double = gnss_observables_map.begin()->second.RX_time; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position East [m] - tmp_double = rx_position_and_time(0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position North [m] - tmp_double = rx_position_and_time(1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - // ECEF User Position Up [m] - tmp_double = rx_position_and_time(2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // User clock offset [s] tmp_double = rx_position_and_time(3); d_dump_file.write(reinterpret_cast(&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(&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(&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(&tmp_double), sizeof(double)); // GEO user position Longitude [deg] - tmp_double = this->get_longitude(); + tmp_double = get_longitude(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // GEO user position Height [m] - tmp_double = this->get_height(); + tmp_double = get_height(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // NUMBER OF VALID SATS + d_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); + // RTKLIB solution status + d_dump_file.write(reinterpret_cast(&pvt_sol.stat), sizeof(uint8_t)); + // RTKLIB solution type (0:xyz-ecef,1:enu-baseline) + d_dump_file.write(reinterpret_cast(&pvt_sol.type), sizeof(uint8_t)); + //AR ratio factor for validation + tmp_double = pvt_sol.ratio; + d_dump_file.write(reinterpret_cast(&pvt_sol.ratio), sizeof(float)); + //AR ratio threshold for validation + tmp_double = pvt_sol.thres; + d_dump_file.write(reinterpret_cast(&pvt_sol.thres), sizeof(float)); + + //GDOP//PDOP//HDOP//VDOP + d_dump_file.write(reinterpret_cast(&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(); } } } diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc index 56824a840..fe7b70878 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc @@ -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(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(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; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc index ae5be3f95..318f6ad46 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc @@ -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(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(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 diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc index 66ef6b758..b1b6d699a 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc @@ -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(std::fmod(static_cast(indext), acq_parameters.samples_per_code)); d_gnss_synchro->Acq_doppler_hz = static_cast(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; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc index 5b81e1929..a056436a5 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc @@ -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(index_time); d_gnss_synchro->Acq_doppler_hz = static_cast(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; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc index 2609670a7..31c28c319 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc @@ -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(index_time); d_gnss_synchro->Acq_doppler_hz = static_cast(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) diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc index bae39c693..34228b89b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc @@ -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(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(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 diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc index 962dee76e..aa8bceffb 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc @@ -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(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(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(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(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; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc index 1c9466db1..06a91dccb 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc @@ -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(d_possible_delay[indext]); d_gnss_synchro->Acq_doppler_hz = static_cast(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; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc index 68694659e..77a714de4 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc @@ -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(indext % d_samples_per_code); d_gnss_synchro->Acq_doppler_hz = static_cast(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 diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 5cdd94726..14d90b902 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -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 */ diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 88cc2ca38..f7e4e76c5 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -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 */ diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f.h similarity index 74% rename from src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h rename to src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f.h index d33629277..c6705adca 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_fast_resamplerxnpuppet_32f.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_high_dynamics_resamplerxnpuppet_32f.h @@ -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
    *
  • Cillian O'Driscoll 2017 cillian.odriscoll at gmail dot com *
  • 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 #include #include @@ -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 diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn.h similarity index 91% rename from src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h rename to src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn.h index 283454bb1..8b666908b 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_fast_resampler_32f_xn.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/kernels/volk_gnsssdr/volk_gnsssdr_32f_xn_high_dynamics_resampler_32f_xn.h @@ -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
      @@ -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 @@ * * Dispatcher Prototype * \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 #include @@ -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 -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 -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 -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 -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 -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 -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 // -//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*/ diff --git a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h index 90281b32f..389e03b14 100644 --- a/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h +++ b/src/algorithms/libs/volk_gnsssdr_module/volk_gnsssdr/lib/kernel_tests.h @@ -93,7 +93,7 @@ std::vector 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)) diff --git a/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc index ab20cab1d..f7a663358 100644 --- a/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc @@ -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) diff --git a/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h index d762eabd8..119e294ad 100644 --- a/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.h @@ -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 @@ -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_; diff --git a/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.cc index 48ed8b216..99ab590ea 100644 --- a/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.cc @@ -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) diff --git a/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h b/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h index 4a4167bf5..022636384 100644 --- a/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h +++ b/src/algorithms/telemetry_decoder/adapters/galileo_e5a_telemetry_decoder.h @@ -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 @@ -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_; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt index 3af26e46b..5a49bf03a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt @@ -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( diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc deleted file mode 100644 index 769902600..000000000 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc +++ /dev/null @@ -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 . - * - * ------------------------------------------------------------------------- - */ - - -#include "galileo_e1b_telemetry_decoder_cc.h" -#include "control_message_factory.h" -#include "convolutional.h" -#include "gnss_synchro.h" -#include -#include -#include -#include -#include - - -#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(this->d_preambles_bits), static_cast(preambles_bits), GALILEO_INAV_PREAMBLE_LENGTH_BITS * sizeof(uint16_t)); - - // preamble bits to sampled symbols - d_preambles_symbols = static_cast(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(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); - out1 = static_cast(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); - state0 = static_cast(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); - state1 = static_cast(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(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(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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(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(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(&output_items[0]); // Get the output buffer pointer - const Gnss_Synchro **in = reinterpret_cast(&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(GALILEO_INAV_PAGE_SYMBOLS) + static_cast(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(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(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(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(d_nav.TOW_5 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(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(d_nav.TOW_6 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(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(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(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(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(d_TOW_at_current_symbol_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_symbol.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&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; - } -} diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc deleted file mode 100644 index b033627e7..000000000 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc +++ /dev/null @@ -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: - *
        - *
      • 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 . - * - * ------------------------------------------------------------------------- - */ - -#include "galileo_e5a_telemetry_decoder_cc.h" -#include "control_message_factory.h" -#include "convolutional.h" -#include "display.h" -#include -#include -#include -#include -#include -#include - - -#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(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(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); - out1 = static_cast(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); - state0 = static_cast(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); - state1 = static_cast(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(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(output_items[0]); // Get the output buffer pointer - const Gnss_Synchro *in = reinterpret_cast(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(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::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(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(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(d_nav.FNAV_TOW_1 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((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(d_nav.FNAV_TOW_2 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((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(d_nav.FNAV_TOW_3 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((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(d_nav.FNAV_TOW_4 * 1000.0); - d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((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(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(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(d_TOW_at_current_symbol_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_sample.Tracking_sample_counter; - d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); - tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; - d_dump_file.write(reinterpret_cast(&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; - } -} diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h deleted file mode 100644 index 522f5777e..000000000 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h +++ /dev/null @@ -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: - *
        - *
      • 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 . - * - * ------------------------------------------------------------------------- - */ - -#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 -#include -#include -#include - - -class galileo_e5a_telemetry_decoder_cc; - -typedef boost::shared_ptr 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 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 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_ */ diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc new file mode 100644 index 000000000..aa53529fe --- /dev/null +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + + +#include "galileo_telemetry_decoder_cc.h" +#include "control_message_factory.h" +#include "convolutional.h" +#include "display.h" +#include "gnss_synchro.h" +#include +#include +#include +#include +#include + + +#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(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(GALILEO_INAV_PAGE_SYMBOLS) + d_samples_per_preamble; + // preamble bits to sampled symbols + d_preamble_samples = static_cast(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(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(GALILEO_FNAV_SYMBOLS_PER_PAGE) * d_samples_per_symbol + d_samples_per_preamble; + // preamble bits to sampled symbols + d_preamble_samples = static_cast(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment())); + + d_secondary_code_samples = static_cast(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(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(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); + out1 = static_cast(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); + state0 = static_cast(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); + state1 = static_cast(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(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(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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(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(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(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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 tmp_obj = std::make_shared(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(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(&output_items[0]); // Get the output buffer pointer + const Gnss_Synchro **in = reinterpret_cast(&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(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(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(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(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(d_inav_nav.TOW_5 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(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(d_inav_nav.TOW_6 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(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(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(d_fnav_nav.FNAV_TOW_1 * 1000.0); + d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((d_required_symbols + 1) * GALILEO_E5a_CODE_PERIOD_MS); + //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((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(d_fnav_nav.FNAV_TOW_2 * 1000.0); + //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((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((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(d_fnav_nav.FNAV_TOW_3 * 1000.0); + //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((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((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(d_fnav_nav.FNAV_TOW_4 * 1000.0); + //d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast((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((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(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(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(d_TOW_at_current_symbol_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_ulong_int = current_symbol.Tracking_sample_counter; + d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); + tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; + d_dump_file.write(reinterpret_cast(&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; + } +} diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.h similarity index 62% rename from src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h rename to src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.h index 663c365b2..f67418868 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_cc.h @@ -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 -class galileo_e1b_telemetry_decoder_cc; +class galileo_telemetry_decoder_cc; -typedef boost::shared_ptr galileo_e1b_telemetry_decoder_cc_sptr; +typedef boost::shared_ptr 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 d_symbol_history; + std::deque 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 diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 73254cd6a..e4a12bd43 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -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(d_symbol_history.at(0).Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(d_symbol_history.at(0).fs)) * 1000.0); + preamble_diff_ms = std::round(((static_cast(d_symbol_history[0].Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(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(d_preamble_time_samples) / static_cast(d_symbol_history.at(0).fs) << " [s]"; + << static_cast(d_preamble_time_samples) / static_cast(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(d_symbol_history.at(0).Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(d_symbol_history.at(0).fs)) * 1000.0); + preamble_diff_ms = round(((static_cast(d_symbol_history[0].Tracking_sample_counter) - static_cast(d_preamble_time_samples)) / static_cast(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; diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index fe4b9292a..ae1754aa2 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -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} diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc new file mode 100644 index 000000000..38b3e3cad --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + + +#include "gps_l1_ca_kf_tracking.h" +#include "gnss_sdr_flags.h" +#include "GPS_L1_CA.h" +#include "configuration_interface.h" +#include + + +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(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_; +} diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h new file mode 100644 index 000000000..a687b8a9e --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_tracking.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 + +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_ diff --git a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc index 8fbd56531..11e2572c3 100644 --- a/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l5_dll_pll_tracking.cc @@ -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; diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index a0bd80004..b1a67d6bb 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -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} diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index 1f72b6884..374cb910f 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -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(Galileo_E5a_CODE_LENGTH_CHIPS); - d_secondary = true; + if (trk_parameters.track_pilot) { + d_secondary = true; d_secondary_code_length = static_cast(Galileo_E5a_Q_SECONDARY_CODE_LENGTH); signal_pretty_name = signal_pretty_name + "Q"; interchange_iq = true; } else { - d_secondary_code_length = static_cast(Galileo_E5a_I_SECONDARY_CODE_LENGTH); - d_secondary_code_string = const_cast(&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(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_rate_step_chips) * static_cast(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(d_rem_code_phase_chips) * static_cast(d_code_samples_per_chip), static_cast(d_code_phase_step_chips) * static_cast(d_code_samples_per_chip), + static_cast(d_code_phase_rate_step_chips) * static_cast(d_code_samples_per_chip), trk_parameters.vector_length); } } diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h index 8a2fb6e80..97f0c54cc 100755 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -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; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h index 4c12b4c92..47090681b 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking_fpga.h @@ -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 diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc new file mode 100644 index 000000000..f277bf216 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 +#include +#include +#include +#include +#include +#include +#include +#include + + +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(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(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(volk_gnsssdr_malloc(static_cast(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(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(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(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(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(d_sample_counter) - static_cast(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(acq_trk_diff_samples) / static_cast(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(d_code_freq_chips) / static_cast(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(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(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(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(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(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(size) / static_cast(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(&abs_VE[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_E[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_P[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_L[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&abs_VL[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_I[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&Prompt_Q[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&PRN_start_sample_count[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&acc_carrier_phase_rad[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_doppler_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_dopplerrate_hz2[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_noise_sigma2[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_filt_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_error_filt_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&CN0_SNV_dB_Hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carrier_lock_test[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux1[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&aux2[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&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(matfp) != NULL) + { + size_t dims[2] = {1, static_cast(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(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(input_items[0]); + Gnss_Synchro **out = reinterpret_cast(&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(d_current_prn_length_samples) - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(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(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(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = static_cast(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(d_fs_in); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(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(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - static_cast(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(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((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + static_cast(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(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(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); + try + { + // EPR + d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_L), sizeof(float)); + d_dump_file.write(reinterpret_cast(&tmp_VL), sizeof(float)); + // PROMPT I and Q (to analyze navigation symbols) + d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); + d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); + // PRN start sample stamp + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(uint64_t)); + // accumulated carrier phase + tmp_float = d_acc_carrier_phase_rad; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier and code frequency + tmp_float = d_carrier_doppler_hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_dopplerrate_hz2; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_code_freq_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // Kalman commands + tmp_float = static_cast(d_carr_phase_error_rad * GPS_TWO_PI); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(d_carr_phase_sigma2); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(d_rem_carr_phase_rad * GPS_TWO_PI); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // DLL commands + tmp_float = code_error_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = code_error_filt_chips; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // CN0 and carrier lock test + tmp_float = d_CN0_SNV_dB_Hz; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = d_carrier_lock_test; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = static_cast(d_sample_counter + static_cast(d_current_prn_length_samples)); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // PRN + uint32_t prn_ = d_acquisition_gnss_synchro->PRN; + d_dump_file.write(reinterpret_cast(&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 +} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h new file mode 100644 index 000000000..da293dc47 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_kf_tracking_cc.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 +#include +#include +#include +#include + +class Gps_L1_Ca_Kf_Tracking_cc; + +typedef boost::shared_ptr + 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 systemName; + std::string sys; + + int32_t save_matfile(); +}; + +#endif // GNSS_SDR_GPS_L1_CA_KF_TRACKING_CC_H diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index 593bfe55f..6ee4d4fce 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -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} diff --git a/src/algorithms/tracking/libs/bayesian_estimation.cc b/src/algorithms/tracking/libs/bayesian_estimation.cc new file mode 100644 index 000000000..30e7d45ec --- /dev/null +++ b/src/algorithms/tracking/libs/bayesian_estimation.cc @@ -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
        + *
      • Gerald LaMountain, 2018. gerald(at)ece.neu.edu + *
      • Jordi Vila-Valls 2018. jvila(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 . + * + * ------------------------------------------------------------------------- + */ + +#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; +} diff --git a/src/algorithms/tracking/libs/bayesian_estimation.h b/src/algorithms/tracking/libs/bayesian_estimation.h new file mode 100644 index 000000000..002e7a75c --- /dev/null +++ b/src/algorithms/tracking/libs/bayesian_estimation.h @@ -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
        + *
      • Gerald LaMountain, 2018. gerald(at)ece.neu.edu + *
      • Jordi Vila-Valls 2018. jvila(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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_BAYESIAN_ESTIMATION_H_ +#define GNSS_SDR_BAYESIAN_ESTIMATION_H_ + +#include +#include + +/*! \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 diff --git a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc index e54d77177..6222ddfb9 100644 --- a/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc +++ b/src/algorithms/tracking/libs/cpu_multicorrelator_real_codes.cc @@ -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
        *
      • Javier Arribas, 2015. jarribas(at)cttc.es *
      • 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 *
      • Javier Arribas, 2015. jarribas(at)cttc.es *
      • 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 *corr_out, const std::complex *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 *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; }; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.cc b/src/algorithms/tracking/libs/dll_pll_conf.cc index 9eadfde8f..525adcceb 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.cc +++ b/src/algorithms/tracking/libs/dll_pll_conf.cc @@ -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; diff --git a/src/algorithms/tracking/libs/dll_pll_conf.h b/src/algorithms/tracking/libs/dll_pll_conf.h index bda1853aa..d557b89c0 100644 --- a/src/algorithms/tracking/libs/dll_pll_conf.h +++ b/src/algorithms/tracking/libs/dll_pll_conf.h @@ -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; diff --git a/src/core/monitor/gnss_synchro_monitor.cc b/src/core/monitor/gnss_synchro_monitor.cc index bde09ef69..d788b52b0 100644 --- a/src/core/monitor/gnss_synchro_monitor.cc +++ b/src/core/monitor/gnss_synchro_monitor.cc @@ -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(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(&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 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 stocks; + stocks.push_back(in[i][epoch]); + udp_sink_ptr->write_gnss_synchro(stocks); + } + count = 0; } } return noutput_items; diff --git a/src/core/monitor/gnss_synchro_monitor.h b/src/core/monitor/gnss_synchro_monitor.h index 36b6f1e3a..9547bf12d 100644 --- a/src/core/monitor/gnss_synchro_monitor.h +++ b/src/core/monitor/gnss_synchro_monitor.h @@ -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 udp_sink_ptr; + int count; + public: gnss_synchro_monitor(unsigned int nchannels, diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 37625af4d..7ef051d26 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -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 GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GPS_L1_CA_KF_Tracking") == 0) + { + std::unique_ptr 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 block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams, @@ -1876,6 +1883,12 @@ std::unique_ptr GNSSBlockFactory::GetTrkBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("GPS_L1_CA_KF_Tracking") == 0) + { + std::unique_ptr 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 block_(new GpsL1CaDllPllCAidTracking(configuration.get(), role, in_streams, diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 3793709f7..72a7a013b 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1157,11 +1157,10 @@ void GNSSFlowgraph::init() */ enable_monitor_ = configuration_->property("Monitor.enable_monitor", false); - std::vector 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 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 available_sbas_prn = {120, 124, 126}; + std::set available_sbas_prn = {123, 131, 135, 136, 138}; std::set 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 GNSSFlowgraph::split_string(const std::string &s, char delim) +{ + std::vector v; + std::stringstream ss(s); + std::string item; + + while (std::getline(ss, item, delim)) + { + *(std::back_inserter(v)++) = item; + } + + return v; +} diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index 49b25eef4..1f1d07ada 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -186,6 +186,7 @@ private: bool enable_monitor_; gr::basic_block_sptr GnssSynchroMonitor_; + std::vector split_string(const std::string &s, char delim); }; #endif /*GNSS_SDR_GNSS_FLOWGRAPH_H_*/ diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index b1d2e6191..e25501e08 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -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; diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 646aa2b69..dd8c8ffc8 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -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"); diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 6db984477..9209c30b4 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -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; diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index 37d03a95b..8ed59e572 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -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} diff --git a/src/tests/common-files/observable_tests_flags.h b/src/tests/common-files/observable_tests_flags.h index 594429f36..e20253f34 100644 --- a/src/tests/common-files/observable_tests_flags.h +++ b/src/tests/common-files/observable_tests_flags.h @@ -35,7 +35,7 @@ #include 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 diff --git a/src/tests/common-files/signal_generator_flags.h b/src/tests/common-files/signal_generator_flags.h index 399c1ce26..b5e737adc 100644 --- a/src/tests/common-files/signal_generator_flags.h +++ b/src/tests/common-files/signal_generator_flags.h @@ -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"); diff --git a/src/tests/system-tests/libs/CMakeLists.txt b/src/tests/system-tests/libs/CMakeLists.txt new file mode 100644 index 000000000..a6459b058 --- /dev/null +++ b/src/tests/system-tests/libs/CMakeLists.txt @@ -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 . +# + + +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) \ No newline at end of file diff --git a/src/tests/system-tests/libs/geofunctions.cc b/src/tests/system-tests/libs/geofunctions.cc new file mode 100644 index 000000000..fbbe97266 --- /dev/null +++ b/src/tests/system-tests/libs/geofunctions.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ +#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; +} diff --git a/src/tests/system-tests/libs/geofunctions.h b/src/tests/system-tests/libs/geofunctions.h new file mode 100644 index 000000000..4cd0fb90d --- /dev/null +++ b/src/tests/system-tests/libs/geofunctions.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GEOFUNCTIONS_H +#define GEOFUNCTIONS_H + +#include +// %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 diff --git a/src/tests/system-tests/libs/position_test_flags.h b/src/tests/system-tests/libs/position_test_flags.h new file mode 100644 index 000000000..e911fb892 --- /dev/null +++ b/src/tests/system-tests/libs/position_test_flags.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_POSITION_TEST_FLAGS_H_ +#define GNSS_SDR_POSITION_TEST_FLAGS_H_ + +#include +#include + +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 diff --git a/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc new file mode 100644 index 000000000..2c2fc87f0 --- /dev/null +++ b/src/tests/system-tests/libs/rtklib_solver_dump_reader.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include "rtklib_solver_dump_reader.h" + +#include + +bool rtklib_solver_dump_reader::read_binary_obs() +{ + try + { + d_dump_file.read(reinterpret_cast(&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(&week), sizeof(week)); + // std::cout << "week: " << week << std::endl; + d_dump_file.read(reinterpret_cast(&RX_time), sizeof(RX_time)); + // std::cout << "RX_time: " << RX_time << std::endl; + d_dump_file.read(reinterpret_cast(&clk_offset_s), sizeof(clk_offset_s)); + // std::cout << "clk_offset_s: " << clk_offset_s << std::endl; + d_dump_file.read(reinterpret_cast(&rr[0]), sizeof(rr)); + // for (int n = 0; n < 6; n++) + // { + // std::cout << "rr: " << rr[n] << std::endl; + // } + d_dump_file.read(reinterpret_cast(&qr[0]), sizeof(qr)); + // for (int n = 0; n < 6; n++) + // { + // std::cout << "qr" << qr[n] << std::endl; + // } + d_dump_file.read(reinterpret_cast(&latitude), sizeof(latitude)); + std::cout << "latitude: " << latitude << std::endl; + d_dump_file.read(reinterpret_cast(&longitude), sizeof(longitude)); + std::cout << "longitude: " << longitude << std::endl; + d_dump_file.read(reinterpret_cast(&height), sizeof(height)); + std::cout << "height: " << height << std::endl; + d_dump_file.read(reinterpret_cast(&ns), sizeof(ns)); + // std::cout << "ns: " << (int)ns << std::endl; + d_dump_file.read(reinterpret_cast(&status), sizeof(status)); + // std::cout << "status: " << (int)status << std::endl; + d_dump_file.read(reinterpret_cast(&type), sizeof(type)); + // std::cout << "type: " << (int)type << std::endl; + d_dump_file.read(reinterpret_cast(&AR_ratio), sizeof(AR_ratio)); + // std::cout << "AR_ratio: " << AR_ratio << std::endl; + d_dump_file.read(reinterpret_cast(&AR_thres), sizeof(AR_thres)); + // std::cout << "AR_thres: " << AR_thres << std::endl; + d_dump_file.read(reinterpret_cast(&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(); + } +} diff --git a/src/tests/system-tests/libs/rtklib_solver_dump_reader.h b/src/tests/system-tests/libs/rtklib_solver_dump_reader.h new file mode 100644 index 000000000..e89659b37 --- /dev/null +++ b/src/tests/system-tests/libs/rtklib_solver_dump_reader.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H +#define GNSS_SDR_RTKLIB_SOLVER_DUMP_READER_H + +#include +#include +#include +#include + +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 diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc new file mode 100644 index 000000000..f4b3d741c --- /dev/null +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include "spirent_motion_csv_dump_reader.h" +#include + +bool spirent_motion_csv_dump_reader::read_csv_obs() +{ + try + { + std::vector vec; + std::string line; + if (getline(d_dump_file, line)) + { + boost::tokenizer> tk( + line, boost::escaped_list_separator('\\', ',', '\"')); + for (boost::tokenizer>::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 &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(); + } +} diff --git a/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h new file mode 100644 index 000000000..c7fe736cf --- /dev/null +++ b/src/tests/system-tests/libs/spirent_motion_csv_dump_reader.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_spirent_motion_csv_dump_READER_H +#define GNSS_SDR_spirent_motion_csv_dump_READER_H + +#include +#include +#include +#include +#include + +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 &vec); +}; + +#endif //GNSS_SDR_spirent_motion_csv_dump_READER_H diff --git a/src/tests/system-tests/obs_gps_l1_system_test.cc b/src/tests/system-tests/obs_gps_l1_system_test.cc deleted file mode 100644 index 7af66e15d..000000000 --- a/src/tests/system-tests/obs_gps_l1_system_test.cc +++ /dev/null @@ -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 . - * - * ------------------------------------------------------------------------- - */ - -#include "concurrent_map.h" -#include "concurrent_queue.h" -#include "control_thread.h" -#include "in_memory_configuration.h" -#include "signal_generator_flags.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -// For GPS NAVIGATION (L1) -concurrent_queue global_gps_acq_assist_queue; -concurrent_map 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& vec); - - std::shared_ptr 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& 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(); - - 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 control_thread; - control_thread = std::make_shared(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>> pseudorange_ref(33); - std::vector>> carrierphase_ref(33); - std::vector>> doppler_ref(33); - - std::vector>> pseudorange_meas(33); - std::vector>> carrierphase_meas(33); - std::vector>> 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(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 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 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 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(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 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 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 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>> pseudorange_ref_aligned(33); - std::vector>> carrierphase_ref_aligned(33); - std::vector>> doppler_ref_aligned(33); - - std::vector>>::iterator iter; - std::vector>::iterator it; - std::vector>::iterator it2; - - std::vector> pr_diff(33); - std::vector> cp_diff(33); - std::vector> doppler_diff(33); - - std::vector>::iterator iter_diff; - std::vector::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 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 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 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::::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; -} diff --git a/src/tests/system-tests/obs_system_test.cc b/src/tests/system-tests/obs_system_test.cc deleted file mode 100644 index 7764d865d..000000000 --- a/src/tests/system-tests/obs_system_test.cc +++ /dev/null @@ -1,1051 +0,0 @@ -/*! - * \file obs_system_test.cc - * \brief This class implements a test for the validation of generated observables. - * \author Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es - * Antonio Ramos, 2017. antonio.ramos(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 . - * - * ------------------------------------------------------------------------- - */ - -#include "gnuplot_i.h" -#include "test_flags.h" -#include "concurrent_map.h" -#include "concurrent_queue.h" -#include "control_thread.h" -#include "file_configuration.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -// For GPS NAVIGATION (L1) -concurrent_queue global_gps_acq_assist_queue; -concurrent_map global_gps_acq_assist_map; - -DEFINE_string(configuration_file, "./default_configuration.conf", "Path of configuration file"); -DEFINE_string(filename_rinex_true, "./default_rinex.txt", "Path of RINEX true observations"); -DEFINE_string(filename_rinex_obs, "default_string", "Path of RINEX true observations"); -DEFINE_double(pr_error_mean_max, 25.0, "Maximum mean error in pseudorange"); -DEFINE_double(pr_error_std_max, 15.0, "Maximum standard deviation in pseudorange"); -DEFINE_double(cp_error_mean_max, 5.0, "Maximum mean error in carrier phase"); -DEFINE_double(cp_error_std_max, 2.5, "Maximum standard deviation in carrier phase"); -DEFINE_double(dp_error_mean_max, 75.0, "Maximum mean error in Doppler frequency"); -DEFINE_double(dp_error_std_max, 25.0, "Maximum standard deviation in Doppler frequency"); -DEFINE_bool(plot_obs_sys_test, false, "Plots results of ObsSystemTest with gnuplot"); - -class ObsSystemTest : public ::testing::Test -{ -public: - int configure_receiver(); - int run_receiver(); - void check_results(); - bool check_valid_rinex_obs(std::string filename, int rinex_ver); // return true if the file is a valid Rinex observation file. - void read_rinex_files( - std::vector& pseudorange_ref, - std::vector& carrierphase_ref, - std::vector& doppler_ref, - std::vector& pseudorange_meas, - std::vector& carrierphase_meas, - std::vector& doppler_meas, - arma::mat& sow_prn_ref, - int signal_type); - void time_alignment_diff( - std::vector& ref, - std::vector& meas, - std::vector& diff); - void time_alignment_diff_cp( - std::vector& ref, - std::vector& meas, - std::vector& diff); - void time_alignment_diff_pr( - std::vector& ref, - std::vector& meas, - std::vector& diff, - arma::mat& sow_prn_ref); - void compute_pseudorange_error(std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name); - void compute_carrierphase_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name); - void compute_doppler_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name); - std::string filename_rinex_obs = FLAGS_filename_rinex_true; - std::string generated_rinex_obs = FLAGS_filename_rinex_obs; - std::string configuration_file_ = FLAGS_configuration_file; - std::shared_ptr config; - bool gps_1C = false; - bool gps_L5 = false; - bool gal_1B = false; - bool gal_E5a = false; - bool internal_rinex_generation = false; - - /****************/ - const int num_prn_gps = 33; - const int num_prn_gal = 31; - - double pseudorange_error_th_mean = FLAGS_pr_error_mean_max; - double pseudorange_error_th_std = FLAGS_pr_error_std_max; - double carrierphase_error_th_mean = FLAGS_cp_error_mean_max; - double carrierphase_error_th_std = FLAGS_cp_error_std_max; - double doppler_error_th_mean = FLAGS_dp_error_mean_max; - double doppler_error_th_std = FLAGS_dp_error_std_max; -}; - - -bool ObsSystemTest::check_valid_rinex_obs(std::string filename, int rinex_ver) -{ - bool res = false; - if (rinex_ver == 2) - { - res = gpstk::isRinexObsFile(filename); - } - if (rinex_ver == 3) - { - res = gpstk::isRinex3ObsFile(filename); - } - return res; -} - - -void ObsSystemTest::read_rinex_files( - std::vector& pseudorange_ref, - std::vector& carrierphase_ref, - std::vector& doppler_ref, - std::vector& pseudorange_meas, - std::vector& carrierphase_meas, - std::vector& doppler_meas, - arma::mat& sow_prn_ref, - int signal_type) -{ - bool ref_exist = false; - bool meas_exist = false; - gpstk::SatID::SatelliteSystem sat_type = gpstk::SatID::systemUnknown; - int max_prn = 0; - std::string pr_string; - std::string cp_string; - std::string dp_string; - std::string signal_type_string; - sow_prn_ref.reset(); - - switch (signal_type) - { - case 0: //GPS L1 - - sat_type = gpstk::SatID::systemGPS; - max_prn = num_prn_gps; - pr_string = "C1C"; - cp_string = "L1C"; - dp_string = "D1C"; - signal_type_string = "GPS L1 C/A"; - break; - - case 1: //Galileo E1B - - sat_type = gpstk::SatID::systemGalileo; - max_prn = num_prn_gal; - pr_string = "C1B"; - cp_string = "L1B"; - dp_string = "D1B"; - signal_type_string = "Galileo E1B"; - break; - - case 2: //GPS L5 - - sat_type = gpstk::SatID::systemGPS; - max_prn = num_prn_gps; - pr_string = "C5X"; - cp_string = "L5X"; - dp_string = "D5X"; - signal_type_string = "GPS L5"; - break; - - case 3: //Galileo E5a - - sat_type = gpstk::SatID::systemGalileo; - max_prn = num_prn_gal; - pr_string = "C5X"; - cp_string = "L5X"; - dp_string = "D5X"; - signal_type_string = "Galileo E5a"; - break; - } - - // Open and read reference RINEX observables file - std::cout << "Read: RINEX " << signal_type_string << " True" << std::endl; - try - { - gpstk::Rinex3ObsStream r_ref(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 < max_prn; myprn++) - { - gpstk::SatID prn(myprn, sat_type); - gpstk::CommonTime time = r_ref_data.time; - double sow(static_cast(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, pr_string, r_ref_header); - double P1 = dataobj.data; - pseudorange_ref.at(myprn).insert_rows(pseudorange_ref.at(myprn).n_rows, arma::rowvec({sow, P1})); - - dataobj = r_ref_data.getObs(prn, cp_string, r_ref_header); - double L1 = dataobj.data; - carrierphase_ref.at(myprn).insert_rows(carrierphase_ref.at(myprn).n_rows, arma::rowvec({sow, L1})); - - dataobj = r_ref_data.getObs(prn, dp_string, r_ref_header); - double D1 = dataobj.data; - doppler_ref.at(myprn).insert_rows(doppler_ref.at(myprn).n_rows, arma::rowvec({sow, D1})); - - ref_exist = true; - } // 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); - } - - // Open and read measured RINEX observables file - std::cout << "Read: RINEX " << signal_type_string << " measures" << std::endl; - try - { - std::string arg2_gen; - if (internal_rinex_generation) - { - arg2_gen = std::string("./") + generated_rinex_obs; - } - else - { - arg2_gen = 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) - { - double pr_min = 0.0; - double sow_insert = 0.0; - double prn_min = 0.0; - bool set_pr_min = true; - for (int myprn = 1; myprn < max_prn; myprn++) - { - gpstk::SatID prn(myprn, sat_type); - gpstk::CommonTime time = r_meas_data.time; - double sow(static_cast(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, pr_string, r_meas_header); - double P1 = dataobj.data; - pseudorange_meas.at(myprn).insert_rows(pseudorange_meas.at(myprn).n_rows, arma::rowvec({sow, P1})); - if (set_pr_min || (P1 < pr_min)) - { - set_pr_min = false; - pr_min = P1; - sow_insert = sow; - prn_min = static_cast(myprn); - } - - dataobj = r_meas_data.getObs(prn, cp_string, r_meas_header); - double L1 = dataobj.data; - carrierphase_meas.at(myprn).insert_rows(carrierphase_meas.at(myprn).n_rows, arma::rowvec({sow, L1})); - - dataobj = r_meas_data.getObs(prn, dp_string, r_meas_header); - double D1 = dataobj.data; - doppler_meas.at(myprn).insert_rows(doppler_meas.at(myprn).n_rows, arma::rowvec({sow, D1})); - - meas_exist = true; - } // End of 'if( pointer == roe.obs.end() )' - } // end for - if (!set_pr_min) - { - sow_prn_ref.insert_rows(sow_prn_ref.n_rows, arma::rowvec({sow_insert, pr_min, prn_min})); - } - } // 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); - } - EXPECT_TRUE(ref_exist) << "RINEX reference file does not contain " << signal_type_string << " information"; - EXPECT_TRUE(meas_exist) << "RINEX generated file does not contain " << signal_type_string << " information"; -} - - -void ObsSystemTest::time_alignment_diff( - std::vector& ref, - std::vector& meas, - std::vector& diff) -{ - std::vector::iterator iter_ref; - std::vector::iterator iter_meas; - std::vector::iterator iter_diff; - arma::mat mat_aux; - - iter_ref = ref.begin(); - iter_diff = diff.begin(); - for (iter_meas = meas.begin(); iter_meas != meas.end(); iter_meas++) - { - if (!iter_meas->is_empty() && !iter_ref->is_empty()) - { - arma::uvec index_ = arma::find(iter_meas->col(0) > iter_ref->at(0, 0)); - arma::uword index_min = arma::min(index_); - index_ = arma::find(iter_meas->col(0) < iter_ref->at(iter_ref->n_rows - 1, 0)); - arma::uword index_max = arma::max(index_); - mat_aux = iter_meas->rows(index_min, index_max); - arma::vec ref_aligned; - arma::interp1(iter_ref->col(0), iter_ref->col(1), mat_aux.col(0), ref_aligned); - *iter_diff = ref_aligned - mat_aux.col(1); - } - iter_ref++; - iter_diff++; - } -} - - -void ObsSystemTest::time_alignment_diff_cp( - std::vector& ref, - std::vector& meas, - std::vector& diff) -{ - std::vector::iterator iter_ref; - std::vector::iterator iter_meas; - std::vector::iterator iter_diff; - arma::mat mat_aux; - - iter_ref = ref.begin(); - iter_diff = diff.begin(); - for (iter_meas = meas.begin(); iter_meas != meas.end(); iter_meas++) - { - if (!iter_meas->is_empty() && !iter_ref->is_empty()) - { - arma::uvec index_ = arma::find(iter_meas->col(0) > iter_ref->at(0, 0)); - arma::uword index_min = arma::min(index_); - index_ = arma::find(iter_meas->col(0) < iter_ref->at(iter_ref->n_rows - 1, 0)); - arma::uword index_max = arma::max(index_); - mat_aux = iter_meas->rows(index_min, index_max); - mat_aux.col(1) -= mat_aux.col(1)(0); - arma::vec ref_aligned; - arma::interp1(iter_ref->col(0), iter_ref->col(1), mat_aux.col(0), ref_aligned); - ref_aligned -= ref_aligned(0); - *iter_diff = ref_aligned - mat_aux.col(1); - } - iter_ref++; - iter_diff++; - } -} - - -void ObsSystemTest::time_alignment_diff_pr( - std::vector& ref, - std::vector& meas, - std::vector& diff, - arma::mat& sow_prn_ref) -{ - std::vector::iterator iter_ref; - std::vector::iterator iter_meas; - std::vector::iterator iter_diff; - arma::mat mat_aux; - arma::vec subtraction_meas; - arma::vec subtraction_ref; - - arma::mat subtraction_pr_ref = sow_prn_ref; - arma::vec::iterator iter_vec0 = subtraction_pr_ref.begin_col(0); - arma::vec::iterator iter_vec1 = subtraction_pr_ref.begin_col(1); - arma::vec::iterator iter_vec2 = subtraction_pr_ref.begin_col(2); - - for (iter_vec1 = subtraction_pr_ref.begin_col(1); iter_vec1 != subtraction_pr_ref.end_col(1); iter_vec1++) - { - arma::vec aux_pr; //vector with only 1 element - arma::vec aux_sow = {*iter_vec0}; //vector with only 1 element - arma::interp1(ref.at(static_cast(*iter_vec2)).col(0), - ref.at(static_cast(*iter_vec2)).col(1), - aux_sow, - aux_pr); - *iter_vec1 = aux_pr(0); - iter_vec0++; - iter_vec2++; - } - - iter_ref = ref.begin(); - iter_diff = diff.begin(); - for (iter_meas = meas.begin(); iter_meas != meas.end(); iter_meas++) - { - if (!iter_meas->is_empty() && !iter_ref->is_empty()) - { - arma::uvec index_ = arma::find(iter_meas->col(0) > iter_ref->at(0, 0)); - arma::uword index_min = arma::min(index_); - index_ = arma::find(iter_meas->col(0) < iter_ref->at(iter_ref->n_rows - 1, 0)); - arma::uword index_max = arma::max(index_); - mat_aux = iter_meas->rows(index_min, index_max); - arma::interp1(sow_prn_ref.col(0), sow_prn_ref.col(1), mat_aux.col(0), subtraction_meas); - mat_aux.col(1) -= subtraction_meas; - arma::vec ref_aligned; - arma::interp1(iter_ref->col(0), iter_ref->col(1), mat_aux.col(0), ref_aligned); - arma::interp1(subtraction_pr_ref.col(0), subtraction_pr_ref.col(1), mat_aux.col(0), subtraction_ref); - ref_aligned -= subtraction_ref; - *iter_diff = ref_aligned - mat_aux.col(1); - } - iter_ref++; - iter_diff++; - } -} - - -int ObsSystemTest::configure_receiver() -{ - config = std::make_shared(configuration_file_); - - if (config->property("Channels_1C.count", 0) > 0) - { - gps_1C = true; - } - if (config->property("Channels_1B.count", 0) > 0) - { - gal_1B = true; - } - if (config->property("Channels_5X.count", 0) > 0) - { - gal_E5a = true; - } - if (config->property("Channels_7X.count", 0) > 0) //NOT DEFINITIVE!!!!! - { - gps_L5 = true; - } - - return 0; -} - - -int ObsSystemTest::run_receiver() -{ - std::shared_ptr control_thread; - control_thread = std::make_shared(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); - generated_rinex_obs = aux.erase(aux.length() - 1, 1); - internal_rinex_generation = true; - } - pclose(fp); - return 0; -} - - -void ObsSystemTest::compute_pseudorange_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name) -{ - int prn_id = 0; - std::vector::iterator iter_diff; - std::vector means; - std::vector stddevs; - std::vector prns; - for (iter_diff = diff.begin(); iter_diff != diff.end(); iter_diff++) - { - if (!iter_diff->is_empty()) - { - while (iter_diff->has_nan()) - { - bool nan_found = false; - int k_aux = 0; - while (!nan_found) - { - if (!iter_diff->row(k_aux).is_finite()) - { - nan_found = true; - iter_diff->shed_row(k_aux); - } - k_aux++; - } - } - double d_mean = std::sqrt(arma::mean(arma::square(*iter_diff))); - means.push_back(d_mean); - double d_stddev = arma::stddev(*iter_diff); - stddevs.push_back(d_stddev); - prns.push_back(static_cast(prn_id)); - std::cout << "-- RMS pseudorange difference for sat " << prn_id << ": " << d_mean; - std::cout << ". Std. dev.: " << d_stddev; - std::cout << " [m]" << std::endl; - EXPECT_LT(d_mean, error_th_mean); - EXPECT_LT(d_stddev, error_th_std); - } - prn_id++; - } - if (FLAGS_plot_obs_sys_test == true) - { - const std::string gnuplot_executable(FLAGS_gnuplot_executable); - if (gnuplot_executable.empty()) - { - std::cout << "WARNING: Although the flag plot_obs_sys_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); - - Gnuplot g1("linespoints"); - if (FLAGS_show_plots) - { - g1.showonscreen(); // window output - } - else - { - g1.disablescreen(); - } - g1.set_title(signal_name + " Pseudorange error"); - g1.set_grid(); - g1.set_xlabel("PRN"); - g1.set_ylabel("Pseudorange error [m]"); - g1.plot_xy(prns, means, "RMS error"); - g1.plot_xy(prns, stddevs, "Standard deviation"); - size_t char_pos = signal_name.find(" "); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find(" "); - } - char_pos = signal_name.find("/"); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find("/"); - } - g1.savetops("Pseudorange_error_" + signal_name); - g1.savetopdf("Pseudorange_error_" + signal_name, 18); - } - catch (const GnuplotException& ge) - { - std::cout << ge.what() << std::endl; - } - } - } -} - - -void ObsSystemTest::compute_carrierphase_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name) -{ - int prn_id = 0; - std::vector means; - std::vector stddevs; - std::vector prns; - std::vector::iterator iter_diff; - for (iter_diff = diff.begin(); iter_diff != diff.end(); iter_diff++) - { - if (!iter_diff->is_empty()) - { - while (iter_diff->has_nan()) - { - bool nan_found = false; - int k_aux = 0; - while (!nan_found) - { - if (!iter_diff->row(k_aux).is_finite()) - { - nan_found = true; - iter_diff->shed_row(k_aux); - } - k_aux++; - } - } - double d_mean = std::sqrt(arma::mean(arma::square(*iter_diff))); - means.push_back(d_mean); - double d_stddev = arma::stddev(*iter_diff); - stddevs.push_back(d_stddev); - prns.push_back(static_cast(prn_id)); - std::cout << "-- RMS carrier phase difference for sat " << prn_id << ": " << d_mean; - std::cout << ". Std. dev.: " << d_stddev; - std::cout << " whole cycles" << std::endl; - EXPECT_LT(d_mean, error_th_mean); - EXPECT_LT(d_stddev, error_th_std); - } - prn_id++; - } - if (FLAGS_plot_obs_sys_test == true) - { - const std::string gnuplot_executable(FLAGS_gnuplot_executable); - if (gnuplot_executable.empty()) - { - std::cout << "WARNING: Although the flag plot_obs_sys_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); - - Gnuplot g1("linespoints"); - if (FLAGS_show_plots) - { - g1.showonscreen(); // window output - } - else - { - g1.disablescreen(); - } - g1.set_title(signal_name + " Carrier phase error"); - g1.set_grid(); - g1.set_xlabel("PRN"); - g1.set_ylabel("Carrier phase error [whole cycles]"); - g1.plot_xy(prns, means, "RMS error"); - g1.plot_xy(prns, stddevs, "Standard deviation"); - size_t char_pos = signal_name.find(" "); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find(" "); - } - char_pos = signal_name.find("/"); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find("/"); - } - g1.savetops("Carrier_phase_error_" + signal_name); - g1.savetopdf("Carrier_phase_error_" + signal_name, 18); - } - catch (const GnuplotException& ge) - { - std::cout << ge.what() << std::endl; - } - } - } -} - - -void ObsSystemTest::compute_doppler_error( - std::vector& diff, - double error_th_mean, double error_th_std, - std::string signal_name) -{ - int prn_id = 0; - std::vector means; - std::vector stddevs; - std::vector prns; - std::vector::iterator iter_diff; - for (iter_diff = diff.begin(); iter_diff != diff.end(); iter_diff++) - { - if (!iter_diff->is_empty()) - { - while (iter_diff->has_nan()) - { - bool nan_found = false; - int k_aux = 0; - while (!nan_found) - { - if (!iter_diff->row(k_aux).is_finite()) - { - nan_found = true; - iter_diff->shed_row(k_aux); - } - k_aux++; - } - } - double d_mean = std::sqrt(arma::mean(arma::square(*iter_diff))); - means.push_back(d_mean); - double d_stddev = arma::stddev(*iter_diff); - stddevs.push_back(d_stddev); - prns.push_back(static_cast(prn_id)); - std::cout << "-- RMS Doppler difference for sat " << prn_id << ": " << d_mean; - std::cout << ". Std. dev.: " << d_stddev; - std::cout << " [Hz]" << std::endl; - EXPECT_LT(d_mean, error_th_mean); - EXPECT_LT(d_stddev, error_th_std); - } - prn_id++; - } - if (FLAGS_plot_obs_sys_test == true) - { - const std::string gnuplot_executable(FLAGS_gnuplot_executable); - if (gnuplot_executable.empty()) - { - std::cout << "WARNING: Although the flag plot_obs_sys_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); - - Gnuplot g1("linespoints"); - if (FLAGS_show_plots) - { - g1.showonscreen(); // window output - } - else - { - g1.disablescreen(); - } - g1.set_title(signal_name + " Doppler error"); - g1.set_grid(); - g1.set_xlabel("PRN"); - g1.set_ylabel("Doppler error [Hz]"); - g1.plot_xy(prns, means, "RMS error"); - g1.plot_xy(prns, stddevs, "Standard deviation"); - size_t char_pos = signal_name.find(" "); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find(" "); - } - char_pos = signal_name.find("/"); - while (char_pos != std::string::npos) - { - signal_name.replace(char_pos, 1, "_"); - char_pos = signal_name.find("/"); - } - g1.savetops("Doppler_error_" + signal_name); - g1.savetopdf("Doppler_error_" + signal_name, 18); - } - catch (const GnuplotException& ge) - { - std::cout << ge.what() << std::endl; - } - } - } -} - - -void ObsSystemTest::check_results() -{ - arma::mat sow_prn_ref; - if (gps_1C) - { - std::vector pseudorange_ref(num_prn_gps); - std::vector carrierphase_ref(num_prn_gps); - std::vector doppler_ref(num_prn_gps); - - std::vector pseudorange_meas(num_prn_gps); - std::vector carrierphase_meas(num_prn_gps); - std::vector doppler_meas(num_prn_gps); - - read_rinex_files(pseudorange_ref, carrierphase_ref, doppler_ref, pseudorange_meas, carrierphase_meas, doppler_meas, sow_prn_ref, 0); - - // Time alignment and difference computation - - std::vector pr_diff(num_prn_gps); - std::vector cp_diff(num_prn_gps); - std::vector dp_diff(num_prn_gps); - time_alignment_diff_pr(pseudorange_ref, pseudorange_meas, pr_diff, sow_prn_ref); - time_alignment_diff_cp(carrierphase_ref, carrierphase_meas, cp_diff); - time_alignment_diff(doppler_ref, doppler_meas, dp_diff); - - // Results - std::cout << std::endl; - std::cout << std::endl; - std::cout << "GPS L1 C/A obs. results" << std::endl; - - // Compute pseudorange error - - compute_pseudorange_error(pr_diff, pseudorange_error_th_mean, pseudorange_error_th_std, "GPS L1 C/A"); - - // Compute carrier phase error - - compute_carrierphase_error(cp_diff, carrierphase_error_th_mean, carrierphase_error_th_std, "GPS L1 C/A"); - - // Compute Doppler error - - compute_doppler_error(dp_diff, doppler_error_th_mean, doppler_error_th_std, "GPS L1 C/A"); - } - if (gps_L5) - { - std::vector pseudorange_ref(num_prn_gps); - std::vector carrierphase_ref(num_prn_gps); - std::vector doppler_ref(num_prn_gps); - - std::vector pseudorange_meas(num_prn_gps); - std::vector carrierphase_meas(num_prn_gps); - std::vector doppler_meas(num_prn_gps); - - read_rinex_files(pseudorange_ref, carrierphase_ref, doppler_ref, pseudorange_meas, carrierphase_meas, doppler_meas, sow_prn_ref, 2); - - // Time alignment and difference computation - - std::vector pr_diff(num_prn_gps); - std::vector cp_diff(num_prn_gps); - std::vector dp_diff(num_prn_gps); - time_alignment_diff_pr(pseudorange_ref, pseudorange_meas, pr_diff, sow_prn_ref); - time_alignment_diff_cp(carrierphase_ref, carrierphase_meas, cp_diff); - time_alignment_diff(doppler_ref, doppler_meas, dp_diff); - - // Results - std::cout << std::endl; - std::cout << std::endl; - std::cout << "GPS L5 obs. results" << std::endl; - - // Compute pseudorange error - - compute_pseudorange_error(pr_diff, pseudorange_error_th_mean, pseudorange_error_th_std, "GPS L5"); - - // Compute carrier phase error - - compute_carrierphase_error(cp_diff, carrierphase_error_th_mean, carrierphase_error_th_std, "GPS L5"); - - // Compute Doppler error - - compute_doppler_error(dp_diff, doppler_error_th_mean, doppler_error_th_std, "GPS L5"); - } - if (gal_1B) - { - std::vector pseudorange_ref(num_prn_gal); - std::vector carrierphase_ref(num_prn_gal); - std::vector doppler_ref(num_prn_gal); - - std::vector pseudorange_meas(num_prn_gal); - std::vector carrierphase_meas(num_prn_gal); - std::vector doppler_meas(num_prn_gal); - - read_rinex_files(pseudorange_ref, carrierphase_ref, doppler_ref, pseudorange_meas, carrierphase_meas, doppler_meas, sow_prn_ref, 1); - - // Time alignment and difference computation - - std::vector pr_diff(num_prn_gal); - std::vector cp_diff(num_prn_gal); - std::vector dp_diff(num_prn_gal); - time_alignment_diff_pr(pseudorange_ref, pseudorange_meas, pr_diff, sow_prn_ref); - time_alignment_diff_cp(carrierphase_ref, carrierphase_meas, cp_diff); - time_alignment_diff(doppler_ref, doppler_meas, dp_diff); - - // Results - std::cout << std::endl; - std::cout << std::endl; - std::cout << "Galileo E1B obs. results" << std::endl; - - // Compute pseudorange error - - compute_pseudorange_error(pr_diff, pseudorange_error_th_mean, pseudorange_error_th_std, "Galileo E1B"); - - // Compute carrier phase error - - compute_carrierphase_error(cp_diff, carrierphase_error_th_mean, carrierphase_error_th_std, "Galileo E1B"); - - // Compute Doppler error - - compute_doppler_error(dp_diff, doppler_error_th_mean, doppler_error_th_std, "Galileo E1B"); - } - if (gal_E5a) - { - std::vector pseudorange_ref(num_prn_gal); - std::vector carrierphase_ref(num_prn_gal); - std::vector doppler_ref(num_prn_gal); - - std::vector pseudorange_meas(num_prn_gal); - std::vector carrierphase_meas(num_prn_gal); - std::vector doppler_meas(num_prn_gal); - - read_rinex_files(pseudorange_ref, carrierphase_ref, doppler_ref, pseudorange_meas, carrierphase_meas, doppler_meas, sow_prn_ref, 3); - - // Time alignment and difference computation - - std::vector pr_diff(num_prn_gal); - std::vector cp_diff(num_prn_gal); - std::vector dp_diff(num_prn_gal); - time_alignment_diff_pr(pseudorange_ref, pseudorange_meas, pr_diff, sow_prn_ref); - time_alignment_diff_cp(carrierphase_ref, carrierphase_meas, cp_diff); - time_alignment_diff(doppler_ref, doppler_meas, dp_diff); - - // Results - std::cout << std::endl; - std::cout << std::endl; - std::cout << "Galileo E5a obs. results" << std::endl; - - // Compute pseudorange error - - compute_pseudorange_error(pr_diff, pseudorange_error_th_mean, pseudorange_error_th_std, "Galileo E5a"); - - // Compute carrier phase error - - compute_carrierphase_error(cp_diff, carrierphase_error_th_mean, carrierphase_error_th_std, "Galileo E5a"); - - // Compute Doppler error - - compute_doppler_error(dp_diff, doppler_error_th_mean, doppler_error_th_std, "Galileo E5a"); - } -} - - -TEST_F(ObsSystemTest, Observables_system_test) -{ - std::cout << "Validating input RINEX obs (TRUE) file: " << filename_rinex_obs << " ..." << std::endl; - bool is_rinex_obs_valid = check_valid_rinex_obs(filename_rinex_obs, 3); - ASSERT_EQ(true, is_rinex_obs_valid) << "The RINEX observation file " << filename_rinex_obs << " is not well formed. Only RINEX v. 3.00 files are allowed"; - std::cout << "The file is valid." << std::endl; - // Configure receiver - configure_receiver(); - if (generated_rinex_obs.compare("default_string") == 0) - { - // Run the receiver - ASSERT_EQ(run_receiver(), 0) << "Problem executing the software-defined signal generator"; - } - std::cout << "Validating RINEX obs file obtained by GNSS-SDR: " << generated_rinex_obs << " ..." << std::endl; - bool is_gen_rinex_obs_valid = false; - if (internal_rinex_generation) - { - is_gen_rinex_obs_valid = check_valid_rinex_obs("./" + generated_rinex_obs, config->property("PVT.rinex_version", 3)); - } - else - { - is_gen_rinex_obs_valid = check_valid_rinex_obs(generated_rinex_obs, config->property("PVT.rinex_version", 3)); - } - ASSERT_EQ(true, is_gen_rinex_obs_valid) << "The RINEX observation file " << 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 GNSS-SDR in Space Observables validation test..." << std::endl; - int res = 0; - try - { - testing::InitGoogleTest(&argc, argv); - } - catch (...) - { - } // catch the "testing::internal::::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; -} diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index 63bc18016..21e8bfd81 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -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
          + *
        • Carles Fernandez-Prades, 2016. cfernandez(at)cttc.es + *
        • Javier Arribas, 2018. jarribas(at)cttc.es + *
        * * * ------------------------------------------------------------------------- @@ -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 +#include #include #include #include @@ -48,10 +55,6 @@ #include #include - -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 global_gps_acq_assist_queue; concurrent_map 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 pos_e; std::vector pos_n; std::vector 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(""); - 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(""); + if (found != std::string::npos) is_header = false; } - std::size_t found = line.find(""); - 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(""); + 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(); diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index 6df8889be..64920b9a1 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -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" diff --git a/src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc b/src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc index 3099d950e..f61727542 100644 --- a/src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc @@ -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."); diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc index e3f915b90..961757087 100644 --- a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -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(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(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(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(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(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(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(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(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* 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 { diff --git a/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/galileo_fnav_inav_decoder_test.cc b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/galileo_fnav_inav_decoder_test.cc new file mode 100644 index 000000000..070a76e42 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/telemetry_decoder/galileo_fnav_inav_decoder_test.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include "galileo_navigation_message.h" +#include "galileo_fnav_message.h" +#include "convolutional.h" +#include +#include +#include +#include +#include +#include +#include + + +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(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(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(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(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(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); + out1 = static_cast(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); + state0 = static_cast(volk_gnsssdr_malloc(max_states * sizeof(int32_t), volk_gnsssdr_get_alignment())); + state1 = static_cast(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 start, end; + std::chrono::duration 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; +} diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc new file mode 100644 index 000000000..f32e82f16 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/bayesian_estimation_test.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#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 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(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); + } + } +} diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc index 1be7bbe49..3b0c24883 100644 --- a/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/cpu_multicorrelator_real_codes_test.cc @@ -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])); } diff --git a/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc new file mode 100644 index 000000000..65e820921 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/tracking/gps_l1_ca_kf_tracking_test.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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_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(); + config = std::make_shared(); + item_size = sizeof(gr_complex); + gnss_synchro = Gnss_Synchro(); + } + + ~GpsL1CAKfTrackingTest() + { + } + + void configure_receiver(); + + gr::top_block_sptr top_block; + std::shared_ptr factory; + std::shared_ptr 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 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 trk_ = factory->GetBlock(config, "Tracking_1C", implementation, 1, 1); + std::shared_ptr tracking = std::dynamic_pointer_cast(trk_); //std::make_shared(config.get(), "Tracking_1C", 1, 1); + + boost::shared_ptr 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 prompt; + std::vector early; + std::vector late; + std::vector promptI; + std::vector promptQ; + + epoch_counter = 0; + while (trk_dump.read_binary_obs()) + { + trk_timestamp_s(epoch_counter) = static_cast(trk_dump.PRN_start_sample_count) / static_cast(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(trk_dump.PRN_start_sample_count) + trk_dump.aux1) / static_cast(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 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 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(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; + } + } + } +} diff --git a/src/utils/matlab/gps_l1_ca_kf_plot_sample.m b/src/utils/matlab/gps_l1_ca_kf_plot_sample.m new file mode 100644 index 000000000..f8fcb0ed7 --- /dev/null +++ b/src/utils/matlab/gps_l1_ca_kf_plot_sample.m @@ -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 . +% +% ------------------------------------------------------------------------- +% + +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 + + diff --git a/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m b/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m new file mode 100644 index 000000000..e1f1c8687 --- /dev/null +++ b/src/utils/matlab/libs/gps_l1_ca_kf_read_tracking_dump.m @@ -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 . +% +% ------------------------------------------------------------------------- +% + +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 \ No newline at end of file diff --git a/src/utils/matlab/libs/plotKalman.m b/src/utils/matlab/libs/plotKalman.m new file mode 100644 index 000000000..e05fd0d52 --- /dev/null +++ b/src/utils/matlab/libs/plotKalman.m @@ -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