diff --git a/CMakeLists.txt b/CMakeLists.txt index a501d2d0a..bc2dfbc50 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,8 @@ option(ENABLE_UHD "Enable the use of UHD (driver for all USRP devices)" ON) option(ENABLE_OSMOSDR "Enable the use of OsmoSDR and other front-ends (RTL-based dongles, HackRF, bladeRF, etc.) as signal source" OFF) +option(ENABLE_LIMESDR "Enable the use of LimeSDR and Custom LimeSDR as signal source" OFF) + option(ENABLE_FMCOMMS2 "Enable the use of FMCOMMS4-EBZ + ZedBoard hardware, requires gr-iio" OFF) option(ENABLE_PLUTOSDR "Enable the use of ADALM-PLUTO Evaluation Boards (Analog Devices Inc.), requires gr-iio" OFF) @@ -2979,6 +2981,33 @@ else() endif() +########################################## +# gr-limesdr - OPTIONAL +# https://github.com/myriadrf/gr-limesdr +########################################## +find_package(GRLIMESDR) +set_package_properties(GRLIMESDR PROPERTIES + PURPOSE "Used for communication with LimeSDR." + TYPE OPTIONAL +) +if(ENABLE_LIMESDR) + if(GRLIMESDR_FOUND) + message(STATUS "The driver for LimeSDR will be compiled.") + message(STATUS " You can disable it with 'cmake -DENABLE_LIMESDR=OFF ..'") + else() + if(ENABLE_PACKAGING) + message(WARNING "gr-limesdr has not been found. Source blocks depending on it will NOT be built.") + else() + message(FATAL_ERROR "gr-limesdr required to build gnss-sdr with the optional LIMESDR driver") + endif() + set(ENABLE_LIMESDR OFF) + endif() +else() + message(STATUS "The (optional) driver for LimeSDR is not enabled.") + message(STATUS " Enable it with 'cmake -DENABLE_LIMESDR=ON ..' to add support for LimeSDR.") +endif() + + ############################################## # gr-iio - OPTIONAL # IIO blocks for GNU Radio @@ -3281,6 +3310,7 @@ add_subdirectory(src) ################################################################################ add_feature_info(ENABLE_UHD ENABLE_UHD "Enables UHD_Signal_Source for using RF front-ends from the USRP family. Requires gr-uhd.") add_feature_info(ENABLE_OSMOSDR ENABLE_OSMOSDR "Enables Osmosdr_Signal_Source and RtlTcp_Signal_Source for using RF front-ends compatible with the OsmoSDR driver. Requires gr-osmosdr.") +add_feature_info(ENABLE_LIMESDR ENABLE_LIMESDR "Enables Limesdr_Signal_Source. Requires gr-limesdr.") add_feature_info(ENABLE_FMCOMMS2 ENABLE_FMCOMMS2 "Enables Fmcomms2_Signal_Source for FMCOMMS2/3/4 devices. Requires gr-iio and libad9361-dev.") add_feature_info(ENABLE_PLUTOSDR ENABLE_PLUTOSDR "Enables Plutosdr_Signal_Source for using ADALM-PLUTO boards. Requires gr-iio.") add_feature_info(ENABLE_AD9361 ENABLE_AD9361 "Enables Ad9361_Fpga_Signal_Source for devices with the AD9361 chipset. Requires libiio and libad9361-dev.") diff --git a/cmake/Modules/FindGRLIMESDR.cmake b/cmake/Modules/FindGRLIMESDR.cmake new file mode 100644 index 000000000..cfd546feb --- /dev/null +++ b/cmake/Modules/FindGRLIMESDR.cmake @@ -0,0 +1,164 @@ +# GNSS-SDR is a Global Navigation Satellite System software-defined receiver. +# This file is part of GNSS-SDR. +# +# SPDX-FileCopyrightText: 2011-2020 C. Fernandez-Prades cfernandez(at)cttc.es +# SPDX-License-Identifier: BSD-3-Clause + +# Tries to find gr-limesdr. +# +# Usage of this module as follows: +# +# find_package(GRLIMESDR) +# +# Variables used by this module, they can change the default behaviour and need +# to be set before calling find_package: +# +# GrLimeSDR_ROOT_DIR Set this variable to the root installation of +# gr-limesdr if the module has problems finding +# the proper installation path. +# +# Variables defined by this module: +# +# GRLIMESDR_FOUND System has gr-limesdr libs/headers +# GRLIMESDR_LIBRARIES The gr-limesdr libraries (gnuradio-limesdr) +# GRLIMESDR_INCLUDE_DIR The location of gr-limesdr headers +# +# Provides the following imported target: +# Gnuradio::limesdr +# + +if(NOT COMMAND feature_summary) + include(FeatureSummary) +endif() + +if(NOT PKG_CONFIG_FOUND) + include(FindPkgConfig) +endif() + +pkg_check_modules(GRLIMESDR_PKG QUIET gnuradio-limesdr) + +if(NOT GRLIMESDR_ROOT) + set(GRLIMESDR_ROOT_USER_DEFINED /usr) +else() + set(GRLIMESDR_ROOT_USER_DEFINED ${GRLIMESDR_ROOT}) +endif() +if(DEFINED ENV{GRLIMESDR_ROOT}) + set(GRLIMESDR_ROOT_USER_DEFINED + ${GRLIMESDR_ROOT_USER_DEFINED} + $ENV{GRLIMESDR_ROOT} + ) +endif() + +find_path(GRLIMESDR_INCLUDE_DIR + NAMES + limesdr/source.h + limesdr/api.h + HINTS + ${GRLIMESDR_PKG_INCLUDEDIR} + PATHS + ${GRLIMESDR_ROOT_USER_DEFINED}/include + /usr/include + /usr/local/include + /opt/local/include +) + +find_library(GRLIMESDR_LIBRARIES + NAMES + gnuradio-limesdr + HINTS + ${GRLIMESDR_PKG_LIBDIR} + PATHS + ${GRLIMESDR_ROOT_USER_DEFINED}/lib + ${GRLIMESDR_ROOT_USER_DEFINED}/lib64 + /usr/lib + /usr/lib64 + /usr/lib/x86_64-linux-gnu + /usr/lib/i386-linux-gnu + /usr/lib/arm-linux-gnueabihf + /usr/lib/arm-linux-gnueabi + /usr/lib/aarch64-linux-gnu + /usr/lib/mipsel-linux-gnu + /usr/lib/mips-linux-gnu + /usr/lib/mips64el-linux-gnuabi64 + /usr/lib/powerpc-linux-gnu + /usr/lib/powerpc64-linux-gnu + /usr/lib/powerpc64le-linux-gnu + /usr/lib/powerpc-linux-gnuspe + /usr/lib/hppa-linux-gnu + /usr/lib/s390x-linux-gnu + /usr/lib/i386-gnu + /usr/lib/hppa-linux-gnu + /usr/lib/x86_64-kfreebsd-gnu + /usr/lib/i386-kfreebsd-gnu + /usr/lib/m68k-linux-gnu + /usr/lib/sh4-linux-gnu + /usr/lib/sparc64-linux-gnu + /usr/lib/x86_64-linux-gnux32 + /usr/lib/riscv64-linux-gnu + /usr/lib/alpha-linux-gnu + /usr/local/lib + /usr/local/lib64 + /opt/local/lib +) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(GRLIMESDR DEFAULT_MSG GRLIMESDR_LIBRARIES GRLIMESDR_INCLUDE_DIR) + +if(GRLIMESDR_PKG_VERSION) + set(GRLIMESDR_VERSION_AUX ${GRLIMESDR_PKG_VERSION}) + string(REGEX REPLACE "^v" "" GRLIMESDR_VERSION ${GRLIMESDR_VERSION_AUX}) +endif() + +set_package_properties(GRLIMESDR PROPERTIES + URL "https://github.com/myriadrf/gr-limesdr" +) + +if(GRLIMESDR_FOUND AND GRLIMESDR_VERSION) + set_package_properties(GRLIMESDR PROPERTIES + DESCRIPTION "LimeSDR GNU Radio blocks (found: v${GRLIMESDR_VERSION})" + ) +else() + set_package_properties(GRLIMESDR PROPERTIES + DESCRIPTION "LimeSDR GNU Radio blocks" + ) +endif() + +if(GRLIMESDR_FOUND AND NOT TARGET Gnuradio::limesdr) + add_library(Gnuradio::limesdr SHARED IMPORTED) + set_target_properties(Gnuradio::limesdr PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" + IMPORTED_LOCATION "${GRLIMESDR_LIBRARIES}" + INTERFACE_INCLUDE_DIRECTORIES "${GRLIMESDR_INCLUDE_DIR};${GRLIMESDR_INCLUDE_DIR}/limesdr" + INTERFACE_LINK_LIBRARIES "${GRLIMESDR_LIBRARIES}" + ) + + message(STATUS "The (optional) gr-limesdr module has been found.") + + # check for PPS custom version + file(READ ${GRLIMESDR_INCLUDE_DIR}/limesdr/source.h TMPTXT) + string(FIND "${TMPTXT}" "enable_PPS_mode" matchres) + if(${matchres} EQUAL -1) + message(STATUS " Using standard gr-limesdr library.") + else() + set(GRLIMESDR_PPS TRUE) + message(STATUS " Using custom gr-limesdr library with PPS support.") + endif() + + # check gr-limesdr branch + set(_g38_branch TRUE) + file(STRINGS ${GRLIMESDR_INCLUDE_DIR}/limesdr/source.h _limesdr_header_content) + foreach(_loop_var IN LISTS _limesdr_header_content) + string(STRIP "${_loop_var}" _file_line) + if("static sptr make(std::string serial, int channel_mode, const std::string& filename);" STREQUAL "${_file_line}") + set(_g38_branch FALSE) + endif() + if("make(std::string serial, int channel_mode, const std::string& filename, bool enable_PPS_mode);" STREQUAL "${_file_line}") + set(_g38_branch FALSE) + endif() + endforeach() + if(${_g38_branch}) + set(GR_LIMESDR_IS_G38_BRANCH TRUE) + endif() +endif() + +mark_as_advanced(GRLIMESDR_LIBRARIES GRLIMESDR_INCLUDE_DIR) diff --git a/conf/gnss-sdr_GPS_L1_LimeSDR.conf b/conf/gnss-sdr_GPS_L1_LimeSDR.conf index f99fefc28..98df86cf8 100644 --- a/conf/gnss-sdr_GPS_L1_LimeSDR.conf +++ b/conf/gnss-sdr_GPS_L1_LimeSDR.conf @@ -1,111 +1,132 @@ ; This is a GNSS-SDR configuration file ; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/ ; SPDX-License-Identifier: GPL-3.0-or-later -; SPDX-FileCopyrightText: (C) 2010-2020 (see AUTHORS file for a list of contributors) +; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors) [GNSS-SDR] ;######### GLOBAL OPTIONS ################## -GNSS-SDR.internal_fs_sps=2000000 +;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second]. +;FOR USE GNSS-SDR WITH RTLSDR DONGLES USER MUST SET THE CALIBRATED SAMPLE RATE HERE +; i.e. using front-end-cal as reported here:http://www.cttc.es/publication/turning-a-television-into-a-gnss-receiver/ +GNSS-SDR.internal_fs_sps=5000000 +GNSS-SDR.use_acquisition_resampler=true ;######### SIGNAL_SOURCE CONFIG ############ -SignalSource.implementation=Osmosdr_Signal_Source +SignalSource.implementation=Limesdr_Signal_Source SignalSource.item_type=gr_complex -SignalSource.sampling_frequency=2000000 -;# LimeSDR RX1 antennas: NONE,LNAH,LNAL,LNAW -SignalSource.antenna=LNAW +SignalSource.sampling_frequency=5000000 SignalSource.freq=1575420000 -SignalSource.gain=40 -SignalSource.rf_gain=40 -SignalSource.if_gain=30 -SignalSource.AGC_enabled=false +SignalSource.gain=50; //0-73 dB//no agc in LimeSDR +;SignalSource.analog_bw //if not set, defaults to sample_rate/2 +;SignalSource.digital_bw // if not set, defaults to 0 (disabled filter) +SignalSource.limesdr_serial // if not set, its automatic +SignalSource.antenna=2 // None(0), LNAH(1), LNAL(2), LNAW(3), AUTO(255) +SignalSource.ext_clock_MHz_=0 //0 -> internal clock +SignalSource.PPS_mode=false; //requires special gr-limesdr +SignalSource.limechannel_mode = 0; //ChannelMode must be A(0), B(1) or (A+B) MIMO(2)" SignalSource.samples=0 SignalSource.repeat=false -;# Next line enables the LimeSDR -SignalSource.osmosdr_args=driver=lime,soapy=0 -SignalSource.enable_throttle_control=false SignalSource.dump=false -SignalSource.dump_filename=./signal_source.dat +SignalSource.dump_filename=../data/signal_source.dat +SignalSource.enable_throttle_control=false -;######### SIGNAL_CONDITIONER CONFIG ############ SignalConditioner.implementation=Signal_Conditioner - -;######### DATA_TYPE_ADAPTER CONFIG ############ DataTypeAdapter.implementation=Pass_Through - -;######### INPUT_FILTER CONFIG ############ -InputFilter.implementation=Freq_Xlating_Fir_Filter -InputFilter.decimation_factor=1 -InputFilter.input_item_type=gr_complex -InputFilter.output_item_type=gr_complex -InputFilter.taps_item_type=float -InputFilter.number_of_taps=5 -InputFilter.number_of_bands=2 -InputFilter.band1_begin=0.0 -InputFilter.band1_end=0.85 -InputFilter.band2_begin=0.9 -InputFilter.band2_end=1.0 -InputFilter.ampl1_begin=1.0 -InputFilter.ampl1_end=1.0 -InputFilter.ampl2_begin=0.0 -InputFilter.ampl2_end=0.0 -InputFilter.band1_error=1.0 -InputFilter.band2_error=1.0 -InputFilter.filter_type=bandpass -InputFilter.grid_density=16 -InputFilter.dump=false -InputFilter.dump_filename=../data/input_filter.dat - -;######### RESAMPLER CONFIG ############ +InputFilter.implementation=Pulse_Blanking_Filter ; <- Required in some locations +InputFilter.pfa=0.001 +InputFilter.segments_est=2500 Resampler.implementation=Pass_Through - ;######### CHANNELS GLOBAL CONFIG ############ -Channels_1C.count=8 +Channels_1C.count=7 +Channels_1B.count=0 Channels.in_acquisition=1 Channel.signal=1C + ;######### ACQUISITION GLOBAL CONFIG ############ -Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition_Fine_Doppler +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition Acquisition_1C.item_type=gr_complex -Acquisition_1C.sampled_ms=1 -Acquisition_1C.threshold=0.015 -Acquisition_1C.doppler_max=10000 -Acquisition_1C.doppler_min=-10000 -Acquisition_1C.doppler_step=500 -Acquisition_1C.max_dwells=15 +Acquisition_1C.coherent_integration_time_ms=1 +Acquisition_1C.use_CFAR_algorithm=false; +Acquisition_1C.threshold=2.4 +Acquisition_1C.doppler_max=6000 +Acquisition_1C.doppler_step=250 Acquisition_1C.dump=false Acquisition_1C.dump_filename=./acq_dump.dat +;######### GALILEO ACQUISITION CONFIG ############ +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.item_type=gr_complex +Acquisition_1B.threshold=2.5 +Acquisition_1B.use_CFAR_algorithm=false +Acquisition_1B.blocking=false +Acquisition_1B.doppler_max=6000 +Acquisition_1B.doppler_step=125 +Acquisition_1B.dump=false +Acquisition_1B.dump_filename=./acq_dump.dat + + ;######### TRACKING GLOBAL CONFIG ############ Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking Tracking_1C.item_type=gr_complex -Tracking_1C.pll_bw_hz=40.0; -Tracking_1C.dll_bw_hz=2.0; -Tracking_1C.order=3; -Tracking_1C.early_late_space_chips=0.5; Tracking_1C.dump=false Tracking_1C.dump_filename=./tracking_ch_ +Tracking_1C.pll_bw_hz=45.0; +Tracking_1C.dll_bw_hz=4.0; +Tracking_1C.pll_bw_narrow_hz=5.0; +Tracking_1C.dll_bw_narrow_hz=0.75; +Tracking_1C.extend_correlation_symbols=1; +Tracking_1C.order=3; +Tracking_1C.early_late_space_chips=0.5; +Tracking_1C.early_late_space_narrow_chips=0.5 + +;######### TRACKING GALILEO CONFIG ############ +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +Tracking_1B.item_type=gr_complex +Tracking_1B.pll_bw_hz=15.0; +Tracking_1B.dll_bw_hz=0.75; +Tracking_1B.early_late_space_chips=0.15; +Tracking_1B.very_early_late_space_chips=0.5; +Tracking_1B.pll_bw_narrow_hz=5.0 +Tracking_1B.dll_bw_narrow_hz=0.5 +Tracking_1B.extend_correlation_symbols=1 +Tracking_1B.track_pilot=true +Tracking_1B.enable_fll_pull_in=true; +;Tracking_1B.pull_in_time_s=60 +Tracking_1B.enable_fll_steady_state=false +Tracking_1B.fll_bw_hz=10 +Tracking_1B.dump=false +Tracking_1B.dump_filename=tracking_ch_ + ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false + ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat + ;######### PVT CONFIG ############ -;PVT.implementation=RTKLIB_PVT -PVT.positioning_mode=Single +PVT.implementation=RTKLIB_PVT +PVT.enable_rx_clock_correction=false +PVT.positioning_mode=Single ; 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.rinexobs_rate_ms=100 PVT.display_rate_ms=500 -PVT.iono_model=Broadcast -PVT.trop_model=Saastamoinen +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.dump=false PVT.flag_rtcm_server=true PVT.flag_rtcm_tty_port=false PVT.rtcm_dump_devname=/dev/pts/1 -PVT.rtcm_tcp_port=2101 -PVT.rtcm_MT1019_rate_ms=5000 -PVT.rtcm_MT1077_rate_ms=1000 -PVT.rinex_version=2 diff --git a/conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf b/conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf index 6e59228fd..adbe9855a 100644 --- a/conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_plutosdr_realtime.conf @@ -13,8 +13,8 @@ ;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [sps]. ;FOR USE GNSS-SDR WITH RTLSDR DONGLES USER MUST SET THE CALIBRATED SAMPLE RATE HERE ; i.e. using front-end-cal as reported here:http://www.cttc.es/publication/turning-a-television-into-a-gnss-receiver/ -GNSS-SDR.internal_fs_sps=2000000 - +GNSS-SDR.internal_fs_sps=4000000 +GNSS-SDR.use_acquisition_resampler=true ;######### SUPL RRLP GPS assistance configuration ##### ; Check https://www.mcc-mnc.com/ @@ -34,10 +34,10 @@ GNSS-SDR.SUPL_CI=0x31b0 SignalSource.implementation=Plutosdr_Signal_Source SignalSource.item_type=gr_complex SignalSource.device_address=192.168.2.1 -SignalSource.sampling_frequency=3000000 +SignalSource.sampling_frequency=4000000 SignalSource.freq=1575420000 -SignalSource.bandwidth=2600000 -SignalSource.gain_mode=manual +SignalSource.bandwidth=2000000 +SignalSource.gain_mode=slow_attack SignalSource.gain=30 SignalSource.samples=0 SignalSource.buffer_size=65000 @@ -47,39 +47,79 @@ SignalSource.dump_filename=./capture.dat SignalSource.enable_throttle_control=false ;######### SIGNAL_CONDITIONER CONFIG ############ -SignalConditioner.implementation=Signal_Conditioner -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 - - -;######### DATA_TYPE_ADAPTER CONFIG ############ -DataTypeAdapter.implementation=Pass_Through -DataTypeAdapter.item_type=gr_complex +SignalConditioner.implementation=Pass_Through ;######### CHANNELS GLOBAL CONFIG ############ -Channels_1C.count=6 +Channels_1C.count=8 +Channels_1B.count=0 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.pfa=0.01 -Acquisition_1C.doppler_max=10000 +Acquisition_1C.coherent_integration_time_ms=1 +Acquisition_1C.use_CFAR_algorithm=false; +Acquisition_1C.threshold=2.6 +Acquisition_1C.doppler_max=50000 Acquisition_1C.doppler_step=250 +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=./acq_dump.dat + +;######### GALILEO ACQUISITION CONFIG ############ +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.item_type=gr_complex +Acquisition_1B.threshold=2.5 +Acquisition_1B.use_CFAR_algorithm=false +Acquisition_1B.blocking=false +Acquisition_1B.doppler_max=6000 +Acquisition_1B.doppler_step=125 +Acquisition_1B.dump=false +Acquisition_1B.dump_filename=./acq_dump.dat + ;######### TRACKING GLOBAL CONFIG ############ Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking Tracking_1C.item_type=gr_complex -Tracking_1C.pll_bw_hz=40.0; -Tracking_1C.dll_bw_hz=4.0; +Tracking_1C.dump=false +Tracking_1C.dump_filename=./tracking_ch_ +Tracking_1C.pll_bw_hz=35.0; +Tracking_1C.dll_bw_hz=2.0; +Tracking_1C.enable_fll_pull_in=true; +Tracking_1C.fll_bw_hz=10 +Tracking_1C.pll_bw_narrow_hz=5.0; +Tracking_1C.dll_bw_narrow_hz=0.75; +Tracking_1C.extend_correlation_symbols=1; +Tracking_1C.order=3; +Tracking_1C.early_late_space_chips=0.5; +Tracking_1C.early_late_space_narrow_chips=0.5 + +;######### TRACKING GALILEO CONFIG ############ +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +Tracking_1B.item_type=gr_complex +Tracking_1B.pll_bw_hz=15.0; +Tracking_1B.dll_bw_hz=0.75; +Tracking_1B.early_late_space_chips=0.15; +Tracking_1B.very_early_late_space_chips=0.5; +Tracking_1B.pll_bw_narrow_hz=5.0 +Tracking_1B.dll_bw_narrow_hz=0.5 +Tracking_1B.extend_correlation_symbols=1 +Tracking_1B.track_pilot=true +Tracking_1B.enable_fll_pull_in=true; +;Tracking_1B.pull_in_time_s=60 +Tracking_1B.enable_fll_steady_state=false +Tracking_1B.fll_bw_hz=10 +Tracking_1B.dump=false +Tracking_1B.dump_filename=tracking_ch_ + ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false + +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false ;######### OBSERVABLES CONFIG ############ Observables.implementation=Hybrid_Observables @@ -89,16 +129,18 @@ 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.enable_rx_clock_correction=false +PVT.positioning_mode=Single ; 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.rinexobs_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=false +PVT.flag_rtcm_server=true +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 \ No newline at end of file diff --git a/conf/gnss-sdr_labsat_kf_vtl.conf b/conf/gnss-sdr_labsat_kf_vtl.conf new file mode 100644 index 000000000..b5f8ce879 --- /dev/null +++ b/conf/gnss-sdr_labsat_kf_vtl.conf @@ -0,0 +1,225 @@ +; This is a GNSS-SDR configuration file +; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/ +; SPDX-License-Identifier: GPL-3.0-or-later +; SPDX-FileCopyrightText: (C) 2010-2021 (see AUTHORS file for a list of contributors) + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +GNSS-SDR.internal_fs_sps=5456000 +;GNSS-SDR.internal_fs_sps=16368000 +GNSS-SDR.use_acquisition_resampler=true + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=Labsat_Signal_Source +SignalSource.selected_channel=1 +;#filename: path to file with the captured GNSS signal samples to be processed +;# Labsat sile source automatically increments the file name when the signal is split in several files +;# the adapter adds "_0000.LS3" to this base path and filename. Next file will be "_0001.LS3" and so on +;# in this example, the first file complete path will be ../signals/GPS_025_ +;SignalSource.filename=/media/javier/WDNASNTFS/satgen_30mins/output/output +SignalSource.filename=/home/javier/signals/satgen_30mins/output/output +;SignalSource.filename=/home/javier/signals/dupli/dupli/dupli +SignalSource.item_type=gr_complex +SignalSource.sampling_frequency=16368000 +SignalSource.samples=0 +SignalSource.repeat=false +SignalSource.dump=false +SignalSource.dump_filename=./out.dat +SignalSource.enable_throttle_control=false + + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER CONFIG ############ +DataTypeAdapter.implementation=Pass_Through +DataTypeAdapter.item_type=gr_complex + +;######### INPUT_FILTER CONFIG ############ +InputFilter.implementation=Freq_Xlating_Fir_Filter +InputFilter.dump=false +InputFilter.dump_filename=/media/javier/WDNASNTFS/output_5.456Msps_gr_complex.dat + +InputFilter.input_item_type=gr_complex +InputFilter.output_item_type=gr_complex +InputFilter.taps_item_type=float +InputFilter.number_of_taps=5 +InputFilter.number_of_bands=2 + +InputFilter.band1_begin=0.0 +InputFilter.band1_end=0.45 +InputFilter.band2_begin=0.55 +InputFilter.band2_end=1.0 + +InputFilter.ampl1_begin=1.0 +InputFilter.ampl1_end=1.0 +InputFilter.ampl2_begin=0.0 +InputFilter.ampl2_end=0.0 + +InputFilter.band1_error=1.0 +InputFilter.band2_error=1.0 + +InputFilter.filter_type=lowpass +InputFilter.grid_density=16 +InputFilter.sampling_frequency=16368000 +InputFilter.IF=0 +InputFilter.decimation_factor=3 + + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_1C.count=6 +Channels_1B.count=0 +Channels_L5.count=0 +Channels_5X.count=0 + +Channels.in_acquisition=1 + +;Channel0.satellite=3 + + +;#signal: +;# "1C" GPS L1 C/A +;# "1B" GALILEO E1 B (I/NAV OS/CS/SoL) +;# "1G" GLONASS L1 C/A +;# "2S" GPS L2 L2C (M) +;# "5X" GALILEO E5a I+Q +;# "L5" GPS L5 + + +;######### GPS ACQUISITION CONFIG ############ +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.item_type=gr_complex +Acquisition_1C.threshold=3.0 +Acquisition_1C.use_CFAR_algorithm=false +Acquisition_1C.blocking=true +Acquisition_1C.doppler_max=5000 +Acquisition_1C.doppler_step=125 +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=./acq_dump.dat + + +;######### GALILEO ACQUISITION CONFIG ############ +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +Acquisition_1B.item_type=gr_complex +Acquisition_1B.threshold=2.8 +Acquisition_1B.use_CFAR_algorithm=false +Acquisition_1B.blocking=false +Acquisition_1B.doppler_max=5000 +Acquisition_1B.doppler_step=125 +Acquisition_1B.dump=false +Acquisition_1B.dump_filename=./acq_dump.dat + + +;######### TRACKING GPS CONFIG ############ +;Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking +;Tracking_1C.item_type=gr_complex +;Tracking_1C.dump=false +;Tracking_1C.dump_filename=./tracking_ch_ +;Tracking_1C.pll_bw_hz=35.0; +;Tracking_1C.dll_bw_hz=1.5; +;Tracking_1C.pll_bw_narrow_hz=2.5; +;Tracking_1C.dll_bw_narrow_hz=0.2; +;Tracking_1C.extend_correlation_symbols=1; +;Tracking_1C.dll_filter_order=2; +;Tracking_1C.pll_filter_order=3; +;Tracking_1C.early_late_space_chips=0.5; +;Tracking_1C.early_late_space_narrow_chips=0.15 + +;### KF tracking +Tracking_1C.implementation=GPS_L1_CA_KF_VTL_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.dump=true +Tracking_1C.dump_filename=./tracking_ch_ +Tracking_1C.extend_correlation_symbols=1; +Tracking_1C.early_late_space_chips=0.5; +Tracking_1C.early_late_space_narrow_chips=0.15 + +Tracking_1C.expected_cn0_dbhz=45.0; +Tracking_1C.enable_dynamic_measurement_covariance=false; +Tracking_1C.use_estimated_cn0=false; +Tracking_1C.carrier_aiding=true; + + +Tracking_1C.code_phase_sd_chips=0.01; +Tracking_1C.code_rate_sd_chips_s=0.001; + +Tracking_1C.carrier_phase_sd_rad=0.001; +Tracking_1C.carrier_freq_sd_hz=0.01; +Tracking_1C.carrier_freq_rate_sd_hz_s=0.1; + + +Tracking_1C.init_code_phase_sd_chips=1; +Tracking_1C.init_code_rate_sd_chips_s=10; + +Tracking_1C.init_carrier_phase_sd_rad=1; +Tracking_1C.init_carrier_freq_sd_hz=10; +Tracking_1C.init_carrier_freq_rate_sd_hz_s=10; + + +;######### TRACKING GALILEO CONFIG ############ +Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +Tracking_1B.item_type=gr_complex +Tracking_1B.pll_bw_hz=15.0; +Tracking_1B.dll_bw_hz=0.75; +Tracking_1B.early_late_space_chips=0.15; +Tracking_1B.very_early_late_space_chips=0.5; +Tracking_1B.early_late_space_narrow_chips=0.10; +Tracking_1B.very_early_late_space_narrow_chips=0.5; +Tracking_1B.pll_bw_narrow_hz=2.5 +Tracking_1B.dll_bw_narrow_hz=0.2 +Tracking_1B.extend_correlation_symbols=5 +Tracking_1B.track_pilot=true +Tracking_1B.enable_fll_pull_in=true; +;Tracking_1B.pull_in_time_s=60 +Tracking_1B.enable_fll_steady_state=false +Tracking_1B.fll_bw_hz=10 +Tracking_1B.dump=false +Tracking_1B.dump_filename=tracking_ch_ + +;######### TELEMETRY DECODER GALILEO CONFIG ############ +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false + + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false + + +;######### OBSERVABLES CONFIG ############ +;#implementation: +Observables.implementation=Hybrid_Observables +Observables.dump=false +Observables.dump_filename=./observables.dat +Observables.enable_carrier_smoothing=false +Observables.smoothing_factor=200 + + + +;######### PVT CONFIG ############ +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=Single ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.enable_rx_clock_correction=false +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=1000; +PVT.rinexobs_rate_ms=1000; +PVT.display_rate_ms=1000; +PVT.elevation_mask=15; +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 +PVT.dump=false +PVT.dump_filename=./PVT +PVT.enable_monitor=false +PVT.monitor_udp_port=1337 +PVT.monitor_client_addresses=127.0.0.1 + +;######### MONITOR CONFIG ############ +Monitor.enable_monitor=false +Monitor.decimation_factor=1 +Monitor.client_addresses=127.0.0.1 +Monitor.udp_port=1234 + + diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index 9f748194c..c1b7ffd93 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -29,6 +29,9 @@ All notable changes to GNSS-SDR will be documented in this file. - Fixed the regeneration of Galileo ephemeris from the reduced clock and ephemeris data (CED) defined in the Galileo E1B INAV message introduced in Galileo OS SIS ICD Issue 2.0. +- Added a `Limesdr_Signal_Source` for interoperability with LimeSDR (requires + [gr-limesdr](https://github.com/myriadrf/gr-limesdr) and the + `-DENABLE_LIMESDR=ON` building flag). ### Improvements in Maintainability: diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 5620d3a51..32d94a2f6 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -853,6 +853,11 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration, // Set maximum clock offset allowed if pvt_output_parameters.enable_rx_clock_correction = false pvt_output_parameters.max_obs_block_rx_clock_offset_ms = configuration->property(role + ".max_clock_offset_ms", pvt_output_parameters.max_obs_block_rx_clock_offset_ms); + + // Source timetag + pvt_output_parameters.log_source_timetag = configuration->property(role + ".log_timetag", pvt_output_parameters.log_source_timetag); + pvt_output_parameters.log_source_timetag_file = configuration->property(role + ".log_source_timetag_file", pvt_output_parameters.log_source_timetag_file); + // make PVT object pvt_ = rtklib_make_pvt_gs(in_streams_, pvt_output_parameters, rtk); DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc index 784412e70..f0c039506 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.cc @@ -56,6 +56,7 @@ #include "rtcm_printer.h" #include "rtklib_rtkcmn.h" #include "rtklib_solver.h" +#include "trackingcmd.h" #include // for xml_iarchive #include // for xml_oarchive #include @@ -167,11 +168,13 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, d_show_local_time_zone(conf_.show_local_time_zone), d_waiting_obs_block_rx_clock_offset_correction_msg(false), d_enable_rx_clock_correction(conf_.enable_rx_clock_correction), - d_an_printer_enabled(conf_.an_output_enabled) + d_an_printer_enabled(conf_.an_output_enabled), + d_log_timetag(conf_.log_source_timetag) { // Send feedback message to observables block with the receiver clock offset this->message_port_register_out(pmt::mp("pvt_to_observables")); - + // Experimental: VLT commands from PVT to tracking channels + this->message_port_register_out(pmt::mp("pvt_to_trk")); // Send PVT status to gnss_flowgraph this->message_port_register_out(pmt::mp("status")); @@ -557,6 +560,21 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels, // set the RTKLIB trace (debug) level tracelevel(conf_.rtk_trace_level); + // timetag + if (d_log_timetag) + { + try + { + d_log_timetag_file.open(conf_.log_source_timetag_file, std::ios::out | std::ios::binary); + std::cout << "Log PVT timetag metadata enabled, log file: " << conf_.log_source_timetag_file << '\n'; + } + catch (const std::exception& e) + { + std::cerr << "Log PVT timetag metadata file cannot be created: " << e.what() << '\n'; + d_log_timetag = false; + } + } + d_start = std::chrono::system_clock::now(); } @@ -1104,6 +1122,18 @@ rtklib_pvt_gs::~rtklib_pvt_gs() LOG(INFO) << "Failed to save BeiDou DNAV UTC model parameters, not valid data"; } } + + if (d_log_timetag_file.is_open()) + { + try + { + d_log_timetag_file.close(); + } + catch (const std::exception& e) + { + LOG(WARNING) << "Problem closing Log PVT timetag metadata file: " << e.what(); + } + } } catch (std::length_error& e) { @@ -1657,6 +1687,23 @@ bool rtklib_pvt_gs::save_gnss_synchro_map_xml(const std::string& file_name) } +void rtklib_pvt_gs::log_source_timetag_info(double RX_time_ns, double TAG_time_ns) +{ + if (d_log_timetag_file.is_open()) + { + try + { + d_log_timetag_file.write(reinterpret_cast(&RX_time_ns), sizeof(double)); + d_log_timetag_file.write(reinterpret_cast(&TAG_time_ns), sizeof(double)); + } + catch (const std::exception& e) + { + std::cerr << "Problem writting at the log PVT timetag metadata file: " << e.what() << '\n'; + } + } +} + + bool rtklib_pvt_gs::load_gnss_synchro_map_xml(const std::string& file_name) { // load from xml (boost serialize) @@ -1899,6 +1946,35 @@ void rtklib_pvt_gs::initialize_and_apply_carrier_phase_offset() int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_items, gr_vector_void_star& output_items __attribute__((unused))) { + //**************** time tags **************** + if (d_enable_rx_clock_correction == false) // todo: currently only works if clock correction is disabled + { + std::vector tags_vec; + // time tag from obs to pvt is always propagated in channel 0 + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + noutput_items); + for (const auto& it : tags_vec) + { + try + { + if (pmt::any_ref(it.value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const auto timetag = boost::any_cast>(pmt::any_ref(it.value)); + // std::cout << "PVT timetag: " << timetag->rx_time << '\n'; + d_TimeChannelTagTimestamps.push(*timetag); + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast& e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + } + //************* end time tags ************** + for (int32_t epoch = 0; epoch < noutput_items; epoch++) { bool flag_display_pvt = false; @@ -2037,6 +2113,46 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item if (d_internal_pvt_solver->get_PVT(d_gnss_observables_map, false)) { const double Rx_clock_offset_s = d_internal_pvt_solver->get_time_offset_s(); + + //**************** time tags **************** + if (d_enable_rx_clock_correction == false) // todo: currently only works if clock correction is disabled (computed clock offset is applied here) + { + //************ Source TimeTag comparison with GNSS computed TOW ************* + + if (!d_TimeChannelTagTimestamps.empty()) + { + double delta_rxtime_to_tag_ms; + GnssTime current_tag; + // 1. Find the nearest timetag to the current rx_time (it is relative to the receiver's start operation) + do + { + current_tag = d_TimeChannelTagTimestamps.front(); + delta_rxtime_to_tag_ms = d_rx_time * 1000.0 - current_tag.rx_time; + d_TimeChannelTagTimestamps.pop(); + } + while (fabs(delta_rxtime_to_tag_ms) >= 100 and !d_TimeChannelTagTimestamps.empty()); + + + // 2. If both timestamps (relative to the receiver's start) are closer than 100 ms (the granularituy of the PVT) + if (fabs(delta_rxtime_to_tag_ms) <= 100) // [ms] + { + std::cout << "GNSS-SDR RX TIME: " << d_rx_time << " TAG RX TIME: " << current_tag.rx_time / 1000.0 << " [s]\n"; + if (d_log_timetag == true) + { + double current_corrected_RX_clock_ns = (d_rx_time - Rx_clock_offset_s) * 1e9; + double TAG_time_ns = (static_cast(current_tag.tow_ms) + current_tag.tow_ms_fraction + delta_rxtime_to_tag_ms) * 1e6; + log_source_timetag_info(current_corrected_RX_clock_ns, TAG_time_ns); + double tow_error_ns = current_corrected_RX_clock_ns - TAG_time_ns; + std::cout << "[Time ch] RX TimeTag Week: " << current_tag.week + << ", TOW: " << current_tag.tow_ms + << " [ms], TOW fraction: " << current_tag.tow_ms_fraction + << " [ms], GNSS-SDR OBS CORRECTED TOW - EXTERNAL TIMETAG TOW: " << tow_error_ns << " [ns] \n"; + } + } + } + } + //********************************************** + if (fabs(Rx_clock_offset_s) * 1000.0 > d_max_obs_block_rx_clock_offset_ms) { if (!d_waiting_obs_block_rx_clock_offset_correction_msg) @@ -2086,7 +2202,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item { flag_compute_pvt_output = true; // std::cout.precision(17); - // std::cout << "current_RX_time: " << current_RX_time << " map time: " << d_gnss_observables_map.begin()->second.RX_time << '\n'; + // std::cout << "current_RX_time: " << current_RX_time_ms << " map time: " << d_gnss_observables_map.begin()->second.RX_time << '\n'; } flag_pvt_valid = true; } @@ -2104,14 +2220,22 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item flag_pvt_valid = d_user_pvt_solver->get_PVT(d_gnss_observables_map, false); } + if (flag_pvt_valid == true) { + // experimental VTL tests + // send tracking command + // const std::shared_ptr trk_cmd_test = std::make_shared(TrackingCmd()); + // trk_cmd_test->carrier_freq_hz = 12345.4; + // trk_cmd_test->sample_counter = d_gnss_observables_map.begin()->second.Tracking_sample_counter; + // this->message_port_pub(pmt::mp("pvt_to_trk"), pmt::make_any(trk_cmd_test)); + // initialize (if needed) the accumulated phase offset and apply it to the active channels // required to report accumulated phase cycles comparable to pseudoranges initialize_and_apply_carrier_phase_offset(); const double Rx_clock_offset_s = d_user_pvt_solver->get_time_offset_s(); - if (d_enable_rx_clock_correction == true && fabs(Rx_clock_offset_s) > 0.000001) // 1us !! + if (d_enable_rx_clock_correction == true and fabs(Rx_clock_offset_s) > 0.000001) // 1us !! { LOG(INFO) << "Warning: Rx clock offset at interpolated RX time: " << Rx_clock_offset_s * 1000.0 << "[ms]" << " at RX time: " << static_cast(d_rx_time * 1000.0) << " [ms]"; diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h index 26bf51d8b..bfd53db63 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_gs.h @@ -19,6 +19,7 @@ #include "gnss_block_interface.h" #include "gnss_synchro.h" +#include "gnss_time.h" #include "rtklib.h" #include #include @@ -29,8 +30,10 @@ #include // for size_t #include // for int32_t #include // for time_t +#include // for std::fstream #include // for map #include // for shared_ptr, unique_ptr +#include // for std::queue #include // for string #include // for key_t #include // for vector @@ -133,6 +136,8 @@ private: const Pvt_Conf& conf_, const rtk_t& rtk); + void log_source_timetag_info(double RX_time_ns, double TAG_time_ns); + void msg_handler_telemetry(const pmt::pmt_t& msg); void msg_handler_has_data(const pmt::pmt_t& msg) const; @@ -163,6 +168,8 @@ private: bool save_gnss_synchro_map_xml(const std::string& file_name); // debug helper function bool load_gnss_synchro_map_xml(const std::string& file_name); // debug helper function + std::fstream d_log_timetag_file; + std::shared_ptr d_internal_pvt_solver; std::shared_ptr d_user_pvt_solver; @@ -208,6 +215,8 @@ private: std::map d_gnss_observables_map_t0; std::map d_gnss_observables_map_t1; + std::queue d_TimeChannelTagTimestamps; + boost::posix_time::time_duration d_utc_diff_time; size_t d_gps_ephemeris_sptr_type_hash_code; @@ -277,6 +286,7 @@ private: bool d_enable_rx_clock_correction; bool d_enable_has_messages; bool d_an_printer_enabled; + bool d_log_timetag; }; diff --git a/src/algorithms/PVT/libs/pvt_conf.cc b/src/algorithms/PVT/libs/pvt_conf.cc new file mode 100644 index 000000000..16c325b0d --- /dev/null +++ b/src/algorithms/PVT/libs/pvt_conf.cc @@ -0,0 +1,74 @@ +/*! + * \file pvt_conf.cc + * \brief Class that contains all the configuration parameters for a PVT block + * \author Carles Fernandez, 2018. cfernandez(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "pvt_conf.h" + +Pvt_Conf::Pvt_Conf() +{ + type_of_receiver = 0U; + observable_interval_ms = 20U; + output_rate_ms = 0; + display_rate_ms = 0; + kml_rate_ms = 1000; + gpx_rate_ms = 1000; + geojson_rate_ms = 1000; + nmea_rate_ms = 1000; + + max_obs_block_rx_clock_offset_ms = 40; + rinex_version = 0; + rinexobs_rate_ms = 0; + rinex_name = std::string("-"); + + dump = false; + dump_mat = true; + + flag_nmea_tty_port = false; + + flag_rtcm_server = false; + flag_rtcm_tty_port = false; + rtcm_tcp_port = 0U; + rtcm_station_id = 0U; + + output_enabled = true; + rinex_output_enabled = true; + gpx_output_enabled = true; + geojson_output_enabled = true; + nmea_output_file_enabled = true; + kml_output_enabled = true; + xml_output_enabled = true; + rtcm_output_file_enabled = true; + + output_path = std::string("."); + rinex_output_path = std::string("."); + gpx_output_path = std::string("."); + geojson_output_path = std::string("."); + nmea_output_file_path = std::string("."); + kml_output_path = std::string("."); + xml_output_path = std::string("."); + rtcm_output_file_path = std::string("."); + + enable_rx_clock_correction = true; + monitor_enabled = false; + monitor_ephemeris_enabled = false; + protobuf_enabled = true; + udp_port = 0; + udp_eph_port = 0; + pre_2009_file = false; + show_local_time_zone = false; + + log_source_timetag = false; + log_source_timetag_file = "PVT_timetag.dat"; +} diff --git a/src/algorithms/PVT/libs/pvt_conf.h b/src/algorithms/PVT/libs/pvt_conf.h index 2bc89adde..5ba9bf710 100644 --- a/src/algorithms/PVT/libs/pvt_conf.h +++ b/src/algorithms/PVT/libs/pvt_conf.h @@ -89,6 +89,8 @@ public: bool pre_2009_file = false; bool dump = false; bool dump_mat = true; + bool log_source_timetag; + std::string log_source_timetag_file; }; diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index 57e614b34..d89acddb6 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -29,6 +29,7 @@ set(GNSS_SPLIBS_SOURCES gnss_sdr_create_directory.cc geofunctions.cc item_type_helpers.cc + trackingcmd.cc pass_through.cc short_x2_to_cshort.cc gnss_sdr_string_literals.cc @@ -60,9 +61,11 @@ set(GNSS_SPLIBS_HEADERS gnss_circular_deque.h geofunctions.h item_type_helpers.h + trackingcmd.h pass_through.h short_x2_to_cshort.h gnss_sdr_string_literals.h + gnss_time.h ) if(ENABLE_OPENCL) diff --git a/src/algorithms/libs/gnss_sdr_flags.cc b/src/algorithms/libs/gnss_sdr_flags.cc index 625220b9b..4961c01aa 100644 --- a/src/algorithms/libs/gnss_sdr_flags.cc +++ b/src/algorithms/libs/gnss_sdr_flags.cc @@ -33,6 +33,9 @@ DEFINE_string(s, "-", DEFINE_string(signal_source, "-", "If defined, path to the file containing the signal samples (overrides the configuration file)."); +DEFINE_string(timestamp_source, "-", + "If defined, path to the file containing the signal timestamp data (overrides the configuration file)."); + DEFINE_bool(rf_shutdown, true, "If set to false, AD9361 RF channels are not shut down when exiting the program. Useful to leave the AD9361 configured and running."); DEFINE_int32(doppler_max, 0, "If defined, sets the maximum Doppler value in the search grid, in Hz (overrides the configuration file)."); diff --git a/src/algorithms/libs/gnss_sdr_flags.h b/src/algorithms/libs/gnss_sdr_flags.h index c1aaac5a1..361195a49 100644 --- a/src/algorithms/libs/gnss_sdr_flags.h +++ b/src/algorithms/libs/gnss_sdr_flags.h @@ -35,9 +35,10 @@ DECLARE_string(config_file); //!< Path to the configuration file. DECLARE_string(log_dir); //!< Path to the folder in which logging will be stored. // Declare flags for signal sources -DECLARE_string(s); //!< Path to the file containing the signal samples. -DECLARE_string(signal_source); //!< Path to the file containing the signal samples. -DECLARE_bool(rf_shutdown); //!< Shutdown RF when program exits. +DECLARE_string(s); //!< Path to the file containing the signal samples. +DECLARE_string(signal_source); //!< Path to the file containing the signal samples. +DECLARE_string(timestamp_source); //!< Path to the file containing the signal samples. +DECLARE_bool(rf_shutdown); //!< Shutdown RF when program exits. // Declare flags for acquisition blocks DECLARE_int32(doppler_max); //!< If defined, maximum Doppler value in the search grid, in Hz (overrides the configuration file). diff --git a/src/algorithms/libs/gnss_time.h b/src/algorithms/libs/gnss_time.h new file mode 100644 index 000000000..b196f8c5a --- /dev/null +++ b/src/algorithms/libs/gnss_time.h @@ -0,0 +1,32 @@ +/*! + * \file gnss_time.h + * \brief class that stores both the receiver time, relative to the receiver start and the GNSS time (absolute) + * \author Javier Arribas 2022. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GNSS_TIME_H +#define GNSS_SDR_GNSS_TIME_H + +#include + +class GnssTime +{ +public: + double rx_time; + int week; /*!< GPS week number (since January 1980) */ + int tow_ms; /* time of week [ms]*/ + double tow_ms_fraction; /* tow ms fractional part [ms]*/ +}; + +#endif diff --git a/src/algorithms/libs/trackingcmd.cc b/src/algorithms/libs/trackingcmd.cc new file mode 100644 index 000000000..d41d155cf --- /dev/null +++ b/src/algorithms/libs/trackingcmd.cc @@ -0,0 +1,23 @@ +/*! + * \file trackingcmd.cc + * \brief Class that stores information to update the GNSS signal tracking estimations + * \author Javier Arribas, 2021. jarribas(at)cttc.es + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "trackingcmd.h" + +TrackingCmd::TrackingCmd() +{ + // TODO Auto-generated constructor stub +} diff --git a/src/algorithms/libs/trackingcmd.h b/src/algorithms/libs/trackingcmd.h new file mode 100644 index 000000000..ce6a44f6c --- /dev/null +++ b/src/algorithms/libs/trackingcmd.h @@ -0,0 +1,36 @@ +/*! + * \file trackingcmd.h + * \brief Class that stores information to update the GNSS signal tracking estimations + * \author Javier Arribas, 2021. jarribas(at)cttc.es + * + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef SRC_ALGORITHMS_LIBS_TRACKINGCMD_H_ +#define SRC_ALGORITHMS_LIBS_TRACKINGCMD_H_ + +#include + +class TrackingCmd +{ +public: + TrackingCmd(); + + bool enable_carrier_nco_cmd; + bool enable_code_nco_cmd; + double code_freq_chips; + double carrier_freq_hz; + double carrier_freq_rate_hz_s; + uint64_t sample_counter; +}; + +#endif /* SRC_ALGORITHMS_LIBS_TRACKINGCMD_H_ */ diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc index a8a96a1bf..e533c3ef0 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.cc @@ -111,6 +111,11 @@ hybrid_observables_gs::hybrid_observables_gs(const Obs_Conf &conf_) d_mapStringValues["B2"] = evBDS_B2; d_mapStringValues["B3"] = evBDS_B3; + + d_SourceTagTimestamps = std::vector>(d_nchannels_out); + last_rx_clock_round20ms_error = 0; + set_tag_propagation_policy(TPP_DONT); // no tag propagation, the time tag will be adjusted and regenerated in work() + // ############# ENABLE DATA FILE LOG ################# if (d_dump) { @@ -204,12 +209,14 @@ void hybrid_observables_gs::msg_handler_pvt_to_observables(const pmt::pmt_t &msg if (pmt::any_ref(msg).type().hash_code() == d_double_type_hash_code) { const auto new_rx_clock_offset_s = wht::any_cast(pmt::any_ref(msg)); + double old_tow_corrected = static_cast(d_T_rx_TOW_ms) - new_rx_clock_offset_s * 1000.0; d_T_rx_TOW_ms = d_T_rx_TOW_ms - static_cast(round(new_rx_clock_offset_s * 1000.0)); // align the receiver clock to integer multiple of d_T_rx_step_ms if (d_T_rx_TOW_ms % d_T_rx_step_ms) { d_T_rx_TOW_ms += d_T_rx_step_ms - d_T_rx_TOW_ms % d_T_rx_step_ms; } + last_rx_clock_round20ms_error = static_cast(d_T_rx_TOW_ms) - old_tow_corrected; // d_Rx_clock_buffer.clear(); // Clear all the elements in the buffer for (uint32_t n = 0; n < d_nchannels_out; n++) { @@ -616,6 +623,55 @@ void hybrid_observables_gs::smooth_pseudoranges(std::vector &data) } +void hybrid_observables_gs::set_tag_timestamp_in_sdr_timeframe(const std::vector &data, uint64_t rx_clock) +{ + // it transforms the HW sample tag timestamp from a relative samplestamp (from receiver start) + // to an absolute GPS TOW samplestamp associated with the current set of pseudoranges + if (!d_TimeChannelTagTimestamps.empty()) + { + double fs = 0; + std::vector::const_iterator it; + for (it = data.begin(); it != data.end(); it++) + { + if (it->Flag_valid_pseudorange == true) + { + fs = static_cast(it->fs); + break; + } + } + + double delta_rxtime_to_tag; + GnssTime current_tag; + do + { + current_tag = d_TimeChannelTagTimestamps.front(); + delta_rxtime_to_tag = (static_cast(rx_clock) / fs) - current_tag.rx_time; // delta time relative to receiver's start time + if (delta_rxtime_to_tag >= 0) + { + d_TimeChannelTagTimestamps.pop(); + } + } + while (delta_rxtime_to_tag >= 0.1 and !d_TimeChannelTagTimestamps.empty()); + + if (delta_rxtime_to_tag >= 0 and delta_rxtime_to_tag <= 0.1) + { + // std::cout << "[Time ch][" << delta_rxtime_to_tag + // << "] OBS RX TimeTag Week: " << current_tag.week + // << ", TOW: " << current_tag.tow_ms + // << " [ms], TOW fraction: " << current_tag.tow_ms_fraction + // << " [ms], DELTA TLM TOW: " << last_rx_clock_round20ms_error + delta_rxtime_to_tag * 1000.0 + static_cast(current_tag.tow_ms) - static_cast(d_T_rx_TOW_ms) + current_tag.tow_ms_fraction << " [ms] \n"; + const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + *tmp_obj = current_tag; + double intpart; + tmp_obj->tow_ms_fraction = tmp_obj->tow_ms_fraction + modf(delta_rxtime_to_tag * 1000.0, &intpart); + tmp_obj->tow_ms = current_tag.tow_ms + static_cast(intpart); + tmp_obj->rx_time = static_cast(d_T_rx_TOW_ms); // new TAG samplestamp in absolute RX time (GPS TOW frame) same as the pseudorange set + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + } + } +} + + int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) @@ -628,6 +684,31 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) if (ninput_items[d_nchannels_in - 1] > 0) { d_Rx_clock_buffer.push_back(in[d_nchannels_in - 1][0].Tracking_sample_counter); + + // time tags + std::vector tags_vec; + this->get_tags_in_range(tags_vec, d_nchannels_in - 1, this->nitems_read(d_nchannels_in - 1), this->nitems_read(d_nchannels_in - 1) + 1); + for (const auto &it : tags_vec) + { + try + { + if (pmt::any_ref(it.value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const auto timetag = boost::any_cast>(pmt::any_ref(it.value)); + // std::cout << "[Time ch ] timetag: " << timetag->rx_time << "\n"; + d_TimeChannelTagTimestamps.push(*timetag); + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + // Consume one item from the clock channel (last of the input channels) consume(static_cast(d_nchannels_in) - 1, 1); } @@ -635,9 +716,34 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) // Push the tracking observables into buffers to allow the observable interpolation at the desired Rx clock for (uint32_t n = 0; n < d_nchannels_out; n++) { - // Push the valid tracking Gnss_Synchros to their corresponding deque + //**************** time tags **************** + // std::vector tags_vec; + // this->get_tags_in_range(tags_vec, n, this->nitems_read(n), this->nitems_read(n) + ninput_items[n]); + // for (std::vector::iterator it = tags_vec.begin(); it != tags_vec.end(); ++it) + // { + // try + // { + // if (pmt::any_ref(it->value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + // { + // const std::shared_ptr timetag = boost::any_cast>(pmt::any_ref(it->value)); + // //std::cout << "[ch " << n << "] timetag: " << timetag->rx_time << "\n"; + // d_SourceTagTimestamps.at(n).push(*timetag); + // } + // else + // { + // std::cout << "hash code not match\n"; + // } + // } + // catch (const boost::bad_any_cast &e) + // { + // std::cout << "msg Bad any_cast: " << e.what(); + // } + // } + + //************* end time tags ************** for (int32_t m = 0; m < ninput_items[n]; m++) { + // Push the valid tracking Gnss_Synchros to their corresponding deque if (in[n][m].Flag_valid_word) { if (d_gnss_synchro_history->size(n) > 0) @@ -695,6 +801,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) if (n_valid > 0) { compute_pranges(epoch_data); + set_tag_timestamp_in_sdr_timeframe(epoch_data, d_Rx_clock_buffer.front()); } // Carrier smoothing (optional) @@ -702,6 +809,7 @@ int hybrid_observables_gs::general_work(int noutput_items __attribute__((unused) { smooth_pseudoranges(epoch_data); } + // output the observables set to the PVT block for (uint32_t n = 0; n < d_nchannels_out; n++) { diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h index eb45ec232..b0d323a07 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_gs.h @@ -21,6 +21,7 @@ #define GNSS_SDR_HYBRID_OBSERVABLES_GS_H #include "gnss_block_interface.h" +#include "gnss_time.h" // for timetags produced by Tracking #include "obs_conf.h" #include // for boost::circular_buffer #include // for block @@ -30,9 +31,10 @@ #include // for std::ofstream #include // for std::map #include // for std::shared, std:unique_ptr -#include // for std::string -#include // for typeid -#include // for std::vector +#include +#include // for std::string +#include // for typeid +#include // for std::vector /** \addtogroup Observables * \{ */ @@ -75,6 +77,8 @@ private: void update_TOW(const std::vector& data); void compute_pranges(std::vector& data) const; void smooth_pseudoranges(std::vector& data); + + void set_tag_timestamp_in_sdr_timeframe(const std::vector& data, uint64_t rx_clock); int32_t save_matfile() const; Obs_Conf d_conf; @@ -101,6 +105,9 @@ private: boost::circular_buffer d_Rx_clock_buffer; // time history + std::vector> d_SourceTagTimestamps; + std::queue d_TimeChannelTagTimestamps; + std::vector d_channel_last_pll_lock; std::vector d_channel_last_pseudorange_smooth; std::vector d_channel_last_carrier_phase_rads; @@ -113,6 +120,7 @@ private: double d_T_rx_step_s; uint32_t d_T_rx_TOW_ms; + double last_rx_clock_round20ms_error; uint32_t d_T_rx_step_ms; uint32_t d_T_status_report_timer_ms; uint32_t d_nchannels_in; diff --git a/src/algorithms/signal_source/adapters/CMakeLists.txt b/src/algorithms/signal_source/adapters/CMakeLists.txt index 113e7365e..7a4c6e065 100644 --- a/src/algorithms/signal_source/adapters/CMakeLists.txt +++ b/src/algorithms/signal_source/adapters/CMakeLists.txt @@ -71,6 +71,13 @@ if(ENABLE_OSMOSDR) endif() endif() +if(ENABLE_LIMESDR) + if(GRLIMESDR_FOUND) + set(OPT_DRIVER_SOURCES ${OPT_DRIVER_SOURCES} limesdr_signal_source.cc) + set(OPT_DRIVER_HEADERS ${OPT_DRIVER_HEADERS} limesdr_signal_source.h) + endif() +endif() + if(ENABLE_UHD) set(OPT_DRIVER_SOURCES ${OPT_DRIVER_SOURCES} uhd_signal_source.cc) @@ -92,6 +99,7 @@ set(SIGNAL_SOURCE_ADAPTER_SOURCES labsat_signal_source.cc two_bit_cpx_file_signal_source.cc two_bit_packed_file_signal_source.cc + file_timestamp_signal_source.cc ${OPT_DRIVER_SOURCES} ) @@ -109,6 +117,7 @@ set(SIGNAL_SOURCE_ADAPTER_HEADERS labsat_signal_source.h two_bit_cpx_file_signal_source.h two_bit_packed_file_signal_source.h + file_timestamp_signal_source.h ${OPT_DRIVER_HEADERS} ) @@ -131,6 +140,10 @@ else() ) endif() +if(GNURADIO_IS_38_OR_GREATER) + target_compile_definitions(signal_source_adapters PUBLIC -DGR_GREATER_38=1) +endif() + target_include_directories(signal_source_adapters PUBLIC ${CMAKE_SOURCE_DIR}/src/core/interfaces @@ -186,6 +199,14 @@ if(ENABLE_OSMOSDR AND GROSMOSDR_FOUND) ) endif() +if(ENABLE_LIMESDR AND GRLIMESDR_FOUND) + target_link_libraries(signal_source_adapters + PUBLIC + Gnuradio::limesdr + ) +endif() + + if(ENABLE_AD9361 AND LIBIIO_FOUND) target_link_libraries(signal_source_adapters PRIVATE @@ -248,6 +269,14 @@ if(ENABLE_PLUTOSDR OR ENABLE_FMCOMMS2) endif() endif() +if(GR_LIMESDR_IS_G38_BRANCH) + target_compile_definitions(signal_source_adapters PRIVATE -DGR_LIMESDR_IS_G38_BRANCH=1) +endif() + +if(GRLIMESDR_PPS) + target_compile_definitions(signal_source_adapters PRIVATE -DLimeSDR_PPS=1) +endif() + if(ENABLE_CLANG_TIDY) if(CLANG_TIDY_EXE) set_target_properties(signal_source_adapters @@ -261,6 +290,8 @@ target_compile_definitions(signal_source_adapters PRIVATE -DGNSSSDR_INSTALL_DIR="${CMAKE_INSTALL_PREFIX}" ) + + set_property(TARGET signal_source_adapters APPEND PROPERTY INTERFACE_INCLUDE_DIRECTORIES $ diff --git a/src/algorithms/signal_source/adapters/file_timestamp_signal_source.cc b/src/algorithms/signal_source/adapters/file_timestamp_signal_source.cc new file mode 100644 index 000000000..1cc29e40a --- /dev/null +++ b/src/algorithms/signal_source/adapters/file_timestamp_signal_source.cc @@ -0,0 +1,71 @@ +/*! + * \file file_timestamp_signal_source.h + * \brief This class reads samples stored in a file and generate stream tags with its timestamp information stored in separated file + * \author Javier Arribas, jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ +#include "file_timestamp_signal_source.h" +#include "gnss_sdr_flags.h" +#include "gnss_sdr_string_literals.h" +#include "gnss_sdr_timestamp.h" +#include +#include + +using namespace std::string_literals; + +FileTimestampSignalSource::FileTimestampSignalSource(const ConfigurationInterface* configuration, + const std::string& role, unsigned int in_streams, unsigned int out_streams, + Concurrent_Queue* queue) + : FileSourceBase(configuration, role, "File_Timestamp_Signal_Source"s, queue, "byte"s), + timestamp_file_(configuration->property(role + ".timestamp_filename"s, "../data/example_capture_timestamp.dat"s)), + timestamp_clock_offset_ms_(configuration->property(role + ".timestamp_clock_offset_ms"s, 0.0)) +{ + if (in_streams > 0) + { + LOG(ERROR) << "A signal source does not have an input stream"; + } + if (out_streams > 1) + { + LOG(ERROR) << "This implementation only supports one output stream"; + } + + // override value with commandline flag, if present + if (FLAGS_timestamp_source != "-") + { + timestamp_file_ = FLAGS_timestamp_source; + } +} + + +gnss_shared_ptr FileTimestampSignalSource::source() const { return timestamp_block_; } + + +void FileTimestampSignalSource::create_file_source_hook() +{ + timestamp_block_ = gnss_sdr_make_Timestamp( + std::get<0>(itemTypeToSize()), + timestamp_file_, + timestamp_clock_offset_ms_); + DLOG(INFO) << "timestamp_block_(" << timestamp_block_->unique_id() << ")"; +} + +void FileTimestampSignalSource::pre_connect_hook(gr::top_block_sptr top_block) +{ + top_block->connect(file_source(), 0, timestamp_block_, 0); + DLOG(INFO) << "connected file_source to timestamp_block_"; +} + +void FileTimestampSignalSource::pre_disconnect_hook(gr::top_block_sptr top_block) +{ + top_block->disconnect(file_source(), 0, timestamp_block_, 0); + DLOG(INFO) << "disconnected file_source from timestamp_block_"; +} diff --git a/src/algorithms/signal_source/adapters/file_timestamp_signal_source.h b/src/algorithms/signal_source/adapters/file_timestamp_signal_source.h new file mode 100644 index 000000000..f95b3622e --- /dev/null +++ b/src/algorithms/signal_source/adapters/file_timestamp_signal_source.h @@ -0,0 +1,62 @@ +/*! + * \file file_timestamp_signal_source.h + * \brief This class reads samples stored in a file and generate stream tags with its timestamp information stored in separated file + * \author Javier Arribas, jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_FILE_TIMESTAMP_SIGNAL_SOURCE_H +#define GNSS_SDR_FILE_TIMESTAMP_SIGNAL_SOURCE_H + +#include "configuration_interface.h" +#include "file_source_base.h" +#include "gnss_sdr_timestamp.h" +#include + +/** \addtogroup Signal_Source + * \{ */ +/** \addtogroup Signal_Source_adapters + * \{ */ + + +/*! + * \brief Class that reads signals samples from a file + * and adapts it to a SignalSourceInterface + */ +class FileTimestampSignalSource : public FileSourceBase +{ +public: + FileTimestampSignalSource(const ConfigurationInterface* configuration, const std::string& role, + unsigned int in_streams, unsigned int out_streams, + Concurrent_Queue* queue); + + ~FileTimestampSignalSource() = default; + +protected: + // std::tuple itemTypeToSize() override; + // double packetsPerSample() const override; + gnss_shared_ptr source() const override; + void create_file_source_hook() override; + void pre_connect_hook(gr::top_block_sptr top_block) override; + void pre_disconnect_hook(gr::top_block_sptr top_block) override; + +private: + gnss_shared_ptr timestamp_block_; + std::string timestamp_file_; + double timestamp_clock_offset_ms_; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_FILE_TIMESTAMP_SIGNAL_SOURCE_H diff --git a/src/algorithms/signal_source/adapters/limesdr_signal_source.cc b/src/algorithms/signal_source/adapters/limesdr_signal_source.cc new file mode 100644 index 000000000..19dee77e2 --- /dev/null +++ b/src/algorithms/signal_source/adapters/limesdr_signal_source.cc @@ -0,0 +1,248 @@ +/*! + * \file limesdr_signal_source.cc + * \brief Signal source for LimeSDR front-end + * \author Javier Arribas, 2021. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "limesdr_signal_source.h" +#include "configuration_interface.h" +#include "gnss_sdr_string_literals.h" +#include "gnss_sdr_valve.h" +#include +#include +#include +#include + +using namespace std::string_literals; + +LimesdrSignalSource::LimesdrSignalSource(const ConfigurationInterface* configuration, + const std::string& role, + unsigned int in_stream, + unsigned int out_stream, + Concurrent_Queue* queue) + : SignalSourceBase(configuration, role, "Limesdr_Signal_Source"s), + in_stream_(in_stream), + out_stream_(out_stream) +{ + // DUMP PARAMETERS + const std::string empty; + const std::string default_dump_file("./data/signal_source.dat"); + const std::string default_item_type("gr_complex"); + samples_ = configuration->property(role + ".samples", static_cast(0)); + dump_ = configuration->property(role + ".dump", false); + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_file); + + // Driver parameters + channel_ = configuration->property(role + ".channel", 0); + freq_ = configuration->property(role + ".freq", 1575420000); + gain_ = configuration->property(role + ".gain", 40.0); + sample_rate_ = configuration->property(role + ".sampling_frequency", 2.0e6); + // todo: check if bw is within limits + analog_bw_hz_ = configuration->property(role + ".analog_bw", sample_rate_ / 2); // LPF analog filters in I,Q branches + digital_bw_hz_ = configuration->property(role + ".digital_bw", 0); // disable by default + item_type_ = configuration->property(role + ".item_type", default_item_type); + limesdr_serial_ = configuration->property(role + ".limesdr_serial", std::string()); + limesdr_file_ = configuration->property(role + ".limesdr_file", std::string()); + antenna_ = configuration->property(role + ".antenna", 255); + + PPS_mode_ = configuration->property(role + ".PPS_mode", false); + ext_clock_MHz_ = configuration->property(role + ".ext_clock_MHz", 0.0); // external clock: 0.0 MHz will enable the internal clock + limechannel_mode_ = configuration->property(role + ".limechannel_mode", 0); + if ((limechannel_mode_ < 0) || (limechannel_mode_ > 2)) + { + std::cerr << "ERROR: source_impl::source_impl(): ChannelMode must be A(0), B(1) or (A+B) MIMO(2)\n"; + exit(0); + } + + if (item_type_ == "short") + { + item_size_ = sizeof(int16_t); + } + else if (item_type_ == "gr_complex") + { + item_size_ = sizeof(gr_complex); + // 1. Make the driver instance + try + { +#ifdef LimeSDR_PPS +#ifdef GR_LIMESDR_IS_G38_BRANCH + limesdr_source_ = gr::limesdr::source::make(limesdr_serial_, limechannel_mode_, limesdr_file_, false, PPS_mode_); +#else + limesdr_source_ = gr::limesdr::source::make(limesdr_serial_, limechannel_mode_, limesdr_file_, PPS_mode_); +#endif + if (ext_clock_MHz_ != 0.0) + { + if (limesdr_source_->set_ext_clk(ext_clock_MHz_)) + { + std::cout << "External clock enabled with expected frequency input of " << ext_clock_MHz_ << " MHz\n"; + } + else + { + std::cout << "Error setting external reference clock\n"; + } + } + else + { + limesdr_source_->disable_ext_clk(); + } +#else // LimeSDR_PPS +#ifdef GR_LIMESDR_IS_G38_BRANCH + limesdr_source_ = gr::limesdr::source::make(limesdr_serial_, limechannel_mode_, limesdr_file_, false); +#else + limesdr_source_ = gr::limesdr::source::make(limesdr_serial_, limechannel_mode_, limesdr_file_); +#endif +#endif // LimeSDR_PPS + } + catch (const boost::exception& e) + { + LOG(WARNING) << "Boost exception: " << boost::diagnostic_information(e); + throw std::invalid_argument("Wrong LimeSDR arguments"); + } + + // For LimeSDR: Set RX antenna + /** + * Set which antenna is used + * + * @param antenna Antenna to set: None(0), LNAH(1), LNAL(2), LNAW(3), AUTO(255) + * + * @param channel Channel selection: A(LMS_CH_0),B(LMS_CH_1). + */ + + limesdr_source_->set_antenna(antenna_, channel_); + std::cout << "LimeSDR RX antenna set to " << antenna_ << " for channel " << channel_ << '\n'; + LOG(INFO) << "LimeSDR RX antenna set to " << antenna_ << " for channel " << channel_; + + // 2. set sampling rate + double actual_sample_rate = limesdr_source_->set_sample_rate(sample_rate_); + std::cout << "Actual RX Rate: " << actual_sample_rate << " [SPS]...\n"; + LOG(INFO) << "Actual RX Rate: " << actual_sample_rate << " [SPS]..."; + + // 3. set rx frequency + double actual_center_freq = limesdr_source_->set_center_freq(freq_); + + std::cout << "Actual RX Freq: " << actual_center_freq << " [Hz]...\n"; + LOG(INFO) << "Actual RX Freq: " << actual_center_freq << " [Hz]..."; + + // TODO: Assign the remnant IF from the PLL tune error + std::cout << "PLL Frequency tune error: " << actual_center_freq - freq_ << " [Hz]...\n"; + LOG(INFO) << "PLL Frequency tune error: " << actual_center_freq - freq_ << " [Hz]...\n"; + + // TODO: gr-limesdr does not report PLL tune frequency error... + + // 4. set rx gain + double actual_gain = limesdr_source_->set_gain(gain_, channel_); + std::cout << "Actual RX Gain: " << actual_gain << " [dB]...\n"; + LOG(INFO) << "Actual RX Gain: " << actual_gain << " [dB]..."; + + // Set analog bandwidth + double current_analog_bw = limesdr_source_->set_bandwidth(analog_bw_hz_, channel_); + std::cout << "Actual Analog Bandwidth: " << current_analog_bw << " [Hz]...\n"; + LOG(INFO) << "Actual Analog Bandwidth: : " << current_analog_bw << " [Hz]..."; + + // Set digital bandwidth + limesdr_source_->set_digital_filter(digital_bw_hz_, channel_); + + limesdr_source_->calibrate(sample_rate_ / 2, channel_); + } + else + { + LOG(WARNING) << item_type_ << " unrecognized item type. Using short."; + item_size_ = sizeof(int16_t); + } + + if (samples_ != 0) + { + DLOG(INFO) << "Send STOP signal after " << samples_ << " samples"; + valve_ = gnss_sdr_make_valve(item_size_, samples_, queue); + DLOG(INFO) << "valve(" << valve_->unique_id() << ")"; + } + + if (dump_) + { + DLOG(INFO) << "Dumping output into file " << dump_filename_; + file_sink_ = gr::blocks::file_sink::make(item_size_, dump_filename_.c_str()); + DLOG(INFO) << "file_sink(" << file_sink_->unique_id() << ")"; + } + + if (in_stream_ > 0) + { + LOG(ERROR) << "A signal source does not have an input stream"; + } + if (out_stream_ > 1) + { + LOG(ERROR) << "This implementation only supports one output stream"; + } +} + + +void LimesdrSignalSource::connect(gr::top_block_sptr top_block) +{ + if (samples_ != 0) + { + top_block->connect(limesdr_source_, 0, valve_, 0); + DLOG(INFO) << "connected limesdr source to valve"; + if (dump_) + { + top_block->connect(valve_, 0, file_sink_, 0); + DLOG(INFO) << "connected valve to file sink"; + } + } + else + { + if (dump_) + { + top_block->connect(limesdr_source_, 0, file_sink_, 0); + DLOG(INFO) << "connected limesdr source to file sink"; + } + } +} + + +void LimesdrSignalSource::disconnect(gr::top_block_sptr top_block) +{ + if (samples_ != 0) + { + top_block->disconnect(limesdr_source_, 0, valve_, 0); + if (dump_) + { + top_block->disconnect(valve_, 0, file_sink_, 0); + } + } + else + { + if (dump_) + { + top_block->disconnect(limesdr_source_, 0, file_sink_, 0); + } + } +} + + +gr::basic_block_sptr LimesdrSignalSource::get_left_block() +{ + LOG(WARNING) << "Trying to get signal source left block."; + return gr::basic_block_sptr(); +} + + +gr::basic_block_sptr LimesdrSignalSource::get_right_block() +{ + if (samples_ != 0) + { + return valve_; + } + else + { + return limesdr_source_; + } +} diff --git a/src/algorithms/signal_source/adapters/limesdr_signal_source.h b/src/algorithms/signal_source/adapters/limesdr_signal_source.h new file mode 100644 index 000000000..6f87fa55b --- /dev/null +++ b/src/algorithms/signal_source/adapters/limesdr_signal_source.h @@ -0,0 +1,96 @@ +/*! + * \file limesdr_signal_source.cc + * \brief Signal source for LimeSDR front-end + * \author Javier Arribas, 2021. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_LIMESDR_SIGNAL_SOURCE_H +#define GNSS_SDR_LIMESDR_SIGNAL_SOURCE_H + +#include "concurrent_queue.h" +#include "signal_source_base.h" +#include +#include +#include +#include +#include +#include +#include + +/** \addtogroup Signal_Source + * \{ */ +/** \addtogroup Signal_Source_adapters + * \{ */ + + +class ConfigurationInterface; + +/*! + * \brief This class instantiates the LimeSDR gnuradio signal source. + * It has support also for a customized LimeSDR firmware and signal source to support PPS samplestamp reading. + */ +class LimesdrSignalSource : public SignalSourceBase +{ +public: + LimesdrSignalSource(const ConfigurationInterface* configuration, + const std::string& role, unsigned int in_stream, + unsigned int out_stream, Concurrent_Queue* queue); + + ~LimesdrSignalSource() = default; + + 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; + +private: + gr::limesdr::source::sptr limesdr_source_; + gnss_shared_ptr valve_; + gr::blocks::file_sink::sptr file_sink_; + + std::string role_; + std::string item_type_; + std::string dump_filename_; + std::string limesdr_serial_; + std::string limesdr_file_; + int antenna_; + int channel_; + + // Front-end settings + double sample_rate_; + double freq_; + double gain_; + double analog_bw_hz_; + double digital_bw_hz_; + double ext_clock_MHz_; + size_t item_size_; + int64_t samples_; + + unsigned int in_stream_; + unsigned int out_stream_; + + int limechannel_mode_; + + bool PPS_mode_; + bool dump_; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_LIMESDR_SIGNAL_SOURCE_H diff --git a/src/algorithms/signal_source/libs/CMakeLists.txt b/src/algorithms/signal_source/libs/CMakeLists.txt index fbb1f2911..b9c40a9fb 100644 --- a/src/algorithms/signal_source/libs/CMakeLists.txt +++ b/src/algorithms/signal_source/libs/CMakeLists.txt @@ -25,6 +25,7 @@ set(SIGNAL_SOURCE_LIB_SOURCES rtl_tcp_commands.cc rtl_tcp_dongle_info.cc gnss_sdr_valve.cc + gnss_sdr_timestamp.cc ${OPT_SIGNAL_SOURCE_LIB_SOURCES} ) @@ -68,6 +69,7 @@ target_include_directories(signal_source_libs PUBLIC ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/core/interfaces + ${CMAKE_SOURCE_DIR}/src/algorithms/libs ) if(GNURADIO_USES_STD_POINTERS) diff --git a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc new file mode 100644 index 000000000..aba51c296 --- /dev/null +++ b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.cc @@ -0,0 +1,130 @@ +/*! + * \file gnss_sdr_timestamp.h + * \brief GNURadio block that adds to sample stream timestamp metadata information stored on a sepparated file + * \author Javier Arribas, 2021. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#include "gnss_sdr_timestamp.h" +#include "command_event.h" +#include // for io_signature +#include // for make_any +#include // for mp +#include // for min +#include +#include // for memcpy +#include +#include + + +Gnss_Sdr_Timestamp::Gnss_Sdr_Timestamp(size_t sizeof_stream_item, + std::string timestamp_file, double clock_offset_ms) + : gr::sync_block("Timestamp", + gr::io_signature::make(1, 20, sizeof_stream_item), + gr::io_signature::make(1, 20, sizeof_stream_item)), + d_timefile(std::move(timestamp_file)), + d_clock_offset_ms(clock_offset_ms) +{ + d_fraction_ms_offset = modf(d_clock_offset_ms, &d_integer_ms_offset); // optional clockoffset parameter to convert UTC timestamps to GPS time in some receiver's configuration + get_next_timetag = true; + next_timetag_samplecount = 0; +} + + +gnss_shared_ptr gnss_sdr_make_Timestamp(size_t sizeof_stream_item, std::string timestamp_file, double clock_offset_ms) +{ + gnss_shared_ptr Timestamp_(new Gnss_Sdr_Timestamp(sizeof_stream_item, std::move(timestamp_file), clock_offset_ms)); + return Timestamp_; +} + + +bool Gnss_Sdr_Timestamp::read_next_timetag() +{ + d_timefilestream.read(reinterpret_cast(&next_timetag_samplecount), sizeof(uint64_t)); + if (!d_timefilestream) + { + return false; + } + d_timefilestream.read(reinterpret_cast(&next_timetag.week), sizeof(int32_t)); + if (!d_timefilestream) + { + return false; + } + d_timefilestream.read(reinterpret_cast(&next_timetag.tow_ms), sizeof(int32_t)); + if (!d_timefilestream) + { + return false; + } + return true; +} + + +bool Gnss_Sdr_Timestamp::start() +{ + d_timefilestream.open(d_timefile, std::ios::in | std::ios::binary); + + if (d_timefilestream.is_open() == false) + { + std::cout << "ERROR: Could not open timestamp file: " << d_timefile << "\n"; + return false; + } + else + { + return true; + } +} + + +int64_t Gnss_Sdr_Timestamp::uint64diff(uint64_t first, uint64_t second) +{ + uint64_t abs_diff = (first > second) ? (first - second) : (second - first); + assert(abs_diff <= INT64_MAX); + return (first > second) ? static_cast(abs_diff) : -static_cast(abs_diff); +} + + +int Gnss_Sdr_Timestamp::work(int noutput_items, + gr_vector_const_void_star& input_items, + gr_vector_void_star& output_items) +{ + // multichannel support + if (get_next_timetag == true) + { + if (read_next_timetag() == false) + { + // std::cout << "End of TimeTag file reached!\n"; + // return 0; // todo: find why return -1 does not stop gnss-sdr! + } + get_next_timetag = false; + } + for (size_t ch = 0; ch < output_items.size(); ch++) + { + std::memcpy(output_items[ch], input_items[ch], noutput_items * input_signature()->sizeof_stream_item(ch)); + uint64_t bytes_to_samples = 2; // todo: improve this.. hardcoded 2 bytes -> 1 complex sample! + int64_t diff_samplecount = uint64diff(this->nitems_written(ch), next_timetag_samplecount * bytes_to_samples); + // std::cout << "diff_samplecount: " << diff_samplecount << ", noutput_items: " << noutput_items << "\n"; + if (diff_samplecount <= noutput_items and std::labs(diff_samplecount) <= noutput_items) + { + const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + tmp_obj->tow_ms = next_timetag.tow_ms + static_cast(d_integer_ms_offset); + tmp_obj->week = next_timetag.week; + tmp_obj->tow_ms_fraction = d_fraction_ms_offset; + tmp_obj->rx_time = 0; + add_item_tag(ch, this->nitems_written(ch) - diff_samplecount, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + // std::cout << "[" << this->nitems_written(ch) - diff_samplecount << "] Sent TimeTag SC: " << next_timetag_samplecount * bytes_to_samples << ", Week: " << next_timetag.week << ", TOW: " << next_timetag.tow_ms << " [ms] \n"; + get_next_timetag = true; + } + } + + return noutput_items; +} diff --git a/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h new file mode 100644 index 000000000..2bd9e1f98 --- /dev/null +++ b/src/algorithms/signal_source/libs/gnss_sdr_timestamp.h @@ -0,0 +1,78 @@ +/*! + * \file gnss_sdr_timestamp.h + * \brief GNURadio block that adds to sample stream timestamp metadata information stored on a sepparated file + * \author Javier Arribas, 2021. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * GNSS-SDR is a Global Navigation Satellite System software-defined receiver. + * This file is part of GNSS-SDR. + * + * Copyright (C) 2010-2021 (see AUTHORS file for a list of contributors) + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GNSS_SDR_TIMESTAMP_H +#define GNSS_SDR_GNSS_SDR_TIMESTAMP_H + +#include "gnss_block_interface.h" +#include "gnss_time.h" +#include // for sync_block +#include // for gr_vector_const_void_star +#include +#include // for size_t +#include +#include +#include + +/** \addtogroup Signal_Source + * \{ */ +/** \addtogroup Signal_Source_libs + * \{ */ + + +class Gnss_Sdr_Timestamp; + +gnss_shared_ptr gnss_sdr_make_Timestamp( + size_t sizeof_stream_item, + std::string timestamp_file, + double clock_offset_ms); + + +class Gnss_Sdr_Timestamp : public gr::sync_block +{ +public: + int work(int noutput_items, + gr_vector_const_void_star& input_items, + gr_vector_void_star& output_items); + bool start(); + +private: + friend gnss_shared_ptr gnss_sdr_make_Timestamp( + size_t sizeof_stream_item, + std::string timestamp_file, + double clock_offset_ms); + + Gnss_Sdr_Timestamp(size_t sizeof_stream_item, + std::string timestamp_file, + double clock_offset_ms); + + int64_t uint64diff(uint64_t first, uint64_t second); + bool read_next_timetag(); + std::string d_timefile; + std::fstream d_timefilestream; + GnssTime next_timetag; + double d_clock_offset_ms; + double d_integer_ms_offset; + double d_fraction_ms_offset; + uint64_t next_timetag_samplecount; + bool get_next_timetag; +}; + + +/** \} */ +/** \} */ +#endif // GNSS_SDR_GNSS_SDR_TIMESTAMP_H diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt index b47f56187..91d80c257 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/CMakeLists.txt @@ -52,6 +52,7 @@ target_link_libraries(telemetry_decoder_gr_blocks PUBLIC telemetry_decoder_libswiftcnav telemetry_decoder_libs + algorithms_libs core_libs core_system_parameters Gnuradio::runtime diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index b23ce7c96..0085c8108 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -685,6 +685,50 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( } } d_sample_counter++; // count for the processed symbols + + // Time Tags from signal source (optional feature) + std::vector tags_vec; + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + 1); // telemetry decoder consumes symbols one-by-one + if (!tags_vec.empty()) + { + for (const auto &it : tags_vec) + { + try + { + if (pmt::any_ref(it.value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const auto timetag = boost::any_cast>(pmt::any_ref(it.value)); + // std::cout << "Old tow: " << d_current_timetag.tow_ms << " new tow: " << timetag->tow_ms << "\n"; + d_current_timetag = *timetag; + d_valid_timetag = true; + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + } + else + { + if (d_valid_timetag == true) + { + // propagate timetag to current symbol + // todo: tag rx_time is set only in the time channel. The tracking tag does not have valid rx_time (it is not required since it is associated to the current symbol) + // d_current_timetag.rx_time+=d_PRN_code_period_ms + d_current_timetag.tow_ms += d_PRN_code_period_ms; + if (d_current_timetag.tow_ms >= 604800000) + { + d_current_timetag.tow_ms -= 604800000; + d_current_timetag.week++; + } + } + } + consume_each(1); d_flag_preamble = false; @@ -873,6 +917,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW5() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW5_flag(false); + // timetag debug + if (d_valid_timetag == true) + { + int decoder_delay_ms = static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); + int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms; + if (rx_tow_at_preamble < 0) + { + rx_tow_at_preamble += 604800000; + } + uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000); // floor to integer number of seconds + std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n"; + } } else if (d_inav_nav.is_TOW6_set() == 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) @@ -881,6 +937,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW6() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW6_flag(false); + // timetag debug + if (d_valid_timetag == true) + { + int decoder_delay_ms = static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); + int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms; + if (rx_tow_at_preamble < 0) + { + rx_tow_at_preamble += 604800000; + } + uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000); // floor to integer number of seconds + std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n"; + } } else if (d_inav_nav.is_TOW0_set() == true) // page 0 arrived and decoded { @@ -888,6 +956,18 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( d_TOW_at_Preamble_ms = static_cast(d_inav_nav.get_TOW0() * 1000.0); d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); d_inav_nav.set_TOW0_flag(false); + // timetag debug + if (d_valid_timetag == true) + { + int decoder_delay_ms = static_cast(GALILEO_INAV_PAGE_PART_MS + (d_required_symbols + 1) * d_PRN_code_period_ms); + int rx_tow_at_preamble = d_current_timetag.tow_ms - decoder_delay_ms; + if (rx_tow_at_preamble < 0) + { + rx_tow_at_preamble += 604800000; + } + uint32_t predicted_tow_at_preamble_ms = 1000 * (rx_tow_at_preamble / 1000); // floor to integer number of seconds + std::cout << "TOW at PREAMBLE: " << d_TOW_at_Preamble_ms << " predicted TOW at preamble: " << predicted_tow_at_preamble_ms << " [ms]\n"; + } } else { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index 85b95c875..cb71472af 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -26,6 +26,7 @@ #include "galileo_inav_message.h" // for Galileo_Inav_Message #include "gnss_block_interface.h" // for gnss_shared_ptr (adapts smart pointer type to GNU Radio version) #include "gnss_satellite.h" // for Gnss_Satellite +#include "gnss_time.h" // for GnssTime #include "nav_message_packet.h" // for Nav_Message_Packet #include "tlm_conf.h" // for Tlm_Conf #include // for boost::circular_buffer @@ -101,6 +102,8 @@ private: Galileo_Fnav_Message d_fnav_nav; Nav_Message_Packet d_nav_msg_packet; + GnssTime d_current_timetag; + std::unique_ptr d_Tlm_CRC_Stats; double d_delta_t; // GPS-GALILEO time offset @@ -143,6 +146,7 @@ private: bool d_enable_navdata_monitor; bool d_dump_crc_stats; bool d_enable_reed_solomon_inav; + bool d_valid_timetag; }; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index cc2307ae8..e73bba790 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -35,6 +35,7 @@ #include // for exception #include // for cout #include // for shared_ptr +#include #ifdef COMPILER_HAS_ROTL #include @@ -114,17 +115,18 @@ gps_l1_ca_telemetry_decoder_gs::gps_l1_ca_telemetry_decoder_gs( if (GPS_CA_PREAMBLE[i] == '1') { d_preamble_samples[n] = 1; - n++; } else { d_preamble_samples[n] = -1; - n++; } + n++; } d_symbol_history.set_capacity(d_required_symbols); + set_tag_propagation_policy(TPP_DONT); // no tag propagation, the time tag will be adjusted and regenerated in work() + if (d_dump_crc_stats) { // initialize the telemetry CRC statistics class @@ -440,6 +442,7 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ } // add new symbol to the symbol queue d_symbol_history.push_back(current_symbol.Prompt_I); + d_sample_counter++; // count for the processed symbols consume_each(1); d_flag_preamble = false; @@ -593,6 +596,31 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ current_symbol.Flag_PLL_180_deg_phase_locked = false; } + // time tags + std::vector tags_vec; + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + 1); + for (const auto &it : tags_vec) + { + try + { + if (pmt::any_ref(it.value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const auto timetag = boost::any_cast>(pmt::any_ref(it.value)); + // std::cout << "[" << this->nitems_written(0) + 1 << "] TLM RX TimeTag Week: " << timetag->week << ", TOW: " << timetag->tow_ms << " [ms], TOW fraction: " << timetag->tow_ms_fraction + // << " [ms], DELTA TLM TOW: " << static_cast(timetag->tow_ms - current_symbol.TOW_at_current_symbol_ms) + timetag->tow_ms_fraction << " [ms] \n"; + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(timetag)); + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + } + if (d_dump == true) { // MULTIPLEXED FILE RECORDING - Record results to file diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h index a98d7a55f..da9dbc50d 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h @@ -16,11 +16,11 @@ #ifndef GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_GS_H #define GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_GS_H - #include "GPS_L1_CA.h" #include "gnss_block_interface.h" #include "gnss_satellite.h" #include "gnss_synchro.h" +#include "gnss_time.h" // for timetags produced by Tracking #include "gps_navigation_message.h" #include "nav_message_packet.h" #include "tlm_conf.h" diff --git a/src/algorithms/tracking/adapters/CMakeLists.txt b/src/algorithms/tracking/adapters/CMakeLists.txt index 374415c77..b4110e1c0 100644 --- a/src/algorithms/tracking/adapters/CMakeLists.txt +++ b/src/algorithms/tracking/adapters/CMakeLists.txt @@ -51,6 +51,7 @@ set(TRACKING_ADAPTER_SOURCES glonass_l2_ca_dll_pll_c_aid_tracking.cc beidou_b1i_dll_pll_tracking.cc beidou_b3i_dll_pll_tracking.cc + gps_l1_ca_kf_vtl_tracking.cc ${OPT_TRACKING_ADAPTERS_SOURCES} ) @@ -71,6 +72,7 @@ set(TRACKING_ADAPTER_HEADERS glonass_l2_ca_dll_pll_c_aid_tracking.h beidou_b1i_dll_pll_tracking.h beidou_b3i_dll_pll_tracking.h + gps_l1_ca_kf_vtl_tracking.h ${OPT_TRACKING_ADAPTERS_HEADERS} ) diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.cc new file mode 100644 index 000000000..b0a3c3b85 --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.cc @@ -0,0 +1,142 @@ +/*! + * \file gps_l1_ca_kf_vtl_tracking.h + * \brief Interface of an adapter of a code + carrier Kalman Filter tracking loop with VTL capabilities block + * for GPS L1 C/A to a TrackingInterface + * \author Javier Arribas, 2020. jarribas(at)cttc.es + * + * + * + * ----------------------------------------------------------------------------- + * + * Copyright (C) 2010-2020 (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. + * + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "gps_l1_ca_kf_vtl_tracking.h" +#include "GPS_L1_CA.h" +#include "configuration_interface.h" +#include "display.h" +#include "gnss_sdr_flags.h" +#include "kf_conf.h" +#include +#include + +GpsL1CaKfVtlTracking::GpsL1CaKfVtlTracking( + const ConfigurationInterface* configuration, const std::string& role, + unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) +{ + Kf_Conf trk_params = Kf_Conf(); + DLOG(INFO) << "role " << role; + trk_params.SetFromConfiguration(configuration, role); + + const auto vector_length = static_cast(std::round(trk_params.fs_in / (GPS_L1_CA_CODE_RATE_CPS / GPS_L1_CA_CODE_LENGTH_CHIPS))); + trk_params.vector_length = vector_length; + if (trk_params.extend_correlation_symbols < 1) + { + trk_params.extend_correlation_symbols = 1; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be bigger than 1. Coherent integration has been set to 1 symbol (1 ms)" << TEXT_RESET << '\n'; + } + else if (trk_params.extend_correlation_symbols > 20) + { + trk_params.extend_correlation_symbols = 20; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A. extend_correlation_symbols must be lower than 21. Coherent integration has been set to 20 symbols (20 ms)" << TEXT_RESET << '\n'; + } + trk_params.track_pilot = configuration->property(role + ".track_pilot", false); + if (trk_params.track_pilot) + { + trk_params.track_pilot = false; + std::cout << TEXT_RED << "WARNING: GPS L1 C/A does not have pilot signal. Data tracking has been enabled" << TEXT_RESET << '\n'; + } + + trk_params.system = 'G'; + const std::array sig_{'1', 'C', '\0'}; + std::memcpy(trk_params.signal, sig_.data(), 3); + + // ################# Make a GNU Radio Tracking block object ################ + if (trk_params.item_type == "gr_complex") + { + item_size_ = sizeof(gr_complex); + tracking_ = kf_vtl_make_tracking(trk_params); + } + else + { + item_size_ = sizeof(gr_complex); + LOG(WARNING) << trk_params.item_type << " unknown tracking item type."; + } + channel_ = 0; + DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")"; + if (in_streams_ > 1) + { + LOG(ERROR) << "This implementation only supports one input stream"; + } + if (out_streams_ > 1) + { + LOG(ERROR) << "This implementation only supports one output stream"; + } +} + + +void GpsL1CaKfVtlTracking::stop_tracking() +{ + tracking_->stop_tracking(); +} + + +void GpsL1CaKfVtlTracking::start_tracking() +{ + tracking_->start_tracking(); +} + + +/* + * Set tracking channel unique ID + */ +void GpsL1CaKfVtlTracking::set_channel(unsigned int channel) +{ + channel_ = channel; + tracking_->set_channel(channel); +} + + +void GpsL1CaKfVtlTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) +{ + tracking_->set_gnss_synchro(p_gnss_synchro); +} + + +void GpsL1CaKfVtlTracking::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 GpsL1CaKfVtlTracking::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 GpsL1CaKfVtlTracking::get_left_block() +{ + return tracking_; +} + + +gr::basic_block_sptr GpsL1CaKfVtlTracking::get_right_block() +{ + return tracking_; +} diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.h new file mode 100644 index 000000000..af011e917 --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_kf_vtl_tracking.h @@ -0,0 +1,93 @@ +/*! + * \file gps_l1_ca_kf_vtl_tracking.h + * \brief Interface of an adapter of a code + carrier Kalman Filter tracking loop with VTL capabilities block + * for GPS L1 C/A to a TrackingInterface + * \author Javier Arribas, 2020. jarribas(at)cttc.es + * + * + * ----------------------------------------------------------------------------- + * + * Copyright (C) 2010-2020 (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. + * + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_GPS_L1_CA_kf_vtl_TRACKING_H +#define GNSS_SDR_GPS_L1_CA_kf_vtl_TRACKING_H + +#include "kf_vtl_tracking.h" +#include "tracking_interface.h" +#include + +class ConfigurationInterface; + +/*! + * \brief This class implements a code + carrier Kalman Filter tracking loop with VTL capabilities + */ +class GpsL1CaKfVtlTracking : public TrackingInterface +{ +public: + GpsL1CaKfVtlTracking( + const ConfigurationInterface* configuration, + const std::string& role, + unsigned int in_streams, + unsigned int out_streams); + + ~GpsL1CaKfVtlTracking() = default; + + inline std::string role() override + { + return role_; + } + + //! Returns "GPS_L1_CA_kf_vtl_Tracking" + inline std::string implementation() override + { + return "GPS_L1_CA_KF_VTL_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; + + /*! + * \brief Stop running tracking + */ + void stop_tracking() override; + +private: + kf_vtl_tracking_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_vtl_TRACKING_H diff --git a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt index 6290cb842..e397f6c40 100644 --- a/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/tracking/gnuradio_blocks/CMakeLists.txt @@ -39,6 +39,7 @@ set(TRACKING_GR_BLOCKS_SOURCES glonass_l2_ca_dll_pll_c_aid_tracking_cc.cc glonass_l2_ca_dll_pll_c_aid_tracking_sc.cc dll_pll_veml_tracking.cc + kf_vtl_tracking.cc ${OPT_TRACKING_BLOCKS_SOURCES} ) @@ -53,6 +54,7 @@ set(TRACKING_GR_BLOCKS_HEADERS glonass_l2_ca_dll_pll_c_aid_tracking_cc.h glonass_l2_ca_dll_pll_c_aid_tracking_sc.h dll_pll_veml_tracking.h + kf_vtl_tracking.h ${OPT_TRACKING_BLOCKS_HEADERS} ) 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 733b8b6fd..c1b6a69d5 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -57,6 +57,7 @@ #include // for exception #include // for cout, cerr #include +#include #include #include @@ -103,7 +104,6 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) d_code_phase_step_chips(0.0), d_code_phase_rate_step_chips(0.0), d_rem_code_phase_samples(0.0), // Residual code phase (in chips) - d_sample_counter(0ULL), d_acq_sample_stamp(0ULL), d_rem_carr_phase_rad(0.0), // Residual carrier phase d_state(0), // initial state: standby @@ -593,6 +593,9 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) d_dump = false; } } + d_last_timetag_samplecounter = 0; + d_timetag_waiting = false; + set_tag_propagation_policy(TPP_DONT); // no tag propagation, the time tag will be adjusted and regenerated in work() } @@ -625,6 +628,10 @@ void dll_pll_veml_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) { LOG(WARNING) << "msg_handler_telemetry_to_trk Bad any_cast: " << e.what(); } + catch (std::exception &ex) + { + LOG(WARNING) << "msg_handler_telemetry_to_trk Bad any_cast: " << ex.what(); + } } @@ -1403,7 +1410,7 @@ void dll_pll_veml_tracking::log_data() d_dump_file.write(reinterpret_cast(&prompt_I), sizeof(float)); d_dump_file.write(reinterpret_cast(&prompt_Q), sizeof(float)); // PRN start sample stamp - tmp_long_int = d_sample_counter + static_cast(d_current_prn_length_samples); + tmp_long_int = this->nitems_read(0) + static_cast(d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); // accumulated carrier phase tmp_float = static_cast(d_acc_carrier_phase_rad); @@ -1437,7 +1444,7 @@ void dll_pll_veml_tracking::log_data() // AUX vars (for debug purposes) tmp_float = static_cast(d_rem_code_phase_samples); d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + tmp_double = static_cast(this->nitems_read(0) + d_current_prn_length_samples); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); // PRN uint32_t prn_ = d_acquisition_gnss_synchro->PRN; @@ -1696,6 +1703,14 @@ void dll_pll_veml_tracking::stop_tracking() } +int64_t dll_pll_veml_tracking::uint64diff(uint64_t first, uint64_t second) +{ + uint64_t abs_diff = (first > second) ? (first - second) : (second - first); + assert(abs_diff <= INT64_MAX); + return (first > second) ? static_cast(abs_diff) : -static_cast(abs_diff); +} + + int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { @@ -1708,7 +1723,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) if (d_pull_in_transitory == true) { - if (d_trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + // if (d_trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + if (d_trk_parameters.pull_in_time_s < (this->nitems_read(0) - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) { d_pull_in_transitory = false; d_carrier_lock_fail_counter = 0; @@ -1719,7 +1735,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) { case 0: // Standby - Consume samples at full throttle, do nothing { - d_sample_counter += static_cast(ninput_items[0]); + // d_sample_counter += static_cast(ninput_items[0]); consume_each(ninput_items[0]); return 0; break; @@ -1727,7 +1743,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) case 1: // Pull-in { // Signal alignment (skip samples until the incoming signal is aligned with local replica) - const int64_t acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); + // const int64_t acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); + const int64_t acq_trk_diff_samples = static_cast(this->nitems_read(0)) - static_cast(d_acq_sample_stamp); const double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_trk_parameters.fs_in; const double delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; @@ -1744,7 +1761,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) const int32_t samples_offset = round(d_acq_code_phase_samples); d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); d_state = 2; - d_sample_counter += samples_offset; // count for the processed samples + // d_sample_counter += samples_offset; // count for the processed samples d_cn0_smoother.reset(); d_carrier_lock_test_smoother.reset(); @@ -1775,7 +1792,8 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) // } // fail-safe: check if the secondary code or bit synchronization has not succeeded in a limited time period - if (d_trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + // if (d_trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + if (d_trk_parameters.bit_synchronization_time_limit_s < (this->nitems_read(0) - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) { d_carrier_lock_fail_counter = 300000; // force loss-of-lock condition LOG(INFO) << d_systemName << " " << d_signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel @@ -1995,15 +2013,67 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused) } } + // time tags + std::vector tags_vec; + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + d_current_prn_length_samples); + for (const auto &it : tags_vec) + { + try + { + if (pmt::any_ref(it.value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + // std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. counter " << d_sample_counter << " vs. nread " << this->nitems_read(0) << " containing "; + // std::cout << "ch[" << d_acquisition_gnss_synchro->Channel_ID << "] tracking time tag with offset " << it->offset << " vs. nread " << this->nitems_read(0) << " containing "; + const auto last_timetag = boost::any_cast>(pmt::any_ref(it.value)); + d_last_timetag = *last_timetag; + d_last_timetag_samplecounter = it.offset; + d_timetag_waiting = true; + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + catch (std::exception &ex) + { + LOG(WARNING) << "Bad any_cast: " << ex.what(); + } + } + consume_each(d_current_prn_length_samples); - d_sample_counter += static_cast(d_current_prn_length_samples); + // d_sample_counter += static_cast(d_current_prn_length_samples); if (current_synchro_data.Flag_valid_symbol_output || loss_of_lock) { current_synchro_data.fs = static_cast(d_trk_parameters.fs_in); - current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Tracking_sample_counter = this->nitems_read(0); current_synchro_data.Flag_valid_symbol_output = !loss_of_lock; current_synchro_data.Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked; *out[0] = current_synchro_data; + + // generate new tag associated with gnss-synchro object + + if (d_timetag_waiting == true) + { + int64_t diff_samplecount = uint64diff(current_synchro_data.Tracking_sample_counter, d_last_timetag_samplecounter); + + double intpart; + d_last_timetag.tow_ms_fraction = d_last_timetag.tow_ms_fraction + modf(1000.0 * static_cast(diff_samplecount) / d_trk_parameters.fs_in, &intpart); + + const std::shared_ptr tmp_obj = std::make_shared(GnssTime()); + tmp_obj->week = d_last_timetag.week; + tmp_obj->tow_ms = d_last_timetag.tow_ms + static_cast(intpart); + tmp_obj->tow_ms_fraction = d_last_timetag.tow_ms_fraction; + tmp_obj->rx_time = static_cast(current_synchro_data.Tracking_sample_counter) / d_trk_parameters.fs_in; + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(tmp_obj)); + + // std::cout << "[" << this->nitems_written(0) + 1 << "][diff_time: " << 1000.0 * static_cast(diff_samplecount) / d_trk_parameters.fs_in << "] Sent TimeTag Week: " << d_last_timetag.week << ", TOW: " << d_last_timetag.tow_ms << " [ms], TOW fraction: " << d_last_timetag.tow_ms_fraction << " [ms] \n"; + d_timetag_waiting = false; + } + return 1; } return 0; 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 a7e914e72..8824cb4db 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h @@ -22,6 +22,7 @@ #include "dll_pll_conf.h" #include "exponential_smoother.h" #include "gnss_block_interface.h" +#include "gnss_time.h" // for timetags produced by File_Timestamp_Signal_Source #include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter #include "tracking_loop_filter.h" // for DLL filter #include @@ -83,6 +84,7 @@ private: void log_data(); bool cn0_and_tracking_lock_status(double coh_integration_time_s); bool acquire_secondary(); + int64_t uint64diff(uint64_t first, uint64_t second); int32_t save_matfile() const; Cpu_Multicorrelator_Real_Codes d_multicorrelator_cpu; @@ -163,8 +165,11 @@ private: std::ofstream d_dump_file; - uint64_t d_sample_counter; + // uint64_t d_sample_counter; uint64_t d_acq_sample_stamp; + GnssTime d_last_timetag; + uint64_t d_last_timetag_samplecounter; + bool d_timetag_waiting; float *d_prompt_data_shift; float d_rem_carr_phase_rad; diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc new file mode 100644 index 000000000..41129231f --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc @@ -0,0 +1,2081 @@ +/*! + * \file kf_vtl_tracking.cc + * \brief Implementation of a Kalman filter based tracking with optional Vector + * Tracking Loop message receiver block. + * \author Javier Arribas, 2020. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * Copyright (C) 2010-2020 (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. + * + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "kf_vtl_tracking.h" +#include "Beidou_B1I.h" +#include "Beidou_B3I.h" +#include "GPS_L1_CA.h" +#include "GPS_L2C.h" +#include "GPS_L5.h" +#include "Galileo_E1.h" +#include "Galileo_E5a.h" +#include "Galileo_E5b.h" +#include "MATH_CONSTANTS.h" +#include "beidou_b1i_signal_replica.h" +#include "beidou_b3i_signal_replica.h" +#include "galileo_e1_signal_replica.h" +#include "galileo_e5_signal_replica.h" +#include "galileo_e6_signal_replica.h" +#include "gnss_satellite.h" +#include "gnss_sdr_create_directory.h" +#include "gnss_synchro.h" +#include "gps_l2c_signal_replica.h" +#include "gps_l5_signal_replica.h" +#include "gps_sdr_signal_replica.h" +#include "lock_detectors.h" +#include "tracking_discriminators.h" +#include "trackingcmd.h" +#include +#include // for io_signature +#include // for scoped_lock +#include // for Mat_VarCreate +#include // for mp +#include +#include // for fill_n +#include +#include // for fmod, round, floor +#include // for exception +#include // for cout, cerr +#include +#include +#include + +#if HAS_GENERIC_LAMBDA +#else +#include +#endif + +#if HAS_STD_FILESYSTEM +#if HAS_STD_FILESYSTEM_EXPERIMENTAL +#include +namespace fs = std::experimental::filesystem; +#else +#include +namespace fs = std::filesystem; +#endif +#else +#include +namespace fs = boost::filesystem; +#endif + + +kf_vtl_tracking_sptr kf_vtl_make_tracking(const Kf_Conf &conf_) +{ + return kf_vtl_tracking_sptr(new kf_vtl_tracking(conf_)); +} + + +kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_) + : gr::block("kf_vtl_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) +{ + // prevent telemetry symbols accumulation in output buffers + this->set_max_noutput_items(1); + d_trk_parameters = conf_; + // Telemetry bit synchronization message port input + this->message_port_register_out(pmt::mp("events")); + this->set_relative_rate(1.0 / static_cast(d_trk_parameters.vector_length)); + + // Telemetry message port input + this->message_port_register_in(pmt::mp("telemetry_to_trk")); + this->set_msg_handler( + pmt::mp("telemetry_to_trk"), +#if HAS_GENERIC_LAMBDA + [this](auto &&PH1) { msg_handler_telemetry_to_trk(PH1); }); +#else +#if USE_BOOST_BIND_PLACEHOLDERS + boost::bind(&kf_vtl_tracking::msg_handler_telemetry_to_trk, this, boost::placeholders::_1)); +#else + boost::bind(&kf_vtl_tracking::msg_handler_telemetry_to_trk, this, _1)); +#endif +#endif + + // PVT message port input + this->message_port_register_in(pmt::mp("pvt_to_trk")); + this->set_msg_handler( + pmt::mp("pvt_to_trk"), +#if HAS_GENERIC_LAMBDA + [this](auto &&PH1) { msg_handler_pvt_to_trk(PH1); }); +#else +#if USE_BOOST_BIND_PLACEHOLDERS + boost::bind(&kf_vtl_tracking::msg_handler_pvt_to_trk, this, boost::placeholders::_1)); +#else + boost::bind(&kf_vtl_tracking::msg_handler_pvt_to_trk, this, _1)); +#endif +#endif + + // initialize internal vars + d_veml = false; + d_cloop = true; + d_pull_in_transitory = true; + d_code_chip_rate = 0.0; + d_secondary_code_length = 0U; + d_data_secondary_code_length = 0U; + d_preamble_length_symbols = 0; + d_interchange_iq = false; + d_signal_type = std::string(d_trk_parameters.signal); + + std::map map_signal_pretty_name; + map_signal_pretty_name["1C"] = "L1 C/A"; + map_signal_pretty_name["1B"] = "E1"; + map_signal_pretty_name["1G"] = "L1 C/A"; + map_signal_pretty_name["2S"] = "L2C"; + map_signal_pretty_name["2G"] = "L2 C/A"; + map_signal_pretty_name["5X"] = "E5a"; + map_signal_pretty_name["7X"] = "E5b"; + map_signal_pretty_name["L5"] = "L5"; + map_signal_pretty_name["B1"] = "B1I"; + map_signal_pretty_name["B3"] = "B3I"; + + d_signal_pretty_name = map_signal_pretty_name[d_signal_type]; + + if (d_trk_parameters.system == 'G') + { + d_systemName = "GPS"; + if (d_signal_type == "1C") + { + d_signal_carrier_freq = GPS_L1_FREQ_HZ; + d_code_period = GPS_L1_CA_CODE_PERIOD_S; + d_code_chip_rate = GPS_L1_CA_CODE_RATE_CPS; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS); + // GPS L1 C/A does not have pilot component nor secondary code + d_secondary = false; + d_trk_parameters.track_pilot = false; + d_trk_parameters.slope = 1.0; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + d_trk_parameters.y_intercept = 1.0; + // symbol integration: 20 trk symbols (20 ms) = 1 tlm bit + // set the preamble in the secondary code acquisition to obtain tlm symbol synchronization + d_secondary_code_length = static_cast(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = GPS_CA_PREAMBLE_SYMBOLS_STR; + d_symbols_per_bit = GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; + } + else if (d_signal_type == "2S") + { + d_signal_carrier_freq = GPS_L2_FREQ_HZ; + d_code_period = GPS_L2_M_PERIOD_S; + d_code_chip_rate = GPS_L2_M_CODE_RATE_CPS; + d_code_length_chips = static_cast(GPS_L2_M_CODE_LENGTH_CHIPS); + // GPS L2C has 1 trk symbol (20 ms) per tlm bit, no symbol integration required + d_symbols_per_bit = GPS_L2_SAMPLES_PER_SYMBOL; + d_correlation_length_ms = 20; + d_code_samples_per_chip = 1; + // GPS L2 does not have pilot component nor secondary code + d_secondary = false; + d_trk_parameters.track_pilot = false; + d_trk_parameters.slope = 1.0; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + d_trk_parameters.y_intercept = 1.0; + } + else if (d_signal_type == "L5") + { + d_signal_carrier_freq = GPS_L5_FREQ_HZ; + d_code_period = GPS_L5I_PERIOD_S; + d_code_chip_rate = GPS_L5I_CODE_RATE_CPS; + // symbol integration: 10 trk symbols (10 ms) = 1 tlm bit + d_symbols_per_bit = GPS_L5_SAMPLES_PER_SYMBOL; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(GPS_L5I_CODE_LENGTH_CHIPS); + d_secondary = true; + d_trk_parameters.slope = 1.0; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + d_trk_parameters.y_intercept = 1.0; + if (d_trk_parameters.track_pilot) + { + // synchronize pilot secondary code + d_secondary_code_length = static_cast(GPS_L5Q_NH_CODE_LENGTH); + d_secondary_code_string = GPS_L5Q_NH_CODE_STR; + // remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) + d_data_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_data_secondary_code_string = GPS_L5I_NH_CODE_STR; + d_signal_pretty_name = d_signal_pretty_name + "Q"; + } + else + { + // synchronize and remove data secondary code + // remove Neuman-Hofman Code (see IS-GPS-705D) + d_secondary_code_length = static_cast(GPS_L5I_NH_CODE_LENGTH); + d_secondary_code_string = GPS_L5I_NH_CODE_STR; + d_signal_pretty_name = d_signal_pretty_name + "I"; + d_interchange_iq = true; + } + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cerr << "Invalid Signal argument when instantiating tracking blocks\n"; + d_correlation_length_ms = 1; + d_secondary = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0U; + d_symbols_per_bit = 0; + } + } + else if (d_trk_parameters.system == 'E') + { + d_systemName = "Galileo"; + if (d_signal_type == "1B") + { + d_signal_carrier_freq = GALILEO_E1_FREQ_HZ; + d_code_period = GALILEO_E1_CODE_PERIOD_S; + d_code_chip_rate = GALILEO_E1_CODE_CHIP_RATE_CPS; + d_code_length_chips = static_cast(GALILEO_E1_B_CODE_LENGTH_CHIPS); + // Galileo E1b has 1 trk symbol (4 ms) per tlm bit, no symbol integration required + d_symbols_per_bit = 1; + d_correlation_length_ms = 4; + d_code_samples_per_chip = 2; // CBOC disabled: 2 samples per chip. CBOC enabled: 12 samples per chip + d_veml = true; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + d_trk_parameters.slope = static_cast(-CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc)); + d_trk_parameters.y_intercept = static_cast(GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc)); + if (d_trk_parameters.track_pilot) + { + d_secondary = true; + d_secondary_code_length = static_cast(GALILEO_E1_C_SECONDARY_CODE_LENGTH); + d_secondary_code_string = GALILEO_E1_C_SECONDARY_CODE; + d_signal_pretty_name = d_signal_pretty_name + "C"; + } + else + { + d_secondary = false; + d_signal_pretty_name = d_signal_pretty_name + "B"; + } + // Note that E1-B and E1-C are in anti-phase, NOT IN QUADRATURE. See Galileo ICD. + } + else if (d_signal_type == "5X") + { + d_signal_carrier_freq = GALILEO_E5A_FREQ_HZ; + d_code_period = GALILEO_E5A_CODE_PERIOD_S; + d_code_chip_rate = GALILEO_E5A_CODE_CHIP_RATE_CPS; + d_symbols_per_bit = 20; + 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; + d_trk_parameters.slope = 1.0; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + d_trk_parameters.y_intercept = 1.0; + if (d_trk_parameters.track_pilot) + { + // synchronize pilot secondary code + d_secondary_code_length = static_cast(GALILEO_E5A_Q_SECONDARY_CODE_LENGTH); + d_signal_pretty_name = d_signal_pretty_name + "Q"; + // remove data secondary code + d_data_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = GALILEO_E5A_I_SECONDARY_CODE; + } + else + { + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(GALILEO_E5A_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = GALILEO_E5A_I_SECONDARY_CODE; + d_signal_pretty_name = d_signal_pretty_name + "I"; + d_interchange_iq = true; + } + } + else if (d_signal_type == "7X") + { + d_signal_carrier_freq = GALILEO_E5B_FREQ_HZ; + d_code_period = GALILEO_E5B_CODE_PERIOD_S; + d_code_chip_rate = GALILEO_E5B_CODE_CHIP_RATE_CPS; + d_symbols_per_bit = 4; + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_code_length_chips = static_cast(GALILEO_E5B_CODE_LENGTH_CHIPS); + d_secondary = true; + d_trk_parameters.slope = 1.0; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + d_trk_parameters.y_intercept = 1.0; + if (d_trk_parameters.track_pilot) + { + // synchronize pilot secondary code + d_secondary_code_length = static_cast(GALILEO_E5B_Q_SECONDARY_CODE_LENGTH); + d_signal_pretty_name = d_signal_pretty_name + "Q"; + // remove data secondary code + d_data_secondary_code_length = static_cast(GALILEO_E5B_I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = GALILEO_E5B_I_SECONDARY_CODE; + } + else + { + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(GALILEO_E5B_I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = GALILEO_E5B_I_SECONDARY_CODE; + d_signal_pretty_name = d_signal_pretty_name + "I"; + d_interchange_iq = true; + } + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks\n"; + d_correlation_length_ms = 1; + d_secondary = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0U; + d_symbols_per_bit = 0; + } + } + else if (d_trk_parameters.system == 'C') + { + d_systemName = "Beidou"; + if (d_signal_type == "B1") + { + // GEO Satellites use different secondary code + d_signal_carrier_freq = BEIDOU_B1I_FREQ_HZ; + d_code_period = BEIDOU_B1I_CODE_PERIOD_S; + d_code_chip_rate = BEIDOU_B1I_CODE_RATE_CPS; + d_code_length_chips = static_cast(BEIDOU_B1I_CODE_LENGTH_CHIPS); + d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = true; + d_trk_parameters.track_pilot = false; + d_trk_parameters.slope = 1.0; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + d_trk_parameters.y_intercept = 1.0; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = BEIDOU_B1I_SECONDARY_CODE_STR; + d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = BEIDOU_B1I_SECONDARY_CODE_STR; + } + else if (d_signal_type == "B3") + { + // GEO Satellites use different secondary code + d_signal_carrier_freq = BEIDOU_B3I_FREQ_HZ; + d_code_period = BEIDOU_B3I_CODE_PERIOD_S; + d_code_chip_rate = BEIDOU_B3I_CODE_RATE_CPS; + d_code_length_chips = static_cast(BEIDOU_B3I_CODE_LENGTH_CHIPS); + d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + d_trk_parameters.track_pilot = false; + d_trk_parameters.slope = 1.0; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + d_trk_parameters.y_intercept = 1.0; + d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = BEIDOU_B3I_SECONDARY_CODE_STR; + d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = BEIDOU_B3I_SECONDARY_CODE_STR; + } + else + { + LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks"; + std::cout << "Invalid Signal argument when instantiating tracking blocks\n"; + d_correlation_length_ms = 1; + d_secondary = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0; + d_symbols_per_bit = 0; + } + } + else + { + LOG(WARNING) << "Invalid System argument when instantiating tracking blocks"; + std::cerr << "Invalid System argument when instantiating tracking blocks\n"; + d_correlation_length_ms = 1; + d_secondary = false; + d_signal_carrier_freq = 0.0; + d_code_period = 0.0; + d_code_length_chips = 0; + d_code_samples_per_chip = 0U; + d_symbols_per_bit = 0; + } + d_T_chip_seconds = 0.0; + d_T_prn_seconds = 0.0; + d_T_prn_samples = 0.0; + d_K_blk_samples = 0.0; + + // Initialize tracking ========================================== + + // Initialization of local code replica + // Get space for a vector with the sinboc(1,1) replica sampled 2x/chip + d_tracking_code.resize(2 * d_code_length_chips, 0.0); + // correlator outputs (scalar) + if (d_veml) + { + // Very-Early, Early, Prompt, Late, Very-Late + d_n_correlator_taps = 5; + } + else + { + // Early, Prompt, Late + d_n_correlator_taps = 3; + } + + d_correlator_outs.reserve(d_n_correlator_taps); + d_local_code_shift_chips.reserve(d_n_correlator_taps); + // map memory pointers of correlator outputs + if (d_veml) + { + d_Very_Early = &d_correlator_outs[0]; + d_Early = &d_correlator_outs[1]; + d_Prompt = &d_correlator_outs[2]; + d_Late = &d_correlator_outs[3]; + d_Very_Late = &d_correlator_outs[4]; + d_local_code_shift_chips[0] = -d_trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = 0.0; + d_local_code_shift_chips[3] = d_trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + d_prompt_data_shift = &d_local_code_shift_chips[2]; + } + else + { + d_Very_Early = nullptr; + d_Early = &d_correlator_outs[0]; + d_Prompt = &d_correlator_outs[1]; + d_Late = &d_correlator_outs[2]; + d_Very_Late = nullptr; + d_local_code_shift_chips[0] = -d_trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = 0.0; + d_local_code_shift_chips[2] = d_trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_prompt_data_shift = &d_local_code_shift_chips[1]; + } + + d_multicorrelator_cpu.init(static_cast(2 * d_trk_parameters.vector_length), d_n_correlator_taps); + + if (d_trk_parameters.extend_correlation_symbols > 1) + { + d_enable_extended_integration = true; + } + else + { + d_enable_extended_integration = false; + d_trk_parameters.extend_correlation_symbols = 1; + } + + // Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal + if (d_trk_parameters.track_pilot) + { + // Extra correlator for the data component + d_correlator_data_cpu.init(static_cast(2 * d_trk_parameters.vector_length), 1); + d_correlator_data_cpu.set_high_dynamics_resampler(d_trk_parameters.high_dyn); + d_data_code.resize(2 * d_code_length_chips, 0.0); + } + + // --- Initializations --- + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + d_multicorrelator_cpu.set_high_dynamics_resampler(d_trk_parameters.high_dyn); + // Initial code frequency basis of NCO + d_code_freq_kf_chips_s = d_code_chip_rate; + // Residual code phase (in chips) + d_rem_code_phase_samples = 0.0; + // Residual carrier phase + d_rem_carr_phase_rad = 0.0; + + // sample synchronization + d_sample_counter = 0ULL; + d_acq_sample_stamp = 0ULL; + + d_current_prn_length_samples = static_cast(d_trk_parameters.vector_length); + d_current_correlation_time_s = 0.0; + + // CN0 estimation and lock detector buffers + d_cn0_estimation_counter = 0; + d_Prompt_buffer.reserve(d_trk_parameters.cn0_samples); + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + d_carrier_lock_threshold = d_trk_parameters.carrier_lock_th; + d_Prompt_Data.reserve(1); + d_cn0_smoother = Exponential_Smoother(); + d_cn0_smoother.set_alpha(d_trk_parameters.cn0_smoother_alpha); + + if (d_code_period > 0.0) + { + d_cn0_smoother.set_samples_for_initialization(d_trk_parameters.cn0_smoother_samples / static_cast(d_code_period * 1000.0)); + } + + d_carrier_lock_test_smoother = Exponential_Smoother(); + d_carrier_lock_test_smoother.set_alpha(d_trk_parameters.carrier_lock_test_smoother_alpha); + d_carrier_lock_test_smoother.set_min_value(-1.0); + d_carrier_lock_test_smoother.set_offset(0.0); + d_carrier_lock_test_smoother.set_samples_for_initialization(d_trk_parameters.carrier_lock_test_smoother_samples); + + d_acquisition_gnss_synchro = nullptr; + d_channel = 0; + d_acq_code_phase_samples = 0.0; + d_acq_carrier_doppler_hz = 0.0; + d_carrier_phase_kf_rad = 0; + d_carrier_doppler_kf_hz = 0.0; + d_carrier_doppler_rate_kf_hz_s = 0.0; + d_acc_carrier_phase_rad = 0.0; + + 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_carrier_phase_rate_step_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_state = 0; // initial state: standby + clear_tracking_vars(); + + d_dump = d_trk_parameters.dump; + d_dump_mat = d_trk_parameters.dump_mat and d_dump; + if (d_dump) + { + d_dump_filename = d_trk_parameters.dump_filename; + std::string dump_path; + // Get path + if (d_dump_filename.find_last_of('/') != std::string::npos) + { + std::string dump_filename_ = d_dump_filename.substr(d_dump_filename.find_last_of('/') + 1); + dump_path = d_dump_filename.substr(0, d_dump_filename.find_last_of('/')); + d_dump_filename = dump_filename_; + } + else + { + dump_path = std::string("."); + } + if (d_dump_filename.empty()) + { + d_dump_filename = "trk_channel_"; + } + // remove extension if any + if (d_dump_filename.substr(1).find_last_of('.') != std::string::npos) + { + d_dump_filename = d_dump_filename.substr(0, d_dump_filename.find_last_of('.')); + } + + d_dump_filename = dump_path + fs::path::preferred_separator + d_dump_filename; + // create directory + if (!gnss_sdr_create_directory(dump_path)) + { + std::cerr << "GNSS-SDR cannot create dump files for the tracking block. Wrong permissions?\n"; + d_dump = false; + } + } + d_corrected_doppler = false; + d_acc_carrier_phase_initialized = false; +} + + +void kf_vtl_tracking::forecast(int noutput_items, + gr_vector_int &ninput_items_required) +{ + if (noutput_items != 0) + { + ninput_items_required[0] = static_cast(d_trk_parameters.vector_length) * 2; + } +} + + +void kf_vtl_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg) +{ + try + { + if (pmt::any_ref(msg).type().hash_code() == d_int_type_hash_code) + { + const int tlm_event = boost::any_cast(pmt::any_ref(msg)); + if (tlm_event == 1) + { + DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel; + gr::thread::scoped_lock lock(d_setlock); + d_carrier_lock_fail_counter = 200000; // force loss-of-lock condition + } + } + } + catch (const boost::bad_any_cast &e) + { + LOG(WARNING) << "msg_handler_telemetry_to_trk Bad any_cast: " << e.what(); + } +} + + +void kf_vtl_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg) +{ + try + { + if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + const auto cmd = boost::any_cast>(pmt::any_ref(msg)); + // std::cout << "RX pvt-to-trk cmd with delay: " + // << static_cast(nitems_read(0) - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n"; + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + LOG(WARNING) << "msg_handler_pvt_to_trk Bad any_cast: " << e.what(); + } +} + + +void kf_vtl_tracking::start_tracking() +{ + gr::thread::scoped_lock l(d_setlock); + // 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_carrier_doppler_kf_hz = d_acq_carrier_doppler_hz; + d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_kf_hz / d_trk_parameters.fs_in; + d_carrier_phase_rate_step_rad = 0.0; + std::array Signal_{}; + Signal_[0] = d_acquisition_gnss_synchro->Signal[0]; + Signal_[1] = d_acquisition_gnss_synchro->Signal[1]; + Signal_[2] = d_acquisition_gnss_synchro->Signal[2]; + + if (d_systemName == "GPS" and d_signal_type == "1C") + { + gps_l1_ca_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + } + else if (d_systemName == "GPS" and d_signal_type == "2S") + { + gps_l2c_m_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + } + else if (d_systemName == "GPS" and d_signal_type == "L5") + { + if (d_trk_parameters.track_pilot) + { + gps_l5q_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + gps_l5i_code_gen_float(d_data_code, d_acquisition_gnss_synchro->PRN); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code.data(), d_prompt_data_shift); + } + else + { + gps_l5i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN); + } + } + else if (d_systemName == "Galileo" and d_signal_type == "1B") + { + if (d_trk_parameters.track_pilot) + { + const std::array pilot_signal = {{'1', 'C', '\0'}}; + galileo_e1_code_gen_sinboc11_float(d_tracking_code, pilot_signal, d_acquisition_gnss_synchro->PRN); + galileo_e1_code_gen_sinboc11_float(d_data_code, Signal_, d_acquisition_gnss_synchro->PRN); + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_correlator_data_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_data_code.data(), d_prompt_data_shift); + } + else + { + galileo_e1_code_gen_sinboc11_float(d_tracking_code, Signal_, d_acquisition_gnss_synchro->PRN); + } + } + else if (d_systemName == "Galileo" and d_signal_type == "5X") + { + volk_gnsssdr::vector aux_code(d_code_length_chips); + const std::array signal_type_ = {{'5', 'X', '\0'}}; + galileo_e5_a_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, signal_type_); + if (d_trk_parameters.track_pilot) + { + d_secondary_code_string = GALILEO_E5A_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]; + for (int32_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(); // the same because it is generated the full signal (E5aI + E5aQ) + } + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code.data(), d_prompt_data_shift); + } + else + { + for (int32_t i = 0; i < d_code_length_chips; i++) + { + d_tracking_code[i] = aux_code[i].real(); + } + } + } + else if (d_systemName == "Galileo" and d_signal_type == "7X") + { + volk_gnsssdr::vector aux_code(d_code_length_chips); + const std::array signal_type_ = {{'7', 'X', '\0'}}; + galileo_e5_b_code_gen_complex_primary(aux_code, d_acquisition_gnss_synchro->PRN, signal_type_); + if (d_trk_parameters.track_pilot) + { + d_secondary_code_string = GALILEO_E5B_Q_SECONDARY_CODE[d_acquisition_gnss_synchro->PRN - 1]; + for (int32_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(); // the same because it is generated the full signal (E5bI + E5bsQ) + } + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_correlator_data_cpu.set_local_code_and_taps(d_code_length_chips, d_data_code.data(), d_prompt_data_shift); + } + else + { + for (int32_t i = 0; i < d_code_length_chips; i++) + { + d_tracking_code[i] = aux_code[i].real(); + } + } + } + else if (d_systemName == "Beidou" and d_signal_type == "B1") + { + beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + // GEO Satellites use different secondary code + if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) + { + d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + d_trk_parameters.track_pilot = false; + // set the preamble in the secondary code acquisition + d_secondary_code_length = static_cast(BEIDOU_B1I_GEO_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = BEIDOU_B1I_GEO_PREAMBLE_SYMBOLS_STR; + d_data_secondary_code_length = 0; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + else + { + d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = true; + d_trk_parameters.track_pilot = false; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = BEIDOU_B1I_SECONDARY_CODE_STR; + d_data_secondary_code_length = static_cast(BEIDOU_B1I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = BEIDOU_B1I_SECONDARY_CODE_STR; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + } + + else if (d_systemName == "Beidou" and d_signal_type == "B3") + { + beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); + // Update secondary code settings for geo satellites + if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) + { + d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = false; + d_trk_parameters.track_pilot = false; + // set the preamble in the secondary code acquisition + d_secondary_code_length = static_cast(BEIDOU_B3I_GEO_PREAMBLE_LENGTH_SYMBOLS); + d_secondary_code_string = BEIDOU_B3I_GEO_PREAMBLE_SYMBOLS_STR; + d_data_secondary_code_length = 0; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + else + { + d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization + d_correlation_length_ms = 1; + d_code_samples_per_chip = 1; + d_secondary = true; + d_trk_parameters.track_pilot = false; + // synchronize and remove data secondary code + d_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_secondary_code_string = BEIDOU_B3I_SECONDARY_CODE_STR; + d_data_secondary_code_length = static_cast(BEIDOU_B3I_SECONDARY_CODE_LENGTH); + d_data_secondary_code_string = BEIDOU_B3I_SECONDARY_CODE_STR; + d_Prompt_circular_buffer.set_capacity(d_secondary_code_length); + } + } + + d_multicorrelator_cpu.set_local_code_and_taps(d_code_samples_per_chip * d_code_length_chips, d_tracking_code.data(), d_local_code_shift_chips.data()); + std::fill_n(d_correlator_outs.begin(), d_n_correlator_taps, gr_complex(0.0, 0.0)); + + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + d_rem_code_phase_samples = 0.0; + d_rem_carr_phase_rad = 0.0; + d_rem_code_phase_chips = 0.0; + d_acc_carrier_phase_rad = 0.0; + d_cn0_estimation_counter = 0; + d_carrier_lock_test = 1.0; + d_CN0_SNV_dB_Hz = 0.0; + + if (d_veml) + { + d_local_code_shift_chips[0] = -d_trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_trk_parameters.very_early_late_space_chips * static_cast(d_code_samples_per_chip); + } + else + { + d_local_code_shift_chips[0] = -d_trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_trk_parameters.early_late_space_chips * static_cast(d_code_samples_per_chip); + } + + d_current_correlation_time_s = d_code_period; + + // Initialize tracking ========================================== + + // DEBUG OUTPUT + std::cout << "Tracking of " << d_systemName << " " << d_signal_pretty_name << " signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN) << '\n'; + DLOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; + + // enable tracking pull-in + d_state = 1; + d_cloop = true; + d_pull_in_transitory = true; + d_Prompt_circular_buffer.clear(); + d_corrected_doppler = false; + d_acc_carrier_phase_initialized = false; +} + + +void kf_vtl_tracking::init_kf(double acq_code_phase_chips, double acq_doppler_hz) +{ + // Kalman Filter class variables + const double Ti = d_correlation_length_ms * 0.001; + // state vector: code_phase_chips, carrier_phase_rads, carrier_freq_hz,carrier_freq_rate_hz, code_freq_chips_s + d_F = arma::mat(5, 5); + d_F << 1 << 0 << 0 << 0 << Ti << arma::endr + << 0 << 1 << 2.0 * GNSS_PI * Ti << GNSS_PI * (Ti * Ti) << 0 << arma::endr + << 0 << 0 << 1 << Ti << 0 << arma::endr + << 0 << 0 << 0 << 1 << 0 << arma::endr + << 0 << 0 << 0 << 0 << 1 << arma::endr; + + const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor + + d_H = arma::mat(2, 5); + d_H << 1 << 0 << -B * Ti / 2.0 << B * (Ti * Ti) / 6.0 << 0 << arma::endr + << 0 << 1 << -GNSS_PI * Ti << GNSS_PI * (Ti * Ti) / 3.0 << 0 << arma::endr; + + // Phase noise variance + // const double CN0_lin = pow(10.0, d_trk_parameters.expected_cn0_dbhz / 10.0); // CN0 in Hz + // const double N_periods = 1; // Only 1 interval + // const double Sigma2_Tau = 0.25 * (1.0 + 2.0 * CN0_lin * Ti) / (N_periods * pow(CN0_lin * Ti, 2.0)) * (1.0 + (1.0 + 2.0 * CN0_lin * Ti) / (pow(N_periods * (CN0_lin * Ti), 2.0))); + // const double Sigma2_Phase = 1.0 / (2.0 * CN0_lin * Ti) * (1.0 + 1.0 / (2.0 * CN0_lin * Ti)); + + // measurement covariance matrix (static) + d_R = arma::mat(2, 2); + // d_R << Sigma2_Tau << 0 << arma::endr + // << 0 << Sigma2_Phase << arma::endr; + + d_R << pow(d_trk_parameters.code_disc_sd_chips, 2.0) << 0 << arma::endr + << 0 << pow(d_trk_parameters.carrier_disc_sd_rads, 2.0) << arma::endr; + + // system covariance matrix (static) + d_Q = arma::mat(5, 5); + d_Q << pow(d_trk_parameters.code_phase_sd_chips, 2.0) << 0 << 0 << 0 << 0 << arma::endr + << 0 << pow(d_trk_parameters.carrier_phase_sd_rad, 2.0) << 0 << 0 << 0 << arma::endr + << 0 << 0 << pow(d_trk_parameters.carrier_freq_sd_hz, 2.0) << 0 << 0 << arma::endr + << 0 << 0 << 0 << pow(d_trk_parameters.carrier_freq_rate_sd_hz_s, 2.0) << 0 << arma::endr + << 0 << 0 << 0 << 0 << pow(d_trk_parameters.code_rate_sd_chips_s, 2.0) << arma::endr; + + // initial Kalman covariance matrix + d_P_old_old = arma::mat(5, 5); + + d_P_old_old << pow(d_trk_parameters.init_code_phase_sd_chips, 2.0) << 0 << 0 << 0 << 0 << arma::endr + << 0 << pow(d_trk_parameters.init_carrier_phase_sd_rad, 2.0) << 0 << 0 << 0 << arma::endr + << 0 << 0 << pow(d_trk_parameters.init_carrier_freq_sd_hz, 2.0) << 0 << 0 << arma::endr + << 0 << 0 << 0 << pow(d_trk_parameters.init_carrier_freq_rate_sd_hz_s, 2.0) << 0 << arma::endr + << 0 << 0 << 0 << 0 << pow(d_trk_parameters.init_code_rate_sd_chips_s, 2.0) << arma::endr; + + // init state vector + d_x_old_old = arma::vec(5); + // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s, code_freq_rate_chips_s + d_x_old_old << acq_code_phase_chips << 0 << acq_doppler_hz << 0 << 0 << arma::endr; + + // std::cout << "F: " << d_F << "\n"; + // std::cout << "H: " << d_H << "\n"; + // std::cout << "R: " << d_R << "\n"; + // std::cout << "Q: " << d_Q << "\n"; + // std::cout << "P: " << d_P_old_old << "\n"; + // std::cout << "x: " << d_x_old_old << "\n"; +} + + +void kf_vtl_tracking::update_kf_narrow_integration_time() +{ + // Kalman Filter class variables + const double Ti = d_current_correlation_time_s; + + // state vector: code_phase_chips, carrier_phase_rads, carrier_freq_hz,carrier_freq_rate_hz, code_freq_chips_s + d_F << 1 << 0 << 0 << 0 << Ti << arma::endr + << 0 << 1 << 2.0 * GNSS_PI * Ti << GNSS_PI * (Ti * Ti) << 0 << arma::endr + << 0 << 0 << 1 << Ti << 0 << arma::endr + << 0 << 0 << 0 << 1 << 0 << arma::endr + << 0 << 0 << 0 << 0 << 1 << arma::endr; + + const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor + + d_H << 1 << 0 << -B * Ti / 2.0 << B * (Ti * Ti) / 6.0 << 0 << arma::endr + << 0 << 1 << -GNSS_PI * Ti << GNSS_PI * (Ti * Ti) / 3.0 << 0 << arma::endr; + + // measurement covariance matrix (static) + d_R << pow(d_trk_parameters.code_disc_sd_chips, 2.0) << 0 << arma::endr + << 0 << pow(d_trk_parameters.carrier_disc_sd_rads, 2.0) << arma::endr; + + // system covariance matrix (static) + d_Q << pow(d_trk_parameters.narrow_code_phase_sd_chips, 2.0) << 0 << 0 << 0 << 0 << arma::endr + << 0 << pow(d_trk_parameters.narrow_carrier_phase_sd_rad, 2.0) << 0 << 0 << 0 << arma::endr + << 0 << 0 << pow(d_trk_parameters.narrow_carrier_freq_sd_hz, 2.0) << 0 << 0 << arma::endr + << 0 << 0 << 0 << pow(d_trk_parameters.narrow_carrier_freq_rate_sd_hz_s, 2.0) << 0 << arma::endr + << 0 << 0 << 0 << 0 << pow(d_trk_parameters.narrow_code_rate_sd_chips_s, 2.0) << arma::endr; +} + + +void kf_vtl_tracking::update_kf_cn0(double current_cn0_dbhz) +{ + // Kalman Filter class variables + const double Ti = d_correlation_length_ms * 0.001; + const double B = d_code_chip_rate / d_signal_carrier_freq; // carrier to code rate factor + + d_H = arma::mat(2, 5); + d_H << 1 << 0 << -B * Ti / 2.0 << B * (Ti * Ti) / 6.0 << 0 << arma::endr + << 0 << 1 << -GNSS_PI * Ti << GNSS_PI * (Ti * Ti) / 3.0 << 0 << arma::endr; + + // Phase noise variance + const double CN0_lin = pow(10.0, current_cn0_dbhz / 10.0); // CN0 in Hz + const double N_periods = 1; // Only 1 interval + const double Sigma2_Tau = 0.25 * (1.0 + 2.0 * CN0_lin * Ti) / (N_periods * pow(CN0_lin * Ti, 2.0)) * (1.0 + (1.0 + 2.0 * CN0_lin * Ti) / (pow(N_periods * (CN0_lin * Ti), 2.0))); + const double Sigma2_Phase = 1.0 / (2.0 * CN0_lin * Ti) * (1.0 + 1.0 / (2.0 * CN0_lin * Ti)); + + // measurement covariance matrix (static) + d_R = arma::mat(2, 2); + d_R << Sigma2_Tau << 0 << arma::endr + << 0 << Sigma2_Phase << arma::endr; +} + + +kf_vtl_tracking::~kf_vtl_tracking() +{ + if (d_dump_file.is_open()) + { + try + { + d_dump_file.close(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in Tracking block destructor: " << ex.what(); + } + } + if (d_dump_mat) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } + try + { + if (d_trk_parameters.track_pilot) + { + d_correlator_data_cpu.free(); + } + d_multicorrelator_cpu.free(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Exception in Tracking block destructor: " << ex.what(); + } +} + + +bool kf_vtl_tracking::acquire_secondary() +{ + // ******* preamble correlation ******** + int32_t corr_value = 0; + for (uint32_t i = 0; i < d_secondary_code_length; i++) + { + if (d_Prompt_circular_buffer[i].real() < 0.0) // symbols clipping + { + if (d_secondary_code_string[i] == '0') + { + corr_value++; + } + else + { + corr_value--; + } + } + else + { + if (d_secondary_code_string[i] == '0') + { + corr_value--; + } + else + { + corr_value++; + } + } + } + + if (abs(corr_value) == static_cast(d_secondary_code_length)) + { + return true; + } + + return false; +} + + +bool kf_vtl_tracking::cn0_and_tracking_lock_status(double coh_integration_time_s) +{ + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < d_trk_parameters.cn0_samples) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_P_accu; + d_cn0_estimation_counter++; + return true; + } + + d_Prompt_buffer[d_cn0_estimation_counter % d_trk_parameters.cn0_samples] = d_P_accu; + d_cn0_estimation_counter++; + // Code lock indicator + const float d_CN0_SNV_dB_Hz_raw = cn0_m2m4_estimator(d_Prompt_buffer.data(), d_trk_parameters.cn0_samples, static_cast(coh_integration_time_s)); + d_CN0_SNV_dB_Hz = d_cn0_smoother.smooth(d_CN0_SNV_dB_Hz_raw); + // Carrier lock indicator + d_carrier_lock_test = d_carrier_lock_test_smoother.smooth(carrier_lock_detector(d_Prompt_buffer.data(), 1)); + // Loss of lock detection + if (!d_pull_in_transitory) + { + if (d_carrier_lock_test < d_carrier_lock_threshold) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) + { + d_carrier_lock_fail_counter--; + } + } + + if (d_CN0_SNV_dB_Hz < d_trk_parameters.cn0_min) + { + d_code_lock_fail_counter++; + } + else + { + if (d_code_lock_fail_counter > 0) + { + d_code_lock_fail_counter--; + } + } + } + if (d_carrier_lock_fail_counter > d_trk_parameters.max_carrier_lock_fail or d_code_lock_fail_counter > d_trk_parameters.max_code_lock_fail) + { + std::cout << "Loss of lock in channel " << d_channel << "!\n"; + LOG(INFO) << "Loss of lock in channel " << d_channel + << " (carrier_lock_fail_counter:" << d_carrier_lock_fail_counter + << " code_lock_fail_counter : " << d_code_lock_fail_counter << ")"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + return false; + } + return true; +} + + +// correlation requires: +// - updated remnant carrier phase in radians (rem_carr_phase_rad) +// - updated remnant code phase in samples (d_rem_code_phase_samples) +// - d_code_freq_chips +// - d_carrier_doppler_hz +void kf_vtl_tracking::do_correlation_step(const gr_complex *input_samples) +{ + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + d_multicorrelator_cpu.set_input_output_vectors(d_correlator_outs.data(), input_samples); + d_multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + static_cast(d_carrier_phase_step_rad), static_cast(d_carrier_phase_rate_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), + d_trk_parameters.vector_length); + + // DATA CORRELATOR (if tracking tracks the pilot signal) + if (d_trk_parameters.track_pilot) + { + d_correlator_data_cpu.set_input_output_vectors(d_Prompt_Data.data(), input_samples); + d_correlator_data_cpu.Carrier_wipeoff_multicorrelator_resampler( + d_rem_carr_phase_rad, + static_cast(d_carrier_phase_step_rad), static_cast(d_carrier_phase_rate_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), + d_trk_parameters.vector_length); + } +} + + +void kf_vtl_tracking::run_Kf() +{ + // Carrier discriminator + if (d_cloop) + { + // Costas loop discriminator, insensitive to 180 deg phase transitions + d_carr_phase_error_disc_hz = pll_cloop_two_quadrant_atan(d_P_accu) / TWO_PI; + } + else + { + // Secondary code acquired. No symbols transition should be present in the signal + d_carr_phase_error_disc_hz = pll_four_quadrant_atan(d_P_accu) / TWO_PI; + } + + // Code discriminator + if (d_veml) + { + d_code_error_disc_chips = dll_nc_vemlp_normalized(d_VE_accu, d_E_accu, d_L_accu, d_VL_accu); // [chips/Ti] + } + else + { + d_code_error_disc_chips = dll_nc_e_minus_l_normalized(d_E_accu, d_L_accu, d_trk_parameters.spc, d_trk_parameters.slope, d_trk_parameters.y_intercept); // [chips/Ti] + } + + // Kalman loop + + // Prediction + d_x_new_old = d_F * d_x_old_old; + d_P_new_old = d_F * d_P_old_old * d_F.t() + d_Q; + + // Innovation + arma::vec z = {d_code_error_disc_chips, d_carr_phase_error_disc_hz * TWO_PI}; + + // Measurement update + arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain + + x_new_new = d_x_new_old + K * z; + + d_P_new_new = (arma::eye(5, 5) - K * d_H) * d_P_new_old; + + // new code phase estimation + d_code_error_kf_chips = x_new_new(0); + x_new_new(0) = 0; // reset error estimation because the NCO corrects the code phase + + // new carrier phase estimation + d_carrier_phase_kf_rad = x_new_new(1); + + // New carrier Doppler frequency estimation + d_carrier_doppler_kf_hz = x_new_new(2); // d_carrier_loop_filter.get_carrier_error(0, static_cast(d_carr_phase_error_hz), static_cast(d_current_correlation_time_s)); + + d_carrier_doppler_rate_kf_hz_s = x_new_new(3); + + // std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << '\n'; + // std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << '\n'; + + // New code Doppler frequency estimation + if (d_trk_parameters.carrier_aiding) + { + // estimate the code rate exclusively based on the carrier Doppler + d_code_freq_kf_chips_s = d_code_chip_rate + d_carrier_doppler_kf_hz * d_code_chip_rate / d_signal_carrier_freq; + } + else + { + // use its own KF code rate estimation + d_code_freq_kf_chips_s -= x_new_new(4); + } + x_new_new(4) = 0; + // Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler + // if (d_trk_parameters.enable_doppler_correction == true) + // { + // if (d_pull_in_transitory == false and d_corrected_doppler == false) + // { + // todo: alforithm here... + // } + // } + + // correct code and carrier phase + d_rem_code_phase_samples += d_trk_parameters.fs_in * d_code_error_kf_chips / d_code_freq_kf_chips_s; + d_rem_carr_phase_rad = d_carrier_phase_kf_rad; + + // prepare data for next KF epoch + d_x_old_old = x_new_new; + d_P_old_old = d_P_new_new; +} + + +void kf_vtl_tracking::check_carrier_phase_coherent_initialization() +{ + if (d_acc_carrier_phase_initialized == false) + { + d_acc_carrier_phase_rad = -d_rem_carr_phase_rad; + d_acc_carrier_phase_initialized = true; + } +} + + +void kf_vtl_tracking::clear_tracking_vars() +{ + std::fill_n(d_correlator_outs.begin(), d_n_correlator_taps, gr_complex(0.0, 0.0)); + if (d_trk_parameters.track_pilot) + { + d_Prompt_Data[0] = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); + } + d_P_accu_old = gr_complex(0.0, 0.0); + d_current_symbol = 0; + d_current_data_symbol = 0; + d_Prompt_circular_buffer.clear(); + d_carrier_phase_rate_step_rad = 0.0; + d_code_phase_rate_step_chips = 0.0; +} + + +// todo: IT DOES NOT WORK WHEN NO KF IS RUNNING (extended correlation epochs!!) +void kf_vtl_tracking::update_tracking_vars() +{ + d_T_chip_seconds = 1.0 / d_code_freq_kf_chips_s; + d_T_prn_seconds = d_T_chip_seconds * static_cast(d_code_length_chips); + + // ################## 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 + d_T_prn_samples = d_T_prn_seconds * d_trk_parameters.fs_in; + // d_K_blk_samples = d_T_prn_samples + d_rem_code_phase_samples + d_trk_parameters.fs_in * d_code_error_kf_chips / d_code_freq_kf_chips_s; + // KF will update d_rem_code_phase_samples + d_K_blk_samples = d_T_prn_samples + d_rem_code_phase_samples; + d_current_prn_length_samples = static_cast(std::floor(d_K_blk_samples)); // round to a discrete number of samples + + // ################### PLL COMMANDS ################################################# + // carrier phase step (NCO phase increment per sample) [rads/sample] + d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_kf_hz / d_trk_parameters.fs_in; + // d_rem_carr_phase_rad = d_carrier_phase_kf_rad; + + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI); + + // carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2] + if (d_trk_parameters.high_dyn) + { + d_carrier_phase_rate_step_rad = TWO_PI * d_carrier_doppler_rate_kf_hz_s / d_trk_parameters.fs_in; + } + // std::cout << d_carrier_phase_rate_step_rad * d_trk_parameters.fs_in * d_trk_parameters.fs_in / TWO_PI << '\n'; + // remnant carrier phase to prevent overflow in the code NCO + // d_rem_carr_phase_rad += static_cast(d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples)); + // d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI); + + // carrier phase accumulator + // double a = d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples); + // double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * static_cast(d_current_prn_length_samples); + // std::cout << fmod(b, TWO_PI) / fmod(a, TWO_PI) << '\n'; + d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast(d_current_prn_length_samples) * 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_kf_chips_s / d_trk_parameters.fs_in; + // todo: extend kf to estimate code rate + // if (d_trk_parameters.high_dyn) + // { + // d_code_phase_rate_step_chips = d_code_freq_kf_rate_chips_s / d_trk_parameters.fs_in; + // } + + // remnant code phase [chips] + d_rem_code_phase_samples = d_K_blk_samples - static_cast(d_current_prn_length_samples); // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_kf_chips_s * d_rem_code_phase_samples / d_trk_parameters.fs_in; +} + + +void kf_vtl_tracking::save_correlation_results() +{ + if (d_secondary) + { + if (d_secondary_code_string[d_current_symbol] == '0') + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + } + else + { + if (d_veml) + { + d_VE_accu -= *d_Very_Early; + d_VL_accu -= *d_Very_Late; + } + d_E_accu -= *d_Early; + d_P_accu -= *d_Prompt; + d_L_accu -= *d_Late; + } + d_current_symbol++; + // secondary code roll-up + d_current_symbol %= d_secondary_code_length; + } + else + { + if (d_veml) + { + d_VE_accu += *d_Very_Early; + d_VL_accu += *d_Very_Late; + } + d_E_accu += *d_Early; + d_P_accu += *d_Prompt; + d_L_accu += *d_Late; + } + + // data secondary code roll-up + if (d_symbols_per_bit > 1) + { + if (d_data_secondary_code_length > 0) + { + if (d_trk_parameters.track_pilot) + { + if (d_data_secondary_code_string[d_current_data_symbol] == '0') + { + d_P_data_accu += d_Prompt_Data[0]; + } + else + { + d_P_data_accu -= d_Prompt_Data[0]; + } + } + else + { + if (d_data_secondary_code_string[d_current_data_symbol] == '0') + { + d_P_data_accu += *d_Prompt; + } + else + { + d_P_data_accu -= *d_Prompt; + } + } + + d_current_data_symbol++; + // data secondary code roll-up + d_current_data_symbol %= d_data_secondary_code_length; + } + else + { + if (d_trk_parameters.track_pilot) + { + d_P_data_accu += d_Prompt_Data[0]; + } + else + { + d_P_data_accu += *d_Prompt; + // std::cout << "s[" << d_current_data_symbol << "]=" << (int)((*d_Prompt).real() > 0) << '\n'; + } + d_current_data_symbol++; + d_current_data_symbol %= d_symbols_per_bit; + } + } + else + { + if (d_trk_parameters.track_pilot) + { + d_P_data_accu = d_Prompt_Data[0]; + } + else + { + d_P_data_accu = *d_Prompt; + } + } + + if (d_trk_parameters.track_pilot) + { + // If tracking pilot, disable Costas loop + d_cloop = false; + } + else + { + d_cloop = true; + } +} + + +void kf_vtl_tracking::log_data() +{ + if (d_dump) + { + // Dump results to file + float prompt_I; + float prompt_Q; + float tmp_VE; + float tmp_E; + float tmp_P; + float tmp_L; + float tmp_VL; + float tmp_float; + double tmp_double; + uint64_t tmp_long_int; + if (d_trk_parameters.track_pilot) + { + prompt_I = d_Prompt_Data.data()->real(); + prompt_Q = d_Prompt_Data.data()->imag(); + } + else + { + prompt_I = d_Prompt->real(); + prompt_Q = d_Prompt->imag(); + } + if (d_veml) + { + tmp_VE = std::abs(d_VE_accu); + tmp_VL = std::abs(d_VL_accu); + } + else + { + tmp_VE = 0.0; + tmp_VL = 0.0; + } + tmp_E = std::abs(d_E_accu); + tmp_P = std::abs(d_P_accu); + tmp_L = std::abs(d_L_accu); + + try + { + // Dump correlators output + 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 + tmp_long_int = d_sample_counter + static_cast(d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_long_int), sizeof(uint64_t)); + // accumulated carrier phase + tmp_float = static_cast(d_acc_carrier_phase_rad); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier and code frequency + tmp_float = static_cast(d_carrier_doppler_kf_hz); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // carrier phase rate [Hz/s] + tmp_float = static_cast(d_carrier_doppler_rate_kf_hz_s); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(d_code_freq_kf_chips_s); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // code phase rate [chips/s^2] + tmp_float = static_cast(d_code_phase_rate_step_chips * d_trk_parameters.fs_in * d_trk_parameters.fs_in); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // Carrier estimation + tmp_float = static_cast(d_carr_phase_error_disc_hz); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(x_new_new(2)); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // code estimation + tmp_float = static_cast(d_code_error_disc_chips); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(d_code_error_kf_chips); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // CN0 and carrier lock test + tmp_float = static_cast(d_CN0_SNV_dB_Hz); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_float = static_cast(d_carrier_lock_test); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + // AUX vars (for debug purposes) + tmp_float = static_cast(d_rem_code_phase_samples); + d_dump_file.write(reinterpret_cast(&tmp_float), sizeof(float)); + tmp_double = static_cast(d_sample_counter + 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(); + } + } +} + + +int32_t kf_vtl_tracking::save_matfile() const +{ + // READ DUMP FILE + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 1; + const int32_t number_of_float_vars = 19; + const 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; + std::string dump_filename_ = d_dump_filename; + // add channel number to the filename + dump_filename_.append(std::to_string(d_channel)); + // add extension + dump_filename_.append(".dat"); + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + 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; + } + auto abs_VE = std::vector(num_epoch); + auto abs_E = std::vector(num_epoch); + auto abs_P = std::vector(num_epoch); + auto abs_L = std::vector(num_epoch); + auto abs_VL = std::vector(num_epoch); + auto Prompt_I = std::vector(num_epoch); + auto Prompt_Q = std::vector(num_epoch); + auto PRN_start_sample_count = std::vector(num_epoch); + auto acc_carrier_phase_rad = std::vector(num_epoch); + auto carrier_doppler_hz = std::vector(num_epoch); + auto carrier_doppler_rate_hz = std::vector(num_epoch); + auto code_freq_chips = std::vector(num_epoch); + auto code_freq_rate_chips = std::vector(num_epoch); + auto carr_error_hz = std::vector(num_epoch); + auto carr_error_filt_hz = std::vector(num_epoch); + auto code_error_chips = std::vector(num_epoch); + auto code_error_filt_chips = std::vector(num_epoch); + auto CN0_SNV_dB_Hz = std::vector(num_epoch); + auto carrier_lock_test = std::vector(num_epoch); + auto aux1 = std::vector(num_epoch); + auto aux2 = std::vector(num_epoch); + auto PRN = std::vector(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_doppler_rate_hz[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&code_freq_rate_chips[i]), sizeof(float)); + dump_file.read(reinterpret_cast(&carr_error_hz[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() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("abs_VE", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims.data(), abs_VE.data(), 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.data(), abs_E.data(), 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.data(), abs_P.data(), 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.data(), abs_L.data(), 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.data(), abs_VL.data(), 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.data(), Prompt_I.data(), 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.data(), Prompt_Q.data(), 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.data(), PRN_start_sample_count.data(), 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.data(), acc_carrier_phase_rad.data(), 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.data(), carrier_doppler_hz.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("carrier_doppler_rate_hz", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims.data(), carrier_doppler_rate_hz.data(), 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.data(), code_freq_chips.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("code_freq_rate_chips", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims.data(), code_freq_rate_chips.data(), 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.data(), carr_error_hz.data(), 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.data(), carr_error_filt_hz.data(), 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.data(), code_error_chips.data(), 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.data(), code_error_filt_chips.data(), 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.data(), CN0_SNV_dB_Hz.data(), 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.data(), carrier_lock_test.data(), 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.data(), aux1.data(), 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.data(), aux2.data(), 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.data(), PRN.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + return 0; +} + + +void kf_vtl_tracking::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) + { + std::string dump_filename_ = d_dump_filename; + // add channel number to the filename + dump_filename_.append(std::to_string(d_channel)); + // add extension + dump_filename_.append(".dat"); + + if (!d_dump_file.is_open()) + { + try + { + d_dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(dump_filename_.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << dump_filename_.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } + } + } +} + + +void kf_vtl_tracking::set_gnss_synchro(Gnss_Synchro *p_gnss_synchro) +{ + gr::thread::scoped_lock l(d_setlock); + d_acquisition_gnss_synchro = p_gnss_synchro; +} + + +void kf_vtl_tracking::stop_tracking() +{ + gr::thread::scoped_lock l(d_setlock); + d_state = 0; +} + + +int kf_vtl_tracking::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) +{ + gr::thread::scoped_lock l(d_setlock); + const auto *in = reinterpret_cast(input_items[0]); + auto **out = reinterpret_cast(&output_items[0]); + Gnss_Synchro current_synchro_data = Gnss_Synchro(); + current_synchro_data.Flag_valid_symbol_output = false; + + if (d_pull_in_transitory == true) + { + if (d_trk_parameters.pull_in_time_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + { + d_pull_in_transitory = false; + d_carrier_lock_fail_counter = 0; + d_code_lock_fail_counter = 0; + } + } + switch (d_state) + { + case 0: // Standby - Consume samples at full throttle, do nothing + { + d_sample_counter += static_cast(ninput_items[0]); + consume_each(ninput_items[0]); + return 0; + break; + } + case 1: // Pull-in + { + // Signal alignment (skip samples until the incoming signal is aligned with local replica) + const int64_t acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); + const double acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / d_trk_parameters.fs_in; + const double delta_trk_to_acq_prn_start_samples = static_cast(acq_trk_diff_samples) - d_acq_code_phase_samples; + + d_code_freq_kf_chips_s = d_code_chip_rate; + d_code_phase_step_chips = d_code_freq_kf_chips_s / d_trk_parameters.fs_in; + d_code_phase_rate_step_chips = 0.0; + const double T_chip_mod_seconds = 1.0 / d_code_freq_kf_chips_s; + const double T_prn_mod_seconds = T_chip_mod_seconds * static_cast(d_code_length_chips); + const double T_prn_mod_samples = T_prn_mod_seconds * d_trk_parameters.fs_in; + + d_acq_code_phase_samples = T_prn_mod_samples - std::fmod(delta_trk_to_acq_prn_start_samples, T_prn_mod_samples); + d_current_prn_length_samples = round(T_prn_mod_samples); + + const int32_t samples_offset = round(d_acq_code_phase_samples); + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * static_cast(samples_offset); + d_state = 2; + d_sample_counter += samples_offset; // count for the processed samples + d_cn0_smoother.reset(); + d_carrier_lock_test_smoother.reset(); + + // init KF + // d_T_chip_seconds = 1.0 / d_code_freq_chips; + // d_T_prn_seconds = d_T_chip_seconds * static_cast(samples_offset); + // // 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 + // d_T_prn_samples = d_T_prn_seconds * d_trk_parameters.fs_in; + // d_K_blk_samples = d_T_prn_samples + d_rem_code_phase_samples; + // // remnant code phase [chips] + // d_rem_code_phase_samples = d_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 / d_trk_parameters.fs_in; + + init_kf(0, d_carrier_doppler_kf_hz); + + LOG(INFO) << "Number of samples between Acquisition and Tracking = " << acq_trk_diff_samples << " ( " << acq_trk_diff_seconds << " s)"; + DLOG(INFO) << "PULL-IN Doppler [Hz] = " << d_carrier_doppler_kf_hz + << ". PULL-IN Code Phase [samples] = " << d_acq_code_phase_samples; + + consume_each(samples_offset); // shift input to perform alignment with local replica + return 0; + } + case 2: // Wide tracking and symbol synchronization + { + do_correlation_step(in); + // Save single correlation step variables + if (d_veml) + { + d_VE_accu = *d_Very_Early; + d_VL_accu = *d_Very_Late; + } + d_E_accu = *d_Early; + d_P_accu = *d_Prompt; + d_L_accu = *d_Late; + d_trk_parameters.spc = d_trk_parameters.early_late_space_chips; + // if (std::string(d_trk_parameters.signal) == "E1") + // { + // d_trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc); + // d_trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc); + // } + + // fail-safe: check if the secondary code or bit synchronization has not succeeded in a limited time period + if (d_trk_parameters.bit_synchronization_time_limit_s < (d_sample_counter - d_acq_sample_stamp) / static_cast(d_trk_parameters.fs_in)) + { + d_carrier_lock_fail_counter = 300000; // force loss-of-lock condition + LOG(INFO) << d_systemName << " " << d_signal_pretty_name << " tracking synchronization time limit reached in channel " << d_channel + << " for satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN) << '\n'; + } + // Check lock status + if (!cn0_and_tracking_lock_status(d_code_period)) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + bool next_state = false; + // Perform DLL/PLL tracking loop computations. Costas Loop enabled + run_Kf(); + update_tracking_vars(); + + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + + if (!d_pull_in_transitory) + { + if (d_secondary) + { + // ####### SECONDARY CODE LOCK ##### + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + LOG(INFO) << d_systemName << " " << d_signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN) << '\n'; + std::cout << d_systemName << " " << d_signal_pretty_name << " secondary code locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN) << '\n'; + } + } + } + else if (d_symbols_per_bit > 1) // Signal does not have secondary code. Search a bit transition by sign change + { + // ******* preamble correlation ******** + d_Prompt_circular_buffer.push_back(*d_Prompt); + if (d_Prompt_circular_buffer.size() == d_secondary_code_length) + { + next_state = acquire_secondary(); + if (next_state) + { + LOG(INFO) << d_systemName << " " << d_signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN) << '\n'; + std::cout << d_systemName << " " << d_signal_pretty_name << " tracking bit synchronization locked in channel " << d_channel + << " for satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN) << '\n'; + } + } + } + else + { + next_state = true; + } + } + else + { + next_state = false; // keep in state 2 during pull-in transitory + } + if (next_state) + { // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_P_data_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + d_Prompt_circular_buffer.clear(); + d_current_symbol = 0; + d_current_data_symbol = 0; + + if (d_enable_extended_integration) + { + // UPDATE INTEGRATION TIME + d_extend_correlation_symbols_count = 0; + d_current_correlation_time_s = static_cast(d_trk_parameters.extend_correlation_symbols) * static_cast(d_code_period); + d_state = 3; // next state is the extended correlator integrator + LOG(INFO) << "Enabled " << d_trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN); + std::cout << "Enabled " << d_trk_parameters.extend_correlation_symbols * static_cast(d_code_period * 1000.0) << " ms extended correlator in channel " + << d_channel + << " for satellite " << Gnss_Satellite(d_systemName, d_acquisition_gnss_synchro->PRN) << '\n'; + // Set narrow taps delay values [chips] + update_kf_narrow_integration_time(); + if (d_veml) + { + d_local_code_shift_chips[0] = -d_trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[1] = -d_trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[3] = d_trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[4] = d_trk_parameters.very_early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_trk_parameters.spc = d_trk_parameters.early_late_space_narrow_chips; + // if (std::string(d_trk_parameters.signal) == "E1") + // { + // d_trk_parameters.slope = -CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc); + // d_trk_parameters.y_intercept = GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc); + // } + } + else + { + d_local_code_shift_chips[0] = -d_trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_local_code_shift_chips[2] = d_trk_parameters.early_late_space_narrow_chips * static_cast(d_code_samples_per_chip); + d_trk_parameters.spc = d_trk_parameters.early_late_space_narrow_chips; + } + } + else + { + d_state = 4; + } + } + } + break; + } + case 3: // coherent integration (correlation time extension) + { + // perform a correlation step + do_correlation_step(in); + save_correlation_results(); + update_tracking_vars(); + if (d_current_data_symbol == 0) + { + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + if (d_interchange_iq) + { + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.imag()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.real()); + } + else + { + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + } + 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_kf_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + d_extend_correlation_symbols_count++; + if (d_extend_correlation_symbols_count == (d_trk_parameters.extend_correlation_symbols - 1)) + { + d_extend_correlation_symbols_count = 0; + d_state = 4; + } + break; + } + case 4: // narrow tracking + { + // perform a correlation step + do_correlation_step(in); + save_correlation_results(); + + // check lock status + if (!cn0_and_tracking_lock_status(d_code_period * static_cast(d_trk_parameters.extend_correlation_symbols))) + { + clear_tracking_vars(); + d_state = 0; // loss-of-lock detected + } + else + { + if (d_trk_parameters.use_estimated_cn0 == true) + { + if (d_CN0_SNV_dB_Hz > 0) + { + update_kf_cn0(d_CN0_SNV_dB_Hz); + } + } + run_Kf(); + update_tracking_vars(); + check_carrier_phase_coherent_initialization(); + if (d_current_data_symbol == 0) + { + // enable write dump file this cycle (valid DLL/PLL cycle) + log_data(); + // ########### Output the tracking results to Telemetry block ########## + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + if (d_interchange_iq) + { + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.imag()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.real()); + } + else + { + current_synchro_data.Prompt_I = static_cast(d_P_data_accu.real()); + current_synchro_data.Prompt_Q = static_cast(d_P_data_accu.imag()); + } + 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_kf_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.correlation_length_ms = d_correlation_length_ms; + current_synchro_data.Flag_valid_symbol_output = true; + d_P_data_accu = gr_complex(0.0, 0.0); + } + + // reset extended correlator + d_VE_accu = gr_complex(0.0, 0.0); + d_E_accu = gr_complex(0.0, 0.0); + d_P_accu = gr_complex(0.0, 0.0); + d_L_accu = gr_complex(0.0, 0.0); + d_VL_accu = gr_complex(0.0, 0.0); + if (d_enable_extended_integration) + { + d_state = 3; // new coherent integration (correlation time extension) cycle + } + } + } + } + consume_each(d_current_prn_length_samples); + d_sample_counter += static_cast(d_current_prn_length_samples); + if (current_synchro_data.Flag_valid_symbol_output) + { + current_synchro_data.fs = static_cast(d_trk_parameters.fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + *out[0] = current_synchro_data; + return 1; + } + return 0; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.h b/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.h new file mode 100644 index 000000000..76bf0c4c7 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.h @@ -0,0 +1,234 @@ +/*! + * \file kf_vtl_tracking.cc + * \brief Implementation of a Kalman filter based tracking with optional Vector + * Tracking Loop message receiver block. + * \author Javier Arribas, 2020. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * Copyright (C) 2010-2020 (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. + * + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_KF_VTL_TRACKING_H +#define GNSS_SDR_KF_VTL_TRACKING_H + +#include "cpu_multicorrelator_real_codes.h" +#include "exponential_smoother.h" +#include "gnss_block_interface.h" +#include "gnss_time.h" // for timetags produced by File_Timestamp_Signal_Source +#include "kf_conf.h" +#include "tracking_FLL_PLL_filter.h" // for PLL/FLL filter +#include "tracking_loop_filter.h" // for DLL filter +#include +#include +#include // for block +#include // for gr_complex +#include // for gr_vector_int, gr_vector... +#include // for pmt_t +#include // for volk_gnsssdr::vector +#include // for size_t +#include // for int32_t +#include // for ofstream +#include +#include // for string +#include // for typeid +#include // for pair + +class Gnss_Synchro; +class kf_vtl_tracking; + +#if GNURADIO_USES_STD_POINTERS +using kf_vtl_tracking_sptr = std::shared_ptr; +#else +using kf_vtl_tracking_sptr = boost::shared_ptr; +#endif + + +kf_vtl_tracking_sptr kf_vtl_make_tracking(const Kf_Conf &conf_); + +/*! + * \brief This class implements a code DLL + carrier PLL tracking block. + */ +class kf_vtl_tracking : public gr::block +{ +public: + ~kf_vtl_tracking(); + + void set_channel(uint32_t channel); + void set_gnss_synchro(Gnss_Synchro *p_gnss_synchro); + void start_tracking(); + void stop_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 kf_vtl_tracking_sptr kf_vtl_make_tracking(const Kf_Conf &conf_); + explicit kf_vtl_tracking(const Kf_Conf &conf_); + + void init_kf(double acq_code_phase_chips, double acq_doppler_hz); + void update_kf_narrow_integration_time(); + void update_kf_cn0(double current_cn0_dbhz); + void run_Kf(); + + void msg_handler_telemetry_to_trk(const pmt::pmt_t &msg); + void msg_handler_pvt_to_trk(const pmt::pmt_t &msg); + void do_correlation_step(const gr_complex *input_samples); + + void check_carrier_phase_coherent_initialization(); + void update_tracking_vars(); + void clear_tracking_vars(); + void save_correlation_results(); + void log_data(); + bool cn0_and_tracking_lock_status(double coh_integration_time_s); + bool acquire_secondary(); + int32_t save_matfile() const; + + Cpu_Multicorrelator_Real_Codes d_multicorrelator_cpu; + Cpu_Multicorrelator_Real_Codes d_correlator_data_cpu; // for data channel + + Kf_Conf d_trk_parameters; + + Exponential_Smoother d_cn0_smoother; + Exponential_Smoother d_carrier_lock_test_smoother; + + Gnss_Synchro *d_acquisition_gnss_synchro; + + volk_gnsssdr::vector d_tracking_code; + volk_gnsssdr::vector d_data_code; + volk_gnsssdr::vector d_local_code_shift_chips; + volk_gnsssdr::vector d_correlator_outs; + volk_gnsssdr::vector d_Prompt_Data; + volk_gnsssdr::vector d_Prompt_buffer; + + boost::circular_buffer d_Prompt_circular_buffer; + + const size_t d_int_type_hash_code = typeid(int).hash_code(); + + // Kalman Filter class variables + arma::mat d_F; + arma::mat d_H; + arma::mat d_R; + arma::mat d_Q; + arma::mat d_P_old_old; + arma::mat d_P_new_old; + arma::mat d_P_new_new; + arma::vec d_x_old_old; + arma::vec d_x_new_old; + arma::vec x_new_new; + + // nominal signal parameters + double d_signal_carrier_freq; + double d_code_period; + double d_code_chip_rate; + + // acquisition + double d_acq_code_phase_samples; + double d_acq_carrier_doppler_hz; + double d_current_correlation_time_s; + + // carrier and code discriminators output + double d_carr_phase_error_disc_hz; + double d_code_error_disc_chips; + + // estimated parameters + // code + double d_code_error_kf_chips; + double d_code_freq_kf_chips_s; + // carrier + double d_carrier_phase_kf_rad; + double d_carrier_doppler_kf_hz; + double d_carrier_doppler_rate_kf_hz_s; + + double d_acc_carrier_phase_rad; + + double d_T_chip_seconds; + double d_T_prn_seconds; + double d_T_prn_samples; + double d_K_blk_samples; + double d_carrier_lock_test; + double d_CN0_SNV_dB_Hz; + double d_carrier_lock_threshold; + + // carrier NCO + double d_carrier_phase_step_rad; + double d_carrier_phase_rate_step_rad; + + // code NCO + double d_code_phase_step_chips; + double d_code_phase_rate_step_chips; + double d_rem_code_phase_chips; + double d_rem_code_phase_samples; + + gr_complex *d_Very_Early; + gr_complex *d_Early; + gr_complex *d_Prompt; + gr_complex *d_Late; + gr_complex *d_Very_Late; + + gr_complex d_VE_accu; + gr_complex d_E_accu; + gr_complex d_P_accu; + gr_complex d_P_accu_old; + gr_complex d_L_accu; + gr_complex d_VL_accu; + gr_complex d_P_data_accu; + + std::string d_secondary_code_string; + std::string d_data_secondary_code_string; + std::string d_systemName; + std::string d_signal_type; + std::string d_signal_pretty_name; + std::string d_dump_filename; + + std::ofstream d_dump_file; + + uint64_t d_sample_counter; + uint64_t d_acq_sample_stamp; + + float *d_prompt_data_shift; + float d_rem_carr_phase_rad; + + int32_t d_symbols_per_bit; + int32_t d_preamble_length_symbols; + int32_t d_state; + int32_t d_correlation_length_ms; + int32_t d_n_correlator_taps; + int32_t d_current_prn_length_samples; + int32_t d_extend_correlation_symbols_count; + int32_t d_current_symbol; + int32_t d_current_data_symbol; + int32_t d_cn0_estimation_counter; + int32_t d_carrier_lock_fail_counter; + int32_t d_code_lock_fail_counter; + int32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled) + int32_t d_code_length_chips; + + uint32_t d_channel; + uint32_t d_secondary_code_length; + uint32_t d_data_secondary_code_length; + + bool d_pull_in_transitory; + bool d_corrected_doppler; + bool d_interchange_iq; + bool d_veml; + bool d_cloop; + bool d_secondary; + bool d_dump; + bool d_dump_mat; + bool d_acc_carrier_phase_initialized; + bool d_enable_extended_integration; +}; + +#endif // GNSS_SDR_KF_VTL_TRACKING_H diff --git a/src/algorithms/tracking/libs/CMakeLists.txt b/src/algorithms/tracking/libs/CMakeLists.txt index ad460a8e7..9f7c42f7b 100644 --- a/src/algorithms/tracking/libs/CMakeLists.txt +++ b/src/algorithms/tracking/libs/CMakeLists.txt @@ -18,6 +18,7 @@ set(TRACKING_LIB_SOURCES tracking_FLL_PLL_filter.cc tracking_loop_filter.cc dll_pll_conf.cc + kf_conf.cc bayesian_estimation.cc exponential_smoother.cc ) @@ -35,6 +36,7 @@ set(TRACKING_LIB_HEADERS tracking_FLL_PLL_filter.h tracking_loop_filter.h dll_pll_conf.h + kf_conf.h bayesian_estimation.h exponential_smoother.h ) diff --git a/src/algorithms/tracking/libs/kf_conf.cc b/src/algorithms/tracking/libs/kf_conf.cc new file mode 100644 index 000000000..ddbaa8fc6 --- /dev/null +++ b/src/algorithms/tracking/libs/kf_conf.cc @@ -0,0 +1,164 @@ +/*! + * \file Kf_conf.cc + * \brief Class that contains all the configuration parameters for generic + * tracking block based on a DLL and a PLL. + * \author Javier Arribas, 2018. jarribas(at)cttc.es + * + * ----------------------------------------------------------------------------- + * + * Copyright (C) 2010-2020 (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. + * + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#include "kf_conf.h" +#include "gnss_sdr_flags.h" +#include "item_type_helpers.h" +#include + + +Kf_Conf::Kf_Conf() +{ + /*KF tracking configuration */ + high_dyn = false; + smoother_length = 10; + fs_in = 2000000.0; + vector_length = 0U; + dump = false; + dump_mat = true; + dump_filename = std::string("./Kf_dump.dat"); + + pull_in_time_s = 10; + bit_synchronization_time_limit_s = pull_in_time_s + 60; + early_late_space_chips = 0.25; + very_early_late_space_chips = 0.5; + early_late_space_narrow_chips = 0.15; + very_early_late_space_narrow_chips = 0.5; + slope = 1.0; + spc = 0.5; + y_intercept = 1.0; + carrier_aiding = true; + extend_correlation_symbols = 1; + cn0_samples = FLAGS_cn0_samples; + cn0_smoother_samples = 200; + cn0_smoother_alpha = 0.002; + carrier_lock_test_smoother_alpha = 0.002; + carrier_lock_test_smoother_samples = 25; + cn0_min = FLAGS_cn0_min; + max_carrier_lock_fail = FLAGS_max_carrier_lock_fail; + max_code_lock_fail = FLAGS_max_lock_fail; + carrier_lock_th = FLAGS_carrier_lock_th; + track_pilot = true; + enable_doppler_correction = false; + system = 'G'; + signal[0] = '1'; + signal[1] = 'C'; + signal[2] = '\0'; + item_type = "gr_complex"; + + expected_cn0_dbhz = 0; + // System covariances (Q) + code_phase_sd_chips = 0; + code_rate_sd_chips_s = 0; + carrier_phase_sd_rad = 0; + carrier_freq_sd_hz = 0; + carrier_freq_rate_sd_hz_s = 0; + // initial Kalman covariance matrix (P) + init_code_phase_sd_chips = 0; + init_code_rate_sd_chips_s = 0; + init_carrier_phase_sd_rad = 0; + init_carrier_freq_sd_hz = 0; + init_carrier_freq_rate_sd_hz_s = 0; + + enable_dynamic_measurement_covariance = false; + use_estimated_cn0 = false; +} + + +void Kf_Conf::SetFromConfiguration(const ConfigurationInterface *configuration, + const std::string &role) +{ + item_type = configuration->property(role + ".item_type", item_type); + if (!item_type_valid(item_type)) + { + LOG(WARNING) << "Unknown item type: " + item_type << ". Set to gr_complex"; + item_type = "gr_complex"; + } + + double fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", fs_in); + fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + high_dyn = configuration->property(role + ".high_dyn", high_dyn); + dump = configuration->property(role + ".dump", dump); + dump_filename = configuration->property(role + ".dump_filename", dump_filename); + dump_mat = configuration->property(role + ".dump_mat", dump_mat); + + pull_in_time_s = configuration->property(role + ".pull_in_time_s", pull_in_time_s); + bit_synchronization_time_limit_s = pull_in_time_s + 60; + early_late_space_chips = configuration->property(role + ".early_late_space_chips", early_late_space_chips); + early_late_space_narrow_chips = configuration->property(role + ".early_late_space_narrow_chips", early_late_space_narrow_chips); + very_early_late_space_chips = configuration->property(role + ".very_early_late_space_chips", very_early_late_space_chips); + very_early_late_space_narrow_chips = configuration->property(role + ".very_early_late_space_narrow_chips", very_early_late_space_narrow_chips); + extend_correlation_symbols = configuration->property(role + ".extend_correlation_symbols", extend_correlation_symbols); + track_pilot = configuration->property(role + ".track_pilot", track_pilot); + cn0_samples = configuration->property(role + ".cn0_samples", cn0_samples); + cn0_min = configuration->property(role + ".cn0_min", cn0_min); + max_code_lock_fail = configuration->property(role + ".max_lock_fail", max_code_lock_fail); + max_carrier_lock_fail = configuration->property(role + ".max_carrier_lock_fail", max_carrier_lock_fail); + carrier_lock_th = configuration->property(role + ".carrier_lock_th", carrier_lock_th); + carrier_aiding = configuration->property(role + ".carrier_aiding", carrier_aiding); + + // tracking lock tests smoother parameters + cn0_smoother_samples = configuration->property(role + ".cn0_smoother_samples", cn0_smoother_samples); + cn0_smoother_alpha = configuration->property(role + ".cn0_smoother_alpha", cn0_smoother_alpha); + smoother_length = configuration->property(role + ".smoother_length", smoother_length); + if (smoother_length < 1) + { + smoother_length = 1; + LOG(WARNING) << "smoother_length must be bigger than 0. It has been set to 1"; + } + carrier_lock_test_smoother_samples = configuration->property(role + ".carrier_lock_test_smoother_samples", carrier_lock_test_smoother_samples); + carrier_lock_test_smoother_alpha = configuration->property(role + ".carrier_lock_test_smoother_alpha", carrier_lock_test_smoother_alpha); + + // Kalman filter covariances + + // Measurement covariances (R) + expected_cn0_dbhz = configuration->property(role + ".expected_cn0_dbhz", 42.0); + + code_disc_sd_chips = configuration->property(role + ".code_disc_sd_chips", 0.01); + carrier_disc_sd_rads = configuration->property(role + ".carrier_disc_sd_rads", 0.1); + + enable_dynamic_measurement_covariance = configuration->property(role + ".enable_dynamic_measurement_covariance", false); + use_estimated_cn0 = configuration->property(role + ".use_estimated_cn0", false); + + // System covariances (Q) + code_phase_sd_chips = configuration->property(role + ".code_phase_sd_chips", 0.001); + code_rate_sd_chips_s = configuration->property(role + ".code_rate_sd_chips_s", 0.001); + + carrier_phase_sd_rad = configuration->property(role + ".carrier_phase_sd_rad", 0.001); + carrier_freq_sd_hz = configuration->property(role + ".carrier_freq_sd_hz", 0.1); + carrier_freq_rate_sd_hz_s = configuration->property(role + ".carrier_freq_rate_sd_hz_s", 1); + + // System covariances (narrow) (Q) + narrow_code_phase_sd_chips = configuration->property(role + ".narrow_code_phase_sd_chips", 0.001); + narrow_code_rate_sd_chips_s = configuration->property(role + ".narrow_code_rate_sd_chips_s", 0.001); + + narrow_carrier_phase_sd_rad = configuration->property(role + ".narrow_carrier_phase_sd_rad", 0.001); + narrow_carrier_freq_sd_hz = configuration->property(role + ".narrow_carrier_freq_sd_hz", 0.1); + narrow_carrier_freq_rate_sd_hz_s = configuration->property(role + ".narrow_carrier_freq_rate_sd_hz_s", 1); + + + // initial Kalman covariance matrix (P) + init_code_phase_sd_chips = configuration->property(role + ".init_code_phase_sd_chips", 1); + init_code_rate_sd_chips_s = configuration->property(role + ".init_code_rate_sd_chips_s", 100); + + init_carrier_phase_sd_rad = configuration->property(role + ".init_carrier_phase_sd_rad", 10); + init_carrier_freq_sd_hz = configuration->property(role + ".init_carrier_freq_sd_hz", 1000); + init_carrier_freq_rate_sd_hz_s = configuration->property(role + ".init_carrier_freq_rate_sd_hz_s", 1000); +} diff --git a/src/algorithms/tracking/libs/kf_conf.h b/src/algorithms/tracking/libs/kf_conf.h new file mode 100644 index 000000000..ee0c6bc32 --- /dev/null +++ b/src/algorithms/tracking/libs/kf_conf.h @@ -0,0 +1,104 @@ +/*! + * \file Kf_conf.h + * \brief Class that contains all the configuration parameters for generic tracking block based on a Kalman Filter. + * \author Javier Arribas, 2020. jarribas(at)cttc.es + * + * Class that contains all the configuration parameters for generic tracking block based on a DLL and a PLL. + * + * ----------------------------------------------------------------------------- + * + * Copyright (C) 2010-2020 (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. + * + * SPDX-License-Identifier: GPL-3.0-or-later + * + * ----------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_Kf_CONF_H +#define GNSS_SDR_Kf_CONF_H + +#include "configuration_interface.h" +#include +#include + +class Kf_Conf +{ +public: + Kf_Conf(); + void SetFromConfiguration(const ConfigurationInterface *configuration, const std::string &role); + + std::string item_type; + std::string dump_filename; + double fs_in; + double carrier_lock_th; + float early_late_space_chips; + float very_early_late_space_chips; + float early_late_space_narrow_chips; + float very_early_late_space_narrow_chips; + float slope; + float spc; + float y_intercept; + float cn0_smoother_alpha; + float carrier_lock_test_smoother_alpha; + uint32_t pull_in_time_s; + uint32_t bit_synchronization_time_limit_s; + uint32_t vector_length; + uint32_t smoother_length; + int32_t extend_correlation_symbols; + int32_t cn0_samples; + int32_t cn0_smoother_samples; + int32_t carrier_lock_test_smoother_samples; + int32_t cn0_min; + int32_t max_code_lock_fail; + int32_t max_carrier_lock_fail; + char signal[3]{}; + char system; + bool track_pilot; + bool enable_doppler_correction; + bool carrier_aiding; + bool high_dyn; + bool dump; + bool dump_mat; + + // KF statistics + // states: code_phase_chips, carrier_phase_rads, carrier_freq_hz, carrier_freq_rate_hz_s, code_freq_rate_chips_s + // Measurement covariances (R) + double expected_cn0_dbhz; + + double code_disc_sd_chips; + double carrier_disc_sd_rads; + + // System covariances (Q) + double code_phase_sd_chips; + double code_rate_sd_chips_s; + + double carrier_phase_sd_rad; + double carrier_freq_sd_hz; + double carrier_freq_rate_sd_hz_s; + + // System covariances (narrow) (Q) + double narrow_code_phase_sd_chips; + double narrow_code_rate_sd_chips_s; + + double narrow_carrier_phase_sd_rad; + double narrow_carrier_freq_sd_hz; + double narrow_carrier_freq_rate_sd_hz_s; + + // initial Kalman covariance matrix (P) + double init_code_phase_sd_chips; + double init_code_rate_sd_chips_s; + + double init_carrier_phase_sd_rad; + double init_carrier_freq_sd_hz; + double init_carrier_freq_rate_sd_hz_s; + + bool enable_dynamic_measurement_covariance; + bool use_estimated_cn0; +}; + +#endif diff --git a/src/core/libs/CMakeLists.txt b/src/core/libs/CMakeLists.txt index cbfed507c..ac8fdea75 100644 --- a/src/core/libs/CMakeLists.txt +++ b/src/core/libs/CMakeLists.txt @@ -88,6 +88,7 @@ target_link_libraries(core_libs core_libs_supl core_system_parameters pvt_libs + algorithms_libs PRIVATE algorithms_libs Boost::serialization diff --git a/src/core/libs/gnss_sdr_sample_counter.cc b/src/core/libs/gnss_sdr_sample_counter.cc index dc1412d41..05ff03745 100644 --- a/src/core/libs/gnss_sdr_sample_counter.cc +++ b/src/core/libs/gnss_sdr_sample_counter.cc @@ -17,12 +17,16 @@ #include "gnss_sdr_sample_counter.h" #include "gnss_synchro.h" +#include "gnss_time.h" #include #include // for from_double #include // for mp #include // for round #include // for operator<< -#include // for string +#include +#include // for string +#include + gnss_sdr_sample_counter::gnss_sdr_sample_counter( double _fs, @@ -48,6 +52,7 @@ gnss_sdr_sample_counter::gnss_sdr_sample_counter( flag_m = false; flag_h = false; flag_days = false; + set_tag_propagation_policy(TPP_DONT); // no tag propagation, the time tag will be adjusted and regenerated in work() } @@ -58,6 +63,14 @@ gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(double _fs, int32_t _i } +int64_t gnss_sdr_sample_counter::uint64diff(uint64_t first, uint64_t second) +{ + uint64_t abs_diff = (first > second) ? (first - second) : (second - first); + assert(abs_diff <= INT64_MAX); + return (first > second) ? (int64_t)abs_diff : -(int64_t)abs_diff; +} + + int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), gr_vector_const_void_star &input_items __attribute__((unused)), gr_vector_void_star &output_items) @@ -129,5 +142,46 @@ int gnss_sdr_sample_counter::work(int noutput_items __attribute__((unused)), sample_counter += samples_per_output; out[0].Tracking_sample_counter = sample_counter; current_T_rx_ms += interval_ms; + + //**************** time tags **************** + std::vector tags_vec; + // notice that nitems_read is updated in decimation blocks after leaving work() with return 1, equivalent to call consume_each + this->get_tags_in_range(tags_vec, 0, this->nitems_read(0), this->nitems_read(0) + samples_per_output); + for (const auto &it : tags_vec) + { + try + { + if (pmt::any_ref(it.value).type().hash_code() == typeid(const std::shared_ptr).hash_code()) + { + // recompute timestamp to match the last sample in the consumed samples in this batch + int64_t diff_samplecount = uint64diff(out[0].Tracking_sample_counter, it.offset); + const auto last_timetag = boost::any_cast>(pmt::any_ref(it.value)); + double intpart; + last_timetag->tow_ms_fraction += modf(1000.0 * static_cast(diff_samplecount) / fs, &intpart); + + last_timetag->tow_ms = last_timetag->tow_ms + static_cast(intpart); + last_timetag->rx_time = static_cast(out[0].Tracking_sample_counter) / fs; + add_item_tag(0, this->nitems_written(0) + 1, pmt::mp("timetag"), pmt::make_any(last_timetag)); + // std::cout << "COUNTER TAG: this->nitems_read(0):" << this->nitems_read(0) << " sample_counter:" << sample_counter + // << " it->offset:" << it->offset << " diff:" << diff_samplecount << "\n"; + // getchar(); + } + else + { + std::cout << "hash code not match\n"; + } + } + catch (const boost::bad_any_cast &e) + { + std::cout << "msg Bad any_cast: " << e.what(); + } + catch (const std::exception &ee) + { + return 1; + } + } + + //************* end time tags ************** + return 1; } diff --git a/src/core/libs/gnss_sdr_sample_counter.h b/src/core/libs/gnss_sdr_sample_counter.h index 40e54d47e..049be6345 100644 --- a/src/core/libs/gnss_sdr_sample_counter.h +++ b/src/core/libs/gnss_sdr_sample_counter.h @@ -58,6 +58,8 @@ private: int32_t _interval_ms, size_t _size); + int64_t uint64diff(uint64_t first, uint64_t second); + double fs; int64_t current_T_rx_ms; // Receiver time in ms since the beginning of the run uint64_t sample_counter; diff --git a/src/core/receiver/CMakeLists.txt b/src/core/receiver/CMakeLists.txt index 8e291a872..b2eacfcfd 100644 --- a/src/core/receiver/CMakeLists.txt +++ b/src/core/receiver/CMakeLists.txt @@ -104,6 +104,10 @@ if(ENABLE_OSMOSDR) endif() endif() +if(ENABLE_LIMESDR) + target_compile_definitions(core_receiver PRIVATE -DLIMESDR_DRIVER=1) +endif() + if(ENABLE_ARRAY) target_compile_definitions(core_receiver PRIVATE -DRAW_ARRAY_DRIVER=1) endif() diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 1004fc51c..6a92df891 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -39,6 +39,7 @@ #include "direct_resampler_conditioner.h" #include "fifo_signal_source.h" #include "file_signal_source.h" +#include "file_timestamp_signal_source.h" #include "fir_filter.h" #include "freq_xlating_fir_filter.h" #include "galileo_e1_dll_pll_veml_tracking.h" @@ -72,6 +73,7 @@ #include "gnss_sdr_string_literals.h" #include "gps_l1_ca_dll_pll_tracking.h" #include "gps_l1_ca_kf_tracking.h" +#include "gps_l1_ca_kf_vtl_tracking.h" #include "gps_l1_ca_pcps_acquisition.h" #include "gps_l1_ca_pcps_acquisition_fine_doppler.h" #include "gps_l1_ca_pcps_assisted_acquisition.h" @@ -161,6 +163,10 @@ #include "ad9361_fpga_signal_source.h" #endif +#if LIMESDR_DRIVER +#include "limesdr_signal_source.h" +#endif + #if FLEXIBAND_DRIVER #include "flexiband_signal_source.h" #endif @@ -656,6 +662,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams, queue); block = std::move(block_); } + else if (implementation == "File_Timestamp_Signal_Source") + { + std::unique_ptr block_ = std::make_unique(configuration, role, in_streams, + out_streams, queue); + block = std::move(block_); + } else if (implementation == "Multichannel_File_Signal_Source") { std::unique_ptr block_ = std::make_unique(configuration, role, in_streams, @@ -739,6 +751,16 @@ std::unique_ptr GNSSBlockFactory::GetBlock( } #endif +#if LIMESDR_DRIVER + else if (implementation == "Limesdr_Signal_Source") + { + std::unique_ptr + block_ = std::make_unique(configuration, role, in_streams, + out_streams, queue); + block = std::move(block_); + } +#endif + #if PLUTOSDR_DRIVER else if (implementation == "Plutosdr_Signal_Source") { @@ -1049,6 +1071,12 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams); block = std::move(block_); } + else if (implementation == "GPS_L1_CA_KF_VTL_Tracking") + { + std::unique_ptr block_ = std::make_unique(configuration, role, in_streams, + out_streams); + block = std::move(block_); + } else if (implementation == "GPS_L1_CA_TCP_CONNECTOR_Tracking") { std::unique_ptr block_ = std::make_unique(configuration, role, in_streams, @@ -1499,6 +1527,12 @@ std::unique_ptr GNSSBlockFactory::GetTrkBlock( out_streams); block = std::move(block_); } + else if (implementation == "GPS_L1_CA_KF_VTL_Tracking") + { + std::unique_ptr block_ = std::make_unique(configuration, role, in_streams, + out_streams); + block = std::move(block_); + } else if (implementation == "GPS_L1_CA_TCP_CONNECTOR_Tracking") { std::unique_ptr block_ = std::make_unique(configuration, role, in_streams, diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index a7d138880..3a15b03a0 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -1437,6 +1437,18 @@ int GNSSFlowgraph::connect_observables_to_pvt() { top_block_->connect(observables_->get_right_block(), i, pvt_->get_left_block(), i); top_block_->msg_connect(channels_.at(i)->get_right_block(), pmt::mp("telemetry"), pvt_->get_left_block(), pmt::mp("telemetry")); + // experimental Vector Tracking Loop (VTL) messages from PVT to Tracking blocks + // not supported by all tracking algorithms + pmt::pmt_t ports_in = channels_.at(i)->get_left_block_trk()->message_ports_in(); + for (size_t n = 0; n < pmt::length(ports_in); n++) + { + // std::cout << "pmt: " << pmt::symbol_to_string(pmt::vector_ref(ports_in, n)) << "\n"; + if (pmt::symbol_to_string(pmt::vector_ref(ports_in, n)) == "pvt_to_trk") + { + top_block_->msg_connect(pvt_->get_left_block(), pmt::mp("pvt_to_trk"), channels_.at(i)->get_left_block_trk(), pmt::mp("pvt_to_trk")); + LOG(INFO) << "pvt_to_trk message port connected in " << channels_.at(i)->implementation(); + } + } } top_block_->msg_connect(observables_->get_right_block(), pmt::mp("status"), channels_status_, pmt::mp("status")); diff --git a/src/core/system_parameters/gnss_satellite.cc b/src/core/system_parameters/gnss_satellite.cc index 4cf372e7d..ff6af54dd 100644 --- a/src/core/system_parameters/gnss_satellite.cc +++ b/src/core/system_parameters/gnss_satellite.cc @@ -552,6 +552,9 @@ std::string Gnss_Satellite::what_block(const std::string& system_, uint32_t PRN_ case 9: block_ = std::string("FOC-FM9"); // Galileo Full Operational Capability (FOC) satellite FM9 / GSAT0209, launched on December 17, 2015. break; + case 10: + block_ = std::string("FOC-FM24"); // Galileo Full Operational Capability (FOC) satellite FM24 / GSAT0224, launched on December 5, 2021. + break; case 11: block_ = std::string("IOV-PFM"); // PFM, the ProtoFlight Model / GSAT0101, launched from French Guiana at 10:30 GMT on October 21, 2011. break; @@ -603,6 +606,9 @@ std::string Gnss_Satellite::what_block(const std::string& system_, uint32_t PRN_ case 33: block_ = std::string("FOC-FM22"); // Galileo Full Operational Capability (FOC) satellite FM22 / GSAT0222, launched on Jul. 25, 2018. UNDER COMMISSIONING. break; + case 34: + block_ = std::string("FOC-FM23"); // Galileo Full Operational Capability (FOC) satellite FM23 / GSAT0223, launched on December 5, 2021. + break; case 36: block_ = std::string("FOC-FM19"); // Galileo Full Operational Capability (FOC) satellite FM19 / GSAT0219, launched on Jul. 25, 2018. UNDER COMMISSIONING. break; diff --git a/src/utils/matlab/dll_pll_veml_plot_sample.m b/src/utils/matlab/dll_pll_veml_plot_sample.m index 16d7a2ec6..6e365899a 100644 --- a/src/utils/matlab/dll_pll_veml_plot_sample.m +++ b/src/utils/matlab/dll_pll_veml_plot_sample.m @@ -21,13 +21,13 @@ if ~exist('dll_pll_veml_read_tracking_dump.m', 'file') addpath('./libs') end -samplingFreq = 2048000; %[Hz] -plot_last_outputs=0; +samplingFreq = 3000000; %[Hz] +plot_last_outputs=0;%1000; -channels = 8; % Number of channels +channels = 1; % Number of channels first_channel = 0; % Number of the first channel -path = '/Users/javier/git/gnss-sdr/build/test_postpro_24h_casa/'; %% CHANGE THIS PATH +path = '/home/javier/git/gnss-sdr/install/test_inta/'; %% CHANGE THIS PATH for N=1:1:channels tracking_log_path = [path 'tracking_ch_' num2str(N+first_channel-1) '.dat']; %% CHANGE track_ch_ BY YOUR dump_filename