diff --git a/conf/gnss-sdr.conf b/conf/gnss-sdr.conf index 88a66f3ee..f56a720a6 100644 --- a/conf/gnss-sdr.conf +++ b/conf/gnss-sdr.conf @@ -253,8 +253,8 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -264,14 +264,13 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: -PVT.implementation=GPS_L1_CA_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=100 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.AR_GPS=PPP-AR ; options: OFF, Continuous, Instantaneous, Fix-and-Hold, PPP-AR ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=10 @@ -279,6 +278,8 @@ PVT.output_rate_ms=10 ;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms <= display_rate_ms. PVT.display_rate_ms=500 +PVT.positioning_mode=PPP_Static + ;# KML, GeoJSON, NMEA and RTCM output configuration ;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump, ".kml" and ".geojson" to GIS-friendly formats. @@ -304,5 +305,3 @@ PVT.rtcm_dump_devname=/dev/pts/1 ;#dump: Enable or disable the PVT internal binary data file logging [true] or [false] PVT.dump=false - - diff --git a/conf/gnss-sdr_GPS_L1_GN3S_realtime.conf b/conf/gnss-sdr_GPS_L1_GN3S_realtime.conf index a1caf56c4..8f3aed872 100644 --- a/conf/gnss-sdr_GPS_L1_GN3S_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_GN3S_realtime.conf @@ -89,17 +89,17 @@ Tracking_1C.order=3; ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false. Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GPS_L1_CA_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=10 PVT.display_rate_ms=500 PVT.dump_filename=./PVT @@ -110,4 +110,3 @@ PVT.dump=false PVT.flag_rtcm_server=false PVT.flag_rtcm_tty_port=false PVT.rtcm_dump_devname=/dev/pts/1 - diff --git a/conf/gnss-sdr_GPS_L1_SPIR.conf b/conf/gnss-sdr_GPS_L1_SPIR.conf index 38a4ce838..5e4335b00 100644 --- a/conf/gnss-sdr_GPS_L1_SPIR.conf +++ b/conf/gnss-sdr_GPS_L1_SPIR.conf @@ -266,8 +266,8 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -278,13 +278,10 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ ;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT - -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=100 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=500 diff --git a/conf/gnss-sdr_GPS_L1_USRP_X300_realtime.conf b/conf/gnss-sdr_GPS_L1_USRP_X300_realtime.conf index 04e1d2786..b71126374 100644 --- a/conf/gnss-sdr_GPS_L1_USRP_X300_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_USRP_X300_realtime.conf @@ -291,8 +291,8 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -302,14 +302,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_GPS_L1_USRP_realtime.conf b/conf/gnss-sdr_GPS_L1_USRP_realtime.conf index 8fcb655f8..22584d4e4 100644 --- a/conf/gnss-sdr_GPS_L1_USRP_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_USRP_realtime.conf @@ -332,8 +332,8 @@ TelemetryDecoder_GPS.dump=false TelemetryDecoder_GPS.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -343,14 +343,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_GPS_L1_acq_QuickSync.conf b/conf/gnss-sdr_GPS_L1_acq_QuickSync.conf index cc3ba40a9..e4177f18e 100644 --- a/conf/gnss-sdr_GPS_L1_acq_QuickSync.conf +++ b/conf/gnss-sdr_GPS_L1_acq_QuickSync.conf @@ -255,8 +255,8 @@ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -267,13 +267,11 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ ;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100; diff --git a/conf/gnss-sdr_GPS_L1_gr_complex.conf b/conf/gnss-sdr_GPS_L1_gr_complex.conf index 14b57ff0a..ca5af458c 100644 --- a/conf/gnss-sdr_GPS_L1_gr_complex.conf +++ b/conf/gnss-sdr_GPS_L1_gr_complex.conf @@ -6,16 +6,16 @@ ;######### GLOBAL OPTIONS ################## ;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz]. -GNSS-SDR.internal_fs_hz=4000000 +GNSS-SDR.internal_fs_hz=2600000 ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=File_Signal_Source -SignalSource.filename=/datalogger/signals/Agilent/New York/4msps.dat ; <- PUT YOUR FILE HERE -SignalSource.item_type=gr_complex -SignalSource.sampling_frequency=4000000 +SignalSource.filename=/home/javier/gnss/gnss-simulator/build/signal_out.bin ; <- PUT YOUR FILE HERE +SignalSource.item_type=byte +SignalSource.sampling_frequency=2600000 SignalSource.freq=1575420000 -SignalSource.samples=250000000 +SignalSource.samples=0 SignalSource.repeat=false SignalSource.dump=false SignalSource.dump_filename=../data/signal_source.dat @@ -23,8 +23,15 @@ SignalSource.enable_throttle_control=false ;######### SIGNAL_CONDITIONER CONFIG ############ -SignalConditioner.implementation=Pass_Through +SignalConditioner.implementation=Signal_Conditioner +;######### DATA_TYPE_ADAPTER CONFIG ############ +;## Changes the type of input data. Please disable it in this version. +;#implementation: [Pass_Through] disables this block +DataTypeAdapter.implementation=Ibyte_To_Complex +DataTypeAdapter.dump=false +;#dump_filename: Log path and filename. +DataTypeAdapter.dump_filename=../data/DataTypeAdapter.dat ;######### CHANNELS GLOBAL CONFIG ############ Channels_1C.count=8 @@ -39,38 +46,38 @@ Acquisition_1C.item_type=gr_complex Acquisition_1C.if=0 Acquisition_1C.sampled_ms=1 Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition -Acquisition_1C.threshold=0.005 +Acquisition_1C.threshold=0.05 ;Acquisition_1C.pfa=0.01 Acquisition_1C.doppler_max=10000 -Acquisition_1C.doppler_step=500 +Acquisition_1C.doppler_step=250 ;######### TRACKING GLOBAL CONFIG ############ -Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking Tracking_1C.item_type=gr_complex Tracking_1C.if=0 Tracking_1C.dump=false Tracking_1C.dump_filename=../data/epl_tracking_ch_ -Tracking_1C.pll_bw_hz=45.0; -Tracking_1C.dll_bw_hz=2.0; +Tracking_1C.pll_bw_hz=25.0; +Tracking_1C.dll_bw_hz=1.0; Tracking_1C.order=3; ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GPS_L1_CA_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false -PVT.output_rate_ms=10 -PVT.display_rate_ms=500 +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=1 +PVT.display_rate_ms=100 PVT.dump_filename=./PVT PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea; PVT.flag_nmea_tty_port=false; @@ -79,4 +86,3 @@ PVT.flag_rtcm_server=false PVT.flag_rtcm_tty_port=false PVT.rtcm_dump_devname=/dev/pts/1 PVT.dump=false - diff --git a/conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf b/conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf index 885bebd88..41e4b61bb 100644 --- a/conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf +++ b/conf/gnss-sdr_GPS_L1_gr_complex_gpu.conf @@ -57,18 +57,18 @@ Tracking_1C.order=3; ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GPS_L1_CA_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=10 PVT.display_rate_ms=500 PVT.dump_filename=./PVT @@ -79,4 +79,3 @@ PVT.flag_rtcm_server=false PVT.flag_rtcm_tty_port=false PVT.rtcm_dump_devname=/dev/pts/1 PVT.dump=false - diff --git a/conf/gnss-sdr_GPS_L1_ishort.conf b/conf/gnss-sdr_GPS_L1_ishort.conf index 95abfc06b..b7376135f 100644 --- a/conf/gnss-sdr_GPS_L1_ishort.conf +++ b/conf/gnss-sdr_GPS_L1_ishort.conf @@ -79,16 +79,17 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GPS_L1_CA_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false -PVT.output_rate_ms=10 +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=100 PVT.display_rate_ms=500 PVT.dump_filename=./PVT PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea; diff --git a/conf/gnss-sdr_GPS_L1_nsr.conf b/conf/gnss-sdr_GPS_L1_nsr.conf index f7d4358d3..076f46260 100644 --- a/conf/gnss-sdr_GPS_L1_nsr.conf +++ b/conf/gnss-sdr_GPS_L1_nsr.conf @@ -28,7 +28,7 @@ GNSS-SDR.SUPL_CI=0x31b0 ;######### SIGNAL_SOURCE CONFIG ############ SignalSource.implementation=Nsr_File_Signal_Source -SignalSource.filename=/datalogger/signals/ifen/E1L1_FE0_Band0.stream ; <- PUT YOUR FILE HERE +SignalSource.filename=/home/javier/Descargas/RoofTop_FE0_Band1.stream ; <- PUT YOUR FILE HERE SignalSource.item_type=byte SignalSource.sampling_frequency=20480000 SignalSource.freq=1575420000 @@ -68,7 +68,8 @@ InputFilter.band2_error=1.0 InputFilter.filter_type=bandpass InputFilter.grid_density=16 InputFilter.sampling_frequency=20480000 -InputFilter.IF=5499998.47412109 +#InputFilter.IF=5499998.47412109 +InputFilter.IF=5679999.2370605494 InputFilter.decimation_factor=8 @@ -81,9 +82,10 @@ Resampler.item_type=gr_complex ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_1C.count=8 +Channels_1C.count=0 +Channels_2S.count=8 Channels.in_acquisition=1 -Channel.signal=1C +#Channel.signal=1C ;######### GPS ACQUISITION CONFIG ############ @@ -98,6 +100,18 @@ Acquisition_1C.threshold=0.0075 Acquisition_1C.doppler_max=10000 Acquisition_1C.doppler_step=500 +Acquisition_2S.dump=false +Acquisition_2S.dump_filename=./acq_dump.dat +Acquisition_2S.item_type=gr_complex +Acquisition_2S.if=0 +Acquisition_2S.coherent_integration_time_ms=20 +Acquisition_2S.implementation=GPS_L2_M_PCPS_Acquisition +Acquisition_2S.threshold=0.00045 +Acquisition_2S.doppler_max=5000 +Acquisition_2S.doppler_step=100 +Acquisition_2S.bit_transition_flag=false +Acquisition_2S.max_dwells=1 + ;######### TRACKING GPS CONFIG ############ Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking @@ -109,22 +123,43 @@ Tracking_1C.pll_bw_hz=45.0; Tracking_1C.dll_bw_hz=2.0; Tracking_1C.order=3; +;######### GPS L2C GENERIC TRACKING CONFIG ############ +Tracking_2S.implementation=GPS_L2_M_DLL_PLL_Tracking +Tracking_2S.item_type=gr_complex +Tracking_2S.if=0 +Tracking_2S.dump=true +Tracking_2S.dump_filename=../data/epl_tracking_ch_ +Tracking_2S.pll_bw_hz=1.5; +Tracking_2S.dll_bw_hz=0.4; +Tracking_2S.order=2; +Tracking_2S.early_late_space_chips=0.5; + + ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; +TelemetryDecoder_2S.implementation=GPS_L2C_Telemetry_Decoder +TelemetryDecoder_2S.dump=false +TelemetryDecoder_2S.decimation_factor=1; + ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables + +;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false + +;#dump_filename: Log path and filename. Observables.dump_filename=./observables.dat - ;######### PVT CONFIG ############ -PVT.implementation=GPS_L1_CA_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false -PVT.output_rate_ms=10 +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=100 PVT.display_rate_ms=500 PVT.dump_filename=./PVT PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea; @@ -134,4 +169,3 @@ PVT.flag_rtcm_server=false PVT.flag_rtcm_tty_port=false PVT.rtcm_dump_devname=/dev/pts/1 PVT.dump=true - diff --git a/conf/gnss-sdr_GPS_L1_nsr_twobit_packed.conf b/conf/gnss-sdr_GPS_L1_nsr_twobit_packed.conf index 06cbb8e7d..809f2bcbb 100644 --- a/conf/gnss-sdr_GPS_L1_nsr_twobit_packed.conf +++ b/conf/gnss-sdr_GPS_L1_nsr_twobit_packed.conf @@ -127,18 +127,17 @@ Tracking_1C.order=3; ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat - ;######### PVT CONFIG ############ -PVT.implementation=GPS_L1_CA_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=10 PVT.display_rate_ms=500 PVT.dump_filename=./PVT @@ -149,4 +148,3 @@ PVT.flag_rtcm_server=false PVT.flag_rtcm_tty_port=false PVT.rtcm_dump_devname=/dev/pts/1 PVT.dump=true - diff --git a/conf/gnss-sdr_GPS_L1_pulse_blanking_gr_complex.conf b/conf/gnss-sdr_GPS_L1_pulse_blanking_gr_complex.conf index 9401dd8c8..d6222161e 100644 --- a/conf/gnss-sdr_GPS_L1_pulse_blanking_gr_complex.conf +++ b/conf/gnss-sdr_GPS_L1_pulse_blanking_gr_complex.conf @@ -30,20 +30,20 @@ SignalSource.implementation=File_Signal_Source ;#When left empty, the device discovery routines will search all vailable transports on the system (ethernet, usb...) SignalSource.device_address=192.168.50.2 - + ;#filename: path to file with the captured GNSS signal samples to be processed SignalSource.filename=/home/javier/signals/signal_source_int.dat ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. SignalSource.item_type=gr_complex -;#sampling_frequency: Original Signal sampling frequency in [Hz] +;#sampling_frequency: Original Signal sampling frequency in [Hz] SignalSource.sampling_frequency=2000000 -;#freq: RF front-end center frequency in [Hz] +;#freq: RF front-end center frequency in [Hz] SignalSource.freq=1575420000 -;#gain: Front-end Gain in [dB] +;#gain: Front-end Gain in [dB] SignalSource.gain=40 ;#subdevice: UHD subdevice specification (for USRP1 use A:0 or B:0) @@ -62,12 +62,12 @@ SignalSource.dump_filename=dump.dat ;#enable_throttle_control: Enabling this option tells the signal source to keep the delay between samples in post processing. -; it helps to not overload the CPU, but the processing time will be longer. +; it helps to not overload the CPU, but the processing time will be longer. SignalSource.enable_throttle_control=false ;######### SIGNAL_CONDITIONER CONFIG ############ -;## It holds blocks to change data type, filter and resample input data. +;## It holds blocks to change data type, filter and resample input data. ;#implementation: Use [Pass_Through] or [Signal_Conditioner] ;#[Pass_Through] disables this block and the [DataTypeAdapter], [InputFilter] and [Resampler] blocks @@ -157,19 +157,18 @@ Tracking_1C.order=3; ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables -Observables.averaging_depth=1 +Observables.implementation=Hybrid_Observables Observables.dump=true Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GPS_L1_CA_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=1 PVT.display_rate_ms=100 PVT.dump_filename=./PVT @@ -180,4 +179,3 @@ PVT.flag_rtcm_server=false PVT.flag_rtcm_tty_port=false PVT.rtcm_dump_devname=/dev/pts/1 PVT.dump=false - diff --git a/conf/gnss-sdr_GPS_L1_rtl_tcp_realtime.conf b/conf/gnss-sdr_GPS_L1_rtl_tcp_realtime.conf index 946176ce3..5b1ffe1d6 100644 --- a/conf/gnss-sdr_GPS_L1_rtl_tcp_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_rtl_tcp_realtime.conf @@ -243,8 +243,8 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -254,14 +254,11 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT - -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf b/conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf index 51f75907f..f7c934703 100644 --- a/conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf +++ b/conf/gnss-sdr_GPS_L1_rtlsdr_realtime.conf @@ -120,20 +120,17 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT - -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=100 PVT.display_rate_ms=500 PVT.dump_filename=./PVT diff --git a/conf/gnss-sdr_GPS_L1_two_bits_cpx.conf b/conf/gnss-sdr_GPS_L1_two_bits_cpx.conf index 0f2521786..b21abf2c7 100644 --- a/conf/gnss-sdr_GPS_L1_two_bits_cpx.conf +++ b/conf/gnss-sdr_GPS_L1_two_bits_cpx.conf @@ -113,15 +113,16 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -Observables.implementation=GPS_L1_CA_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GPS_L1_CA_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=10 PVT.display_rate_ms=500 PVT.dump_filename=./PVT diff --git a/conf/gnss-sdr_GPS_L2C_USRP1_realtime.conf b/conf/gnss-sdr_GPS_L2C_USRP1_realtime.conf index 3b2629bdd..e96fa0e94 100644 --- a/conf/gnss-sdr_GPS_L2C_USRP1_realtime.conf +++ b/conf/gnss-sdr_GPS_L2C_USRP1_realtime.conf @@ -180,18 +180,17 @@ Tracking_2S.early_late_space_chips=0.5; ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_2S.implementation=GPS_L2C_Telemetry_Decoder TelemetryDecoder_2S.dump=false -TelemetryDecoder_2S.decimation_factor=1; ;######### OBSERVABLES CONFIG ############. Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat - ;######### PVT CONFIG ############ -PVT.implementation=Hybrid_PVT -PVT.averaging_depth=10 -PVT.flag_averaging=true +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=100 PVT.display_rate_ms=500 PVT.dump_filename=./PVT diff --git a/conf/gnss-sdr_GPS_L2C_USRP_X300_realtime.conf b/conf/gnss-sdr_GPS_L2C_USRP_X300_realtime.conf index 838b7d505..4bebf9fcb 100644 --- a/conf/gnss-sdr_GPS_L2C_USRP_X300_realtime.conf +++ b/conf/gnss-sdr_GPS_L2C_USRP_X300_realtime.conf @@ -136,18 +136,17 @@ Tracking_2S.early_late_space_chips=0.5; ;######### TELEMETRY DECODER GPS CONFIG ############ TelemetryDecoder_2S.implementation=GPS_L2C_Telemetry_Decoder TelemetryDecoder_2S.dump=true -TelemetryDecoder_2S.decimation_factor=1; ;######### OBSERVABLES CONFIG ############. Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat - ;######### PVT CONFIG ############ -PVT.implementation=Hybrid_PVT -PVT.averaging_depth=10 -PVT.flag_averaging=true +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=100 PVT.display_rate_ms=500 PVT.dump_filename=./PVT diff --git a/conf/gnss-sdr_Galileo_E1_USRP_X300_realtime.conf b/conf/gnss-sdr_Galileo_E1_USRP_X300_realtime.conf index 5fbe59f95..a305cd277 100644 --- a/conf/gnss-sdr_Galileo_E1_USRP_X300_realtime.conf +++ b/conf/gnss-sdr_Galileo_E1_USRP_X300_realtime.conf @@ -62,14 +62,15 @@ TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder TelemetryDecoder_1B.dump=false ;######### OBSERVABLES CONFIG ############ -Observables.implementation=Galileo_E1B_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GALILEO_E1_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=false +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=100; PVT.display_rate_ms=500; PVT.dump=false diff --git a/conf/gnss-sdr_Galileo_E1_acq_QuickSync.conf b/conf/gnss-sdr_Galileo_E1_acq_QuickSync.conf index 59059be2b..9be99d94d 100644 --- a/conf/gnss-sdr_Galileo_E1_acq_QuickSync.conf +++ b/conf/gnss-sdr_Galileo_E1_acq_QuickSync.conf @@ -237,7 +237,7 @@ TelemetryDecoder_1B.dump=false ;######### OBSERVABLES CONFIG ############ ;#implementation: -Observables.implementation=Galileo_E1B_Observables +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -248,13 +248,10 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ ;#implementation: Position Velocity and Time (PVT) implementation algorithm: -PVT.implementation=GALILEO_E1_PVT - -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=100 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100; diff --git a/conf/gnss-sdr_Galileo_E1_ishort.conf b/conf/gnss-sdr_Galileo_E1_ishort.conf index e26cdf15f..cada1263c 100644 --- a/conf/gnss-sdr_Galileo_E1_ishort.conf +++ b/conf/gnss-sdr_Galileo_E1_ishort.conf @@ -239,8 +239,8 @@ TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder TelemetryDecoder_1B.dump=false ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=Galileo_E1B_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -250,14 +250,11 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GALILEO_E1_PVT - -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=100 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100; diff --git a/conf/gnss-sdr_Galileo_E1_nsr.conf b/conf/gnss-sdr_Galileo_E1_nsr.conf index a4b37d02e..c357bd013 100644 --- a/conf/gnss-sdr_Galileo_E1_nsr.conf +++ b/conf/gnss-sdr_Galileo_E1_nsr.conf @@ -106,15 +106,16 @@ TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder TelemetryDecoder_1B.dump=false ;######### OBSERVABLES CONFIG ############ -Observables.implementation=Galileo_E1B_Observables +Observables.implementation=Hybrid_Observables Observables.dump=true Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GALILEO_E1_PVT -PVT.averaging_depth=1 -PVT.flag_averaging=false +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=100 PVT.display_rate_ms=500 PVT.dump=true diff --git a/conf/gnss-sdr_Galileo_E5a.conf b/conf/gnss-sdr_Galileo_E5a.conf index fd4cfe55f..1528a5d2b 100644 --- a/conf/gnss-sdr_Galileo_E5a.conf +++ b/conf/gnss-sdr_Galileo_E5a.conf @@ -286,8 +286,7 @@ TelemetryDecoder_5X.dump=false ;######### OBSERVABLES CONFIG ############ ;#implementation: -;Use [Galileo_E1B_Observables] for E5a also. -Observables.implementation=Galileo_E1B_Observables +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -298,14 +297,11 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ ;#implementation: Position Velocity and Time (PVT) implementation algorithm: -;Use [GALILEO_E1_PVT] for E5a also. -PVT.implementation=GALILEO_E1_PVT +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=100 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +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 ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf b/conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf index f5c045cc6..6b733b4dd 100644 --- a/conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf +++ b/conf/gnss-sdr_Galileo_E5a_IFEN_CTTC.conf @@ -134,15 +134,16 @@ TelemetryDecoder_5X.implementation=Galileo_E5a_Telemetry_Decoder TelemetryDecoder_5X.dump=false ;######### OBSERVABLES CONFIG ############ -Observables.implementation=Galileo_E1B_Observables +Observables.implementation=Hybrid_Observables Observables.dump=false Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -PVT.implementation=GALILEO_E1_PVT -PVT.averaging_depth=100 -PVT.flag_averaging=true +PVT.implementation=RTKLIB_PVT +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=OFF ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=OFF ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad PVT.output_rate_ms=100 PVT.dump=false PVT.dump_filename=./PVT @@ -152,4 +153,3 @@ PVT.nmea_dump_devname=/dev/pts/4 PVT.flag_rtcm_server=true PVT.flag_rtcm_tty_port=false PVT.rtcm_dump_devname=/dev/pts/1 - diff --git a/conf/gnss-sdr_Hybrid_byte.conf b/conf/gnss-sdr_Hybrid_byte.conf index d4b2a6a16..047c2bc6f 100644 --- a/conf/gnss-sdr_Hybrid_byte.conf +++ b/conf/gnss-sdr_Hybrid_byte.conf @@ -295,10 +295,9 @@ TelemetryDecoder_1C.decimation_factor=4; ;#implementation: Use [Galileo_E1B_Telemetry_Decoder] for Galileo E1B TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder TelemetryDecoder_1B.dump=false -TelemetryDecoder_1B.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. +;#implementation: Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] @@ -309,14 +308,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100; diff --git a/conf/gnss-sdr_Hybrid_byte_sim.conf b/conf/gnss-sdr_Hybrid_byte_sim.conf index df7ae450a..32bea4e58 100644 --- a/conf/gnss-sdr_Hybrid_byte_sim.conf +++ b/conf/gnss-sdr_Hybrid_byte_sim.conf @@ -7,7 +7,8 @@ ;######### GLOBAL OPTIONS ################## ;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz]. -GNSS-SDR.internal_fs_hz=4000000 +;#GNSS-SDR.internal_fs_hz=2048000 +GNSS-SDR.internal_fs_hz=2600000 ;######### SIGNAL_SOURCE CONFIG ############ @@ -15,7 +16,8 @@ GNSS-SDR.internal_fs_hz=4000000 SignalSource.implementation=File_Signal_Source ;#filename: path to file with the captured GNSS signal samples to be processed -SignalSource.filename=/datalogger/signals/gnss-sim/signal_out.bin ; <- PUT YOUR FILE HERE +;#SignalSource.filename=/home/javier/Descargas/rtlsdr_tcxo_l1/rtlsdr_tcxo_l1.bin ; <- PUT YOUR FILE HERE +SignalSource.filename=/Users/carlesfernandez/git/cttc/build/signal_out.bin ; <- PUT YOUR FILE HERE ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. SignalSource.item_type=byte @@ -53,7 +55,7 @@ SignalConditioner.implementation=Signal_Conditioner ;######### DATA_TYPE_ADAPTER CONFIG ############ ;## Changes the type of input data. Please disable it in this version. ;#implementation: [Pass_Through] disables this block -DataTypeAdapter.implementation=Ibyte_To_Cshort +DataTypeAdapter.implementation=Ibyte_To_Complex DataTypeAdapter.dump=false ;#dump_filename: Log path and filename. DataTypeAdapter.dump_filename=../data/DataTypeAdapter.dat @@ -81,10 +83,10 @@ InputFilter.dump_filename=../data/input_filter.dat ;#These function calculates the optimal (in the Chebyshev/minimax sense) FIR filter inpulse reponse given a set of band edges, the desired reponse on those bands, and the weight given to the error in those bands. ;#input_item_type: Type and resolution for input signal samples. Use only gr_complex in this version. -InputFilter.input_item_type=cshort +InputFilter.input_item_type=gr_complex ;#outut_item_type: Type and resolution for output filtered signal samples. Use only gr_complex in this version. -InputFilter.output_item_type=cshort +InputFilter.output_item_type=gr_complex ;#taps_item_type: Type and resolution for the taps of the filter. Use only float in this version. InputFilter.taps_item_type=float @@ -127,7 +129,7 @@ InputFilter.grid_density=16 ;#The following options are used only in Freq_Xlating_Fir_Filter implementation. ;#InputFilter.IF is the intermediate frequency (in Hz) shifted down to zero Hz -InputFilter.sampling_frequency=4000000 +InputFilter.sampling_frequency=2600000 InputFilter.IF=0 @@ -135,30 +137,12 @@ InputFilter.IF=0 ;######### RESAMPLER CONFIG ############ ;## Resamples the input data. -;#implementation: Use [Pass_Through] or [Direct_Resampler] -;#[Pass_Through] disables this block -;#[Direct_Resampler] enables a resampler that implements a nearest neigbourhood interpolation -;Resampler.implementation=Direct_Resampler Resampler.implementation=Pass_Through - -;#dump: Dump the resamplered data to a file. -Resampler.dump=false -;#dump_filename: Log path and filename. -Resampler.dump_filename=../data/resampler.dat - -;#item_type: Type and resolution for each of the signal samples. -Resampler.item_type=cshort - -;#sample_freq_in: the sample frequency of the input signal -Resampler.sample_freq_in=4000000 - -;#sample_freq_out: the desired sample frequency of the output signal -Resampler.sample_freq_out=4000000 - +Resampler.item_type = gr_complex; ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_1C.count=12 +Channels_1C.count=11 ;#count: Number of available Galileo satellite channels. Channels_1B.count=0 ;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver @@ -167,7 +151,7 @@ Channels.in_acquisition=1 ;#IMPORTANT: When cshort is used as input type for Acq and Trk, please set the Channel type to cshort here ;#item_type: Type and resolution for each of the signal samples. -Channel.item_type=cshort +Channel.item_type=gr_complex ;#signal: ;#if the option is disabled by default is assigned "1C" GPS L1 C/A Channel1.signal=1C @@ -194,7 +178,7 @@ Acquisition_1C.dump=false ;#filename: Log path and filename Acquisition_1C.dump_filename=./acq_dump.dat ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. -Acquisition_1C.item_type=cshort +Acquisition_1C.item_type=gr_complex ;#if: Signal intermediate frequency in [Hz] Acquisition_1C.if=0 ;#sampled_ms: Signal block duration for the acquisition signal detection [ms] @@ -205,7 +189,7 @@ Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition ;#notice that this affects the Acquisition threshold range! Acquisition_1C.use_CFAR_algorithm=false; ;#threshold: Acquisition threshold -Acquisition_1C.threshold=11 +Acquisition_1C.threshold=15 ;#pfa: Acquisition false alarm probability. This option overrides the threshold option. Only use with implementations: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] ;Acquisition_1C.pfa=0.01 ;#doppler_max: Maximum expected Doppler shift [Hz] @@ -221,7 +205,7 @@ Acquisition_1B.dump=false ;#filename: Log path and filename Acquisition_1B.dump_filename=./acq_dump.dat ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. -Acquisition_1B.item_type=cshort +Acquisition_1B.item_type=gr_complex ;#if: Signal intermediate frequency in [Hz] Acquisition_1B.if=0 ;#sampled_ms: Signal block duration for the acquisition signal detection [ms] @@ -240,9 +224,9 @@ Acquisition_1B.doppler_step=125 ;######### TRACKING GPS CONFIG ############ ;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] -Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_C_Aid_Tracking +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking ;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. -Tracking_1C.item_type=cshort +Tracking_1C.item_type=gr_complex ;#sampling_frequency: Signal Intermediate Frequency in [Hz] Tracking_1C.if=0 @@ -254,7 +238,7 @@ Tracking_1C.dump=false Tracking_1C.dump_filename=../data/epl_tracking_ch_ ;#pll_bw_hz: PLL loop filter bandwidth [Hz] -Tracking_1C.pll_bw_hz=15.0; +Tracking_1C.pll_bw_hz=20.0; ;#dll_bw_hz: DLL loop filter bandwidth [Hz] Tracking_1C.dll_bw_hz=1.5; @@ -267,7 +251,7 @@ Tracking_1C.order=3; ;#implementation: Selected tracking algorithm: [GPS_L1_CA_DLL_PLL_Tracking] or [GPS_L1_CA_DLL_PLL_C_Aid_Tracking] or [GPS_L1_CA_TCP_CONNECTOR_Tracking] or [Galileo_E1_DLL_PLL_VEML_Tracking] Tracking_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking ;#item_type: Type and resolution for each of the signal samples. -Tracking_1B.item_type=cshort +Tracking_1B.item_type=gr_complex ;#sampling_frequency: Signal Intermediate Frequency in [Hz] Tracking_1B.if=0 @@ -308,8 +292,8 @@ TelemetryDecoder_1B.dump=false TelemetryDecoder_1B.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -320,13 +304,11 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ ;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100; diff --git a/conf/gnss-sdr_Hybrid_gr_complex.conf b/conf/gnss-sdr_Hybrid_gr_complex.conf index a0f10bdb4..e444a9326 100644 --- a/conf/gnss-sdr_Hybrid_gr_complex.conf +++ b/conf/gnss-sdr_Hybrid_gr_complex.conf @@ -304,10 +304,9 @@ TelemetryDecoder_1C.decimation_factor=4; ;#implementation: Use [Galileo_E1B_Telemetry_Decoder] for Galileo E1B TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder TelemetryDecoder_1B.dump=false -TelemetryDecoder_1B_factor=4; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. +;#implementation: Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] @@ -318,14 +317,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=10; diff --git a/conf/gnss-sdr_Hybrid_ishort.conf b/conf/gnss-sdr_Hybrid_ishort.conf index e326d475f..b6cbe8591 100644 --- a/conf/gnss-sdr_Hybrid_ishort.conf +++ b/conf/gnss-sdr_Hybrid_ishort.conf @@ -311,7 +311,7 @@ TelemetryDecoder_1B.dump=false TelemetryDecoder_1B.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. +;#implementation: Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] @@ -322,14 +322,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100; diff --git a/conf/gnss-sdr_Hybrid_nsr.conf b/conf/gnss-sdr_Hybrid_nsr.conf index a87f44cc3..51ca610e5 100644 --- a/conf/gnss-sdr_Hybrid_nsr.conf +++ b/conf/gnss-sdr_Hybrid_nsr.conf @@ -17,7 +17,7 @@ GNSS-SDR.internal_fs_hz=2560000 SignalSource.implementation=Nsr_File_Signal_Source ;#filename: path to file with the captured GNSS signal samples to be processed -SignalSource.filename=/datalogger/signals/ifen/E1L1_FE0_Band0.stream ; <- PUT YOUR FILE HERE +SignalSource.filename=/media/javier/SISTEMA/signals/ifen/E1L1_FE0_Band0.stream ; <- PUT YOUR FILE HERE ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. SignalSource.item_type=byte @@ -304,7 +304,7 @@ TelemetryDecoder_1B.dump=false TelemetryDecoder_1B_factor=4; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. +;#implementation: Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] @@ -315,14 +315,13 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT +;#implementation: Position Velocity and Time (PVT) implementation algorithm. +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=10; diff --git a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_bin_file_III_1a.conf b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_bin_file_III_1a.conf index 7879cb02c..924d72f9e 100644 --- a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_bin_file_III_1a.conf +++ b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_bin_file_III_1a.conf @@ -289,12 +289,10 @@ Tracking_1C.early_late_space_chips=0.5; ;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -;#decimation factor -TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -304,14 +302,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_III_1a.conf b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_III_1a.conf index 2808b251d..b6de1c3cd 100644 --- a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_III_1a.conf +++ b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_III_1a.conf @@ -300,8 +300,8 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -311,14 +311,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_III_1b.conf b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_III_1b.conf index f7a56a6ff..2e33999f6 100644 --- a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_III_1b.conf +++ b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_III_1b.conf @@ -294,12 +294,10 @@ Tracking_1C.early_late_space_chips=0.5; ;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -;#decimation factor -TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -309,14 +307,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_II_3b.conf b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_II_3b.conf index 9bd32b4ca..b49d1c795 100644 --- a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_II_3b.conf +++ b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_II_3b.conf @@ -299,8 +299,8 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -310,14 +310,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_I_1b.conf b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_I_1b.conf index 65cc26133..8cf620713 100644 --- a/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_I_1b.conf +++ b/conf/gnss-sdr_multichannel_GPS_L1_Flexiband_realtime_I_1b.conf @@ -294,8 +294,8 @@ TelemetryDecoder_1C.dump=false TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -306,13 +306,11 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ ;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_multichannel_GPS_L1_L2_Flexiband_realtime_III_1b.conf b/conf/gnss-sdr_multichannel_GPS_L1_L2_Flexiband_realtime_III_1b.conf index 3f54ad14d..2f2ba159d 100644 --- a/conf/gnss-sdr_multichannel_GPS_L1_L2_Flexiband_realtime_III_1b.conf +++ b/conf/gnss-sdr_multichannel_GPS_L1_L2_Flexiband_realtime_III_1b.conf @@ -444,7 +444,7 @@ TelemetryDecoder_2S.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A.Mixed_Observables +;#implementation: Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] @@ -455,8 +455,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT + +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#averaging_depth: Number of PVT observations in the moving average algorithm PVT.averaging_depth=10 diff --git a/conf/gnss-sdr_multichannel_GPS_L1_L2_Galileo_E1B_Flexiband_bin_file_III_1b.conf b/conf/gnss-sdr_multichannel_GPS_L1_L2_Galileo_E1B_Flexiband_bin_file_III_1b.conf index 6932bd009..436b1070f 100644 --- a/conf/gnss-sdr_multichannel_GPS_L1_L2_Galileo_E1B_Flexiband_bin_file_III_1b.conf +++ b/conf/gnss-sdr_multichannel_GPS_L1_L2_Galileo_E1B_Flexiband_bin_file_III_1b.conf @@ -452,7 +452,7 @@ TelemetryDecoder_1B.decimation_factor=5; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. +;#implementation: Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] @@ -464,13 +464,11 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ ;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_multichannel_GPS_L1_USRP_X300_realtime.conf b/conf/gnss-sdr_multichannel_GPS_L1_USRP_X300_realtime.conf index b5e865ea8..62938acd4 100644 --- a/conf/gnss-sdr_multichannel_GPS_L1_USRP_X300_realtime.conf +++ b/conf/gnss-sdr_multichannel_GPS_L1_USRP_X300_realtime.conf @@ -364,12 +364,10 @@ Tracking_1C.early_late_space_chips=0.5; ;#implementation: Use [GPS_L1_CA_Telemetry_Decoder] for GPS L1 C/A TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -;#decimation factor -TelemetryDecoder_1C.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. -Observables.implementation=GPS_L1_CA_Observables +;#implementation: +Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false @@ -379,14 +377,12 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=GPS_L1_CA_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 diff --git a/conf/gnss-sdr_multichannel_GPS_L2_M_Flexiband_bin_file_III_1b.conf b/conf/gnss-sdr_multichannel_GPS_L2_M_Flexiband_bin_file_III_1b.conf index bfc6b40c9..40913cc7f 100644 --- a/conf/gnss-sdr_multichannel_GPS_L2_M_Flexiband_bin_file_III_1b.conf +++ b/conf/gnss-sdr_multichannel_GPS_L2_M_Flexiband_bin_file_III_1b.conf @@ -29,7 +29,7 @@ GNSS-SDR.SUPL_CI=0x31b0 SignalSource.implementation=Flexiband_Signal_Source SignalSource.flag_read_file=true -SignalSource.signal_file=/datalogger/signals/Fraunhofer/L125_III1b_210s.usb ; <- PUT YOUR FILE HERE +SignalSource.signal_file=/media/javier/SISTEMA/signals/fraunhofer/L125_III1b_210s.usb ; <- PUT YOUR FILE HERE ;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. SignalSource.item_type=gr_complex @@ -74,7 +74,7 @@ DataTypeAdapter0.item_type=gr_complex InputFilter0.implementation=Freq_Xlating_Fir_Filter ;#dump: Dump the filtered data to a file. -InputFilter0.dump=false +InputFilter0.dump=true ;#dump_filename: Log path and filename. InputFilter0.dump_filename=../data/input_filter_ch0.dat @@ -267,8 +267,8 @@ Resampler2.implementation=Pass_Through ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_1C.count=1 -Channels_2S.count=8 +Channels_1C.count=11 +Channels_2S.count=0 ;#GPS.prns=7,8 @@ -284,14 +284,25 @@ Channels.in_acquisition=1 ;# CHANNEL CONNECTION Channel0.RF_channel_ID=0 -Channel1.RF_channel_ID=1 -Channel2.RF_channel_ID=1 -Channel3.RF_channel_ID=1 -Channel4.RF_channel_ID=1 -Channel5.RF_channel_ID=1 -Channel6.RF_channel_ID=1 -Channel7.RF_channel_ID=1 -Channel8.RF_channel_ID=1 +Channel1.RF_channel_ID=0 +Channel2.RF_channel_ID=0 +Channel3.RF_channel_ID=0 +Channel4.RF_channel_ID=0 +Channel5.RF_channel_ID=0 +Channel6.RF_channel_ID=0 +Channel7.RF_channel_ID=0 +Channel8.RF_channel_ID=0 +Channel9.RF_channel_ID=0 +Channel10.RF_channel_ID=1 +Channel11.RF_channel_ID=1 +Channel12.RF_channel_ID=1 +Channel13.RF_channel_ID=1 +Channel14.RF_channel_ID=1 +Channel15.RF_channel_ID=1 +Channel16.RF_channel_ID=1 +Channel17.RF_channel_ID=1 +Channel18.RF_channel_ID=1 +Channel19.RF_channel_ID=1 ;######### ACQUISITION GENERIC CONFIG ###### @@ -309,32 +320,19 @@ Acquisition_1C.doppler_step=250 Acquisition_1C.bit_transition_flag=false Acquisition_1C.max_dwells=1 +;# GPS L2C M Acquisition_2S.dump=false Acquisition_2S.dump_filename=./acq_dump.dat Acquisition_2S.item_type=gr_complex Acquisition_2S.if=0 -Acquisition_2S.coherent_integration_time_ms=1 Acquisition_2S.implementation=GPS_L2_M_PCPS_Acquisition -Acquisition_2S.threshold=0.0005 +Acquisition_2S.threshold=0.00074 +;Acquisition_2S.pfa=0.001 Acquisition_2S.doppler_max=5000 -Acquisition_2S.doppler_step=100 -Acquisition_2S.bit_transition_flag=false +Acquisition_2S.doppler_min=-5000 +Acquisition_2S.doppler_step=60 Acquisition_2S.max_dwells=1 -;# channel specific config - -Acquisition_2S1.dump=false -Acquisition_2S1.dump_filename=./acq_dump.dat -Acquisition_2S1.item_type=gr_complex -Acquisition_2S1.if=0 -Acquisition_2S1.coherent_integration_time_ms=1 -Acquisition_2S1.implementation=GPS_L2_M_PCPS_Acquisition -Acquisition_2S1.threshold=0.0005 -Acquisition_2S1.doppler_max=5000 -Acquisition_2S1.doppler_step=100 -Acquisition_2S1.bit_transition_flag=false -Acquisition_2S1.max_dwells=1 - ;######### TRACKING CONFIG ############ @@ -342,7 +340,7 @@ Acquisition_2S1.max_dwells=1 Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking Tracking_1C.item_type=gr_complex Tracking_1C.if=0 -Tracking_1C.dump=true +Tracking_1C.dump=false Tracking_1C.dump_filename=../data/epl_tracking_ch_ Tracking_1C.pll_bw_hz=40.0; Tracking_1C.dll_bw_hz=3.0; @@ -354,29 +352,18 @@ Tracking_1C.early_late_space_chips=0.5; Tracking_2S.implementation=GPS_L2_M_DLL_PLL_Tracking Tracking_2S.item_type=gr_complex Tracking_2S.if=0 -Tracking_2S.dump=true -Tracking_2S.dump_filename=../data/epl_tracking_ch_ +Tracking_2S.dump=false +Tracking_2S.dump_filename=./tracking_ch_ Tracking_2S.pll_bw_hz=2.0; -Tracking_2S.dll_bw_hz=0.5; +Tracking_2S.dll_bw_hz=0.25; Tracking_2S.order=2; Tracking_2S.early_late_space_chips=0.5; -;######### GPS L2C SPECIFIC CHANNEL TRACKING CONFIG ############ -Tracking_2S1.implementation=GPS_L2_M_DLL_PLL_Tracking -Tracking_2S1.item_type=gr_complex -Tracking_2S1.if=0 -Tracking_2S1.dump=true -Tracking_2S1.dump_filename=../data/epl_tracking_ch_ -Tracking_2S1.pll_bw_hz=2.0; -Tracking_2S1.dll_bw_hz=0.5; -Tracking_2S1.order=2; -Tracking_2S1.early_late_space_chips=0.5; - ;######### TELEMETRY DECODER CONFIG ############ TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false -TelemetryDecoder_1C.decimation_factor=20; +TelemetryDecoder_1C.decimation_factor=1; TelemetryDecoder_2S.implementation=GPS_L2C_Telemetry_Decoder TelemetryDecoder_2S.dump=false @@ -384,31 +371,27 @@ TelemetryDecoder_2S.decimation_factor=1; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. +;#implementation: Observables.implementation=Hybrid_Observables - ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] Observables.dump=false - ;#dump_filename: Log path and filename. Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=true +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100 ;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms<=display_rate_ms. -PVT.display_rate_ms=500 +PVT.display_rate_ms=100 ;# KML, GeoJSON, NMEA and RTCM output configuration diff --git a/conf/gnss-sdr_multichannel_GPS_L2_M_Flexiband_bin_file_III_1b_real.conf b/conf/gnss-sdr_multichannel_GPS_L2_M_Flexiband_bin_file_III_1b_real.conf new file mode 100644 index 000000000..26128cf81 --- /dev/null +++ b/conf/gnss-sdr_multichannel_GPS_L2_M_Flexiband_bin_file_III_1b_real.conf @@ -0,0 +1,411 @@ +; Default configuration file +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_hz: Internal signal sampling frequency after the signal conditioning stage [Hz]. +GNSS-SDR.internal_fs_hz=5000000 + + +;######### SUPL RRLP GPS assistance configuration ##### +; Check http://www.mcc-mnc.com/ +; On Android: https://play.google.com/store/apps/details?id=net.its_here.cellidinfo&hl=en +GNSS-SDR.SUPL_gps_enabled=false +GNSS-SDR.SUPL_read_gps_assistance_xml=true +GNSS-SDR.SUPL_gps_ephemeris_server=supl.google.com +GNSS-SDR.SUPL_gps_ephemeris_port=7275 +GNSS-SDR.SUPL_gps_acquisition_server=supl.google.com +GNSS-SDR.SUPL_gps_acquisition_port=7275 +GNSS-SDR.SUPL_MCC=244 +GNSS-SDR.SUPL_MNS=5 +GNSS-SDR.SUPL_LAC=0x59e2 +GNSS-SDR.SUPL_CI=0x31b0 + +;######### SIGNAL_SOURCE CONFIG ############ +;#implementation: Use [File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] (experimental) +SignalSource.implementation=Flexiband_Signal_Source + +SignalSource.flag_read_file=true +SignalSource.signal_file=/home/javier/signals/20140923_20-24-17_L125_roof_210s.usb ; <- PUT YOUR FILE HERE + +;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version. +SignalSource.item_type=gr_complex + +;# FPGA firmware file +SignalSource.firmware_file=flexiband_III-1b.bit + +;#RF_channels: Number of RF channels present in the frontend device, must agree the FPGA firmware file +SignalSource.RF_channels=2 + +;#frontend channels gain. Not usable yet! +SignalSource.gain1=0 +SignalSource.gain2=0 +SignalSource.gain3=0 + +;#frontend channels AGC +SignalSource.AGC=true + +;# USB 3.0 packet buffer size (number of SuperSpeed packets) +SignalSource.usb_packet_buffer=128 + +;###################################################### +;######### RF CHANNEL 0 SIGNAL CONDITIONER ############ +;###################################################### + +;######### SIGNAL_CONDITIONER 0 CONFIG ############ +;## It holds blocks to change data type, filter and resample input data. +SignalConditioner0.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER 0 CONFIG ############ +DataTypeAdapter0.implementation=Pass_Through +DataTypeAdapter0.item_type=gr_complex + +;######### INPUT_FILTER 0 CONFIG ############ +;## Filter the input data. Can be combined with frequency translation for IF signals + +;#implementation: Use [Pass_Through] or [Fir_Filter] or [Freq_Xlating_Fir_Filter] +;#[Pass_Through] disables this block +;#[Fir_Filter] enables a FIR Filter +;#[Freq_Xlating_Fir_Filter] enables FIR filter and a composite frequency translation that shifts IF down to zero Hz. + +InputFilter0.implementation=Freq_Xlating_Fir_Filter + +;#dump: Dump the filtered data to a file. +InputFilter0.dump=false + +;#dump_filename: Log path and filename. +InputFilter0.dump_filename=../data/input_filter_ch0.dat + +;#The following options are used in the filter design of Fir_Filter and Freq_Xlating_Fir_Filter implementation. +;#These options are based on parameters of gnuradio's function: gr_remez. +;#These function calculates the optimal (in the Chebyshev/minimax sense) FIR filter inpulse reponse given a set of band edges, +;#the desired reponse on those bands, and the weight given to the error in those bands. + +;#input_item_type: Type and resolution for input signal samples. Use only gr_complex in this version. +InputFilter0.input_item_type=gr_complex + +;#outut_item_type: Type and resolution for output filtered signal samples. Use only gr_complex in this version. +InputFilter0.output_item_type=gr_complex + +;#taps_item_type: Type and resolution for the taps of the filter. Use only float in this version. +InputFilter0.taps_item_type=float + +;#number_of_taps: Number of taps in the filter. Increasing this parameter increases the processing time +InputFilter0.number_of_taps=5 + +;#number_of _bands: Number of frequency bands in the filter. +InputFilter0.number_of_bands=2 + +;#bands: frequency at the band edges [ b1 e1 b2 e2 b3 e3 ...]. +;#Frequency is in the range [0, 1], with 1 being the Nyquist frequency (Fs/2) +;#The number of band_begin and band_end elements must match the number of bands + +InputFilter0.band1_begin=0.0 +InputFilter0.band1_end=0.45 +InputFilter0.band2_begin=0.55 +InputFilter0.band2_end=1.0 + +;#ampl: desired amplitude at the band edges [ a(b1) a(e1) a(b2) a(e2) ...]. +;#The number of ampl_begin and ampl_end elements must match the number of bands + +InputFilter0.ampl1_begin=1.0 +InputFilter0.ampl1_end=1.0 +InputFilter0.ampl2_begin=0.0 +InputFilter0.ampl2_end=0.0 + +;#band_error: weighting applied to each band (usually 1). +;#The number of band_error elements must match the number of bands +InputFilter0.band1_error=1.0 +InputFilter0.band2_error=1.0 + +;#filter_type: one of "bandpass", "hilbert" or "differentiator" +InputFilter0.filter_type=bandpass + +;#grid_density: determines how accurately the filter will be constructed. +;The minimum value is 16; higher values are slower to compute the filter. +InputFilter0.grid_density=16 + +;#The following options are used only in Freq_Xlating_Fir_Filter implementation. +;#InputFilter0.IF is the intermediate frequency (in Hz) shifted down to zero Hz +;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/ +InputFilter0.sampling_frequency=20000000 +;# IF deviation due to front-end LO inaccuracies [HZ] +InputFilter0.IF=0 + +;# Decimation factor after the frequency tranaslating block +InputFilter0.decimation_factor=4 + +;######### RESAMPLER CONFIG 0 ############ +;## Resamples the input data. +Resampler0.implementation=Pass_Through + +;###################################################### +;######### RF CHANNEL 1 SIGNAL CONDITIONER ############ +;###################################################### + +;######### SIGNAL_CONDITIONER 1 CONFIG ############ +;## It holds blocks to change data type, filter and resample input data. +SignalConditioner1.implementation=Signal_Conditioner + +;######### DATA_TYPE_ADAPTER 1 CONFIG ############ +DataTypeAdapter1.implementation=Pass_Through +DataTypeAdapter1.item_type=gr_complex + +;######### INPUT_FILTER 0 CONFIG ############ +;## Filter the input data. Can be combined with frequency translation for IF signals + +;#implementation: Use [Pass_Through] or [Fir_Filter] or [Freq_Xlating_Fir_Filter] +;#[Pass_Through] disables this block +;#[Fir_Filter] enables a FIR Filter +;#[Freq_Xlating_Fir_Filter] enables FIR filter and a composite frequency translation that shifts IF down to zero Hz. + +InputFilter1.implementation=Freq_Xlating_Fir_Filter + +;#dump: Dump the filtered data to a file. +InputFilter1.dump=false + +;#dump_filename: Log path and filename. +InputFilter1.dump_filename=../data/input_filter_ch1.dat + +;#The following options are used in the filter design of Fir_Filter and Freq_Xlating_Fir_Filter implementation. +;#These options are based on parameters of gnuradio's function: gr_remez. +;#These function calculates the optimal (in the Chebyshev/minimax sense) FIR filter inpulse reponse given a set of band edges, +;#the desired reponse on those bands, and the weight given to the error in those bands. + +;#input_item_type: Type and resolution for input signal samples. Use only gr_complex in this version. +InputFilter1.input_item_type=gr_complex + +;#outut_item_type: Type and resolution for output filtered signal samples. Use only gr_complex in this version. +InputFilter1.output_item_type=gr_complex + +;#taps_item_type: Type and resolution for the taps of the filter. Use only float in this version. +InputFilter1.taps_item_type=float + +;#number_of_taps: Number of taps in the filter. Increasing this parameter increases the processing time +InputFilter1.number_of_taps=5 + +;#number_of _bands: Number of frequency bands in the filter. +InputFilter1.number_of_bands=2 + +;#bands: frequency at the band edges [ b1 e1 b2 e2 b3 e3 ...]. +;#Frequency is in the range [0, 1], with 1 being the Nyquist frequency (Fs/2) +;#The number of band_begin and band_end elements must match the number of bands + +InputFilter1.band1_begin=0.0 +InputFilter1.band1_end=0.45 +InputFilter1.band2_begin=0.55 +InputFilter1.band2_end=1.0 + +;#ampl: desired amplitude at the band edges [ a(b1) a(e1) a(b2) a(e2) ...]. +;#The number of ampl_begin and ampl_end elements must match the number of bands + +InputFilter1.ampl1_begin=1.0 +InputFilter1.ampl1_end=1.0 +InputFilter1.ampl2_begin=0.0 +InputFilter1.ampl2_end=0.0 + +;#band_error: weighting applied to each band (usually 1). +;#The number of band_error elements must match the number of bands +InputFilter1.band1_error=1.0 +InputFilter1.band2_error=1.0 + +;#filter_type: one of "bandpass", "hilbert" or "differentiator" +InputFilter1.filter_type=bandpass + +;#grid_density: determines how accurately the filter will be constructed. +;The minimum value is 16; higher values are slower to compute the filter. +InputFilter1.grid_density=16 + +;#The following options are used only in Freq_Xlating_Fir_Filter implementation. +;#InputFilter0.IF is the intermediate frequency (in Hz) shifted down to zero Hz +;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/ +InputFilter1.sampling_frequency=20000000 +;# IF deviation due to front-end LO inaccuracies [HZ] +InputFilter1.IF=0 + +;# Decimation factor after the frequency tranaslating block +InputFilter1.decimation_factor=4 + + +;######### RESAMPLER CONFIG 1 ############ +;## Resamples the input data. +Resampler1.implementation=Pass_Through + +;######### SIGNAL_CONDITIONER 2 CONFIG ############ +;## It holds blocks to change data type, filter and resample input data. +SignalConditioner2.implementation=Pass_Through + +;######### DATA_TYPE_ADAPTER 2 CONFIG ############ +DataTypeAdapter2.implementation=Pass_Through +DataTypeAdapter2.item_type=gr_complex + +;######### INPUT_FILTER 2 CONFIG ############ +InputFilter2.implementation=Pass_Through + +;#dump: Dump the filtered data to a file. +InputFilter2.dump=false + +;#dump_filename: Log path and filename. +InputFilter2.dump_filename=../data/input_filter.dat + +;#input_item_type: Type and resolution for input signal samples. Use only gr_complex in this version. +InputFilter2.input_item_type=gr_complex + +;#outut_item_type: Type and resolution for output filtered signal samples. Use only gr_complex in this version. +InputFilter2.output_item_type=gr_complex + +;######### RESAMPLER CONFIG 2 ############ +;## Resamples the input data. +Resampler2.implementation=Pass_Through + + +;######### CHANNELS GLOBAL CONFIG ############ +;#count: Number of available GPS satellite channels. +Channels_1C.count=10 +Channels_2S.count=4 + +;#GPS.prns=7,8 + +;#in_acquisition: Number of channels simultaneously acquiring for the whole receiver +Channels.in_acquisition=1 + +;# signal: +;# "1C" GPS L1 C/A +;# "2S" GPS L2 L2C (M) +;# "1B" GALILEO E1 B (I/NAV OS/CS/SoL) +;# "5X" GALILEO E5a I+Q +;# CHANNEL NUMBERING ORDER: GPS L1 C/A, GPS L2 L2C (M), GALILEO E1 B, GALILEO E5a + +;# CHANNEL CONNECTION +Channel0.RF_channel_ID=0 +Channel1.RF_channel_ID=0 +Channel2.RF_channel_ID=0 +Channel3.RF_channel_ID=0 +Channel4.RF_channel_ID=0 +Channel5.RF_channel_ID=0 +Channel6.RF_channel_ID=0 +Channel7.RF_channel_ID=0 +Channel8.RF_channel_ID=0 +Channel9.RF_channel_ID=0 +Channel10.RF_channel_ID=1 +Channel11.RF_channel_ID=1 +Channel12.RF_channel_ID=1 +Channel13.RF_channel_ID=1 +Channel14.RF_channel_ID=1 +Channel15.RF_channel_ID=1 +Channel16.RF_channel_ID=1 +Channel17.RF_channel_ID=1 +Channel18.RF_channel_ID=1 +Channel19.RF_channel_ID=1 + + +;######### ACQUISITION GENERIC CONFIG ###### +;#The following options are specific to each channel and overwrite the generic options + +Acquisition_1C.dump=false +Acquisition_1C.dump_filename=./acq_dump.dat +Acquisition_1C.item_type=gr_complex +Acquisition_1C.if=0 +Acquisition_1C.coherent_integration_time_ms=1 +Acquisition_1C.implementation=GPS_L1_CA_PCPS_Acquisition +Acquisition_1C.threshold=0.005 +Acquisition_1C.doppler_max=5000 +Acquisition_1C.doppler_step=250 +Acquisition_1C.bit_transition_flag=false +Acquisition_1C.max_dwells=1 + +;# GPS L2C M +Acquisition_2S.dump=false +Acquisition_2S.dump_filename=./acq_dump.dat +Acquisition_2S.item_type=gr_complex +Acquisition_2S.if=0 +Acquisition_2S.implementation=GPS_L2_M_PCPS_Acquisition +Acquisition_2S.threshold=0.00074 +;Acquisition_2S.pfa=0.001 +Acquisition_2S.doppler_max=5000 +Acquisition_2S.doppler_min=-5000 +Acquisition_2S.doppler_step=60 +Acquisition_2S.max_dwells=1 + + +;######### TRACKING CONFIG ############ + +;######### GPS L1 C/A GENERIC TRACKING CONFIG ############ +Tracking_1C.implementation=GPS_L1_CA_DLL_PLL_Tracking +Tracking_1C.item_type=gr_complex +Tracking_1C.if=0 +Tracking_1C.dump=false +Tracking_1C.dump_filename=../data/epl_tracking_ch_ +Tracking_1C.pll_bw_hz=40.0; +Tracking_1C.dll_bw_hz=3.0; +Tracking_1C.order=3; +Tracking_1C.early_late_space_chips=0.5; + + +;######### GPS L2C GENERIC TRACKING CONFIG ############ +Tracking_2S.implementation=GPS_L2_M_DLL_PLL_Tracking +Tracking_2S.item_type=gr_complex +Tracking_2S.if=0 +Tracking_2S.dump=false +Tracking_2S.dump_filename=./tracking_ch_ +Tracking_2S.pll_bw_hz=2.0; +Tracking_2S.dll_bw_hz=0.25; +Tracking_2S.order=2; +Tracking_2S.early_late_space_chips=0.5; + + +;######### TELEMETRY DECODER CONFIG ############ +TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder +TelemetryDecoder_1C.dump=false +TelemetryDecoder_1C.decimation_factor=20; + +TelemetryDecoder_2S.implementation=GPS_L2C_Telemetry_Decoder +TelemetryDecoder_2S.dump=false +TelemetryDecoder_2S.decimation_factor=1; + + +;######### OBSERVABLES CONFIG ############ +;#implementation: +Observables.implementation=Hybrid_Observables +;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] +Observables.dump=true + +;#dump_filename: Log path and filename. +Observables.dump_filename=./observables.dat + + +;######### PVT CONFIG ############ +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT + +;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] +PVT.output_rate_ms=100 + +;#display_rate_ms: Position console print (std::out) interval [ms]. Notice that output_rate_ms<=display_rate_ms. +PVT.display_rate_ms=100 + +;# KML, GeoJSON, NMEA and RTCM output configuration + +;#dump_filename: Log path and filename without extension. Notice that PVT will add ".dat" to the binary dump and ".kml" to GoogleEarth dump. +PVT.dump_filename=./PVT + +;#nmea_dump_filename: NMEA log path and filename +PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea; + +;#flag_nmea_tty_port: Enable or disable the NMEA log to a serial TTY port (Can be used with real hardware or virtual one) +PVT.flag_nmea_tty_port=false; + +;#nmea_dump_devname: serial device descriptor for NMEA logging +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 + +;#dump: Enable or disable the PVT internal binary data file logging [true] or [false] +PVT.dump=false diff --git a/conf/gnss-sdr_multisource_Hybrid_ishort.conf b/conf/gnss-sdr_multisource_Hybrid_ishort.conf index 00d85930f..31f777693 100644 --- a/conf/gnss-sdr_multisource_Hybrid_ishort.conf +++ b/conf/gnss-sdr_multisource_Hybrid_ishort.conf @@ -420,7 +420,7 @@ TelemetryDecoder_Galileo.dump=false ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. +;#implementation: Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] @@ -431,14 +431,8 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT - -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=100; diff --git a/conf/gnss-sdr_multisource_Hybrid_nsr.conf b/conf/gnss-sdr_multisource_Hybrid_nsr.conf index 5ced6c5a5..f0a644f18 100644 --- a/conf/gnss-sdr_multisource_Hybrid_nsr.conf +++ b/conf/gnss-sdr_multisource_Hybrid_nsr.conf @@ -446,7 +446,7 @@ TelemetryDecoder_1B.decimation_factor=4; ;######### OBSERVABLES CONFIG ############ -;#implementation: Use [GPS_L1_CA_Observables] for GPS L1 C/A. +;#implementation: Observables.implementation=Hybrid_Observables ;#dump: Enable or disable the Observables internal binary data file logging [true] or [false] @@ -457,14 +457,8 @@ Observables.dump_filename=./observables.dat ;######### PVT CONFIG ############ -;#implementation: Position Velocity and Time (PVT) implementation algorithm: Use [GPS_L1_CA_PVT] in this version. -PVT.implementation=Hybrid_PVT - -;#averaging_depth: Number of PVT observations in the moving average algorithm -PVT.averaging_depth=10 - -;#flag_average: Enables the PVT averaging between output intervals (arithmetic mean) [true] or [false] -PVT.flag_averaging=false +;#implementation: Position Velocity and Time (PVT) implementation: +PVT.implementation=RTKLIB_PVT ;#output_rate_ms: Period between two PVT outputs. Notice that the minimum period is equal to the tracking integration time (for GPS CA L1 is 1ms) [ms] PVT.output_rate_ms=10; diff --git a/src/algorithms/PVT/adapters/CMakeLists.txt b/src/algorithms/PVT/adapters/CMakeLists.txt index 3a7e524dd..a051c7953 100644 --- a/src/algorithms/PVT/adapters/CMakeLists.txt +++ b/src/algorithms/PVT/adapters/CMakeLists.txt @@ -16,10 +16,8 @@ # along with GNSS-SDR. If not, see . # -set(PVT_ADAPTER_SOURCES - gps_l1_ca_pvt.cc - galileo_e1_pvt.cc - hybrid_pvt.cc +set(PVT_ADAPTER_SOURCES + rtklib_pvt.cc ) include_directories( @@ -29,6 +27,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} diff --git a/src/algorithms/PVT/adapters/galileo_e1_pvt.cc b/src/algorithms/PVT/adapters/galileo_e1_pvt.cc deleted file mode 100644 index e2c75df27..000000000 --- a/src/algorithms/PVT/adapters/galileo_e1_pvt.cc +++ /dev/null @@ -1,141 +0,0 @@ -/*! - * \file galileo_e1_pvt.cc - * \brief Implementation of an adapter of a GALILEO E1 PVT solver block to a - * PvtInterface - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#include "galileo_e1_pvt.h" -#include -#include -#include "configuration_interface.h" - - -using google::LogMessage; - -GalileoE1Pvt::GalileoE1Pvt(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) -{ - // dump parameters - std::string default_dump_filename = "./pvt.dat"; - std::string default_nmea_dump_filename = "./nmea_pvt.nmea"; - std::string default_nmea_dump_devname = "/dev/tty1"; - std::string default_rtcm_dump_devname = "/dev/pts/1"; - DLOG(INFO) << "role " << role; - - dump_ = configuration->property(role + ".dump", false); - dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - - // moving average depth parameters - int averaging_depth = configuration->property(role + ".averaging_depth", 10); - bool flag_averaging = configuration->property(role + ".flag_averaging", false); - - // output rate - int output_rate_ms = configuration->property(role + ".output_rate_ms", 500); - - // display rate - int display_rate_ms = configuration->property(role + ".display_rate_ms", 500); - - // NMEA Printer settings - bool flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); - std::string nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); - std::string nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname); - - // RTCM Printer settings - bool flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); - std::string rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname); - bool flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false); - unsigned short rtcm_tcp_port = configuration->property(role + ".rtcm_tcp_port", 2101); - unsigned short rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); - // RTCM message rates: least common multiple with output_rate_ms - int rtcm_MT1045_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), output_rate_ms); - int rtcm_MSM_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), output_rate_ms); - std::map rtcm_msg_rate_ms; - rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; - for (int k = 1091; k < 1098; k++) // All Galileo MSM - { - rtcm_msg_rate_ms[k] = rtcm_MSM_rate_ms; - } - - // make PVT object - pvt_ = galileo_e1_make_pvt_cc(in_streams_, - dump_, - dump_filename_, - averaging_depth, - flag_averaging, - output_rate_ms, - display_rate_ms, - flag_nmea_tty_port, - nmea_dump_filename, - nmea_dump_devname, - flag_rtcm_server, - flag_rtcm_tty_port, - rtcm_tcp_port, - rtcm_station_id, - rtcm_msg_rate_ms, - rtcm_dump_devname); - - DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; -} - - -GalileoE1Pvt::~GalileoE1Pvt() -{} - - -void GalileoE1Pvt::connect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to connect internally - DLOG(INFO) << "nothing to connect internally"; -} - - -void GalileoE1Pvt::disconnect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to disconnect -} - -gr::basic_block_sptr GalileoE1Pvt::get_left_block() -{ - return pvt_; -} - - -gr::basic_block_sptr GalileoE1Pvt::get_right_block() -{ - return pvt_; -} - diff --git a/src/algorithms/PVT/adapters/galileo_e1_pvt.h b/src/algorithms/PVT/adapters/galileo_e1_pvt.h deleted file mode 100644 index 3614c123a..000000000 --- a/src/algorithms/PVT/adapters/galileo_e1_pvt.h +++ /dev/null @@ -1,94 +0,0 @@ -/*! - * \file galileo_e1_pvt.h - * \brief Interface of an adapter of a GALILEO E1 PVT solver block to a - * PvtInterface. - * \author Javier Arribas, 2013. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - - -#ifndef GNSS_SDR_GALILEO_E1_PVT_H_ -#define GNSS_SDR_GALILEO_E1_PVT_H_ - -#include -#include "pvt_interface.h" -#include "galileo_e1_pvt_cc.h" - - -class ConfigurationInterface; - -/*! - * \brief This class implements a PvtInterface for Galileo E1 - */ -class GalileoE1Pvt : public PvtInterface -{ -public: - GalileoE1Pvt(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams); - - virtual ~GalileoE1Pvt(); - - std::string role() - { - return role_; - } - - //! Returns "GALILEO_E1_PVT" - std::string implementation() - { - return "GALILEO_E1_PVT"; - } - - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); - - void reset() - { - return; - } - - //! All blocks must have an item_size() function implementation. Returns sizeof(gr_complex) - size_t item_size() - { - return sizeof(gr_complex); - } - -private: - galileo_e1_pvt_cc_sptr pvt_; - bool dump_; - //unsigned int fs_in_; - std::string dump_filename_; - std::string role_; - unsigned int in_streams_; - unsigned int out_streams_; -}; - -#endif diff --git a/src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc b/src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc deleted file mode 100644 index ba244b5ae..000000000 --- a/src/algorithms/PVT/adapters/gps_l1_ca_pvt.cc +++ /dev/null @@ -1,250 +0,0 @@ -/*! - * \file gps_l1_ca_pvt.cc - * \brief Implementation of an adapter of a GPS L1 C/A PVT solver block to a - * PvtInterface - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#include "gps_l1_ca_pvt.h" -#include -#include -#include -#include -#include -#include "configuration_interface.h" - -using google::LogMessage; - -GpsL1CaPvt::GpsL1CaPvt(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) -{ - // dump parameters - std::string default_dump_filename = "./pvt.dat"; - std::string default_nmea_dump_filename = "./nmea_pvt.nmea"; - std::string default_nmea_dump_devname = "/dev/tty1"; - std::string default_rtcm_dump_devname = "/dev/pts/1"; - DLOG(INFO) << "role " << role; - dump_ = configuration->property(role + ".dump", false); - dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - // moving average depth parameters - int averaging_depth = configuration->property(role + ".averaging_depth", 10); - bool flag_averaging = configuration->property(role + ".flag_averaging", false); - - // output rate - int output_rate_ms = configuration->property(role + ".output_rate_ms", 500); - - // display rate - int display_rate_ms = configuration->property(role + ".display_rate_ms", 500); - - // NMEA Printer settings - bool flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); - std::string nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); - std::string nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname); - - // RTCM Printer settings - bool flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); - std::string rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname); - bool flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false); - unsigned short rtcm_tcp_port = configuration->property(role + ".rtcm_tcp_port", 2101); - unsigned short rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); - // RTCM message rates: least common multiple with output_rate_ms - int rtcm_MT1019_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), output_rate_ms); - int rtcm_MSM_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), output_rate_ms); - std::map rtcm_msg_rate_ms; - rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; - for (int k = 1071; k < 1078; k++) // All GPS MSM - { - rtcm_msg_rate_ms[k] = rtcm_MSM_rate_ms; - } - - // getting names from the config file, if available - // default filename for assistance data - const std::string eph_default_xml_filename = "./gps_ephemeris.xml"; - eph_xml_filename_= configuration->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); - - //const std::string utc_default_xml_filename = "./gps_utc_model.xml"; - //const std::string iono_default_xml_filename = "./gps_iono.xml"; - //const std::string ref_time_default_xml_filename = "./gps_ref_time.xml"; - //const std::string ref_location_default_xml_filename = "./gps_ref_location.xml"; - - //std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); - //std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); - //std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); - //std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); - - // RINEX version - int conf_rinex_version; - conf_rinex_version = configuration->property(role + ".rinex_version", 0); - - // make PVT object - pvt_ = gps_l1_ca_make_pvt_cc(in_streams_, - dump_, - dump_filename_, - averaging_depth, - flag_averaging, - output_rate_ms, - display_rate_ms, - flag_nmea_tty_port, - nmea_dump_filename, - nmea_dump_devname, - flag_rtcm_server, - flag_rtcm_tty_port, - rtcm_tcp_port, - rtcm_station_id, - rtcm_msg_rate_ms, - rtcm_dump_devname, - conf_rinex_version ); - - DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; -} - - -bool GpsL1CaPvt::save_assistance_to_XML() -{ - // return variable (true == succeeded) - bool ret = false; - - LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename_; - std::map eph_map = pvt_->get_GPS_L1_ephemeris_map(); - - if (eph_map.size() > 0) - { - try - { - std::ofstream ofs(eph_xml_filename_.c_str(), std::ofstream::trunc | std::ofstream::out); - boost::archive::xml_oarchive xml(ofs); - xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_map); - ofs.close(); - LOG(INFO) << "Saved GPS L1 Ephemeris map data"; - } - catch (std::exception& e) - { - LOG(WARNING) << e.what(); - return false; - } - return true; - } - else - { - LOG(WARNING) << "Failed to save Ephemeris, map is empty"; - return false; - } - // Only try to save {utc, iono, ref time, ref location} if SUPL is enabled - // bool enable_gps_supl_assistance = configuration_->property("GNSS-SDR.SUPL_gps_enabled", false); - // if (enable_gps_supl_assistance == true) - // { - // // try to save utc model xml file - // std::map utc_copy = global_gps_utc_model_map.get_map_copy(); - // if (supl_client_acquisition_.save_utc_map_xml(utc_xml_filename, utc_copy) == true) - // { - // LOG(INFO) << "SUPL: Successfully saved UTC Model XML file"; - // //ret = true; - // } - // else - // { - // LOG(INFO) << "SUPL: Error while trying to save utc XML file"; - // //ret = false; - // } - // // try to save iono model xml file - // std::map iono_copy = global_gps_iono_map.get_map_copy(); - // if (supl_client_acquisition_.save_iono_map_xml(iono_xml_filename, iono_copy) == true) - // { - // LOG(INFO) << "SUPL: Successfully saved IONO Model XML file"; - // //ret = true; - // } - // else - // { - // LOG(INFO) << "SUPL: Error while trying to save iono XML file"; - // //ret = false; - // } - // // try to save ref time xml file - // std::map ref_time_copy = global_gps_ref_time_map.get_map_copy(); - // if (supl_client_acquisition_.save_ref_time_map_xml(ref_time_xml_filename, ref_time_copy) == true) - // { - // LOG(INFO) << "SUPL: Successfully saved Ref Time XML file"; - // //ret = true; - // } - // else - // { - // LOG(INFO) << "SUPL: Error while trying to save ref time XML file"; - // //ref = false; - // } - // // try to save ref location xml file - // std::map ref_location_copy = global_gps_ref_location_map.get_map_copy(); - // if (supl_client_acquisition_.save_ref_location_map_xml(ref_location_xml_filename, ref_location_copy) == true) - // { - // LOG(INFO) << "SUPL: Successfully saved Ref Location XML file"; - // //ref = true; - // } - // else - // { - // LOG(INFO) << "SUPL: Error while trying to save ref location XML file"; - // //ret = false; - // } - // } - return ret; -} - - -GpsL1CaPvt::~GpsL1CaPvt() -{ - save_assistance_to_XML(); -} - - -void GpsL1CaPvt::connect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to connect internally - DLOG(INFO) << "nothing to connect internally"; -} - - -void GpsL1CaPvt::disconnect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to disconnect -} - - -gr::basic_block_sptr GpsL1CaPvt::get_left_block() -{ - return pvt_; -} - - -gr::basic_block_sptr GpsL1CaPvt::get_right_block() -{ - return pvt_; -} diff --git a/src/algorithms/PVT/adapters/gps_l1_ca_pvt.h b/src/algorithms/PVT/adapters/gps_l1_ca_pvt.h deleted file mode 100644 index 4787f94bd..000000000 --- a/src/algorithms/PVT/adapters/gps_l1_ca_pvt.h +++ /dev/null @@ -1,98 +0,0 @@ -/*! - * \file gps_l1_ca_pvt.h - * \brief Interface of an adapter of a GPS L1 C/A PVT solver block to a - * PvtInterface - * Position Velocity and Time - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - - -#ifndef GNSS_SDR_GPS_L1_CA_PVT_H_ -#define GNSS_SDR_GPS_L1_CA_PVT_H_ - -#include -#include "pvt_interface.h" -#include "gps_l1_ca_pvt_cc.h" - - -class ConfigurationInterface; - -/*! - * \brief This class implements a PvtInterface for GPS L1 C/A - */ -class GpsL1CaPvt : public PvtInterface -{ -public: - GpsL1CaPvt(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams); - - virtual ~GpsL1CaPvt(); - - std::string role() - { - return role_; - } - - //! Returns "GPS_L1_CA_PVT" - std::string implementation() - { - return "GPS_L1_CA_PVT"; - } - - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); - - void reset() - { - return; - } - - //! All blocks must have an item_size() function implementation. Returns sizeof(gr_complex) - size_t item_size() - { - return sizeof(gr_complex); - } - -private: - gps_l1_ca_pvt_cc_sptr pvt_; - bool dump_; - std::string dump_filename_; - std::string role_; - unsigned int in_streams_; - unsigned int out_streams_; - - std::string eph_xml_filename_; - bool save_assistance_to_XML(); -}; - -#endif diff --git a/src/algorithms/PVT/adapters/hybrid_pvt.cc b/src/algorithms/PVT/adapters/hybrid_pvt.cc deleted file mode 100644 index 98b7a3836..000000000 --- a/src/algorithms/PVT/adapters/hybrid_pvt.cc +++ /dev/null @@ -1,237 +0,0 @@ -/*! - * \file hybrid_pvt.cc - * \brief Implementation of an adapter of a GALILEO E1 PVT solver block to a - * PvtInterface - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#include "hybrid_pvt.h" -#include -#include -#include -#include -#include -#include "configuration_interface.h" - - -using google::LogMessage; - -HybridPvt::HybridPvt(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) -{ - // dump parameters - std::string default_dump_filename = "./pvt.dat"; - std::string default_nmea_dump_filename = "./nmea_pvt.nmea"; - std::string default_nmea_dump_devname = "/dev/tty1"; - std::string default_rtcm_dump_devname = "/dev/pts/1"; - DLOG(INFO) << "role " << role; - dump_ = configuration->property(role + ".dump", false); - dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - - // moving average depth parameters - int averaging_depth = configuration->property(role + ".averaging_depth", 10); - bool flag_averaging = configuration->property(role + ".flag_averaging", false); - - // output rate - int output_rate_ms = configuration->property(role + ".output_rate_ms", 500); - - // display rate - int display_rate_ms = configuration->property(role + ".display_rate_ms", 500); - - // NMEA Printer settings - bool flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); - std::string nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); - std::string nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname); - - // RTCM Printer settings - bool flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); - std::string rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname); - bool flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false); - unsigned short rtcm_tcp_port = configuration->property(role + ".rtcm_tcp_port", 2101); - unsigned short rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); - // RTCM message rates: least common multiple with output_rate_ms - int rtcm_MT1019_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), output_rate_ms); - int rtcm_MT1045_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), output_rate_ms); - int rtcm_MSM_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), output_rate_ms); - int rtcm_MT1077_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); - int rtcm_MT1097_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); - std::map rtcm_msg_rate_ms; - rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; - rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; - for (int k = 1071; k < 1078; k++) // All GPS MSM - { - rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms; - } - for (int k = 1091; k < 1098; k++) // All Galileo MSM - { - rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; - } - // getting names from the config file, if available - // default filename for assistance data - const std::string eph_default_xml_filename = "./gps_ephemeris.xml"; - const std::string utc_default_xml_filename = "./gps_utc_model.xml"; - const std::string iono_default_xml_filename = "./gps_iono.xml"; - const std::string ref_time_default_xml_filename = "./gps_ref_time.xml"; - const std::string ref_location_default_xml_filename = "./gps_ref_location.xml"; - eph_xml_filename_ = configuration->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); - //std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); - //std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); - //std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); - //std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); - - // Infer the type of receiver - /* - * TYPE | RECEIVER - * 0 | Unknown - * 1 | GPS L1 C/A - * 2 | GPS L2C - * 3 | GPS L5 - * 4 | Galileo E1B - * 5 | Galileo E5a - * 6 | Galileo E5b - * 7 | GPS L1 C/A + GPS L2C - * 8 | GPS L1 C/A + GPS L5 - * 9 | GPS L1 C/A + Galileo E1B - * 10 | GPS L1 C/A + Galileo E5a - * 11 | GPS L1 C/A + Galileo E5b - * 12 | Galileo E1B + GPS L2C - * 13 | Galileo E1B + GPS L5 - * 14 | Galileo E1B + Galileo E5a - * 15 | Galileo E1B + Galileo E5b - * 16 | GPS L2C + GPS L5 - * 17 | GPS L2C + Galileo E5a - * 18 | GPS L2C + Galileo E5b - * 19 | GPS L5 + Galileo E5a - * 20 | GPS L5 + Galileo E5b - * 21 | GPS L1 C/A + Galileo E1B + GPS L2C - * 22 | GPS L1 C/A + Galileo E1B + GPS L5 - */ - int gps_1C_count = configuration->property("Channels_1C.count", 0); - int gps_2S_count = configuration->property("Channels_2S.count", 0); - int gal_1B_count = configuration->property("Channels_1B.count", 0); - int gal_E5a_count = configuration->property("Channels_5X.count", 0); // GPS L5 or Galileo E5a ? - int gal_E5b_count = configuration->property("Channels_7X.count", 0); - - unsigned int type_of_receiver = 0; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 1; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 2; - - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 4; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 5; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 6; - - if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 7; - //if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 8; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 9; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 10; - if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 11; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 12; - //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 13; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 14; - if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 15; - //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 16; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 17; - if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 18; - //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 19; - //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 20; - if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 21; - //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count = 0)) type_of_receiver = 22; - - // make PVT object - pvt_ = hybrid_make_pvt_cc(in_streams_, dump_, dump_filename_, averaging_depth, flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname, type_of_receiver); - DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; -} - - -bool HybridPvt::save_assistance_to_XML() -{ - LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename_; - std::map eph_map = pvt_->get_GPS_L1_ephemeris_map(); - - if (eph_map.size() > 0) - { - try - { - std::ofstream ofs(eph_xml_filename_.c_str(), std::ofstream::trunc | std::ofstream::out); - boost::archive::xml_oarchive xml(ofs); - xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_map); - ofs.close(); - LOG(INFO) << "Saved GPS L1 Ephemeris map data"; - } - catch (std::exception& e) - { - LOG(WARNING) << e.what(); - return false; - } - return true; // return variable (true == succeeded) - } - else - { - LOG(WARNING) << "Failed to save Ephemeris, map is empty"; - return false; - } -} - - -HybridPvt::~HybridPvt() -{ - save_assistance_to_XML(); -} - - -void HybridPvt::connect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to connect internally - DLOG(INFO) << "nothing to connect internally"; -} - - -void HybridPvt::disconnect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to disconnect -} - - -gr::basic_block_sptr HybridPvt::get_left_block() -{ - return pvt_; -} - - -gr::basic_block_sptr HybridPvt::get_right_block() -{ - return pvt_; // this is a sink, nothing downstream -} diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc new file mode 100644 index 000000000..7f63dc1e1 --- /dev/null +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -0,0 +1,531 @@ +/*! + * \file rtklib_pvt.cc + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#include "rtklib_pvt.h" +#include +#include +#include +#include +#include +#include "configuration_interface.h" + + +using google::LogMessage; + +RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, + std::string role, + unsigned int in_streams, + unsigned int out_streams) : + role_(role), + in_streams_(in_streams), + out_streams_(out_streams) +{ + // dump parameters + std::string default_dump_filename = "./pvt.dat"; + std::string default_nmea_dump_filename = "./nmea_pvt.nmea"; + std::string default_nmea_dump_devname = "/dev/tty1"; + std::string default_rtcm_dump_devname = "/dev/pts/1"; + DLOG(INFO) << "role " << role; + dump_ = configuration->property(role + ".dump", false); + dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); + + // output rate + int output_rate_ms = configuration->property(role + ".output_rate_ms", 500); + + // display rate + int display_rate_ms = configuration->property(role + ".display_rate_ms", 500); + + // NMEA Printer settings + bool flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false); + std::string nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename); + std::string nmea_dump_devname = configuration->property(role + ".nmea_dump_devname", default_nmea_dump_devname); + + // RINEX version + int rinex_version = configuration->property(role + ".rinex_version", 3); + if( (rinex_version < 2) || (rinex_version > 3) ) + { + //warn user and set the default + rinex_version = 3; + } + + // RTCM Printer settings + bool flag_rtcm_tty_port = configuration->property(role + ".flag_rtcm_tty_port", false); + std::string rtcm_dump_devname = configuration->property(role + ".rtcm_dump_devname", default_rtcm_dump_devname); + bool flag_rtcm_server = configuration->property(role + ".flag_rtcm_server", false); + unsigned short rtcm_tcp_port = configuration->property(role + ".rtcm_tcp_port", 2101); + unsigned short rtcm_station_id = configuration->property(role + ".rtcm_station_id", 1234); + // RTCM message rates: least common multiple with output_rate_ms + int rtcm_MT1019_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1019_rate_ms", 5000), output_rate_ms); + int rtcm_MT1045_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1045_rate_ms", 5000), output_rate_ms); + int rtcm_MSM_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MSM_rate_ms", 1000), output_rate_ms); + int rtcm_MT1077_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1077_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); + int rtcm_MT1097_rate_ms = boost::math::lcm(configuration->property(role + ".rtcm_MT1097_rate_ms", rtcm_MSM_rate_ms), output_rate_ms); + std::map rtcm_msg_rate_ms; + rtcm_msg_rate_ms[1019] = rtcm_MT1019_rate_ms; + rtcm_msg_rate_ms[1045] = rtcm_MT1045_rate_ms; + for (int k = 1071; k < 1078; k++) // All GPS MSM + { + rtcm_msg_rate_ms[k] = rtcm_MT1077_rate_ms; + } + for (int k = 1091; k < 1098; k++) // All Galileo MSM + { + rtcm_msg_rate_ms[k] = rtcm_MT1097_rate_ms; + } + // getting names from the config file, if available + // default filename for assistance data + const std::string eph_default_xml_filename = "./gps_ephemeris.xml"; + const std::string utc_default_xml_filename = "./gps_utc_model.xml"; + const std::string iono_default_xml_filename = "./gps_iono.xml"; + const std::string ref_time_default_xml_filename = "./gps_ref_time.xml"; + const std::string ref_location_default_xml_filename = "./gps_ref_location.xml"; + eph_xml_filename_ = configuration->property("GNSS-SDR.SUPL_gps_ephemeris_xml", eph_default_xml_filename); + //std::string utc_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_utc_model.xml", utc_default_xml_filename); + //std::string iono_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_iono_xml", iono_default_xml_filename); + //std::string ref_time_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_time_xml", ref_time_default_xml_filename); + //std::string ref_location_xml_filename = configuration_->property("GNSS-SDR.SUPL_gps_ref_location_xml", ref_location_default_xml_filename); + + // Infer the type of receiver + /* + * TYPE | RECEIVER + * 0 | Unknown + * 1 | GPS L1 C/A + * 2 | GPS L2C + * 3 | GPS L5 + * 4 | Galileo E1B + * 5 | Galileo E5a + * 6 | Galileo E5b + * 7 | GPS L1 C/A + GPS L2C + * 8 | GPS L1 C/A + GPS L5 + * 9 | GPS L1 C/A + Galileo E1B + * 10 | GPS L1 C/A + Galileo E5a + * 11 | GPS L1 C/A + Galileo E5b + * 12 | Galileo E1B + GPS L2C + * 13 | Galileo E1B + GPS L5 + * 14 | Galileo E1B + Galileo E5a + * 15 | Galileo E1B + Galileo E5b + * 16 | GPS L2C + GPS L5 + * 17 | GPS L2C + Galileo E5a + * 18 | GPS L2C + Galileo E5b + * 19 | GPS L5 + Galileo E5a + * 20 | GPS L5 + Galileo E5b + * 21 | GPS L1 C/A + Galileo E1B + GPS L2C + * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + */ + int gps_1C_count = configuration->property("Channels_1C.count", 0); + int gps_2S_count = configuration->property("Channels_2S.count", 0); + int gal_1B_count = configuration->property("Channels_1B.count", 0); + int gal_E5a_count = configuration->property("Channels_5X.count", 0); // GPS L5 or Galileo E5a ? + int gal_E5b_count = configuration->property("Channels_7X.count", 0); + + unsigned int type_of_receiver = 0; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 1; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 2; + + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 4; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 5; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 6; + + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 7; + //if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 8; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 9; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 10; + if( (gps_1C_count != 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 11; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 12; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 13; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 14; + if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 15; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 16; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count != 0) && (gal_E5b_count == 0)) type_of_receiver = 17; + if( (gps_1C_count == 0) && (gps_2S_count != 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count != 0)) type_of_receiver = 18; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 19; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 20; + if( (gps_1C_count != 0) && (gps_2S_count != 0) && (gal_1B_count != 0) && (gal_E5a_count == 0) && (gal_E5b_count == 0)) type_of_receiver = 21; + //if( (gps_1C_count == 0) && (gps_2S_count == 0) && (gal_1B_count == 0) && (gal_E5a_count == 0) && (gal_E5b_count = 0)) type_of_receiver = 22; + + //RTKLIB PVT solver options + // Settings 1 + int positioning_mode = -1; + std::string default_pos_mode("Single"); + std::string positioning_mode_str = configuration->property(role + ".positioning_mode", default_pos_mode); /* (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + if(positioning_mode_str.compare("Single") == 0) positioning_mode = PMODE_SINGLE; + if(positioning_mode_str.compare("Static") == 0) positioning_mode = PMODE_STATIC; + if(positioning_mode_str.compare("Kinematic") == 0) positioning_mode = PMODE_KINEMA; + if(positioning_mode_str.compare("PPP_Static") == 0) positioning_mode = PMODE_PPP_STATIC; + if(positioning_mode_str.compare("PPP_Kinematic") == 0) positioning_mode = PMODE_PPP_KINEMA; + + if( positioning_mode == -1 ) + { + //warn user and set the default + std::cout << "WARNING: Bad specification of positioning mode." << std::endl; + std::cout << "positioning_mode possible values: Single / Static / Kinematic / PPP_Static / PPP_Kinematic" << std::endl; + std::cout << "positioning_mode specified value: " << positioning_mode_str << std::endl; + std::cout << "Setting positioning_mode to Single" << std::endl; + positioning_mode = PMODE_SINGLE; + } + + int num_bands = 0; + if ((gps_1C_count > 0) || (gal_1B_count > 0)) num_bands = 1; + if (((gps_1C_count > 0) || (gal_1B_count > 0)) && (gps_2S_count > 0) ) num_bands = 2; + if (((gps_1C_count > 0) || (gal_1B_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5a_count > 0))) num_bands = 3; + int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */ + if( (number_of_frequencies < 1) || (number_of_frequencies > 3) ) + { + //warn user and set the default + number_of_frequencies = num_bands; + } + + double elevation_mask = configuration->property(role + ".elevation_mask", 15.0); + if( (elevation_mask < 0.0) || (elevation_mask > 90.0) ) + { + //warn user and set the default + elevation_mask = 15.0; + } + + int dynamics_model = configuration->property(role + ".dynamics_model", 0); /* dynamics model (0:none, 1:velocity, 2:accel) */ + if( (dynamics_model < 0) || (dynamics_model > 2) ) + { + //warn user and set the default + dynamics_model = 0; + } + + std::string default_iono_model("OFF"); + std::string iono_model_str = configuration->property(role + ".iono_model", default_iono_model); /* (IONOOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + int iono_model = -1; + if(iono_model_str.compare("OFF") == 0) iono_model = IONOOPT_OFF; + if(iono_model_str.compare("Broadcast") == 0) iono_model = IONOOPT_BRDC; + if(iono_model_str.compare("SBAS") == 0) iono_model = IONOOPT_SBAS; + if(iono_model_str.compare("Iono-Free-LC") == 0) iono_model = IONOOPT_IFLC; + if(iono_model_str.compare("Estimate_STEC") == 0) iono_model = IONOOPT_EST; + if(iono_model_str.compare("IONEX") == 0) iono_model = IONOOPT_TEC; + if( iono_model == -1 ) + { + //warn user and set the default + std::cout << "WARNING: Bad specification of ionospheric model." << std::endl; + std::cout << "iono_model possible values: OFF / Broadcast / SBAS / Iono-Free-LC / Estimate_STEC / IONEX" << std::endl; + std::cout << "iono_model specified value: " << iono_model_str << std::endl; + std::cout << "Setting iono_model to OFF" << std::endl; + iono_model = IONOOPT_OFF; /* 0: ionosphere option: correction off */ + } + + std::string default_trop_model("OFF"); + int trop_model = -1; + std::string trop_model_str = configuration->property(role + ".trop_model", default_trop_model); /* (TROPOPT_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + if(trop_model_str.compare("OFF") == 0) trop_model = TROPOPT_OFF; + if(trop_model_str.compare("Saastamoinen") == 0) trop_model = TROPOPT_SAAS; + if(trop_model_str.compare("SBAS") == 0) trop_model = TROPOPT_SBAS; + if(trop_model_str.compare("Estimate_ZTD") == 0) trop_model = TROPOPT_EST; + if(trop_model_str.compare("Estimate_ZTD_Grad") == 0) trop_model = TROPOPT_ESTG; + if( trop_model == -1 ) + { + //warn user and set the default + std::cout << "WARNING: Bad specification of tropospheric model." << std::endl; + std::cout << "trop_model possible values: OFF / Saastamoinen / SBAS / Estimate_ZTD / Estimate_ZTD_Grad" << std::endl; + std::cout << "trop_model specified value: " << trop_model_str << std::endl; + std::cout << "Setting trop_model to OFF" << std::endl; + trop_model = TROPOPT_OFF; + } + + /* RTKLIB positioning options */ + int sat_PCV = 0; /* Set whether the satellite antenna PCV (phase center variation) model is used or not. This feature requires a Satellite Antenna PCV File. */ + int rec_PCV = 0; /* Set whether the receiver antenna PCV (phase center variation) model is used or not. This feature requires a Receiver Antenna PCV File. */ + int phwindup = 0; /* Set whether the phase windup correction for PPP modes is applied or not. Only applicable to PPP‐* modes.*/ + int reject_GPS_IIA = 0; /* Set whether the GPS Block IIA satellites in eclipse are excluded or not. + The eclipsing Block IIA satellites often degrade the PPP solutions due to unpredicted behavior of yaw‐attitude. Only applicable to PPP‐* modes.*/ + + int raim_fde = 0; /* Set whether RAIM (receiver autonomous integrity monitoring) FDE (fault detection and exclusion) feature is enabled or not. + In case of RAIM FDE enabled, a satellite is excluded if SSE (sum of squared errors) of residuals is over a threshold. + The excluded satellite is selected to indicate the minimum SSE. */ + + int nsys = 0; + if ((gps_1C_count > 0) || (gps_2S_count > 0)) nsys += SYS_GPS; + if ((gal_1B_count > 0) || (gal_E5a_count > 0) || (gal_E5b_count > 0)) nsys += SYS_GAL; + int navigation_system = configuration->property(role + ".navigation_system", nsys); /* (SYS_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + if( (navigation_system < 1) || (navigation_system > 255) ) /* GPS: 1 SBAS: 2 GPS+SBAS: 3 Galileo: 8 Galileo+GPS: 9 GPS+SBAS+Galileo: 11 All: 255 */ + { + //warn user and set the default + navigation_system = nsys; + } + + // Settings 2 + std::string default_gps_ar("Continuous"); + std::string integer_ambiguity_resolution_gps_str = configuration->property(role + ".AR_GPS", default_gps_ar); /* Integer Ambiguity Resolution mode for GPS (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ + int integer_ambiguity_resolution_gps = -1; + if(integer_ambiguity_resolution_gps_str.compare("OFF") == 0) integer_ambiguity_resolution_gps = ARMODE_OFF; + if(integer_ambiguity_resolution_gps_str.compare("Continuous") == 0) integer_ambiguity_resolution_gps = ARMODE_CONT; + if(integer_ambiguity_resolution_gps_str.compare("Instantaneous") == 0) integer_ambiguity_resolution_gps = ARMODE_INST; + if(integer_ambiguity_resolution_gps_str.compare("Fix-and-Hold") == 0) integer_ambiguity_resolution_gps = ARMODE_FIXHOLD; + if(integer_ambiguity_resolution_gps_str.compare("PPP-AR") == 0) integer_ambiguity_resolution_gps = ARMODE_PPPAR; + if( integer_ambiguity_resolution_gps == -1 ) + { + //warn user and set the default + std::cout << "WARNING: Bad specification of GPS ambiguity resolution method." << std::endl; + std::cout << "AR_GPS possible values: OFF / Continuous / Instantaneous / Fix-and-Hold / PPP-AR" << std::endl; + std::cout << "AR_GPS specified value: " << integer_ambiguity_resolution_gps_str << std::endl; + std::cout << "Setting AR_GPS to OFF" << std::endl; + integer_ambiguity_resolution_gps = ARMODE_OFF; + } + + int integer_ambiguity_resolution_glo = configuration->property(role + ".AR_GLO", 1); /* Integer Ambiguity Resolution mode for GLONASS (0:off,1:on,2:auto cal,3:ext cal) */ + if( (integer_ambiguity_resolution_glo < 0) || (integer_ambiguity_resolution_glo > 3) ) + { + //warn user and set the default + integer_ambiguity_resolution_glo = 1; + } + + int integer_ambiguity_resolution_bds = configuration->property(role + ".AR_DBS", 1); /* Integer Ambiguity Resolution mode for BEIDOU (0:off,1:on) */ + if( (integer_ambiguity_resolution_bds < 0) || (integer_ambiguity_resolution_bds > 1) ) + { + //warn user and set the default + integer_ambiguity_resolution_bds = 1; + } + + double min_ratio_to_fix_ambiguity = configuration->property(role + ".min_ratio_to_fix_ambiguity", 3.0); /* Set the integer ambiguity validation threshold for ratio‐test, + which uses the ratio of squared residuals of the best integer vector to the second‐best vector. */ + + int min_lock_to_fix_ambiguity = configuration->property(role + ".min_lock_to_fix_ambiguity", 0); /* Set the minimum lock count to fix integer ambiguity. + If the lock count is less than the value, the ambiguity is excluded from the fixed integer vector. */ + + double min_elevation_to_fix_ambiguity = configuration->property(role + ".min_elevation_to_fix_ambiguity", 0.0); /* Set the minimum elevation (deg) to fix integer ambiguity. + If the elevation of the satellite is less than the value, the ambiguity is excluded from the fixed integer vector. */ + + int outage_reset_ambiguity = configuration->property(role + ".outage_reset_ambiguity", 5); /* Set the outage count to reset ambiguity. If the data outage count is over the value, the estimated ambiguity is reset to the initial value. */ + + double slip_threshold = configuration->property(role + ".slip_threshold", 0.05); /* set the cycle‐slip threshold (m) of geometry‐free LC carrier‐phase difference between epochs */ + + double threshold_reject_gdop = configuration->property(role + ".threshold_reject_gdop", 30.0); /* reject threshold of GDOP. If the GDOP is over the value, the observable is excluded for the estimation process as an outlier. */ + + double threshold_reject_innovation = configuration->property(role + ".threshold_reject_innovation", 30.0); /* reject threshold of innovation (m). If the innovation is over the value, the observable is excluded for the estimation process as an outlier. */ + + int number_filter_iter = configuration->property(role + ".number_filter_iter", 1); /* Set the number of iteration in the measurement update of the estimation filter. + If the baseline length is very short like 1 m, the iteration may be effective to handle + the nonlinearity of measurement equation. */ + + /// Statistics + double bias_0 = configuration->property(role + ".bias_0", 30.0); + + double iono_0 = configuration->property(role + ".iono_0", 0.03); + + double trop_0 = configuration->property(role + ".trop_0", 0.3); + + double sigma_bias = configuration->property(role + ".sigma_bias", 1e-4); /* Set the process noise standard deviation of carrier‐phase + bias (ambiguity) (cycle/sqrt(s)) */ + + double sigma_iono = configuration->property(role + ".sigma_iono", 1e-3); /* Set the process noise standard deviation of vertical ionospheric delay per 10 km baseline (m/sqrt(s)). */ + + double sigma_trop = configuration->property(role + ".sigma_trop", 1e-4); /* Set the process noise standard deviation of zenith tropospheric delay (m/sqrt(s)). */ + + double sigma_acch = configuration->property(role + ".sigma_acch", 1e-1); /* Set the process noise standard deviation of the receiver acceleration as + the horizontal component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */ + + double sigma_accv = configuration->property(role + ".sigma_accv", 1e-2); /* Set the process noise standard deviation of the receiver acceleration as + the vertical component. (m/s2/sqrt(s)). If Receiver Dynamics is set to OFF, they are not used. */ + + double sigma_pos = configuration->property(role + ".sigma_pos", 0.0); + + snrmask_t snrmask = { {}, {{},{}} }; + + prcopt_t rtklib_configuration_options = {positioning_mode, /* positioning mode (PMODE_XXX) see src/algorithms/libs/rtklib/rtklib.h */ + 0, /* solution type (0:forward,1:backward,2:combined) */ + number_of_frequencies, /* number of frequencies (1:L1, 2:L1+L2, 3:L1+L2+L5)*/ + navigation_system, /* navigation system */ + elevation_mask * D2R, /* elevation mask angle (degrees) */ + snrmask, /* snrmask_t snrmask SNR mask */ + 0, /* satellite ephemeris/clock (EPHOPT_XXX) */ + integer_ambiguity_resolution_gps, /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ + integer_ambiguity_resolution_glo, /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */ + integer_ambiguity_resolution_bds, /* BeiDou AR mode (0:off,1:on) */ + outage_reset_ambiguity, /* obs outage count to reset bias */ + min_lock_to_fix_ambiguity, /* min lock count to fix ambiguity */ + 10, /* min fix count to hold ambiguity */ + 1, /* max iteration to resolve ambiguity */ + iono_model, /* ionosphere option (IONOOPT_XXX) */ + trop_model, /* troposphere option (TROPOPT_XXX) */ + dynamics_model, /* dynamics model (0:none, 1:velocity, 2:accel) */ + 0, /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */ + number_filter_iter, /* number of filter iteration */ + 0, /* code smoothing window size (0:none) */ + 0, /* interpolate reference obs (for post mission) */ + 0, /* sbssat_t sbssat SBAS correction options */ + 0, /* sbsion_t sbsion[MAXBAND+1] SBAS satellite selection (0:all) */ + 0, /* rover position for fixed mode */ + 0, /* base position for relative mode */ + /* 0:pos in prcopt, 1:average of single pos, */ + /* 2:read from file, 3:rinex header, 4:rtcm pos */ + {100.0,100.0,100.0}, /* eratio[NFREQ] code/phase error ratio */ + {100.0,0.003,0.003,0.0,1.0}, /* err[5]: measurement error factor [0]:reserved, [1-3]:error factor a/b/c of phase (m) , [4]:doppler frequency (hz) */ + {bias_0,iono_0,trop_0}, /* std[3]: initial-state std [0]bias,[1]iono [2]trop*/ + {sigma_bias,sigma_iono,sigma_trop,sigma_acch,sigma_accv,sigma_pos}, /* prn[6] process-noise std */ + 5e-12, /* sclkstab: satellite clock stability (sec/sec) */ + {min_ratio_to_fix_ambiguity,0.9999,0.25,0.1,0.05,0.0,0.0,0.0}, /* thresar[8]: AR validation threshold */ + min_elevation_to_fix_ambiguity, /* elevation mask of AR for rising satellite (deg) */ + 0.0, /* elevation mask to hold ambiguity (deg) */ + slip_threshold, /* slip threshold of geometry-free phase (m) */ + 30.0, /* max difference of time (sec) */ + threshold_reject_innovation, /* reject threshold of innovation (m) */ + threshold_reject_gdop, /* reject threshold of gdop */ + {}, /* double baseline[2] baseline length constraint {const,sigma} (m) */ + {}, /* double ru[3] rover position for fixed mode {x,y,z} (ecef) (m) */ + {}, /* double rb[3] base position for relative mode {x,y,z} (ecef) (m) */ + {"",""}, /* char anttype[2][MAXANT] antenna types {rover,base} */ + {{},{}}, /* double antdel[2][3] antenna delta {{rov_e,rov_n,rov_u},{ref_e,ref_n,ref_u}} */ + {}, /* pcv_t pcvr[2] receiver antenna parameters {rov,base} */ + {}, /* unsigned char exsats[MAXSAT] excluded satellites (1:excluded, 2:included) */ + 0, /* max averaging epoches */ + 0, /* initialize by restart */ + 1, /* output single by dgps/float/fix/ppp outage */ + {"",""}, /* char rnxopt[2][256] rinex options {rover,base} */ + {sat_PCV,rec_PCV,phwindup,reject_GPS_IIA,raim_fde}, /* posopt[6] positioning options [0]: satellite and receiver antenna PCV model; [1]: interpolate antenna parameters; [2]: apply phase wind-up correction for PPP modes; [3]: exclude measurements of GPS Block IIA satellites satellite [4]: RAIM FDE (fault detection and exclusion) [5]: handle day-boundary clock jump */ + 0, /* solution sync mode (0:off,1:on) */ + {{},{}}, /* odisp[2][6*11] ocean tide loading parameters {rov,base} */ + { {}, {{},{}}, {{},{}}, {}, {} }, /* exterr_t exterr extended receiver error model */ + 0, /* disable L2-AR */ + {} /* char pppopt[256] ppp option "-GAP_RESION=" default gap to reset iono parameters (ep) */ + }; + + sol_t sol_ = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, '0', '0', '0', 0, 0, 0 }; + + ambc_t ambc_ = { {{0,0}, {0,0}, {0,0}, {0,0}}, {0, 0, 0, 0}, {}, {}, 0, {'0'}}; + + ssat_t ssat_ = { '0', /* navigation system */ + '0', /* valid satellite flag single */ + {0.0}, /* azel[2] azimuth/elevation angles {az,el} (rad) */ + {0.0}, /* residuals of pseudorange (m) */ + {0.0}, /* residuals of carrier-phase (m) */ + {'0'}, /* valid satellite flag */ + {'0'}, /* signal strength (0.25 dBHz) */ + {'0'}, /* ambiguity fix flag (1:fix,2:float,3:hold) */ + {'0'}, /* cycle-slip flag */ + {'0'}, /* half-cycle valid flag */ + {}, /* lock counter of phase */ + {}, /* obs outage counter of phase */ + {}, /* cycle-slip counter */ + {}, /* reject counter */ + 0.0, /* geometry-free phase L1-L2 (m) */ + 0.0, /* geometry-free phase L1-L5 (m) */ + 0.0, /* MW-LC (m) */ + 0.0, /* phase windup (cycle) */ + {{{0,0}},{{0,0}}}, /* previous carrier-phase time */ + {{},{}} /* previous carrier-phase observable (cycle) */ + }; + + int nx = 0; /* Number of estimated states */ + if(positioning_mode <= PMODE_FIXED) nx = 4 + 3; + if(positioning_mode >= PMODE_PPP_KINEMA) nx = NX_PPP(&rtklib_configuration_options); + int na = NP_PPP(&rtklib_configuration_options); + + double x[nx]; + double Px[nx*nx]; + double xa[na]; + double Pa[na*na]; + rtk = { sol_, /* RTK solution */ + {}, /* base position/velocity (ecef) (m|m/s) */ + nx, /* number of float states */ + na, /* number of fixed states */ + output_rate_ms / 1000.0, /* time difference between current and previous (s) */ + x, /* float states */ + Px, /* float states covariance */ + xa, /* fixed states */ + Pa, /* fixed states covariance */ + 3, /* number of continuous fixes of ambiguity */ + {ambc_}, /* ambiguity control */ + {ssat_}, /* satellite status */ + 256, /* bytes in error message buffer */ + {'0'}, /* error message buffer */ + rtklib_configuration_options /* processing options */ + }; + + // make PVT object + pvt_ = rtklib_make_pvt_cc(in_streams_, dump_, dump_filename_, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, rinex_version, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname, type_of_receiver, rtk); + DLOG(INFO) << "pvt(" << pvt_->unique_id() << ")"; +} + + +bool RtklibPvt::save_assistance_to_XML() +{ + LOG(INFO) << "SUPL: Try to save GPS ephemeris to XML file " << eph_xml_filename_; + std::map eph_map = pvt_->get_GPS_L1_ephemeris_map(); + + if (eph_map.size() > 0) + { + try + { + std::ofstream ofs(eph_xml_filename_.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", eph_map); + ofs.close(); + LOG(INFO) << "Saved GPS L1 Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + return false; + } + return true; // return variable (true == succeeded) + } + else + { + LOG(WARNING) << "Failed to save Ephemeris, map is empty"; + return false; + } +} + + +RtklibPvt::~RtklibPvt() +{ + save_assistance_to_XML(); +} + + +void RtklibPvt::connect(gr::top_block_sptr top_block) +{ + if(top_block) { /* top_block is not null */}; + // Nothing to connect internally + DLOG(INFO) << "nothing to connect internally"; +} + + +void RtklibPvt::disconnect(gr::top_block_sptr top_block) +{ + if(top_block) { /* top_block is not null */}; + // Nothing to disconnect +} + + +gr::basic_block_sptr RtklibPvt::get_left_block() +{ + return pvt_; +} + + +gr::basic_block_sptr RtklibPvt::get_right_block() +{ + return pvt_; // this is a sink, nothing downstream +} diff --git a/src/algorithms/PVT/adapters/hybrid_pvt.h b/src/algorithms/PVT/adapters/rtklib_pvt.h similarity index 81% rename from src/algorithms/PVT/adapters/hybrid_pvt.h rename to src/algorithms/PVT/adapters/rtklib_pvt.h index c2a5c30d3..33d04ecf0 100644 --- a/src/algorithms/PVT/adapters/hybrid_pvt.h +++ b/src/algorithms/PVT/adapters/rtklib_pvt.h @@ -1,8 +1,7 @@ /*! - * \file hybrid_pvt.h - * \brief Interface of an adapter of a GALILEO E1 PVT solver block to a - * PvtInterface. - * \author Javier Arribas, 2013. jarribas(at)cttc.es + * \file rtklib_pvt.h + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es * * ------------------------------------------------------------------------- * @@ -31,12 +30,12 @@ -#ifndef GNSS_SDR_HYBRID_PVT_H_ -#define GNSS_SDR_HYBRID_PVT_H_ +#ifndef GNSS_SDR_RTKLIB_PVT_H_ +#define GNSS_SDR_RTKLIB_PVT_H_ #include #include "pvt_interface.h" -#include "hybrid_pvt_cc.h" +#include "rtklib_pvt_cc.h" class ConfigurationInterface; @@ -44,25 +43,25 @@ class ConfigurationInterface; /*! * \brief This class implements a PvtInterface for Galileo E1 */ -class HybridPvt : public PvtInterface +class RtklibPvt : public PvtInterface { public: - HybridPvt(ConfigurationInterface* configuration, + RtklibPvt(ConfigurationInterface* configuration, std::string role, unsigned int in_streams, unsigned int out_streams); - virtual ~HybridPvt(); + virtual ~RtklibPvt(); std::string role() { return role_; } - //! Returns "Hybrid_Pvt" + //! Returns "RTKLIB_Pvt" std::string implementation() { - return "Hybrid_PVT"; + return "RTKLIB_PVT"; } void connect(gr::top_block_sptr top_block); @@ -82,13 +81,16 @@ public: } private: - hybrid_pvt_cc_sptr pvt_; + rtklib_pvt_cc_sptr pvt_; + + rtk_t rtk; + bool dump_; std::string dump_filename_; std::string role_; unsigned int in_streams_; unsigned int out_streams_; - + std::string eph_xml_filename_; bool save_assistance_to_XML(); }; diff --git a/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt b/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt index 1eb1eaf1c..a80c236a4 100644 --- a/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/PVT/gnuradio_blocks/CMakeLists.txt @@ -16,10 +16,8 @@ # along with GNSS-SDR. If not, see . # -set(PVT_GR_BLOCKS_SOURCES - gps_l1_ca_pvt_cc.cc - galileo_e1_pvt_cc.cc - hybrid_pvt_cc.cc +set(PVT_GR_BLOCKS_SOURCES + rtklib_pvt_cc.cc ) include_directories( @@ -28,6 +26,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${ARMADILLO_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc deleted file mode 100644 index e7c1f9813..000000000 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.cc +++ /dev/null @@ -1,424 +0,0 @@ -/*! - * \file galileo_e1_pvt_cc.cc - * \brief Implementation of a Position Velocity and Time computation block for GPS L1 C/A - * \author Javier Arribas, 2013. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "galileo_e1_pvt_cc.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include "concurrent_map.h" - -using google::LogMessage; - - -galileo_e1_pvt_cc_sptr galileo_e1_make_pvt_cc(unsigned int nchannels, bool dump, std::string dump_filename, int averaging_depth, - bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, - std::string nmea_dump_devname, bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname) -{ - return galileo_e1_pvt_cc_sptr(new galileo_e1_pvt_cc(nchannels, dump, dump_filename, averaging_depth, - flag_averaging, output_rate_ms, display_rate_ms, flag_nmea_tty_port, nmea_dump_filename, nmea_dump_devname, - flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_msg_rate_ms, rtcm_dump_devname)); -} - - -void galileo_e1_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) -{ - try { - if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo EPHEMERIS ### - std::shared_ptr galileo_eph; - galileo_eph = boost::any_cast>(pmt::any_ref(msg)); - // insert new ephemeris record - DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5 - << ", GALILEO Week Number =" << galileo_eph->WN_5 - << " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris; - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo IONO ### - std::shared_ptr galileo_iono; - galileo_iono = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->galileo_iono = *galileo_iono; - DLOG(INFO) << "New IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo UTC MODEL ### - std::shared_ptr galileo_utc_model; - galileo_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->galileo_utc_model = *galileo_utc_model; - DLOG(INFO) << "New UTC record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo Almanac ### - std::shared_ptr galileo_almanac; - galileo_almanac = boost::any_cast>(pmt::any_ref(msg)); - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->galileo_almanac = *galileo_almanac; - DLOG(INFO) << "New Galileo Almanac has arrived "; - } - else - { - LOG(WARNING) << "msg_handler_telemetry unknown object type!"; - } - - } - catch(boost::bad_any_cast& e) - { - LOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; - } -} - - -galileo_e1_pvt_cc::galileo_e1_pvt_cc(unsigned int nchannels, bool dump, std::string dump_filename, int averaging_depth, - bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname, - bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname) : - gr::block("galileo_e1_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), gr::io_signature::make(0, 0, sizeof(gr_complex))) -{ - d_output_rate_ms = output_rate_ms; - d_display_rate_ms = display_rate_ms; - d_dump = dump; - d_nchannels = nchannels; - d_dump_filename = dump_filename; - std::string dump_ls_pvt_filename = dump_filename; - - // GPS Ephemeris data message port in - this->message_port_register_in(pmt::mp("telemetry")); - this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&galileo_e1_pvt_cc::msg_handler_telemetry, this, _1)); - - //initialize kml_printer - std::string kml_dump_filename; - kml_dump_filename = d_dump_filename; - d_kml_dump = std::make_shared(); - d_kml_dump->set_headers(kml_dump_filename); - - //initialize geojson_printer - std::string geojson_dump_filename; - geojson_dump_filename = d_dump_filename; - d_geojson_printer = std::make_shared(); - d_geojson_printer->set_headers(geojson_dump_filename); - - //initialize nmea_printer - d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); - - //initialize rtcm_printer - std::string rtcm_dump_filename; - rtcm_dump_filename = d_dump_filename; - unsigned short _port = rtcm_tcp_port; - unsigned short _station_id = rtcm_station_id; - d_rtcm_printer = std::make_shared(rtcm_dump_filename, flag_rtcm_server, flag_rtcm_tty_port, _port, _station_id, rtcm_dump_devname); - if(rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end()) - { - d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045]; - } - else - { - d_rtcm_MT1045_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set - } - if(rtcm_msg_rate_ms.find(1091) != rtcm_msg_rate_ms.end()) // whatever between 1091 and 1097 - { - d_rtcm_MSM_rate_ms = rtcm_msg_rate_ms[1091]; - } - else - { - d_rtcm_MSM_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set - } - b_rtcm_writing_started = false; - - d_dump_filename.append("_raw.dat"); - dump_ls_pvt_filename.append("_ls_pvt.dat"); - d_averaging_depth = averaging_depth; - d_flag_averaging = flag_averaging; - - d_ls_pvt = std::make_shared(nchannels, dump_ls_pvt_filename, d_dump); - d_ls_pvt->set_averaging_depth(d_averaging_depth); - - d_sample_counter = 0; - d_last_sample_nav_output = 0; - d_rx_time = 0.0; - - b_rinex_header_written = false; - b_rinex_header_updated = false; - - rp = std::make_shared(); - - d_last_status_print_seg = 0; - - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "PVT dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception opening PVT dump file " << e.what(); - } - } - } - - // Create Sys V message queue - first_fix = true; - sysv_msg_key = 1101; - int msgflg = IPC_CREAT | 0666; - if ((sysv_msqid = msgget(sysv_msg_key, msgflg )) == -1) - { - std::cout << "GNSS-SDR can not create message queues!" << std::endl; - throw new std::exception(); - } -} - - - -galileo_e1_pvt_cc::~galileo_e1_pvt_cc() -{ - msgctl(sysv_msqid, IPC_RMID, NULL); -} - - -void galileo_e1_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchronization_data) -{ - // Print the current receiver status using std::cout every second - int current_rx_seg = floor(channels_synchronization_data[0][0].Tracking_timestamp_secs); - if ( current_rx_seg != d_last_status_print_seg) - { - d_last_status_print_seg = current_rx_seg; - std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl << std::flush; - //DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) - // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; - } -} - - -bool galileo_e1_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) -{ - /* Fill Sys V message structures */ - int msgsend_size; - ttff_msgbuf msg; - msg.ttff = ttff.ttff; - msgsend_size = sizeof(msg.ttff); - msg.mtype = 1; /* default message ID */ - - /* SEND SOLUTION OVER A MESSAGE QUEUE */ - /* non-blocking Sys V message send */ - msgsnd(sysv_msqid, &msg, msgsend_size, IPC_NOWAIT); - return true; -} - -int galileo_e1_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) -{ - d_sample_counter++; - - std::map gnss_observables_map; - - Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer - - print_receiver_status(in); - - // ############ 1. READ PSEUDORANGES #### - for (unsigned int i = 0; i < d_nchannels; i++) - { - if (in[i][0].Flag_valid_pseudorange == true) - { - gnss_observables_map.insert(std::pair(in[i][0].PRN, in[i][0])); // store valid pseudoranges in a map - d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges) - if(d_ls_pvt->galileo_ephemeris_map.size() > 0) - { - std::map::iterator tmp_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN); - if(tmp_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time - } - } - } - } - - // ############ 2 COMPUTE THE PVT ################################ - if (gnss_observables_map.size() > 0 and d_ls_pvt->galileo_ephemeris_map.size() > 0) - { - // compute on the fly PVT solution - if ((d_sample_counter % d_output_rate_ms) == 0) - { - bool pvt_result; - pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); - - if (pvt_result == true) - { - // correct the observable to account for the receiver clock offset - for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) - { - it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s; - } - if( first_fix == true) - { - std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; - ttff_msgbuf ttff; - ttff.mtype = 1; - ttff.ttff = d_sample_counter; - send_sys_v_ttff_msg(ttff); - first_fix = false; - } - d_kml_dump->print_position(d_ls_pvt, d_flag_averaging); - d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging); - d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); - - if (!b_rinex_header_written) - { - std::map::iterator galileo_ephemeris_iter; - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time); - rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(b_rinex_header_written) // Put here another condition to separate annotations (e.g 30 s) - { - // Limit the RINEX navigation output rate to 1/6 seg - // Notice that d_sample_counter period is 4ms (for Galileo correlators) - if ((d_sample_counter - d_last_sample_nav_output) >= 6000) - { - rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); - d_last_sample_nav_output = d_sample_counter; - } - std::map::iterator galileo_ephemeris_iter; - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); - } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) - { - rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); - b_rinex_header_updated = true; - } - } - - if(b_rtcm_writing_started) - { - if((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4) ) == 0) - { - for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); - } - } - if((d_sample_counter % (d_rtcm_MSM_rate_ms / 4) ) == 0) - { - std::map::iterator gal_ephemeris_iter; - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } - } - if(!b_rtcm_writing_started) // the first time - { - for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); - } - - std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - } - } - - // DEBUG MESSAGE: Display position in console output - if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true) - { - std::cout << "Galileo Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; - - LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]"; - - LOG(INFO) << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " - << d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP - << " GDOP = " << d_ls_pvt->d_GDOP; - } - - // MULTIPLEXED FILE RECORDING - Record results to file - if(d_dump == true) - { - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) - { - tmp_double = in[i][0].Pseudorange_m; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = 0; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - d_dump_file.write((char*)&d_rx_time, sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } - } - } - - consume_each(1); //one by one - return 1; -} - - diff --git a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h deleted file mode 100644 index 6a16dd566..000000000 --- a/src/algorithms/PVT/gnuradio_blocks/galileo_e1_pvt_cc.h +++ /dev/null @@ -1,155 +0,0 @@ -/*! - * \file galileo_e1_pvt_cc.h - * \brief Interface of a Position Velocity and Time computation block for Galileo E1 - * \author Javier Arribas, 2013. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_GALILEO_E1_PVT_CC_H -#define GNSS_SDR_GALILEO_E1_PVT_CC_H - -#include -#include -#include -#include -#include -#include -#include -#include "nmea_printer.h" -#include "kml_printer.h" -#include "rinex_printer.h" -#include "geojson_printer.h" -#include "rtcm_printer.h" -#include "galileo_e1_ls_pvt.h" - - -class galileo_e1_pvt_cc; - -typedef boost::shared_ptr galileo_e1_pvt_cc_sptr; - -galileo_e1_pvt_cc_sptr galileo_e1_make_pvt_cc(unsigned int n_channels, - bool dump, - std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname); - -/*! - * \brief This class implements a block that computes the PVT solution with Galileo E1 signals - */ -class galileo_e1_pvt_cc : public gr::block -{ -private: - friend galileo_e1_pvt_cc_sptr galileo_e1_make_pvt_cc(unsigned int nchannels, - bool dump, - std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname); - galileo_e1_pvt_cc(unsigned int nchannels, - bool dump, std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname); - - void msg_handler_telemetry(pmt::pmt_t msg); - - bool d_dump; - bool b_rinex_header_written; - bool b_rinex_header_updated; - bool b_rtcm_writing_started; - - void print_receiver_status(Gnss_Synchro** channels_synchronization_data); - int d_last_status_print_seg; //for status printer - - unsigned int d_nchannels; - std::string d_dump_filename; - std::ofstream d_dump_file; - int d_averaging_depth; - bool d_flag_averaging; - int d_output_rate_ms; - int d_display_rate_ms; - long unsigned int d_sample_counter; - long unsigned int d_last_sample_nav_output; - int d_rtcm_MT1045_rate_ms; - int d_rtcm_MSM_rate_ms; - - std::shared_ptr rp; - std::shared_ptr d_kml_dump; - std::shared_ptr d_nmea_printer; - std::shared_ptr d_geojson_printer; - std::shared_ptr d_rtcm_printer; - - double d_rx_time; - std::shared_ptr d_ls_pvt; - - bool first_fix; - key_t sysv_msg_key; - int sysv_msqid; - typedef struct { - long mtype;//required by sys v message - double ttff; - } ttff_msgbuf; - bool send_sys_v_ttff_msg(ttff_msgbuf ttff); - -public: - ~galileo_e1_pvt_cc (); //!< Default destructor - - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing -}; - -#endif diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc deleted file mode 100644 index dddb3ee16..000000000 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.cc +++ /dev/null @@ -1,505 +0,0 @@ -/*! - * \file gps_l1_ca_pvt_cc.cc - * \brief Implementation of a Position Velocity and Time computation block for GPS L1 C/A - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "gps_l1_ca_pvt_cc.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "concurrent_map.h" -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" - -using google::LogMessage; - -gps_l1_ca_pvt_cc_sptr -gps_l1_ca_make_pvt_cc(unsigned int nchannels, - bool dump, std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname, - int rinex_version) -{ - return gps_l1_ca_pvt_cc_sptr(new gps_l1_ca_pvt_cc(nchannels, - dump, - dump_filename, - averaging_depth, - flag_averaging, - output_rate_ms, - display_rate_ms, - flag_nmea_tty_port, - nmea_dump_filename, - nmea_dump_devname, - flag_rtcm_server, - flag_rtcm_tty_port, - rtcm_tcp_port, - rtcm_station_id, - rtcm_msg_rate_ms, - rtcm_dump_devname, - rinex_version)); -} - - -void gps_l1_ca_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) -{ - try { - if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS EPHEMERIS ### - std::shared_ptr gps_eph; - gps_eph = boost::any_cast>(pmt::any_ref(msg)); - DLOG(INFO) << "Ephemeris record has arrived from SAT ID " - << gps_eph->i_satellite_PRN << " (Block " - << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" - << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" - << gps_eph->i_GPS_week; - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS IONO ### - std::shared_ptr gps_iono; - gps_iono = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_iono = *gps_iono; - DLOG(INFO) << "New IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS UTC MODEL ### - std::shared_ptr gps_utc_model; - gps_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_utc_model = *gps_utc_model; - DLOG(INFO) << "New UTC record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### SBAS IONO ### - std::shared_ptr sbas_iono; - sbas_iono = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->sbas_iono = *sbas_iono; - DLOG(INFO) << "New SBAS IONO record has arrived "; - } - - //TODO: add SBAS correction maps here - //d_ls_pvt->sbas_sat_corr_map = global_sbas_sat_corr_map.get_map_copy(); - //d_ls_pvt->sbas_ephemeris_map = global_sbas_ephemeris_map.get_map_copy(); - - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - std::shared_ptr sbas_raw_msg_ptr; - sbas_raw_msg_ptr = boost::any_cast>(pmt::any_ref(msg)); - Sbas_Raw_Msg sbas_raw_msg = *sbas_raw_msg_ptr; - // read SBAS raw messages directly from queue and write them into rinex file - // create the header of not yet done - if(!b_rinex_sbs_header_written) - { - rp->rinex_sbs_header(rp->sbsFile); - b_rinex_sbs_header_written = true; - } - - // Define the RX time of the SBAS message by using the GPS time. - // It has only an effect if there has not been yet a SBAS MT12 available - // when the message was received. - if(sbas_raw_msg.get_rx_time_obj().is_related() == false - && gnss_observables_map.size() > 0 - && d_ls_pvt->gps_ephemeris_map.size() > 0) - { - // doesn't matter which channel/satellite we choose - Gnss_Synchro gs = gnss_observables_map.begin()->second; - Gps_Ephemeris eph = d_ls_pvt->gps_ephemeris_map.begin()->second; - - double relative_rx_time = gs.Tracking_timestamp_secs; - int gps_week = eph.i_GPS_week; - double gps_sec = gs.d_TOW_at_current_symbol; - - Sbas_Time_Relation time_rel(relative_rx_time, gps_week, gps_sec); - sbas_raw_msg.relate(time_rel); - } - - // send the message to the rinex logger if it has a valid GPS time stamp - if(sbas_raw_msg.get_rx_time_obj().is_related()) - { - rp->log_rinex_sbs(rp->sbsFile, sbas_raw_msg); - } - } - else - { - LOG(WARNING) << "msg_handler_telemetry unknown object type!"; - } - } - catch(boost::bad_any_cast& e) - { - LOG(WARNING) << "msg_handler_telemetry Bad any cast!\n"; - } -} - - -std::map gps_l1_ca_pvt_cc::get_GPS_L1_ephemeris_map() -{ - return d_ls_pvt->gps_ephemeris_map; -} - -bool gps_l1_ca_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) -{ - /* Fill Sys V message structures */ - int msgsend_size; - ttff_msgbuf msg; - msg.ttff = ttff.ttff; - msgsend_size = sizeof(msg.ttff); - msg.mtype = 1; /* default message ID */ - - /* SEND SOLUTION OVER A MESSAGE QUEUE */ - /* non-blocking Sys V message send */ - msgsnd(sysv_msqid, &msg, msgsend_size, IPC_NOWAIT); - return true; -} - -gps_l1_ca_pvt_cc::gps_l1_ca_pvt_cc(unsigned int nchannels, - bool dump, std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname, - int rinex_version) : - gr::block("gps_l1_ca_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(0, 0, sizeof(gr_complex)) ) -{ - d_output_rate_ms = output_rate_ms; - d_display_rate_ms = display_rate_ms; - d_dump = dump; - d_nchannels = nchannels; - d_dump_filename = dump_filename; - std::string dump_ls_pvt_filename = dump_filename; - - // GPS Ephemeris data message port in - this->message_port_register_in(pmt::mp("telemetry")); - this->set_msg_handler(pmt::mp("telemetry"), - boost::bind(&gps_l1_ca_pvt_cc::msg_handler_telemetry, this, _1)); - - // initialize kml_printer - std::string kml_dump_filename; - kml_dump_filename = d_dump_filename; - d_kml_printer = std::make_shared(); - d_kml_printer->set_headers(kml_dump_filename); - - // initialize geojson_printer - std::string geojson_dump_filename; - geojson_dump_filename = d_dump_filename; - d_geojson_printer = std::make_shared(); - d_geojson_printer->set_headers(geojson_dump_filename); - - // initialize nmea_printer - d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); - - // initialize rtcm_printer - std::string rtcm_dump_filename; - rtcm_dump_filename = d_dump_filename; - d_rtcm_tcp_port = rtcm_tcp_port; - d_rtcm_station_id = rtcm_station_id; - d_rtcm_printer = std::make_shared(rtcm_dump_filename, flag_rtcm_server, flag_rtcm_tty_port, d_rtcm_tcp_port, d_rtcm_station_id, rtcm_dump_devname); - if(rtcm_msg_rate_ms.find(1019) != rtcm_msg_rate_ms.end()) - { - d_rtcm_MT1019_rate_ms = rtcm_msg_rate_ms[1019]; - } - else - { - d_rtcm_MT1019_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set - } - if(rtcm_msg_rate_ms.find(1071) != rtcm_msg_rate_ms.end()) // whatever between 1071 and 1077 - { - d_rtcm_MSM_rate_ms = rtcm_msg_rate_ms[1071]; - } - else - { - d_rtcm_MSM_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set - } - b_rtcm_writing_started = false; - - d_dump_filename.append("_raw.dat"); - dump_ls_pvt_filename.append("_ls_pvt.dat"); - d_averaging_depth = averaging_depth; - d_flag_averaging = flag_averaging; - - d_ls_pvt = std::make_shared((int)nchannels, dump_ls_pvt_filename, d_dump); - d_ls_pvt->set_averaging_depth(d_averaging_depth); - - d_sample_counter = 0; - d_last_sample_nav_output = 0; - d_rx_time = 0.0; - - d_last_status_print_seg = 0; - - b_rinex_header_written = false; - b_rinex_header_updated = false; - b_rinex_sbs_header_written = false; - rp = std::make_shared(rinex_version); - - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "PVT dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(INFO) << "Exception opening PVT dump file " << e.what(); - } - } - } - - // Create Sys V message queue - first_fix = true; - sysv_msg_key = 1101; - int msgflg = IPC_CREAT | 0666; - if ((sysv_msqid = msgget(sysv_msg_key, msgflg )) == -1) - { - std::cout << "GNSS-SDR can not create message queues!" << std::endl; - throw new std::exception(); - } -} - - -gps_l1_ca_pvt_cc::~gps_l1_ca_pvt_cc() -{ - msgctl(sysv_msqid, IPC_RMID, NULL); -} - - -void gps_l1_ca_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchronization_data) -{ - // Print the current receiver status using std::cout every second - int current_rx_seg = floor(channels_synchronization_data[0][0].Tracking_timestamp_secs); - if ( current_rx_seg!= d_last_status_print_seg) - { - d_last_status_print_seg = current_rx_seg; - std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl << std::flush; - //DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) - // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]"; - } -} - - -int gps_l1_ca_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) -{ - gnss_observables_map.clear(); - d_sample_counter++; - Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer - - print_receiver_status(in); - - // ############ 1. READ PSEUDORANGES #### - for (unsigned int i = 0; i < d_nchannels; i++) - { - if (in[i][0].Flag_valid_pseudorange == true) - { - gnss_observables_map.insert(std::pair(in[i][0].PRN, in[i][0])); // store valid pseudoranges in a map - d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges) - if(d_ls_pvt->gps_ephemeris_map.size() > 0) - { - std::map::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN); - if(tmp_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time - } - } - } - } - - // ############ 2 COMPUTE THE PVT ################################ - if (gnss_observables_map.size() > 0 and d_ls_pvt->gps_ephemeris_map.size() > 0) - { - // compute on the fly PVT solution - if ((d_sample_counter % d_output_rate_ms) == 0) - { - bool pvt_result; - pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); - if (pvt_result == true) - { - // correct the observable to account for the receiver clock offset - for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) - { - it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s; - } - if(first_fix == true) - { - std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; - ttff_msgbuf ttff; - ttff.mtype = 1; - ttff.ttff = d_sample_counter; - send_sys_v_ttff_msg(ttff); - first_fix = false; - } - d_kml_printer->print_position(d_ls_pvt, d_flag_averaging); - d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging); - d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); - - if (!b_rinex_header_written) - { - std::map::iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time); - rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); - b_rinex_header_written = true; // do not write header anymore - } - } - if(b_rinex_header_written) - { - // Limit the RINEX navigation output rate to 1/6 seg - // Notice that d_sample_counter period is 1ms (for GPS correlators) - if ((d_sample_counter - d_last_sample_nav_output) >= 6000) - { - rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map); - d_last_sample_nav_output = d_sample_counter; - } - std::map::iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); - } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) - { - rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); - rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); - b_rinex_header_updated = true; - } - } - if(b_rtcm_writing_started) - { - if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0) - { - for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if((d_sample_counter % d_rtcm_MSM_rate_ms) == 0) - { - std::map::iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } - } - - if(!b_rtcm_writing_started) // the first time - { - for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - - std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - } - } - - // DEBUG MESSAGE: Display position in console output - if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true) - { - std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; - - LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]"; - - LOG(INFO) << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " - << d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP << " GDOP = " << d_ls_pvt->d_GDOP; - } - // MULTIPLEXED FILE RECORDING - Record results to file - if(d_dump == true) - { - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels ; i++) - { - tmp_double = in[i][0].Pseudorange_m; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = 0; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - d_dump_file.write((char*)&d_rx_time, sizeof(double)); - } - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } - } - } - - consume_each(1); //one by one - return 1; -} diff --git a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h deleted file mode 100644 index 07f9d8607..000000000 --- a/src/algorithms/PVT/gnuradio_blocks/gps_l1_ca_pvt_cc.h +++ /dev/null @@ -1,170 +0,0 @@ -/*! - * \file gps_l1_ca_pvt_cc.h - * \brief Interface of a Position Velocity and Time computation block for GPS L1 C/A - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_GPS_L1_CA_PVT_CC_H -#define GNSS_SDR_GPS_L1_CA_PVT_CC_H - -#include -#include -#include -#include -#include -#include -#include "nmea_printer.h" -#include "kml_printer.h" -#include "rinex_printer.h" -#include "geojson_printer.h" -#include "rtcm_printer.h" -#include "gps_l1_ca_ls_pvt.h" - - -class gps_l1_ca_pvt_cc; - -typedef boost::shared_ptr gps_l1_ca_pvt_cc_sptr; - -gps_l1_ca_pvt_cc_sptr gps_l1_ca_make_pvt_cc(unsigned int n_channels, - bool dump, - std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname, - int rinex_version -); - -/*! - * \brief This class implements a block that computes the PVT solution - */ -class gps_l1_ca_pvt_cc : public gr::block -{ -private: - friend gps_l1_ca_pvt_cc_sptr gps_l1_ca_make_pvt_cc(unsigned int nchannels, - bool dump, - std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname, - int rinex_version); - gps_l1_ca_pvt_cc(unsigned int nchannels, - bool dump, - std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname, - int rinex_version); - - void msg_handler_telemetry(pmt::pmt_t msg); - - bool d_dump; - bool b_rinex_header_written; - bool b_rinex_sbs_header_written; - bool b_rinex_header_updated; - bool b_rtcm_writing_started; - unsigned short d_rtcm_tcp_port; - unsigned short d_rtcm_station_id; - int d_rtcm_MT1019_rate_ms; - int d_rtcm_MSM_rate_ms; - - void print_receiver_status(Gnss_Synchro** channels_synchronization_data); - int d_last_status_print_seg; //for status printer - - unsigned int d_nchannels; - std::string d_dump_filename; - std::ofstream d_dump_file; - int d_averaging_depth; - bool d_flag_averaging; - int d_output_rate_ms; - int d_display_rate_ms; - long unsigned int d_sample_counter; - long unsigned int d_last_sample_nav_output; - - std::shared_ptr rp; - std::shared_ptr d_kml_printer; - std::shared_ptr d_nmea_printer; - std::shared_ptr d_geojson_printer; - std::shared_ptr d_rtcm_printer; - double d_rx_time; - std::shared_ptr d_ls_pvt; - - std::map gnss_observables_map; - - bool first_fix; - key_t sysv_msg_key; - int sysv_msqid; - typedef struct { - long mtype;//required by sys v message - double ttff; - } ttff_msgbuf; - bool send_sys_v_ttff_msg(ttff_msgbuf ttff); - -public: - - /*! - * \brief Get latest set of GPS L1 ephemeris from PVT block - * - * It is used to save the assistance data at the receiver shutdown - */ - std::map get_GPS_L1_ephemeris_map(); - - ~gps_l1_ca_pvt_cc (); //!< Default destructor - - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing -}; - -#endif diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc deleted file mode 100644 index 04d8ab6d4..000000000 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.cc +++ /dev/null @@ -1,1106 +0,0 @@ -/*! - * \file hybrid_pvt_cc.cc - * \brief Implementation of a Position Velocity and Time computation block for GPS L1 C/A - * \author Javier Arribas, 2013. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "hybrid_pvt_cc.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "concurrent_map.h" - -using google::LogMessage; - -hybrid_pvt_cc_sptr -hybrid_make_pvt_cc(unsigned int nchannels, - bool dump, - std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname, - const unsigned int type_of_receiver) -{ - return hybrid_pvt_cc_sptr(new hybrid_pvt_cc(nchannels, - dump, - dump_filename, - averaging_depth, - flag_averaging, - output_rate_ms, - display_rate_ms, - flag_nmea_tty_port, - nmea_dump_filename, - nmea_dump_devname, - flag_rtcm_server, - flag_rtcm_tty_port, - rtcm_tcp_port, - rtcm_station_id, - rtcm_msg_rate_ms, - rtcm_dump_devname, - type_of_receiver)); -} - - -void hybrid_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) -{ - try { - if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS EPHEMERIS ### - std::shared_ptr gps_eph; - gps_eph = boost::any_cast>(pmt::any_ref(msg)); - DLOG(INFO) << "Ephemeris record has arrived from SAT ID " - << gps_eph->i_satellite_PRN << " (Block " - << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" - << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" - << gps_eph->i_GPS_week; - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS IONO ### - std::shared_ptr gps_iono; - gps_iono = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_iono = *gps_iono; - DLOG(INFO) << "New IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS UTC MODEL ### - std::shared_ptr gps_utc_model; - gps_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_utc_model = *gps_utc_model; - DLOG(INFO) << "New UTC record has arrived "; - } - - if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo EPHEMERIS ### - std::shared_ptr galileo_eph; - galileo_eph = boost::any_cast>(pmt::any_ref(msg)); - // insert new ephemeris record - DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5 - << ", GALILEO Week Number =" << galileo_eph->WN_5 - << " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris; - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo IONO ### - std::shared_ptr galileo_iono; - galileo_iono = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->galileo_iono = *galileo_iono; - DLOG(INFO) << "New IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo UTC MODEL ### - std::shared_ptr galileo_utc_model; - galileo_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->galileo_utc_model = *galileo_utc_model; - DLOG(INFO) << "New UTC record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### Galileo Almanac ### - std::shared_ptr galileo_almanac; - galileo_almanac = boost::any_cast>(pmt::any_ref(msg)); - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->galileo_almanac = *galileo_almanac; - DLOG(INFO) << "New Galileo Almanac has arrived "; - } - - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS CNAV message ### - std::shared_ptr gps_cnav_ephemeris; - gps_cnav_ephemeris = boost::any_cast>(pmt::any_ref(msg)); - // update/insert new ephemeris record to the global ephemeris map - d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; - LOG(INFO) << "New GPS CNAV ephemeris record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS CNAV IONO ### - std::shared_ptr gps_cnav_iono; - gps_cnav_iono = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_cnav_iono = *gps_cnav_iono; - DLOG(INFO) << "New CNAV IONO record has arrived "; - } - else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) - { - // ### GPS CNAV UTC MODEL ### - std::shared_ptr gps_cnav_utc_model; - gps_cnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); - d_ls_pvt->gps_cnav_utc_model = *gps_cnav_utc_model; - DLOG(INFO) << "New CNAV UTC record has arrived "; - } - else - { - LOG(WARNING) << "msg_handler_telemetry unknown object type!"; - } - - } - catch(boost::bad_any_cast& e) - { - LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; - } -} - - -std::map hybrid_pvt_cc::get_GPS_L1_ephemeris_map() -{ - return d_ls_pvt->gps_ephemeris_map; -} - - -hybrid_pvt_cc::hybrid_pvt_cc(unsigned int nchannels, bool dump, std::string dump_filename, - int averaging_depth, bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, - std::string nmea_dump_filename, std::string nmea_dump_devname, - bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname, const unsigned int type_of_receiver) : - gr::block("hybrid_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(0, 0, sizeof(gr_complex))) - -{ - d_output_rate_ms = output_rate_ms; - d_display_rate_ms = display_rate_ms; - d_dump = dump; - d_nchannels = nchannels; - d_dump_filename = dump_filename; - std::string dump_ls_pvt_filename = dump_filename; - type_of_rx = type_of_receiver; - - // GPS Ephemeris data message port in - this->message_port_register_in(pmt::mp("telemetry")); - this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&hybrid_pvt_cc::msg_handler_telemetry, this, _1)); - - //initialize kml_printer - std::string kml_dump_filename; - kml_dump_filename = d_dump_filename; - d_kml_dump = std::make_shared(); - d_kml_dump->set_headers(kml_dump_filename); - - //initialize geojson_printer - std::string geojson_dump_filename; - geojson_dump_filename = d_dump_filename; - d_geojson_printer = std::make_shared(); - d_geojson_printer->set_headers(geojson_dump_filename); - - //initialize nmea_printer - d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); - - //initialize rtcm_printer - std::string rtcm_dump_filename; - rtcm_dump_filename = d_dump_filename; - d_rtcm_printer = std::make_shared(rtcm_dump_filename, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_dump_devname); - if(rtcm_msg_rate_ms.find(1019) != rtcm_msg_rate_ms.end()) - { - d_rtcm_MT1019_rate_ms = rtcm_msg_rate_ms[1019]; - } - else - { - d_rtcm_MT1019_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set - } - if(rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end()) - { - d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045]; - } - else - { - d_rtcm_MT1045_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set - } - if(rtcm_msg_rate_ms.find(1077) != rtcm_msg_rate_ms.end()) // whatever between 1071 and 1077 - { - d_rtcm_MT1077_rate_ms = rtcm_msg_rate_ms[1077]; - } - else - { - d_rtcm_MT1077_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set - } - if(rtcm_msg_rate_ms.find(1097) != rtcm_msg_rate_ms.end()) // whatever between 1091 and 1097 - { - d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms[1097]; - d_rtcm_MSM_rate_ms = rtcm_msg_rate_ms[1097]; - } - else - { - d_rtcm_MT1097_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set - d_rtcm_MSM_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set - } - b_rtcm_writing_started = false; - - d_dump_filename.append("_raw.dat"); - dump_ls_pvt_filename.append("_ls_pvt.dat"); - d_averaging_depth = averaging_depth; - d_flag_averaging = flag_averaging; - - d_ls_pvt = std::make_shared((int)nchannels, dump_ls_pvt_filename, d_dump); - d_ls_pvt->set_averaging_depth(d_averaging_depth); - - d_sample_counter = 0; - d_last_sample_nav_output = 0; - d_rx_time = 0.0; - d_TOW_at_curr_symbol_constellation = 0.0; - b_rinex_header_written = false; - b_rinex_header_updated = false; - rp = std::make_shared(); - - d_last_status_print_seg = 0; - - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "PVT dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception opening PVT dump file " << e.what(); - } - } - } - - // Create Sys V message queue - first_fix = true; - sysv_msg_key = 1101; - int msgflg = IPC_CREAT | 0666; - if ((sysv_msqid = msgget(sysv_msg_key, msgflg )) == -1) - { - std::cout << "GNSS-SDR can not create message queues!" << std::endl; - throw new std::exception(); - } -} - - - -hybrid_pvt_cc::~hybrid_pvt_cc() -{ - msgctl(sysv_msqid, IPC_RMID, NULL); - - //save GPS L2CM ephemeris to XML file - std::string file_name="eph_GPS_L2CM.xml"; - - if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) - { - try - { - std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); - boost::archive::xml_oarchive xml(ofs); - xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_cnav_ephemeris_map); - ofs.close(); - LOG(INFO) << "Saved GPS L2CM Ephemeris map data"; - } - catch (std::exception& e) - { - LOG(WARNING) << e.what(); - } - } - else - { - LOG(WARNING) << "Failed to save GPS L2CM Ephemeris, map is empty"; - } - - //save GPS L1 CA ephemeris to XML file - file_name="eph_GPS_L1CA.xml"; - - if (d_ls_pvt->gps_ephemeris_map.size() > 0) - { - try - { - std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); - boost::archive::xml_oarchive xml(ofs); - xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_ephemeris_map); - ofs.close(); - LOG(INFO) << "Saved GPS L1 CA Ephemeris map data"; - } - catch (std::exception& e) - { - LOG(WARNING) << e.what(); - } - } - else - { - LOG(WARNING) << "Failed to save GPS L1 CA Ephemeris, map is empty"; - } - - //save Galileo E1 ephemeris to XML file - file_name="eph_Galileo_E1.xml"; - - if (d_ls_pvt->galileo_ephemeris_map.size() > 0) - { - try - { - std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); - boost::archive::xml_oarchive xml(ofs); - xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->galileo_ephemeris_map); - ofs.close(); - LOG(INFO) << "Saved Galileo E1 Ephemeris map data"; - } - catch (std::exception& e) - { - LOG(WARNING) << e.what(); - } - } - else - { - LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty"; - } - -} - - - -bool hybrid_pvt_cc::observables_pairCompare_min(const std::pair& a, const std::pair& b) -{ - return (a.second.Pseudorange_m) < (b.second.Pseudorange_m); -} - - -void hybrid_pvt_cc::print_receiver_status(Gnss_Synchro** channels_synchronization_data) -{ - // Print the current receiver status using std::cout every second - int current_rx_seg = floor(channels_synchronization_data[0][0].Tracking_timestamp_secs); - if ( current_rx_seg != d_last_status_print_seg) - { - d_last_status_print_seg = current_rx_seg; - std::cout << "Current input signal time = " << current_rx_seg << " [s]" << std::endl << std::flush; - //DLOG(INFO) << "GPS L1 C/A Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) - // << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]" << std::endl; - } -} - - -bool hybrid_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) -{ - /* Fill Sys V message structures */ - int msgsend_size; - ttff_msgbuf msg; - msg.ttff = ttff.ttff; - msgsend_size = sizeof(msg.ttff); - msg.mtype = 1; /* default message ID */ - - /* SEND SOLUTION OVER A MESSAGE QUEUE */ - /* non-blocking Sys V message send */ - msgsnd(sysv_msqid, &msg, msgsend_size, IPC_NOWAIT); - return true; -} - - -int hybrid_pvt_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) -{ - d_sample_counter++; - unsigned int gps_channel = 0; - unsigned int gal_channel = 0; - - gnss_observables_map.clear(); - - Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer - - print_receiver_status(in); - - // ############ 1. READ PSEUDORANGES #### - for (unsigned int i = 0; i < d_nchannels; i++) - { - if (in[i][0].Flag_valid_pseudorange == true) - { - gnss_observables_map.insert(std::pair(i, in[i][0])); // store valid observables in a map. - //d_rx_time = in[i][0].d_TOW_at_current_symbol; // all the channels have the same RX timestamp (common RX time pseudoranges) - d_TOW_at_curr_symbol_constellation = in[i][0].d_TOW_at_current_symbol; // d_TOW_at_current_symbol not corrected by delta t (just for debug) - d_rx_time = in[i][0].d_TOW_hybrid_at_current_symbol; // hybrid rx time, all the channels have the same RX timestamp (common RX time pseudoranges) - if(d_ls_pvt->gps_ephemeris_map.size() > 0) - { - std::map::iterator tmp_eph_iter = d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN); - if(tmp_eph_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time - } - } - if(d_ls_pvt->galileo_ephemeris_map.size() > 0) - { - std::map::iterator tmp_eph_iter = d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN); - if(tmp_eph_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time - } - } - if(d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) - { - std::map::iterator tmp_eph_iter = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][0].PRN); - if(tmp_eph_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) - { - d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][0].PRN)->second, d_rx_time, in[i][0]); // keep track of locking time - } - } - } - } - - std::map::iterator galileo_ephemeris_iter; - std::map::iterator gps_ephemeris_iter; - std::map::iterator gps_cnav_ephemeris_iter; - std::map::iterator gnss_observables_iter; - - /* - * TYPE | RECEIVER - * 0 | Unknown - * 1 | GPS L1 C/A - * 2 | GPS L2C - * 3 | GPS L5 - * 4 | Galileo E1B - * 5 | Galileo E5a - * 6 | Galileo E5b - * 7 | GPS L1 C/A + GPS L2C - * 8 | GPS L1 C/A + GPS L5 - * 9 | GPS L1 C/A + Galileo E1B - * 10 | GPS L1 C/A + Galileo E5a - * 11 | GPS L1 C/A + Galileo E5b - * 12 | Galileo E1B + GPS L2C - * 13 | Galileo E1B + GPS L5 - * 14 | Galileo E1B + Galileo E5a - * 15 | Galileo E1B + Galileo E5b - * 16 | GPS L2C + GPS L5 - * 17 | GPS L2C + Galileo E5a - * 18 | GPS L2C + Galileo E5b - * 19 | GPS L5 + Galileo E5a - * 20 | GPS L5 + Galileo E5b - * 21 | GPS L1 C/A + Galileo E1B + GPS L2C - * 22 | GPS L1 C/A + Galileo E1B + GPS L5 - */ - - // ############ 2 COMPUTE THE PVT ################################ - if (gnss_observables_map.size() > 0) - { - // compute on the fly PVT solution - if ((d_sample_counter % d_output_rate_ms) == 0) - { - bool pvt_result; - //std::cout<<"obs map size at pvt="<get_PVT(gnss_observables_map, d_rx_time, d_flag_averaging); - - if (pvt_result == true) - { - // correct the observable to account for the receiver clock offset - for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) - { - it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s; - } - if(first_fix == true) - { - std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; - ttff_msgbuf ttff; - ttff.mtype = 1; - ttff.ttff = d_sample_counter; - send_sys_v_ttff_msg(ttff); - first_fix = false; - } - d_kml_dump->print_position(d_ls_pvt, d_flag_averaging); - d_geojson_printer->print_position(d_ls_pvt, d_flag_averaging); - d_nmea_printer->Print_Nmea_Line(d_ls_pvt, d_flag_averaging); - - // ####################### RINEX FILES ################# - if (!b_rinex_header_written) // & we have utc data in nav message! - { - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); - - if(type_of_rx == 1) // GPS L1 C/A only - { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time); - rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 2) // GPS L2C only - { - if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) - { - rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time); - rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 4) // Galileo E1B only - { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time); - rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 5) // Galileo E5a only - { - std::string signal("5X"); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); - rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 6) // Galileo E5b only - { - std::string signal("7X"); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); - rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) - { - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time); - rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); - b_rinex_header_written = true; // do not write header anymore - } - } - - if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B - { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) - { - std::string gal_signal("1B"); - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 10) // GPS L1 C/A + Galileo E5a - { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) - { - std::string gal_signal("5X"); - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 11) // GPS L1 C/A + Galileo E5b - { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) - { - std::string gal_signal("7X"); - rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 14) // Galileo E1B + Galileo E5a - { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) - { - std::string gal_signal("1B 5X"); - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - if(type_of_rx == 15) // Galileo E1B + Galileo E5b - { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) - { - std::string gal_signal("1B 7X"); - rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); - rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_written = true; // do not write header anymore - } - } - } - if(b_rinex_header_written) // The header is already written, we can now log the navigation message data - { - // Limit the RINEX navigation output rate - // Notice that d_sample_counter period is 4ms (for Galileo correlators) - if ((d_sample_counter - d_last_sample_nav_output) >= 6000) - { - if(type_of_rx == 1) // GPS L1 C/A only - { - rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map); - } - if(type_of_rx == 2) // GPS L2C only - { - rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); - } - if( (type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) ) // Galileo - { - rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); - } - if(type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); - } - if((type_of_rx == 9) || (type_of_rx == 10) || (type_of_rx == 11)) // GPS L1 C/A + Galileo - { - rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); - } - if((type_of_rx == 14) || (type_of_rx == 15)) // Galileo E1B + Galileo E5a - { - rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); - } - - d_last_sample_nav_output = d_sample_counter; - } - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); - - // Log observables into the RINEX file - if(type_of_rx == 1) // GPS L1 C/A only - { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); - } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) - { - rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); - rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); - b_rinex_header_updated = true; - } - } - if(type_of_rx == 2) // GPS L2C only - { - if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); - } - if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) - { - rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); - rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); - b_rinex_header_updated = true; - } - } - if(type_of_rx == 4) // Galileo E1B only - { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B"); - } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) - { - rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); - b_rinex_header_updated = true; - } - } - if(type_of_rx == 5) // Galileo E5a only - { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "5X"); - } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) - { - rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); - b_rinex_header_updated = true; - } - } - if(type_of_rx == 6) // Galileo E5b only - { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "7X"); - } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) - { - rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); - b_rinex_header_updated = true; - } - } - if(type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - if( (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) - { - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); - } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) - { - rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); - rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); - b_rinex_header_updated = true; - } - } - if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B - { - if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) - { - rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); - } - if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) - { - rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); - rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - b_rinex_header_updated = true; - } - } - if(type_of_rx == 14) // Galileo E1B + Galileo E5a - { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 5X"); - } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) - { - rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); - b_rinex_header_updated = true; - } - } - if(type_of_rx == 15) // Galileo E1B + Galileo E5b - { - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 7X"); - } - if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) - { - rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); - rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); - b_rinex_header_updated = true; - } - } - } - - // ####################### RTCM MESSAGES ################# - if(b_rtcm_writing_started) - { - if(type_of_rx == 1) // GPS L1 C/A - { - if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0) - { - for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if((d_sample_counter % d_rtcm_MSM_rate_ms) == 0) - { - std::map::iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } - } - if((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo - { - if((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4) ) == 0) - { - for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); - } - } - if((d_sample_counter % (d_rtcm_MSM_rate_ms / 4) ) == 0) - { - std::map::iterator gal_ephemeris_iter; - gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } - } - if(type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - if((d_sample_counter % d_rtcm_MT1019_rate_ms) == 0) - { - for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if((d_sample_counter % d_rtcm_MSM_rate_ms) == 0) - { - std::map::iterator gps_ephemeris_iter; - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - std::map::iterator gps_cnav_ephemeris_iter; - gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } - } - if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B - { - if(((d_sample_counter % (d_rtcm_MT1019_rate_ms / 4)) == 0) && (d_rtcm_MT1019_rate_ms != 0)) - { - for(gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if(((d_sample_counter % (d_rtcm_MT1045_rate_ms / 4)) == 0) && (d_rtcm_MT1045_rate_ms != 0)) - { - for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); - } - } - if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) || ((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0)) - { - //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); - //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); - unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) - { - std::string system(&gnss_observables_iter->second.System, 1); - if(gps_channel == 0) - { - if(system.compare("G") == 0) - { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - gps_channel = i; - } - } - } - if(gal_channel == 0) - { - if(system.compare("E") == 0) - { - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - gal_channel = i; - } - } - } - i++; - } - if(((d_sample_counter % (d_rtcm_MT1097_rate_ms / 4) ) == 0) && (d_rtcm_MT1097_rate_ms != 0) ) - { - - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } - if(((d_sample_counter % (d_rtcm_MT1077_rate_ms / 4) ) == 0) && (d_rtcm_MT1077_rate_ms != 0) ) - { - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - } - } - } - } - - if(!b_rtcm_writing_started) // the first time - { - if(type_of_rx == 1) // GPS L1 C/A - { - for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - - std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - - if((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo - { - for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); - } - - std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - - if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if(type_of_rx == 7) // GPS L1 C/A + GPS L2C - { - for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - - std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); - std::map::iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); - - if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B - { - if(d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 - { - for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); - } - } - if(d_rtcm_MT1045_rate_ms != 0) - { - for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) - { - d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); - } - } - - unsigned int i = 0; - for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) - { - std::string system(&gnss_observables_iter->second.System, 1); - if(gps_channel == 0) - { - if(system.compare("G") == 0) - { - // This is a channel with valid GPS signal - gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) - { - gps_channel = i; - } - } - } - if(gal_channel == 0) - { - if(system.compare("E") == 0) - { - galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) - { - gal_channel = i; - } - } - } - i++; - } - - if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) - { - d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - - if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0) ) - { - d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); - } - b_rtcm_writing_started = true; - } - } - } - } - - // DEBUG MESSAGE: Display position in console output - if (((d_sample_counter % d_display_rate_ms) == 0) and d_ls_pvt->b_valid_position == true) - { - std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; - - LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d - << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]"; - - /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) - << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " - << d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP - << " GDOP = " << d_ls_pvt->d_GDOP << std::endl; */ - } - - // MULTIPLEXED FILE RECORDING - Record results to file - if(d_dump == true) - { - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) - { - tmp_double = in[i][0].Pseudorange_m; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = 0; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - d_dump_file.write((char*)&d_rx_time, sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } - } - } - - consume_each(1); //one by one - return 1; -} diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc new file mode 100644 index 000000000..3bd5e23db --- /dev/null +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -0,0 +1,1163 @@ +/*! + * \file rtklib_pvt_cc.cc + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "rtklib_pvt_cc.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using google::LogMessage; + + +rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels, + bool dump, + std::string dump_filename, + int output_rate_ms, + int display_rate_ms, + bool flag_nmea_tty_port, + std::string nmea_dump_filename, + std::string nmea_dump_devname, + int rinex_version, + bool flag_rtcm_server, + bool flag_rtcm_tty_port, + unsigned short rtcm_tcp_port, + unsigned short rtcm_station_id, + std::map rtcm_msg_rate_ms, + std::string rtcm_dump_devname, + const unsigned int type_of_receiver, + rtk_t & rtk) +{ + return rtklib_pvt_cc_sptr(new rtklib_pvt_cc(nchannels, + dump, + dump_filename, + output_rate_ms, + display_rate_ms, + flag_nmea_tty_port, + nmea_dump_filename, + nmea_dump_devname, + rinex_version, + flag_rtcm_server, + flag_rtcm_tty_port, + rtcm_tcp_port, + rtcm_station_id, + rtcm_msg_rate_ms, + rtcm_dump_devname, + type_of_receiver, + rtk)); +} + + +void rtklib_pvt_cc::msg_handler_telemetry(pmt::pmt_t msg) +{ + try + { + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS EPHEMERIS ### + std::shared_ptr gps_eph; + gps_eph = boost::any_cast>(pmt::any_ref(msg)); + DLOG(INFO) << "Ephemeris record has arrived from SAT ID " + << gps_eph->i_satellite_PRN << " (Block " + << gps_eph->satelliteBlock[gps_eph->i_satellite_PRN] << ")" + << "inserted with Toe="<< gps_eph->d_Toe<<" and GPS Week=" + << gps_eph->i_GPS_week; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->gps_ephemeris_map[gps_eph->i_satellite_PRN] = *gps_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS IONO ### + std::shared_ptr gps_iono; + gps_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_iono = *gps_iono; + DLOG(INFO) << "New IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS UTC MODEL ### + std::shared_ptr gps_utc_model; + gps_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_utc_model = *gps_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + + if( pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo EPHEMERIS ### + std::shared_ptr galileo_eph; + galileo_eph = boost::any_cast>(pmt::any_ref(msg)); + // insert new ephemeris record + DLOG(INFO) << "Galileo New Ephemeris record inserted in global map with TOW =" << galileo_eph->TOW_5 + << ", GALILEO Week Number =" << galileo_eph->WN_5 + << " and Ephemeris IOD = " << galileo_eph->IOD_ephemeris; + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->galileo_ephemeris_map[galileo_eph->i_satellite_PRN] = *galileo_eph; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo IONO ### + std::shared_ptr galileo_iono; + galileo_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->galileo_iono = *galileo_iono; + DLOG(INFO) << "New IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo UTC MODEL ### + std::shared_ptr galileo_utc_model; + galileo_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->galileo_utc_model = *galileo_utc_model; + DLOG(INFO) << "New UTC record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### Galileo Almanac ### + std::shared_ptr galileo_almanac; + galileo_almanac = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->galileo_almanac = *galileo_almanac; + DLOG(INFO) << "New Galileo Almanac has arrived "; + } + + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV message ### + std::shared_ptr gps_cnav_ephemeris; + gps_cnav_ephemeris = boost::any_cast>(pmt::any_ref(msg)); + // update/insert new ephemeris record to the global ephemeris map + d_ls_pvt->gps_cnav_ephemeris_map[gps_cnav_ephemeris->i_satellite_PRN] = *gps_cnav_ephemeris; + LOG(INFO) << "New GPS CNAV ephemeris record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV IONO ### + std::shared_ptr gps_cnav_iono; + gps_cnav_iono = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_cnav_iono = *gps_cnav_iono; + DLOG(INFO) << "New CNAV IONO record has arrived "; + } + else if (pmt::any_ref(msg).type() == typeid(std::shared_ptr) ) + { + // ### GPS CNAV UTC MODEL ### + std::shared_ptr gps_cnav_utc_model; + gps_cnav_utc_model = boost::any_cast>(pmt::any_ref(msg)); + d_ls_pvt->gps_cnav_utc_model = *gps_cnav_utc_model; + DLOG(INFO) << "New CNAV UTC record has arrived "; + } + else + { + LOG(WARNING) << "msg_handler_telemetry unknown object type!"; + } + + } + catch(boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + } +} + + +std::map rtklib_pvt_cc::get_GPS_L1_ephemeris_map() +{ + return d_ls_pvt->gps_ephemeris_map; +} + + +rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump_filename, + int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, + std::string nmea_dump_filename, std::string nmea_dump_devname, int rinex_version, + bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port, + unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname, const unsigned int type_of_receiver, rtk_t & rtk) : + gr::sync_block("rtklib_pvt_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), + gr::io_signature::make(0, 0, 0)) +{ + d_output_rate_ms = output_rate_ms; + d_display_rate_ms = display_rate_ms; + d_dump = dump; + d_nchannels = nchannels; + d_dump_filename = dump_filename; + std::string dump_ls_pvt_filename = dump_filename; + type_of_rx = type_of_receiver; + + // GPS Ephemeris data message port in + this->message_port_register_in(pmt::mp("telemetry")); + this->set_msg_handler(pmt::mp("telemetry"), boost::bind(&rtklib_pvt_cc::msg_handler_telemetry, this, _1)); + + //initialize kml_printer + std::string kml_dump_filename; + kml_dump_filename = d_dump_filename; + d_kml_dump = std::make_shared(); + d_kml_dump->set_headers(kml_dump_filename); + + //initialize geojson_printer + std::string geojson_dump_filename; + geojson_dump_filename = d_dump_filename; + d_geojson_printer = std::make_shared(); + d_geojson_printer->set_headers(geojson_dump_filename); + + //initialize nmea_printer + d_nmea_printer = std::make_shared(nmea_dump_filename, flag_nmea_tty_port, nmea_dump_devname); + + //initialize rtcm_printer + std::string rtcm_dump_filename; + rtcm_dump_filename = d_dump_filename; + d_rtcm_printer = std::make_shared(rtcm_dump_filename, flag_rtcm_server, flag_rtcm_tty_port, rtcm_tcp_port, rtcm_station_id, rtcm_dump_devname); + if(rtcm_msg_rate_ms.find(1019) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1019_rate_ms = rtcm_msg_rate_ms[1019]; + } + else + { + d_rtcm_MT1019_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set + } + if(rtcm_msg_rate_ms.find(1045) != rtcm_msg_rate_ms.end()) + { + d_rtcm_MT1045_rate_ms = rtcm_msg_rate_ms[1045]; + } + else + { + d_rtcm_MT1045_rate_ms = boost::math::lcm(5000, d_output_rate_ms); // default value if not set + } + if(rtcm_msg_rate_ms.find(1077) != rtcm_msg_rate_ms.end()) // whatever between 1071 and 1077 + { + d_rtcm_MT1077_rate_ms = rtcm_msg_rate_ms[1077]; + } + else + { + d_rtcm_MT1077_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set + } + if(rtcm_msg_rate_ms.find(1097) != rtcm_msg_rate_ms.end()) // whatever between 1091 and 1097 + { + d_rtcm_MT1097_rate_ms = rtcm_msg_rate_ms[1097]; + d_rtcm_MSM_rate_ms = rtcm_msg_rate_ms[1097]; + } + else + { + d_rtcm_MT1097_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set + d_rtcm_MSM_rate_ms = boost::math::lcm(1000, d_output_rate_ms); // default value if not set + } + b_rtcm_writing_started = false; + + d_dump_filename.append("_raw.dat"); + dump_ls_pvt_filename.append("_ls_pvt.dat"); + + d_ls_pvt = std::make_shared((int)nchannels, dump_ls_pvt_filename, d_dump, rtk); + d_ls_pvt->set_averaging_depth(1); + + d_rx_time = 0.0; + last_pvt_display_T_rx_s = 0.0; + last_RTCM_1019_output_time = 0.0; + last_RTCM_1045_output_time = 0.0; + last_RTCM_1077_output_time = 0.0; + last_RTCM_1097_output_time = 0.0; + last_RTCM_MSM_output_time = 0.0; + last_RINEX_obs_output_time = 0.0; + last_RINEX_nav_output_time = 0.0; + + b_rinex_header_written = false; + b_rinex_header_updated = false; + rp = std::make_shared(rinex_version); + + d_last_status_print_seg = 0; + + // ############# ENABLE DATA FILE LOG ################# + if (d_dump == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "PVT dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception opening PVT dump file " << e.what(); + } + } + } + + // Create Sys V message queue + first_fix = true; + sysv_msg_key = 1101; + int msgflg = IPC_CREAT | 0666; + if ((sysv_msqid = msgget(sysv_msg_key, msgflg )) == -1) + { + std::cout << "GNSS-SDR can not create message queues!" << std::endl; + throw new std::exception(); + } + gettimeofday(&tv, NULL); + begin = tv.tv_sec * 1000000 + tv.tv_usec; +} + + +rtklib_pvt_cc::~rtklib_pvt_cc() +{ + msgctl(sysv_msqid, IPC_RMID, NULL); + + //save GPS L2CM ephemeris to XML file + std::string file_name="eph_GPS_L2CM.xml"; + + if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_cnav_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved GPS L2CM Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save GPS L2CM Ephemeris, map is empty"; + } + + //save GPS L1 CA ephemeris to XML file + file_name = "eph_GPS_L1CA.xml"; + + if (d_ls_pvt->gps_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->gps_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved GPS L1 CA Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save GPS L1 CA Ephemeris, map is empty"; + } + + //save Galileo E1 ephemeris to XML file + file_name = "eph_Galileo_E1.xml"; + + if (d_ls_pvt->galileo_ephemeris_map.size() > 0) + { + try + { + std::ofstream ofs(file_name.c_str(), std::ofstream::trunc | std::ofstream::out); + boost::archive::xml_oarchive xml(ofs); + xml << boost::serialization::make_nvp("GNSS-SDR_ephemeris_map", d_ls_pvt->galileo_ephemeris_map); + ofs.close(); + LOG(INFO) << "Saved Galileo E1 Ephemeris map data"; + } + catch (std::exception& e) + { + LOG(WARNING) << e.what(); + } + } + else + { + LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty"; + } + +} + + +bool rtklib_pvt_cc::observables_pairCompare_min(const std::pair& a, const std::pair& b) +{ + return (a.second.Pseudorange_m) < (b.second.Pseudorange_m); +} + + +bool rtklib_pvt_cc::send_sys_v_ttff_msg(ttff_msgbuf ttff) +{ + /* Fill Sys V message structures */ + int msgsend_size; + ttff_msgbuf msg; + msg.ttff = ttff.ttff; + msgsend_size = sizeof(msg.ttff); + msg.mtype = 1; /* default message ID */ + + /* SEND SOLUTION OVER A MESSAGE QUEUE */ + /* non-blocking Sys V message send */ + msgsnd(sysv_msqid, &msg, msgsend_size, IPC_NOWAIT); + return true; +} + + +int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items __attribute__((unused))) +{ + for(int epoch = 0; epoch < noutput_items; epoch++) + { + bool flag_display_pvt = false; + bool flag_compute_pvt_output = false; + bool flag_write_RTCM_1019_output = false; + bool flag_write_RTCM_1045_output = false; + bool flag_write_RTCM_1077_output = false; + bool flag_write_RTCM_1097_output = false; + bool flag_write_RTCM_MSM_output = false; + bool flag_write_RINEX_obs_output = false; + bool flag_write_RINEX_nav_output = false; + unsigned int gps_channel = 0; + unsigned int gal_channel = 0; + + gnss_observables_map.clear(); + Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; //Get the input pointer + + // ############ 1. READ PSEUDORANGES #### + for (unsigned int i = 0; i < d_nchannels; i++) + { + if (in[i][epoch].Flag_valid_pseudorange == true) + { + std::map::iterator tmp_eph_iter_gps = d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN); + std::map::iterator tmp_eph_iter_gal = d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN); + std::map::iterator tmp_eph_iter_cnav = d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN); + if(((tmp_eph_iter_gps->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1C") == 0)) + || ((tmp_eph_iter_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("1B") == 0)) + || ((tmp_eph_iter_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 0))) + { + // store valid observables in a map. + gnss_observables_map.insert(std::pair(i, in[i][epoch])); + } + if(d_ls_pvt->gps_ephemeris_map.size() > 0) + { + if(tmp_eph_iter_gps != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if(d_ls_pvt->galileo_ephemeris_map.size() > 0) + { + if(tmp_eph_iter_gal != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->galileo_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + if(d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + if(tmp_eph_iter_cnav != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + d_rtcm_printer->lock_time(d_ls_pvt->gps_cnav_ephemeris_map.find(in[i][epoch].PRN)->second, in[i][epoch].RX_time, in[i][epoch]); // keep track of locking time + } + } + } + } + + // ############ 2 COMPUTE THE PVT ################################ + if (gnss_observables_map.size() > 0) + { + double current_RX_time = gnss_observables_map.begin()->second.RX_time; + + if (std::fabs(current_RX_time - d_rx_time) * 1000.0 >= static_cast(d_output_rate_ms)) + { + flag_compute_pvt_output = true; + d_rx_time = current_RX_time; + } + + // compute on the fly PVT solution + if (flag_compute_pvt_output == true) + { + bool pvt_result; + pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false); + + if (pvt_result == true) + { + if (std::fabs(current_RX_time - last_pvt_display_T_rx_s) * 1000.0 >= static_cast(d_display_rate_ms)) + { + flag_display_pvt = true; + last_pvt_display_T_rx_s = current_RX_time; + } + if ((std::fabs(current_RX_time - last_RTCM_1019_output_time) * 1000.0 >= static_cast(d_rtcm_MT1019_rate_ms)) && (d_rtcm_MT1019_rate_ms != 0) ) // allows deactivating messages by setting rate = 0 + { + flag_write_RTCM_1019_output = true; + last_RTCM_1019_output_time = current_RX_time; + } + + if ((std::fabs(current_RX_time - last_RTCM_1045_output_time) * 1000.0 >= static_cast(d_rtcm_MT1045_rate_ms)) && (d_rtcm_MT1045_rate_ms != 0) ) + { + flag_write_RTCM_1045_output = true; + last_RTCM_1045_output_time = current_RX_time; + } + + if ((std::fabs(current_RX_time - last_RTCM_1077_output_time) * 1000.0 >= static_cast(d_rtcm_MT1077_rate_ms)) && (d_rtcm_MT1077_rate_ms != 0) ) + { + flag_write_RTCM_1077_output = true; + last_RTCM_1077_output_time = current_RX_time; + } + + if ((std::fabs(current_RX_time - last_RTCM_1097_output_time) * 1000.0 >= static_cast(d_rtcm_MT1097_rate_ms)) && (d_rtcm_MT1097_rate_ms != 0) ) + { + flag_write_RTCM_1097_output = true; + last_RTCM_1097_output_time = current_RX_time; + } + + if ((std::fabs(current_RX_time - last_RTCM_MSM_output_time) * 1000.0 >= static_cast(d_rtcm_MSM_rate_ms)) && (d_rtcm_MSM_rate_ms != 0) ) + { + flag_write_RTCM_MSM_output = true; + last_RTCM_MSM_output_time = current_RX_time; + } + if ((std::fabs(current_RX_time - last_RINEX_obs_output_time) >= 1.0) ) // TODO: Make it configurable + { + flag_write_RINEX_obs_output = true; + last_RINEX_obs_output_time = current_RX_time; + } + + if ((std::fabs(current_RX_time - last_RINEX_nav_output_time) >= 6.0) ) // TODO: Make it configurable + { + flag_write_RINEX_nav_output = true; + last_RINEX_nav_output_time = current_RX_time; + } + + // correct the observable to account for the receiver clock offset + + for (std::map::iterator it = gnss_observables_map.begin(); it != gnss_observables_map.end(); ++it) + { + it->second.Pseudorange_m = it->second.Pseudorange_m - d_ls_pvt->d_rx_dt_s * GPS_C_m_s; + } + if(first_fix == true) + { + std::cout << "First position fix at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; + ttff_msgbuf ttff; + ttff.mtype = 1; + gettimeofday(&tv, NULL); + long long int end = tv.tv_sec * 1000000 + tv.tv_usec; + ttff.ttff = static_cast(end - begin) / 1000000.0; + send_sys_v_ttff_msg(ttff); + first_fix = false; + } + d_kml_dump->print_position(d_ls_pvt, false); + d_geojson_printer->print_position(d_ls_pvt, false); + d_nmea_printer->Print_Nmea_Line(d_ls_pvt, false); + + /* + * TYPE | RECEIVER + * 0 | Unknown + * 1 | GPS L1 C/A + * 2 | GPS L2C + * 3 | GPS L5 + * 4 | Galileo E1B + * 5 | Galileo E5a + * 6 | Galileo E5b + * 7 | GPS L1 C/A + GPS L2C + * 8 | GPS L1 C/A + GPS L5 + * 9 | GPS L1 C/A + Galileo E1B + * 10 | GPS L1 C/A + Galileo E5a + * 11 | GPS L1 C/A + Galileo E5b + * 12 | Galileo E1B + GPS L2C + * 13 | Galileo E1B + GPS L5 + * 14 | Galileo E1B + Galileo E5a + * 15 | Galileo E1B + Galileo E5b + * 16 | GPS L2C + GPS L5 + * 17 | GPS L2C + Galileo E5a + * 18 | GPS L2C + Galileo E5b + * 19 | GPS L5 + Galileo E5a + * 20 | GPS L5 + Galileo E5b + * 21 | GPS L1 C/A + Galileo E1B + GPS L2C + * 22 | GPS L1 C/A + Galileo E1B + GPS L5 + */ + + // ####################### RINEX FILES ################# + + std::map::iterator galileo_ephemeris_iter; + std::map::iterator gps_ephemeris_iter; + std::map::iterator gps_cnav_ephemeris_iter; + std::map::iterator gnss_observables_iter; + + if (!b_rinex_header_written) // & we have utc data in nav message! + { + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + + if(type_of_rx == 1) // GPS L1 C/A only + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + + } + } + if(type_of_rx == 2) // GPS L2C only + { + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_cnav_iono, d_ls_pvt->gps_cnav_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 4) // Galileo E1B only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 5) // Galileo E5a only + { + std::string signal("5X"); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 6) // Galileo E5b only + { + std::string signal("7X"); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + { + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time); + rp->rinex_nav_header(rp->navFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model); + b_rinex_header_written = true; // do not write header anymore + } + } + + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("1B"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 10) // GPS L1 C/A + Galileo E5a + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("5X"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 11) // GPS L1 C/A + Galileo E5b + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + std::string gal_signal("7X"); + rp->rinex_obs_header(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 14) // Galileo E1B + Galileo E5a + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) + { + std::string gal_signal("1B 5X"); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + if(type_of_rx == 15) // Galileo E1B + Galileo E5b + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) ) + { + std::string gal_signal("1B 7X"); + rp->rinex_obs_header(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gal_signal); + rp->rinex_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_written = true; // do not write header anymore + } + } + } + if(b_rinex_header_written) // The header is already written, we can now log the navigation message data + { + if(flag_write_RINEX_nav_output) + { + if(type_of_rx == 1) // GPS L1 C/A only + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_ephemeris_map); + } + if(type_of_rx == 2) // GPS L2C only + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); + } + if( (type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) ) // Galileo + { + rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + rp->log_rinex_nav(rp->navFile, d_ls_pvt->gps_cnav_ephemeris_map); + } + if((type_of_rx == 9) || (type_of_rx == 10) || (type_of_rx == 11)) // GPS L1 C/A + Galileo + { + rp->log_rinex_nav(rp->navMixFile, d_ls_pvt->gps_ephemeris_map, d_ls_pvt->galileo_ephemeris_map); + } + if((type_of_rx == 14) || (type_of_rx == 15)) // Galileo E1B + Galileo E5a + { + rp->log_rinex_nav(rp->navGalFile, d_ls_pvt->galileo_ephemeris_map); + } + } + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + + // Log observables into the RINEX file + if(flag_write_RINEX_obs_output) + { + if(type_of_rx == 1) // GPS L1 C/A only + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 2) // GPS L2C only + { + if (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_cnav_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_cnav_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_cnav_utc_model, d_ls_pvt->gps_cnav_iono); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 4) // Galileo E1B only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 5) // Galileo E5a only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "5X"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 6) // Galileo E5b only + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "7X"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if( (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navFile, d_ls_pvt->gps_utc_model, d_ls_pvt->gps_iono); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if ((galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) && (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) ) + { + rp->log_rinex_obs(rp->obsFile, gps_ephemeris_iter->second, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map); + } + if (!b_rinex_header_updated && (d_ls_pvt->gps_utc_model.d_A0 != 0)) + { + rp->update_obs_header(rp->obsFile, d_ls_pvt->gps_utc_model); + rp->update_nav_header(rp->navMixFile, d_ls_pvt->gps_iono, d_ls_pvt->gps_utc_model, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 14) // Galileo E1B + Galileo E5a + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 5X"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + if(type_of_rx == 15) // Galileo E1B + Galileo E5b + { + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + rp->log_rinex_obs(rp->obsFile, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, "1B 7X"); + } + if (!b_rinex_header_updated && (d_ls_pvt->galileo_utc_model.A0_6 != 0)) + { + rp->update_nav_header(rp->navGalFile, d_ls_pvt->galileo_iono, d_ls_pvt->galileo_utc_model, d_ls_pvt->galileo_almanac); + rp->update_obs_header(rp->obsFile, d_ls_pvt->galileo_utc_model); + b_rinex_header_updated = true; + } + } + } + } + + // ####################### RTCM MESSAGES ################# + if(b_rtcm_writing_started) + { + if(type_of_rx == 1) // GPS L1 C/A + { + if(flag_write_RTCM_1019_output == true) + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if(flag_write_RTCM_MSM_output == true) + { + std::map::iterator gps_ephemeris_iter; + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + { + if(flag_write_RTCM_1045_output == true) + { + for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + } + } + if(flag_write_RTCM_MSM_output == true) + { + std::map::iterator gal_ephemeris_iter; + gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); + if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + if(flag_write_RTCM_1019_output == true) + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if(flag_write_RTCM_MSM_output == true) + { + std::map::iterator gps_ephemeris_iter; + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + std::map::iterator gps_cnav_ephemeris_iter; + gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end()) ) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if(flag_write_RTCM_1019_output == true) + { + for(gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if(flag_write_RTCM_1045_output == true) + { + for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + if(flag_write_RTCM_MSM_output == true) + { + //gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.end(); + //galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.end(); + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if(gps_channel == 0) + { + if(system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = i; + } + } + } + if(gal_channel == 0) + { + if(system.compare("E") == 0) + { + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + gal_channel = i; + } + } + } + i++; + } + if(flag_write_RTCM_MSM_output == true) + { + + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + if(flag_write_RTCM_MSM_output == true) + { + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + } + } + } + } + + if(!b_rtcm_writing_started) // the first time + { + if(type_of_rx == 1) // GPS L1 C/A + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + + std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + + if((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo + { + for(std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); gal_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(gal_ephemeris_iter->second); + } + + std::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); + + if (gal_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, gal_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + if(type_of_rx == 7) // GPS L1 C/A + GPS L2C + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + + std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); + std::map::iterator gps_cnav_ephemeris_iter = d_ls_pvt->gps_cnav_ephemeris_map.begin(); + + if ((gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) && (gps_cnav_ephemeris_iter != d_ls_pvt->gps_cnav_ephemeris_map.end())) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, gps_cnav_ephemeris_iter->second, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B + { + if(d_rtcm_MT1019_rate_ms != 0) // allows deactivating messages by setting rate = 0 + { + for(std::map::iterator gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.begin(); gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end(); gps_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1019(gps_ephemeris_iter->second); + } + } + if(d_rtcm_MT1045_rate_ms != 0) + { + for(galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end(); galileo_ephemeris_iter++ ) + { + d_rtcm_printer->Print_Rtcm_MT1045(galileo_ephemeris_iter->second); + } + } + + unsigned int i = 0; + for (gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + std::string system(&gnss_observables_iter->second.System, 1); + if(gps_channel == 0) + { + if(system.compare("G") == 0) + { + // This is a channel with valid GPS signal + gps_ephemeris_iter = d_ls_pvt->gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end()) + { + gps_channel = i; + } + } + } + if(gal_channel == 0) + { + if(system.compare("E") == 0) + { + galileo_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end()) + { + gal_channel = i; + } + } + } + i++; + } + + if (gps_ephemeris_iter != d_ls_pvt->gps_ephemeris_map.end() && (d_rtcm_MT1077_rate_ms != 0)) + { + d_rtcm_printer->Print_Rtcm_MSM(7, gps_ephemeris_iter->second, {}, {}, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + + if (galileo_ephemeris_iter != d_ls_pvt->galileo_ephemeris_map.end() && (d_rtcm_MT1097_rate_ms != 0) ) + { + d_rtcm_printer->Print_Rtcm_MSM(7, {}, {}, galileo_ephemeris_iter->second, d_rx_time, gnss_observables_map, 0, 0, 0, 0, 0); + } + b_rtcm_writing_started = true; + } + } + } + } + + // DEBUG MESSAGE: Display position in console output + if( (d_ls_pvt->b_valid_position == true) && (flag_display_pvt == true) ) + { + std::cout << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]" << std::endl; + + LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is Lat = " << d_ls_pvt->d_latitude_d << " [deg], Long = " << d_ls_pvt->d_longitude_d + << " [deg], Height= " << d_ls_pvt->d_height_m << " [m]"; + + /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + << " UTC using "<< d_ls_pvt->d_valid_observations<<" observations is HDOP = " << d_ls_pvt->d_HDOP << " VDOP = " + << d_ls_pvt->d_VDOP <<" TDOP = " << d_ls_pvt->d_TDOP + << " GDOP = " << d_ls_pvt->d_GDOP << std::endl; */ + } + + // MULTIPLEXED FILE RECORDING - Record results to file + if(d_dump == true) + { + try + { + double tmp_double; + for (unsigned int i = 0; i < d_nchannels; i++) + { + tmp_double = in[i][epoch].Pseudorange_m; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_double = 0; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + d_dump_file.write((char*)&d_rx_time, sizeof(double)); + } + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception writing observables dump file " << e.what(); + } + } + } + } + + return noutput_items; +} diff --git a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h similarity index 70% rename from src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h rename to src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h index 1865a6e1f..a23cb4b38 100644 --- a/src/algorithms/PVT/gnuradio_blocks/hybrid_pvt_cc.h +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.h @@ -1,7 +1,7 @@ /*! - * \file hybrid_pvt_cc.h - * \brief Interface of a Position Velocity and Time computation block for Galileo E1 - * \author Javier Arribas, 2013. jarribas(at)cttc.es + * \file rtklib_pvt_cc.h + * \brief Interface of a Position Velocity and Time computation block + * \author Javier Arribas, 2017. jarribas(at)cttc.es * * ------------------------------------------------------------------------- * @@ -28,85 +28,70 @@ * ------------------------------------------------------------------------- */ -#ifndef GNSS_SDR_HYBRID_PVT_CC_H -#define GNSS_SDR_HYBRID_PVT_CC_H +#ifndef GNSS_SDR_RTKLIB_PVT_CC_H +#define GNSS_SDR_RTKLIB_PVT_CC_H +#include #include #include #include #include #include #include -#include +#include #include "nmea_printer.h" #include "kml_printer.h" #include "geojson_printer.h" #include "rinex_printer.h" #include "rtcm_printer.h" -#include "hybrid_ls_pvt.h" +#include "rtklib_solver.h" -class hybrid_pvt_cc; +class rtklib_pvt_cc; -typedef boost::shared_ptr hybrid_pvt_cc_sptr; +typedef boost::shared_ptr rtklib_pvt_cc_sptr; -hybrid_pvt_cc_sptr hybrid_make_pvt_cc(unsigned int n_channels, +rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int n_channels, bool dump, std::string dump_filename, - int averaging_depth, - bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname, + int rinex_version, bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port, unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname, - const unsigned int type_of_receiver); + const unsigned int type_of_receiver, + rtk_t & rtk); /*! * \brief This class implements a block that computes the PVT solution with Galileo E1 signals */ -class hybrid_pvt_cc : public gr::block +class rtklib_pvt_cc : public gr::sync_block { private: - friend hybrid_pvt_cc_sptr hybrid_make_pvt_cc(unsigned int nchannels, + friend rtklib_pvt_cc_sptr rtklib_make_pvt_cc(unsigned int nchannels, bool dump, std::string dump_filename, - int averaging_depth, - bool flag_averaging, int output_rate_ms, int display_rate_ms, bool flag_nmea_tty_port, std::string nmea_dump_filename, std::string nmea_dump_devname, + int rinex_version, bool flag_rtcm_server, bool flag_rtcm_tty_port, unsigned short rtcm_tcp_port, unsigned short rtcm_station_id, std::map rtcm_msg_rate_ms, std::string rtcm_dump_devname, - const unsigned int type_of_receiver); - hybrid_pvt_cc(unsigned int nchannels, - bool dump, std::string dump_filename, - int averaging_depth, - bool flag_averaging, - int output_rate_ms, - int display_rate_ms, - bool flag_nmea_tty_port, - std::string nmea_dump_filename, - std::string nmea_dump_devname, - bool flag_rtcm_server, - bool flag_rtcm_tty_port, - unsigned short rtcm_tcp_port, - unsigned short rtcm_station_id, - std::map rtcm_msg_rate_ms, - std::string rtcm_dump_devname, - const unsigned int type_of_receiver); + const unsigned int type_of_receiver, + rtk_t & rtk); void msg_handler_telemetry(pmt::pmt_t msg); @@ -120,18 +105,14 @@ private: int d_rtcm_MT1097_rate_ms; int d_rtcm_MSM_rate_ms; - void print_receiver_status(Gnss_Synchro** channels_synchronization_data); int d_last_status_print_seg; //for status printer unsigned int d_nchannels; std::string d_dump_filename; std::ofstream d_dump_file; - int d_averaging_depth; - bool d_flag_averaging; + int d_output_rate_ms; int d_display_rate_ms; - long unsigned int d_sample_counter; - long unsigned int d_last_sample_nav_output; std::shared_ptr rp; std::shared_ptr d_kml_dump; @@ -139,8 +120,16 @@ private: std::shared_ptr d_geojson_printer; std::shared_ptr d_rtcm_printer; double d_rx_time; - double d_TOW_at_curr_symbol_constellation; - std::shared_ptr d_ls_pvt; + double last_pvt_display_T_rx_s; + double last_RTCM_1019_output_time; + double last_RTCM_1045_output_time; + double last_RTCM_1077_output_time; + double last_RTCM_1097_output_time; + double last_RTCM_MSM_output_time; + double last_RINEX_obs_output_time; + double last_RINEX_nav_output_time; + std::shared_ptr d_ls_pvt; + std::map gnss_observables_map; bool observables_pairCompare_min(const std::pair& a, const std::pair& b); @@ -154,8 +143,27 @@ private: double ttff; } ttff_msgbuf; bool send_sys_v_ttff_msg(ttff_msgbuf ttff); + struct timeval tv; + long long int begin; public: + rtklib_pvt_cc(unsigned int nchannels, + bool dump, std::string dump_filename, + int output_rate_ms, + int display_rate_ms, + bool flag_nmea_tty_port, + std::string nmea_dump_filename, + std::string nmea_dump_devname, + int rinex_version, + bool flag_rtcm_server, + bool flag_rtcm_tty_port, + unsigned short rtcm_tcp_port, + unsigned short rtcm_station_id, + std::map rtcm_msg_rate_ms, + std::string rtcm_dump_devname, + const unsigned int type_of_receiver, + rtk_t & rtk); + /*! * \brief Get latest set of GPS L1 ephemeris from PVT block * @@ -163,10 +171,10 @@ public: */ std::map get_GPS_L1_ephemeris_map(); - ~hybrid_pvt_cc (); //!< Default destructor + ~rtklib_pvt_cc(); //!< Default destructor - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); //!< PVT Signal Processing + int work (int noutput_items, gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); //!< PVT Signal Processing }; #endif diff --git a/src/algorithms/PVT/libs/CMakeLists.txt b/src/algorithms/PVT/libs/CMakeLists.txt index b3b323d16..d223454d3 100644 --- a/src/algorithms/PVT/libs/CMakeLists.txt +++ b/src/algorithms/PVT/libs/CMakeLists.txt @@ -21,14 +21,13 @@ add_definitions( -DGNSS_SDR_VERSION="${VERSION}" ) set(PVT_LIB_SOURCES pvt_solution.cc ls_pvt.cc - gps_l1_ca_ls_pvt.cc - galileo_e1_ls_pvt.cc hybrid_ls_pvt.cc kml_printer.cc rinex_printer.cc nmea_printer.cc rtcm_printer.cc geojson_printer.cc + rtklib_solver.cc ) include_directories( @@ -37,6 +36,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/core/interfaces ${CMAKE_SOURCE_DIR}/src/core/receiver ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/adapters + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${Boost_INCLUDE_DIRS} ${ARMADILLO_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} @@ -46,5 +46,17 @@ file(GLOB PVT_LIB_HEADERS "*.h") list(SORT PVT_LIB_HEADERS) add_library(pvt_lib ${PVT_LIB_SOURCES} ${PVT_LIB_HEADERS}) source_group(Headers FILES ${PVT_LIB_HEADERS}) -add_dependencies(pvt_lib armadillo-${armadillo_RELEASE} glog-${glog_RELEASE}) -target_link_libraries(pvt_lib ${Boost_LIBRARIES} ${GFlags_LIBS} ${GLOG_LIBRARIES} ${ARMADILLO_LIBRARIES}) +add_dependencies(pvt_lib rtklib_lib armadillo-${armadillo_RELEASE} glog-${glog_RELEASE}) + + +target_link_libraries( + pvt_lib + rtklib_lib + ${Boost_LIBRARIES} + ${GFlags_LIBS} + ${GLOG_LIBRARIES} + ${ARMADILLO_LIBRARIES} + ${BLAS} + ${LAPACK} + ) + diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc deleted file mode 100644 index 48b7d4db7..000000000 --- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.cc +++ /dev/null @@ -1,257 +0,0 @@ -/*! - * \file galileo_e1_ls_pvt.cc - * \brief Implementation of a Least Squares Position, Velocity, and Time - * (PVT) solver, based on K.Borre's Matlab receiver. - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "galileo_e1_ls_pvt.h" -#include -#include "Galileo_E1.h" - - -using google::LogMessage; - -galileo_e1_ls_pvt::galileo_e1_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file) : Ls_Pvt() -{ - // init empty ephemeris for all the available GNSS channels - d_nchannels = nchannels; - d_ephemeris = new Galileo_Navigation_Message[nchannels]; - d_dump_filename = dump_filename; - d_flag_dump_enabled = flag_dump_to_file; - d_galileo_current_time = 0; - d_flag_averaging = false; - - // ############# ENABLE DATA FILE LOG ################# - if (d_flag_dump_enabled == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception opening PVT lib dump file " << e.what(); - } - } - } -} - - -galileo_e1_ls_pvt::~galileo_e1_ls_pvt() -{ - d_dump_file.close(); - delete[] d_ephemeris; -} - - - -bool galileo_e1_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging) -{ - std::map::iterator gnss_pseudoranges_iter; - std::map::iterator galileo_ephemeris_iter; - - arma::vec W; // channels weight vector - arma::vec obs; // pseudoranges observation vector - arma::mat satpos; // satellite positions matrix - - int Galileo_week_number = 0; - double utc = 0.0; - double GST = 0.0; - double TX_time_corrected_s = 0.0; - double SV_clock_bias_s = 0.0; - - d_flag_averaging = flag_averaging; - - // ******************************************************************************** - // ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) **** - // ******************************************************************************** - int valid_obs = 0; //valid observations counter - - for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin(); - gnss_pseudoranges_iter != gnss_pseudoranges_map.end(); - gnss_pseudoranges_iter++) - { - // 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key - galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first); - if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) - { - /*! - * \todo Place here the satellite CN0 (power level, or weight factor) - */ - W.resize(valid_obs + 1, 1); - W(valid_obs) = 1; - - // COMMON RX TIME PVT ALGORITHM - double Rx_time = galileo_current_time; - double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GALILEO_C_m_s; - - // 2- compute the clock drift using the clock model (broadcast) for this SV, including relativistic effect - SV_clock_bias_s = galileo_ephemeris_iter->second.sv_clock_drift(Tx_time); - - // 3- compute the current ECEF position for this SV using corrected TX time - TX_time_corrected_s = Tx_time - SV_clock_bias_s; - galileo_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); - - //store satellite positions in a matrix - satpos.resize(3, valid_obs + 1); - satpos(0, valid_obs) = galileo_ephemeris_iter->second.d_satpos_X; - satpos(1, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Y; - satpos(2, valid_obs) = galileo_ephemeris_iter->second.d_satpos_Z; - - // 4- fill the observations vector with the corrected pseudoranges - obs.resize(valid_obs + 1, 1); - obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + SV_clock_bias_s * GALILEO_C_m_s - d_rx_dt_s * GALILEO_C_m_s; - d_visible_satellites_IDs[valid_obs] = galileo_ephemeris_iter->second.i_satellite_PRN; - d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz; - - - Galileo_week_number = galileo_ephemeris_iter->second.WN_5; //for GST - GST = galileo_ephemeris_map.find(gnss_pseudoranges_iter->first)->second.Galileo_System_Time(Galileo_week_number, galileo_current_time); - - // SV ECEF DEBUG OUTPUT - DLOG(INFO) << "ECEF satellite SV ID=" << galileo_ephemeris_iter->second.i_satellite_PRN - << " X=" << galileo_ephemeris_iter->second.d_satpos_X - << " [m] Y=" << galileo_ephemeris_iter->second.d_satpos_Y - << " [m] Z=" << galileo_ephemeris_iter->second.d_satpos_Z - << " [m] PR_obs=" << obs(valid_obs) << " [m]"; - - valid_obs++; - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV "<< gnss_pseudoranges_iter->first; - } - } - - // ******************************************************************************** - // ****** SOLVE LEAST SQUARES****************************************************** - // ******************************************************************************** - d_valid_observations = valid_obs; - LOG(INFO) << "Galileo PVT: valid observations=" << valid_obs; - - if (valid_obs >= 4) - { - arma::vec rx_position_and_time; - DLOG(INFO) << "satpos=" << satpos; - DLOG(INFO) << "obs="<< obs; - DLOG(INFO) << "W=" << W; - try - { - // check if this is the initial position computation - if (d_rx_dt_s == 0) - { - // execute Bancroft's algorithm to estimate initial receiver position and time - DLOG(INFO) << " Executing Bancroft algorithm..."; - rx_position_and_time = bancroftPos(satpos.t(), obs); - d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration - d_rx_dt_s = rx_position_and_time(3) / GALILEO_C_m_s; // save time for the next iteration [meters]->[seconds] - } - // Execute WLS using previous position as the initialization point - rx_position_and_time = leastSquarePos(satpos, obs, W); - - d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration - d_rx_dt_s += rx_position_and_time(3) / GALILEO_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] - - // Compute Gregorian time - utc = galileo_utc_model.GST_to_UTC_time(GST, Galileo_week_number); - // get time string Gregorian calendar - boost::posix_time::time_duration t = boost::posix_time::seconds(utc); - // 22 August 1999 00:00 last Galileo start GST epoch (ICD sec 5.1.2) - boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); - d_position_UTC_time = p_time; - - DLOG(INFO) << "Galileo Position at TOW=" << galileo_current_time << " in ECEF (X,Y,Z) = " << rx_position_and_time; - - cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); - d_rx_dt_s = rx_position_and_time(3)/GALILEO_C_m_s; // Convert RX time offset from meters to seconds - DLOG(INFO) << "Galileo Position at " << boost::posix_time::to_simple_string(p_time) - << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d - << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]"; - - // ###### Compute DOPs ######## - compute_DOP(); - - // ######## LOG FILE ######### - if(d_flag_dump_enabled == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - // PVT GPS time - tmp_double = galileo_current_time; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position East [m] - tmp_double = rx_position_and_time(0); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position North [m] - tmp_double = rx_position_and_time(1); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position Up [m] - tmp_double = rx_position_and_time(2); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // User clock offset [s] - tmp_double = rx_position_and_time(3); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // GEO user position Latitude [deg] - tmp_double = d_latitude_d; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // GEO user position Longitude [deg] - tmp_double = d_longitude_d; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // GEO user position Height [m] - tmp_double = d_height_m; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing PVT LS dump file "<< e.what(); - } - } - - // MOVING AVERAGE PVT - galileo_e1_ls_pvt::pos_averaging(flag_averaging); - } - catch(const std::exception & e) - { - d_rx_dt_s = 0; //reset rx time estimation - LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what(); - b_valid_position = false; - } - } - else - { - b_valid_position = false; - } - return b_valid_position; -} - diff --git a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h b/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h deleted file mode 100644 index 2220847e5..000000000 --- a/src/algorithms/PVT/libs/galileo_e1_ls_pvt.h +++ /dev/null @@ -1,74 +0,0 @@ -/*! - * \file galileo_e1_ls_pvt.h - * \brief Interface of a Least Squares Position, Velocity, and Time (PVT) - * solver, based on K.Borre's Matlab receiver. - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_GALILEO_E1_LS_PVT_H_ -#define GNSS_SDR_GALILEO_E1_LS_PVT_H_ - -#include -#include -#include -#include -#include "ls_pvt.h" -#include "galileo_navigation_message.h" -#include "gnss_synchro.h" -#include "galileo_ephemeris.h" -#include "galileo_utc_model.h" - - -/*! - * \brief This class implements a simple PVT Least Squares solution - */ -class galileo_e1_ls_pvt : public Ls_Pvt -{ -public: - galileo_e1_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file); - ~galileo_e1_ls_pvt(); - - bool get_PVT(std::map gnss_pseudoranges_map, double galileo_current_time, bool flag_averaging); - - int d_nchannels; //!< Number of available channels for positioning - - Galileo_Navigation_Message* d_ephemeris; - std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris - Galileo_Utc_Model galileo_utc_model; - Galileo_Iono galileo_iono; - Galileo_Almanac galileo_almanac; - - double d_galileo_current_time; - - bool d_flag_dump_enabled; - bool d_flag_averaging; - - std::string d_dump_filename; - std::ofstream d_dump_file; -}; - -#endif diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc deleted file mode 100644 index 342729127..000000000 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ /dev/null @@ -1,270 +0,0 @@ -/*! - * \file gps_l1_ca_ls_pvt.cc - * \brief Implementation of a Least Squares Position, Velocity, and Time - * (PVT) solver, based on K.Borre's Matlab receiver. - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#include "gps_l1_ca_ls_pvt.h" -#include -#include -#include "GPS_L1_CA.h" -#include "GPS_L2C.h" - -using google::LogMessage; - - -gps_l1_ca_ls_pvt::gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file) : Ls_Pvt() -{ - // init empty ephemeris for all the available GNSS channels - d_nchannels = nchannels; - d_ephemeris = new Gps_Navigation_Message[nchannels]; - d_dump_filename = dump_filename; - d_flag_dump_enabled = flag_dump_to_file; - d_flag_averaging = false; - d_GPS_current_time = 0; - - // ############# ENABLE DATA FILE LOG ################# - if (d_flag_dump_enabled == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception opening PVT lib dump file " << e.what(); - } - } - } -} - - - -gps_l1_ca_ls_pvt::~gps_l1_ca_ls_pvt() -{ - d_dump_file.close(); - delete[] d_ephemeris; -} - - - -bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, double GPS_current_time, bool flag_averaging) -{ - std::map::iterator gnss_pseudoranges_iter; - std::map::iterator gps_ephemeris_iter; - - arma::vec W; // channels weight vector - arma::vec obs; // pseudoranges observation vector - arma::mat satpos; // satellite positions matrix - - int GPS_week = 0; - double utc = 0.0; - double TX_time_corrected_s; - double SV_clock_bias_s = 0.0; - - d_flag_averaging = flag_averaging; - - // ******************************************************************************** - // ****** PREPARE THE LEAST SQUARES DATA (SV POSITIONS MATRIX AND OBS VECTORS) **** - // ******************************************************************************** - int valid_obs = 0; //valid observations counter - for(gnss_pseudoranges_iter = gnss_pseudoranges_map.begin(); - gnss_pseudoranges_iter != gnss_pseudoranges_map.end(); - gnss_pseudoranges_iter++) - { - // 1- find the ephemeris for the current SV observation. The SV PRN ID is the map key - gps_ephemeris_iter = gps_ephemeris_map.find(gnss_pseudoranges_iter->first); - if (gps_ephemeris_iter != gps_ephemeris_map.end()) - { - /*! - * \todo Place here the satellite CN0 (power level, or weight factor) - */ - W.resize(valid_obs + 1, 1); - W(valid_obs) = 1; - - // COMMON RX TIME PVT ALGORITHM MODIFICATION (Like RINEX files) - // first estimate of transmit time - double Rx_time = GPS_current_time; - double Tx_time = Rx_time - gnss_pseudoranges_iter->second.Pseudorange_m / GPS_C_m_s; - - // 2- compute the clock drift using the clock model (broadcast) for this SV, not including relativistic effect - SV_clock_bias_s = gps_ephemeris_iter->second.sv_clock_drift(Tx_time); //- gps_ephemeris_iter->second.d_TGD; - - // 3- compute the current ECEF position for this SV using corrected TX time and obtain clock bias including relativistic effect - TX_time_corrected_s = Tx_time - SV_clock_bias_s; - double dtr = gps_ephemeris_iter->second.satellitePosition(TX_time_corrected_s); - - //store satellite positions in a matrix - satpos.resize(3, valid_obs + 1); - satpos(0, valid_obs) = gps_ephemeris_iter->second.d_satpos_X; - satpos(1, valid_obs) = gps_ephemeris_iter->second.d_satpos_Y; - satpos(2, valid_obs) = gps_ephemeris_iter->second.d_satpos_Z; - - // 4- fill the observations vector with the corrected pseudoranges - // compute code bias: TGD for single frequency - // See IS-GPS-200E section 20.3.3.3.3.2 - double sqrt_Gamma=GPS_L1_FREQ_HZ/GPS_L2_FREQ_HZ; - double Gamma=sqrt_Gamma*sqrt_Gamma; - double P1_P2=(1.0-Gamma)*(gps_ephemeris_iter->second.d_TGD* GPS_C_m_s); - double Code_bias_m= P1_P2/(1.0-Gamma); - obs.resize(valid_obs + 1, 1); - obs(valid_obs) = gnss_pseudoranges_iter->second.Pseudorange_m + dtr * GPS_C_m_s-Code_bias_m-d_rx_dt_s * GPS_C_m_s; - d_visible_satellites_IDs[valid_obs] = gps_ephemeris_iter->second.i_satellite_PRN; - d_visible_satellites_CN0_dB[valid_obs] = gnss_pseudoranges_iter->second.CN0_dB_hz; - - // SV ECEF DEBUG OUTPUT - DLOG(INFO) << "(new)ECEF satellite SV ID=" << gps_ephemeris_iter->second.i_satellite_PRN - << " X=" << gps_ephemeris_iter->second.d_satpos_X - << " [m] Y=" << gps_ephemeris_iter->second.d_satpos_Y - << " [m] Z=" << gps_ephemeris_iter->second.d_satpos_Z - << " [m] PR_obs=" << obs(valid_obs) << " [m]"; - - valid_obs++; - // compute the UTC time for this SV (just to print the associated UTC timestamp) - GPS_week = gps_ephemeris_iter->second.i_GPS_week; - utc = gps_utc_model.utc_time(TX_time_corrected_s, GPS_week); - } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_pseudoranges_iter->first; - } - } - - // ******************************************************************************** - // ****** SOLVE LEAST SQUARES****************************************************** - // ******************************************************************************** - d_valid_observations = valid_obs; - DLOG(INFO) << "(new)PVT: valid observations=" << valid_obs; - - if (valid_obs >= 4) - { - arma::vec rx_position_and_time; - DLOG(INFO) << "satpos=" << satpos; - DLOG(INFO) << "obs=" << obs; - DLOG(INFO) << "W=" << W; - - try - { - // check if this is the initial position computation - if (d_rx_dt_s == 0) - { - // execute Bancroft's algorithm to estimate initial receiver position and time - DLOG(INFO) << " Executing Bancroft algorithm..."; - rx_position_and_time = bancroftPos(satpos.t(), obs); - d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration - d_rx_dt_s = rx_position_and_time(3) / GPS_C_m_s; // save time for the next iteration [meters]->[seconds] - } - - // Execute WLS using previous position as the initialization point - rx_position_and_time = leastSquarePos(satpos, obs, W); - - d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration - d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] - - DLOG(INFO) << "(new)Position at TOW=" << GPS_current_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; - DLOG(INFO) << "Accumulated rx clock error=" << d_rx_dt_s << " clock error for this iteration=" << rx_position_and_time(3) / GPS_C_m_s << " [s]"; - - cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); - - // Compute UTC time and print PVT solution - double secondsperweek = 604800.0; // number of seconds in one week (7*24*60*60) - boost::posix_time::time_duration t = boost::posix_time::seconds(utc + secondsperweek * static_cast(GPS_week)); - // 22 August 1999 last GPS time roll over - boost::posix_time::ptime p_time(boost::gregorian::date(1999, 8, 22), t); - d_position_UTC_time = p_time; - DLOG(INFO) << "Position at " << boost::posix_time::to_simple_string(p_time) - << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d - << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]"; - - // ###### Compute DOPs ######## - compute_DOP(); - - // ######## LOG FILE ######### - if(d_flag_dump_enabled == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - // PVT GPS time - tmp_double = GPS_current_time; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position East [m] - tmp_double = d_rx_pos(0); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position North [m] - tmp_double = d_rx_pos(1); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // ECEF User Position Up [m] - tmp_double = d_rx_pos(2); - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // User clock offset [s] - tmp_double = d_rx_dt_s; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // GEO user position Latitude [deg] - tmp_double = d_latitude_d; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // GEO user position Longitude [deg] - tmp_double = d_longitude_d; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - // GEO user position Height [m] - tmp_double = d_height_m; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); - } - } - - // MOVING AVERAGE PVT - pos_averaging(flag_averaging); - - } - catch(const std::exception & e) - { - d_rx_dt_s = 0; //reset rx time estimation - LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what(); - LOG(WARNING) << "satpos=" << satpos; - LOG(WARNING) << "obs=" << obs; - LOG(WARNING) << "W=" << W; - b_valid_position = false; - } - } - else - { - b_valid_position = false; - } - return b_valid_position; -} - - diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h deleted file mode 100644 index c926d24c4..000000000 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.h +++ /dev/null @@ -1,81 +0,0 @@ -/*! - * \file gps_l1_ca_ls_pvt.h - * \brief Interface of a Least Squares Position, Velocity, and Time (PVT) - * solver for GPS L1 C/A, based on K.Borre's Matlab receiver. - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_GPS_L1_CA_LS_PVT_H_ -#define GNSS_SDR_GPS_L1_CA_LS_PVT_H_ - -#include -#include -#include -#include "ls_pvt.h" -#include "GPS_L1_CA.h" -#include "gnss_synchro.h" -#include "gps_ephemeris.h" -#include "gps_navigation_message.h" -#include "gps_utc_model.h" -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" -#include "sbas_ephemeris.h" - - -/*! - * \brief This class implements a simple PVT Least Squares solution for GPS L1 C/A signals - */ -class gps_l1_ca_ls_pvt : public Ls_Pvt -{ -public: - gps_l1_ca_ls_pvt(int nchannels, std::string dump_filename, bool flag_dump_to_file); - ~gps_l1_ca_ls_pvt(); - - bool get_PVT(std::map gnss_pseudoranges_map, double GPS_current_time, bool flag_averaging); - int d_nchannels; //!< Number of available channels for positioning - - Gps_Navigation_Message* d_ephemeris; - - // new ephemeris storage - std::map gps_ephemeris_map; //!< Map storing new Gps_Ephemeris - Gps_Utc_Model gps_utc_model; - Gps_Iono gps_iono; - - Sbas_Ionosphere_Correction sbas_iono; - std::map sbas_sat_corr_map; - std::map sbas_ephemeris_map; - - double d_GPS_current_time; - - bool d_flag_dump_enabled; - bool d_flag_averaging; - - std::string d_dump_filename; - std::ofstream d_dump_file; -}; - -#endif diff --git a/src/algorithms/PVT/libs/hybrid_ls_pvt.h b/src/algorithms/PVT/libs/hybrid_ls_pvt.h index 6cc9fe76f..e1084d6e7 100644 --- a/src/algorithms/PVT/libs/hybrid_ls_pvt.h +++ b/src/algorithms/PVT/libs/hybrid_ls_pvt.h @@ -41,18 +41,20 @@ #include "gps_navigation_message.h" #include "gps_cnav_navigation_message.h" #include "gnss_synchro.h" - +#include "rtklib_rtkcmn.h" /*! * \brief This class implements a simple PVT Least Squares solution */ class hybrid_ls_pvt : public Ls_Pvt { +private: + public: hybrid_ls_pvt(int nchannels,std::string dump_filename, bool flag_dump_to_file); ~hybrid_ls_pvt(); - bool get_PVT(std::map gnss_observables_map, double hybrid_current_time, bool flag_averaging); + bool get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging); int d_nchannels; //!< Number of available channels for positioning std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris diff --git a/src/algorithms/PVT/libs/rinex_printer.cc b/src/algorithms/PVT/libs/rinex_printer.cc index 51d1bad0c..0b6fb5030 100644 --- a/src/algorithms/PVT/libs/rinex_printer.cc +++ b/src/algorithms/PVT/libs/rinex_printer.cc @@ -187,6 +187,7 @@ Rinex_Printer::Rinex_Printer(int conf_version) } numberTypesObservations = 4; // Number of available types of observable in the system + fake_cnav_iode = 1; } @@ -1897,9 +1898,17 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapsecond.d_Toe1 == gps_ephemeris_iter->second.d_Toe2) && (gps_ephemeris_iter->second.d_Toe1 == gps_ephemeris_iter->second.d_Toc)) ) // Toe1: Toe in message type 10, Toe2: Toe in message type 11 + { + // Toe1: Toe in message type 10, Toe2: Toe in message type 11, + fake_cnav_iode = fake_cnav_iode + 1; + if(fake_cnav_iode == 240) fake_cnav_iode = 1; + } + + line += Rinex_Printer::doub2for(fake_cnav_iode, 18, 2); line += std::string(1, ' '); line += Rinex_Printer::doub2for(gps_ephemeris_iter->second.d_Crs, 18, 2); line += std::string(1, ' '); @@ -1962,6 +1971,7 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapsecond.d_IDOT, 18, 2); line += std::string(1, ' '); // No data flag for L2 P code + double my_zero = 0.0; line += Rinex_Printer::doub2for(my_zero, 18, 2); line += std::string(1, ' '); double GPS_week_continuous_number = static_cast(gps_ephemeris_iter->second.i_GPS_week + 1024); // valid until April 7, 2019 (check http://www.colorado.edu/geography/gcraft/notes/gps/gpseow.htm) @@ -1981,8 +1991,8 @@ void Rinex_Printer::log_rinex_nav(std::fstream& out, const std::mapsecond.d_TGD, 18, 2); line += std::string(1, ' '); - // no IODC in CNAV, so we set it to zero - line += Rinex_Printer::doub2for(my_zero, 18, 2); + // no IODC in CNAV, so we fake it (see above) + line += Rinex_Printer::doub2for(fake_cnav_iode, 18, 2); Rinex_Printer::lengthCheck(line); out << line << std::endl; @@ -4760,87 +4770,87 @@ void Rinex_Printer::to_date_time(int gps_week, int gps_tow, int &year, int &mont } -void Rinex_Printer::log_rinex_sbs(std::fstream& out, const Sbas_Raw_Msg& sbs_message) -{ - // line 1: PRN / EPOCH / RCVR - std::stringstream line1; - - // SBAS PRN - line1 << sbs_message.get_prn(); - line1 << " "; - - // gps time of reception - int gps_week; - double gps_sec; - if(sbs_message.get_rx_time_obj().get_gps_time(gps_week, gps_sec)) - { - int year; - int month; - int day; - int hour; - int minute; - int second; - - double gps_sec_one_digit_precicion = round(gps_sec *10)/10; // to prevent rounding towards 60.0sec in the stream output - int gps_tow = trunc(gps_sec_one_digit_precicion); - double sub_sec = gps_sec_one_digit_precicion - double(gps_tow); - - to_date_time(gps_week, gps_tow, year, month, day, hour, minute, second); - line1 << asFixWidthString(year, 2, '0') << " " << asFixWidthString(month, 2, '0') << " " << asFixWidthString(day, 2, '0') << " " << asFixWidthString(hour, 2, '0') << " " << asFixWidthString(minute, 2, '0') << " " << rightJustify(asString(double(second)+sub_sec,1),4,' '); - } - else - { - line1 << std::string(19, ' '); - } - line1 << " "; - - // band - line1 << "L1"; - line1 << " "; - - // Length of data message (bytes) - line1 << asFixWidthString(sbs_message.get_msg().size(), 3, ' '); - line1 << " "; - // File-internal receiver index - line1 << " 0"; - line1 << " "; - // Transmission System Identifier - line1 << "SBA"; - line1 << std::string(35, ' '); - lengthCheck(line1.str()); - out << line1.str() << std::endl; - - // DATA RECORD - 1 - std::stringstream line2; - line2 << " "; - // Message frame identifier - if (sbs_message.get_msg_type() < 10) line2 << " "; - line2 << sbs_message.get_msg_type(); - line2 << std::string(4, ' '); - // First 18 bytes of message (hex) - std::vector msg = sbs_message.get_msg(); - for (size_t i = 0; i < 18 && i < msg.size(); ++i) - { - line2 << std::hex << std::setfill('0') << std::setw(2); - line2 << int(msg[i]) << " "; - } - line2 << std::string(19, ' '); - lengthCheck(line2.str()); - out << line2.str() << std::endl; - - // DATA RECORD - 2 - std::stringstream line3; - line3 << std::string(7, ' '); - // Remaining bytes of message (hex) - for (size_t i = 18; i < 36 && i < msg.size(); ++i) - { - line3 << std::hex << std::setfill('0') << std::setw(2); - line3 << int(msg[i]) << " "; - } - line3 << std::string(31, ' '); - lengthCheck(line3.str()); - out << line3.str() << std::endl; -} +//void Rinex_Printer::log_rinex_sbs(std::fstream& out, const Sbas_Raw_Msg& sbs_message) +//{ +// // line 1: PRN / EPOCH / RCVR +// std::stringstream line1; +// +// // SBAS PRN +// line1 << sbs_message.get_prn(); +// line1 << " "; +// +// // gps time of reception +// int gps_week; +// double gps_sec; +// if(sbs_message.get_rx_time_obj().get_gps_time(gps_week, gps_sec)) +// { +// int year; +// int month; +// int day; +// int hour; +// int minute; +// int second; +// +// double gps_sec_one_digit_precicion = round(gps_sec *10)/10; // to prevent rounding towards 60.0sec in the stream output +// int gps_tow = trunc(gps_sec_one_digit_precicion); +// double sub_sec = gps_sec_one_digit_precicion - double(gps_tow); +// +// to_date_time(gps_week, gps_tow, year, month, day, hour, minute, second); +// line1 << asFixWidthString(year, 2, '0') << " " << asFixWidthString(month, 2, '0') << " " << asFixWidthString(day, 2, '0') << " " << asFixWidthString(hour, 2, '0') << " " << asFixWidthString(minute, 2, '0') << " " << rightJustify(asString(double(second)+sub_sec,1),4,' '); +// } +// else +// { +// line1 << std::string(19, ' '); +// } +// line1 << " "; +// +// // band +// line1 << "L1"; +// line1 << " "; +// +// // Length of data message (bytes) +// line1 << asFixWidthString(sbs_message.get_msg().size(), 3, ' '); +// line1 << " "; +// // File-internal receiver index +// line1 << " 0"; +// line1 << " "; +// // Transmission System Identifier +// line1 << "SBA"; +// line1 << std::string(35, ' '); +// lengthCheck(line1.str()); +// out << line1.str() << std::endl; +// +// // DATA RECORD - 1 +// std::stringstream line2; +// line2 << " "; +// // Message frame identifier +// if (sbs_message.get_msg_type() < 10) line2 << " "; +// line2 << sbs_message.get_msg_type(); +// line2 << std::string(4, ' '); +// // First 18 bytes of message (hex) +// std::vector msg = sbs_message.get_msg(); +// for (size_t i = 0; i < 18 && i < msg.size(); ++i) +// { +// line2 << std::hex << std::setfill('0') << std::setw(2); +// line2 << int(msg[i]) << " "; +// } +// line2 << std::string(19, ' '); +// lengthCheck(line2.str()); +// out << line2.str() << std::endl; +// +// // DATA RECORD - 2 +// std::stringstream line3; +// line3 << std::string(7, ' '); +// // Remaining bytes of message (hex) +// for (size_t i = 18; i < 36 && i < msg.size(); ++i) +// { +// line3 << std::hex << std::setfill('0') << std::setw(2); +// line3 << int(msg[i]) << " "; +// } +// line3 << std::string(31, ' '); +// lengthCheck(line3.str()); +// out << line3.str() << std::endl; +//} int Rinex_Printer::signalStrength(const double snr) diff --git a/src/algorithms/PVT/libs/rinex_printer.h b/src/algorithms/PVT/libs/rinex_printer.h index 0d1dc940e..af168ab9b 100644 --- a/src/algorithms/PVT/libs/rinex_printer.h +++ b/src/algorithms/PVT/libs/rinex_printer.h @@ -61,7 +61,6 @@ #include "gps_navigation_message.h" #include "gps_cnav_navigation_message.h" #include "galileo_navigation_message.h" -#include "sbas_telemetry_data.h" #include "GPS_L1_CA.h" #include "Galileo_E1.h" #include "gnss_synchro.h" @@ -214,7 +213,7 @@ public: /*! * \brief Writes raw SBAS messages into the RINEX file */ - void log_rinex_sbs(std::fstream & out, const Sbas_Raw_Msg & sbs_message); + //void log_rinex_sbs(std::fstream & out, const Sbas_Raw_Msg & sbs_message); void update_nav_header(std::fstream & out, const Gps_Utc_Model & gps_utc, const Gps_Iono & gps_iono); @@ -277,6 +276,8 @@ private: */ void lengthCheck(const std::string & line); + double fake_cnav_iode; + /* * If the string is bigger than length, truncate it from the right. * otherwise, add pad characters to its right. diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc new file mode 100644 index 000000000..526392885 --- /dev/null +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -0,0 +1,311 @@ +/*! + * \file rtklib_solver.cc + * \brief PVT solver based on rtklib library functions adapted to the GNSS-SDR + * data flow and structures + * \authors
    + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
  • 2007-2013, T. Takasu + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * -----------------------------------------------------------------------*/ + +#include "rtklib_solver.h" +#include +#include "rtklib_conversions.h" +#include "GPS_L1_CA.h" +#include "Galileo_E1.h" + + +using google::LogMessage; + +rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag_dump_to_file, rtk_t & rtk) +{ + // init empty ephemeris for all the available GNSS channels + d_nchannels = nchannels; + d_dump_filename = dump_filename; + d_flag_dump_enabled = flag_dump_to_file; + count_valid_position = 0; + d_flag_averaging = false; + rtk_ = rtk; + + pvt_sol = {{0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, {0,0,0,0,0,0}, '0', '0', '0', 0, 0, 0 }; + + // ############# ENABLE DATA FILE LOG ################# + if (d_flag_dump_enabled == true) + { + if (d_dump_file.is_open() == false) + { + try + { + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "PVT lib dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception opening PVT lib dump file " << e.what(); + } + } + } +} + + +rtklib_solver::~rtklib_solver() +{ + d_dump_file.close(); +} + + +bool rtklib_solver::get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging) +{ + std::map::iterator gnss_observables_iter; + std::map::iterator galileo_ephemeris_iter; + std::map::iterator gps_ephemeris_iter; + std::map::iterator gps_cnav_ephemeris_iter; + + d_flag_averaging = flag_averaging; + + // ******************************************************************************** + // ****** PREPARE THE DATA (SV EPHEMERIS AND OBSERVATIONS) ************************ + // ******************************************************************************** + int valid_obs = 0; //valid observations counter + + obsd_t obs_data[MAXOBS]; + eph_t eph_data[MAXOBS]; + + for(gnss_observables_iter = gnss_observables_map.begin(); + gnss_observables_iter != gnss_observables_map.end(); + gnss_observables_iter++) + { + switch(gnss_observables_iter->second.System) + { + case 'E': + { + // 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key + galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (galileo_ephemeris_iter != galileo_ephemeris_map.end()) + { + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + galileo_ephemeris_iter->second.WN_5, + 0); + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + break; + } + case 'G': + { + // 1 GPS - find the ephemeris for the current GPS SV observation. The SV PRN ID is the map key + std::string sig_(gnss_observables_iter->second.Signal); + if(sig_.compare("1C") == 0) + { + gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != gps_ephemeris_map.end()) + { + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(gps_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + gps_ephemeris_iter->second.i_GPS_week, + 0); + valid_obs++; + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first; + } + } + if(sig_.compare("2S") == 0) + { + gps_cnav_ephemeris_iter = gps_cnav_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_cnav_ephemeris_iter != gps_cnav_ephemeris_map.end()) + { + // 1. Find the same satellite in GPS L1 band + gps_ephemeris_iter = gps_ephemeris_map.find(gnss_observables_iter->second.PRN); + if (gps_ephemeris_iter != gps_ephemeris_map.end()) + { + // 2. If found, replace the existing GPS L1 ephemeris with the GPS L2 ephemeris + // (more precise!), and attach the L2 observation to the L1 observation in RTKLIB structure + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == static_cast(gnss_observables_iter->second.PRN)) + { + eph_data[i] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + obs_data[valid_obs] = insert_obs_to_rtklib(obs_data[valid_obs], + gnss_observables_iter->second, + gps_cnav_ephemeris_iter->second.i_GPS_week, + 1);//Band 2 (L2) + break; + } + } + } + else + { + // 3. If not found, insert the GPS L2 ephemeris and the observation + //convert ephemeris from GNSS-SDR class to RTKLIB structure + eph_data[valid_obs] = eph_to_rtklib(gps_cnav_ephemeris_iter->second); + //convert observation from GNSS-SDR class to RTKLIB structure + obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + obs_data[valid_obs] = insert_obs_to_rtklib(newobs, + gnss_observables_iter->second, + gps_cnav_ephemeris_iter->second.i_GPS_week, + 1);//Band 2 (L2) + valid_obs++; + } + } + else // the ephemeris are not available for this SV + { + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; + } + } + break; + } + default : + DLOG(INFO) << "Hybrid observables: Unknown GNSS"; + break; + } + } + + // ********************************************************************** + // ****** SOLVE PVT****************************************************** + // ********************************************************************** + + b_valid_position = false; + if (valid_obs > 0) + { + int result = 0; + nav_t nav_data; + nav_data.eph = eph_data; + nav_data.n = valid_obs; + for (int i = 0; i < MAXSAT; i++) + { + nav_data.lam[i][0] = SPEED_OF_LIGHT / FREQ1; /* L1/E1 */ + nav_data.lam[i][1] = SPEED_OF_LIGHT / FREQ2; /* L2 */ + nav_data.lam[i][2] = SPEED_OF_LIGHT / FREQ5; /* L2 */ + } + + result = rtkpos(&rtk_, obs_data, valid_obs, &nav_data); + if(result == 0) + { + LOG(INFO) << "RTKLIB rtkpos error message: " << rtk_.errbuf; + d_rx_dt_s = 0; //reset rx time estimation + d_valid_observations = 0; + } + else + { + d_valid_observations = rtk_.sol.ns; //record the number of valid satellites used by the PVT solver + pvt_sol = rtk_.sol; + b_valid_position = true; + arma::vec rx_position_and_time(4); + rx_position_and_time(0) = pvt_sol.rr[0]; + rx_position_and_time(1) = pvt_sol.rr[1]; + rx_position_and_time(2) = pvt_sol.rr[2]; + rx_position_and_time(3) = pvt_sol.dtr[0]; + d_rx_pos = rx_position_and_time.rows(0, 2); // save ECEF position for the next iteration + d_rx_dt_s += rx_position_and_time(3) / GPS_C_m_s; // accumulate the rx time error for the next iteration [meters]->[seconds] + DLOG(INFO) << "RTKLIB Position at TOW=" << Rx_time << " in ECEF (X,Y,Z,t[meters]) = " << rx_position_and_time; + + boost::posix_time::ptime p_time; + gtime_t rtklib_utc_time = gpst2utc(pvt_sol.time); + p_time = boost::posix_time::from_time_t(rtklib_utc_time.time); + p_time+=boost::posix_time::microseconds(round(rtklib_utc_time.sec * 1e6)); + d_position_UTC_time = p_time; + cart2geo(static_cast(rx_position_and_time(0)), static_cast(rx_position_and_time(1)), static_cast(rx_position_and_time(2)), 4); + + DLOG(INFO) << "RTKLIB Position at " << boost::posix_time::to_simple_string(p_time) + << " is Lat = " << d_latitude_d << " [deg], Long = " << d_longitude_d + << " [deg], Height= " << d_height_m << " [m]" << " RX time offset= " << d_rx_dt_s << " [s]"; + + // ######## LOG FILE ######### + if(d_flag_dump_enabled == true) + { + // MULTIPLEXED FILE RECORDING - Record results to file + try + { + double tmp_double; + // PVT GPS time + tmp_double = Rx_time; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position East [m] + tmp_double = rx_position_and_time(0); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position North [m] + tmp_double = rx_position_and_time(1); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // ECEF User Position Up [m] + tmp_double = rx_position_and_time(2); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // User clock offset [s] + tmp_double = rx_position_and_time(3); + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // GEO user position Latitude [deg] + tmp_double = d_latitude_d; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // GEO user position Longitude [deg] + tmp_double = d_longitude_d; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + // GEO user position Height [m] + tmp_double = d_height_m; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + } + catch (const std::ifstream::failure& e) + { + LOG(WARNING) << "Exception writing PVT LS dump file " << e.what(); + } + } + } + } + return b_valid_position; +} diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h new file mode 100644 index 000000000..5ab5ee5bb --- /dev/null +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -0,0 +1,107 @@ +/*! + * \file rtklib_solver.h + * \brief PVT solver based on rtklib library functions adapted to the GNSS-SDR + * data flow and structures + * \authors
    + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
  • 2007-2013, T. Takasu + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * -------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_SOLVER_H_ +#define GNSS_SDR_RTKLIB_SOLVER_H_ + +#include +#include +#include +#include +#include "rtklib_rtkpos.h" +#include "galileo_navigation_message.h" +#include "gps_navigation_message.h" +#include "gps_cnav_navigation_message.h" +#include "gnss_synchro.h" +#include "pvt_solution.h" + +/*! + * \brief This class implements a simple PVT Least Squares solution + */ +class rtklib_solver : public Pvt_Solution +{ +private: + +public: + rtklib_solver(int nchannels,std::string dump_filename, bool flag_dump_to_file, rtk_t & rtk); + ~rtklib_solver(); + + bool get_PVT(std::map gnss_observables_map, double Rx_time, bool flag_averaging); + int d_nchannels; //!< Number of available channels for positioning + + std::map galileo_ephemeris_map; //!< Map storing new Galileo_Ephemeris + std::map gps_ephemeris_map; //!< Map storing new GPS_Ephemeris + std::map gps_cnav_ephemeris_map; + + Galileo_Utc_Model galileo_utc_model; + Galileo_Iono galileo_iono; + Galileo_Almanac galileo_almanac; + + Gps_Utc_Model gps_utc_model; + Gps_Iono gps_iono; + + Gps_CNAV_Iono gps_cnav_iono; + Gps_CNAV_Utc_Model gps_cnav_utc_model; + + int count_valid_position; + + bool d_flag_dump_enabled; + + sol_t pvt_sol; + rtk_t rtk_; + + std::string d_dump_filename; + std::ofstream d_dump_file; +}; + +#endif diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc index d62fefd6b..106c959ac 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_e5a_noncoherent_iq_acquisition_caf_cc.cc @@ -275,7 +275,6 @@ void galileo_e5a_noncoherentIQ_acquisition_caf_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc index b2ebba1df..ab2d501b9 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/galileo_pcps_8ms_acquisition_cc.cc @@ -153,7 +153,6 @@ void galileo_pcps_8ms_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc index c7910ff97..449433328 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.cc @@ -162,6 +162,7 @@ void pcps_acquisition_cc::set_local_code(std::complex * code) // Here we want to create a buffer that looks like this: // [ 0 0 0 ... 0 c_0 c_1 ... c_L] // where c_i is the local code and there are L zeros and L chips + gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler if( d_bit_transition_flag ) { int offset = d_fft_size/2; @@ -193,7 +194,6 @@ void pcps_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; @@ -217,6 +217,7 @@ void pcps_acquisition_cc::init() void pcps_acquisition_cc::set_state(int state) { + gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_state = state; if (d_state == 1) { @@ -237,6 +238,42 @@ void pcps_acquisition_cc::set_state(int state) } +void pcps_acquisition_cc::send_positive_acquisition() +{ + // 6.1- Declare positive acquisition using a message port + //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL + DLOG(INFO) << "positive acquisition" + << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN + << "sample_stamp " << d_sample_counter + << "test statistics value " << d_test_statistics + << "test statistics threshold " << d_threshold + << "code phase " << d_gnss_synchro->Acq_delay_samples + << "doppler " << d_gnss_synchro->Acq_doppler_hz + << "magnitude " << d_mag + << "input signal power " << d_input_power; + + this->message_port_pub(pmt::mp("events"), pmt::from_long(1)); + +} + +void pcps_acquisition_cc::send_negative_acquisition() +{ + // 6.2- Declare negative acquisition using a message port + //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL + DLOG(INFO) << "negative acquisition" + << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN + << "sample_stamp " << d_sample_counter + << "test statistics value " << d_test_statistics + << "test statistics threshold " << d_threshold + << "code phase " << d_gnss_synchro->Acq_delay_samples + << "doppler " << d_gnss_synchro->Acq_doppler_hz + << "magnitude " << d_mag + << "input signal power " << d_input_power; + + this->message_port_pub(pmt::mp("events"), pmt::from_long(2)); + +} + int pcps_acquisition_cc::general_work(int noutput_items, gr_vector_int &ninput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items __attribute__((unused))) @@ -252,8 +289,6 @@ int pcps_acquisition_cc::general_work(int noutput_items, * 6. Declare positive or negative acquisition using a message port */ - int acquisition_message = -1; //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL - switch (d_state) { case 0: @@ -268,15 +303,12 @@ int pcps_acquisition_cc::general_work(int noutput_items, d_mag = 0.0; d_input_power = 0.0; d_test_statistics = 0.0; - d_state = 1; } d_sample_counter += d_fft_size * ninput_items[0]; // sample counter consume_each(ninput_items[0]); - //DLOG(INFO) << "Consumed " << ninput_items[0] << " items"; - break; } @@ -294,16 +326,14 @@ int pcps_acquisition_cc::general_work(int noutput_items, d_input_power = 0.0; d_mag = 0.0; - d_sample_counter += d_fft_size; // sample counter - d_well_count++; - DLOG(INFO) << "Channel: " << d_channel + DLOG(INFO)<< "Channel: " << d_channel << " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN << " ,sample stamp: " << d_sample_counter << ", threshold: " << d_threshold << ", doppler_max: " << d_doppler_max - << ", doppler_step: " << d_doppler_step; + << ", doppler_step: " << d_doppler_step< d_threshold) { - d_state = 2; // Positive acquisition + d_state = 0; // Positive acquisition + d_active = false; + send_positive_acquisition(); } else if (d_well_count == d_max_dwells) { - d_state = 3; // Negative acquisition + d_state = 0; + d_active = false; + send_negative_acquisition(); } } else @@ -418,69 +452,23 @@ int pcps_acquisition_cc::general_work(int noutput_items, { if (d_test_statistics > d_threshold) { - d_state = 2; // Positive acquisition + d_state = 0; // Positive acquisition + d_active = false; + send_positive_acquisition(); } else { - d_state = 3; // Negative acquisition + d_state = 0; // Negative acquisition + d_active = false; + send_negative_acquisition(); } } } consume_each(1); - - DLOG(INFO) << "Done. Consumed 1 item."; - break; } - case 2: - { - // 6.1- Declare positive acquisition using a message port - DLOG(INFO) << "positive acquisition"; - DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; - DLOG(INFO) << "sample_stamp " << d_sample_counter; - DLOG(INFO) << "test statistics value " << d_test_statistics; - DLOG(INFO) << "test statistics threshold " << d_threshold; - DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; - DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; - DLOG(INFO) << "magnitude " << d_mag; - DLOG(INFO) << "input signal power " << d_input_power; - - d_active = false; - d_state = 0; - d_sample_counter += d_fft_size * ninput_items[0]; // sample counter - consume_each(ninput_items[0]); - - acquisition_message = 1; - this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message)); - - break; - } - - case 3: - { - // 6.2- Declare negative acquisition using a message port - DLOG(INFO) << "negative acquisition"; - DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN; - DLOG(INFO) << "sample_stamp " << d_sample_counter; - DLOG(INFO) << "test statistics value " << d_test_statistics; - DLOG(INFO) << "test statistics threshold " << d_threshold; - DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples; - DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz; - DLOG(INFO) << "magnitude " << d_mag; - DLOG(INFO) << "input signal power " << d_input_power; - - d_active = false; - d_state = 0; - - d_sample_counter += d_fft_size * ninput_items[0]; // sample counter - consume_each(ninput_items[0]); - acquisition_message = 2; - this->message_port_pub(pmt::mp("events"), pmt::from_long(acquisition_message)); - - break; - } } return noutput_items; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h index a0ab23131..00a602fc4 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_cc.h @@ -95,6 +95,8 @@ private: void update_local_carrier(gr_complex* carrier_vector, int correlator_length_samples, float freq); + void send_negative_acquisition(); + void send_positive_acquisition(); long d_fs_in; long d_freq; int d_samples_per_ms; @@ -143,6 +145,7 @@ public: */ void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) { + gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_gnss_synchro = p_gnss_synchro; } @@ -172,6 +175,7 @@ public: */ void set_active(bool active) { + gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_active = active; } @@ -188,6 +192,7 @@ public: */ void set_channel(unsigned int channel) { + gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_channel = channel; } @@ -198,6 +203,7 @@ public: */ void set_threshold(float threshold) { + gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_threshold = threshold; } @@ -207,6 +213,7 @@ public: */ void set_doppler_max(unsigned int doppler_max) { + gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_doppler_max = doppler_max; } @@ -216,6 +223,7 @@ public: */ void set_doppler_step(unsigned int doppler_step) { + gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler d_doppler_step = doppler_step; } diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc index 6200efeda..d61d89f66 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc @@ -165,7 +165,6 @@ void pcps_acquisition_fine_doppler_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc index 40201b0ba..92466e159 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_sc.cc @@ -190,7 +190,6 @@ void pcps_acquisition_sc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc index 5504e6676..cb671a2dd 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_assisted_acquisition_cc.cc @@ -158,7 +158,6 @@ void pcps_assisted_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc index e03d6c2d5..4b9a775ee 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_cccwsr_acquisition_cc.cc @@ -166,7 +166,6 @@ void pcps_cccwsr_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_multithread_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_multithread_acquisition_cc.cc index 2cf411d32..a4f03815b 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_multithread_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_multithread_acquisition_cc.cc @@ -156,7 +156,6 @@ void pcps_multithread_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc index b0a17c830..de7584ac5 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_opencl_acquisition_cc.cc @@ -293,7 +293,6 @@ void pcps_opencl_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc index 1d5bd8566..8c9d053df 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_quicksync_acquisition_cc.cc @@ -197,7 +197,6 @@ void pcps_quicksync_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; //DLOG(INFO) << "START init"; d_gnss_synchro->Acq_delay_samples = 0.0; diff --git a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc index 8810a23d0..1e3a0118c 100644 --- a/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc +++ b/src/algorithms/acquisition/gnuradio_blocks/pcps_tong_acquisition_cc.cc @@ -167,7 +167,6 @@ void pcps_tong_acquisition_cc::init() d_gnss_synchro->Flag_valid_symbol_output = false; d_gnss_synchro->Flag_valid_pseudorange = false; d_gnss_synchro->Flag_valid_word = false; - d_gnss_synchro->Flag_preamble = false; d_gnss_synchro->Acq_delay_samples = 0.0; d_gnss_synchro->Acq_doppler_hz = 0.0; diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index f0a62912f..f15c9ddb8 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -16,11 +16,13 @@ # along with GNSS-SDR. If not, see . # +add_subdirectory(rtklib) set(GNSS_SPLIBS_SOURCES gps_l2c_signal.cc galileo_e1_signal_processing.cc gnss_sdr_valve.cc + gnss_sdr_sample_counter.cc gnss_signal_processing.cc gps_sdr_signal_processing.cc pass_through.cc diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.cc b/src/algorithms/libs/gnss_sdr_sample_counter.cc new file mode 100644 index 000000000..ec93ce09c --- /dev/null +++ b/src/algorithms/libs/gnss_sdr_sample_counter.cc @@ -0,0 +1,71 @@ +/*! + * \file gnss_sdr_sample_counter.cc + * \brief Simple block to report the current receiver time based on the output of the tracking or telemetry blocks + * \author Javier Arribas 2017. jarribas(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "gnss_sdr_sample_counter.h" +#include "gnss_synchro.h" +#include + +gnss_sdr_sample_counter::gnss_sdr_sample_counter () : gr::sync_block("sample_counter", + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + gr::io_signature::make(0,0,0)) +{ + this->message_port_register_out(pmt::mp("sample_counter")); + last_T_rx_s=0; + report_interval_s=1;//default reporting 1 second + flag_enable_send_msg=false; //enable it for reporting time with asynchronous message +} + + +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter () +{ + gnss_sdr_sample_counter_sptr sample_counter_(new gnss_sdr_sample_counter()); + return sample_counter_; +} + + +int gnss_sdr_sample_counter::work (int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) +{ + const Gnss_Synchro *in = (const Gnss_Synchro *) input_items[0]; // input + + double current_T_rx_s=in[noutput_items-1].Tracking_sample_counter/(double)in[noutput_items-1].fs; + if ((current_T_rx_s-last_T_rx_s)>report_interval_s) + { + std::cout<<"Current receiver time: "<message_port_pub(pmt::mp("receiver_time"), pmt::from_double(current_T_rx_s)); + } + last_T_rx_s=current_T_rx_s; + } + return noutput_items; +} diff --git a/src/algorithms/libs/gnss_sdr_sample_counter.h b/src/algorithms/libs/gnss_sdr_sample_counter.h new file mode 100644 index 000000000..ec1560ee8 --- /dev/null +++ b/src/algorithms/libs/gnss_sdr_sample_counter.h @@ -0,0 +1,58 @@ +/*! + * \file gnss_sdr_sample_counter.h + * \brief Simple block to report the current receiver time based on the output of the tracking or telemetry blocks + * \author Javier Arribas 2017. jarribas(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ +#ifndef GNSS_SDR_sample_counter_H_ +#define GNSS_SDR_sample_counter_H_ + +#include +#include + + +class gnss_sdr_sample_counter; + +typedef boost::shared_ptr gnss_sdr_sample_counter_sptr; + +gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter (); + +class gnss_sdr_sample_counter : public gr::sync_block +{ + friend gnss_sdr_sample_counter_sptr gnss_sdr_make_sample_counter(); + gnss_sdr_sample_counter (); + double last_T_rx_s; + double report_interval_s; + bool flag_enable_send_msg; + +public: + int work(int noutput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items); +}; + +#endif /*GNSS_SDR_sample_counter_H_*/ diff --git a/src/algorithms/libs/rtklib/CMakeLists.txt b/src/algorithms/libs/rtklib/CMakeLists.txt new file mode 100644 index 000000000..a6764ed7c --- /dev/null +++ b/src/algorithms/libs/rtklib/CMakeLists.txt @@ -0,0 +1,58 @@ +# Copyright (C) 2012-2017 (see AUTHORS file for a list of contributors) +# +# This file is part of GNSS-SDR. +# +# GNSS-SDR is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# GNSS-SDR is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with GNSS-SDR. If not, see . +# + +add_definitions( -DGNSS_SDR_VERSION="${VERSION}" ) + +set(RTKLIB_LIB_SOURCES + rtklib_rtkcmn.cc + rtklib_ephemeris.cc + rtklib_preceph.cc + rtklib_sbas.cc + rtklib_ionex.cc + rtklib_pntpos.cc + rtklib_ppp.cc + rtklib_tides.cc + rtklib_lambda.cc + rtklib_rtkpos.cc + rtklib_conversions.cc +) + +include_directories( + $(CMAKE_CURRENT_SOURCE_DIR) + ${CMAKE_SOURCE_DIR}/src/core/system_parameters + ${CMAKE_SOURCE_DIR}/src/core/interfaces + ${CMAKE_SOURCE_DIR}/src/core/receiver + ${Boost_INCLUDE_DIRS} + ${GFlags_INCLUDE_DIRS} + ${GLOG_INCLUDE_DIRS} +) + +file(GLOB RTKLIB_LIB_HEADERS "*.h") +list(SORT RTKLIB_LIB_HEADERS) +add_library(rtklib_lib ${RTKLIB_LIB_SOURCES} ${RTKLIB_LIB_HEADERS}) +source_group(Headers FILES ${RTKLIB_LIB_HEADERS}) +add_dependencies(rtklib_lib glog-${glog_RELEASE}) + +target_link_libraries( + rtklib_lib + ${Boost_LIBRARIES} + ${GFlags_LIBS} + ${GLOG_LIBRARIES} + ${BLAS} + ${LAPACK} + ) diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h new file mode 100644 index 000000000..b7259b89c --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -0,0 +1,1009 @@ +/*! + * \file rtklib.h + * \brief main header file for the rtklib library + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_H_ +#define GNSS_SDR_RTKLIB_H_ + +#include +#include +#include +#include +#include +#include +#include +#include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" +#include "gnss_obs_codes.h" + + +const int FILEPATHSEP = '/'; + +const double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84) (m) +const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84) + +const double HION = 350000.0; //!< ionosphere height (m) +const double PRN_HWBIAS = 1e-6; //!< process noise of h/w bias (m/MHz/sqrt(s)) + +const double INT_SWAP_STAT = 86400.0; //!< swap interval of solution status file (s) + +const unsigned int POLYCRC32 = 0xEDB88320u; //!< CRC32 polynomial +const unsigned int POLYCRC24Q = 0x1864CFBu; //!< CRC24Q polynomial + +const int PMODE_SINGLE = 0; //!< positioning mode: single +const int PMODE_DGPS = 1; //!< positioning mode: DGPS/DGNSS +const int PMODE_KINEMA = 2; //!< positioning mode: kinematic +const int PMODE_STATIC = 3; //!< positioning mode: static +const int PMODE_MOVEB = 4; //!< positioning mode: moving-base +const int PMODE_FIXED = 5; //!< positioning mode: fixed +const int PMODE_PPP_KINEMA = 6; //!< positioning mode: PPP-kinemaric +const int PMODE_PPP_STATIC = 7; //!< positioning mode: PPP-static +const int PMODE_PPP_FIXED = 8; //!< positioning mode: PPP-fixed + +const int SOLF_LLH = 0; //!< solution format: lat/lon/height +const int SOLF_XYZ = 1; //!< solution format: x/y/z-ecef +const int SOLF_ENU = 2; //!< solution format: e/n/u-baseline +const int SOLF_NMEA = 3; //!< solution format: NMEA-183 +const int SOLF_STAT = 4; //!< solution format: solution status +const int SOLF_GSIF = 5; //!< solution format: GSI F1/F2 + +const int SOLQ_NONE = 0; //!< solution status: no solution +const int SOLQ_FIX = 1; //!< solution status: fix +const int SOLQ_FLOAT = 2; //!< solution status: float +const int SOLQ_SBAS = 3; //!< solution status: SBAS +const int SOLQ_DGPS = 4; //!< solution status: DGPS/DGNSS +const int SOLQ_SINGLE = 5; //!< solution status: single +const int SOLQ_PPP = 6; //!< solution status: PPP +const int SOLQ_DR = 7; //!< solution status: dead reckoning +const int MAXSOLQ = 7; //!< max number of solution status + +const int TIMES_GPST = 0; //!< time system: gps time +const int TIMES_UTC = 1; //!< time system: utc +const int TIMES_JST = 2; //!< time system: jst + + +const double ERR_SAAS = 0.3; //!< saastamoinen model error std (m) +const double ERR_BRDCI = 0.5; //!< broadcast iono model error factor +const double ERR_CBIAS = 0.3; //!< code bias error std (m) +const double REL_HUMI = 0.7; //!< relative humidity for saastamoinen model +const double GAP_RESION = 120; //!< default gap to reset ionos parameters (ep) + +const int MAXFREQ = 7; //!< max NFREQ + +const int MAXLEAPS = 64; //!< max number of leap seconds table +const double DTTOL = 0.005; //!< tolerance of time difference (s) + +const int NFREQ = 3; //!< number of carrier frequencies +const int NFREQGLO = 2; //!< number of carrier frequencies of GLONASS +const int NEXOBS = 0; //!< number of extended obs codes +const int MAXANT = 64; //!< max length of station name/antenna type + +const int MINPRNGPS = 1; //!< min satellite PRN number of GPS +const int MAXPRNGPS = 32; //!< max satellite PRN number of GPS +const int NSATGPS = (MAXPRNGPS - MINPRNGPS + 1); //!< number of GPS satellites +const int NSYSGPS = 1; + +const int SYS_NONE = 0x00; //!< navigation system: none +const int SYS_GPS = 0x01; //!< navigation system: GPS +const int SYS_SBS = 0x02; //!< navigation system: SBAS +const int SYS_GLO = 0x04; //!< navigation system: GLONASS +const int SYS_GAL = 0x08; //!< navigation system: Galileo +const int SYS_QZS = 0x10; //!< navigation system: QZSS +const int SYS_BDS = 0x20; //!< navigation system: BeiDou +const int SYS_IRN = 0x40; //!< navigation system: IRNS +const int SYS_LEO = 0x80; //!< navigation system: LEO +const int SYS_ALL = 0xFF; //!< navigation system: all + + + +#ifdef ENAGLO +const int MINPRNGLO = 1; //!< min satellite slot number of GLONASS +const int MAXPRNGLO = 27; //!< max satellite slot number of GLONASS +const int NSATGLO = (MAXPRNGLO - MINPRNGLO + 1); //!< number of GLONASS satellites +const int NSYSGLO = 1; +#else +const int MINPRNGLO = 0; +const int MAXPRNGLO = 0; +const int NSATGLO = 0; +const int NSYSGLO = 0; +#endif + +const int MINPRNGAL = 1; //!< min satellite PRN number of Galileo +const int MAXPRNGAL = 30; //!< max satellite PRN number of Galileo +const int NSATGAL = (MAXPRNGAL - MINPRNGAL + 1); //!< number of Galileo satellites +const int NSYSGAL = 1; + +#ifdef ENAQZS +const int MINPRNQZS = 193; //!< min satellite PRN number of QZSS +const int MAXPRNQZS = 199; //!< max satellite PRN number of QZSS +const int MINPRNQZS_S = 183; //!< min satellite PRN number of QZSS SAIF +const int MAXPRNQZS_S = 189; //!< max satellite PRN number of QZSS SAIF +const int NSATQZS = (MAXPRNQZS - MINPRNQZS + 1); //!< number of QZSS satellites +const int NSYSQZS = 1; +#else +const int MINPRNQZS = 0; +const int MAXPRNQZS = 0; +const int MINPRNQZS_S = 0; +const int MAXPRNQZS_S = 0; +const int NSATQZS = 0; +const int NSYSQZS = 0; +#endif + +#ifdef ENABDS +const int MINPRNBDS = 1; //!< min satellite sat number of BeiDou +const int MAXPRNBDS = 35; //!< max satellite sat number of BeiDou +const int NSATBDS = (MAXPRNBDS - MINPRNCM + 1); //!< number of BeiDou satellites +const int NSYSBDS = 1; +#else +const int MINPRNBDS = 0; +const int MAXPRNBDS = 0; +const int NSATBDS = 0; +const int NSYSBDS = 0; +#endif + +#ifdef ENAIRN +const int MINPRNIRN = 1; //!< min satellite sat number of IRNSS +const int MAXPRNIRN = 7; //!< max satellite sat number of IRNSS +const int NSATIRN = (MAXPRNIRN - MINPRNIRN + 1); //!< number of IRNSS satellites +const int NSYSIRN = 1; +#else +const int MINPRNIRN = 0; +const int MAXPRNIRN = 0; +const int NSATIRN = 0; +const int NSYSIRN = 0; +#endif + +#ifdef ENALEO +const int MINPRNLEO = 1; //!< min satellite sat number of LEO +const int NSATLEO = 10; //!< max satellite sat number of LEO +const int NSATLEO = (MAXPRNLEO - MINPRNLEO + 1); //!< number of LEO satellites +const int NSYSLEO = 1; +#else +const int MINPRNLEO = 0; +const int MAXPRNLEO = 0; +const int NSATLEO = 0; +const int NSYSLEO = 0; +#endif + +const int NSYS = (NSYSGPS + NSYSGLO + NSYSGAL + NSYSQZS + NSYSBDS + NSYSIRN + NSYSLEO); //!< number of systems + +const int MINPRNSBS = 120; //!< min satellite PRN number of SBAS +const int MAXPRNSBS = 142; //!< max satellite PRN number of SBAS +const int NSATSBS = (MAXPRNSBS - MINPRNSBS + 1); //!< number of SBAS satellites + +const int MAXSAT = (NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATBDS + NSATIRN + NSATSBS + NSATLEO); + +const int MAXSTA = 255; + +#ifndef MAXOBS +const int MAXOBS = 64; //!< max number of obs in an epoch +#endif + +const int MAXRCV = 64; //!< max receiver number (1 to MAXRCV) +const int MAXOBSTYPE = 64; //!< max number of obs type in RINEX +const double MAXDTOE = 7200.0; //!< max time difference to GPS Toe (s) +const double MAXDTOE_QZS = 7200.0; //!< max time difference to QZSS Toe (s) +const double MAXDTOE_GAL = 10800.0; //!< max time difference to Galileo Toe (s) +const double MAXDTOE_BDS = 21600.0; //!< max time difference to BeiDou Toe (s) +const double MAXDTOE_GLO = 1800.0; //!< max time difference to GLONASS Toe (s) +const double MAXDTOE_SBS = 360.0; //!< max time difference to SBAS Toe (s) +const double MAXDTOE_S = 86400.0; //!< max time difference to ephem toe (s) for other +const double MAXGDOP = 300.0; //!< max GDOP + +const int MAXSBSURA = 8; //!< max URA of SBAS satellite +const int MAXBAND = 10; //!< max SBAS band of IGP +const int MAXNIGP = 201; //!< max number of IGP in SBAS band +const int MAXNGEO = 4; //!< max number of GEO satellites + +const int MAXSOLMSG = 8191; //!< max length of solution message +const int MAXERRMSG = 4096; //!< max length of error/warning message + +const int IONOOPT_OFF = 0; //!< ionosphere option: correction off +const int IONOOPT_BRDC = 1; //!< ionosphere option: broadcast model +const int IONOOPT_SBAS = 2; //!< ionosphere option: SBAS model +const int IONOOPT_IFLC = 3; //!< ionosphere option: L1/L2 or L1/L5 iono-free LC +const int IONOOPT_EST = 4; //!< ionosphere option: estimation +const int IONOOPT_TEC = 5; //!< ionosphere option: IONEX TEC model +const int IONOOPT_QZS = 6; //!< ionosphere option: QZSS broadcast model +const int IONOOPT_LEX = 7; //!< ionosphere option: QZSS LEX ionospehre +const int IONOOPT_STEC = 8; //!< ionosphere option: SLANT TEC model + +const int TROPOPT_OFF = 0; //!< troposphere option: correction off +const int TROPOPT_SAAS = 1; //!< troposphere option: Saastamoinen model +const int TROPOPT_SBAS = 2; //!< troposphere option: SBAS model +const int TROPOPT_EST = 3; //!< troposphere option: ZTD estimation +const int TROPOPT_ESTG = 4; //!< troposphere option: ZTD+grad estimation +const int TROPOPT_COR = 5; //!< troposphere option: ZTD correction +const int TROPOPT_CORG = 6; //!< troposphere option: ZTD+grad correction + + +const int EPHOPT_BRDC = 0; //!< ephemeris option: broadcast ephemeris +const int EPHOPT_PREC = 1; //!< ephemeris option: precise ephemeris +const int EPHOPT_SBAS = 2; //!< ephemeris option: broadcast + SBAS +const int EPHOPT_SSRAPC = 3; //!< ephemeris option: broadcast + SSR_APC +const int EPHOPT_SSRCOM = 4; //!< ephemeris option: broadcast + SSR_COM +const int EPHOPT_LEX = 5; //!< ephemeris option: QZSS LEX ephemeris + +const double EFACT_GPS = 1.0; //!< error factor: GPS +const double EFACT_GLO = 1.5; //!< error factor: GLONASS +const double EFACT_GAL = 1.0; //!< error factor: Galileo +const double EFACT_QZS = 1.0; //!< error factor: QZSS +const double EFACT_BDS = 1.0; //!< error factor: BeiDou +const double EFACT_IRN = 1.5; //!< error factor: IRNSS +const double EFACT_SBS = 3.0; //!< error factor: SBAS + +const int MAXEXFILE = 1024; //!< max number of expanded files +const double MAXSBSAGEF = 30.0; //!< max age of SBAS fast correction (s) +const double MAXSBSAGEL = 1800.0; //!< max age of SBAS long term corr (s) + +const int ARMODE_OFF = 0; //!< AR mode: off +const int ARMODE_CONT = 1; //!< AR mode: continuous +const int ARMODE_INST = 2; //!< AR mode: instantaneous +const int ARMODE_FIXHOLD = 3; //!< AR mode: fix and hold +const int ARMODE_PPPAR = 4; //!< AR mode: PPP-AR +const int ARMODE_PPPAR_ILS = 5; //!< AR mode: AR mode: PPP-AR ILS +const int ARMODE_WLNL = 6; +const int ARMODE_TCAR = 7; + + +const int POSOPT_RINEX = 3; //!< pos option: rinex header pos */ + +typedef void fatalfunc_t(const char *); //!< fatal callback function type + + +#define NP_PPP(opt) ((opt)->dynamics?9:3) /* number of pos solution */ +#define IC_PPP(s,opt) (NP_PPP(opt)+(s)) /* state index of clocks (s=0:gps,1:glo) */ +#define IT_PPP(opt) (IC_PPP(0,opt)+NSYS) /* state index of tropos */ +#define NR_PPP(opt) (IT_PPP(opt)+((opt)->tropopttropopt==TROPOPT_EST?1:3))) /* number of solutions */ +#define IB_PPP(s,opt) (NR_PPP(opt)+(s)-1) /* state index of phase bias */ +#define NX_PPP(opt) (IB_PPP(MAXSAT,opt)+1) /* number of estimated states */ + + +typedef struct { /* time struct */ + time_t time; /* time (s) expressed by standard time_t */ + double sec; /* fraction of second under 1 s */ +} gtime_t; + + +typedef struct { /* observation data record */ + gtime_t time; /* receiver sampling time (GPST) */ + unsigned char sat,rcv; /* satellite/receiver number */ + unsigned char SNR [NFREQ+NEXOBS]; /* signal strength (0.25 dBHz) */ + unsigned char LLI [NFREQ+NEXOBS]; /* loss of lock indicator */ + unsigned char code[NFREQ+NEXOBS]; /* code indicator (CODE_???) */ + double L[NFREQ+NEXOBS]; /* observation data carrier-phase (cycle) */ + double P[NFREQ+NEXOBS]; /* observation data pseudorange (m) */ + float D[NFREQ+NEXOBS]; /* observation data doppler frequency (Hz) */ +} obsd_t; + + +typedef struct { /* observation data */ + int n,nmax; /* number of obervation data/allocated */ + obsd_t *data; /* observation data records */ +} obs_t; + + +typedef struct { /* earth rotation parameter data type */ + double mjd; /* mjd (days) */ + double xp,yp; /* pole offset (rad) */ + double xpr,ypr; /* pole offset rate (rad/day) */ + double ut1_utc; /* ut1-utc (s) */ + double lod; /* length of day (s/day) */ +} erpd_t; + + +typedef struct { /* earth rotation parameter type */ + int n,nmax; /* number and max number of data */ + erpd_t *data; /* earth rotation parameter data */ +} erp_t; + + +typedef struct { /* antenna parameter type */ + int sat; /* satellite number (0:receiver) */ + char type[MAXANT]; /* antenna type */ + char code[MAXANT]; /* serial number or satellite code */ + gtime_t ts,te; /* valid time start and end */ + double off[NFREQ][ 3]; /* phase center offset e/n/u or x/y/z (m) */ + double var[NFREQ][19]; /* phase center variation (m) */ + /* el=90,85,...,0 or nadir=0,1,2,3,... (deg) */ +} pcv_t; + + +typedef struct { /* antenna parameters type */ + int n,nmax; /* number of data/allocated */ + pcv_t *pcv; /* antenna parameters data */ +} pcvs_t; + + +typedef struct { /* almanac type */ + int sat; /* satellite number */ + int svh; /* sv health (0:ok) */ + int svconf; /* as and sv config */ + int week; /* GPS/QZS: gps week, GAL: galileo week */ + gtime_t toa; /* Toa */ + /* SV orbit parameters */ + double A,e,i0,OMG0,omg,M0,OMGd; + double toas; /* Toa (s) in week */ + double f0,f1; /* SV clock parameters (af0,af1) */ +} alm_t; + + +typedef struct { /* GPS/QZS/GAL broadcast ephemeris type */ + int sat; /* satellite number */ + int iode,iodc; /* IODE,IODC */ + int sva; /* SV accuracy (URA index) */ + int svh; /* SV health (0:ok) */ + int week; /* GPS/QZS: gps week, GAL: galileo week */ + int code; /* GPS/QZS: code on L2, GAL/BDS: data sources */ + int flag; /* GPS/QZS: L2 P data flag, BDS: nav type */ + gtime_t toe,toc,ttr; /* Toe,Toc,T_trans */ + /* SV orbit parameters */ + double A,e,i0,OMG0,omg,M0,deln,OMGd,idot; + double crc,crs,cuc,cus,cic,cis; + double toes; /* Toe (s) in week */ + double fit; /* fit interval (h) */ + double f0,f1,f2; /* SV clock parameters (af0,af1,af2) */ + double tgd[4]; /* group delay parameters */ + /* GPS/QZS:tgd[0]=TGD */ + /* GAL :tgd[0]=BGD E5a/E1,tgd[1]=BGD E5b/E1 */ + /* BDS :tgd[0]=BGD1,tgd[1]=BGD2 */ + double Adot,ndot; /* Adot,ndot for CNAV */ +} eph_t; + + +typedef struct { /* GLONASS broadcast ephemeris type */ + int sat; /* satellite number */ + int iode; /* IODE (0-6 bit of tb field) */ + int frq; /* satellite frequency number */ + int svh,sva,age; /* satellite health, accuracy, age of operation */ + gtime_t toe; /* epoch of epherides (gpst) */ + gtime_t tof; /* message frame time (gpst) */ + double pos[3]; /* satellite position (ecef) (m) */ + double vel[3]; /* satellite velocity (ecef) (m/s) */ + double acc[3]; /* satellite acceleration (ecef) (m/s^2) */ + double taun,gamn; /* SV clock bias (s)/relative freq bias */ + double dtaun; /* delay between L1 and L2 (s) */ +} geph_t; + + +typedef struct { /* precise ephemeris type */ + gtime_t time; /* time (GPST) */ + int index; /* ephemeris index for multiple files */ + double pos[MAXSAT][4]; /* satellite position/clock (ecef) (m|s) */ + float std[MAXSAT][4]; /* satellite position/clock std (m|s) */ + double vel[MAXSAT][4]; /* satellite velocity/clk-rate (m/s|s/s) */ + float vst[MAXSAT][4]; /* satellite velocity/clk-rate std (m/s|s/s) */ + float cov[MAXSAT][3]; /* satellite position covariance (m^2) */ + float vco[MAXSAT][3]; /* satellite velocity covariance (m^2) */ +} peph_t; + + +typedef struct { /* precise clock type */ + gtime_t time; /* time (GPST) */ + int index; /* clock index for multiple files */ + double clk[MAXSAT][1]; /* satellite clock (s) */ + float std[MAXSAT][1]; /* satellite clock std (s) */ +} pclk_t; + + +typedef struct { /* SBAS ephemeris type */ + int sat; /* satellite number */ + gtime_t t0; /* reference epoch time (GPST) */ + gtime_t tof; /* time of message frame (GPST) */ + int sva; /* SV accuracy (URA index) */ + int svh; /* SV health (0:ok) */ + double pos[3]; /* satellite position (m) (ecef) */ + double vel[3]; /* satellite velocity (m/s) (ecef) */ + double acc[3]; /* satellite acceleration (m/s^2) (ecef) */ + double af0,af1; /* satellite clock-offset/drift (s,s/s) */ +} seph_t; + + +typedef struct { /* norad two line element data type */ + char name [32]; /* common name */ + char alias[32]; /* alias name */ + char satno[16]; /* satellite catalog number */ + char satclass; /* classification */ + char desig[16]; /* international designator */ + gtime_t epoch; /* element set epoch (UTC) */ + double ndot; /* 1st derivative of mean motion */ + double nddot; /* 2st derivative of mean motion */ + double bstar; /* B* drag term */ + int etype; /* element set type */ + int eleno; /* element number */ + double inc; /* orbit inclination (deg) */ + double OMG; /* right ascension of ascending node (deg) */ + double ecc; /* eccentricity */ + double omg; /* argument of perigee (deg) */ + double M; /* mean anomaly (deg) */ + double n; /* mean motion (rev/day) */ + int rev; /* revolution number at epoch */ +} tled_t; + + +typedef struct { /* norad two line element type */ + int n,nmax; /* number/max number of two line element data */ + tled_t *data; /* norad two line element data */ +} tle_t; + + +typedef struct { /* TEC grid type */ + gtime_t time; /* epoch time (GPST) */ + int ndata[3]; /* TEC grid data size {nlat,nlon,nhgt} */ + double rb; /* earth radius (km) */ + double lats[3]; /* latitude start/interval (deg) */ + double lons[3]; /* longitude start/interval (deg) */ + double hgts[3]; /* heights start/interval (km) */ + double *data; /* TEC grid data (tecu) */ + float *rms; /* RMS values (tecu) */ +} tec_t; + + +typedef struct { /* satellite fcb data type */ + gtime_t ts,te; /* start/end time (GPST) */ + double bias[MAXSAT][3]; /* fcb value (cyc) */ + double std [MAXSAT][3]; /* fcb std-dev (cyc) */ +} fcbd_t; + + +typedef struct { /* SBAS message type */ + int week,tow; /* receiption time */ + int prn; /* SBAS satellite PRN number */ + unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */ +} sbsmsg_t; + + +typedef struct { /* SBAS messages type */ + int n,nmax; /* number of SBAS messages/allocated */ + sbsmsg_t *msgs; /* SBAS messages */ +} sbs_t; + + +typedef struct { /* SBAS fast correction type */ + gtime_t t0; /* time of applicability (TOF) */ + double prc; /* pseudorange correction (PRC) (m) */ + double rrc; /* range-rate correction (RRC) (m/s) */ + double dt; /* range-rate correction delta-time (s) */ + int iodf; /* IODF (issue of date fast corr) */ + short udre; /* UDRE+1 */ + short ai; /* degradation factor indicator */ +} sbsfcorr_t; + + +typedef struct { /* SBAS long term satellite error correction type */ + gtime_t t0; /* correction time */ + int iode; /* IODE (issue of date ephemeris) */ + double dpos[3]; /* delta position (m) (ecef) */ + double dvel[3]; /* delta velocity (m/s) (ecef) */ + double daf0,daf1; /* delta clock-offset/drift (s,s/s) */ +} sbslcorr_t; + + +typedef struct { /* SBAS satellite correction type */ + int sat; /* satellite number */ + sbsfcorr_t fcorr; /* fast correction */ + sbslcorr_t lcorr; /* long term correction */ +} sbssatp_t; + + +typedef struct { /* SBAS satellite corrections type */ + int iodp; /* IODP (issue of date mask) */ + int nsat; /* number of satellites */ + int tlat; /* system latency (s) */ + sbssatp_t sat[MAXSAT]; /* satellite correction */ +} sbssat_t; + + +typedef struct { /* SBAS ionospheric correction type */ + gtime_t t0; /* correction time */ + short lat,lon; /* latitude/longitude (deg) */ + short give; /* GIVI+1 */ + float delay; /* vertical delay estimate (m) */ +} sbsigp_t; + + +typedef struct { /* IGP band type */ + short x; /* longitude/latitude (deg) */ + const short *y; /* latitudes/longitudes (deg) */ + unsigned char bits; /* IGP mask start bit */ + unsigned char bite; /* IGP mask end bit */ +} sbsigpband_t; + + +typedef struct { /* SBAS ionospheric corrections type */ + int iodi; /* IODI (issue of date ionos corr) */ + int nigp; /* number of igps */ + sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ +} sbsion_t; + + +typedef struct { /* DGPS/GNSS correction type */ + gtime_t t0; /* correction time */ + double prc; /* pseudorange correction (PRC) (m) */ + double rrc; /* range rate correction (RRC) (m/s) */ + int iod; /* issue of data (IOD) */ + double udre; /* UDRE */ +} dgps_t; + + +typedef struct { /* SSR correction type */ + gtime_t t0[6]; /* epoch time (GPST) {eph,clk,hrclk,ura,bias,pbias} */ + double udi[6]; /* SSR update interval (s) */ + int iod[6]; /* iod ssr {eph,clk,hrclk,ura,bias,pbias} */ + int iode; /* issue of data */ + int iodcrc; /* issue of data crc for beidou/sbas */ + int ura; /* URA indicator */ + int refd; /* sat ref datum (0:ITRF,1:regional) */ + double deph [3]; /* delta orbit {radial,along,cross} (m) */ + double ddeph[3]; /* dot delta orbit {radial,along,cross} (m/s) */ + double dclk [3]; /* delta clock {c0,c1,c2} (m,m/s,m/s^2) */ + double hrclk; /* high-rate clock corection (m) */ + float cbias[MAXCODE]; /* code biases (m) */ + double pbias[MAXCODE]; /* phase biases (m) */ + float stdpb[MAXCODE]; /* std-dev of phase biases (m) */ + double yaw_ang,yaw_rate; /* yaw angle and yaw rate (deg,deg/s) */ + unsigned char update; /* update flag (0:no update,1:update) */ +} ssr_t; + + +typedef struct { /* QZSS LEX message type */ + int prn; /* satellite PRN number */ + int type; /* message type */ + int alert; /* alert flag */ + unsigned char stat; /* signal tracking status */ + unsigned char snr; /* signal C/N0 (0.25 dBHz) */ + unsigned int ttt; /* tracking time (ms) */ + unsigned char msg[212]; /* LEX message data part 1695 bits */ +} lexmsg_t; + + +typedef struct { /* QZSS LEX messages type */ + int n,nmax; /* number of LEX messages and allocated */ + lexmsg_t *msgs; /* LEX messages */ +} lex_t; + + +typedef struct { /* QZSS LEX ephemeris type */ + gtime_t toe; /* epoch time (GPST) */ + gtime_t tof; /* message frame time (GPST) */ + int sat; /* satellite number */ + unsigned char health; /* signal health (L1,L2,L1C,L5,LEX) */ + unsigned char ura; /* URA index */ + double pos[3]; /* satellite position (m) */ + double vel[3]; /* satellite velocity (m/s) */ + double acc[3]; /* satellite acceleration (m/s2) */ + double jerk[3]; /* satellite jerk (m/s3) */ + double af0,af1; /* satellite clock bias and drift (s,s/s) */ + double tgd; /* TGD */ + double isc[8]; /* ISC */ +} lexeph_t; + + +typedef struct { /* QZSS LEX ionosphere correction type */ + gtime_t t0; /* epoch time (GPST) */ + double tspan; /* valid time span (s) */ + double pos0[2]; /* reference position {lat,lon} (rad) */ + double coef[3][2]; /* coefficients lat x lon (3 x 2) */ +} lexion_t; + + +typedef struct { /* stec data type */ + gtime_t time; /* time (GPST) */ + unsigned char sat; /* satellite number */ + double ion; /* slant ionos delay (m) */ + float std; /* std-dev (m) */ + float azel[2]; /* azimuth/elevation (rad) */ + unsigned char flag; /* fix flag */ +} stec_t; + + +typedef struct { /* trop data type */ + gtime_t time; /* time (GPST) */ + double trp[3]; /* zenith tropos delay/gradient (m) */ + float std[3]; /* std-dev (m) */ +} trop_t; + + +typedef struct { /* ppp corrections type */ + int nsta; /* number of stations */ + char stas[MAXSTA][8]; /* station names */ + double rr[MAXSTA][3]; /* station ecef positions (m) */ + int ns[MAXSTA],nsmax[MAXSTA]; /* number of stec data */ + int nt[MAXSTA],ntmax[MAXSTA]; /* number of trop data */ + stec_t *stec[MAXSTA]; /* stec data */ + trop_t *trop[MAXSTA]; /* trop data */ +} pppcorr_t; + + +typedef struct { /* navigation data type */ + int n,nmax; /* number of broadcast ephemeris */ + int ng,ngmax; /* number of glonass ephemeris */ + int ns,nsmax; /* number of sbas ephemeris */ + int ne,nemax; /* number of precise ephemeris */ + int nc,ncmax; /* number of precise clock */ + int na,namax; /* number of almanac data */ + int nt,ntmax; /* number of tec grid data */ + int nf,nfmax; /* number of satellite fcb data */ + eph_t *eph; /* GPS/QZS/GAL ephemeris */ + geph_t *geph; /* GLONASS ephemeris */ + seph_t *seph; /* SBAS ephemeris */ + peph_t *peph; /* precise ephemeris */ + pclk_t *pclk; /* precise clock */ + alm_t *alm; /* almanac data */ + tec_t *tec; /* tec grid data */ + fcbd_t *fcb; /* satellite fcb data */ + erp_t erp; /* earth rotation parameters */ + double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */ + double utc_glo[4]; /* GLONASS UTC GPS time parameters */ + double utc_gal[4]; /* Galileo UTC GPS time parameters */ + double utc_qzs[4]; /* QZS UTC GPS time parameters */ + double utc_cmp[4]; /* BeiDou UTC parameters */ + double utc_irn[4]; /* IRNSS UTC parameters */ + double utc_sbs[4]; /* SBAS UTC parameters */ + double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */ + double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + double ion_irn[8]; /* IRNSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ + int leaps; /* leap seconds (s) */ + double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */ + double cbias[MAXSAT][3]; /* satellite dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */ + double rbias[MAXRCV][2][3]; /* receiver dcb (0:p1-p2,1:p1-c1,2:p2-c2) (m) */ + double wlbias[MAXSAT]; /* wide-lane bias (cycle) */ + double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */ + char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */ + pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ + sbssat_t sbssat; /* SBAS satellite corrections */ + sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */ + dgps_t dgps[MAXSAT]; /* DGPS corrections */ + ssr_t ssr[MAXSAT]; /* SSR corrections */ + lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */ + lexion_t lexion; /* LEX ionosphere correction */ + pppcorr_t pppcorr; /* ppp corrections */ +} nav_t; + + +typedef struct { /* station parameter type */ + char name [MAXANT]; /* marker name */ + char marker [MAXANT]; /* marker number */ + char antdes [MAXANT]; /* antenna descriptor */ + char antsno [MAXANT]; /* antenna serial number */ + char rectype[MAXANT]; /* receiver type descriptor */ + char recver [MAXANT]; /* receiver firmware version */ + char recsno [MAXANT]; /* receiver serial number */ + int antsetup; /* antenna setup id */ + int itrf; /* ITRF realization year */ + int deltype; /* antenna delta type (0:enu,1:xyz) */ + double pos[3]; /* station position (ecef) (m) */ + double del[3]; /* antenna position delta (e/n/u or x/y/z) (m) */ + double hgt; /* antenna height (m) */ +} sta_t; + + +typedef struct { /* solution type */ + gtime_t time; /* time (GPST) */ + double rr[6]; /* position/velocity (m|m/s) */ + /* {x,y,z,vx,vy,vz} or {e,n,u,ve,vn,vu} */ + float qr[6]; /* position variance/covariance (m^2) */ + /* {c_xx,c_yy,c_zz,c_xy,c_yz,c_zx} or */ + /* {c_ee,c_nn,c_uu,c_en,c_nu,c_ue} */ + double dtr[6]; /* receiver clock bias to time systems (s) */ + unsigned char type; /* type (0:xyz-ecef,1:enu-baseline) */ + unsigned char stat; /* solution status (SOLQ_???) */ + unsigned char ns; /* number of valid satellites */ + float age; /* age of differential (s) */ + float ratio; /* AR ratio factor for validation */ + float thres; /* AR ratio threshold for validation */ +} sol_t; + + +typedef struct { /* solution buffer type */ + int n,nmax; /* number of solution/max number of buffer */ + int cyclic; /* cyclic buffer flag */ + int start,end; /* start/end index */ + gtime_t time; /* current solution time */ + sol_t *data; /* solution data */ + double rb[3]; /* reference position {x,y,z} (ecef) (m) */ + unsigned char buff[MAXSOLMSG+1]; /* message buffer */ + int nb; /* number of byte in message buffer */ +} solbuf_t; + + +typedef struct { /* solution status type */ + gtime_t time; /* time (GPST) */ + unsigned char sat; /* satellite number */ + unsigned char frq; /* frequency (1:L1,2:L2,...) */ + float az,el; /* azimuth/elevation angle (rad) */ + float resp; /* pseudorange residual (m) */ + float resc; /* carrier-phase residual (m) */ + unsigned char flag; /* flags: (vsat<<5)+(slip<<3)+fix */ + unsigned char snr; /* signal strength (0.25 dBHz) */ + unsigned short lock; /* lock counter */ + unsigned short outc; /* outage counter */ + unsigned short slipc; /* slip counter */ + unsigned short rejc; /* reject counter */ +} solstat_t; + + +typedef struct { /* solution status buffer type */ + int n,nmax; /* number of solution/max number of buffer */ + solstat_t *data; /* solution status data */ +} solstatbuf_t; + + +typedef struct { /* RTCM control struct type */ + int staid; /* station id */ + int stah; /* station health */ + int seqno; /* sequence number for rtcm 2 or iods msm */ + int outtype; /* output message type */ + gtime_t time; /* message time */ + gtime_t time_s; /* message start time */ + obs_t obs; /* observation data (uncorrected) */ + nav_t nav; /* satellite ephemerides */ + sta_t sta; /* station parameters */ + dgps_t *dgps; /* output of dgps corrections */ + ssr_t ssr[MAXSAT]; /* output of ssr corrections */ + char msg[128]; /* special message */ + char msgtype[256]; /* last message type */ + char msmtype[6][128]; /* msm signal types */ + int obsflag; /* obs data complete flag (1:ok,0:not complete) */ + int ephsat; /* update satellite of ephemeris */ + double cp[MAXSAT][NFREQ+NEXOBS]; /* carrier-phase measurement */ + unsigned short lock[MAXSAT][NFREQ+NEXOBS]; /* lock time */ + unsigned short loss[MAXSAT][NFREQ+NEXOBS]; /* loss of lock count */ + gtime_t lltime[MAXSAT][NFREQ+NEXOBS]; /* last lock time */ + int nbyte; /* number of bytes in message buffer */ + int nbit; /* number of bits in word buffer */ + int len; /* message length (bytes) */ + unsigned char buff[1200]; /* message buffer */ + unsigned int word; /* word buffer for rtcm 2 */ + unsigned int nmsg2[100]; /* message count of RTCM 2 (1-99:1-99,0:other) */ + unsigned int nmsg3[400]; /* message count of RTCM 3 (1-299:1001-1299,300-399:2000-2099,0:ohter) */ + char opt[256]; /* RTCM dependent options */ +} rtcm_t; + + +typedef struct { /* download url type */ + char type[32]; /* data type */ + char path[1024]; /* url path */ + char dir [1024]; /* local directory */ + double tint; /* time interval (s) */ +} url_t; + + +typedef struct { /* option type */ + const char *name; /* option name */ + int format; /* option format (0:int,1:double,2:string,3:enum) */ + void *var; /* pointer to option variable */ + const char *comment; /* option comment/enum labels/unit */ +} opt_t; + + +typedef struct { /* extended receiver error model */ + int ena[4]; /* model enabled */ + double cerr[4][NFREQ*2]; /* code errors (m) */ + double perr[4][NFREQ*2]; /* carrier-phase errors (m) */ + double gpsglob[NFREQ]; /* gps-glonass h/w bias (m) */ + double gloicb [NFREQ]; /* glonass interchannel bias (m/fn) */ +} exterr_t; + + +typedef struct { /* SNR mask type */ + int ena[2]; /* enable flag {rover,base} */ + double mask[NFREQ][9]; /* mask (dBHz) at 5,10,...85 deg */ +} snrmask_t; + + +typedef struct { /* processing options type */ + int mode; /* positioning mode (PMODE_???) */ + int soltype; /* solution type (0:forward,1:backward,2:combined) */ + int nf; /* number of frequencies (1:L1,2:L1+L2,3:L1+L2+L5) */ + int navsys; /* navigation system */ + double elmin; /* elevation mask angle (rad) */ + snrmask_t snrmask; /* SNR mask */ + int sateph; /* satellite ephemeris/clock (EPHOPT_???) */ + int modear; /* AR mode (0:off,1:continuous,2:instantaneous,3:fix and hold,4:ppp-ar) */ + int glomodear; /* GLONASS AR mode (0:off,1:on,2:auto cal,3:ext cal) */ + int bdsmodear; /* BeiDou AR mode (0:off,1:on) */ + int maxout; /* obs outage count to reset bias */ + int minlock; /* min lock count to fix ambiguity */ + int minfix; /* min fix count to hold ambiguity */ + int armaxiter; /* max iteration to resolve ambiguity */ + int ionoopt; /* ionosphere option (IONOOPT_???) */ + int tropopt; /* troposphere option (TROPOPT_???) */ + int dynamics; /* dynamics model (0:none,1:velociy,2:accel) */ + int tidecorr; /* earth tide correction (0:off,1:solid,2:solid+otl+pole) */ + int niter; /* number of filter iteration */ + int codesmooth; /* code smoothing window size (0:none) */ + int intpref; /* interpolate reference obs (for post mission) */ + int sbascorr; /* SBAS correction options */ + int sbassatsel; /* SBAS satellite selection (0:all) */ + int rovpos; /* rover position for fixed mode */ + int refpos; /* base position for relative mode */ + /* (0:pos in prcopt, 1:average of single pos, */ + /* 2:read from file, 3:rinex header, 4:rtcm pos) */ + double eratio[NFREQ]; /* code/phase error ratio */ + double err[5]; /* measurement error factor */ + /* [0]:reserved */ + /* [1-3]:error factor a/b/c of phase (m) */ + /* [4]:doppler frequency (hz) */ + double std[3]; /* initial-state std [0]bias,[1]iono [2]trop */ + double prn[6]; /* process-noise std [0]bias,[1]iono [2]trop [3]acch [4]accv [5] pos */ + double sclkstab; /* satellite clock stability (sec/sec) */ + double thresar[8]; /* AR validation threshold */ + double elmaskar; /* elevation mask of AR for rising satellite (deg) */ + double elmaskhold; /* elevation mask to hold ambiguity (deg) */ + double thresslip; /* slip threshold of geometry-free phase (m) */ + double maxtdiff; /* max difference of time (sec) */ + double maxinno; /* reject threshold of innovation (m) */ + double maxgdop; /* reject threshold of gdop */ + double baseline[2]; /* baseline length constraint {const,sigma} (m) */ + double ru[3]; /* rover position for fixed mode {x,y,z} (ecef) (m) */ + double rb[3]; /* base position for relative mode {x,y,z} (ecef) (m) */ + char anttype[2][MAXANT]; /* antenna types {rover,base} */ + double antdel[2][3]; /* antenna delta {{rov_e,rov_n,rov_u},{ref_e,ref_n,ref_u}} */ + pcv_t pcvr[2]; /* receiver antenna parameters {rov,base} */ + unsigned char exsats[MAXSAT]; /* excluded satellites (1:excluded,2:included) */ + int maxaveep; /* max averaging epoches */ + int initrst; /* initialize by restart */ + int outsingle; /* output single by dgps/float/fix/ppp outage */ + char rnxopt[2][256]; /* rinex options {rover,base} */ + int posopt[6]; /* positioning options */ + int syncsol; /* solution sync mode (0:off,1:on) */ + double odisp[2][6*11]; /* ocean tide loading parameters {rov,base} */ + exterr_t exterr; /* extended receiver error model */ + int freqopt; /* disable L2-AR */ + char pppopt[256]; /* ppp option */ +} prcopt_t; + + +typedef struct { /* solution options type */ + int posf; /* solution format (SOLF_???) */ + int times; /* time system (TIMES_???) */ + int timef; /* time format (0:sssss.s,1:yyyy/mm/dd hh:mm:ss.s) */ + int timeu; /* time digits under decimal point */ + int degf; /* latitude/longitude format (0:ddd.ddd,1:ddd mm ss) */ + int outhead; /* output header (0:no,1:yes) */ + int outopt; /* output processing options (0:no,1:yes) */ + int datum; /* datum (0:WGS84,1:Tokyo) */ + int height; /* height (0:ellipsoidal,1:geodetic) */ + int geoid; /* geoid model (0:EGM96,1:JGD2000) */ + int solstatic; /* solution of static mode (0:all,1:single) */ + int sstat; /* solution statistics level (0:off,1:states,2:residuals) */ + int trace; /* debug trace level (0:off,1-5:debug) */ + double nmeaintv[2]; /* nmea output interval (s) (<0:no,0:all) */ + /* nmeaintv[0]:gprmc,gpgga,nmeaintv[1]:gpgsv */ + char sep[64]; /* field separator */ + char prog[64]; /* program name */ + double maxsolstd; /* max std-dev for solution output (m) (0:all) */ +} solopt_t; + + +typedef struct { /* satellite status type */ + unsigned char sys; /* navigation system */ + unsigned char vs; /* valid satellite flag single */ + double azel[2]; /* azimuth/elevation angles {az,el} (rad) */ + double resp[NFREQ]; /* residuals of pseudorange (m) */ + double resc[NFREQ]; /* residuals of carrier-phase (m) */ + unsigned char vsat[NFREQ]; /* valid satellite flag */ + unsigned char snr [NFREQ]; /* signal strength (0.25 dBHz) */ + unsigned char fix [NFREQ]; /* ambiguity fix flag (1:fix,2:float,3:hold) */ + unsigned char slip[NFREQ]; /* cycle-slip flag */ + unsigned char half[NFREQ]; /* half-cycle valid flag */ + int lock [NFREQ]; /* lock counter of phase */ + unsigned int outc [NFREQ]; /* obs outage counter of phase */ + unsigned int slipc[NFREQ]; /* cycle-slip counter */ + unsigned int rejc [NFREQ]; /* reject counter */ + double gf; /* geometry-free phase L1-L2 (m) */ + double gf2; /* geometry-free phase L1-L5 (m) */ + double mw; /* MW-LC (m) */ + double phw; /* phase windup (cycle) */ + gtime_t pt[2][NFREQ]; /* previous carrier-phase time */ + double ph[2][NFREQ]; /* previous carrier-phase observable (cycle) */ +} ssat_t; + + +typedef struct { /* ambiguity control type */ + gtime_t epoch[4]; /* last epoch */ + int n[4]; /* number of epochs */ + double LC [4]; /* linear combination average */ + double LCv[4]; /* linear combination variance */ + int fixcnt; /* fix count */ + char flags[MAXSAT]; /* fix flags */ +} ambc_t; + + +typedef struct { /* RTK control/result type */ + sol_t sol; /* RTK solution */ + double rb[6]; /* base position/velocity (ecef) (m|m/s) */ + int nx,na; /* number of float states/fixed states */ + double tt; /* time difference between current and previous (s) */ + double *x, *P; /* float states and their covariance */ + double *xa,*Pa; /* fixed states and their covariance */ + int nfix; /* number of continuous fixes of ambiguity */ + ambc_t ambc[MAXSAT]; /* ambiguity control */ + ssat_t ssat[MAXSAT]; /* satellite status */ + int neb; /* bytes in error message buffer */ + char errbuf[MAXERRMSG]; /* error message buffer */ + prcopt_t opt; /* processing options */ +} rtk_t; + + +typedef struct half_cyc_tag { /* half-cycle correction list type */ + unsigned char sat; /* satellite number */ + unsigned char freq; /* frequency number (0:L1,1:L2,2:L5) */ + unsigned char valid; /* half-cycle valid flag */ + char corr; /* half-cycle corrected (x 0.5 cyc) */ + gtime_t ts,te; /* time start, time end */ + struct half_cyc_tag *next; /* pointer to next correction */ +} half_cyc_t; + + +const double chisqr[100] = { /* chi-sqr(n) (alpha=0.001) */ + 10.8, 13.8, 16.3, 18.5, 20.5, 22.5, 24.3, 26.1, 27.9, 29.6, + 31.3, 32.9, 34.5, 36.1, 37.7, 39.3, 40.8, 42.3, 43.8, 45.3, + 46.8, 48.3, 49.7, 51.2, 52.6, 54.1, 55.5, 56.9, 58.3, 59.7, + 61.1, 62.5, 63.9, 65.2, 66.6, 68.0, 69.3, 70.7, 72.1, 73.4, + 74.7, 76.0, 77.3, 78.6, 80.0, 81.3, 82.6, 84.0, 85.4, 86.7, + 88.0, 89.3, 90.6, 91.9, 93.3, 94.7, 96.0, 97.4, 98.7, 100 , + 101 , 102 , 103 , 104 , 105 , 107 , 108 , 109 , 110 , 112 , + 113 , 114 , 115 , 116 , 118 , 119 , 120 , 122 , 123 , 125 , + 126 , 127 , 128 , 129 , 131 , 132 , 133 , 134 , 135 , 137 , + 138 , 139 , 140 , 142 , 143 , 144 , 145 , 147 , 148 , 149 +}; + + +const double lam_carr[MAXFREQ] = { /* carrier wave length (m) */ + SPEED_OF_LIGHT / FREQ1, SPEED_OF_LIGHT / FREQ2, SPEED_OF_LIGHT / FREQ5, SPEED_OF_LIGHT / FREQ6, SPEED_OF_LIGHT / FREQ7, + SPEED_OF_LIGHT / FREQ8, SPEED_OF_LIGHT / FREQ9 +}; + +#endif diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc new file mode 100644 index 000000000..c5e673b9c --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -0,0 +1,214 @@ +/*! + * \file rtklib_conversions.cc + * \brief GNSS-SDR to RTKLIB data structures conversion functions + * \author 2017, Javier Arribas + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "rtklib_conversions.h" +#include "rtklib_rtkcmn.h" + +obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synchro, int week, int band) +{ + rtklib_obs.D[band] = gnss_synchro.Carrier_Doppler_hz; + rtklib_obs.P[band] = gnss_synchro.Pseudorange_m; + rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / (2.0 * PI); + + double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz; + if (CN0_dB_Hz_est > 63.75) CN0_dB_Hz_est = 63.75; + if (CN0_dB_Hz_est < 0.0) CN0_dB_Hz_est = 0.0; + unsigned char CN0_dB_Hz = static_cast(std::round(CN0_dB_Hz_est / 0.25 )); + rtklib_obs.SNR[band] = CN0_dB_Hz; + //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris + switch(gnss_synchro.System) + { + case 'G': + rtklib_obs.sat = gnss_synchro.PRN; + break; + case 'E': + rtklib_obs.sat = gnss_synchro.PRN+NSATGPS+NSATGLO; + break; + default: + rtklib_obs.sat = gnss_synchro.PRN; + } + rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time); + rtklib_obs.rcv = 1; + //printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec); + return rtklib_obs; +} + +eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph) +{ + eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + //Galileo is the third satellite system for RTKLIB, so, add the required offset to discriminate Galileo ephemeris + rtklib_sat.sat = gal_eph.i_satellite_PRN+NSATGPS+NSATGLO; + rtklib_sat.A = gal_eph.A_1 * gal_eph.A_1; + rtklib_sat.M0 = gal_eph.M0_1; + rtklib_sat.deln = gal_eph.delta_n_3; + rtklib_sat.OMG0 = gal_eph.OMEGA_0_2; + rtklib_sat.OMGd = gal_eph.OMEGA_dot_3; + rtklib_sat.omg = gal_eph.omega_2; + rtklib_sat.i0 = gal_eph.i_0_2; + rtklib_sat.idot = gal_eph.iDot_2; + rtklib_sat.e = gal_eph.e_1; + rtklib_sat.Adot = 0; //only in CNAV; + rtklib_sat.ndot = 0; //only in CNAV; + + rtklib_sat.week = adjgpsweek(gal_eph.WN_5); /* week of tow */ + rtklib_sat.cic = gal_eph.C_ic_4; + rtklib_sat.cis = gal_eph.C_is_4; + rtklib_sat.cuc = gal_eph.C_uc_3; + rtklib_sat.cus = gal_eph.C_us_3; + rtklib_sat.crc = gal_eph.C_rc_3; + rtklib_sat.crs = gal_eph.C_rs_3; + rtklib_sat.f0 = gal_eph.af0_4; + rtklib_sat.f1 = gal_eph.af1_4; + rtklib_sat.f2 = gal_eph.af2_4; + rtklib_sat.tgd[0] = 0; + rtklib_sat.tgd[1] = 0; + rtklib_sat.tgd[2] = 0; + rtklib_sat.tgd[3] = 0; + rtklib_sat.toes = gal_eph.t0e_1; + rtklib_sat.toc = gpst2time(rtklib_sat.week, gal_eph.t0c_4); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, gal_eph.TOW_5); + + /* adjustment for week handover */ + double tow, toc; + tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc, NULL); + if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;} + else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;} + rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes); + rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); + + return rtklib_sat; +} + + +eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph) +{ + eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + rtklib_sat.sat = gps_eph.i_satellite_PRN; + rtklib_sat.A = gps_eph.d_sqrt_A * gps_eph.d_sqrt_A; + rtklib_sat.M0 = gps_eph.d_M_0; + rtklib_sat.deln = gps_eph.d_Delta_n; + rtklib_sat.OMG0 = gps_eph.d_OMEGA0; + rtklib_sat.OMGd = gps_eph.d_OMEGA_DOT; + rtklib_sat.omg = gps_eph.d_OMEGA; + rtklib_sat.i0 = gps_eph.d_i_0; + rtklib_sat.idot = gps_eph.d_IDOT; + rtklib_sat.e = gps_eph.d_e_eccentricity; + rtklib_sat.Adot = 0; //only in CNAV; + rtklib_sat.ndot = 0; //only in CNAV; + + rtklib_sat.week = adjgpsweek(gps_eph.i_GPS_week); /* week of tow */ + rtklib_sat.cic = gps_eph.d_Cic; + rtklib_sat.cis = gps_eph.d_Cis; + rtklib_sat.cuc = gps_eph.d_Cuc; + rtklib_sat.cus = gps_eph.d_Cus; + rtklib_sat.crc = gps_eph.d_Crc; + rtklib_sat.crs = gps_eph.d_Crs; + rtklib_sat.f0 = gps_eph.d_A_f0; + rtklib_sat.f1 = gps_eph.d_A_f1; + rtklib_sat.f2 = gps_eph.d_A_f2; + rtklib_sat.tgd[0] = gps_eph.d_TGD; + rtklib_sat.tgd[1] = 0; + rtklib_sat.tgd[2] = 0; + rtklib_sat.tgd[3] = 0; + rtklib_sat.toes = gps_eph.d_Toe; + rtklib_sat.toc = gpst2time(rtklib_sat.week, gps_eph.d_Toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, gps_eph.d_TOW); + + /* adjustment for week handover */ + double tow, toc; + tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc, NULL); + if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;} + else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;} + rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes); + rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); + + //printf("EPHEMERIS TIME [%i]: %s,%f\n\r",rtklib_sat.sat,time_str(rtklib_sat.toe,3),rtklib_sat.toe.sec); + + return rtklib_sat; +} + + +eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph) +{ + eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + rtklib_sat.sat = gps_cnav_eph.i_satellite_PRN; + const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 + rtklib_sat.A = A_REF + gps_cnav_eph.d_DELTA_A; + rtklib_sat.M0 = gps_cnav_eph.d_M_0; + rtklib_sat.deln = gps_cnav_eph.d_Delta_n; + rtklib_sat.OMG0 = gps_cnav_eph.d_OMEGA0; + // Compute the angle between the ascending node and the Greenwich meridian + const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200H pp. 164 + double d_OMEGA_DOT = OMEGA_DOT_REF * GPS_L2_PI + gps_cnav_eph.d_DELTA_OMEGA_DOT; + rtklib_sat.OMGd = d_OMEGA_DOT; + rtklib_sat.omg = gps_cnav_eph.d_OMEGA; + rtklib_sat.i0 = gps_cnav_eph.d_i_0; + rtklib_sat.idot = gps_cnav_eph.d_IDOT; + rtklib_sat.e = gps_cnav_eph.d_e_eccentricity; + rtklib_sat.Adot = gps_cnav_eph.d_A_DOT; //only in CNAV; + rtklib_sat.ndot = gps_cnav_eph.d_DELTA_DOT_N; //only in CNAV; + + rtklib_sat.week = adjgpsweek(gps_cnav_eph.i_GPS_week); /* week of tow */ + rtklib_sat.cic = gps_cnav_eph.d_Cic; + rtklib_sat.cis = gps_cnav_eph.d_Cis; + rtklib_sat.cuc = gps_cnav_eph.d_Cuc; + rtklib_sat.cus = gps_cnav_eph.d_Cus; + rtklib_sat.crc = gps_cnav_eph.d_Crc; + rtklib_sat.crs = gps_cnav_eph.d_Crs; + rtklib_sat.f0 = gps_cnav_eph.d_A_f0; + rtklib_sat.f1 = gps_cnav_eph.d_A_f1; + rtklib_sat.f2 = gps_cnav_eph.d_A_f2; + rtklib_sat.tgd[0] = gps_cnav_eph.d_TGD; + rtklib_sat.tgd[1] = 0; + rtklib_sat.tgd[2] = 0; + rtklib_sat.tgd[3] = 0; + rtklib_sat.toes = gps_cnav_eph.d_Toe1; + rtklib_sat.toc = gpst2time(rtklib_sat.week,gps_cnav_eph.d_Toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week,gps_cnav_eph.d_TOW); + + /* adjustment for week handover */ + double tow, toc; + tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week); + toc = time2gpst(rtklib_sat.toc, NULL); + if (rtklib_sat.toes < tow - 302400.0) {rtklib_sat.week++; tow -= 604800.0;} + else if (rtklib_sat.toes > tow + 302400.0) {rtklib_sat.week--; tow += 604800.0;} + rtklib_sat.toe = gpst2time(rtklib_sat.week, rtklib_sat.toes); + rtklib_sat.toc = gpst2time(rtklib_sat.week, toc); + rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow); + + return rtklib_sat; +} diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.h b/src/algorithms/libs/rtklib/rtklib_conversions.h new file mode 100644 index 000000000..18336754f --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_conversions.h @@ -0,0 +1,46 @@ +/*! + * \file rtklib_conversions.h + * \brief GNSS-SDR to RTKLIB data structures conversion functions + * \author 2017, Javier Arribas + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_RTKLIB_CONVERSIONS_H_ +#define GNSS_SDR_RTKLIB_CONVERSIONS_H_ + +#include "rtklib.h" +#include "gnss_synchro.h" +#include "galileo_ephemeris.h" +#include "gps_ephemeris.h" +#include "gps_cnav_ephemeris.h" + +eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph); +eph_t eph_to_rtklib(const Gps_Ephemeris & gps_eph); +eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris & gps_cnav_eph); + +obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synchro, int week, int band); + +#endif /* GNSS_SDR_RTKLIB_CONVERSIONS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc new file mode 100644 index 000000000..07954f822 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -0,0 +1,872 @@ +/*! + * \file rtklib_ephemeris.cc + * \brief satellite ephemeris and clock functions + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_ephemeris.h" +#include "rtklib_rtkcmn.h" +#include "rtklib_sbas.h" +#include "rtklib_preceph.h" + +/* constants ------------------------------------------------------*/ + +const double RE_GLO = 6378136.0; /* radius of earth (m) ref [2] */ +const double MU_GPS = 3.9860050e14; /* gravitational constant ref [1] */ +const double MU_GLO = 3.9860044e14; /* gravitational constant ref [2] */ +const double MU_GAL = 3.986004418e14; /* earth gravitational constant ref [7] */ +const double MU_BDS = 3.986004418e14; /* earth gravitational constant ref [9] */ +const double J2_GLO = 1.0826257e-3; /* 2nd zonal harmonic of geopot ref [2] */ + +const double OMGE_GLO = 7.292115e-5; /* earth angular velocity (rad/s) ref [2] */ +const double OMGE_GAL = 7.2921151467e-5; /* earth angular velocity (rad/s) ref [7] */ +const double OMGE_BDS = 7.292115e-5; /* earth angular velocity (rad/s) ref [9] */ + +const double SIN_5 = -0.0871557427476582; /* sin(-5.0 deg) */ +const double COS_5 = 0.9961946980917456; /* cos(-5.0 deg) */ + +const double ERREPH_GLO = 5.0; /* error of glonass ephemeris (m) */ +const double TSTEP = 60.0; /* integration step glonass ephemeris (s) */ +const double RTOL_KEPLER = 1e-13; /* relative tolerance for Kepler equation */ + +const double DEFURASSR = 0.15; /* default accurary of ssr corr (m) */ +const double MAXECORSSR = 10.0; /* max orbit correction of ssr (m) */ +const double MAXCCORSSR = 1e-6 * SPEED_OF_LIGHT; /* max clock correction of ssr (m) */ +const double MAXAGESSR = 90.0; /* max age of ssr orbit and clock (s) */ +const double MAXAGESSR_HRCLK = 10.0; /* max age of ssr high-rate clock (s) */ +const double STD_BRDCCLK = 30.0; /* error of broadcast clock (m) */ + +const int MAX_ITER_KEPLER = 30; /* max number of iteration of Kelpler */ + + +/* variance by ura ephemeris (ref [1] 20.3.3.3.1.1) --------------------------*/ +double var_uraeph(int ura) +{ + const double ura_value[] = { + 2.4, 3.4, 4.85, 6.85, 9.65, 13.65, 24.0, 48.0, 96.0, 192.0, 384.0, 768.0, 1536.0, + 3072.0, 6144.0 + }; + return ura < 0 || 15 < ura ? std::pow(6144.0, 2.0) : std::pow(ura_value[ura], 2.0); +} + + +/* variance by ura ssr (ref [4]) ---------------------------------------------*/ +double var_urassr(int ura) +{ + double std_; + if (ura <= 0) return std::pow(DEFURASSR, 2.0); + if (ura >= 63) return std::pow(5.4665, 2.0); + std_ = (std::pow((ura >> 3) & 7, 2.0) * (1.0 + (ura & 7) / 4.0) - 1.0) * 1e-3; + return std::pow(std_, 2.0); +} + + +/* almanac to satellite position and clock bias -------------------------------- + * compute satellite position and clock bias with almanac (gps, galileo, qzss) + * args : gtime_t time I time (gpst) + * alm_t *alm I almanac + * double *rs O satellite position (ecef) {x,y,z} (m) + * double *dts O satellite clock bias (s) + * return : none + * notes : see ref [1],[7],[8] + *-----------------------------------------------------------------------------*/ +void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts) +{ + double tk, M, E, Ek, sinE, cosE, u, r, i, O, x, y, sinO, cosO, cosi, mu; + int n; + + trace(4, "alm2pos : time=%s sat=%2d\n", time_str(time, 3), alm->sat); + + tk = timediff(time, alm->toa); + + if (alm->A <= 0.0) + { + rs[0] = rs[1] = rs[2] = *dts = 0.0; + return; + } + mu = satsys(alm->sat, NULL) == SYS_GAL ? MU_GAL : MU_GPS; + + M = alm->M0 + sqrt(mu / (alm->A * alm->A * alm->A)) * tk; + for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++) + { + Ek = E; + E -= (E - alm->e * sin(E) - M) / (1.0 - alm->e * cos(E)); + } + if (n >= MAX_ITER_KEPLER) + { + trace(2, "alm2pos: kepler iteration overflow sat=%2d\n", alm->sat); + return; + } + sinE = sin(E); + cosE = cos(E); + u = atan2(sqrt(1.0 - alm->e * alm->e) * sinE, cosE - alm->e) + alm->omg; + r = alm->A * (1.0 - alm->e*cosE); + i = alm->i0; + O = alm->OMG0 + (alm->OMGd - DEFAULT_OMEGA_EARTH_DOT) * tk - DEFAULT_OMEGA_EARTH_DOT * alm->toas; + x = r * cos(u); + y = r * sin(u); + sinO = sin(O); + cosO = cos(O); + cosi = cos(i); + rs[0] = x * cosO - y * cosi * sinO; + rs[1] = x * sinO + y * cosi * cosO; + rs[2] = y * sin(i); + *dts = alm->f0 + alm->f1 * tk; +} + + +/* broadcast ephemeris to satellite clock bias --------------------------------- + * compute satellite clock bias with broadcast ephemeris (gps, galileo, qzss) + * args : gtime_t time I time by satellite clock (gpst) + * eph_t *eph I broadcast ephemeris + * return : satellite clock bias (s) without relativeity correction + * notes : see ref [1],[7],[8] + * satellite clock does not include relativity correction and tdg + *-----------------------------------------------------------------------------*/ +double eph2clk(gtime_t time, const eph_t *eph) +{ + double t; + int i; + + trace(4, "eph2clk : time=%s sat=%2d\n", time_str(time, 3), eph->sat); + + t = timediff(time, eph->toc); + + for (i = 0; i < 2; i++) + { + t -= eph->f0 + eph->f1 * t + eph->f2 * t * t; + } + return eph->f0 + eph->f1 * t + eph->f2 * t *t; +} + + +/* broadcast ephemeris to satellite position and clock bias -------------------- + * compute satellite position and clock bias with broadcast ephemeris (gps, + * galileo, qzss) + * args : gtime_t time I time (gpst) + * eph_t *eph I broadcast ephemeris + * double *rs O satellite position (ecef) {x,y,z} (m) + * double *dts O satellite clock bias (s) + * double *var O satellite position and clock variance (m^2) + * return : none + * notes : see ref [1],[7],[8] + * satellite clock includes relativity correction without code bias + * (tgd or bgd) + *-----------------------------------------------------------------------------*/ +void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, + double *var) +{ + double tk, M, E, Ek, sinE, cosE, u, r, i, O, sin2u, cos2u, x, y, sinO, cosO, cosi, mu, omge; + double xg, yg, zg, sino, coso; + int n, sys, prn; + + trace(4, "eph2pos : time=%s sat=%2d\n", time_str(time, 3), eph->sat); + + if (eph->A <= 0.0) + { + rs[0] = rs[1] = rs[2] = *dts = *var = 0.0; + return; + } + tk = timediff(time , eph->toe); + + switch ((sys = satsys(eph->sat, &prn))) + { + case SYS_GAL: mu = MU_GAL; omge = OMGE_GAL; break; + case SYS_BDS: mu = MU_BDS; omge = OMGE_BDS; break; + default: mu = MU_GPS; omge = DEFAULT_OMEGA_EARTH_DOT; break; + } + M = eph->M0 + (sqrt(mu / (eph->A * eph->A * eph->A)) + eph->deln) * tk; + + for (n = 0, E = M, Ek = 0.0; fabs(E - Ek) > RTOL_KEPLER && n < MAX_ITER_KEPLER; n++) + { + Ek = E; + E -= (E - eph->e * sin(E) - M) / (1.0 - eph->e * cos(E)); + } + if (n >= MAX_ITER_KEPLER) + { + trace(2, "eph2pos: kepler iteration overflow sat=%2d\n", eph->sat); + return; + } + sinE = sin(E); + cosE = cos(E); + + trace(4, "kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n", eph->sat, eph->e, n, E - Ek); + + u = atan2(sqrt(1.0 - eph->e*eph->e) * sinE, cosE-eph->e) + eph->omg; + r = eph->A * (1.0 - eph->e * cosE); + i = eph->i0 + eph->idot * tk; + sin2u = sin(2.0 * u); cos2u = cos(2.0 * u); + u += eph->cus * sin2u + eph->cuc * cos2u; + r += eph->crs * sin2u + eph->crc * cos2u; + i += eph->cis * sin2u + eph->cic * cos2u; + x = r * cos(u); + y = r * sin(u); + cosi = cos(i); + + /* beidou geo satellite (ref [9]) */ + if (sys == SYS_BDS && prn <= 5) + { + O = eph->OMG0 + eph->OMGd * tk - omge * eph->toes; + sinO = sin(O); + cosO = cos(O); + xg = x * cosO - y * cosi * sinO; + yg = x * sinO + y * cosi * cosO; + zg = y * sin(i); + sino = sin(omge * tk); + coso = cos(omge * tk); + rs[0] = xg * coso + yg * sino * COS_5 + zg * sino * SIN_5; + rs[1] = -xg * sino + yg * coso * COS_5 + zg * coso * SIN_5; + rs[2] = -yg * SIN_5 + zg * COS_5; + } + else + { + O = eph->OMG0 + (eph->OMGd - omge) * tk - omge * eph->toes; + sinO = sin(O); + cosO = cos(O); + rs[0] = x * cosO - y * cosi * sinO; + rs[1] = x * sinO + y *cosi * cosO; + rs[2] = y * sin(i); + } + tk = timediff(time, eph->toc); + *dts = eph->f0 + eph->f1 * tk + eph->f2 * tk * tk; + + /* relativity correction */ + *dts -= 2.0 * sqrt(mu * eph->A) * eph-> e* sinE / std::pow(SPEED_OF_LIGHT, 2.0); + + /* position and clock error variance */ + *var = var_uraeph(eph->sva); +} + + +/* glonass orbit differential equations --------------------------------------*/ +void deq(const double *x, double *xdot, const double *acc) +{ + double a, b, c, r2 = dot(x, x, 3), r3 = r2 * sqrt(r2), omg2 = std::pow(OMGE_GLO, 2.0); + + if (r2 <= 0.0) + { + xdot[0] = xdot[1] = xdot[2] = xdot[3] = xdot[4] = xdot[5] = 0.0; + return; + } + /* ref [2] A.3.1.2 with bug fix for xdot[4],xdot[5] */ + a = 1.5 * J2_GLO * MU_GLO * std::pow(RE_GLO, 2.0) / r2 / r3; /* 3/2*J2*mu*Ae^2/r^5 */ + b = 5.0 * x[2] * x[2] / r2; /* 5*z^2/r^2 */ + c = -MU_GLO / r3 - a * (1.0 - b); /* -mu/r^3-a(1-b) */ + xdot[0] = x[3]; + xdot[1] = x[4]; xdot[2] = x[5]; + xdot[3] = (c + omg2) * x[0] + 2.0 * OMGE_GLO * x[4] + acc[0]; + xdot[4] = (c + omg2) * x[1] - 2.0 * OMGE_GLO * x[3] + acc[1]; + xdot[5] = (c - 2.0 * a) * x[2] + acc[2]; +} + + +/* glonass position and velocity by numerical integration --------------------*/ +void glorbit(double t, double *x, const double *acc) +{ + double k1[6], k2[6], k3[6], k4[6], w[6]; + int i; + + deq(x, k1, acc); for (i = 0; i< 6; i++) w[i] = x[i] + k1[i] * t / 2.0; + deq(w, k2, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k2[i] * t / 2.0; + deq(w, k3, acc); for (i = 0; i < 6; i++) w[i] = x[i] + k3[i] * t; + deq(w, k4, acc); + for (i = 0; i < 6; i++) x[i] += (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]) * t / 6.0; +} + + +/* glonass ephemeris to satellite clock bias ----------------------------------- + * compute satellite clock bias with glonass ephemeris + * args : gtime_t time I time by satellite clock (gpst) + * geph_t *geph I glonass ephemeris + * return : satellite clock bias (s) + * notes : see ref [2] + *-----------------------------------------------------------------------------*/ +double geph2clk(gtime_t time, const geph_t *geph) +{ + double t; + int i; + + trace(4, "geph2clk: time=%s sat=%2d\n", time_str(time, 3), geph->sat); + + t = timediff(time, geph->toe); + + for (i = 0; i < 2; i++) + { + t -= -geph->taun + geph->gamn * t; + } + return -geph->taun + geph->gamn * t; +} + + +/* glonass ephemeris to satellite position and clock bias ---------------------- + * compute satellite position and clock bias with glonass ephemeris + * args : gtime_t time I time (gpst) + * geph_t *geph I glonass ephemeris + * double *rs O satellite position {x,y,z} (ecef) (m) + * double *dts O satellite clock bias (s) + * double *var O satellite position and clock variance (m^2) + * return : none + * notes : see ref [2] + *-----------------------------------------------------------------------------*/ +void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts, + double *var) +{ + double t, tt, x[6]; + int i; + + trace(4, "geph2pos: time=%s sat=%2d\n", time_str(time, 3), geph->sat); + + t = timediff(time, geph->toe); + + *dts = -geph->taun + geph->gamn * t; + + for (i = 0; i < 3; i++) + { + x[i ] = geph->pos[i]; + x[i+3] = geph->vel[i]; + } + for (tt = t < 0.0 ? - TSTEP : TSTEP; fabs(t) > 1e-9; t -= tt) + { + if (fabs(t) < TSTEP) tt = t; + glorbit(tt, x, geph->acc); + } + for (i = 0; i < 3; i++) rs[i] = x[i]; + + *var = std::pow(ERREPH_GLO, 2.0); +} + + +/* sbas ephemeris to satellite clock bias -------------------------------------- + * compute satellite clock bias with sbas ephemeris + * args : gtime_t time I time by satellite clock (gpst) + * seph_t *seph I sbas ephemeris + * return : satellite clock bias (s) + * notes : see ref [3] + *-----------------------------------------------------------------------------*/ +double seph2clk(gtime_t time, const seph_t *seph) +{ + double t; + int i; + + trace(4, "seph2clk: time=%s sat=%2d\n", time_str(time, 3), seph->sat); + + t = timediff(time, seph->t0); + + for (i = 0; i < 2; i++) + { + t-=seph->af0 + seph->af1 * t; + } + return seph->af0 + seph->af1 * t; +} + + +/* sbas ephemeris to satellite position and clock bias ------------------------- + * compute satellite position and clock bias with sbas ephemeris + * args : gtime_t time I time (gpst) + * seph_t *seph I sbas ephemeris + * double *rs O satellite position {x,y,z} (ecef) (m) + * double *dts O satellite clock bias (s) + * double *var O satellite position and clock variance (m^2) + * return : none + * notes : see ref [3] + *-----------------------------------------------------------------------------*/ +void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts, + double *var) +{ + double t; + int i; + + trace(4, "seph2pos: time=%s sat=%2d\n", time_str(time, 3), seph->sat); + + t = timediff(time, seph->t0); + + for (i = 0; i < 3; i++) + { + rs[i] = seph->pos[i] + seph->vel[i] * t + seph->acc[i] * t * t / 2.0; + } + *dts = seph->af0 + seph->af1 * t; + + *var = var_uraeph(seph->sva); +} + + +/* select ephememeris --------------------------------------------------------*/ +eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav) +{ + double t, tmax, tmin; + int i, j = -1; + + trace(4, "seleph : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode); + + switch (satsys(sat, NULL)) + { + case SYS_QZS: tmax = MAXDTOE_QZS + 1.0; break; + case SYS_GAL: tmax = MAXDTOE_GAL + 1.0; break; + case SYS_BDS: tmax = MAXDTOE_BDS + 1.0; break; + default: tmax = MAXDTOE + 1.0; break; + } + tmin = tmax + 1.0; + + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + if (iode >= 0 && nav->eph[i].iode != iode) continue; + if ((t = fabs(timediff(nav->eph[i].toe, time))) > tmax) continue; + if (iode >= 0) return nav->eph + i; + if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */ + } + if (iode >= 0 || j<0) + { + trace(3, "no broadcast ephemeris: %s sat=%2d iode=%3d\n", time_str(time, 0), + sat, iode); + return NULL; + } + return nav->eph + j; +} + + +/* select glonass ephememeris ------------------------------------------------*/ +geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav) +{ + double t, tmax = MAXDTOE_GLO, tmin = tmax + 1.0; + int i, j = -1; + + trace(4, "selgeph : time=%s sat=%2d iode=%2d\n", time_str(time, 3), sat, iode); + + for (i = 0; i < nav->ng; i++) + { + if (nav->geph[i].sat != sat) continue; + if (iode >= 0 && nav->geph[i].iode != iode) continue; + if ((t = fabs(timediff(nav->geph[i].toe, time))) > tmax) continue; + if (iode >= 0) return nav->geph + i; + if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */ + } + if (iode >= 0 || j < 0) + { + trace(3, "no glonass ephemeris : %s sat=%2d iode=%2d\n", time_str(time, 0), + sat, iode); + return NULL; + } + return nav->geph + j; +} + + +/* select sbas ephememeris ---------------------------------------------------*/ +seph_t *selseph(gtime_t time, int sat, const nav_t *nav) +{ + double t, tmax = MAXDTOE_SBS, tmin = tmax + 1.0; + int i, j = -1; + + trace(4, "selseph : time=%s sat=%2d\n", time_str(time, 3), sat); + + for (i = 0; i < nav->ns; i++) + { + if (nav->seph[i].sat != sat) continue; + if ((t = fabs(timediff(nav->seph[i].t0, time))) > tmax) continue; + if (t <= tmin) {j = i; tmin = t;} /* toe closest to time */ + } + if (j < 0) + { + trace(3, "no sbas ephemeris : %s sat=%2d\n", time_str(time, 0), sat); + return NULL; + } + return nav->seph + j; +} + + +/* satellite clock with broadcast ephemeris ----------------------------------*/ +int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + double *dts) +{ + eph_t *eph; + geph_t *geph; + seph_t *seph; + int sys; + + trace(4, "ephclk : time=%s sat=%2d\n", time_str(time, 3), sat); + + sys = satsys(sat, NULL); + + if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) + { + if (!(eph = seleph(teph, sat, -1, nav))) return 0; + *dts = eph2clk(time, eph); + } + else if (sys == SYS_GLO) + { + if (!(geph = selgeph(teph, sat, -1, nav))) return 0; + *dts = geph2clk(time, geph); + } + else if (sys == SYS_SBS) + { + if (!(seph = selseph(teph, sat, nav))) return 0; + *dts = seph2clk(time, seph); + } + else return 0; + + return 1; +} + + +/* satellite position and clock by broadcast ephemeris -----------------------*/ +int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + int iode, double *rs, double *dts, double *var, int *svh) +{ + eph_t *eph; + geph_t *geph; + seph_t *seph; + double rst[3], dtst[1], tt = 1e-3; + int i, sys; + + trace(4, "ephpos : time=%s sat=%2d iode=%d\n", time_str(time, 3), sat, iode); + + sys = satsys(sat, NULL); + + *svh = -1; + + if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) + { + if (!(eph = seleph(teph, sat, iode, nav))) return 0; + + eph2pos(time, eph, rs, dts, var); + time = timeadd(time, tt); + eph2pos(time, eph, rst, dtst, var); + *svh = eph->svh; + } + else if (sys == SYS_GLO) + { + if (!(geph = selgeph(teph, sat, iode, nav))) return 0; + geph2pos(time, geph, rs, dts, var); + time = timeadd(time, tt); + geph2pos(time, geph, rst, dtst, var); + *svh = geph->svh; + } + else if (sys == SYS_SBS) + { + if (!(seph = selseph(teph, sat, nav))) return 0; + + seph2pos(time, seph, rs, dts, var); + time = timeadd(time, tt); + seph2pos(time, seph, rst, dtst, var); + *svh = seph->svh; + } + else return 0; + + /* satellite velocity and clock drift by differential approx */ + for (i = 0; i < 3; i++) rs[i + 3] = (rst[i] - rs[i]) / tt; + dts[1] = (dtst[0] - dts[0]) / tt; + + return 1; +} + + +/* satellite position and clock with sbas correction -------------------------*/ +int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + double *rs, double *dts, double *var, int *svh) +{ + const sbssatp_t *sbs; + int i; + + trace(4, "satpos_sbas: time=%s sat=%2d\n", time_str(time, 3), sat); + + /* search sbas satellite correciton */ + for (i = 0; i < nav->sbssat.nsat; i++) + { + sbs = nav->sbssat.sat + i; + if (sbs->sat == sat) break; + } + if (i >= nav->sbssat.nsat) + { + trace(2, "no sbas correction for orbit: %s sat=%2d\n", time_str(time, 0), sat); + ephpos(time, teph, sat, nav, -1, rs, dts, var, svh); + *svh = -1; + return 0; + } + /* satellite postion and clock by broadcast ephemeris */ + if (!ephpos(time, teph, sat, nav, sbs->lcorr.iode, rs, dts, var, svh)) return 0; + + /* sbas satellite correction (long term and fast) */ + if (sbssatcorr(time, sat, nav, rs, dts, var)) return 1; + *svh = -1; + return 0; +} + + +/* satellite position and clock with ssr correction --------------------------*/ +int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + int opt, double *rs, double *dts, double *var, int *svh) +{ + const ssr_t *ssr; + eph_t *eph; + double t1, t2, t3, er[3], ea[3], ec[3], rc[3], deph[3], dclk, dant[3] = {0}, tk; + int i, sys; + + trace(4, "satpos_ssr: time=%s sat=%2d\n", time_str(time, 3), sat); + + ssr = nav->ssr + sat - 1; + + if (!ssr->t0[0].time) + { + trace(2, "no ssr orbit correction: %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } + if (!ssr->t0[1].time) + { + trace(2, "no ssr clock correction: %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } + /* inconsistency between orbit and clock correction */ + if (ssr->iod[0] != ssr->iod[1]) + { + trace(2, "inconsist ssr correction: %s sat=%2d iod=%d %d\n", + time_str(time, 0), sat, ssr->iod[0], ssr->iod[1]); + *svh = -1; + return 0; + } + t1 = timediff(time, ssr->t0[0]); + t2 = timediff(time, ssr->t0[1]); + t3 = timediff(time, ssr->t0[2]); + + /* ssr orbit and clock correction (ref [4]) */ + if (fabs(t1) > MAXAGESSR || fabs(t2) > MAXAGESSR) + { + trace(2, "age of ssr error: %s sat=%2d t=%.0f %.0f\n", time_str(time, 0), + sat, t1, t2); + *svh = -1; + return 0; + } + if (ssr->udi[0] >= 1.0) t1 -= ssr->udi[0] / 2.0; + if (ssr->udi[1] >= 1.0) t2 -= ssr->udi[0] / 2.0; + + for (i = 0; i < 3; i++) deph[i] = ssr->deph[i] + ssr->ddeph[i] * t1; + dclk = ssr->dclk[0] + ssr->dclk[1] * t2 + ssr->dclk[2] * t2 * t2; + + /* ssr highrate clock correction (ref [4]) */ + if (ssr->iod[0] == ssr->iod[2] && ssr->t0[2].time && fabs(t3) < MAXAGESSR_HRCLK) + { + dclk += ssr->hrclk; + } + if (norm_rtk(deph, 3) > MAXECORSSR || fabs(dclk) > MAXCCORSSR) + { + trace(3, "invalid ssr correction: %s deph=%.1f dclk=%.1f\n", + time_str(time, 0), norm_rtk(deph, 3), dclk); + *svh = -1; + return 0; + } + /* satellite postion and clock by broadcast ephemeris */ + if (!ephpos(time, teph, sat, nav, ssr->iode, rs, dts, var, svh)) return 0; + + /* satellite clock for gps, galileo and qzss */ + sys = satsys(sat, NULL); + if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS || sys == SYS_BDS) + { + if (!(eph = seleph(teph, sat, ssr->iode, nav))) return 0; + + /* satellite clock by clock parameters */ + tk=timediff(time, eph->toc); + dts[0] = eph->f0 + eph->f1*tk + eph->f2 * tk * tk; + dts[1] = eph->f1 + 2.0*eph->f2 * tk; + + /* relativity correction */ + dts[0] -= 2.0 * dot(rs, rs + 3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT; + } + /* radial-along-cross directions in ecef */ + if (!normv3(rs + 3, ea)) return 0; + cross3(rs, rs + 3, rc); + if (!normv3(rc, ec)) + { + *svh = -1; + return 0; + } + cross3(ea, ec, er); + + /* satellite antenna offset correction */ + if (opt) + { + satantoff(time, rs, sat, nav, dant); + } + for (i = 0; i < 3; i++) + { + rs[i] += -(er[i] * deph[0] + ea[i] * deph[1] + ec[i] * deph[2]) + dant[i]; + } + /* t_corr = t_sv - (dts(brdc) + dclk(ssr) / SPEED_OF_LIGHT) (ref [10] eq.3.12-7) */ + dts[0] += dclk / SPEED_OF_LIGHT; + + /* variance by ssr ura */ + *var = var_urassr(ssr->ura); + + trace(5, "satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n", + time_str(time, 2), sat, deph[0], deph[1], deph[2], er[0], er[1], er[2], dclk, *var); + + return 1; +} + + +/* satellite position and clock ------------------------------------------------ + * compute satellite position, velocity and clock + * args : gtime_t time I time (gpst) + * gtime_t teph I time to select ephemeris (gpst) + * int sat I satellite number + * nav_t *nav I navigation data + * int ephopt I ephemeris option (EPHOPT_???) + * double *rs O sat position and velocity (ecef) + * {x,y,z,vx,vy,vz} (m|m/s) + * double *dts O sat clock {bias,drift} (s|s/s) + * double *var O sat position and clock error variance (m^2) + * int *svh O sat health flag (-1:correction not available) + * return : status (1:ok,0:error) + * notes : satellite position is referenced to antenna phase center + * satellite clock does not include code bias correction (tgd or bgd) + *-----------------------------------------------------------------------------*/ +int satpos(gtime_t time, gtime_t teph, int sat, int ephopt, + const nav_t *nav, double *rs, double *dts, double *var, + int *svh) +{ + trace(4, "satpos : time=%s sat=%2d ephopt=%d\n", time_str(time, 3), sat, ephopt); + + *svh = 0; + + switch (ephopt) + { + case EPHOPT_BRDC : return ephpos (time, teph, sat, nav, -1, rs, dts, var, svh); + case EPHOPT_SBAS : return satpos_sbas(time, teph, sat, nav, rs, dts, var, svh); + case EPHOPT_SSRAPC: return satpos_ssr (time, teph, sat, nav, 0, rs, dts, var, svh); + case EPHOPT_SSRCOM: return satpos_ssr (time, teph, sat, nav, 1, rs, dts, var, svh); + case EPHOPT_PREC : + if (!peph2pos(time, sat, nav, 1, rs, dts, var)) break; else return 1; + //TODO: enable lex + //case EPHOPT_LEX : + // if (!lexeph2pos(time, sat, nav, rs, dts, var)) break; else return 1; + } + *svh = -1; + return 0; +} + + +/* satellite positions and clocks ---------------------------------------------- + * compute satellite positions, velocities and clocks + * args : gtime_t teph I time to select ephemeris (gpst) + * obsd_t *obs I observation data + * int n I number of observation data + * nav_t *nav I navigation data + * int ephopt I ephemeris option (EPHOPT_???) + * double *rs O satellite positions and velocities (ecef) + * double *dts O satellite clocks + * double *var O sat position and clock error variances (m^2) + * int *svh O sat health flag (-1:correction not available) + * return : none + * notes : rs [(0:2)+i*6]= obs[i] sat position {x,y,z} (m) + * rs [(3:5)+i*6]= obs[i] sat velocity {vx,vy,vz} (m/s) + * dts[(0:1)+i*2]= obs[i] sat clock {bias,drift} (s|s/s) + * var[i] = obs[i] sat position and clock error variance (m^2) + * svh[i] = obs[i] sat health flag + * if no navigation data, set 0 to rs[], dts[], var[] and svh[] + * satellite position and clock are values at signal transmission time + * satellite position is referenced to antenna phase center + * satellite clock does not include code bias correction (tgd or bgd) + * any pseudorange and broadcast ephemeris are always needed to get + * signal transmission time + *-----------------------------------------------------------------------------*/ +void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, + int ephopt, double *rs, double *dts, double *var, int *svh) +{ + gtime_t time[MAXOBS] = {}; + double dt, pr; + int i, j; + + trace(3, "satposs : teph=%s n=%d ephopt=%d\n", time_str(teph, 3), n, ephopt); + + for (i = 0; i < n && i < MAXOBS; i++) + { + for (j = 0; j < 6; j++) rs [j + i * 6] = 0.0; + for (j = 0; j < 2; j++) dts[j + i * 2] = 0.0; + var[i] = 0.0; + svh[i] = 0; + + /* search any pseudorange */ + for (j = 0, pr = 0.0; j < NFREQ; j++) if ((pr = obs[i].P[j]) != 0.0) break; + + if (j >= NFREQ) + { + trace(2, "no pseudorange %s sat=%2d\n", time_str(obs[i].time, 3), obs[i].sat); + continue; + } + /* transmission time by satellite clock */ + time[i] = timeadd(obs[i].time, -pr / SPEED_OF_LIGHT); + + /* satellite clock bias by broadcast ephemeris */ + if (!ephclk(time[i], teph, obs[i].sat, nav, &dt)) + { + trace(3, "no broadcast clock %s sat=%2d\n", time_str(time[i], 3), obs[i].sat); + continue; + } + time[i] = timeadd(time[i], -dt); + + /* satellite position and clock at transmission time */ + if (!satpos(time[i], teph, obs[i].sat, ephopt, nav, rs + i * 6, dts + i * 2, var + i, + svh + i)) + { + trace(3, "no ephemeris %s sat=%2d\n", time_str(time[i], 3), obs[i].sat); + continue; + } + /* if no precise clock available, use broadcast clock instead */ + if (dts[i * 2] == 0.0) + { + if (!ephclk(time[i], teph, obs[i].sat, nav, dts + i * 2)) continue; + dts[1 + i * 2] = 0.0; + *var = std::pow(STD_BRDCCLK, 2.0); + } + } + for (i = 0; i < n && i < MAXOBS; i++) + { + trace(4, "%s sat=%2d rs=%13.3f %13.3f %13.3f dts=%12.3f var=%7.3f svh=%02X\n", + time_str(time[i], 6), obs[i].sat, rs[i * 6], rs[1 + i * 6], rs[2 + i * 6], + dts[i * 2] * 1e9, var[i], svh[i]); + } +} diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.h b/src/algorithms/libs/rtklib/rtklib_ephemeris.h new file mode 100644 index 000000000..1ccc32f4a --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.h @@ -0,0 +1,97 @@ +/*! + * \file rtklib_ephemeris.h + * \brief satellite ephemeris and clock functions + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + + +#ifndef GNSS_SDR_RTKLIB_EPHEMERIS_H_ +#define GNSS_SDR_RTKLIB_EPHEMERIS_H_ + +#include "rtklib.h" + + +double var_uraeph(int ura); +double var_urassr(int ura); +void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts); +double eph2clk(gtime_t time, const eph_t *eph); +void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, + double *var); +void deq(const double *x, double *xdot, const double *acc); +void glorbit(double t, double *x, const double *acc); +double geph2clk(gtime_t time, const geph_t *geph); + +void geph2pos(gtime_t time, const geph_t *geph, double *rs, double *dts, + double *var); +double seph2clk(gtime_t time, const seph_t *seph); +void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts, + double *var); +eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav); +geph_t *selgeph(gtime_t time, int sat, int iode, const nav_t *nav); +seph_t *selseph(gtime_t time, int sat, const nav_t *nav); +int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + double *dts); +//satellite position and clock by broadcast ephemeris +int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + int iode, double *rs, double *dts, double *var, int *svh); +int satpos_sbas(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + double *rs, double *dts, double *var, int *svh); +int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav, + int opt, double *rs, double *dts, double *var, int *svh); + +int satpos(gtime_t time, gtime_t teph, int sat, int ephopt, + const nav_t *nav, double *rs, double *dts, double *var, + int *svh); +void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, + int ephopt, double *rs, double *dts, double *var, int *svh); + + + + +#endif /* GNSS_SDR_RTKLIB_EPHEMERIS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.cc b/src/algorithms/libs/rtklib/rtklib_ionex.cc new file mode 100644 index 000000000..404cd34f9 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ionex.cc @@ -0,0 +1,590 @@ +/*! + * \file rtklib_ionex.h + * \brief ionex functions + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * References: + * [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange + * Format Version 1, February 25, 1998 + * [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere + * Maps based on GPS Carrier Phase Data Routinely producted by CODE + * Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_ionex.h" +#include "rtklib_rtkcmn.h" + +/* get index -----------------------------------------------------------------*/ +int getindex(double value, const double *range) +{ + if (range[2] == 0.0) return 0; + if (range[1] > 0.0 && (value < range[0] || range[1]nt >= nav->ntmax) + { + nav->ntmax+=256; + if (!(nav_tec = (tec_t *)realloc(nav->tec, sizeof(tec_t)*nav->ntmax))) + { + trace(1, "readionex malloc error ntmax=%d\n", nav->ntmax); + free(nav->tec); nav->tec = NULL; nav->nt = nav->ntmax = 0; + return NULL; + } + nav->tec = nav_tec; + } + p = nav->tec+nav->nt; + p->time = time0; + p->rb = rb; + for (i = 0; i < 3; i++) + { + p->ndata[i] = ndata[i]; + p->lats[i] = lats[i]; + p->lons[i] = lons[i]; + p->hgts[i] = hgts[i]; + } + n = ndata[0] * ndata[1] * ndata[2]; + + if (!(p->data = (double *)malloc(sizeof(double) * n)) || + !(p->rms = (float *)malloc(sizeof(float ) * n))) + { + return NULL; + } + for (i = 0; i < n; i++) + { + p->data[i] = 0.0; + p->rms [i] = 0.0f; + } + nav->nt++; + return p; +} + + +/* read ionex dcb aux data ----------------------------------------------------*/ +void readionexdcb(FILE *fp, double *dcb, double *rms) +{ + int i, sat; + char buff[1024], id[32], *label; + + trace(3, "readionexdcb:\n"); + + for (i = 0; i < MAXSAT; i++) dcb[i] = rms[i] = 0.0; + + while (fgets(buff, sizeof(buff), fp)) + { + if (strlen(buff) < 60) continue; + label = buff + 60; + + if (strstr(label, "PRN / BIAS / RMS") == label) + { + strncpy(id, buff+3, 3); id[3] = '\0'; + + if (!(sat = satid2no(id))) + { + trace(2, "ionex invalid satellite: %s\n", id); + continue; + } + dcb[sat-1] = str2num(buff, 6, 10); + rms[sat-1] = str2num(buff, 16, 10); + } + else if (strstr(label, "END OF AUX DATA") == label) break; + } +} + + +/* read ionex header ---------------------------------------------------------*/ +double readionexh(FILE *fp, double *lats, double *lons, double *hgts, + double *rb, double *nexp, double *dcb, double *rms) +{ + double ver = 0.0; + char buff[1024], *label; + + trace(3, "readionexh:\n"); + + while (fgets(buff, sizeof(buff), fp)) + { + + if (strlen(buff) < 60) continue; + label = buff + 60; + + if (strstr(label, "IONEX VERSION / TYPE") == label) + { + if (buff[20] == 'I') ver = str2num(buff, 0, 8); + } + else if (strstr(label, "BASE RADIUS") == label) + { + *rb = str2num(buff, 0, 8); + } + else if (strstr(label, "HGT1 / HGT2 / DHGT") == label) + { + hgts[0] = str2num(buff, 2, 6); + hgts[1] = str2num(buff, 8, 6); + hgts[2] = str2num(buff, 14, 6); + } + else if (strstr(label, "LAT1 / LAT2 / DLAT") == label) + { + lats[0] = str2num(buff, 2, 6); + lats[1] = str2num(buff, 8, 6); + lats[2] = str2num(buff, 14, 6); + } + else if (strstr(label,"LON1 / LON2 / DLON") == label) + { + lons[0] = str2num(buff, 2, 6); + lons[1] = str2num(buff, 8, 6); + lons[2] = str2num(buff, 14, 6); + } + else if (strstr(label, "EXPONENT") == label) + { + *nexp = str2num(buff, 0, 6); + } + else if (strstr(label, "START OF AUX DATA") == label && + strstr(buff, "DIFFERENTIAL CODE BIASES")) + { + readionexdcb(fp, dcb, rms); + } + else if (strstr(label, "END OF HEADER") == label) + { + return ver; + } + } + return 0.0; +} + + +/* read ionex body -----------------------------------------------------------*/ +int readionexb(FILE *fp, const double *lats, const double *lons, + const double *hgts, double rb, double nexp, nav_t *nav) +{ + tec_t *p = NULL; + gtime_t time = {0, 0}; + double lat, lon[3], hgt, x; + int i, j, k, n, m, index, type = 0; + char buff[1024], *label = buff + 60; + + trace(3, "readionexb:\n"); + + while (fgets(buff, sizeof(buff), fp)) + { + if (strlen(buff) < 60) continue; + + if (strstr(label, "START OF TEC MAP") == label) + { + if ((p = addtec(lats, lons, hgts, rb, nav))) type = 1; + } + else if (strstr(label, "END OF TEC MAP") == label) + { + type = 0; + p = NULL; + } + else if (strstr(label, "START OF RMS MAP") == label) + { + type = 2; + p = NULL; + } + else if (strstr(label, "END OF RMS MAP") == label) + { + type = 0; + p = NULL; + } + else if (strstr(label, "EPOCH OF CURRENT MAP") == label) + { + if (str2time(buff, 0, 36, &time)) + { + trace(2, "ionex epoch invalid: %-36.36s\n", buff); + continue; + } + if (type == 2) + { + for (i = nav->nt-1; i >= 0; i--) + { + if (fabs(timediff(time, nav->tec[i].time)) >= 1.0) continue; + p = nav->tec + i; + break; + } + } + else if (p) p->time = time; + } + else if (strstr(label, "LAT/LON1/LON2/DLON/H") == label && p) + { + lat = str2num(buff, 2, 6); + lon[0] = str2num(buff, 8, 6); + lon[1] = str2num(buff, 14, 6); + lon[2] = str2num(buff, 20, 6); + hgt = str2num(buff, 26, 6); + + i = getindex(lat, p->lats); + k = getindex(hgt, p->hgts); + n = nitem(lon); + + for (m = 0; m < n; m++) + { + if (m % 16 == 0 && !fgets(buff, sizeof(buff), fp)) break; + + j = getindex(lon[0] + lon[2] * m, p->lons); + if ((index = dataindex(i, j, k, p->ndata)) < 0) continue; + + if ((x = str2num(buff, m % 16 * 5, 5)) == 9999.0) continue; + + if (type == 1) p->data[index] = x * std::pow(10.0, nexp); + else p->rms[index] = (float)(x * std::pow(10.0, nexp)); + } + } + } + return 1; +} + + +/* combine tec grid data -----------------------------------------------------*/ +void combtec(nav_t *nav) +{ + tec_t tmp; + int i, j, n = 0; + + trace(3, "combtec : nav->nt=%d\n", nav->nt); + + for (i = 0; i < nav->nt - 1; i++) + { + for (j = i + 1; j < nav->nt; j++) + { + if (timediff(nav->tec[j].time, nav->tec[i].time) < 0.0) + { + tmp = nav->tec[i]; + nav->tec[i] = nav->tec[j]; + nav->tec[j] = tmp; + } + } + } + for (i = 0; i < nav->nt; i++) + { + if (i > 0 && timediff(nav->tec[i].time, nav->tec[n - 1].time) == 0.0) + { + free(nav->tec[n - 1].data); + free(nav->tec[n - 1].rms ); + nav->tec[n - 1] = nav->tec[i]; + continue; + } + nav->tec[n++] = nav->tec[i]; + } + nav->nt = n; + + trace(4, "combtec : nav->nt=%d\n", nav->nt); +} + + +/* read ionex tec grid file ---------------------------------------------------- + * read ionex ionospheric tec grid file + * args : char *file I ionex tec grid file + * (wind-card * is expanded) + * nav_t *nav IO navigation data + * nav->nt, nav->ntmax and nav->tec are modified + * int opt I read option (1: no clear of tec data,0:clear) + * return : none + * notes : see ref [1] + *-----------------------------------------------------------------------------*/ +void readtec(const char *file, nav_t *nav, int opt) +{ + FILE *fp; + double lats[3] = {0}, lons[3] = {0}, hgts[3] = {0}, rb = 0.0, nexp = -1.0; + double dcb[MAXSAT] = {0}, rms[MAXSAT] = {0}; + int i, n; + char *efiles[MAXEXFILE]; + + trace(3, "readtec : file=%s\n", file); + + /* clear of tec grid data option */ + if (!opt) + { + free(nav->tec); + nav->tec = NULL; + nav->nt = nav->ntmax = 0; + } + for (i = 0; i < MAXEXFILE; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return; + } + } + /* expand wild card in file path */ + n = expath(file, efiles, MAXEXFILE); + + for (i = 0; i < n; i++) + { + if (!(fp = fopen(efiles[i], "r"))) + { + trace(2, "ionex file open error %s\n", efiles[i]); + continue; + } + /* read ionex header */ + if (readionexh(fp, lats, lons, hgts, &rb, &nexp, dcb, rms) <= 0.0) + { + trace(2, "ionex file format error %s\n", efiles[i]); + continue; + } + /* read ionex body */ + readionexb(fp, lats, lons, hgts, rb, nexp, nav); + + fclose(fp); + } + for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); + + /* combine tec grid data */ + if (nav->nt > 0) combtec(nav); + + /* P1-P2 dcb */ + for (i = 0; i < MAXSAT; i++) + { + nav->cbias[i][0] = SPEED_OF_LIGHT * dcb[i] * 1e-9; /* ns->m */ + } +} + + +/* interpolate tec grid data -------------------------------------------------*/ +int interptec(const tec_t *tec, int k, const double *posp, double *value, + double *rms) +{ + double dlat, dlon, a, b, d[4] = {0}, r[4] = {0}; + int i, j, n, index; + + trace(3, "interptec: k=%d posp=%.2f %.2f\n", k, posp[0] * R2D, posp[1] * R2D); + *value = *rms = 0.0; + + if (tec->lats[2] == 0.0 || tec->lons[2] == 0.0) return 0; + + dlat = posp[0] * R2D - tec->lats[0]; + dlon = posp[1] * R2D - tec->lons[0]; + if (tec->lons[2] > 0.0) dlon -= floor( dlon / 360) * 360.0; /* 0 <= dlon<360 */ + else dlon += floor(-dlon / 360) * 360.0; /* -360lats[2]; + b = dlon / tec->lons[2]; + i = (int)floor(a); + a -= i; + j = (int)floor(b); + b -= j; + + /* get gridded tec data */ + for (n = 0; n < 4; n++) + { + if ((index = dataindex(i + (n % 2), j + (n < 2 ? 0 : 1), k, tec->ndata)) < 0) continue; + d[n] = tec->data[index]; + r[n] = tec->rms[index]; + } + if (d[0] > 0.0 && d[1] > 0.0 && d[2] > 0.0 && d[3] > 0.0) + { + /* bilinear interpolation (inside of grid) */ + *value = (1.0 - a) * (1.0 - b) * d[0] + a*(1.0 -b) * d[1] + (1.0 - a) * b * d[2] + a * b * d[3]; + *rms = (1.0 - a) * (1.0 - b) * r[0] + a*(1.0 - b) * r[1] + (1.0 - a) * b * r[2] + a * b * r[3]; + } + /* nearest-neighbour extrapolation (outside of grid) */ + else if (a <= 0.5 && b <= 0.5 && d[0] > 0.0) {*value = d[0]; *rms = r[0];} + else if (a > 0.5 && b <= 0.5 && d[1] > 0.0) {*value = d[1]; *rms = r[1];} + else if (a <= 0.5 && b > 0.5 && d[2] > 0.0) {*value = d[2]; *rms = r[2];} + else if (a > 0.5 && b > 0.5 && d[3] > 0.0) {*value = d[3]; *rms = r[3];} + else + { + i = 0; + for (n = 0; n < 4; n++) if (d[n] > 0.0) {i++; *value += d[n]; *rms += r[n];} + if(i == 0) return 0; + *value /= i; + *rms /= i; + } + return 1; +} + + +/* ionosphere delay by tec grid data -----------------------------------------*/ +int iondelay(gtime_t time, const tec_t *tec, const double *pos, + const double *azel, int opt, double *delay, double *var) +{ + const double fact = 40.30E16 / FREQ1 / FREQ1; /* tecu->L1 iono (m) */ + double fs, posp[3] = {0}, vtec, rms, hion, rp; + int i; + + trace(3, "iondelay: time=%s pos=%.1f %.1f azel=%.1f %.1f\n", time_str(time, 0), + pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D); + + *delay = *var = 0.0; + + for (i = 0;i < tec->ndata[2]; i++) + { /* for a layer */ + hion = tec->hgts[0] + tec->hgts[2] * i; + + /* ionospheric pierce point position */ + fs = ionppp(pos, azel, tec->rb, hion, posp); + + if (opt&2) + { + /* modified single layer mapping function (M-SLM) ref [2] */ + rp = tec->rb / (tec->rb + hion) * sin(0.9782 * (PI / 2.0 - azel[1])); + fs = 1.0 / sqrt(1.0 - rp * rp); + } + if (opt&1) + { + /* earth rotation correction (sun-fixed coordinate) */ + posp[1] += 2.0 * PI * timediff(time, tec->time) / 86400.0; + } + /* interpolate tec grid data */ + if (!interptec(tec, i, posp, &vtec, &rms)) return 0; + + *delay += fact * fs * vtec; + *var += fact * fact * fs * fs * rms * rms; + } + trace(4, "iondelay: delay=%7.2f std=%6.2f\n", *delay, sqrt(*var)); + + return 1; +} + + +/* ionosphere model by tec grid data ------------------------------------------- + * compute ionospheric delay by tec grid data + * args : gtime_t time I time (gpst) + * nav_t *nav I navigation data + * double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * int opt I model option + * bit0: 0:earth-fixed,1:sun-fixed + * bit1: 0:single-layer,1:modified single-layer + * double *delay O ionospheric delay (L1) (m) + * double *var O ionospheric dealy (L1) variance (m^2) + * return : status (1:ok,0:error) + * notes : before calling the function, read tec grid data by calling readtec() + * return ok with delay=0 and var=VAR_NOTEC if elnt; i++) + { + if (timediff(nav->tec[i].time, time) > 0.0) break; + } + if (i == 0 || i >= nav->nt) + { + trace(2, "%s: tec grid out of period\n", time_str(time, 0)); + return 0; + } + if ((tt = timediff(nav->tec[i].time, nav->tec[i-1].time)) == 0.0) + { + trace(2, "tec grid time interval error\n"); + return 0; + } + /* ionospheric delay by tec grid data */ + stat[0] = iondelay(time, nav->tec+i-1, pos, azel, opt, dels , vars ); + stat[1] = iondelay(time, nav->tec+i , pos, azel, opt, dels+1, vars+1); + + if (!stat[0] && !stat[1]) + { + trace(2, "%s: tec grid out of area pos=%6.2f %7.2f azel=%6.1f %5.1f\n", + time_str(time, 0), pos[0] * R2D, pos[1] * R2D, azel[0] * R2D, azel[1] * R2D); + return 0; + } + if (stat[0] && stat[1]) + { /* linear interpolation by time */ + a = timediff(time, nav->tec[i-1].time) / tt; + *delay = dels[0] * (1.0 - a) + dels[1] * a; + *var = vars[0] * (1.0 - a) + vars[1] * a; + } + else if (stat[0]) + { /* nearest-neighbour extrapolation by time */ + *delay = dels[0]; + *var = vars[0]; + } + else + { + *delay = dels[1]; + *var = vars[1]; + } + trace(3, "iontec : delay=%5.2f std=%5.2f\n", *delay, sqrt(*var)); + return 1; +} diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.h b/src/algorithms/libs/rtklib/rtklib_ionex.h new file mode 100644 index 000000000..7ed73e3ee --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ionex.h @@ -0,0 +1,90 @@ +/*! + * \file rtklib_ionex.h + * \brief ionex functions + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * References: + * [1] S.Schear, W.Gurtner and J.Feltens, IONEX: The IONosphere Map EXchange + * Format Version 1, February 25, 1998 + * [2] S.Schaer, R.Markus, B.Gerhard and A.S.Timon, Daily Global Ionosphere + * Maps based on GPS Carrier Phase Data Routinely producted by CODE + * Analysis Center, Proceeding of the IGS Analysis Center Workshop, 1996 + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_IONEX_H_ +#define GNSS_SDR_RTKLIB_IONEX_H_ + +#include "rtklib.h" + +const double VAR_NOTEC = 30.0 * 30.0; /* variance of no tec */ +const double MIN_EL = 0.0; /* min elevation angle (rad) */ +const double MIN_HGT = -1000.0; /* min user height (m) */ + +int getindex(double value, const double *range); + +int nitem(const double *range); +int dataindex(int i, int j, int k, const int *ndata); +tec_t *addtec(const double *lats, const double *lons, const double *hgts, + double rb, nav_t *nav); +void readionexdcb(FILE *fp, double *dcb, double *rms); +double readionexh(FILE *fp, double *lats, double *lons, double *hgts, + double *rb, double *nexp, double *dcb, double *rms); +int readionexb(FILE *fp, const double *lats, const double *lons, + const double *hgts, double rb, double nexp, nav_t *nav); +void combtec(nav_t *nav); +void readtec(const char *file, nav_t *nav, int opt); +int interptec(const tec_t *tec, int k, const double *posp, double *value, + double *rms); + +int iondelay(gtime_t time, const tec_t *tec, const double *pos, + const double *azel, int opt, double *delay, double *var); +int iontec(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, int opt, double *delay, double *var); + +#endif /* GNSS_SDR_RTKLIB_IONEX_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_lambda.cc b/src/algorithms/libs/rtklib/rtklib_lambda.cc new file mode 100644 index 000000000..32082ac97 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_lambda.cc @@ -0,0 +1,327 @@ +/*! + * \file rtklib_lambda.cc + * \brief Integer ambiguity resolution + * \authors
    + *
  • 2007-2008, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2008, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_lambda.h" +#include "rtklib_rtkcmn.h" + +/* LD factorization (Q=L'*diag(D)*L) -----------------------------------------*/ +int LD(int n, const double *Q, double *L, double *D) +{ + int i,j,k,info = 0; + double a,*A = mat(n,n); + + memcpy(A,Q,sizeof(double)*n*n); + for (i = n-1; i >= 0; i--) + { + if ((D[i] = A[i+i*n])<=0.0) {info = -1; break;} + a = sqrt(D[i]); + for (j = 0; j<=i; j++) L[i+j*n] = A[i+j*n]/a; + for (j = 0; j<=i-1; j++) for (k = 0;k<=j;k++) A[j+k*n] -= L[i+k*n]*L[i+j*n]; + for (j = 0; j<=i; j++) L[i+j*n] /= L[i+i*n]; + } + free(A); + if (info) fprintf(stderr,"%s : LD factorization error\n",__FILE__); + return info; +} + + +/* integer gauss transformation ----------------------------------------------*/ +void gauss(int n, double *L, double *Z, int i, int j) +{ + int k,mu; + + if ((mu = (int)ROUND_LAMBDA(L[i+j*n]))!=0) + { + for (k = i; k= 0) + { + if (j<=k) for (i = j+1; is[imax]) imax = nn; + for (i = 0; i= LOOPMAX) + { + fprintf(stderr,"%s : search loop count overflow\n",__FILE__); + return -1; + } + return 0; +} + + +/* lambda/mlambda integer least-square estimation ------------------------------ + * integer least-square estimation. reduction is performed by lambda (ref.[1]), + * and search by mlambda (ref.[2]). + * args : int n I number of float parameters + * int m I number of fixed solutions + * double *a I float parameters (n x 1) + * double *Q I covariance matrix of float parameters (n x n) + * double *F O fixed solutions (n x m) + * double *s O sum of squared residulas of fixed solutions (1 x m) + * return : status (0:ok,other:error) + * notes : matrix stored by column-major order (fortran convension) + *-----------------------------------------------------------------------------*/ +int lambda(int n, int m, const double *a, const double *Q, double *F, + double *s) +{ + int info; + double *L,*D,*Z,*z,*E; + + if (n<=0||m<=0) return -1; + L = zeros(n,n); + D = mat(n,1); + Z = eye(n); + z = mat(n,1); + E = mat(n,m); + + /* LD factorization */ + if (!(info = LD(n,Q,L,D))) + { + + /* lambda reduction */ + reduction(n,L,D,Z); + matmul("TN",n,1,n,1.0,Z,a,0.0,z); /* z=Z'*a */ + + /* mlambda search */ + if (!(info = search(n,m,L,D,z,E,s))) + { + + info = solve("T",Z,E,n,m,F); /* F=Z'\E */ + } + } + free(L); free(D); free(Z); free(z); free(E); + return info; +} + + + +/* lambda reduction ------------------------------------------------------------ + * reduction by lambda (ref [1]) for integer least square + * args : int n I number of float parameters + * double *Q I covariance matrix of float parameters (n x n) + * double *Z O lambda reduction matrix (n x n) + * return : status (0:ok,other:error) + *-----------------------------------------------------------------------------*/ +int lambda_reduction(int n, const double *Q, double *Z) +{ + double *L,*D; + int i,j,info; + + if (n<=0) return -1; + + L = zeros(n,n); + D = mat(n,1); + + for (i = 0; i + *
  • 2007-2008, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + * + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2008, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * References: + * [1] P.J.G.Teunissen, The least-square ambiguity decorrelation adjustment: + * a method for fast GPS ambiguity estimation, J.Geodesy, Vol.70, 65-82, + * 1995 + * [2] X.-W.Chang, X.Yang, T.Zhou, MLAMBDA: A modified LAMBDA method for + * integer least-squares estimation, J.Geodesy, Vol.79, 552-565, 2005 + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_LAMBDA_H_ +#define GNSS_SDR_RTKLIB_LAMBDA_H_ + + +#include "rtklib.h" + +/* constants/macros ----------------------------------------------------------*/ +const int LOOPMAX = 10000; /* maximum count of search loop */ +#define SGN_LAMBDA(x) ((x)<=0.0?-1.0:1.0) +#define ROUND_LAMBDA(x) (floor((x)+0.5)) +#define SWAP_LAMBDA(x,y) do {double tmp_; tmp_=x; x=y; y=tmp_;} while (0) + +int LD(int n, const double *Q, double *L, double *D); +void gauss(int n, double *L, double *Z, int i, int j); +void perm(int n, double *L, double *D, int j, double del, double *Z); +void reduction(int n, double *L, double *D, double *Z); +int search(int n, int m, const double *L, const double *D, + const double *zs, double *zn, double *s); + +int lambda(int n, int m, const double *a, const double *Q, double *F, double *s); + +int lambda_reduction(int n, const double *Q, double *Z); + +int lambda_search(int n, int m, const double *a, const double *Q, + double *F, double *s); + + +#endif diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc new file mode 100644 index 000000000..1f95ba32f --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -0,0 +1,790 @@ +/*! + * \file rtklib_pntpos.cc + * \brief standard code-based positioning + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_pntpos.h" +#include "rtklib_ephemeris.h" +#include "rtklib_ionex.h" +#include "rtklib_sbas.h" + +/* pseudorange measurement error variance ------------------------------------*/ +double varerr(const prcopt_t *opt, double el, int sys) +{ + double fact, varr; + fact = sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); + varr = std::pow(opt->err[0], 2.0) * (std::pow(opt->err[1], 2.0) + std::pow(opt->err[2], 2.0) / sin(el)); + if (opt->ionoopt == IONOOPT_IFLC) varr *= std::pow(2, 3.0); /* iono-free */ + return std::pow(fact, 2.0) * varr; +} + + +/* get tgd parameter (m) -----------------------------------------------------*/ +double gettgd(int sat, const nav_t *nav) +{ + int i; + for (i = 0; i < nav->n; i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT * nav->eph[i].tgd[0]; + } + return 0.0; +} + + +/* psendorange with code bias correction -------------------------------------*/ +double prange(const obsd_t *obs, const nav_t *nav, const double *azel, + int iter, const prcopt_t *opt, double *var) +{ + const double *lam = nav->lam[obs->sat - 1]; + double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma; + int i = 0, j = 1, sys; + + *var = 0.0; + + if (!(sys = satsys(obs->sat, NULL))) + { + trace(4, "prange: satsys NULL\n"); + return 0.0; + } + + + /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ + if (NFREQ >= 3 && (sys & (SYS_GAL | SYS_SBS))) j = 2; + + if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) + { + trace(4, "prange: NFREQ<2||lam[i]==0.0||lam[j]==0.0\n"); + printf("i: %d j:%d, lam[i]: %f lam[j] %f\n", i, j, lam[i], lam[j]); + return 0.0; + } + + /* test snr mask */ + if (iter>0) + { + if (testsnr(0, i, azel[1], obs->SNR[i] * 0.25, &opt->snrmask)) + { + trace(4, "snr mask: %s sat=%2d el=%.1f snr=%.1f\n", + time_str(obs->time, 0), obs->sat, azel[1] * R2D, obs->SNR[i] * 0.25); + return 0.0; + } + if (opt->ionoopt == IONOOPT_IFLC) + { + if (testsnr(0, j, azel[1], obs->SNR[j] * 0.25, &opt->snrmask)) + { + trace(4, "prange: testsnr error\n"); + return 0.0; + } + } + } + gamma = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */ + P1 = obs->P[i]; + P2 = obs->P[j]; + P1_P2 = nav->cbias[obs->sat-1][0]; + P1_C1 = nav->cbias[obs->sat-1][1]; + P2_C2 = nav->cbias[obs->sat-1][2]; + + /* if no P1-P2 DCB, use TGD instead */ + if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) + { + P1_P2 = (1.0 - gamma) * gettgd(obs->sat, nav); + } + if (opt->ionoopt == IONOOPT_IFLC) + { /* dual-frequency */ + + if (P1 == 0.0 || P2 == 0.0) return 0.0; + if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ + if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */ + + /* iono-free combination */ + PC = (gamma * P1 - P2) / (gamma - 1.0); + } + else + { /* single-frequency */ + + if (P1 == 0.0) return 0.0; + if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ + PC = P1 - P1_P2 / (1.0 - gamma); + } + if (opt->sateph == EPHOPT_SBAS) PC -= P1_C1; /* sbas clock based C1 */ + + *var = std::pow(ERR_CBIAS, 2.0); + + return PC; +} + + +/* ionospheric correction ------------------------------------------------------ + * compute ionospheric correction + * args : gtime_t time I time + * nav_t *nav I navigation data + * int sat I satellite number + * double *pos I receiver position {lat,lon,h} (rad|m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * int ionoopt I ionospheric correction option (IONOOPT_???) + * double *ion O ionospheric delay (L1) (m) + * double *var O ionospheric delay (L1) variance (m^2) + * return : status(1:ok,0:error) + *-----------------------------------------------------------------------------*/ +int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, + const double *azel, int ionoopt, double *ion, double *var) +{ + trace(4, "ionocorr: time=%s opt=%d sat=%2d pos=%.3f %.3f azel=%.3f %.3f\n", + time_str(time, 3), ionoopt, sat, pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, + azel[1]*R2D); + + /* broadcast model */ + if (ionoopt == IONOOPT_BRDC) + { + *ion = ionmodel(time, nav->ion_gps, pos, azel); + *var = std::pow(*ion * ERR_BRDCI, 2.0); + return 1; + } + /* sbas ionosphere model */ + if (ionoopt == IONOOPT_SBAS) + { + return sbsioncorr(time, nav, pos, azel, ion, var); + } + /* ionex tec model */ + if (ionoopt == IONOOPT_TEC) + { + return iontec(time, nav, pos, azel, 1, ion, var); + } + /* qzss broadcast model */ + if (ionoopt == IONOOPT_QZS && norm_rtk(nav->ion_qzs, 8)>0.0) + { + *ion = ionmodel(time, nav->ion_qzs, pos, azel); + *var = std::pow(*ion * ERR_BRDCI, 2.0); + return 1; + } + /* lex ionosphere model */ + //if (ionoopt == IONOOPT_LEX) { + // return lexioncorr(time, nav, pos, azel, ion, var); + //} + *ion = 0.0; + *var = ionoopt == IONOOPT_OFF ? std::pow(ERR_ION, 2.0) : 0.0; + return 1; +} + + +/* tropospheric correction ----------------------------------------------------- + * compute tropospheric correction + * args : gtime_t time I time + * nav_t *nav I navigation data + * double *pos I receiver position {lat,lon,h} (rad|m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * int tropopt I tropospheric correction option (TROPOPT_???) + * double *trp O tropospheric delay (m) + * double *var O tropospheric delay variance (m^2) + * return : status(1:ok,0:error) + *-----------------------------------------------------------------------------*/ +int tropcorr(gtime_t time, const nav_t *nav __attribute__((unused)), const double *pos, + const double *azel, int tropopt, double *trp, double *var) +{ + trace(4, "tropcorr: time=%s opt=%d pos=%.3f %.3f azel=%.3f %.3f\n", + time_str(time, 3), tropopt, pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, + azel[1]*R2D); + + /* saastamoinen model */ + if (tropopt == TROPOPT_SAAS || tropopt == TROPOPT_EST || tropopt == TROPOPT_ESTG) + { + *trp = tropmodel(time, pos, azel, REL_HUMI); + *var = std::pow(ERR_SAAS / (sin(azel[1]) + 0.1), 2.0); + return 1; + } + /* sbas troposphere model */ + if (tropopt == TROPOPT_SBAS) + { + *trp = sbstropcorr(time, pos, azel, var); + return 1; + } + /* no correction */ + *trp = 0.0; + *var = tropopt == TROPOPT_OFF ? std::pow(ERR_TROP, 2.0) : 0.0; + return 1; +} + + +/* pseudorange residuals -----------------------------------------------------*/ +int rescode(int iter, const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, const prcopt_t *opt, + double *v, double *H, double *var, double *azel, int *vsat, + double *resp, int *ns) +{ + double r, dion, dtrp, vmeas, vion, vtrp, rr[3], pos[3], dtr, e[3], P, lam_L1; + int i, j, nv = 0, sys, mask[4] = {0}; + + trace(3, "resprng : n=%d\n", n); + + for (i = 0; i < 3; i++) rr[i] = x[i]; + dtr = x[3]; + + ecef2pos(rr, pos); + + for (i = *ns = 0; i < n && i < MAXOBS; i++) + { + vsat[i] = 0; + azel[i*2] = azel[1+i*2] = resp[i] = 0.0; + + if (!(sys = satsys(obs[i].sat, NULL))) continue; + + /* reject duplicated observation data */ + if (i < n - 1 && i < MAXOBS - 1 && obs[i].sat == obs[i+1].sat) + { + trace(2, "duplicated observation data %s sat=%2d\n", + time_str(obs[i].time, 3), obs[i].sat); + i++; + continue; + } + /* geometric distance/azimuth/elevation angle */ + if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 || satazel(pos, e, azel + i * 2) < opt->elmin) + { + trace(4, "geodist / satazel error\n"); + continue; + } + + /* psudorange with code bias correction */ + if ((P = prange(obs+i, nav, azel+i*2, iter, opt, &vmeas)) == 0.0) + { + trace(4, "prange error\n"); + continue; + } + + /* excluded satellite? */ + if (satexclude(obs[i].sat, svh[i], opt)) + { + trace(4, "satexclude error\n"); + continue; + } + + /* ionospheric corrections */ + if (!ionocorr(obs[i].time, nav, obs[i].sat, pos, azel+i*2, + iter>0 ? opt->ionoopt : IONOOPT_BRDC, &dion, &vion)) + { + trace(4, "ionocorr error\n"); + continue; + } + + /* GPS-L1 -> L1/B1 */ + if ((lam_L1 = nav->lam[obs[i].sat - 1][0]) > 0.0) + { + dion *= std::pow(lam_L1 / lam_carr[0], 2.0); + } + /* tropospheric corrections */ + if (!tropcorr(obs[i].time, nav, pos, azel + i*2, + iter > 0 ? opt->tropopt : TROPOPT_SAAS, &dtrp, &vtrp)) + { + trace(4, "tropocorr error\n"); + continue; + } + /* pseudorange residual */ + v[nv] = P - (r + dtr - SPEED_OF_LIGHT * dts[i*2] + dion + dtrp); + + /* design matrix */ + for (j = 0; j < NX; j++) H[j + nv * NX] = j < 3 ? - e[j] : (j == 3 ? 1.0 : 0.0); + + /* time system and receiver bias offset correction */ + if (sys == SYS_GLO) {v[nv] -= x[4]; H[4+nv*NX] = 1.0; mask[1] = 1;} + else if (sys == SYS_GAL) {v[nv] -= x[5]; H[5+nv*NX] = 1.0; mask[2] = 1;} + else if (sys == SYS_BDS) {v[nv] -= x[6]; H[6+nv*NX] = 1.0; mask[3] = 1;} + else mask[0] = 1; + + vsat[i] = 1; + resp[i] = v[nv]; + (*ns)++; + + /* error variance */ + var[nv++] = varerr(opt, azel[1+i*2], sys) + vare[i] + vmeas + vion + vtrp; + + trace(4, "sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n", obs[i].sat, + azel[i*2] * R2D, azel[1+i*2] * R2D, resp[i], sqrt(var[nv-1])); + } + /* constraint to avoid rank-deficient */ + for (i = 0; i < 4; i++) + { + if (mask[i]) continue; + v[nv] = 0.0; + for (j = 0; j < NX; j++) H[j + nv * NX] = j == i + 3 ? 1.0 : 0.0; + var[nv++] = 0.01; + } + return nv; +} + + +/* validate solution ---------------------------------------------------------*/ +int valsol(const double *azel, const int *vsat, int n, + const prcopt_t *opt, const double *v, int nv, int nx, + char *msg) +{ + double azels[MAXOBS*2], dop[4], vv; + int i, ns; + + trace(3, "valsol : n=%d nv=%d\n", n, nv); + + /* chi-square validation of residuals */ + vv = dot(v, v, nv); + if (nv > nx && vv > chisqr[nv-nx-1]) + { + sprintf(msg, "chi-square error nv=%d vv=%.1f cs=%.1f", nv, vv, chisqr[nv-nx-1]); + return 0; + } + /* large gdop check */ + for (i = ns = 0; i < n; i++) + { + if (!vsat[i]) continue; + azels[ ns*2] = azel[ i*2]; + azels[1+ns*2] = azel[1+i*2]; + ns++; + } + dops(ns, azels, opt->elmin, dop); + if (dop[0] <= 0.0 || dop[0] > opt->maxgdop) + { + sprintf(msg, "gdop error nv=%d gdop=%.1f", nv, dop[0]); + return 0; + } + return 1; +} + + +/* estimate receiver position ------------------------------------------------*/ +int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, + const double *vare, const int *svh, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, + double *resp, char *msg) +{ + double x[NX] = {0}, dx[NX], Q[NX*NX], *v, *H, *var, sig; + int i, j, k, info, stat, nv, ns; + + trace(3, "estpos : n=%d\n", n); + + v = mat(n + 4, 1); + H = mat(NX, n + 4); + var = mat(n + 4, 1); + + for (i = 0; i < 3; i++) x[i] = sol->rr[i]; + + for (i = 0; i < MAXITR; i++) + { + /* pseudorange residuals */ + nv = rescode(i, obs, n, rs, dts, vare, svh, nav, x, opt, v, H, var, azel, vsat, resp, &ns); + + if (nv < NX) + { + sprintf(msg, "lack of valid sats ns=%d", nv); + break; + } + /* weight by variance */ + for (j = 0;j < nv; j++) + { + sig = sqrt(var[j]); + v[j] /= sig; + for (k = 0; k < NX; k++) H[k + j * NX] /= sig; + } + /* least square estimation */ + if ((info = lsq(H, v, NX, nv, dx, Q))) + { + sprintf(msg, "lsq error info=%d", info); + break; + } + for (j = 0; j < NX; j++) x[j] += dx[j]; + + if (norm_rtk(dx, NX) < 1e-4) + { + sol->type = 0; + sol->time = timeadd(obs[0].time, -x[3] / SPEED_OF_LIGHT); + sol->dtr[0] = x[3] / SPEED_OF_LIGHT; /* receiver clock bias (s) */ + sol->dtr[1] = x[4] / SPEED_OF_LIGHT; /* glo-gps time offset (s) */ + sol->dtr[2] = x[5] / SPEED_OF_LIGHT; /* gal-gps time offset (s) */ + sol->dtr[3] = x[6] / SPEED_OF_LIGHT; /* bds-gps time offset (s) */ + for (j = 0; j < 6; j++) sol->rr[j] = j < 3 ? x[j] : 0.0; + for (j = 0; j < 3; j++) sol->qr[j] = (float)Q[j + j * NX]; + sol->qr[3] = (float)Q[1]; /* cov xy */ + sol->qr[4] = (float)Q[2 + NX]; /* cov yz */ + sol->qr[5] = (float)Q[2]; /* cov zx */ + sol->ns = (unsigned char)ns; + sol->age = sol->ratio = 0.0; + + /* validate solution */ + if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) + { + sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE; + } + free(v); + free(H); + free(var); + + return stat; + } + } + if (i >= MAXITR) sprintf(msg, "iteration divergent i=%d", i); + + free(v); + free(H); + free(var); + + return 0; +} + + +/* raim fde (failure detection and exclution) -------------------------------*/ +int raim_fde(const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + double *azel, int *vsat, double *resp, char *msg) +{ + obsd_t *obs_e; + sol_t sol_e = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; + char tstr[32], name[16], msg_e[128]; + double *rs_e, *dts_e, *vare_e, *azel_e, *resp_e, rms_e, rms = 100.0; + int i, j, k, nvsat, stat = 0, *svh_e, *vsat_e, sat = 0; + + trace(3, "raim_fde: %s n=%2d\n", time_str(obs[0].time, 0), n); + + if (!(obs_e = (obsd_t *)malloc(sizeof(obsd_t) * n))) return 0; + rs_e = mat(6, n); + dts_e = mat(2, n); + vare_e = mat(1, n); + azel_e = zeros(2, n); + svh_e = imat(1, n); + vsat_e = imat(1, n); + resp_e = mat(1, n); + + for (i = 0; i < n; i++) + { + /* satellite exclution */ + for (j = k = 0; j < n; j++) + { + if (j == i) continue; + obs_e[k] = obs[j]; + matcpy(rs_e + 6 * k, rs + 6 * j, 6, 1); + matcpy(dts_e + 2 * k, dts + 2 * j, 2, 1); + vare_e[k] = vare[j]; + svh_e[k++] = svh[j]; + } + /* estimate receiver position without a satellite */ + if (!estpos(obs_e, n-1, rs_e, dts_e, vare_e, svh_e, nav, opt, &sol_e, azel_e, + vsat_e, resp_e, msg_e)) + { + trace(3, "raim_fde: exsat=%2d (%s)\n", obs[i].sat, msg); + continue; + } + for (j = nvsat = 0, rms_e = 0.0; j < n - 1; j++) + { + if (!vsat_e[j]) continue; + rms_e += std::pow(resp_e[j], 2.0); + nvsat++; + } + if (nvsat < 5) + { + trace(3, "raim_fde: exsat=%2d lack of satellites nvsat=%2d\n", + obs[i].sat, nvsat); + continue; + } + rms_e = sqrt(rms_e / nvsat); + + trace(3, "raim_fde: exsat=%2d rms=%8.3f\n", obs[i].sat, rms_e); + + if (rms_e > rms) continue; + + /* save result */ + for (j = k = 0; j < n; j++) + { + if (j == i) continue; + matcpy(azel + 2 * j, azel_e + 2 * k, 2, 1); + vsat[j] = vsat_e[k]; + resp[j] = resp_e[k++]; + } + stat = 1; + *sol = sol_e; + sat = obs[i].sat; + rms = rms_e; + vsat[i] = 0; + strcpy(msg, msg_e); + } + if (stat) + { + time2str(obs[0].time, tstr, 2); satno2id(sat, name); + trace(2, "%s: %s excluded by raim\n", tstr + 11, name); + } + free(obs_e); + free(rs_e); + free(dts_e); + free(vare_e); + free(azel_e); + free(svh_e); + free(vsat_e); + free(resp_e); + + return stat; +} + + +/* doppler residuals ---------------------------------------------------------*/ +int resdop(const obsd_t *obs, int n, const double *rs, const double *dts, + const nav_t *nav, const double *rr, const double *x, + const double *azel, const int *vsat, double *v, double *H) +{ + double lam, rate, pos[3], E[9], a[3], e[3], vs[3], cosel; + int i, j, nv = 0; + + trace(3, "resdop : n=%d\n", n); + + ecef2pos(rr, pos); + xyz2enu(pos, E); + + for (i = 0; i < n && i < MAXOBS; i++) + { + lam = nav->lam[obs[i].sat-1][0]; + + if (obs[i].D[0] == 0.0 || lam == 0.0 || !vsat[i] || norm_rtk(rs + 3 + i * 6, 3) <= 0.0) + { + continue; + } + /* line-of-sight vector in ecef */ + cosel = cos(azel[1+i*2]); + a[0] = sin(azel[i*2]) * cosel; + a[1] = cos(azel[i*2]) * cosel; + a[2] = sin(azel[1+i*2]); + matmul("TN", 3, 1, 3, 1.0, E, a, 0.0, e); + + /* satellite velocity relative to receiver in ecef */ + for (j = 0; j < 3; j++) vs[j] = rs[j+3+i*6] - x[j]; + + /* range rate with earth rotation correction */ + rate = dot(vs, e, 3) + DEFAULT_OMEGA_EARTH_DOT / SPEED_OF_LIGHT * (rs[4 + i * 6] * rr[0] + rs[1 + i * 6] * x[0]- + rs[3 + i * 6] * rr[1] - rs[i * 6] * x[1]); + + /* doppler residual */ + v[nv] =- lam * obs[i].D[0] - (rate + x[3] - SPEED_OF_LIGHT * dts[1 + i *2]); + + /* design matrix */ + for (j = 0; j < 4; j++) H[j + nv * 4] = j < 3 ? - e[j] : 1.0; + + nv++; + } + return nv; +} + + +/* estimate receiver velocity ------------------------------------------------*/ +void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, + const nav_t *nav, const prcopt_t *opt __attribute__((unused)), sol_t *sol, + const double *azel, const int *vsat) +{ + double x[4] = {0}, dx[4], Q[16], *v, *H; + int i, j, nv; + + trace(3, "estvel : n=%d\n", n); + + v = mat(n, 1); + H = mat(4, n); + + for (i = 0; i < MAXITR; i++) + { + /* doppler residuals */ + if ((nv = resdop(obs, n, rs, dts, nav, sol->rr, x, azel, vsat, v, H)) < 4) + { + break; + } + /* least square estimation */ + if (lsq(H, v, 4, nv, dx, Q)) break; + + for (j = 0; j < 4; j++) x[j] += dx[j]; + + if (norm_rtk(dx, 4) < 1e-6) + { + for (i = 0; i < 3; i++) sol->rr[i+3] = x[i]; + break; + } + } + free(v); + free(H); +} + + +/* single-point positioning ---------------------------------------------------- + * compute receiver position, velocity, clock bias by single-point positioning + * with pseudorange and doppler observables + * args : obsd_t *obs I observation data + * int n I number of observation data + * nav_t *nav I navigation data + * prcopt_t *opt I processing options + * sol_t *sol IO solution + * double *azel IO azimuth/elevation angle (rad) (NULL: no output) + * ssat_t *ssat IO satellite status (NULL: no output) + * char *msg O error message for error exit + * return : status(1:ok,0:error) + * notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and + * receiver bias are negligible (only involving glonass-gps time offset + * and receiver bias) + *-----------------------------------------------------------------------------*/ +int pntpos(const obsd_t *obs, int n, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, + char *msg) +{ + // int k = 0; + // for (k = 0;kn;k++) + // { + // printf("NAV[%i]: sat %i, %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \r\n", + // k, + // nav->eph[k].sat, + // nav->eph[k].A, + // nav->eph[k].Adot, + // nav->eph[k].M0, + // nav->eph[k].OMG0, + // nav->eph[k].OMGd, + // nav->eph[k].cic, + // nav->eph[k].cis, + // nav->eph[k].code, + // nav->eph[k].crc, + // nav->eph[k].crs, + // nav->eph[k].cuc, + // nav->eph[k].cus, + // nav->eph[k].deln, + // nav->eph[k].e, + // nav->eph[k].f0, + // nav->eph[k].f1, + // nav->eph[k].f2, + // nav->eph[k].fit, + // nav->eph[k].flag, + // nav->eph[k].i0, + // nav->eph[k].idot, + // nav->eph[k].iodc, + // nav->eph[k].iode, + // nav->eph[k].ndot, + // nav->eph[k].omg, + // nav->eph[k].sat, + // nav->eph[k].sva, + // nav->eph[k].svh, + // nav->eph[k].tgd[0], + // nav->eph[k].toc.sec, + // nav->eph[k].toe.sec, + // nav->eph[k].toes, + // nav->eph[k].ttr.sec, + // nav->eph[k].week); + // } + + prcopt_t opt_ = *opt; + double *rs, *dts, *var, *azel_, *resp; + int i, stat, vsat[MAXOBS] = {0}, svh[MAXOBS]; + + sol->stat = SOLQ_NONE; + + if (n <= 0) {strcpy(msg, "no observation data"); return 0;} + + trace(3, "pntpos : tobs=%s n=%d\n", time_str(obs[0].time, 3), n); + + sol->time = obs[0].time; + msg[0] = '\0'; + + rs = mat(6, n); + dts = mat(2, n); + var = mat(1, n); + azel_ = zeros(2, n); + resp = mat(1, n); + + if (opt_.mode != PMODE_SINGLE) + { /* for precise positioning */ +#if 0 + opt_.sateph = EPHOPT_BRDC; +#endif + opt_.ionoopt = IONOOPT_BRDC; + opt_.tropopt = TROPOPT_SAAS; + } + /* satellite positons, velocities and clocks */ + satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh); + + /* estimate receiver position with pseudorange */ + stat = estpos(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); + + /* raim fde */ + if (!stat && n >= 6 && opt->posopt[4]) + { + stat = raim_fde(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); + } + /* estimate receiver velocity with doppler */ + if (stat) estvel(obs, n, rs, dts, nav, &opt_, sol, azel_, vsat); + + if (azel) + { + for (i = 0; i < n * 2; i++) azel[i] = azel_[i]; + } + if (ssat) + { + for (i = 0; i < MAXSAT; i++) + { + ssat[i].vs = 0; + ssat[i].azel[0] = ssat[i].azel[1] = 0.0; + ssat[i].resp[0] = ssat[i].resc[0] = 0.0; + ssat[i].snr[0] = 0; + } + for (i = 0; i < n; i++) + { + ssat[obs[i].sat-1].azel[0] = azel_[ i*2]; + ssat[obs[i].sat-1].azel[1] = azel_[1+i*2]; + ssat[obs[i].sat-1].snr[0] = obs[i].SNR[0]; + if (!vsat[i]) continue; + ssat[obs[i].sat-1].vs = 1; + ssat[obs[i].sat-1].resp[0] = resp[i]; + } + } + free(rs); + free(dts); + free(var); + free(azel_); + free(resp); + return stat; +} diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.h b/src/algorithms/libs/rtklib/rtklib_pntpos.h new file mode 100644 index 000000000..5dd969943 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.h @@ -0,0 +1,159 @@ +/*! + * \file rtklib_pntpos.h + * \brief standard code-based positioning + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_PNTPOS_H_ +#define GNSS_SDR_RTKLIB_PNTPOS_H_ + +#include "rtklib.h" +#include "rtklib_rtkcmn.h" + +/* constants -----------------------------------------------------------------*/ +const int NX = 4 + 3; //!< # of estimated parameters +const int MAXITR = 10; //!< max number of iteration for point pos +const double ERR_ION = 5.0; //!< ionospheric delay std (m) +const double ERR_TROP = 3.0; //!< tropspheric delay std (m) + + +/* pseudorange measurement error variance ------------------------------------*/ +double varerr(const prcopt_t *opt, double el, int sys); + +/* get tgd parameter (m) -----------------------------------------------------*/ +double gettgd(int sat, const nav_t *nav); + +/* psendorange with code bias correction -------------------------------------*/ +double prange(const obsd_t *obs, const nav_t *nav, const double *azel, + int iter, const prcopt_t *opt, double *var); + +/* ionospheric correction ------------------------------------------------------ +* compute ionospheric correction +* args : gtime_t time I time +* nav_t *nav I navigation data +* int sat I satellite number +* double *pos I receiver position {lat,lon,h} (rad|m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* int ionoopt I ionospheric correction option (IONOOPT_???) +* double *ion O ionospheric delay (L1) (m) +* double *var O ionospheric delay (L1) variance (m^2) +* return : status(1:ok,0:error) +*-----------------------------------------------------------------------------*/ +int ionocorr(gtime_t time, const nav_t *nav, int sat, const double *pos, + const double *azel, int ionoopt, double *ion, double *var); +/* tropospheric correction ----------------------------------------------------- +* compute tropospheric correction +* args : gtime_t time I time +* nav_t *nav I navigation data +* double *pos I receiver position {lat,lon,h} (rad|m) +* double *azel I azimuth/elevation angle {az,el} (rad) +* int tropopt I tropospheric correction option (TROPOPT_???) +* double *trp O tropospheric delay (m) +* double *var O tropospheric delay variance (m^2) +* return : status(1:ok,0:error) +*-----------------------------------------------------------------------------*/ +int tropcorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, int tropopt, double *trp, double *var); + +/* pseudorange residuals -----------------------------------------------------*/ +int rescode(int iter, const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, const prcopt_t *opt, + double *v, double *H, double *var, double *azel, int *vsat, + double *resp, int *ns); + +/* validate solution ---------------------------------------------------------*/ +int valsol(const double *azel, const int *vsat, int n, + const prcopt_t *opt, const double *v, int nv, int nx, + char *msg); + +/* estimate receiver position ------------------------------------------------*/ +int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, + const double *vare, const int *svh, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, + double *resp, char *msg); + +/* raim fde (failure detection and exclution) -------------------------------*/ +int raim_fde(const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + double *azel, int *vsat, double *resp, char *msg); + +/* doppler residuals ---------------------------------------------------------*/ +int resdop(const obsd_t *obs, int n, const double *rs, const double *dts, + const nav_t *nav, const double *rr, const double *x, + const double *azel, const int *vsat, double *v, double *H); + +/* estimate receiver velocity ------------------------------------------------*/ +void estvel(const obsd_t *obs, int n, const double *rs, const double *dts, + const nav_t *nav, const prcopt_t *opt, sol_t *sol, + const double *azel, const int *vsat); + +/*! +* \brief single-point positioning +* compute receiver position, velocity, clock bias by single-point positioning +* with pseudorange and doppler observables +* args : obsd_t *obs I observation data +* int n I number of observation data +* nav_t *nav I navigation data +* prcopt_t *opt I processing options +* sol_t *sol IO solution +* double *azel IO azimuth/elevation angle (rad) (NULL: no output) +* ssat_t *ssat IO satellite status (NULL: no output) +* char *msg O error message for error exit +* return : status(1:ok,0:error) +* notes : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and +* receiver bias are negligible (only involving glonass-gps time offset +* and receiver bias) +*/ +int pntpos(const obsd_t *obs, int n, const nav_t *nav, + const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, + char *msg); + +#endif /* GNSS_SDR_RTKLIB_PNTPOS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.cc b/src/algorithms/libs/rtklib/rtklib_ppp.cc new file mode 100644 index 000000000..5fe24ed68 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ppp.cc @@ -0,0 +1,1700 @@ +/*! + * \file rtklib_ppp.cc + * \brief Precise Point Positioning + * \authors
      + *
    • 2007-2008, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2008, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_ppp.h" +#include "rtklib_rtkcmn.h" +#include "rtklib_sbas.h" +#include "rtklib_ephemeris.h" +#include "rtklib_ionex.h" +#include "rtklib_tides.h" +#include "rtklib_lambda.h" + +/* wave length of LC (m) -----------------------------------------------------*/ +double lam_LC(int i, int j, int k) +{ + const double f1 = FREQ1, f2 = FREQ2, f5 = FREQ5; + + return SPEED_OF_LIGHT/(i*f1+j*f2+k*f5); +} + + +/* carrier-phase LC (m) ------------------------------------------------------*/ +double L_LC(int i, int j, int k, const double *L) +{ + const double f1 = FREQ1, f2 = FREQ2, f5 = FREQ5; + double L1, L2, L5; + + if ((i && !L[0]) || (j && !L[1]) || (k && !L[2])) + { + return 0.0; + } + L1 = SPEED_OF_LIGHT/f1*L[0]; + L2 = SPEED_OF_LIGHT/f2*L[1]; + L5 = SPEED_OF_LIGHT/f5*L[2]; + return (i*f1*L1+j*f2*L2+k*f5*L5)/(i*f1+j*f2+k*f5); +} + + +/* pseudorange LC (m) --------------------------------------------------------*/ +double P_LC(int i, int j, int k, const double *P) +{ + const double f1 = FREQ1, f2 = FREQ2, f5 = FREQ5; + double P1, P2, P5; + + if ((i && !P[0]) || (j && !P[1]) || (k && !P[2])) + { + return 0.0; + } + P1 = P[0]; + P2 = P[1]; + P5 = P[2]; + return (i*f1*P1+j*f2*P2+k*f5*P5)/(i*f1+j*f2+k*f5); +} + + +/* noise variance of LC (m) --------------------------------------------------*/ +double var_LC(int i, int j, int k, double sig) +{ + const double f1 = FREQ1, f2 = FREQ2, f5 = FREQ5; + + return (std::pow(i*f1, 2.0)+std::pow(j*f2, 2.0)+std::pow(k*f5, 2.0))/std::pow(i*f1+j*f2+k*f5, 2.0)*std::pow(sig, 2.0); +} + + +/* complementaty error function (ref [1] p.227-229) --------------------------*/ +double p_gamma(double a, double x, double log_gamma_a) +{ + double y, w; + int i; + + if (x == 0.0) return 0.0; + if (x >= a+1.0) return 1.0-q_gamma(a, x, log_gamma_a); + + y = w = exp(a*log(x)-x-log_gamma_a)/a; + + for (i = 1;i<100;i++) + { + w *= x/(a+i); + y += w; + if (fabs(w)<1E-15) break; + } + return y; +} + + +double q_gamma(double a, double x, double log_gamma_a) +{ + double y, w, la = 1.0, lb = x+1.0-a, lc; + int i; + + if (x= 0.0?q_gamma(0.5, x*x, LOG_PI/2.0):1.0+p_gamma(0.5, x*x, LOG_PI/2.0); +} + + +/* confidence function of integer ambiguity ----------------------------------*/ +double conffunc(int N, double B, double sig) +{ + double x, p = 1.0; + int i; + + x = fabs(B-N); + for (i = 1;i<8;i++) + { + p -= f_erfc((i-x)/(SQRT2*sig))-f_erfc((i+x)/(SQRT2*sig)); + } + return p; +} + + +/* average LC ----------------------------------------------------------------*/ +void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav __attribute__((unused)), + const double *azel) +{ + ambc_t *amb; + double LC1, LC2, LC3, var1, var2, var3, sig; + int i, j, sat; + + for (i = 0;iopt.elmin) continue; + + if (satsys(sat, NULL) != SYS_GPS) continue; + + /* triple-freq carrier and code LC (m) */ + LC1 = L_LC(1, -1, 0, obs[i].L)-P_LC(1, 1, 0, obs[i].P); + LC2 = L_LC(0, 1, -1, obs[i].L)-P_LC(0, 1, 1, obs[i].P); + LC3 = L_LC(1, -6, 5, obs[i].L)-P_LC(1, 1, 0, obs[i].P); + + sig = std::sqrt(std::pow(rtk->opt.err[1], 2.0)+std::pow(rtk->opt.err[2]/sin(azel[1+2*i]), 2.0)); + + /* measurement noise variance (m) */ + var1 = var_LC(1, 1, 0, sig*rtk->opt.eratio[0]); + var2 = var_LC(0, 1, 1, sig*rtk->opt.eratio[0]); + var3 = var_LC(1, 1, 0, sig*rtk->opt.eratio[0]); + + amb = rtk->ambc+sat-1; + + if (rtk->ssat[sat-1].slip[0] || rtk->ssat[sat-1].slip[1] || + rtk->ssat[sat-1].slip[2] || amb->n[0] == 0.0 || + fabs(timediff(amb->epoch[0], obs[0].time))>MIN_ARC_GAP) + { + + amb->n[0] = amb->n[1] = amb->n[2] = 0.0; + amb->LC[0] = amb->LC[1] = amb->LC[2] = 0.0; + amb->LCv[0] = amb->LCv[1] = amb->LCv[2] = 0.0; + amb->fixcnt = 0; + for (j = 0;jflags[j] = 0; + } + /* averaging */ + if (LC1) + { + amb->n[0] += 1.0; + amb->LC [0] += (LC1 -amb->LC [0])/amb->n[0]; + amb->LCv[0] += (var1-amb->LCv[0])/amb->n[0]; + } + if (LC2) + { + amb->n[1] += 1.0; + amb->LC [1] += (LC2 -amb->LC [1])/amb->n[1]; + amb->LCv[1] += (var2-amb->LCv[1])/amb->n[1]; + } + if (LC3) + { + amb->n[2] += 1.0; + amb->LC [2] += (LC3 -amb->LC [2])/amb->n[2]; + amb->LCv[2] += (var3-amb->LCv[2])/amb->n[2]; + } + amb->epoch[0] = obs[0].time; + } +} + + +/* fix wide-lane ambiguity ---------------------------------------------------*/ +int fix_amb_WL(rtk_t *rtk, const nav_t *nav, int sat1, int sat2, int *NW) +{ + ambc_t *amb1, *amb2; + double BW, vW, lam_WL = lam_LC(1, -1, 0); + + amb1 = rtk->ambc+sat1-1; + amb2 = rtk->ambc+sat2-1; + if (!amb1->n[0] || !amb2->n[0]) return 0; + + /* wide-lane ambiguity */ +#ifndef REV_WL_FCB + BW = (amb1->LC[0]-amb2->LC[0])/lam_WL+nav->wlbias[sat1-1]-nav->wlbias[sat2-1]; +#else + BW = (amb1->LC[0]-amb2->LC[0])/lam_WL-nav->wlbias[sat1-1]+nav->wlbias[sat2-1]; +#endif + *NW = ROUND_PPP(BW); + + /* variance of wide-lane ambiguity */ + vW = (amb1->LCv[0]/amb1->n[0]+amb2->LCv[0]/amb2->n[0])/std::pow(lam_WL, 2.0); + + /* validation of integer wide-lane ambigyity */ + return fabs(*NW-BW) <= rtk->opt.thresar[2] && + conffunc(*NW, BW, sqrt(vW)) >= rtk->opt.thresar[1]; +} + + +/* linear dependency check ---------------------------------------------------*/ +int is_depend(int sat1, int sat2, int *flgs, int *max_flg) +{ + int i; + + if (flgs[sat1-1] == 0 && flgs[sat2-1] == 0){ + flgs[sat1-1]=flgs[sat2-1]=++(*max_flg); + } + else if (flgs[sat1-1] == 0 && flgs[sat2-1] != 0) + { + flgs[sat1-1] = flgs[sat2-1]; + } + else if (flgs[sat1-1] != 0 && flgs[sat2-1] == 0) + { + flgs[sat2-1] = flgs[sat1-1]; + } + else if (flgs[sat1-1]>flgs[sat2-1]) + { + for (i = 0;i= var[j-1]) continue; + SWAP_I(sat1[j], sat1[j-1]); + SWAP_I(sat2[j], sat2[j-1]); + SWAP_D(N[j], N[j-1]); + SWAP_D(var[j], var[j-1]); + } + /* select linearly independent satellite pair */ + for (i = j = 0;inx, n); R = zeros(n, n); + + /* constraints to fixed ambiguities */ + for (i = 0;iopt); + k = IB_PPP(sat2[i], &rtk->opt); + v[i] = NC[i]-(rtk->x[j]-rtk->x[k]); + H[j+i*rtk->nx] = 1.0; + H[k+i*rtk->nx] = -1.0; + R[i+i*n] = std::pow(CONST_AMB, 2.0); + } + /* update states with constraints */ + if ((info = filter(rtk->x, rtk->P, H, v, R, rtk->nx, n))) + { + trace(1, "filter error (info=%d)\n", info); + free(v); free(H); free(R); + return 0; + } + /* set solution */ + for (i = 0;ina;i++) + { + rtk->xa[i] = rtk->x[i]; + for (j = 0;jna;j++) + { + rtk->Pa[i+j*rtk->na] = rtk->Pa[j+i*rtk->na] = rtk->P[i+j*rtk->nx]; + } + } + /* set flags */ + for (i = 0;iambc[sat1[i]-1].flags[sat2[i]-1] = 1; + rtk->ambc[sat2[i]-1].flags[sat1[i]-1] = 1; + } + free(v); free(H); free(R); + return 1; +} + + +/* fix narrow-lane ambiguity by rounding -------------------------------------*/ +int fix_amb_ROUND(rtk_t *rtk, int *sat1, int *sat2, const int *NW, int n) +{ + double C1, C2, B1, v1, BC, v, vc, *NC, *var, lam_NL = lam_LC(1, 1, 0), lam1, lam2; + int i, j, k, m = 0, N1, stat; + + lam1 = lam_carr[0]; lam2 = lam_carr[1]; + + C1 = std::pow(lam2, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0)); + C2 = -std::pow(lam1, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0)); + + NC = zeros(n, 1); var = zeros(n, 1); + + for (i = 0;iopt); + k = IB_PPP(sat2[i], &rtk->opt); + + /* narrow-lane ambiguity */ + B1 = (rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL; + N1 = ROUND_PPP(B1); + + /* variance of narrow-lane ambiguity */ + var[m] = rtk->P[j+j*rtk->nx]+rtk->P[k+k*rtk->nx]-2.0*rtk->P[j+k*rtk->nx]; + v1 = var[m]/std::pow(lam_NL, 2.0); + + /* validation of narrow-lane ambiguity */ + if (fabs(N1-B1)>rtk->opt.thresar[2] || + conffunc(N1, B1, sqrt(v1))opt.thresar[1]) + { + continue; + } + /* iono-free ambiguity (m) */ + BC = C1*lam1*N1+C2*lam2*(N1-NW[i]); + + /* check residuals */ + v = rtk->ssat[sat1[i]-1].resc[0]-rtk->ssat[sat2[i]-1].resc[0]; + vc = v+(BC-(rtk->x[j]-rtk->x[k])); + if (fabs(vc)>THRES_RES) continue; + + sat1[m] = sat1[i]; + sat2[m] = sat2[i]; + NC[m++] = BC; + } + /* select fixed ambiguities by dependancy check */ + m = sel_amb(sat1, sat2, NC, var, m); + + /* fixed solution */ + stat = fix_sol(rtk, sat1, sat2, NC, m); + + free(NC); free(var); + + return stat && m >= 3; +} + + +/* fix narrow-lane ambiguity by ILS ------------------------------------------*/ +int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n) +{ + double C1, C2, *B1, *N1, *NC, *D, *E, *Q, s[2], lam_NL = lam_LC(1, 1, 0), lam1, lam2; + int i, j, k, m = 0, info, stat, flgs[MAXSAT] = {0}, max_flg = 0; + + lam1 = lam_carr[0]; lam2 = lam_carr[1]; + + C1 = std::pow(lam2, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0)); + C2 = -std::pow(lam1, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0)); + + B1 = zeros(n, 1); N1 = zeros(n, 2); D = zeros(rtk->nx, n); E = mat(n, rtk->nx); + Q = mat(n, n); NC = mat(n, 1); + + for (i = 0;iopt); + k = IB_PPP(sat2[i], &rtk->opt); + + /* float narrow-lane ambiguity (cycle) */ + B1[m] = (rtk->x[j]-rtk->x[k]+C2*lam2*NW[i])/lam_NL; + N1[m] = ROUND_PPP(B1[m]); + + /* validation of narrow-lane ambiguity */ + if (fabs(N1[m]-B1[m])>rtk->opt.thresar[2]) continue; + + /* narrow-lane ambiguity transformation matrix */ + D[j+m*rtk->nx] = 1.0/lam_NL; + D[k+m*rtk->nx] = -1.0/lam_NL; + + sat1[m] = sat1[i]; + sat2[m] = sat2[i]; + NW[m++] = NW[i]; + } + if (m<3) return 0; + + /* covariance of narrow-lane ambiguities */ + matmul("TN", m, rtk->nx, rtk->nx, 1.0, D, rtk->P, 0.0, E); + matmul("NN", m, m, rtk->nx, 1.0, E, D, 0.0, Q); + + /* integer least square */ + if ((info = lambda(m, 2, B1, Q, N1, s))) + { + trace(2, "lambda error: info=%d\n", info); + return 0; + } + if (s[0] <= 0.0) return 0; + + rtk->sol.ratio = (float)(MIN_PPP(s[1]/s[0], 999.9)); + + /* varidation by ratio-test */ + if (rtk->opt.thresar[0]>0.0 && rtk->sol.ratioopt.thresar[0]) + { + trace(2, "varidation error: n=%2d ratio=%8.3f\n", m, rtk->sol.ratio); + return 0; + } + trace(2, "varidation ok: %s n=%2d ratio=%8.3f\n", time_str(rtk->sol.time, 0), m, + rtk->sol.ratio); + + /* narrow-lane to iono-free ambiguity */ + for (i = 0;iopt.ionoopt != IONOOPT_IFLC || rtk->opt.nf<2) return 0; + + trace(3, "pppamb: time=%s n=%d\n", time_str(obs[0].time, 0), n); + + elmask = rtk->opt.elmaskar>0.0?rtk->opt.elmaskar:rtk->opt.elmin; + + sat1 = imat(n*n, 1); sat2 = imat(n*n, 1); NW = imat(n*n, 1); + + /* average LC */ + average_LC(rtk, obs, n, nav, azel); + + /* fix wide-lane ambiguity */ + for (i = 0;issat[obs[i].sat-1].vsat[0] || + !rtk->ssat[obs[j].sat-1].vsat[0] || + azel[1+i*2]ambc[obs[i].sat-1].flags[obs[j].sat-1] && + rtk->ambc[obs[j].sat-1].flags[obs[i].sat-1]) continue; +#endif + sat1[m] = obs[i].sat; + sat2[m] = obs[j].sat; + if (fix_amb_WL(rtk, nav, sat1[m], sat2[m], NW+m)) m++; + } + /* fix narrow-lane ambiguity */ + if (rtk->opt.modear == ARMODE_PPPAR) + { + stat = fix_amb_ROUND(rtk, sat1, sat2, NW, m); + } + else if (rtk->opt.modear == ARMODE_PPPAR_ILS) + { + stat = fix_amb_ILS(rtk, sat1, sat2, NW, m); + } + free(sat1); free(sat2); free(NW); + + return stat; +} + + +void pppoutsolstat(rtk_t *rtk, int level, FILE *fp) +{ + ssat_t *ssat; + double tow, pos[3], vel[3], acc[3]; + int i, j, week, nfreq = 1; + char id[32]; + + if (level <= 0 || !fp) return; + + trace(3, "pppoutsolstat:\n"); + + tow = time2gpst(rtk->sol.time, &week); + + /* receiver position */ + fprintf(fp, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, + rtk->sol.stat, rtk->x[0], rtk->x[1], rtk->x[2], 0.0, 0.0, 0.0); + + /* receiver velocity and acceleration */ + if (rtk->opt.dynamics) + { + ecef2pos(rtk->sol.rr, pos); + ecef2enu(pos, rtk->x+3, vel); + ecef2enu(pos, rtk->x+6, acc); + fprintf(fp, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[2], + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + } + /* receiver clocks */ + i = IC_PPP(0, &rtk->opt); + fprintf(fp, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", + week, tow, rtk->sol.stat, 1, rtk->x[i]*1E9/SPEED_OF_LIGHT, rtk->x[i+1]*1E9/SPEED_OF_LIGHT, + 0.0, 0.0); + + /* tropospheric parameters */ + if (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG) + { + i = IT_PPP(&rtk->opt); + fprintf(fp, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, + 1, rtk->x[i], 0.0); + } + if (rtk->opt.tropopt == TROPOPT_ESTG) + { + i = IT_PPP(&rtk->opt); + fprintf(fp, "$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n", week, tow, + rtk->sol.stat, 1, rtk->x[i+1], rtk->x[i+2], 0.0, 0.0); + } + if (rtk->sol.stat == SOLQ_NONE || level <= 1) return; + + /* residuals and status */ + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + for (j = 0;jazel[0]*R2D, ssat->azel[1]*R2D, + ssat->resp[j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25, + ssat->fix[j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j], + ssat->slipc[j], ssat->rejc[j]); + } + } +} + + +/* solar/lunar tides (ref [2] 7) ---------------------------------------------*/ +void tide_pl(const double *eu, const double *rp, double GMp, + const double *pos, double *dr) +{ + const double H3 = 0.292, L3 = 0.015; + double r, ep[3], latp, lonp, p, K2, K3, a, H2, L2, dp, du, cosp, sinl, cosl; + int i; + + trace(4, "tide_pl : pos=%.3f %.3f\n", pos[0]*R2D, pos[1]*R2D); + + if ((r = norm_rtk(rp, 3)) <= 0.0) return; + + for (i = 0;i<3;i++) ep[i] = rp[i]/r; + + K2 = GMp / GME * std::pow(RE_WGS84, 2.0) * std::pow(RE_WGS84, 2.0) / (r*r*r); + K3 = K2*RE_WGS84/r; + latp = asin(ep[2]); lonp = atan2(ep[1], ep[0]); + cosp = cos(latp); sinl = sin(pos[0]); cosl = cos(pos[0]); + + /* step1 in phase (degree 2) */ + p = (3.0*sinl*sinl-1.0)/2.0; + H2 = 0.6078-0.0006*p; + L2 = 0.0847+0.0002*p; + a = dot(ep, eu, 3); + dp = K2*3.0*L2*a; + du = K2*(H2*(1.5*a*a-0.5)-3.0*L2*a*a); + + /* step1 in phase (degree 3) */ + dp += K3*L3*(7.5*a*a-1.5); + du += K3*(H3*(2.5*a*a*a-1.5*a)-L3*(7.5*a*a-1.5)*a); + + /* step1 out-of-phase (only radial) */ + du += 3.0/4.0*0.0025*K2*sin(2.0*latp)*sin(2.0*pos[0])*sin(pos[1]-lonp); + du += 3.0/4.0*0.0022*K2*cosp*cosp*cosl*cosl*sin(2.0*(pos[1]-lonp)); + + dr[0] = dp*ep[0]+du*eu[0]; + dr[1] = dp*ep[1]+du*eu[1]; + dr[2] = dp*ep[2]+du*eu[2]; + + trace(5, "tide_pl : dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]); +} + + +/* displacement by solid earth tide (ref [2] 7) ------------------------------*/ +void tide_solid(const double *rsun, const double *rmoon, + const double *pos, const double *E, double gmst, int opt, + double *dr) +{ + double dr1[3], dr2[3], eu[3], du, dn, sinl, sin2l; + + trace(3, "tide_solid: pos=%.3f %.3f opt=%d\n", pos[0]*R2D, pos[1]*R2D, opt); + + /* step1: time domain */ + eu[0] = E[2]; eu[1] = E[5]; eu[2] = E[8]; + tide_pl(eu, rsun, GMS, pos, dr1); + tide_pl(eu, rmoon, GMM, pos, dr2); + + /* step2: frequency domain, only K1 radial */ + sin2l = sin(2.0*pos[0]); + du = -0.012*sin2l*sin(gmst+pos[1]); + + dr[0] = dr1[0]+dr2[0]+du*E[2]; + dr[1] = dr1[1]+dr2[1]+du*E[5]; + dr[2] = dr1[2]+dr2[2]+du*E[8]; + + /* eliminate permanent deformation */ + if (opt&8) + { + sinl = sin(pos[0]); + du = 0.1196*(1.5*sinl*sinl-0.5); + dn = 0.0247*sin2l; + dr[0] += du*E[2]+dn*E[1]; + dr[1] += du*E[5]+dn*E[4]; + dr[2] += du*E[8]+dn*E[7]; + } + trace(5, "tide_solid: dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]); +} + + +/* displacement by ocean tide loading (ref [2] 7) ----------------------------*/ +void tide_oload(gtime_t tut, const double *odisp, double *denu) +{ + const double args[][5] = { + {1.40519E-4, 2.0,-2.0, 0.0, 0.00}, /* M2 */ + {1.45444E-4, 0.0, 0.0, 0.0, 0.00}, /* S2 */ + {1.37880E-4, 2.0,-3.0, 1.0, 0.00}, /* N2 */ + {1.45842E-4, 2.0, 0.0, 0.0, 0.00}, /* K2 */ + {0.72921E-4, 1.0, 0.0, 0.0, 0.25}, /* K1 */ + {0.67598E-4, 1.0,-2.0, 0.0,-0.25}, /* O1 */ + {0.72523E-4,-1.0, 0.0, 0.0,-0.25}, /* P1 */ + {0.64959E-4, 1.0,-3.0, 1.0,-0.25}, /* Q1 */ + {0.53234E-5, 0.0, 2.0, 0.0, 0.00}, /* Mf */ + {0.26392E-5, 0.0, 1.0,-1.0, 0.00}, /* Mm */ + {0.03982E-5, 2.0, 0.0, 0.0, 0.00} /* Ssa */ + }; + const double ep1975[] = {1975, 1, 1, 0, 0, 0}; + double ep[6], fday, days, t, t2, t3, a[5], ang, dp[3] = {0}; + int i, j; + + trace(3, "tide_oload:\n"); + + /* angular argument: see subroutine arg.f for reference [1] */ + time2epoch(tut, ep); + fday = ep[3]*3600.0+ep[4]*60.0+ep[5]; + ep[3] = ep[4] = ep[5] = 0.0; + days = timediff(epoch2time(ep), epoch2time(ep1975))/86400.0; + t = (27392.500528+1.000000035*days)/36525.0; + t2 = t*t; t3 = t2*t; + + a[0] = fday; + a[1] = (279.69668+36000.768930485*t+3.03E-4*t2)*D2R; /* H0 */ + a[2] = (270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R; /* S0 */ + a[3] = (334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R; /* P0 */ + a[4] = 2.0*PI; + + /* displacements by 11 constituents */ + for (i = 0;i<11;i++) + { + ang = 0.0; + for (j = 0;j<5;j++) ang += a[j]*args[i][j]; + for (j = 0;j<3;j++) dp[j] += odisp[j+i*6]*cos(ang-odisp[j+3+i*6]*D2R); + } + denu[0] = -dp[1]; + denu[1] = -dp[2]; + denu[2] = dp[0]; + + trace(5, "tide_oload: denu=%.3f %.3f %.3f\n", denu[0], denu[1], denu[2]); +} + + +/* iers mean pole (ref [7] eq.7.25) ------------------------------------------*/ +void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar) +{ + const double ep2000[] = {2000, 1, 1, 0, 0, 0}; + double y, y2, y3; + + y = timediff(tut, epoch2time(ep2000))/86400.0/365.25; + + if (y<3653.0/365.25) + { /* until 2010.0 */ + y2 = y*y; y3 = y2*y; + *xp_bar = 55.974+1.8243*y+0.18413*y2+0.007024*y3; /* (mas) */ + *yp_bar = 346.346+1.7896*y-0.10729*y2-0.000908*y3; + } + else + { /* after 2010.0 */ + *xp_bar = 23.513+7.6141*y; /* (mas) */ + *yp_bar = 358.891-0.6287*y; + } +} + + +/* displacement by pole tide (ref [7] eq.7.26) --------------------------------*/ +void tide_pole(gtime_t tut, const double *pos, const double *erpv, + double *denu) +{ + double xp_bar, yp_bar, m1, m2, cosl, sinl; + + trace(3, "tide_pole: pos=%.3f %.3f\n", pos[0]*R2D, pos[1]*R2D); + + /* iers mean pole (mas) */ + iers_mean_pole(tut, &xp_bar, &yp_bar); + + /* ref [7] eq.7.24 */ + m1 = erpv[0]/AS2R-xp_bar*1E-3; /* (as) */ + m2 = -erpv[1]/AS2R+yp_bar*1E-3; + + /* sin(2*theta) = sin(2*phi), cos(2*theta)=-cos(2*phi) */ + cosl = cos(pos[1]); + sinl = sin(pos[1]); + denu[0] = 9E-3*sin(pos[0]) *(m1*sinl-m2*cosl); /* de= Slambda (m) */ + denu[1] = -9E-3*cos(2.0*pos[0])*(m1*cosl+m2*sinl); /* dn=-Stheta (m) */ + denu[2] = -33E-3*sin(2.0*pos[0])*(m1*cosl+m2*sinl); /* du= Sr (m) */ + + trace(5, "tide_pole : denu=%.3f %.3f %.3f\n", denu[0], denu[1], denu[2]); +} + + +/* tidal displacement ---------------------------------------------------------- + * displacements by earth tides + * args : gtime_t tutc I time in utc + * double *rr I site position (ecef) (m) + * int opt I options (or of the followings) + * 1: solid earth tide + * 2: ocean tide loading + * 4: pole tide + * 8: elimate permanent deformation + * double *erp I earth rotation parameters (NULL: not used) + * double *odisp I ocean loading parameters (NULL: not used) + * odisp[0+i*6]: consituent i amplitude radial(m) + * odisp[1+i*6]: consituent i amplitude west (m) + * odisp[2+i*6]: consituent i amplitude south (m) + * odisp[3+i*6]: consituent i phase radial (deg) + * odisp[4+i*6]: consituent i phase west (deg) + * odisp[5+i*6]: consituent i phase south (deg) + * (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1, + * 8:Mf,9:Mm,10:Ssa) + * double *dr O displacement by earth tides (ecef) (m) + * return : none + * notes : see ref [1], [2] chap 7 + * see ref [4] 5.2.1, 5.2.2, 5.2.3 + * ver.2.4.0 does not use ocean loading and pole tide corrections + *-----------------------------------------------------------------------------*/ +void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, + const double *odisp, double *dr) +{ + gtime_t tut; + double pos[2], E[9], drt[3], denu[3], rs[3], rm[3], gmst, erpv[5] = {0}; + int i; +#ifdef IERS_MODEL + double ep[6], fhr; + int year, mon, day; +#endif + + trace(3, "tidedisp: tutc=%s\n", time_str(tutc, 0)); + + if (erp) geterp(erp, tutc, erpv); + + tut = timeadd(tutc, erpv[2]); + + dr[0] = dr[1] = dr[2] = 0.0; + + if (norm_rtk(rr, 3) <= 0.0) return; + + pos[0] = asin(rr[2]/norm_rtk(rr, 3)); + pos[1] = atan2(rr[1], rr[0]); + xyz2enu(pos, E); + + if (opt&1) + { /* solid earth tides */ + + /* sun and moon position in ecef */ + sunmoonpos(tutc, erpv, rs, rm, &gmst); + +#ifdef IERS_MODEL + time2epoch(tutc, ep); + year = (int)ep[0]; + mon = (int)ep[1]; + day = (int)ep[2]; + fhr = ep[3]+ep[4]/60.0+ep[5]/3600.0; + + /* call DEHANTTIDEINEL */ + dehanttideinel_((double *)rr, &year, &mon, &day, &fhr, rs, rm, drt); +#else + tide_solid(rs, rm, pos, E, gmst, opt, drt); +#endif + for (i = 0;i<3;i++) dr[i] += drt[i]; + } + if ((opt&2) && odisp) + { /* ocean tide loading */ + tide_oload(tut, odisp, denu); + matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt); + for (i = 0;i<3;i++) dr[i] += drt[i]; + } + if ((opt&4) && erp) + { /* pole tide */ + tide_pole(tut, pos, erpv, denu); + matmul("TN", 3, 1, 3, 1.0, E, denu, 0.0, drt); + for (i = 0;i<3;i++) dr[i] += drt[i]; + } + trace(5, "tidedisp: dr=%.3f %.3f %.3f\n", dr[0], dr[1], dr[2]); +} + + +/* exclude meas of eclipsing satellite (block IIA) ---------------------------*/ +void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs) +{ + double rsun[3], esun[3], r, ang, erpv[5] = {0}, cosa; + int i, j; + const char *type; + + trace(3, "testeclipse:\n"); + + /* unit vector of sun direction (ecef) */ + sunmoonpos(gpst2utc(obs[0].time), erpv, rsun, NULL, NULL); + normv3(rsun, esun); + + for (i = 0;ipcvs[obs[i].sat-1].type; + + if ((r = norm_rtk(rs+i*6, 3)) <= 0.0) continue; +#if 1 + /* only block IIA */ + if (*type && !strstr(type, "BLOCK IIA")) continue; +#endif + /* sun-earth-satellite angle */ + cosa = dot(rs+i*6, esun, 3)/r; + cosa = cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa); + ang = acos(cosa); + + /* test eclipse */ + if (angRE_WGS84) continue; + + trace(2, "eclipsing sat excluded %s sat=%2d\n", time_str(obs[0].time, 0), + obs[i].sat); + + for (j = 0;j<3;j++) rs[j+i*6] = 0.0; + } +} + + +/* measurement error variance ------------------------------------------------*/ +double varerr(int sat __attribute__((unused)), int sys, double el, int type, const prcopt_t *opt) +{ + double a, b, a2, b2, fact = 1.0; + double sinel = sin(el); + int i = sys == SYS_GLO?1:(sys == SYS_GAL?2:0); + + /* extended error model */ + if (type == 1 && opt->exterr.ena[0]) + { /* code */ + a = opt->exterr.cerr[i][0]; + b = opt->exterr.cerr[i][1]; + if (opt->ionoopt == IONOOPT_IFLC) + { + a2 = opt->exterr.cerr[i][2]; + b2 = opt->exterr.cerr[i][3]; + a = std::sqrt(std::pow(2.55, 2.0)*a*a+std::pow(1.55, 2.0)*a2*a2); + b = std::sqrt(std::pow(2.55, 2.0)*b*b+std::pow(1.55, 2.0)*b2*b2); + } + } + else if (type == 0 && opt->exterr.ena[1]) + { /* phase */ + a = opt->exterr.perr[i][0]; + b = opt->exterr.perr[i][1]; + if (opt->ionoopt == IONOOPT_IFLC) + { + a2 = opt->exterr.perr[i][2]; + b2 = opt->exterr.perr[i][3]; + a = std::sqrt(std::pow(2.55, 2.0)*a*a+std::pow(1.55, 2.0)*a2*a2); + b = std::sqrt(std::pow(2.55, 2.0)*b*b+std::pow(1.55, 2.0)*b2*b2); + } + } + else + { /* normal error model */ + if (type == 1) fact *= opt->eratio[0]; + fact *= sys == SYS_GLO?EFACT_GLO:(sys == SYS_SBS?EFACT_SBS:EFACT_GPS); + if (opt->ionoopt == IONOOPT_IFLC) fact *= 3.0; + a = fact*opt->err[1]; + b = fact*opt->err[2]; + } + return a*a+b*b/sinel/sinel; +} + + +/* initialize state and covariance -------------------------------------------*/ +void initx(rtk_t *rtk, double xi, double var, int i) +{ + int j; + rtk->x[i] = xi; + for (j = 0;jnx;j++) + { + rtk->P[i+j*rtk->nx] = rtk->P[j+i*rtk->nx] = i == j?var:0.0; + } +} + + +/* dual-frequency iono-free measurements -------------------------------------*/ +int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel, + const prcopt_t *opt, const double *dantr, const double *dants, + double phw, double *meas, double *var) +{ + const double *lam = nav->lam[obs->sat-1]; + double c1, c2, L1, L2, P1, P2, P1_C1, P2_C2, gamma; + int i = 0, j = 1, k; + + trace(4, "ifmeas :\n"); + + /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */ + if (NFREQ >= 3 && (satsys(obs->sat, NULL)&(SYS_GAL|SYS_SBS))) j = 2; + + if (NFREQ<2 || lam[i] == 0.0 || lam[j] == 0.0) return 0; + + /* test snr mask */ + if (testsnr(0, i, azel[1], obs->SNR[i]*0.25, &opt->snrmask) || + testsnr(0, j, azel[1], obs->SNR[j]*0.25, &opt->snrmask)) + { + return 0; + } + gamma = std::pow(lam[j], 2.0)/std::pow(lam[i], 2.0); /* f1^2/f2^2 */ + c1 = gamma/(gamma-1.0); /* f1^2/(f1^2-f2^2) */ + c2 = -1.0 /(gamma-1.0); /* -f2^2/(f1^2-f2^2) */ + + L1 = obs->L[i]*lam[i]; /* cycle -> m */ + L2 = obs->L[j]*lam[j]; + P1 = obs->P[i]; + P2 = obs->P[j]; + P1_C1 = nav->cbias[obs->sat-1][1]; + P2_C2 = nav->cbias[obs->sat-1][2]; + if (opt->sateph == EPHOPT_LEX) + { + P1_C1 = nav->lexeph[obs->sat-1].isc[0]*SPEED_OF_LIGHT; /* ISC_L1C/A */ + } + if (L1 == 0.0 || L2 == 0.0 || P1 == 0.0 || P2 == 0.0) return 0; + + /* iono-free phase with windup correction */ + meas[0] = c1*L1+c2*L2-(c1*lam[i]+c2*lam[j])*phw; + + /* iono-free code with dcb correction */ + if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ + if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */ + meas[1] = c1*P1+c2*P2; + var[1] = std::pow(ERR_CBIAS, 2.0); + + if (opt->sateph == EPHOPT_SBAS) meas[1] -= P1_C1; /* sbas clock based C1 */ + + /* gps-glonass h/w bias correction for code */ + if (opt->exterr.ena[3] && satsys(obs->sat, NULL) == SYS_GLO) + { + meas[1] += c1*opt->exterr.gpsglob[0]+c2*opt->exterr.gpsglob[1]; + } + /* antenna phase center variation correction */ + for (k = 0;k<2;k++) + { + if (dants) meas[k] -= c1*dants[i]+c2*dants[j]; + if (dantr) meas[k] -= c1*dantr[i]+c2*dantr[j]; + } + return 1; +} + + +/* get tgd parameter (m) -----------------------------------------------------*/ +double gettgd_ppp(int sat, const nav_t *nav) +{ + int i; + for (i = 0;in;i++) + { + if (nav->eph[i].sat != sat) continue; + return SPEED_OF_LIGHT*nav->eph[i].tgd[0]; + } + return 0.0; +} + + +/* slant ionospheric delay ---------------------------------------------------*/ +int corr_ion(gtime_t time, const nav_t *nav, int sat __attribute__((unused)), const double *pos, + const double *azel, int ionoopt, double *ion, double *var, + int *brk __attribute__((unused))) +{ +#ifdef EXTSTEC + double rate; +#endif + /* sbas ionosphere model */ + if (ionoopt == IONOOPT_SBAS) + { + return sbsioncorr(time, nav, pos, azel, ion, var); + } + /* ionex tec model */ + if (ionoopt == IONOOPT_TEC) + { + return iontec(time, nav, pos, azel, 1, ion, var); + } +#ifdef EXTSTEC + /* slant tec model */ + if (ionoopt == IONOOPT_STEC) + { + return stec_ion(time, nav, sat, pos, azel, ion, &rate, var, brk); + } +#endif + /* broadcast model */ + if (ionoopt == IONOOPT_BRDC) + { + *ion = ionmodel(time, nav->ion_gps, pos, azel); + *var = std::pow(*ion*ERR_BRDCI, 2.0); + return 1; + } + /* ionosphere model off */ + *ion = 0.0; + *var = VAR_IONO_OFF; + return 1; +} + + +/* ionosphere and antenna corrected measurements -----------------------------*/ +int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos, + const double *azel, const prcopt_t *opt, + const double *dantr, const double *dants, double phw, + double *meas, double *var, int *brk) +{ + const double *lam = nav->lam[obs->sat-1]; + double ion = 0.0, L1, P1, PC, P1_P2, P1_C1, vari, gamma; + int i; + + trace(4, "corrmeas:\n"); + + meas[0] = meas[1] = var[0] = var[1] = 0.0; + + /* iono-free LC */ + if (opt->ionoopt == IONOOPT_IFLC) + { + return ifmeas(obs, nav, azel, opt, dantr, dants, phw, meas, var); + } + if (lam[0] == 0.0 || obs->L[0] == 0.0 || obs->P[0] == 0.0) return 0; + + if (testsnr(0, 0, azel[1], obs->SNR[0]*0.25, &opt->snrmask)) return 0; + + L1 = obs->L[0]*lam[0]; + P1 = obs->P[0]; + + /* dcb correction */ + gamma = std::pow(lam[1]/lam[0], 2.0); /* f1^2/f2^2 */ + P1_P2 = nav->cbias[obs->sat-1][0]; + P1_C1 = nav->cbias[obs->sat-1][1]; + if (P1_P2 == 0.0 && (satsys(obs->sat, NULL)&(SYS_GPS|SYS_GAL|SYS_QZS))) + { + P1_P2 = (1.0-gamma)*gettgd_ppp(obs->sat, nav); + } + if (obs->code[0] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ + PC = P1-P1_P2/(1.0-gamma); /* P1->PC */ + + /* slant ionospheric delay L1 (m) */ + if (!corr_ion(obs->time, nav, obs->sat, pos, azel, opt->ionoopt, &ion, &vari, brk)) + { + trace(2, "iono correction error: time=%s sat=%2d ionoopt=%d\n", + time_str(obs->time, 2), obs->sat, opt->ionoopt); + return 0; + } + /* ionosphere and windup corrected phase and code */ + meas[0] = L1+ion-lam[0]*phw; + meas[1] = PC-ion; + + var[0] += vari; + var[1] += vari+std::pow(ERR_CBIAS, 2.0); + + /* antenna phase center variation correction */ + for (i = 0;i<2;i++) + { + if (dants) meas[i] -= dants[0]; + if (dantr) meas[i] -= dantr[0]; + } + return 1; +} + + +/* L1/L2 geometry-free phase measurement -------------------------------------*/ +double gfmeas(const obsd_t *obs, const nav_t *nav) +{ + const double *lam = nav->lam[obs->sat-1]; + + if (lam[0] == 0.0 || lam[1] == 0.0 || obs->L[0] == 0.0 || obs->L[1] == 0.0) return 0.0; + + return lam[0]*obs->L[0]-lam[1]*obs->L[1]; +} + + +/* temporal update of position -----------------------------------------------*/ +void udpos_ppp(rtk_t *rtk) +{ + int i; + + trace(3, "udpos_ppp:\n"); + + /* fixed mode */ + if (rtk->opt.mode == PMODE_PPP_FIXED) + { + for (i = 0;i<3;i++) initx(rtk, rtk->opt.ru[i], 1E-8, i); + return; + } + /* initialize position for first epoch */ + if (norm_rtk(rtk->x, 3) <= 0.0) + { + for (i = 0;i<3;i++) initx(rtk, rtk->sol.rr[i], VAR_POS_PPP, i); + } + /* static ppp mode */ + if (rtk->opt.mode == PMODE_PPP_STATIC) return; + + /* kinmatic mode without dynamics */ + for (i = 0;i<3;i++) + { + initx(rtk, rtk->sol.rr[i], VAR_POS_PPP, i); + } +} + + +/* temporal update of clock --------------------------------------------------*/ +void udclk_ppp(rtk_t *rtk) +{ + double dtr; + int i; + + trace(3, "udclk_ppp:\n"); + + /* initialize every epoch for clock (white noise) */ + for (i = 0;iopt.sateph == EPHOPT_PREC) + { + /* time of prec ephemeris is based gpst */ + /* negelect receiver inter-system bias */ + dtr = rtk->sol.dtr[0]; + } + else + { + dtr = i == 0?rtk->sol.dtr[0]:rtk->sol.dtr[0]+rtk->sol.dtr[i]; + } + initx(rtk, SPEED_OF_LIGHT*dtr, VAR_CLK, IC_PPP(i, &rtk->opt)); + } +} + + +/* temporal update of tropospheric parameters --------------------------------*/ +void udtrop_ppp(rtk_t *rtk) +{ + double pos[3], azel[] = {0.0, PI/2.0}, ztd, var; + int i = IT_PPP(&rtk->opt), j; + + trace(3, "udtrop_ppp:\n"); + + if (rtk->x[i] == 0.0) + { + ecef2pos(rtk->sol.rr, pos); + ztd = sbstropcorr(rtk->sol.time, pos, azel, &var); + initx(rtk, ztd, var, i); + + if (rtk->opt.tropopt >= TROPOPT_ESTG) + { + for (j = 0;j<2;j++) initx(rtk, 1E-6, VAR_GRA_PPP, ++i); + } + } + else + { + rtk->P[i*(1+rtk->nx)] += std::pow(rtk->opt.prn[2], 2.0)*fabs(rtk->tt); + + if (rtk->opt.tropopt >= TROPOPT_ESTG) + { + for (j = 0;j<2;j++) + { + rtk->P[++i*(1+rtk->nx)] += std::pow(rtk->opt.prn[2]*0.1, 2.0)*fabs(rtk->tt); + } + } + } +} + + +/* detect cycle slip by LLI --------------------------------------------------*/ +void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n) +{ + int i, j; + + trace(3, "detslp_ll: n=%d\n", n); + + for (i = 0;iopt.nf;j++) + { + if (obs[i].L[j] == 0.0 || !(obs[i].LLI[j]&3)) continue; + + trace(3, "detslp_ll: slip detected sat=%2d f=%d\n", obs[i].sat, j+1); + + rtk->ssat[obs[i].sat-1].slip[j] = 1; + } +} + + +/* detect cycle slip by geometry free phase jump -----------------------------*/ +void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + double g0, g1; + int i, j; + + trace(3, "detslp_gf: n=%d\n", n); + + for (i = 0;issat[obs[i].sat-1].gf; + rtk->ssat[obs[i].sat-1].gf = g1; + + trace(4, "detslip_gf: sat=%2d gf0=%8.3f gf1=%8.3f\n", obs[i].sat, g0, g1); + + if (g0 != 0.0 && fabs(g1-g0)>rtk->opt.thresslip) + { + trace(3, "detslip_gf: slip detected sat=%2d gf=%8.3f->%8.3f\n", + obs[i].sat, g0, g1); + + for (j = 0;jopt.nf;j++) rtk->ssat[obs[i].sat-1].slip[j]|=1; + } + } +} + + +/* temporal update of phase biases -------------------------------------------*/ +void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + double meas[2], var[2], bias[MAXOBS] = {0}, offset = 0.0, pos[3] = {0}; + int i, j, k, sat, brk = 0; + + trace(3, "udbias : n=%d\n", n); + + for (i = 0;iopt.nf;j++) + { + rtk->ssat[i].slip[j] = 0; + } + /* detect cycle slip by LLI */ + detslp_ll(rtk, obs, n); + + /* detect cycle slip by geometry-free phase jump */ + detslp_gf(rtk, obs, n, nav); + + /* reset phase-bias if expire obs outage counter */ + for (i = 0;issat[i].outc[0]>(unsigned int)rtk->opt.maxout) + { + initx(rtk, 0.0, 0.0, IB_PPP(i+1, &rtk->opt)); + } + } + ecef2pos(rtk->sol.rr, pos); + + for (i = k = 0;iopt); + if (!corrmeas(obs+i, nav, pos, rtk->ssat[sat-1].azel, &rtk->opt, NULL, NULL, + 0.0, meas, var, &brk)) continue; + + if (brk) + { + rtk->ssat[sat-1].slip[0] = 1; + trace(2, "%s: sat=%2d correction break\n", time_str(obs[i].time, 0), sat); + } + bias[i] = meas[0]-meas[1]; + if (rtk->x[j] == 0.0 || + rtk->ssat[sat-1].slip[0] || rtk->ssat[sat-1].slip[1]) continue; + offset += bias[i]-rtk->x[j]; + k++; + } + /* correct phase-code jump to enssure phase-code coherency */ + if (k >= 2 && fabs(offset/k)>0.0005*SPEED_OF_LIGHT) + { + for (i = 0;iopt); + if (rtk->x[j] != 0.0) rtk->x[j] += offset/k; + } + trace(2, "phase-code jump corrected: %s n=%2d dt=%12.9fs\n", + time_str(rtk->sol.time, 0), k, offset/k/SPEED_OF_LIGHT); + } + for (i = 0;iopt); + + rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[0], 2.0)*fabs(rtk->tt); + + if (rtk->x[j] != 0.0 && + !rtk->ssat[sat-1].slip[0] && !rtk->ssat[sat-1].slip[1]) continue; + + if (bias[i] == 0.0) continue; + + /* reinitialize phase-bias if detecting cycle slip */ + initx(rtk, bias[i], VAR_BIAS, IB_PPP(sat, &rtk->opt)); + + trace(5, "udbias_ppp: sat=%2d bias=%.3f\n", sat, meas[0]-meas[1]); + } +} + + +/* temporal update of states --------------------------------------------------*/ +void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + trace(3, "udstate_ppp: n=%d\n", n); + + /* temporal update of position */ + udpos_ppp(rtk); + + /* temporal update of clock */ + udclk_ppp(rtk); + + /* temporal update of tropospheric parameters */ + if (rtk->opt.tropopt >= TROPOPT_EST) + { + udtrop_ppp(rtk); + } + /* temporal update of phase-bias */ + udbias_ppp(rtk, obs, n, nav); +} + + +/* satellite antenna phase center variation ----------------------------------*/ +void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, + double *dant) +{ + double ru[3], rz[3], eu[3], ez[3], nadir, cosa; + int i; + + for (i = 0;i<3;i++) + { + ru[i] = rr[i]-rs[i]; + rz[i] = -rs[i]; + } + if (!normv3(ru, eu) || !normv3(rz, ez)) return; + + cosa = dot(eu, ez, 3); + cosa = cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa); + nadir = acos(cosa); + + antmodel_s(pcv, nadir, dant); +} + + +/* precise tropospheric model ------------------------------------------------*/ +double prectrop(gtime_t time, const double *pos, const double *azel, + const prcopt_t *opt, const double *x, double *dtdx, + double *var) +{ + const double zazel[] = {0.0, PI/2.0}; + double zhd, m_h, m_w, cotz, grad_n, grad_e; + + /* zenith hydrostatic delay */ + zhd = tropmodel(time, pos, zazel, 0.0); + + /* mapping function */ + m_h = tropmapf(time, pos, azel, &m_w); + + if ((opt->tropopt == TROPOPT_ESTG || opt->tropopt == TROPOPT_CORG) && azel[1]>0.0) + { + /* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */ + cotz = 1.0/tan(azel[1]); + grad_n = m_w*cotz*cos(azel[0]); + grad_e = m_w*cotz*sin(azel[0]); + m_w += grad_n*x[1]+grad_e*x[2]; + dtdx[1] = grad_n*(x[0]-zhd); + dtdx[2] = grad_e*(x[0]-zhd); + } + dtdx[0] = m_w; + *var = std::pow(0.01, 2.0); + return m_h*zhd+m_w*(x[0]-zhd); +} + + +/* phase and code residuals --------------------------------------------------*/ +int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, rtk_t *rtk, double *v, + double *H, double *R, double *azel) +{ + prcopt_t *opt = &rtk->opt; + double r, rr[3], disp[3], pos[3], e[3], meas[2], dtdx[3], dantr[NFREQ] = {0}; + double dants[NFREQ] = {0}, var[MAXOBS*2], dtrp = 0.0, vart = 0.0, varm[2] = {0}; + int i, j, k, sat, sys, nv = 0, nx = rtk->nx, brk, tideopt; + + trace(3, "res_ppp : n=%d nx=%d\n", n, nx); + + for (i = 0;issat[i].vsat[0] = 0; + + for (i = 0;i<3;i++) rr[i] = x[i]; + + /* earth tides correction */ + if (opt->tidecorr) + { + tideopt = opt->tidecorr == 1?1:7; /* 1:solid, 2:solid+otl+pole */ + + tidedisp(gpst2utc(obs[0].time), rr, tideopt, &nav->erp, opt->odisp[0], + disp); + for (i = 0;i<3;i++) rr[i] += disp[i]; + } + ecef2pos(rr, pos); + + for (i = 0;issat[sat-1].vs) continue; + + /* geometric distance/azimuth/elevation angle */ + if ((r = geodist(rs+i*6, rr, e)) <= 0.0 || + satazel(pos, e, azel+i*2)elmin) continue; + + /* excluded satellite? */ + if (satexclude(obs[i].sat, svh[i], opt)) continue; + + /* tropospheric delay correction */ + if (opt->tropopt == TROPOPT_SAAS) + { + dtrp = tropmodel(obs[i].time, pos, azel+i*2, REL_HUMI); + vart = std::pow(ERR_SAAS, 2.0); + } + else if (opt->tropopt == TROPOPT_SBAS) + { + dtrp = sbstropcorr(obs[i].time, pos, azel+i*2, &vart); + } + else if (opt->tropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG) + { + dtrp = prectrop(obs[i].time, pos, azel+i*2, opt, x+IT_PPP(opt), dtdx, &vart); + } + else if (opt->tropopt == TROPOPT_COR || opt->tropopt == TROPOPT_CORG) + { + dtrp = prectrop(obs[i].time, pos, azel+i*2, opt, x, dtdx, &vart); + } + /* satellite antenna model */ + if (opt->posopt[0]) + { + satantpcv(rs+i*6, rr, nav->pcvs+sat-1, dants); + } + /* receiver antenna model */ + antmodel(opt->pcvr, opt->antdel[0], azel+i*2, opt->posopt[1], dantr); + + /* phase windup correction */ + if (opt->posopt[2]) + { + windupcorr(rtk->sol.time, rs+i*6, rr, &rtk->ssat[sat-1].phw); + } + /* ionosphere and antenna phase corrected measurements */ + if (!corrmeas(obs+i, nav, pos, azel+i*2, &rtk->opt, dantr, dants, + rtk->ssat[sat-1].phw, meas, varm, &brk)) + { + continue; + } + /* satellite clock and tropospheric delay */ + r += -SPEED_OF_LIGHT*dts[i*2]+dtrp; + + trace(5, "sat=%2d azel=%6.1f %5.1f dtrp=%.3f dantr=%6.3f %6.3f dants=%6.3f %6.3f phw=%6.3f\n", + sat, azel[i*2]*R2D, azel[1+i*2]*R2D, dtrp, dantr[0], dantr[1], dants[0], + dants[1], rtk->ssat[sat-1].phw); + + for (j = 0;j<2;j++) + { /* for phase and code */ + + if (meas[j] == 0.0) continue; + + for (k = 0;ktropopt >= TROPOPT_EST) + { + for (k = 0;k<(opt->tropopt >= TROPOPT_ESTG?3:1);k++) + { + H[IT_PPP(opt)+k+nx*nv] = dtdx[k]; + } + } + if (j == 0) + { + v[nv] -= x[IB_PPP(obs[i].sat, opt)]; + H[IB_PPP(obs[i].sat, opt)+nx*nv] = 1.0; + } + var[nv] = varerr(obs[i].sat, sys, azel[1+i*2], j, opt)+varm[j]+vare[i]+vart; + + if (j == 0) rtk->ssat[sat-1].resc[0] = v[nv]; + else rtk->ssat[sat-1].resp[0] = v[nv]; + + /* test innovation */ +#if 0 + if (opt->maxinno>0.0 && fabs(v[nv])>opt->maxinno) + { +#else + if (opt->maxinno>0.0 && fabs(v[nv])>opt->maxinno && sys != SYS_GLO) + { +#endif + trace(2, "ppp outlier rejected %s sat=%2d type=%d v=%.3f\n", + time_str(obs[i].time, 0), sat, j, v[nv]); + rtk->ssat[sat-1].rejc[0]++; + continue; + } + if (j == 0) rtk->ssat[sat-1].vsat[0] = 1; + nv++; + } + } + for (i = 0;iopt; + double *rs, *dts, *var, *v, *H, *R, *azel, *xp, *Pp; + int i, nv, info, svh[MAXOBS], stat = SOLQ_SINGLE; + + trace(3, "pppos : nx=%d n=%d\n", rtk->nx, n); + + rs = mat(6, n); dts = mat(2, n); var = mat(1, n); azel = zeros(2, n); + + for (i = 0; i < MAXSAT; i++) rtk->ssat[i].fix[0] = 0; + + /* temporal update of states */ + udstate_ppp(rtk, obs, n, nav); + + trace(4, "x(0)="); tracemat(4, rtk->x, 1, NR_PPP(opt), 13, 4); + + /* satellite positions and clocks */ + satposs(obs[0].time, obs, n, nav, rtk->opt.sateph, rs, dts, var, svh); + + /* exclude measurements of eclipsing satellite */ + if (rtk->opt.posopt[3]) + { + testeclipse(obs, n, nav, rs); + } + xp = mat(rtk->nx, 1); Pp = zeros(rtk->nx, rtk->nx); + matcpy(xp, rtk->x, rtk->nx, 1); + nv = n*rtk->opt.nf*2; v = mat(nv, 1); H = mat(rtk->nx, nv); R = mat(nv, nv); + + for (i = 0; i < rtk->opt.niter; i++) + { + /* phase and code residuals */ + if ((nv = res_ppp(i, obs, n, rs, dts, var, svh, nav, xp, rtk, v, H, R, azel)) <= 0) break; + + /* measurement update */ + matcpy(Pp, rtk->P, rtk->nx, rtk->nx); + + if ((info = filter(xp, Pp, H, v, R, rtk->nx, nv))) + { + trace(2, "ppp filter error %s info=%d\n", time_str(rtk->sol.time, 0), info); + break; + } + trace(4, "x(%d)=", i+1); tracemat(4, xp, 1, NR_PPP(opt), 13, 4); + + stat = SOLQ_PPP; + } + if (stat == SOLQ_PPP) + { + /* postfit residuals */ + res_ppp(1, obs, n, rs, dts, var, svh, nav, xp, rtk, v, H, R, azel); + + /* update state and covariance matrix */ + matcpy(rtk->x, xp, rtk->nx, 1); + matcpy(rtk->P, Pp, rtk->nx, rtk->nx); + + /* ambiguity resolution in ppp */ + if (opt->modear == ARMODE_PPPAR || opt->modear == ARMODE_PPPAR_ILS) + { + if (pppamb(rtk, obs, n, nav, azel)) stat = SOLQ_FIX; + } + /* update solution status */ + rtk->sol.ns = 0; + for (i = 0; i < n && i < MAXOBS; i++) + { + if (!rtk->ssat[obs[i].sat-1].vsat[0]) continue; + rtk->ssat[obs[i].sat-1].lock[0]++; + rtk->ssat[obs[i].sat-1].outc[0] = 0; + rtk->ssat[obs[i].sat-1].fix [0] = 4; + rtk->sol.ns++; + } + rtk->sol.stat = stat; + + for (i = 0; i < 3; i++) + { + rtk->sol.rr[i] = rtk->x[i]; + rtk->sol.qr[i] = (float)rtk->P[i+i*rtk->nx]; + } + rtk->sol.qr[3] = (float)rtk->P[1]; + rtk->sol.qr[4] = (float)rtk->P[2+rtk->nx]; + rtk->sol.qr[5] = (float)rtk->P[2]; + rtk->sol.dtr[0] = rtk->x[IC_PPP(0, opt)]; + rtk->sol.dtr[1] = rtk->x[IC_PPP(1, opt)]-rtk->x[IC_PPP(0, opt)]; + for (i = 0; i < n && i < MAXOBS; i++) + { + rtk->ssat[obs[i].sat-1].snr[0] = MIN_PPP(obs[i].SNR[0], obs[i].SNR[1]); + } + for (i = 0; i < MAXSAT; i++) + { + if (rtk->ssat[i].slip[0]&3) rtk->ssat[i].slipc[0]++; + } + } + free(rs); free(dts); free(var); free(azel); + free(xp); free(Pp); free(v); free(H); free(R); +} diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.h b/src/algorithms/libs/rtklib/rtklib_ppp.h new file mode 100644 index 000000000..314e0d164 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_ppp.h @@ -0,0 +1,186 @@ +/*! + * \file rtklib_ppp.h + * \brief Precise Point Positioning + * \authors
      + *
    • 2007-2008, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2008, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + + +#ifndef GNSS_SDR_RTKLIB_PPP_H_ +#define GNSS_SDR_RTKLIB_PPP_H_ + +#include "rtklib.h" + + +#define MIN_PPP(x,y) ((x)<=(y)?(x):(y)) +#define ROUND_PPP(x) (int)floor((x)+0.5) + +#define SWAP_I(x,y) do {int _z=x; x=y; y=_z;} while (0) +#define SWAP_D(x,y) do {double _z=x; x=y; y=_z;} while (0) + +const double MIN_ARC_GAP = 300.0; /* min arc gap (s) */ +const double CONST_AMB = 0.001; /* constraint to fixed ambiguity */ +const double THRES_RES = 0.3; /* threashold of residuals test (m) */ +const double LOG_PI = 1.14472988584940017; /* log(pi) */ +const double SQRT2 = 1.41421356237309510; /* sqrt(2) */ + +const double VAR_POS_PPP = std::pow(100.0, 2.0); /* init variance receiver position (m^2) */ +const double VAR_CLK = std::pow(100.0, 2.0); /* init variance receiver clock (m^2) */ +const double VAR_ZTD = std::pow( 0.3, 2.0); /* init variance ztd (m^2) */ +const double VAR_GRA_PPP = std::pow(0.001, 2.0); /* init variance gradient (m^2) */ +const double VAR_BIAS = std::pow(100.0, 2.0); /* init variance phase-bias (m^2) */ + +const double VAR_IONO_OFF = std::pow(10.0, 2.0); /* variance of iono-model-off */ + + +/* functions originally included in RTKLIB/src/ppp_ar.c v2.4.2*/ +double lam_LC(int i, int j, int k); + +double L_LC(int i, int j, int k, const double *L); + +double P_LC(int i, int j, int k, const double *P); + +double var_LC(int i, int j, int k, double sig); + +double q_gamma(double a, double x, double log_gamma_a); + +double p_gamma(double a, double x, double log_gamma_a); + +double f_erfc(double x); + +double conffunc(int N, double B, double sig); + +void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, const double *azel); + +int fix_amb_WL(rtk_t *rtk, const nav_t *nav, int sat1, int sat2, int *NW); + +int is_depend(int sat1, int sat2, int *flgs, int *max_flg); + +int sel_amb(int *sat1, int *sat2, double *N, double *var, int n); + +int fix_sol(rtk_t *rtk, const int *sat1, const int *sat2, const double *NC, int n); + +int fix_amb_ROUND(rtk_t *rtk, int *sat1, int *sat2, const int *NW, int n); + +int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n); + +int pppamb(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav, const double *azel); + + + +/* functions originally included in RTKLIB/src/ppp.c v2.4.2 */ +void pppoutsolstat(rtk_t *rtk, int level, FILE *fp); + +void tide_pl(const double *eu, const double *rp, double GMp, const double *pos, double *dr); + +void tide_solid(const double *rsun, const double *rmoon, + const double *pos, const double *E, double gmst, int opt, + double *dr); + +void tide_oload(gtime_t tut, const double *odisp, double *denu); + +void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar); + +void tide_pole(gtime_t tut, const double *pos, const double *erpv, double *denu); + +void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, + const double *odisp, double *dr); + +void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs); + +double varerr(int sat, int sys, double el, int type, const prcopt_t *opt); + +void initx(rtk_t *rtk, double xi, double var, int i); + +int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel, + const prcopt_t *opt, const double *dantr, const double *dants, + double phw, double *meas, double *var); + +double gettgd_ppp(int sat, const nav_t *nav); + +int corr_ion(gtime_t time, const nav_t *nav, int sat, const double *pos, + const double *azel, int ionoopt, double *ion, double *var, + int *brk); + +int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos, + const double *azel, const prcopt_t *opt, + const double *dantr, const double *dants, double phw, + double *meas, double *var, int *brk); + +double gfmeas(const obsd_t *obs, const nav_t *nav); + +void udpos_ppp(rtk_t *rtk); + +void udclk_ppp(rtk_t *rtk); + +void udtrop_ppp(rtk_t *rtk); + +void detslp_ll(rtk_t *rtk, const obsd_t *obs, int n); + +void detslp_gf(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); + +void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); + +void udstate_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); + +void satantpcv(const double *rs, const double *rr, const pcv_t *pcv, double *dant); + +double prectrop(gtime_t time, const double *pos, const double *azel, + const prcopt_t *opt, const double *x, double *dtdx, + double *var); + +int res_ppp(int iter, const obsd_t *obs, int n, const double *rs, + const double *dts, const double *vare, const int *svh, + const nav_t *nav, const double *x, rtk_t *rtk, double *v, + double *H, double *R, double *azel); + +int pppnx(const prcopt_t *opt); + +void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); + +#endif diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.cc b/src/algorithms/libs/rtklib/rtklib_preceph.cc new file mode 100644 index 000000000..1b42081e1 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_preceph.cc @@ -0,0 +1,873 @@ +/*! + * \file rtklib_preceph.cc + * \brief precise ephemeris and clock functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * References : + * [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c), + * 12 February, 2007 + * [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information, + * 27 August, 1998 + * [3] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996 + * [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed, + * Space Technology Library, 2004 + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_preceph.h" +#include "rtklib_rtkcmn.h" + +/* satellite code to satellite system ----------------------------------------*/ +int code2sys(char code) +{ + if (code == 'G' || code == ' ') return SYS_GPS; + if (code == 'R') return SYS_GLO; + if (code == 'E') return SYS_GAL; /* extension to sp3-c */ + if (code == 'J') return SYS_QZS; /* extension to sp3-c */ + if (code == 'C') return SYS_BDS; /* extension to sp3-c */ + if (code == 'L') return SYS_LEO; /* extension to sp3-c */ + return SYS_NONE; +} + + +/* read sp3 header -----------------------------------------------------------*/ +int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats, + double *bfact, char *tsys) +{ + int i, j, k = 0, ns = 0, sys, prn; + char buff[1024]; + + trace(3, "readsp3h:\n"); + + for (i = 0;i<22;i++) + { + if (!fgets(buff, sizeof(buff), fp)) break; + + if (i == 0) + { + *type = buff[2]; + if (str2time(buff, 3, 28, time)) return 0; + } + else if (2<=i && i<=6) + { + if (i == 2) + { + ns = (int)str2num(buff, 4, 2); + } + for (j = 0;j<17 && kne >= nav->nemax) + { + nav->nemax += 256; + if (!(nav_peph = (peph_t *)realloc(nav->peph, sizeof(peph_t)*nav->nemax))) + { + trace(1, "readsp3b malloc error n=%d\n", nav->nemax); + free(nav->peph); + nav->peph = NULL; + nav->ne = nav->nemax = 0; + return 0; + } + nav->peph = nav_peph; + } + nav->peph[nav->ne++] = *peph; + return 1; +} + + +/* read sp3 body -------------------------------------------------------------*/ +void readsp3b(FILE *fp, char type, int *sats __attribute__((unused)), int ns, double *bfact, + char *tsys, int index, int opt, nav_t *nav) +{ + peph_t peph; + gtime_t time; + double val, std, base; + int i, j, sat, sys, prn, n = ns*(type == 'P' ? 1 : 2), pred_o, pred_c, v; + char buff[1024]; + + trace(3, "readsp3b: type=%c ns=%d index=%d opt=%d\n", type, ns, index, opt); + + while (fgets(buff, sizeof(buff), fp)) + { + if (!strncmp(buff, "EOF", 3)) break; + + if (buff[0] != '*' || str2time(buff, 3, 28, &time)) + { + trace(2, "sp3 invalid epoch %31.31s\n", buff); + continue; + } + if (!strcmp(tsys, "UTC")) time = utc2gpst(time); /* utc->gpst */ + peph.time = time; + peph.index = index; + + for (i = 0; i < MAXSAT; i++) + { + for (j = 0; j < 4; j++) + { + peph.pos[i][j] = 0.0; + peph.std[i][j] = 0.0f; + peph.vel[i][j] = 0.0; + peph.vst[i][j] = 0.0f; + } + for (j = 0; j < 3; j++) + { + peph.cov[i][j] = 0.0f; + peph.vco[i][j] = 0.0f; + } + } + for (i = pred_o = pred_c = v = 0;i= 76 && buff[75] == 'P'; + pred_o = strlen(buff) >= 80 && buff[79] == 'P'; + } + for (j = 0;j<4;j++) + { + /* read option for predicted value */ + if (j< 3 && (opt&1) && pred_o) continue; + if (j< 3 && (opt&2) && !pred_o) continue; + if (j == 3 && (opt&1) && pred_c) continue; + if (j == 3 && (opt&2) && !pred_c) continue; + + val = str2num(buff, 4 + j * 14, 14); + std = str2num(buff, 61 + j * 3, j < 3 ? 2 : 3); + + if (buff[0] == 'P') { /* position */ + if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6) + { + peph.pos[sat-1][j] = val*(j < 3 ? 1000.0 : 1e-6); + v = 1; /* valid epoch */ + } + if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0) + { + peph.std[sat-1][j] = (float)(std::pow(base, std)*(j < 3 ? 1e-3 : 1e-12)); + } + } + else if (v) { /* velocity */ + if (val != 0.0 && fabs(val - 999999.999999) >= 1e-6) + { + peph.vel[sat-1][j] = val*(j < 3 ? 0.1 : 1e-10); + } + if ((base = bfact[j < 3 ? 0 : 1]) > 0.0 && std > 0.0) + { + peph.vst[sat-1][j] = (float)(std::pow(base, std)*(j < 3 ? 1e-7 : 1e-16)); + } + } + } + } + if (v) + { + if (!addpeph(nav, &peph)) return; + } + } +} + + +/* compare precise ephemeris -------------------------------------------------*/ +int cmppeph(const void *p1, const void *p2) +{ + peph_t *q1 = (peph_t *)p1, *q2 = (peph_t *)p2; + double tt = timediff(q1->time, q2->time); + return tt < -1e-9 ? -1 : (tt > 1e-9 ? 1 : q1->index - q2->index); +} + + +/* combine precise ephemeris -------------------------------------------------*/ +void combpeph(nav_t *nav, int opt) +{ + int i, j, k, m; + + trace(3, "combpeph: ne=%d\n", nav->ne); + + qsort(nav->peph, nav->ne, sizeof(peph_t), cmppeph); + + if (opt&4) return; + + for (i = 0, j = 1; j < nav->ne; j++) + { + if (fabs(timediff(nav->peph[i].time, nav->peph[j].time)) < 1e-9) + { + for (k = 0; k < MAXSAT; k++) + { + if (norm_rtk(nav->peph[j].pos[k], 4) <= 0.0) continue; + for (m = 0;m < 4; m++) nav->peph[i].pos[k][m] = nav->peph[j].pos[k][m]; + for (m = 0;m < 4; m++) nav->peph[i].std[k][m] = nav->peph[j].std[k][m]; + for (m = 0;m < 4; m++) nav->peph[i].vel[k][m] = nav->peph[j].vel[k][m]; + for (m = 0;m < 4; m++) nav->peph[i].vst[k][m] = nav->peph[j].vst[k][m]; + } + } + else if (++i < j) nav->peph[i] = nav->peph[j]; + } + nav->ne = i+1; + + trace(4, "combpeph: ne=%d\n", nav->ne); +} + + +/* read sp3 precise ephemeris file --------------------------------------------- + * read sp3 precise ephemeris/clock files and set them to navigation data + * args : char *file I sp3-c precise ephemeris file + * (wind-card * is expanded) + * nav_t *nav IO navigation data + * int opt I options (1: only observed + 2: only predicted + + * 4: not combined) + * return : none + * notes : see ref [1] + * precise ephemeris is appended and combined + * nav->peph and nav->ne must by properly initialized before calling the + * function + * only files with extensions of .sp3, .SP3, .eph* and .EPH* are read + *-----------------------------------------------------------------------------*/ +void readsp3(const char *file, nav_t *nav, int opt) +{ + FILE *fp; + gtime_t time = {0, 0}; + double bfact[2] = {}; + int i, j, n, ns, sats[MAXSAT] = {}; + char *efiles[MAXEXFILE], *ext, type = ' ', tsys[4] = ""; + + trace(3, "readpephs: file=%s\n", file); + + for (i = 0; i < MAXEXFILE; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return; + } + } + /* expand wild card in file path */ + n = expath(file, efiles, MAXEXFILE); + + for (i = j = 0; i < n; i++) + { + if (!(ext = strrchr(efiles[i], '.'))) continue; + + if (!strstr(ext + 1, "sp3") && !strstr(ext + 1, ".SP3") && + !strstr(ext + 1, "eph") && !strstr(ext + 1, ".EPH")) continue; + + if (!(fp = fopen(efiles[i], "r"))) + { + trace(2, "sp3 file open error %s\n", efiles[i]); + continue; + } + /* read sp3 header */ + ns = readsp3h(fp, &time, &type, sats, bfact, tsys); + + /* read sp3 body */ + readsp3b(fp, type, sats, ns, bfact, tsys, j++, opt, nav); + + fclose(fp); + } + for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); + + /* combine precise ephemeris */ + if (nav->ne > 0) combpeph(nav, opt); +} + + +/* read satellite antenna parameters ------------------------------------------- + * read satellite antenna parameters + * args : char *file I antenna parameter file + * gtime_t time I time + * nav_t *nav IO navigation data + * return : status (1:ok,0:error) + * notes : only support antex format for the antenna parameter file + *-----------------------------------------------------------------------------*/ +int readsap(const char *file, gtime_t time, nav_t *nav) +{ + pcv_t aux = { 0, {}, {}, {0,0}, {0,0}, {{},{}}, {{},{}} }; + pcvs_t pcvs = {0, 0, &aux }; + pcv_t pcv0 = { 0, {}, {}, {0,0}, {0,0}, {{},{}}, {{},{}} }, *pcv; + int i; + + trace(3, "readsap : file=%s time=%s\n", file, time_str(time, 0)); + + if (!readpcv(file, &pcvs)) return 0; + + for (i = 0; i < MAXSAT; i++) + { + pcv = searchpcv(i + 1, "", time, &pcvs); + nav->pcvs[i] = pcv ? *pcv : pcv0; + } + free(pcvs.pcv); + return 1; +} + + +/* read dcb parameters file --------------------------------------------------*/ +int readdcbf(const char *file, nav_t *nav, const sta_t *sta) +{ + FILE *fp; + double cbias; + char buff[256], str1[32], str2[32] = ""; + int i, j, sat, type = 0; + + trace(3, "readdcbf: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) + { + trace(2, "dcb parameters file open error: %s\n", file); + return 0; + } + while (fgets(buff, sizeof(buff), fp)) + { + if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES")) type = 1; + else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES")) type = 2; + else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES")) type = 3; + + if (!type || sscanf(buff, "%s %s", str1, str2) < 1) continue; + + if ((cbias = str2num(buff, 26, 9)) == 0.0) continue; + + if (sta && (!strcmp(str1, "G") || !strcmp(str1, "R"))) + { /* receiver dcb */ + for (i = 0; i < MAXRCV; i++) + { + if (!strcmp(sta[i].name, str2)) break; + } + if (i < MAXRCV) + { + j = !strcmp(str1, "G") ? 0 : 1; + nav->rbias[i][j][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */ + } + } + else if ((sat = satid2no(str1))) + { /* satellite dcb */ + nav->cbias[sat-1][type-1] = cbias * 1e-9 * SPEED_OF_LIGHT; /* ns -> m */ + } + } + fclose(fp); + + return 1; +} + + +/* read dcb parameters --------------------------------------------------------- + * read differential code bias (dcb) parameters + * args : char *file I dcb parameters file (wild-card * expanded) + * nav_t *nav IO navigation data + * sta_t *sta I station info data to inport receiver dcb + * (NULL: no use) + * return : status (1:ok,0:error) + * notes : currently only p1-c1 bias of code *.dcb file + *-----------------------------------------------------------------------------*/ +int readdcb(const char *file, nav_t *nav, const sta_t *sta) +{ + int i, j, n; + char *efiles[MAXEXFILE] = {}; + + trace(3, "readdcb : file=%s\n", file); + + for (i = 0;i < MAXSAT; i++) for (j = 0; j < 3; j++) + { + nav->cbias[i][j] = 0.0; + } + for (i = 0; i < MAXEXFILE; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return 0; + } + } + n = expath(file, efiles, MAXEXFILE); + + for (i = 0; i < n; i++) + { + readdcbf(efiles[i], nav, sta); + } + for (i = 0; i < MAXEXFILE;i ++) free(efiles[i]); + + return 1; +} + + +/* add satellite fcb ---------------------------------------------------------*/ +int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat, + const double *bias, const double *std) +{ + fcbd_t *nav_fcb; + int i, j; + + if (nav->nf > 0 && fabs(timediff(ts, nav->fcb[nav->nf-1].ts)) <= 1e-3) + { + for (i = 0; i < 3; i++) + { + nav->fcb[nav->nf-1].bias[sat-1][i] = bias[i]; + nav->fcb[nav->nf-1].std [sat-1][i] = std [i]; + } + return 1; + } + if (nav->nf >= nav->nfmax) + { + nav->nfmax = nav->nfmax <= 0 ? 2048 : nav->nfmax * 2; + if (!(nav_fcb = (fcbd_t *)realloc(nav->fcb, sizeof(fcbd_t)*nav->nfmax))) + { + free(nav->fcb); + nav->nf = nav->nfmax = 0; + return 0; + } + nav->fcb = nav_fcb; + } + for (i = 0; i < MAXSAT; i++) for (j = 0; j < 3; j++) + { + nav->fcb[nav->nf].bias[i][j] = nav->fcb[nav->nf].std[i][j] = 0.0; + } + for (i = 0; i < 3; i++) + { + nav->fcb[nav->nf].bias[sat-1][i] = bias[i]; + nav->fcb[nav->nf].std [sat-1][i] = std [i]; + } + nav->fcb[nav->nf ].ts = ts; + nav->fcb[nav->nf++].te = te; + return 1; +} + + +/* read satellite fcb file ---------------------------------------------------*/ +int readfcbf(const char *file, nav_t *nav) +{ + FILE *fp; + gtime_t ts, te; + double ep1[6], ep2[6], bias[3] = {}, std[3] = {}; + char buff[1024], str[32], *p; + int sat; + + trace(3, "readfcbf: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) + { + trace(2, "fcb parameters file open error: %s\n", file); + return 0; + } + while (fgets(buff, sizeof(buff), fp)) + { + if ((p = strchr(buff, '#'))) *p = '\0'; + if (sscanf(buff, "%lf/%lf/%lf %lf:%lf:%lf %lf/%lf/%lf %lf:%lf:%lf %s" + "%lf %lf %lf %lf %lf %lf", ep1, ep1+1, ep1+2, ep1+3, ep1+4, ep1+5, + ep2, ep2+1, ep2+2, ep2+3, ep2+4, ep2+5, str, bias, std, bias+1, std+1, + bias+2, std+2) < 17) continue; + if (!(sat = satid2no(str))) continue; + ts = epoch2time(ep1); + te = epoch2time(ep2); + if (!addfcb(nav, ts, te, sat, bias, std)) return 0; + } + fclose(fp); + return 1; +} + + +/* compare satellite fcb -----------------------------------------------------*/ +int cmpfcb(const void *p1, const void *p2) +{ + fcbd_t *q1 = (fcbd_t *)p1, *q2 = (fcbd_t *)p2; + double tt = timediff(q1->ts, q2->ts); + return tt < -1e-3 ? -1 : (tt > 1e-3 ? 1 : 0); +} + + +/* read satellite fcb data ----------------------------------------------------- + * read satellite fractional cycle bias (dcb) parameters + * args : char *file I fcb parameters file (wild-card * expanded) + * nav_t *nav IO navigation data + * return : status (1:ok,0:error) + * notes : fcb data appended to navigation data + *-----------------------------------------------------------------------------*/ +int readfcb(const char *file, nav_t *nav) +{ + char *efiles[MAXEXFILE] = {}; + int i, n; + + trace(3, "readfcb : file=%s\n", file); + + for (i = 0; i < MAXEXFILE; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return 0; + } + } + n = expath(file, efiles, MAXEXFILE); + + for (i = 0; i < n; i++) + { + readfcbf(efiles[i], nav); + } + for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); + + if (nav->nf > 1) + { + qsort(nav->fcb, nav->nf, sizeof(fcbd_t), cmpfcb); + } + return 1; +} + + +/* polynomial interpolation by Neville's algorithm ---------------------------*/ +double interppol(const double *x, double *y, int n) +{ + int i, j; + + for (j = 1; j < n; j++) + { + for (i = 0; i < n - j; i++) + { + y[i] = (x[i+j] * y[i] - x[i] * y[i+1]) / (x[i+j] - x[i]); + } + } + return y[0]; +} + + +/* satellite position by precise ephemeris -----------------------------------*/ +int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs, + double *dts, double *vare, double *varc) +{ + double t[NMAX+1], p[3][NMAX+1], c[2], *pos, std = 0.0, s[3], sinl, cosl; + int i, j, k, index; + + trace(4, "pephpos : time=%s sat=%2d\n", time_str(time, 3), sat); + + rs[0] = rs[1] = rs[2] = dts[0] = 0.0; + + if (nav->ne < NMAX + 1 || + timediff(time, nav->peph[0].time) < -MAXDTE || + timediff(time, nav->peph[nav->ne-1].time) > MAXDTE) + { + trace(3, "no prec ephem %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } + /* binary search */ + for (i = 0, j = nav->ne - 1; i < j;) + { + k = (i + j) / 2; + if (timediff(nav->peph[k].time, time) < 0.0) i = k + 1; else j = k; + } + index = i <= 0 ? 0 : i-1; + + /* polynomial interpolation for orbit */ + i = index - (NMAX + 1) / 2; + if (i < 0) i = 0; else if (i + NMAX >= nav->ne) i = nav->ne - NMAX - 1; + + for (j = 0; j <= NMAX; j++) + { + t[j] = timediff(nav->peph[i+j].time, time); + if (norm_rtk(nav->peph[i+j].pos[sat-1], 3) <= 0.0) + { + trace(3, "prec ephem outage %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } + } + for (j = 0;j<=NMAX;j++) + { + pos = nav->peph[i+j].pos[sat-1]; +#if 0 + p[0][j] = pos[0]; + p[1][j] = pos[1]; +#else + /* correciton for earh rotation ver.2.4.0 */ + sinl = sin(DEFAULT_OMEGA_EARTH_DOT * t[j]); + cosl = cos(DEFAULT_OMEGA_EARTH_DOT * t[j]); + p[0][j] = cosl * pos[0] - sinl * pos[1]; + p[1][j] = sinl * pos[0] + cosl * pos[1]; +#endif + p[2][j] = pos[2]; + } + for (i = 0; i < 3; i++) + { + rs[i] = interppol(t, p[i], NMAX + 1); + } + if (vare) + { + for (i = 0; i < 3; i++) s[i] = nav->peph[index].std[sat-1][i]; + std = norm_rtk(s, 3); + + /* extrapolation error for orbit */ + if (t[0 ] > 0.0) std += EXTERR_EPH * std::pow(t[0 ], 2.0) / 2.0; + else if (t[NMAX] < 0.0) std += EXTERR_EPH * std::pow(t[NMAX], 2.0) / 2.0; + *vare = std::pow(std, 2.0); + } + /* linear interpolation for clock */ + t[0] = timediff(time, nav->peph[index ].time); + t[1] = timediff(time, nav->peph[index+1].time); + c[0] = nav->peph[index ].pos[sat-1][3]; + c[1] = nav->peph[index+1].pos[sat-1][3]; + + if (t[0] <= 0.0) + { + if ((dts[0] = c[0]) != 0.0) + { + std = nav->peph[index].std[sat-1][3] * SPEED_OF_LIGHT - EXTERR_CLK * t[0]; + } + } + else if (t[1] >= 0.0) + { + if ((dts[0] = c[1]) != 0.0) + { + std = nav->peph[index+1].std[sat-1][3] * SPEED_OF_LIGHT + EXTERR_CLK * t[1]; + } + } + else if (c[0] != 0.0 && c[1] != 0.0) + { + dts[0] = (c[1] * t[0] - c[0] * t[1]) / (t[0] - t[1]); + i = t[0] < -t[1] ? 0: 1; + std = nav->peph[index+i].std[sat-1][3] + EXTERR_CLK * fabs(t[i]); + } + else + { + dts[0] = 0.0; + } + if (varc) *varc = std::pow(std, 2.0); + return 1; +} + + +/* satellite clock by precise clock ------------------------------------------*/ +int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, + double *varc) +{ + double t[2], c[2], std; + int i, j, k, index; + + trace(4, "pephclk : time=%s sat=%2d\n", time_str(time, 3), sat); + + if (nav->nc < 2 || + timediff(time, nav->pclk[0].time) < -MAXDTE || + timediff(time, nav->pclk[nav->nc-1].time) > MAXDTE) + { + trace(3, "no prec clock %s sat=%2d\n", time_str(time, 0), sat); + return 1; + } + /* binary search */ + for (i = 0, j = nav->nc - 1; i < j;) + { + k = (i + j) / 2; + if (timediff(nav->pclk[k].time, time) < 0.0) i = k + 1; else j = k; + } + index = i<=0?0:i-1; + + /* linear interpolation for clock */ + t[0] = timediff(time, nav->pclk[index ].time); + t[1] = timediff(time, nav->pclk[index+1].time); + c[0] = nav->pclk[index ].clk[sat-1][0]; + c[1] = nav->pclk[index+1].clk[sat-1][0]; + + if (t[0] <= 0.0) + { + if ((dts[0] = c[0]) == 0.0) return 0; + std = nav->pclk[index].std[sat-1][0] * SPEED_OF_LIGHT - EXTERR_CLK * t[0]; + } + else if (t[1] >= 0.0) + { + if ((dts[0] = c[1]) == 0.0) return 0; + std = nav->pclk[index+1].std[sat-1][0] * SPEED_OF_LIGHT + EXTERR_CLK * t[1]; + } + else if (c[0] != 0.0 && c[1] != 0.0) + { + dts[0] = (c[1]*t[0]-c[0]*t[1]) / (t[0] - t[1]); + i = t[0] < -t[1] ? 0 : 1; + std = nav->pclk[index+i].std[sat-1][0] * SPEED_OF_LIGHT + EXTERR_CLK * fabs(t[i]); + } + else + { + trace(3, "prec clock outage %s sat=%2d\n", time_str(time, 0), sat); + return 0; + } + if (varc) *varc = std::pow(std, 2.0); + return 1; +} + + +/* satellite antenna phase center offset --------------------------------------- + * compute satellite antenna phase center offset in ecef + * args : gtime_t time I time (gpst) + * double *rs I satellite position and velocity (ecef) + * {x,y,z,vx,vy,vz} (m|m/s) + * int sat I satellite number + * nav_t *nav I navigation data + * double *dant I satellite antenna phase center offset (ecef) + * {dx,dy,dz} (m) (iono-free LC value) + * return : none + *-----------------------------------------------------------------------------*/ +void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav, + double *dant) +{ + const double *lam = nav->lam[sat-1]; + const pcv_t *pcv = nav->pcvs+sat-1; + double ex[3], ey[3], ez[3], es[3], r[3], rsun[3], gmst, erpv[5] = {}; + double gamma, C1, C2, dant1, dant2; + int i, j = 0, k = 1; + + trace(4, "satantoff: time=%s sat=%2d\n", time_str(time, 3), sat); + + /* sun position in ecef */ + sunmoonpos(gpst2utc(time), erpv, rsun, NULL, &gmst); + + /* unit vectors of satellite fixed coordinates */ + for (i = 0; i < 3; i++) r[i] = -rs[i]; + if (!normv3(r, ez)) return; + for (i = 0; i < 3; i++) r[i] = rsun[i]-rs[i]; + if (!normv3(r, es)) return; + cross3(ez, es, r); + if (!normv3(r, ey)) return; + cross3(ey, ez, ex); + + if (NFREQ >= 3 && (satsys(sat, NULL) & (SYS_GAL | SYS_SBS))) k = 2; + + if (NFREQ < 2 || lam[j] == 0.0 || lam[k] == 0.0) return; + + gamma = std::pow(lam[k], 2.0) / std::pow(lam[j], 2.0); + C1 = gamma / (gamma - 1.0); + C2 = -1.0 / (gamma - 1.0); + + /* iono-free LC */ + for (i = 0; i < 3; i++) + { + dant1 = pcv->off[j][0] * ex[i] + pcv->off[j][1] * ey[i] + pcv->off[j][2] * ez[i]; + dant2 = pcv->off[k][0] * ex[i] + pcv->off[k][1] * ey[i] + pcv->off[k][2] * ez[i]; + dant[i] = C1 * dant1 + C2 * dant2; + } +} + + +/* satellite position/clock by precise ephemeris/clock ------------------------- + * compute satellite position/clock with precise ephemeris/clock + * args : gtime_t time I time (gpst) + * int sat I satellite number + * nav_t *nav I navigation data + * int opt I sat postion option + * (0: center of mass, 1: antenna phase center) + * double *rs O sat position and velocity (ecef) + * {x,y,z,vx,vy,vz} (m|m/s) + * double *dts O sat clock {bias,drift} (s|s/s) + * double *var IO sat position and clock error variance (m) + * (NULL: no output) + * return : status (1:ok,0:error or data outage) + * notes : clock includes relativistic correction but does not contain code bias + * before calling the function, nav->peph, nav->ne, nav->pclk and + * nav->nc must be set by calling readsp3(), readrnx() or readrnxt() + * if precise clocks are not set, clocks in sp3 are used instead + *-----------------------------------------------------------------------------*/ +int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, + double *rs, double *dts, double *var) +{ + double rss[3], rst[3], dtss[1], dtst[1], dant[3] = {}, vare = 0.0, varc = 0.0, tt = 1e-3; + int i; + + trace(4, "peph2pos: time=%s sat=%2d opt=%d\n", time_str(time, 3), sat, opt); + + if (sat <= 0 || MAXSAT < sat) return 0; + + /* satellite position and clock bias */ + if (!pephpos(time, sat, nav, rss, dtss, &vare, &varc) || + !pephclk(time, sat, nav, dtss, &varc)) return 0; + + time = timeadd(time, tt); + if (!pephpos(time, sat, nav, rst, dtst, NULL, NULL) || + !pephclk(time, sat, nav, dtst, NULL)) return 0; + + /* satellite antenna offset correction */ + if (opt) + { + satantoff(time, rss, sat, nav, dant); + } + for (i = 0;i<3;i++) + { + rs[i ] = rss[i] + dant[i]; + rs[i+3] = (rst[i] - rss[i]) / tt; + } + /* relativistic effect correction */ + if (dtss[0] != 0.0) + { + dts[0] = dtss[0] - 2.0 * dot(rs, rs+3, 3) / SPEED_OF_LIGHT / SPEED_OF_LIGHT; + dts[1] = (dtst[0] - dtss[0]) / tt; + } + else + { /* no precise clock */ + dts[0] = dts[1] = 0.0; + } + if (var) *var = vare + varc; + + return 1; +} diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.h b/src/algorithms/libs/rtklib/rtklib_preceph.h new file mode 100644 index 000000000..056963916 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_preceph.h @@ -0,0 +1,107 @@ +/*! + * \file rtklib_preceph.h + * \brief precise ephemeris and clock functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * References : + * [1] S.Hilla, The Extended Standard Product 3 Orbit Format (SP3-c), + * 12 February, 2007 + * [2] J.Ray, W.Gurtner, RINEX Extensions to Handle Clock Information, + * 27 August, 1998 + * [3] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996 + * [4] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed, + * Space Technology Library, 2004 + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_PRECEPH_H_ +#define GNSS_SDR_RTKLIB_PRECEPH_H_ + +#include "rtklib.h" + + +const int NMAX = 10; /* order of polynomial interpolation */ +const double MAXDTE = 900.0; /* max time difference to ephem time (s) */ +const double EXTERR_CLK = 1e-3; /* extrapolation error for clock (m/s) */ +const double EXTERR_EPH = 5e-7; /* extrapolation error for ephem (m/s^2) */ + +int code2sys(char code); +int readsp3h(FILE *fp, gtime_t *time, char *type, int *sats, + double *bfact, char *tsys); +int addpeph(nav_t *nav, peph_t *peph); +void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact, + char *tsys, int index, int opt, nav_t *nav); +int cmppeph(const void *p1, const void *p2); +void combpeph(nav_t *nav, int opt); +void readsp3(const char *file, nav_t *nav, int opt); +int readsap(const char *file, gtime_t time, nav_t *nav); +int readdcbf(const char *file, nav_t *nav, const sta_t *sta); +int readdcb(const char *file, nav_t *nav, const sta_t *sta); +int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat, + const double *bias, const double *std); +int readfcbf(const char *file, nav_t *nav); +int readdcb(const char *file, nav_t *nav, const sta_t *sta); +int addfcb(nav_t *nav, gtime_t ts, gtime_t te, int sat, + const double *bias, const double *std); +int readfcbf(const char *file, nav_t *nav); +int cmpfcb(const void *p1, const void *p2); +int readfcb(const char *file, nav_t *nav); +double interppol(const double *x, double *y, int n); +int pephpos(gtime_t time, int sat, const nav_t *nav, double *rs, + double *dts, double *vare, double *varc); + +int pephclk(gtime_t time, int sat, const nav_t *nav, double *dts, + double *varc); + +void satantoff(gtime_t time, const double *rs, int sat, const nav_t *nav, + double *dant); +int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, + double *rs, double *dts, double *var); + +#endif // GNSS_SDR_RTKLIB_PRECEPH_H_ diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc new file mode 100644 index 000000000..f3530f276 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -0,0 +1,4065 @@ +/*! + * \file rtklib_rtkcmn.cc + * \brief rtklib common functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_rtkcmn.h" +//#include +//#include +#include +#include +//#include +#include +#include +#include + + +const double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */ +const double gst0 [] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */ +const double bdt0 [] = {2006, 1, 1, 0, 0, 0}; /* beidou time reference */ + +double leaps[MAXLEAPS+1][7] = { /* leap seconds (y,m,d,h,m,s,utc-gpst) */ + {2017, 1, 1, 0, 0, 0, -18}, + {2015, 7, 1, 0, 0, 0, -17}, + {2012, 7, 1, 0, 0, 0, -16}, + {2009, 1, 1, 0, 0, 0, -15}, + {2006, 1, 1, 0, 0, 0, -14}, + {1999, 1, 1, 0, 0, 0, -13}, + {1997, 7, 1, 0, 0, 0, -12}, + {1996, 1, 1, 0, 0, 0, -11}, + {1994, 7, 1, 0, 0, 0, -10}, + {1993, 7, 1, 0, 0, 0, -9}, + {1992, 7, 1, 0, 0, 0, -8}, + {1991, 1, 1, 0, 0, 0, -7}, + {1990, 1, 1, 0, 0, 0, -6}, + {1988, 1, 1, 0, 0, 0, -5}, + {1985, 7, 1, 0, 0, 0, -4}, + {1983, 7, 1, 0, 0, 0, -3}, + {1982, 7, 1, 0, 0, 0, -2}, + {1981, 7, 1, 0, 0, 0, -1}, + {} +}; + + +const prcopt_t prcopt_default = { /* defaults processing options */ + PMODE_SINGLE, 0, 2, SYS_GPS, /* mode, soltype, nf, navsys */ + 15.0*D2R, { {}, {{},{}} }, /* elmin, snrmask */ + 0, 1, 1, 1, /* sateph, modear, glomodear, bdsmodear */ + 5, 0, 10, 1, /* maxout, minlock, minfix, armaxiter */ + 0, 0, 0, 0, /* estion, esttrop, dynamics, tidecorr */ + 1, 0, 0, 0, 0, /* niter, codesmooth, intpref, sbascorr, sbassatsel */ + 0, 0, /* rovpos, refpos */ + {100.0, 100.0, 100.0}, /* eratio[] */ + {100.0, 0.003, 0.003, 0.0, 1.0}, /* err[] */ + {30.0, 0.03, 0.3}, /* std[] */ + {1e-4, 1e-3, 1e-4, 1e-1, 1e-2, 0.0}, /* prn[] */ + 5E-12, /* sclkstab */ + {3.0, 0.9999, 0.25, 0.1, 0.05, 0, 0, 0}, /* thresar */ + 0.0, 0.0, 0.05, /* elmaskar, almaskhold, thresslip */ + 30.0, 30.0, 30.0, /* maxtdif, maxinno, maxgdop */ + {}, {}, {}, /* baseline, ru, rb */ + {"",""}, /* anttype */ + {} , {}, {}, /* antdel, pcv, exsats */ + 0, 0, 0, {"",""}, {}, 0, {{},{}}, { {}, {{},{}}, {{},{}}, {}, {} }, 0, {} +}; + + +const solopt_t solopt_default = { /* defaults solution output options */ + SOLF_LLH, TIMES_GPST, 1, 3, /* posf, times, timef, timeu */ + 0, 1, 0, 0, 0, 0, /* degf, outhead, outopt, datum, height, geoid */ + 0, 0, 0, /* solstatic, sstat, trace */ + {0.0, 0.0}, /* nmeaintv */ + " ", "", 0 /* separator/program name */ +}; + + +const char *formatstrs[32] = { /* stream format strings */ + "RTCM 2", /* 0 */ + "RTCM 3", /* 1 */ + "NovAtel OEM6", /* 2 */ + "NovAtel OEM3", /* 3 */ + "u-blox", /* 4 */ + "Superstar II", /* 5 */ + "Hemisphere", /* 6 */ + "SkyTraq", /* 7 */ + "GW10", /* 8 */ + "Javad", /* 9 */ + "NVS BINR", /* 10 */ + "BINEX", /* 11 */ + "Trimble RT17", /* 12 */ + "Septentrio", /* 13 */ + "CMR/CMR+", /* 14 */ + "LEX Receiver", /* 15 */ + "RINEX", /* 16 */ + "SP3", /* 17 */ + "RINEX CLK", /* 18 */ + "SBAS", /* 19 */ + "NMEA 0183", /* 20 */ + NULL +}; + + +char obscodes[][3] = { /* observation code strings */ + "" ,"1C","1P","1W","1Y", "1M","1N","1S","1L","1E", /* 0- 9 */ + "1A","1B","1X","1Z","2C", "2D","2S","2L","2X","2P", /* 10-19 */ + "2W","2Y","2M","2N","5I", "5Q","5X","7I","7Q","7X", /* 20-29 */ + "6A","6B","6C","6X","6Z", "6S","6L","8L","8Q","8X", /* 30-39 */ + "2I","2Q","6I","6Q","3I", "3Q","3X","1I","1Q","5A", /* 40-49 */ + "5B","5C","9A","9B","9C", "9X","" ,"" ,"" ,"" /* 50-59 */ +}; + + +unsigned char obsfreqs[] = { + /* 1:L1/E1, 2:L2/B1, 3:L5/E5a/L3, 4:L6/LEX/B3, 5:E5b/B2, 6:E5(a+b), 7:S */ + 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, /* 0- 9 */ + 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, /* 10-19 */ + 2, 2, 2, 2, 3, 3, 3, 5, 5, 5, /* 20-29 */ + 4, 4, 4, 4, 4, 4, 4, 6, 6, 6, /* 30-39 */ + 2, 2, 4, 4, 3, 3, 3, 1, 1, 3, /* 40-49 */ + 3, 3, 7, 7, 7, 7, 0, 0, 0, 0 /* 50-59 */ +}; + + +char codepris[7][MAXFREQ][16] = { /* code priority table */ + + /* L1/E1 L2/B1 L5/E5a/L3 L6/LEX/B3 E5b/B2 E5(a+b) S */ + {"CPYWMNSL", "PYWCMNDSLX", "IQX" , "" , "" , "" , "" }, /* GPS */ + {"PC" , "PC" , "IQX" , "" , "" , "" , "" }, /* GLO */ + {"CABXZ" , "" , "IQX" , "ABCXZ" , "IQX" , "IQX" , "" }, /* GAL */ + {"CSLXZ" , "SLX" , "IQX" , "SLX" , "" , "" , "" }, /* QZS */ + {"C" , "" , "IQX" , "" , "" , "" , "" }, /* SBS */ + {"IQX" , "IQX" , "IQX" , "IQX" , "IQX" , "" , "" }, /* BDS */ + {"" , "" , "ABCX" , "" , "" , "" , "ABCX"} /* IRN */ +}; + + +fatalfunc_t *fatalfunc = NULL; /* fatal callback function */ + +/* crc tables generated by util/gencrc ---------------------------------------*/ +const unsigned short tbl_CRC16[] = { + 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, + 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF, + 0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, + 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE, + 0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, + 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D, + 0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, + 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC, + 0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, + 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B, + 0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, + 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A, + 0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, + 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49, + 0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, + 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78, + 0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, + 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067, + 0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, + 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256, + 0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, + 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, + 0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, + 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634, + 0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, + 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3, + 0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, + 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92, + 0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, + 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1, + 0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, + 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0 +}; + + +const unsigned int tbl_CRC24Q[] = { + 0x000000, 0x864CFB, 0x8AD50D, 0x0C99F6, 0x93E6E1, 0x15AA1A, 0x1933EC, 0x9F7F17, + 0xA18139, 0x27CDC2, 0x2B5434, 0xAD18CF, 0x3267D8, 0xB42B23, 0xB8B2D5, 0x3EFE2E, + 0xC54E89, 0x430272, 0x4F9B84, 0xC9D77F, 0x56A868, 0xD0E493, 0xDC7D65, 0x5A319E, + 0x64CFB0, 0xE2834B, 0xEE1ABD, 0x685646, 0xF72951, 0x7165AA, 0x7DFC5C, 0xFBB0A7, + 0x0CD1E9, 0x8A9D12, 0x8604E4, 0x00481F, 0x9F3708, 0x197BF3, 0x15E205, 0x93AEFE, + 0xAD50D0, 0x2B1C2B, 0x2785DD, 0xA1C926, 0x3EB631, 0xB8FACA, 0xB4633C, 0x322FC7, + 0xC99F60, 0x4FD39B, 0x434A6D, 0xC50696, 0x5A7981, 0xDC357A, 0xD0AC8C, 0x56E077, + 0x681E59, 0xEE52A2, 0xE2CB54, 0x6487AF, 0xFBF8B8, 0x7DB443, 0x712DB5, 0xF7614E, + 0x19A3D2, 0x9FEF29, 0x9376DF, 0x153A24, 0x8A4533, 0x0C09C8, 0x00903E, 0x86DCC5, + 0xB822EB, 0x3E6E10, 0x32F7E6, 0xB4BB1D, 0x2BC40A, 0xAD88F1, 0xA11107, 0x275DFC, + 0xDCED5B, 0x5AA1A0, 0x563856, 0xD074AD, 0x4F0BBA, 0xC94741, 0xC5DEB7, 0x43924C, + 0x7D6C62, 0xFB2099, 0xF7B96F, 0x71F594, 0xEE8A83, 0x68C678, 0x645F8E, 0xE21375, + 0x15723B, 0x933EC0, 0x9FA736, 0x19EBCD, 0x8694DA, 0x00D821, 0x0C41D7, 0x8A0D2C, + 0xB4F302, 0x32BFF9, 0x3E260F, 0xB86AF4, 0x2715E3, 0xA15918, 0xADC0EE, 0x2B8C15, + 0xD03CB2, 0x567049, 0x5AE9BF, 0xDCA544, 0x43DA53, 0xC596A8, 0xC90F5E, 0x4F43A5, + 0x71BD8B, 0xF7F170, 0xFB6886, 0x7D247D, 0xE25B6A, 0x641791, 0x688E67, 0xEEC29C, + 0x3347A4, 0xB50B5F, 0xB992A9, 0x3FDE52, 0xA0A145, 0x26EDBE, 0x2A7448, 0xAC38B3, + 0x92C69D, 0x148A66, 0x181390, 0x9E5F6B, 0x01207C, 0x876C87, 0x8BF571, 0x0DB98A, + 0xF6092D, 0x7045D6, 0x7CDC20, 0xFA90DB, 0x65EFCC, 0xE3A337, 0xEF3AC1, 0x69763A, + 0x578814, 0xD1C4EF, 0xDD5D19, 0x5B11E2, 0xC46EF5, 0x42220E, 0x4EBBF8, 0xC8F703, + 0x3F964D, 0xB9DAB6, 0xB54340, 0x330FBB, 0xAC70AC, 0x2A3C57, 0x26A5A1, 0xA0E95A, + 0x9E1774, 0x185B8F, 0x14C279, 0x928E82, 0x0DF195, 0x8BBD6E, 0x872498, 0x016863, + 0xFAD8C4, 0x7C943F, 0x700DC9, 0xF64132, 0x693E25, 0xEF72DE, 0xE3EB28, 0x65A7D3, + 0x5B59FD, 0xDD1506, 0xD18CF0, 0x57C00B, 0xC8BF1C, 0x4EF3E7, 0x426A11, 0xC426EA, + 0x2AE476, 0xACA88D, 0xA0317B, 0x267D80, 0xB90297, 0x3F4E6C, 0x33D79A, 0xB59B61, + 0x8B654F, 0x0D29B4, 0x01B042, 0x87FCB9, 0x1883AE, 0x9ECF55, 0x9256A3, 0x141A58, + 0xEFAAFF, 0x69E604, 0x657FF2, 0xE33309, 0x7C4C1E, 0xFA00E5, 0xF69913, 0x70D5E8, + 0x4E2BC6, 0xC8673D, 0xC4FECB, 0x42B230, 0xDDCD27, 0x5B81DC, 0x57182A, 0xD154D1, + 0x26359F, 0xA07964, 0xACE092, 0x2AAC69, 0xB5D37E, 0x339F85, 0x3F0673, 0xB94A88, + 0x87B4A6, 0x01F85D, 0x0D61AB, 0x8B2D50, 0x145247, 0x921EBC, 0x9E874A, 0x18CBB1, + 0xE37B16, 0x6537ED, 0x69AE1B, 0xEFE2E0, 0x709DF7, 0xF6D10C, 0xFA48FA, 0x7C0401, + 0x42FA2F, 0xC4B6D4, 0xC82F22, 0x4E63D9, 0xD11CCE, 0x575035, 0x5BC9C3, 0xDD8538 +}; + + +extern "C" { + void dgemm_(char *, char *, int *, int *, int *, double *, double *, int *, double *, int *, double *, double *, int *); + extern void dgetrf_(int *, int *, double *, int *, int *, int *); + extern void dgetri_(int *, double *, int *, int *, double *, int *, int *); + extern void dgetrs_(char *, int *, int *, double *, int *, int *, double *, int *, int *); +} + + +/* function prototypes -------------------------------------------------------*/ + + +#ifdef IERS_MODEL +extern int gmf_(double *mjd, double *lat, double *lon, double *hgt, double *zd, + double *gmfh, double *gmfw); +#endif + + +/* fatal error ---------------------------------------------------------------*/ +void fatalerr(const char *format, ...) +{ + char msg[1024]; + va_list ap; + va_start(ap, format); vsprintf(msg, format, ap); va_end(ap); + fprintf(stderr, "%s", msg); + exit(-9); +} + + +/* satellite system+prn/slot number to satellite number ------------------------ + * convert satellite system+prn/slot number to satellite number + * args : int sys I satellite system (SYS_GPS,SYS_GLO,...) + * int prn I satellite prn/slot number + * return : satellite number (0:error) + *-----------------------------------------------------------------------------*/ +int satno(int sys, int prn) +{ + if (prn <= 0) return 0; + switch (sys) + { + case SYS_GPS: + if (prnexsats[sat-1] == 1) + { + trace(3, "excluded satellite: sat=%3d svh=%02X\n", sat, svh); + return 1; /* excluded satellite */ + } + if (opt->exsats[sat-1] == 2) return 0; /* included satellite */ + if (!(sys&opt->navsys)) + { + trace(3, "unselected sat sys: sat=%3d svh=%02X\n", sat, svh); + return 1; /* unselected sat sys */ + } + } + if (sys == SYS_QZS) svh &= 0xFE; /* mask QZSS LEX health */ + if (svh) + { + trace(3, "unhealthy satellite: sat=%3d svh=%02X\n", sat, svh); + return 1; + } + return 0; +} + + +/* test SNR mask --------------------------------------------------------------- + * test SNR mask + * args : int base I rover or base-station (0:rover,1:base station) + * int freq I frequency (0:L1,1:L2,2:L3,...) + * double el I elevation angle (rad) + * double snr I C/N0 (dBHz) + * snrmask_t *mask I SNR mask + * return : status (1:masked,0:unmasked) + *-----------------------------------------------------------------------------*/ +int testsnr(int base, int freq, double el, double snr, + const snrmask_t *mask) +{ + double minsnr, a; + int i; + + if (!mask->ena[base] || freq<0 || freq >= NFREQ) return 0; + + a = (el*R2D+5.0)/10.0; + i = (int)floor(a); a -= i; + if (i<1) minsnr = mask->mask[freq][0]; + else if (i>8) minsnr = mask->mask[freq][8]; + else minsnr = (1.0-a)*mask->mask[freq][i-1]+a*mask->mask[freq][i]; + + return snr>(7-i%8))&1u); + return bits; +} + + +int getbits(const unsigned char *buff, int pos, int len) +{ + unsigned int bits = getbitu(buff, pos, len); + if (len <= 0 || 32 <= len || !(bits&(1u<<(len-1)))) return (int)bits; + return (int)(bits|(~0u<>=1) + { + if (data&mask) buff[i/8]|=1u<<(7-i%8); else buff[i/8]&=~(1u<<(7-i%8)); + } +} + + +void setbits(unsigned char *buff, int pos, int len, int data) +{ + if (data<0) data|=1<<(len-1); else data&=~(1<<(len-1)); /* set sign bit */ + setbitu(buff, pos, len, (unsigned int)data); +} + + +/* crc-32 parity --------------------------------------------------------------- + * compute crc-32 parity for novatel raw + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-32 parity + * notes : see NovAtel OEMV firmware manual 1.7 32-bit CRC + *-----------------------------------------------------------------------------*/ +unsigned int rtk_crc32(const unsigned char *buff, int len) +{ + unsigned int crc = 0; + int i, j; + + trace(4, "rtk_crc32: len=%d\n", len); + + for (i = 0; i>1)^POLYCRC32; else crc>>=1; + } + } + return crc; +} + + +/* crc-24q parity -------------------------------------------------------------- + * compute crc-24q parity for sbas, rtcm3 + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-24Q parity + * notes : see reference [2] A.4.3.3 Parity + *-----------------------------------------------------------------------------*/ +unsigned int rtk_crc24q(const unsigned char *buff, int len) +{ + unsigned int crc = 0; + int i; + + trace(4, "rtk_crc24q: len=%d\n", len); + + for (i = 0; i>16)^buff[i]]; + return crc; +} + + +/* crc-16 parity --------------------------------------------------------------- + * compute crc-16 parity for binex, nvs + * args : unsigned char *buff I data + * int len I data length (bytes) + * return : crc-16 parity + * notes : see reference [10] A.3. + *-----------------------------------------------------------------------------*/ +unsigned short rtk_crc16(const unsigned char *buff, int len) +{ + unsigned short crc = 0; + int i; + + trace(4, "rtk_crc16: len=%d\n", len); + + for (i = 0; i>8)^buff[i])&0xFF]; + } + return crc; +} + + +/* decode navigation data word ------------------------------------------------- + * check party and decode navigation data word + * args : unsigned int word I navigation data word (2+30bit) + * (previous word D29*-30* + current word D1-30) + * unsigned char *data O decoded navigation data without parity + * (8bitx3) + * return : status (1:ok,0:parity error) + * notes : see reference [1] 20.3.5.2 user parity algorithm + *-----------------------------------------------------------------------------*/ +int decode_word(unsigned int word, unsigned char *data) +{ + const unsigned int hamming[] = { + 0xBB1F3480, 0x5D8F9A40, 0xAEC7CD00, 0x5763E680, 0x6BB1F340, 0x8B7A89C0 + }; + unsigned int parity = 0, w; + int i; + + trace(5, "decodeword: word=%08x\n", word); + + if (word&0x40000000) word^=0x3FFFFFC0; + + for (i = 0; i<6; i++) + { + parity<<= 1; + for (w = (word&hamming[i])>>6; w; w>>=1) parity^=w&1; + } + if (parity != (word&0x3F)) return 0; + + for (i = 0; i<3; i++) data[i] = (unsigned char)(word>>(22-i*8)); + return 1; +} + + +/* new matrix ------------------------------------------------------------------ + * allocate memory of matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n<=0 or m<=0, return NULL) + *-----------------------------------------------------------------------------*/ +double *mat(int n, int m) +{ + double *p; + + if (n <= 0 || m <= 0) return NULL; + if (!(p = (double *)malloc(sizeof(double)*n*m))) + { + fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m); + } + return p; +} + + +/* new integer matrix ---------------------------------------------------------- + * allocate memory of integer matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n <= 0 or m <= 0, return NULL) + *-----------------------------------------------------------------------------*/ +int *imat(int n, int m) +{ + int *p; + + if (n <= 0 || m <= 0) return NULL; + if (!(p = (int *)malloc(sizeof(int)*n*m))) + { + fatalerr("integer matrix memory allocation error: n=%d,m=%d\n", n, m); + } + return p; +} + + +/* zero matrix ----------------------------------------------------------------- + * generate new zero matrix + * args : int n,m I number of rows and columns of matrix + * return : matrix pointer (if n <= 0 or m <= 0, return NULL) + *-----------------------------------------------------------------------------*/ +double *zeros(int n, int m) +{ + double *p; + +#if NOCALLOC + if ((p = mat(n, m))) for (n = n*m-1; n >= 0; n--) p[n] = 0.0; +#else + if (n <= 0 || m <= 0) return NULL; + if (!(p = (double *)calloc(sizeof(double), n*m))) { + fatalerr("matrix memory allocation error: n=%d,m=%d\n", n, m); + } +#endif + return p; +} + + +/* identity matrix ------------------------------------------------------------- + * generate new identity matrix + * args : int n I number of rows and columns of matrix + * return : matrix pointer (if n <= 0, return NULL) + *-----------------------------------------------------------------------------*/ +double *eye(int n) +{ + double *p; + int i; + + if ((p = zeros(n, n))) for (i = 0; i= 0) c += a[n]*b[n]; + return c; +} + + +/* euclid norm ----------------------------------------------------------------- + * euclid norm of vector + * args : double *a I vector a (n x 1) + * int n I size of vector a + * return : || a || + *-----------------------------------------------------------------------------*/ +double norm_rtk(const double *a, int n) +{ + return std::sqrt(dot(a, a, n)); +} + + +/* outer product of 3d vectors ------------------------------------------------- + * outer product of 3d vectors + * args : double *a,*b I vector a,b (3 x 1) + * double *c O outer product (a x b) (3 x 1) + * return : none + *-----------------------------------------------------------------------------*/ +void cross3(const double *a, const double *b, double *c) +{ + c[0] = a[1]*b[2]-a[2]*b[1]; + c[1] = a[2]*b[0]-a[0]*b[2]; + c[2] = a[0]*b[1]-a[1]*b[0]; +} + + +/* normalize 3d vector --------------------------------------------------------- + * normalize 3d vector + * args : double *a I vector a (3 x 1) + * double *b O normlized vector (3 x 1) || b || = 1 + * return : status (1:ok,0:error) + *-----------------------------------------------------------------------------*/ +int normv3(const double *a, double *b) +{ + double r; + if ((r = norm_rtk(a, 3)) <= 0.0) return 0; + b[0] = a[0]/r; + b[1] = a[1]/r; + b[2] = a[2]/r; + return 1; +} + + +/* copy matrix ----------------------------------------------------------------- + * copy matrix + * args : double *A O destination matrix A (n x m) + * double *B I source matrix B (n x m) + * int n,m I number of rows and columns of matrix + * return : none + *-----------------------------------------------------------------------------*/ +void matcpy(double *A, const double *B, int n, int m) +{ + memcpy(A, B, sizeof(double)*n*m); +} + +/* matrix routines -----------------------------------------------------------*/ + + +/* multiply matrix (wrapper of blas dgemm) ------------------------------------- + * multiply matrix by matrix (C=alpha*A*B+beta*C) + * args : char *tr I transpose flags ("N":normal,"T":transpose) + * int n,k,m I size of (transposed) matrix A,B + * double alpha I alpha + * double *A,*B I (transposed) matrix A (n x m), B (m x k) + * double beta I beta + * double *C IO matrix C (n x k) + * return : none + *-----------------------------------------------------------------------------*/ +void matmul(const char *tr, int n, int k, int m, double alpha, + const double *A, const double *B, double beta, double *C) +{ + int lda = tr[0] == 'T' ? m : n, ldb = tr[1] == 'T' ? k : m; + + dgemm_((char *)tr, (char *)tr+1, &n, &k, &m, &alpha, (double *)A, &lda, (double *)B, + &ldb, &beta, C, &n); +} + + +/* inverse of matrix ----------------------------------------------------------- + * inverse of matrix (A=A^-1) + * args : double *A IO matrix (n x n) + * int n I size of matrix A + * return : status (0:ok,0>:error) + *-----------------------------------------------------------------------------*/ +int matinv(double *A, int n) +{ + double *work; + int info, lwork = n*16, *ipiv = imat(n, 1); + + work = mat(lwork, 1); + dgetrf_(&n, &n, A, &n, ipiv, &info); + if (!info) dgetri_(&n, A, &n, ipiv, work, &lwork, &info); + free(ipiv); free(work); + return info; +} + + +/* solve linear equation ------------------------------------------------------- + * solve linear equation (X=A\Y or X=A'\Y) + * args : char *tr I transpose flag ("N":normal,"T":transpose) + * double *A I input matrix A (n x n) + * double *Y I input matrix Y (n x m) + * int n,m I size of matrix A,Y + * double *X O X=A\Y or X=A'\Y (n x m) + * return : status (0:ok,0>:error) + * notes : matirix stored by column-major order (fortran convention) + * X can be same as Y + *-----------------------------------------------------------------------------*/ +int solve(const char *tr, const double *A, const double *Y, int n, + int m, double *X) +{ + double *B = mat(n, n); + int info, *ipiv = imat(n, 1); + + matcpy(B, A, n, n); + matcpy(X, Y, n, m); + dgetrf_(&n, &n, B, &n, ipiv, &info); + if (!info) dgetrs_((char *)tr, &n, &m, B, &n, ipiv, X, &n, &info); + free(ipiv); free(B); + return info; +} + + +/* end of matrix routines ----------------------------------------------------*/ + +/* least square estimation ----------------------------------------------------- + * least square estimation by solving normal equation (x=(A*A')^-1*A*y) + * args : double *A I transpose of (weighted) design matrix (n x m) + * double *y I (weighted) measurements (m x 1) + * int n,m I number of parameters and measurements (n <= m) + * double *x O estmated parameters (n x 1) + * double *Q O esimated parameters covariance matrix (n x n) + * return : status (0:ok,0>:error) + * notes : for weighted least square, replace A and y by A*w and w*y (w=W^(1/2)) + * matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ +int lsq(const double *A, const double *y, int n, int m, double *x, + double *Q) +{ + double *Ay; + int info; + + if (m0.0) ix[k++] = i; + x_ = mat(k, 1); xp_ = mat(k, 1); P_ = mat(k, k); Pp_ = mat(k, k); H_ = mat(k, m); + for (i = 0; i:error) + * notes : see reference [4] 5.2 + * matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ +int smoother(const double *xf, const double *Qf, const double *xb, + const double *Qb, int n, double *xs, double *Qs) +{ + double *invQf = mat(n, n), *invQb = mat(n, n), *xx = mat(n, 1); + int i, info = -1; + + matcpy(invQf, Qf, n, n); + matcpy(invQb, Qb, n, n); + if (!matinv(invQf, n) && !matinv(invQb, n)) + { + for (i = 0; i= 0; s++) *p++=*s == 'd' || *s == 'D' ? 'E' : *s; *p = '\0'; + return sscanf(str, "%lf", &value) == 1 ? value : 0.0; +} + + +/* string to time -------------------------------------------------------------- + * convert substring in string to gtime_t struct + * args : char *s I string ("... yyyy mm dd hh mm ss ...") + * int i,n I substring position and width + * gtime_t *t O gtime_t struct + * return : status (0:ok,0>:error) + *-----------------------------------------------------------------------------*/ +int str2time(const char *s, int i, int n, gtime_t *t) +{ + double ep[6]; + char str[256], *p = str; + + if (i<0 || (int)strlen(s)= 0;) *p++=*s++; *p = '\0'; + if (sscanf(str, "%lf %lf %lf %lf %lf %lf", ep, ep+1, ep+2, ep+3, ep+4, ep+5)<6) + return -1; + if (ep[0]<100.0) ep[0] += ep[0]<80.0 ? 2000.0 : 1900.0; + *t = epoch2time(ep); + return 0; +} + + +/* convert calendar day/time to time ------------------------------------------- + * convert calendar day/time to gtime_t struct + * args : double *ep I day/time {year,month,day,hour,min,sec} + * return : gtime_t struct + * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) + *-----------------------------------------------------------------------------*/ +gtime_t epoch2time(const double *ep) +{ + const int doy[] = {1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}; + gtime_t time = {0, 0}; + int days, sec, year = (int)ep[0], mon = (int)ep[1], day = (int)ep[2]; + + if (year<1970 || 2099= 3 ? 1 : 0); + sec = (int)floor(ep[5]); + time.time = (time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec; + time.sec = ep[5]-sec; + return time; +} + + +/* time to calendar day/time --------------------------------------------------- + * convert gtime_t struct to calendar day/time + * args : gtime_t t I gtime_t struct + * double *ep O day/time {year,month,day,hour,min,sec} + * return : none + * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) + *-----------------------------------------------------------------------------*/ +void time2epoch(gtime_t t, double *ep) +{ + const int mday[] = { /* # of days in a month */ + 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, + 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 + }; + int days, sec, mon, day; + + /* leap year if year%4==0 in 1901-2099 */ + days = (int)(t.time/86400); + sec = (int)(t.time-(time_t)days*86400); + for (day = days%1461, mon = 0; mon<48; mon++) + { + if (day >= mday[mon]) day -= mday[mon]; else break; + } + ep[0] = 1970+days/1461*4+mon/12; ep[1] = mon%12+1; ep[2] = day+1; + ep[3] = sec/3600; ep[4] = sec%3600/60; ep[5] = sec%60+t.sec; +} + + +/* gps time to time ------------------------------------------------------------ + * convert week and tow in gps time to gtime_t struct + * args : int week I week number in gps time + * double sec I time of week in gps time (s) + * return : gtime_t struct + *-----------------------------------------------------------------------------*/ +gtime_t gpst2time(int week, double sec) +{ + gtime_t t = epoch2time(gpst0); + + if (sec<-1e9 || 1e9tm_year+1900; ep[1] = tt->tm_mon+1; ep[2] = tt->tm_mday; + ep[3] = tt->tm_hour; ep[4] = tt->tm_min; ep[5] = tt->tm_sec+tv.tv_usec*1e-6; + } + time = epoch2time(ep); + +#ifdef CPUTIME_IN_GPST /* cputime operated in gpst */ + time = gpst2utc(time); +#endif + return time; +} + + +/* set current time in utc ----------------------------------------------------- + * set current time in utc + * args : gtime_t I current time in utc + * return : none + * notes : just set time offset between cpu time and current time + * the time offset is reflected to only timeget() + * not reentrant + *----------------------------------------------------------------------------- +void timeset(gtime_t t) +{ + timeoffset_+=timediff(t,timeget()); +} + */ +/* read leap seconds table by text -------------------------------------------*/ +int read_leaps_text(FILE *fp) +{ + char buff[256], *p; + int i, n = 0, ep[6], ls; + + rewind(fp); + + while (fgets(buff, sizeof(buff), fp) && n= 13) continue; + ls[n][0] = y; + ls[n][1] = m; + ls[n][2] = d; + ls[n++][6] = (char)(19.0-tai_utc); + } + for (i = 0; i0; i++) + { + tu = timeadd(t, leaps[i][6]); + if (timediff(tu, epoch2time(leaps[i])) >= 0.0) return tu; + } + return t; +} + + +/* utc to gpstime -------------------------------------------------------------- + * convert utc to gpstime considering leap seconds + * args : gtime_t t I time expressed in utc + * return : time expressed in gpstime + * notes : ignore slight time offset under 100 ns + *-----------------------------------------------------------------------------*/ +gtime_t utc2gpst(gtime_t t) +{ + int i; + + for (i = 0; leaps[i][0]>0; i++) + { + if (timediff(t, epoch2time(leaps[i])) >= 0.0) return timeadd(t, -leaps[i][6]); + } + return t; +} + + +/* gpstime to bdt -------------------------------------------------------------- + * convert gpstime to bdt (beidou navigation satellite system time) + * args : gtime_t t I time expressed in gpstime + * return : time expressed in bdt + * notes : ref [8] 3.3, 2006/1/1 00:00 BDT = 2006/1/1 00:00 UTC + * no leap seconds in BDT + * ignore slight time offset under 100 ns + *-----------------------------------------------------------------------------*/ +gtime_t gpst2bdt(gtime_t t) +{ + return timeadd(t, -14.0); +} + + +/* bdt to gpstime -------------------------------------------------------------- + * convert bdt (beidou navigation satellite system time) to gpstime + * args : gtime_t t I time expressed in bdt + * return : time expressed in gpstime + * notes : see gpst2bdt() + *-----------------------------------------------------------------------------*/ +gtime_t bdt2gpst(gtime_t t) +{ + return timeadd(t, 14.0); +} + + +/* time to day and sec -------------------------------------------------------*/ +double time2sec(gtime_t time, gtime_t *day) +{ + double ep[6], sec; + time2epoch(time, ep); + sec = ep[3]*3600.0+ep[4]*60.0+ep[5]; + ep[3] = ep[4] = ep[5] = 0.0; + *day = epoch2time(ep); + return sec; +} + + +/* utc to gmst ----------------------------------------------------------------- + * convert utc to gmst (Greenwich mean sidereal time) + * args : gtime_t t I time expressed in utc + * double ut1_utc I UT1-UTC (s) + * return : gmst (rad) + *-----------------------------------------------------------------------------*/ +double utc2gmst(gtime_t t, double ut1_utc) +{ + const double ep2000[] = {2000, 1, 1, 12, 0, 0}; + gtime_t tut, tut0; + double ut, t1, t2, t3, gmst0, gmst; + + tut = timeadd(t, ut1_utc); + ut = time2sec(tut, &tut0); + t1 = timediff(tut0, epoch2time(ep2000))/86400.0/36525.0; + t2 = t1*t1; t3 = t2*t1; + gmst0 = 24110.54841+8640184.812866*t1+0.093104*t2-6.2E-6*t3; + gmst = gmst0+1.002737909350795*ut; + + return fmod(gmst, 86400.0)*PI/43200.0; /* 0 <= gmst <= 2*PI */ +} + + +/* time to string -------------------------------------------------------------- + * convert gtime_t struct to string + * args : gtime_t t I gtime_t struct + * char *s O string ("yyyy/mm/dd hh:mm:ss.ssss") + * int n I number of decimals + * return : none + *-----------------------------------------------------------------------------*/ +void time2str(gtime_t t, char *s, int n) +{ + double ep[6]; + + if (n<0) n = 0; else if (n>12) n = 12; + if (1.0-t.sec<0.5/pow(10.0, n)) {t.time++; t.sec = 0.0;}; + time2epoch(t, ep); + sprintf(s, "%04.0f/%02.0f/%02.0f %02.0f:%02.0f:%0*.*f", ep[0], ep[1], ep[2], + ep[3], ep[4], n <= 0 ? 2:n+3, n <= 0 ? 0:n, ep[5]); +} + + +/* get time string ------------------------------------------------------------- + * get time string + * args : gtime_t t I gtime_t struct + * int n I number of decimals + * return : time string + * notes : not reentrant, do not use multiple in a function + *-----------------------------------------------------------------------------*/ +char *time_str(gtime_t t, int n) +{ + static char buff[64]; + time2str(t, buff, n); + return buff; +} + + +/* time to day of year --------------------------------------------------------- + * convert time to day of year + * args : gtime_t t I gtime_t struct + * return : day of year (days) + *-----------------------------------------------------------------------------*/ +double time2doy(gtime_t t) +{ + double ep[6]; + + time2epoch(t, ep); + ep[1] = ep[2] = 1.0; ep[3] = ep[4] = ep[5] = 0.0; + return timediff(t, epoch2time(ep))/86400.0+1.0; +} + + +/* adjust gps week number ------------------------------------------------------ + * adjust gps week number using cpu time + * args : int week I not-adjusted gps week number + * return : adjusted gps week number + *-----------------------------------------------------------------------------*/ +int adjgpsweek(int week) +{ + int w; + (void)time2gpst(utc2gpst(timeget()), &w); + if (w<1560) w = 1560; /* use 2009/12/1 if time is earlier than 2009/12/1 */ + return week+(w-week+512)/1024*1024; +} + + +/* get tick time --------------------------------------------------------------- + * get current tick in ms + * args : none + * return : current tick in ms + *-----------------------------------------------------------------------------*/ +unsigned int tickget(void) +{ + struct timespec tp = {0, 0}; + struct timeval tv = {0, 0}; + +#ifdef CLOCK_MONOTONIC_RAW + /* linux kernel > 2.6.28 */ + if (!clock_gettime(CLOCK_MONOTONIC_RAW, &tp)) + { + return tp.tv_sec*1000u+tp.tv_nsec/1000000u; + } + else + { + gettimeofday(&tv, NULL); + return tv.tv_sec*1000u+tv.tv_usec/1000u; + } +#else + gettimeofday(&tv, NULL); + return tv.tv_sec*1000u+tv.tv_usec/1000u; +#endif +} + + +/* sleep ms -------------------------------------------------------------------- + * sleep ms + * args : int ms I miliseconds to sleep (<0:no sleep) + * return : none + *-----------------------------------------------------------------------------*/ +void sleepms(int ms) +{ + struct timespec ts = {0, 0}; + if (ms <= 0) return; + ts.tv_sec = (time_t)(ms/1000); + ts.tv_nsec = (long)(ms%1000*1000000); + nanosleep(&ts, NULL); +} + + +/* convert degree to deg-min-sec ----------------------------------------------- + * convert degree to degree-minute-second + * args : double deg I degree + * double *dms O degree-minute-second {deg, min, sec} + * int ndec I number of decimals of second + * return : none + *-----------------------------------------------------------------------------*/ +void deg2dms(double deg, double *dms, int ndec) +{ + double sign = deg<0.0 ? -1.0 : 1.0, a = fabs(deg); + double unit = pow(0.1, ndec); + dms[0] = floor(a); a = (a-dms[0])*60.0; + dms[1] = floor(a); a = (a-dms[1])*60.0; + dms[2] = floor(a/unit+0.5)*unit; + if (dms[2] >= 60.0) + { + dms[2] = 0.0; + dms[1] += 1.0; + if (dms[1] >= 60.0) + { + dms[1] = 0.0; + dms[0] += 1.0; + } + } + dms[0]*=sign; +} + + +/* convert deg-min-sec to degree ----------------------------------------------- + * convert degree-minute-second to degree + * args : double *dms I degree-minute-second {deg,min,sec} + * return : degree + *-----------------------------------------------------------------------------*/ +double dms2deg(const double *dms) +{ + double sign = dms[0]<0.0 ? -1.0 : 1.0; + return sign*(fabs(dms[0])+dms[1]/60.0+dms[2]/3600.0); +} + + +/* transform ecef to geodetic postion ------------------------------------------ + * transform ecef position to geodetic position + * args : double *r I ecef position {x,y,z} (m) + * double *pos O geodetic position {lat,lon,h} (rad,m) + * return : none + * notes : WGS84, ellipsoidal height + *-----------------------------------------------------------------------------*/ +void ecef2pos(const double *r, double *pos) +{ + double e2 = FE_WGS84*(2.0-FE_WGS84), r2 = dot(r, r, 2), z, zk, v = RE_WGS84, sinp; + + for (z = r[2], zk = 0.0; fabs(z-zk) >= 1e-4;) + { + zk = z; + sinp = z/sqrt(r2+z*z); + v = RE_WGS84/sqrt(1.0-e2*sinp*sinp); + z = r[2]+v*e2*sinp; + } + pos[0] = r2>1e-12 ? atan(z/sqrt(r2)) : (r[2]>0.0 ? PI/2.0 : -PI/2.0); + pos[1] = r2>1e-12 ? atan2(r[1], r[0]) : 0.0; + pos[2] = sqrt(r2+z*z)-v; +} + + +/* transform geodetic to ecef position ----------------------------------------- + * transform geodetic position to ecef position + * args : double *pos I geodetic position {lat, lon,h} (rad,m) + * double *r O ecef position {x,y,z} (m) + * return : none + * notes : WGS84, ellipsoidal height + *-----------------------------------------------------------------------------*/ +void pos2ecef(const double *pos, double *r) +{ + double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]); + double e2 = FE_WGS84*(2.0-FE_WGS84), v = RE_WGS84/sqrt(1.0-e2*sinp*sinp); + + r[0] = (v+pos[2])*cosp*cosl; + r[1] = (v+pos[2])*cosp*sinl; + r[2] = (v*(1.0-e2)+pos[2])*sinp; +} + + +/* ecef to local coordinate transfromation matrix ------------------------------ + * compute ecef to local coordinate transfromation matrix + * args : double *pos I geodetic position {lat,lon} (rad) + * double *E O ecef to local coord transformation matrix (3x3) + * return : none + * notes : matirix stored by column-major order (fortran convention) + *-----------------------------------------------------------------------------*/ +void xyz2enu(const double *pos, double *E) +{ + double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]); + + E[0] = -sinl; E[3] = cosl; E[6] = 0.0; + E[1] = -sinp*cosl; E[4] = -sinp*sinl; E[7] = cosp; + E[2] = cosp*cosl; E[5] = cosp*sinl; E[8] = sinp; +} + + +/* transform ecef vector to local tangental coordinate ------------------------- + * transform ecef vector to local tangental coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *r I vector in ecef coordinate {x,y,z} + * double *e O vector in local tangental coordinate {e,n,u} + * return : none + *-----------------------------------------------------------------------------*/ +void ecef2enu(const double *pos, const double *r, double *e) +{ + double E[9]; + + xyz2enu(pos,E); + matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e); +} + + +/* transform local vector to ecef coordinate ----------------------------------- + * transform local tangental coordinate vector to ecef + * args : double *pos I geodetic position {lat,lon} (rad) + * double *e I vector in local tangental coordinate {e,n,u} + * double *r O vector in ecef coordinate {x,y,z} + * return : none + *-----------------------------------------------------------------------------*/ +void enu2ecef(const double *pos, const double *e, double *r) +{ + double E[9]; + + xyz2enu(pos, E); + matmul("TN", 3, 1, 3, 1.0, E, e, 0.0, r); +} + + +/* transform covariance to local tangental coordinate -------------------------- + * transform ecef covariance to local tangental coordinate + * args : double *pos I geodetic position {lat, lon} (rad) + * double *P I covariance in ecef coordinate + * double *Q O covariance in local tangental coordinate + * return : none + *-----------------------------------------------------------------------------*/ +void covenu(const double *pos, const double *P, double *Q) +{ + double E[9], EP[9]; + + xyz2enu(pos, E); + matmul("NN", 3, 3, 3, 1.0, E, P, 0.0, EP); + matmul("NT", 3, 3, 3, 1.0, EP, E, 0.0, Q); +} + + +/* transform local enu coordinate covariance to xyz-ecef ----------------------- + * transform local enu covariance to xyz-ecef coordinate + * args : double *pos I geodetic position {lat,lon} (rad) + * double *Q I covariance in local enu coordinate + * double *P O covariance in xyz-ecef coordinate + * return : none + *-----------------------------------------------------------------------------*/ +void covecef(const double *pos, const double *Q, double *P) +{ + double E[9], EQ[9]; + + xyz2enu(pos, E); + matmul("TN", 3, 3, 3, 1.0, E, Q, 0.0, EQ); + matmul("NN", 3, 3, 3, 1.0, EQ, E, 0.0, P); +} + + +/* astronomical arguments: f={l,l',F,D,OMG} (rad) ----------------------------*/ +void ast_args(double t, double *f) +{ + static const double fc[][5] = { /* coefficients for iau 1980 nutation */ + { 134.96340251, 1717915923.2178, 31.8792, 0.051635, -0.00024470}, + { 357.52910918, 129596581.0481, -0.5532, 0.000136, -0.00001149}, + { 93.27209062, 1739527262.8478, -12.7512, -0.001037, 0.00000417}, + { 297.85019547, 1602961601.2090, -6.3706, 0.006593, -0.00003169}, + { 125.04455501, -6962890.2665, 7.4722, 0.007702, -0.00005939} + }; + double tt[4]; + int i, j; + + for (tt[0] = t, i = 1; i<4; i++) tt[i] = tt[i-1]*t; + for (i = 0; i<5; i++) + { + f[i] = fc[i][0]*3600.0; + for (j = 0; j<4; j++) f[i] += fc[i][j+1]*tt[j]; + f[i] = fmod(f[i]*AS2R, 2.0*PI); + } +} + + +/* iau 1980 nutation ---------------------------------------------------------*/ +void nut_iau1980(double t, const double *f, double *dpsi, double *deps) +{ + static const double nut[106][10] = { + { 0, 0, 0, 0, 1, -6798.4, -171996, -174.2, 92025, 8.9}, + { 0, 0, 2, -2, 2, 182.6, -13187, -1.6, 5736, -3.1}, + { 0, 0, 2, 0, 2, 13.7, -2274, -0.2, 977, -0.5}, + { 0, 0, 0, 0, 2, -3399.2, 2062, 0.2, -895, 0.5}, + { 0, -1, 0, 0, 0, -365.3, -1426, 3.4, 54, -0.1}, + { 1, 0, 0, 0, 0, 27.6, 712, 0.1, -7, 0.0}, + { 0, 1, 2, -2, 2, 121.7, -517, 1.2, 224, -0.6}, + { 0, 0, 2, 0, 1, 13.6, -386, -0.4, 200, 0.0}, + { 1, 0, 2, 0, 2, 9.1, -301, 0.0, 129, -0.1}, + { 0, -1, 2, -2, 2, 365.2, 217, -0.5, -95, 0.3}, + { -1, 0, 0, 2, 0, 31.8, 158, 0.0, -1, 0.0}, + { 0, 0, 2, -2, 1, 177.8, 129, 0.1, -70, 0.0}, + { -1, 0, 2, 0, 2, 27.1, 123, 0.0, -53, 0.0}, + { 1, 0, 0, 0, 1, 27.7, 63, 0.1, -33, 0.0}, + { 0, 0, 0, 2, 0, 14.8, 63, 0.0, -2, 0.0}, + { -1, 0, 2, 2, 2, 9.6, -59, 0.0, 26, 0.0}, + { -1, 0, 0, 0, 1, -27.4, -58, -0.1, 32, 0.0}, + { 1, 0, 2, 0, 1, 9.1, -51, 0.0, 27, 0.0}, + { -2, 0, 0, 2, 0, -205.9, -48, 0.0, 1, 0.0}, + { -2, 0, 2, 0, 1, 1305.5, 46, 0.0, -24, 0.0}, + { 0, 0, 2, 2, 2, 7.1, -38, 0.0, 16, 0.0}, + { 2, 0, 2, 0, 2, 6.9, -31, 0.0, 13, 0.0}, + { 2, 0, 0, 0, 0, 13.8, 29, 0.0, -1, 0.0}, + { 1, 0, 2, -2, 2, 23.9, 29, 0.0, -12, 0.0}, + { 0, 0, 2, 0, 0, 13.6, 26, 0.0, -1, 0.0}, + { 0, 0, 2, -2, 0, 173.3, -22, 0.0, 0, 0.0}, + { -1, 0, 2, 0, 1, 27.0, 21, 0.0, -10, 0.0}, + { 0, 2, 0, 0, 0, 182.6, 17, -0.1, 0, 0.0}, + { 0, 2, 2, -2, 2, 91.3, -16, 0.1, 7, 0.0}, + { -1, 0, 0, 2, 1, 32.0, 16, 0.0, -8, 0.0}, + { 0, 1, 0, 0, 1, 386.0, -15, 0.0, 9, 0.0}, + { 1, 0, 0, -2, 1, -31.7, -13, 0.0, 7, 0.0}, + { 0, -1, 0, 0, 1, -346.6, -12, 0.0, 6, 0.0}, + { 2, 0, -2, 0, 0, -1095.2, 11, 0.0, 0, 0.0}, + { -1, 0, 2, 2, 1, 9.5, -10, 0.0, 5, 0.0}, + { 1, 0, 2, 2, 2, 5.6, -8, 0.0, 3, 0.0}, + { 0, -1, 2, 0, 2, 14.2, -7, 0.0, 3, 0.0}, + { 0, 0, 2, 2, 1, 7.1, -7, 0.0, 3, 0.0}, + { 1, 1, 0, -2, 0, -34.8, -7, 0.0, 0, 0.0}, + { 0, 1, 2, 0, 2, 13.2, 7, 0.0, -3, 0.0}, + { -2, 0, 0, 2, 1, -199.8, -6, 0.0, 3, 0.0}, + { 0, 0, 0, 2, 1, 14.8, -6, 0.0, 3, 0.0}, + { 2, 0, 2, -2, 2, 12.8, 6, 0.0, -3, 0.0}, + { 1, 0, 0, 2, 0, 9.6, 6, 0.0, 0, 0.0}, + { 1, 0, 2, -2, 1, 23.9, 6, 0.0, -3, 0.0}, + { 0, 0, 0, -2, 1, -14.7, -5, 0.0, 3, 0.0}, + { 0, -1, 2, -2, 1, 346.6, -5, 0.0, 3, 0.0}, + { 2, 0, 2, 0, 1, 6.9, -5, 0.0, 3, 0.0}, + { 1, -1, 0, 0, 0, 29.8, 5, 0.0, 0, 0.0}, + { 1, 0, 0, -1, 0, 411.8, -4, 0.0, 0, 0.0}, + { 0, 0, 0, 1, 0, 29.5, -4, 0.0, 0, 0.0}, + { 0, 1, 0, -2, 0, -15.4, -4, 0.0, 0, 0.0}, + { 1, 0, -2, 0, 0, -26.9, 4, 0.0, 0, 0.0}, + { 2, 0, 0, -2, 1, 212.3, 4, 0.0, -2, 0.0}, + { 0, 1, 2, -2, 1, 119.6, 4, 0.0, -2, 0.0}, + { 1, 1, 0, 0, 0, 25.6, -3, 0.0, 0, 0.0}, + { 1, -1, 0, -1, 0, -3232.9, -3, 0.0, 0, 0.0}, + { -1, -1, 2, 2, 2, 9.8, -3, 0.0, 1, 0.0}, + { 0, -1, 2, 2, 2, 7.2, -3, 0.0, 1, 0.0}, + { 1, -1, 2, 0, 2, 9.4, -3, 0.0, 1, 0.0}, + { 3, 0, 2, 0, 2, 5.5, -3, 0.0, 1, 0.0}, + { -2, 0, 2, 0, 2, 1615.7, -3, 0.0, 1, 0.0}, + { 1, 0, 2, 0, 0, 9.1, 3, 0.0, 0, 0.0}, + { -1, 0, 2, 4, 2, 5.8, -2, 0.0, 1, 0.0}, + { 1, 0, 0, 0, 2, 27.8, -2, 0.0, 1, 0.0}, + { -1, 0, 2, -2, 1, -32.6, -2, 0.0, 1, 0.0}, + { 0, -2, 2, -2, 1, 6786.3, -2, 0.0, 1, 0.0}, + { -2, 0, 0, 0, 1, -13.7, -2, 0.0, 1, 0.0}, + { 2, 0, 0, 0, 1, 13.8, 2, 0.0, -1, 0.0}, + { 3, 0, 0, 0, 0, 9.2, 2, 0.0, 0, 0.0}, + { 1, 1, 2, 0, 2, 8.9, 2, 0.0, -1, 0.0}, + { 0, 0, 2, 1, 2, 9.3, 2, 0.0, -1, 0.0}, + { 1, 0, 0, 2, 1, 9.6, -1, 0.0, 0, 0.0}, + { 1, 0, 2, 2, 1, 5.6, -1, 0.0, 1, 0.0}, + { 1, 1, 0, -2, 1, -34.7, -1, 0.0, 0, 0.0}, + { 0, 1, 0, 2, 0, 14.2, -1, 0.0, 0, 0.0}, + { 0, 1, 2, -2, 0, 117.5, -1, 0.0, 0, 0.0}, + { 0, 1, -2, 2, 0, -329.8, -1, 0.0, 0, 0.0}, + { 1, 0, -2, 2, 0, 23.8, -1, 0.0, 0, 0.0}, + { 1, 0, -2, -2, 0, -9.5, -1, 0.0, 0, 0.0}, + { 1, 0, 2, -2, 0, 32.8, -1, 0.0, 0, 0.0}, + { 1, 0, 0, -4, 0, -10.1, -1, 0.0, 0, 0.0}, + { 2, 0, 0, -4, 0, -15.9, -1, 0.0, 0, 0.0}, + { 0, 0, 2, 4, 2, 4.8, -1, 0.0, 0, 0.0}, + { 0, 0, 2, -1, 2, 25.4, -1, 0.0, 0, 0.0}, + { -2, 0, 2, 4, 2, 7.3, -1, 0.0, 1, 0.0}, + { 2, 0, 2, 2, 2, 4.7, -1, 0.0, 0, 0.0}, + { 0, -1, 2, 0, 1, 14.2, -1, 0.0, 0, 0.0}, + { 0, 0, -2, 0, 1, -13.6, -1, 0.0, 0, 0.0}, + { 0, 0, 4, -2, 2, 12.7, 1, 0.0, 0, 0.0}, + { 0, 1, 0, 0, 2, 409.2, 1, 0.0, 0, 0.0}, + { 1, 1, 2, -2, 2, 22.5, 1, 0.0, -1, 0.0}, + { 3, 0, 2, -2, 2, 8.7, 1, 0.0, 0, 0.0}, + { -2, 0, 2, 2, 2, 14.6, 1, 0.0, -1, 0.0}, + { -1, 0, 0, 0, 2, -27.3, 1, 0.0, -1, 0.0}, + { 0, 0, -2, 2, 1, -169.0, 1, 0.0, 0, 0.0}, + { 0, 1, 2, 0, 1, 13.1, 1, 0.0, 0, 0.0}, + { -1, 0, 4, 0, 2, 9.1, 1, 0.0, 0, 0.0}, + { 2, 1, 0, -2, 0, 131.7, 1, 0.0, 0, 0.0}, + { 2, 0, 0, 2, 0, 7.1, 1, 0.0, 0, 0.0}, + { 2, 0, 2, -2, 1, 12.8, 1, 0.0, -1, 0.0}, + { 2, 0, -2, 0, 1, -943.2, 1, 0.0, 0, 0.0}, + { 1, -1, 0, -2, 0, -29.3, 1, 0.0, 0, 0.0}, + { -1, 0, 0, 1, 1, -388.3, 1, 0.0, 0, 0.0}, + { -1, -1, 0, 2, 1, 35.0, 1, 0.0, 0, 0.0}, + { 0, 1, 0, 1, 0, 27.3, 1, 0.0, 0, 0.0} + }; + double ang; + int i, j; + + *dpsi = *deps = 0.0; + + for (i = 0; i<106; i++) + { + ang = 0.0; + for (j = 0; j<5; j++) ang += nut[i][j]*f[j]; + *dpsi += (nut[i][6]+nut[i][7]*t)*sin(ang); + *deps += (nut[i][8]+nut[i][9]*t)*cos(ang); + } + *dpsi*=1e-4*AS2R; /* 0.1 mas -> rad */ + *deps*=1e-4*AS2R; +} + + +/* eci to ecef transformation matrix ------------------------------------------- + * compute eci to ecef transformation matrix + * args : gtime_t tutc I time in utc + * double *erpv I erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * double *U O eci to ecef transformation matrix (3 x 3) + * double *gmst IO greenwich mean sidereal time (rad) + * (NULL: no output) + * return : none + * note : see ref [3] chap 5 + * not thread-safe + *-----------------------------------------------------------------------------*/ +void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst) +{ + const double ep2000[] = {2000, 1, 1, 12, 0, 0}; + static gtime_t tutc_; + static double U_[9], gmst_; + gtime_t tgps; + double eps, ze, th, z, t, t2, t3, dpsi, deps, gast, f[5]; + double R1[9], R2[9], R3[9], R[9], W[9], N[9], P[9], NP[9]; + int i; + + trace(4, "eci2ecef: tutc=%s\n", time_str(tutc, 3)); + + if (fabs(timediff(tutc, tutc_))<0.01) + { /* read cache */ + for (i = 0; i<9; i++) U[i] = U_[i]; + if (gmst) *gmst = gmst_; + return; + } + tutc_ = tutc; + + /* terrestrial time */ + tgps = utc2gpst(tutc_); + t = (timediff(tgps, epoch2time(ep2000))+19.0+32.184)/86400.0/36525.0; + t2 = t*t; t3 = t2*t; + + /* astronomical arguments */ + ast_args(t, f); + + /* iau 1976 precession */ + ze = (2306.2181*t+0.30188*t2+0.017998*t3)*AS2R; + th = (2004.3109*t-0.42665*t2-0.041833*t3)*AS2R; + z = (2306.2181*t+1.09468*t2+0.018203*t3)*AS2R; + eps = (84381.448-46.8150*t-0.00059*t2+0.001813*t3)*AS2R; + Rz(-z, R1); Ry(th, R2); Rz(-ze, R3); + matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, R); + matmul("NN", 3, 3, 3, 1.0, R, R3, 0.0, P); /* P=Rz(-z)*Ry(th)*Rz(-ze) */ + + /* iau 1980 nutation */ + nut_iau1980(t, f, &dpsi, &deps); + Rx(-eps-deps, R1); Rz(-dpsi, R2); Rx(eps, R3); + matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, R); + matmul("NN", 3, 3, 3, 1.0, R , R3, 0.0, N); /* N=Rx(-eps)*Rz(-dspi)*Rx(eps) */ + + /* greenwich aparent sidereal time (rad) */ + gmst_ = utc2gmst(tutc_, erpv[2]); + gast = gmst_+dpsi*cos(eps); + gast += (0.00264*sin(f[4])+0.000063*sin(2.0*f[4]))*AS2R; + + /* eci to ecef transformation matrix */ + Ry(-erpv[0], R1); Rx(-erpv[1], R2); Rz(gast, R3); + matmul("NN", 3, 3, 3, 1.0, R1, R2, 0.0, W ); + matmul("NN", 3, 3, 3, 1.0, W , R3, 0.0, R ); /* W=Ry(-xp)*Rx(-yp) */ + matmul("NN", 3, 3, 3, 1.0, N , P , 0.0, NP); + matmul("NN", 3, 3, 3, 1.0, R , NP, 0.0, U_); /* U=W*Rz(gast)*N*P */ + + for (i = 0; i<9; i++) U[i] = U_[i]; + if (gmst) *gmst = gmst_; + + trace(5, "gmst=%.12f gast=%.12f\n", gmst_, gast); + trace(5, "P=\n"); //tracemat(5, P, 3, 3, 15, 12); + trace(5, "N=\n"); //tracemat(5, N, 3, 3, 15, 12); + trace(5, "W=\n"); //tracemat(5, W, 3, 3, 15, 12); + trace(5, "U=\n"); //tracemat(5, U, 3, 3, 15, 12); +} + + +/* decode antenna parameter field --------------------------------------------*/ +int decodef(char *p, int n, double *v) +{ + int i; + + for (i = 0; inmax <= pcvs->n) + { + pcvs->nmax += 256; + if (!(pcvs_pcv = (pcv_t *)realloc(pcvs->pcv, sizeof(pcv_t)*pcvs->nmax))) + { + trace(1, "addpcv: memory allocation error\n"); + free(pcvs->pcv); pcvs->pcv = NULL; pcvs->n = pcvs->nmax = 0; + return; + } + pcvs->pcv = pcvs_pcv; + } + pcvs->pcv[pcvs->n++] = *pcv; +} + + +/* read ngs antenna parameter file -------------------------------------------*/ +int readngspcv(const char *file, pcvs_t *pcvs) +{ + FILE *fp; + static const pcv_t pcv0 = {0, {}, {}, {0,0}, {0,0}, {{},{}}, {{},{}} }; + pcv_t pcv; + double neu[3]; + int n = 0; + char buff[256]; + + if (!(fp = fopen(file, "r"))) + { + trace(2, "ngs pcv file open error: %s\n", file); + return 0; + } + while (fgets(buff, sizeof(buff), fp)) + { + if (strlen(buff) >= 62 && buff[61] == '|') continue; + + if (buff[0] != ' ') n = 0; /* start line */ + if (++n == 1) + { + pcv = pcv0; + strncpy(pcv.type, buff, 61); pcv.type[61] = '\0'; + } + else if (n == 2) + { + if (decodef(buff, 3, neu)<3) continue; + pcv.off[0][0] = neu[1]; + pcv.off[0][1] = neu[0]; + pcv.off[0][2] = neu[2]; + } + else if (n == 3) decodef(buff, 10, pcv.var[0]); + else if (n == 4) decodef(buff, 9, pcv.var[0]+10); + else if (n == 5) + { + if (decodef(buff, 3, neu)<3) continue;; + pcv.off[1][0] = neu[1]; + pcv.off[1][1] = neu[0]; + pcv.off[1][2] = neu[2]; + } + else if (n == 6) decodef(buff, 10, pcv.var[1]); + else if (n == 7) + { + decodef(buff, 9, pcv.var[1]+10); + addpcv(&pcv, pcvs); + } + } + fclose(fp); + + return 1; +} + + +/* read antex file ----------------------------------------------------------*/ +int readantex(const char *file, pcvs_t *pcvs) +{ + FILE *fp; + static const pcv_t pcv0 = {0, {}, {}, {0,0}, {0,0}, {{},{}}, {{},{}} }; + pcv_t pcv; + double neu[3]; + int i, f, freq = 0, state = 0, freqs[] = {1, 2, 5, 6, 7, 8, 0}; + char buff[256]; + + trace(3, "readantex: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) + { + trace(2, "antex pcv file open error: %s\n", file); + return 0; + } + while (fgets(buff, sizeof(buff), fp)) + { + if (strlen(buff)<60 || strstr(buff+60, "COMMENT")) continue; + + if (strstr(buff+60, "START OF ANTENNA")) + { + pcv = pcv0; + state = 1; + } + if (strstr(buff+60, "END OF ANTENNA")) + { + addpcv(&pcv, pcvs); + state = 0; + } + if (!state) continue; + + if (strstr(buff+60, "TYPE / SERIAL NO")) + { + strncpy(pcv.type, buff , 20); pcv.type[20] = '\0'; + strncpy(pcv.code, buff+20, 20); pcv.code[20] = '\0'; + if (!strncmp(pcv.code+3, " ", 8)) + { + pcv.sat = satid2no(pcv.code); + } + } + else if (strstr(buff+60, "VALID FROM")) + { + if (!str2time(buff, 0, 43, &pcv.ts)) continue; + } + else if (strstr(buff+60, "VALID UNTIL")) + { + if (!str2time(buff, 0, 43, &pcv.te)) continue; + } + else if (strstr(buff+60, "START OF FREQUENCY")) + { + if (sscanf(buff+4, "%d", &f)<1) continue; + for (i = 0; in; i++) + { + pcv = pcvs->pcv+i; + trace(4, "sat=%2d type=%20s code=%s off=%8.4f %8.4f %8.4f %8.4f %8.4f %8.4f\n", + pcv->sat, pcv->type, pcv->code, pcv->off[0][0], pcv->off[0][1], + pcv->off[0][2], pcv->off[1][0], pcv->off[1][1], pcv->off[1][2]); + } + return stat; +} + + +/* search antenna parameter ---------------------------------------------------- + * read satellite antenna phase center position + * args : int sat I satellite number (0: receiver antenna) + * char *type I antenna type for receiver antenna + * gtime_t time I time to search parameters + * pcvs_t *pcvs IO antenna parameters + * return : antenna parameter (NULL: no antenna) + *-----------------------------------------------------------------------------*/ +pcv_t *searchpcv(int sat, const char *type, gtime_t time, + const pcvs_t *pcvs) +{ + pcv_t *pcv; + char buff[MAXANT], *types[2], *p; + int i, j, n = 0; + + trace(3, "searchpcv: sat=%2d type=%s\n", sat, type); + + if (sat) { /* search satellite antenna */ + for (i = 0; in; i++) + { + pcv = pcvs->pcv+i; + if (pcv->sat != sat) continue; + if (pcv->ts.time != 0 && timediff(pcv->ts, time)>0.0) continue; + if (pcv->te.time != 0 && timediff(pcv->te, time)<0.0) continue; + return pcv; + } + } + else + { + strcpy(buff, type); + for (p = strtok(buff, " "); p && n<2; p = strtok(NULL, " ")) types[n++] = p; + if (n <= 0) return NULL; + + /* search receiver antenna with radome at first */ + for (i = 0; in; i++) + { + pcv = pcvs->pcv+i; + for (j = 0; jtype, types[j])) break; + if (j >= n) return pcv; + } + /* search receiver antenna without radome */ + for (i = 0; in; i++) + { + pcv = pcvs->pcv+i; + if (strstr(pcv->type, types[0]) != pcv->type) continue; + + trace(2, "pcv without radome is used type=%s\n", type); + return pcv; + } + } + return NULL; +} + + +/* read station positions ------------------------------------------------------ + * read positions from station position file + * args : char *file I station position file containing + * lat(deg) lon(deg) height(m) name in a line + * char *rcvs I station name + * double *pos O station position {lat,lon,h} (rad/m) + * (all 0 if search error) + * return : none + *-----------------------------------------------------------------------------*/ +void readpos(const char *file, const char *rcv, double *pos) +{ + static double poss[2048][3]; + static char stas[2048][16]; + FILE *fp; + int i, j, len, np = 0; + char buff[256], str[256]; + + trace(3, "readpos: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) + { + fprintf(stderr, "reference position file open error : %s\n", file); + return; + } + while (np<2048 && fgets(buff, sizeof(buff), fp)) + { + if (buff[0] == '%' || buff[0] == '#') continue; + if (sscanf(buff, "%lf %lf %lf %s", &poss[np][0], &poss[np][1], &poss[np][2], + str)<4) continue; + strncpy(stas[np], str, 15); stas[np++][15] = '\0'; + } + fclose(fp); + len = (int)strlen(rcv); + for (i = 0; in >= erp->nmax) + { + erp->nmax = erp->nmax <= 0 ? 128 : erp->nmax*2; + erp_data = (erpd_t *)realloc(erp->data, sizeof(erpd_t)*erp->nmax); + if (!erp_data) + { + free(erp->data); erp->data = NULL; erp->n = erp->nmax = 0; + fclose(fp); + return 0; + } + erp->data = erp_data; + } + erp->data[erp->n].mjd = v[0]; + erp->data[erp->n].xp = v[1]*1e-6*AS2R; + erp->data[erp->n].yp = v[2]*1e-6*AS2R; + erp->data[erp->n].ut1_utc = v[3]*1e-7; + erp->data[erp->n].lod = v[4]*1e-7; + erp->data[erp->n].xpr = v[12]*1e-6*AS2R; + erp->data[erp->n++].ypr = v[13]*1e-6*AS2R; + } + fclose(fp); + return 1; +} + + + +/* get earth rotation parameter values ----------------------------------------- + * get earth rotation parameter values + * args : erp_t *erp I earth rotation parameters + * gtime_t time I time (gpst) + * double *erpv O erp values {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * return : status (1:ok,0:error) + *-----------------------------------------------------------------------------*/ +int geterp(const erp_t *erp, gtime_t time, double *erpv) +{ + const double ep[] = {2000, 1, 1, 12, 0, 0}; + double mjd, day, a; + int i, j, k; + + trace(4, "geterp:\n"); + + if (erp->n <= 0) return 0; + + mjd = 51544.5+(timediff(gpst2utc(time), epoch2time(ep)))/86400.0; + + if (mjd <= erp->data[0].mjd) + { + day = mjd-erp->data[0].mjd; + erpv[0] = erp->data[0].xp +erp->data[0].xpr*day; + erpv[1] = erp->data[0].yp +erp->data[0].ypr*day; + erpv[2] = erp->data[0].ut1_utc-erp->data[0].lod*day; + erpv[3] = erp->data[0].lod; + return 1; + } + if (mjd >= erp->data[erp->n-1].mjd) + { + day = mjd-erp->data[erp->n-1].mjd; + erpv[0] = erp->data[erp->n-1].xp +erp->data[erp->n-1].xpr*day; + erpv[1] = erp->data[erp->n-1].yp +erp->data[erp->n-1].ypr*day; + erpv[2] = erp->data[erp->n-1].ut1_utc-erp->data[erp->n-1].lod*day; + erpv[3] = erp->data[erp->n-1].lod; + return 1; + } + for (j = 0, k = erp->n-1; jdata[i].mjd) k = i; else j = i; + } + if (erp->data[j].mjd == erp->data[j+1].mjd) + { + a = 0.5; + } + else + { + a = (mjd-erp->data[j].mjd)/(erp->data[j+1].mjd-erp->data[j].mjd); + } + erpv[0] = (1.0-a)*erp->data[j].xp +a*erp->data[j+1].xp; + erpv[1] = (1.0-a)*erp->data[j].yp +a*erp->data[j+1].yp; + erpv[2] = (1.0-a)*erp->data[j].ut1_utc+a*erp->data[j+1].ut1_utc; + erpv[3] = (1.0-a)*erp->data[j].lod +a*erp->data[j+1].lod; + return 1; +} + +/* compare ephemeris ---------------------------------------------------------*/ +int cmpeph(const void *p1, const void *p2) +{ + eph_t *q1 = (eph_t *)p1, *q2 = (eph_t *)p2; + return q1->ttr.time != q2->ttr.time ? (int)(q1->ttr.time-q2->ttr.time) : + (q1->toe.time != q2->toe.time ? (int)(q1->toe.time-q2->toe.time) : + q1->sat-q2->sat); +} + + +/* sort and unique ephemeris -------------------------------------------------*/ +void uniqeph(nav_t *nav) +{ + eph_t *nav_eph; + int i,j; + + trace(3, "uniqeph: n=%d\n", nav->n); + + if (nav->n <= 0) return; + + qsort(nav->eph, nav->n, sizeof(eph_t), cmpeph); + + for (i = 1, j = 0; in; i++) + { + if (nav->eph[i].sat != nav->eph[j].sat || + nav->eph[i].iode != nav->eph[j].iode) + { + nav->eph[++j] = nav->eph[i]; + } + } + nav->n = j+1; + + if (!(nav_eph = (eph_t *)realloc(nav->eph, sizeof(eph_t)*nav->n))) + { + trace(1, "uniqeph malloc error n=%d\n", nav->n); + free(nav->eph); nav->eph = NULL; nav->n = nav->nmax = 0; + return; + } + nav->eph = nav_eph; + nav->nmax = nav->n; + + trace(4, "uniqeph: n=%d\n", nav->n); +} + + +/* compare glonass ephemeris -------------------------------------------------*/ +int cmpgeph(const void *p1, const void *p2) +{ + geph_t *q1 = (geph_t *)p1, *q2 = (geph_t *)p2; + return q1->tof.time != q2->tof.time ? (int)(q1->tof.time-q2->tof.time) : + (q1->toe.time != q2->toe.time ? (int)(q1->toe.time-q2->toe.time) : + q1->sat-q2->sat); +} + + +/* sort and unique glonass ephemeris -----------------------------------------*/ +void uniqgeph(nav_t *nav) +{ + geph_t *nav_geph; + int i, j; + + trace(3, "uniqgeph: ng=%d\n", nav->ng); + + if (nav->ng <= 0) return; + + qsort(nav->geph, nav->ng, sizeof(geph_t), cmpgeph); + + for (i = j = 0; ing; i++) + { + if (nav->geph[i].sat != nav->geph[j].sat || + nav->geph[i].toe.time != nav->geph[j].toe.time || + nav->geph[i].svh != nav->geph[j].svh) + { + nav->geph[++j] = nav->geph[i]; + } + } + nav->ng = j+1; + + if (!(nav_geph = (geph_t *)realloc(nav->geph, sizeof(geph_t)*nav->ng))) + { + trace(1, "uniqgeph malloc error ng=%d\n", nav->ng); + free(nav->geph); nav->geph = NULL; nav->ng = nav->ngmax = 0; + return; + } + nav->geph = nav_geph; + nav->ngmax = nav->ng; + + trace(4, "uniqgeph: ng=%d\n", nav->ng); +} + + +/* compare sbas ephemeris ----------------------------------------------------*/ +int cmpseph(const void *p1, const void *p2) +{ + seph_t *q1 = (seph_t *)p1, *q2 = (seph_t *)p2; + return q1->tof.time != q2->tof.time ? (int)(q1->tof.time-q2->tof.time) : + (q1->t0.time != q2->t0.time ? (int)(q1->t0.time-q2->t0.time) : + q1->sat-q2->sat); +} + + +/* sort and unique sbas ephemeris --------------------------------------------*/ +void uniqseph(nav_t *nav) +{ + seph_t *nav_seph; + int i, j; + + trace(3, "uniqseph: ns=%d\n", nav->ns); + + if (nav->ns <= 0) return; + + qsort(nav->seph, nav->ns, sizeof(seph_t), cmpseph); + + for (i = j = 0; ins; i++) + { + if (nav->seph[i].sat != nav->seph[j].sat || + nav->seph[i].t0.time != nav->seph[j].t0.time) + { + nav->seph[++j] = nav->seph[i]; + } + } + nav->ns = j+1; + + if (!(nav_seph = (seph_t *)realloc(nav->seph, sizeof(seph_t)*nav->ns))) + { + trace(1, "uniqseph malloc error ns=%d\n", nav->ns); + free(nav->seph); nav->seph = NULL; nav->ns = nav->nsmax = 0; + return; + } + nav->seph = nav_seph; + nav->nsmax = nav->ns; + + trace(4, "uniqseph: ns=%d\n", nav->ns); +} + + +/* unique ephemerides ---------------------------------------------------------- + * unique ephemerides in navigation data and update carrier wave length + * args : nav_t *nav IO navigation data + * return : number of epochs + *-----------------------------------------------------------------------------*/ +void uniqnav(nav_t *nav) +{ + int i, j; + + trace(3, "uniqnav: neph=%d ngeph=%d nseph=%d\n", nav->n, nav->ng, nav->ns); + + /* unique ephemeris */ + uniqeph (nav); + uniqgeph(nav); + uniqseph(nav); + + /* update carrier wave length */ + for (i = 0; ilam[i][j] = satwavelen(i+1, j, nav); + } +} + + +/* compare observation data -------------------------------------------------*/ +int cmpobs(const void *p1, const void *p2) +{ + obsd_t *q1 = (obsd_t *)p1, *q2 = (obsd_t *)p2; + double tt = timediff(q1->time, q2->time); + if (fabs(tt)>DTTOL) return tt<0 ? -1 : 1; + if (q1->rcv != q2->rcv) return (int)q1->rcv-(int)q2->rcv; + return (int)q1->sat-(int)q2->sat; +} + + +/* sort and unique observation data -------------------------------------------- + * sort and unique observation data by time, rcv, sat + * args : obs_t *obs IO observation data + * return : number of epochs + *-----------------------------------------------------------------------------*/ +int sortobs(obs_t *obs) +{ + int i, j, n; + + trace(3, "sortobs: nobs=%d\n", obs->n); + + if (obs->n <= 0) return 0; + + qsort(obs->data, obs->n, sizeof(obsd_t), cmpobs); + + /* delete duplicated data */ + for (i = j = 0; in; i++) + { + if (obs->data[i].sat != obs->data[j].sat || + obs->data[i].rcv != obs->data[j].rcv || + timediff(obs->data[i].time, obs->data[j].time) != 0.0) + { + obs->data[++j] = obs->data[i]; + } + } + obs->n = j+1; + + for (i = n = 0; in; i = j, n++) + { + for (j = i+1; jn; j++) + { + if (timediff(obs->data[j].time, obs->data[i].time)>DTTOL) break; + } + } + return n; +} + + +/* screen by time -------------------------------------------------------------- + * screening by time start, time end, and time interval + * args : gtime_t time I time + * gtime_t ts I time start (ts.time==0:no screening by ts) + * gtime_t te I time end (te.time==0:no screening by te) + * double tint I time interval (s) (0.0:no screen by tint) + * return : 1:on condition, 0:not on condition + *-----------------------------------------------------------------------------*/ +int screent(gtime_t time, gtime_t ts, gtime_t te, double tint) +{ + return (tint <= 0.0 || fmod(time2gpst(time, NULL)+DTTOL, tint) <= DTTOL*2.0) && + (ts.time == 0 || timediff(time, ts) >= -DTTOL) && + (te.time == 0 || timediff(time, te)< DTTOL); +} + + +/* read/save navigation data --------------------------------------------------- + * save or load navigation data + * args : char file I file path + * nav_t nav O/I navigation data + * return : status (1:ok,0:no file) + *-----------------------------------------------------------------------------*/ +int readnav(const char *file, nav_t *nav) +{ + FILE *fp; + eph_t eph0 = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, 0.0, 0.0 }; + geph_t geph0 = {0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {}, {}, {}, 0.0, 0.0, 0.0}; + char buff[4096], *p; + long toe_time, tof_time, toc_time, ttr_time; + int i, sat, prn; + + trace(3, "loadnav: file=%s\n", file); + + if (!(fp = fopen(file, "r"))) return 0; + + while (fgets(buff, sizeof(buff), fp)) + { + if (!strncmp(buff, "IONUTC", 6)) + { + for (i = 0; i<8; i++) nav->ion_gps[i] = 0.0; + for (i = 0; i<4; i++) nav->utc_gps[i] = 0.0; + nav->leaps = 0; + sscanf(buff, "IONUTC,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d", + &nav->ion_gps[0], &nav->ion_gps[1], &nav->ion_gps[2], &nav->ion_gps[3], + &nav->ion_gps[4], &nav->ion_gps[5], &nav->ion_gps[6], &nav->ion_gps[7], + &nav->utc_gps[0], &nav->utc_gps[1], &nav->utc_gps[2], &nav->utc_gps[3], + &nav->leaps); + continue; + } + if ((p = strchr(buff, ','))) *p = '\0'; else continue; + if (!(sat = satid2no(buff))) continue; + if (satsys(sat, &prn) == SYS_GLO) + { + nav->geph[prn-1] = geph0; + nav->geph[prn-1].sat = sat; + toe_time = tof_time = 0; + sscanf(p+1, "%d,%d,%d,%d,%d,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + "%lf,%lf,%lf,%lf", + &nav->geph[prn-1].iode, &nav->geph[prn-1].frq, &nav->geph[prn-1].svh, + &nav->geph[prn-1].sva, &nav->geph[prn-1].age, + &toe_time, &tof_time, + &nav->geph[prn-1].pos[0], &nav->geph[prn-1].pos[1], &nav->geph[prn-1].pos[2], + &nav->geph[prn-1].vel[0], &nav->geph[prn-1].vel[1], &nav->geph[prn-1].vel[2], + &nav->geph[prn-1].acc[0], &nav->geph[prn-1].acc[1], &nav->geph[prn-1].acc[2], + &nav->geph[prn-1].taun , &nav->geph[prn-1].gamn , &nav->geph[prn-1].dtaun); + nav->geph[prn-1].toe.time = toe_time; + nav->geph[prn-1].tof.time = tof_time; + } + else + { + nav->eph[sat-1] = eph0; + nav->eph[sat-1].sat = sat; + toe_time = toc_time = ttr_time = 0; + sscanf(p+1, "%d,%d,%d,%d,%ld,%ld,%ld,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf," + "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%d,%d", + &nav->eph[sat-1].iode, &nav->eph[sat-1].iodc, &nav->eph[sat-1].sva , + &nav->eph[sat-1].svh , + &toe_time, &toc_time, &ttr_time, + &nav->eph[sat-1].A , &nav->eph[sat-1].e , &nav->eph[sat-1].i0 , + &nav->eph[sat-1].OMG0, &nav->eph[sat-1].omg , &nav->eph[sat-1].M0 , + &nav->eph[sat-1].deln, &nav->eph[sat-1].OMGd, &nav->eph[sat-1].idot, + &nav->eph[sat-1].crc , &nav->eph[sat-1].crs , &nav->eph[sat-1].cuc , + &nav->eph[sat-1].cus , &nav->eph[sat-1].cic , &nav->eph[sat-1].cis , + &nav->eph[sat-1].toes, &nav->eph[sat-1].fit , &nav->eph[sat-1].f0 , + &nav->eph[sat-1].f1 , &nav->eph[sat-1].f2 , &nav->eph[sat-1].tgd[0], + &nav->eph[sat-1].code, &nav->eph[sat-1].flag); + nav->eph[sat-1].toe.time = toe_time; + nav->eph[sat-1].toc.time = toc_time; + nav->eph[sat-1].ttr.time = ttr_time; + } + } + fclose(fp); + return 1; +} + + +int savenav(const char *file, const nav_t *nav) +{ + FILE *fp; + int i; + char id[32]; + + trace(3, "savenav: file=%s\n", file); + + if (!(fp = fopen(file, "w"))) return 0; + + for (i = 0; ieph[i].ttr.time == 0) continue; + satno2id(nav->eph[i].sat, id); + fprintf(fp, "%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%d,%d\n", + id, nav->eph[i].iode, nav->eph[i].iodc, nav->eph[i].sva , + nav->eph[i].svh , (int)nav->eph[i].toe.time, + (int)nav->eph[i].toc.time, (int)nav->eph[i].ttr.time, + nav->eph[i].A , nav->eph[i].e , nav->eph[i].i0 , nav->eph[i].OMG0, + nav->eph[i].omg , nav->eph[i].M0 , nav->eph[i].deln, nav->eph[i].OMGd, + nav->eph[i].idot, nav->eph[i].crc, nav->eph[i].crs , nav->eph[i].cuc , + nav->eph[i].cus , nav->eph[i].cic, nav->eph[i].cis , nav->eph[i].toes, + nav->eph[i].fit , nav->eph[i].f0 , nav->eph[i].f1 , nav->eph[i].f2 , + nav->eph[i].tgd[0], nav->eph[i].code, nav->eph[i].flag); + } + for (i = 0; igeph[i].tof.time == 0) continue; + satno2id(nav->geph[i].sat, id); + fprintf(fp, "%s,%d,%d,%d,%d,%d,%d,%d,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%.14E,%.14E,%.14E\n", + id, nav->geph[i].iode, nav->geph[i].frq, nav->geph[i].svh, + nav->geph[i].sva, nav->geph[i].age, (int)nav->geph[i].toe.time, + (int)nav->geph[i].tof.time, + nav->geph[i].pos[0], nav->geph[i].pos[1], nav->geph[i].pos[2], + nav->geph[i].vel[0], nav->geph[i].vel[1], nav->geph[i].vel[2], + nav->geph[i].acc[0], nav->geph[i].acc[1], nav->geph[i].acc[2], + nav->geph[i].taun, nav->geph[i].gamn, nav->geph[i].dtaun); + } + fprintf(fp,"IONUTC,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E,%.14E," + "%.14E,%.14E,%.14E,%d", + nav->ion_gps[0], nav->ion_gps[1], nav->ion_gps[2], nav->ion_gps[3], + nav->ion_gps[4], nav->ion_gps[5], nav->ion_gps[6], nav->ion_gps[7], + nav->utc_gps[0], nav->utc_gps[1], nav->utc_gps[2], nav->utc_gps[3], + nav->leaps); + + fclose(fp); + return 1; +} + + +/* free observation data ------------------------------------------------------- + * free memory for observation data + * args : obs_t *obs IO observation data + * return : none + *-----------------------------------------------------------------------------*/ +void freeobs(obs_t *obs) +{ + free(obs->data); obs->data = NULL; obs->n = obs->nmax = 0; +} + + +/* free navigation data --------------------------------------------------------- + * free memory for navigation data + * args : nav_t *nav IO navigation data + * int opt I option (or of followings) + * (0x01: gps/qzs ephmeris, 0x02: glonass ephemeris, + * 0x04: sbas ephemeris, 0x08: precise ephemeris, + * 0x10: precise clock 0x20: almanac, + * 0x40: tec data) + * return : none + *-----------------------------------------------------------------------------*/ +void freenav(nav_t *nav, int opt) +{ + if (opt&0x01) {free(nav->eph ); nav->eph = NULL; nav->n = nav->nmax = 0;} + if (opt&0x02) {free(nav->geph); nav->geph = NULL; nav->ng = nav->ngmax = 0;} + if (opt&0x04) {free(nav->seph); nav->seph = NULL; nav->ns = nav->nsmax = 0;} + if (opt&0x08) {free(nav->peph); nav->peph = NULL; nav->ne = nav->nemax = 0;} + if (opt&0x10) {free(nav->pclk); nav->pclk = NULL; nav->nc = nav->ncmax = 0;} + if (opt&0x20) {free(nav->alm ); nav->alm = NULL; nav->na = nav->namax = 0;} + if (opt&0x40) {free(nav->tec ); nav->tec = NULL; nav->nt = nav->ntmax = 0;} + if (opt&0x80) {free(nav->fcb ); nav->fcb = NULL; nav->nf = nav->nfmax = 0;} +} + + +/* debug trace functions -----------------------------------------------------*/ +//#ifdef TRACE +// +//static FILE *fp_trace=NULL; /* file pointer of trace */ +//static char file_trace[1024]; /* trace file */ +//static int level_trace=0; /* level of trace */ +//static unsigned int tick_trace=0; /* tick time at traceopen (ms) */ +//static gtime_t time_trace={0}; /* time at traceopen */ +//static pthread_mutex_t lock_trace; /* lock for trace */ +// +//static void traceswap(void) +//{ +// gtime_t time=utc2gpst(timeget()); +// char path[1024]; +// +// lock(&lock_trace); +// +// if ((int)(time2gpst(time ,NULL)/INT_SWAP_TRAC)== +// (int)(time2gpst(time_trace,NULL)/INT_SWAP_TRAC)) { +// unlock(&lock_trace); +// return; +// } +// time_trace=time; +// +// if (!reppath(file_trace,path,time,"","")) { +// unlock(&lock_trace); +// return; +// } +// if (fp_trace) fclose(fp_trace); +// +// if (!(fp_trace=fopen(path,"w"))) { +// fp_trace=stderr; +// } +// unlock(&lock_trace); +//} +//extern void traceopen(const char *file) +//{ +// gtime_t time=utc2gpst(timeget()); +// char path[1024]; +// +// reppath(file,path,time,"",""); +// if (!*path||!(fp_trace=fopen(path,"w"))) fp_trace=stderr; +// strcpy(file_trace,file); +// tick_trace=tickget(); +// time_trace=time; +// initlock(&lock_trace); +//} +//extern void traceclose(void) +//{ +// if (fp_trace&&fp_trace!=stderr) fclose(fp_trace); +// fp_trace=NULL; +// file_trace[0]='\0'; +//} +//extern void tracelevel(int level) +//{ +// level_trace=level; +//} +//extern void trace(int level, const char *format, ...) +//{ +// va_list ap; +// +// /* print error message to stderr */ +// if (level <= 1) { +// va_start(ap,format); vfprintf(stderr,format,ap); va_end(ap); +// } +// if (!fp_trace||level>level_trace) return; +// traceswap(); +// fprintf(fp_trace,"%d ",level); +// va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap); +// fflush(fp_trace); +//} +//extern void tracet(int level, const char *format, ...) +//{ +// va_list ap; +// +// if (!fp_trace||level>level_trace) return; +// traceswap(); +// fprintf(fp_trace,"%d %9.3f: ",level,(tickget()-tick_trace)/1000.0); +// va_start(ap,format); vfprintf(fp_trace,format,ap); va_end(ap); +// fflush(fp_trace); +//} +void tracemat(int level __attribute__((unused)), const double *A __attribute__((unused)), +int n __attribute__((unused)), int m __attribute__((unused)), int p __attribute__((unused)), +int q __attribute__((unused))) +{ +// if (!fp_trace||level>level_trace) return; +// matfprint(A,n,m,p,q,fp_trace); fflush(fp_trace); +} + + +void traceobs(int level __attribute__((unused)), const obsd_t *obs __attribute__((unused)), int n __attribute__((unused))) +{ +// char str[64],id[16]; +// int i; +// +// if (!fp_trace||level>level_trace) return; +// for (i=0;ilevel_trace) return; +// for (i=0;in;i++) { +// time2str(nav->eph[i].toe,s1,0); +// time2str(nav->eph[i].ttr,s2,0); +// satno2id(nav->eph[i].sat,id); +// fprintf(fp_trace,"(%3d) %-3s : %s %s %3d %3d %02x\n",i+1, +// id,s1,s2,nav->eph[i].iode,nav->eph[i].iodc,nav->eph[i].svh); +// } +// fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gps[0], +// nav->ion_gps[1],nav->ion_gps[2],nav->ion_gps[3]); +// fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gps[4], +// nav->ion_gps[5],nav->ion_gps[6],nav->ion_gps[7]); +// fprintf(fp_trace,"(ion) %9.4e %9.4e %9.4e %9.4e\n",nav->ion_gal[0], +// nav->ion_gal[1],nav->ion_gal[2],nav->ion_gal[3]); +//} +//extern void tracegnav(int level, const nav_t *nav) +//{ +// char s1[64],s2[64],id[16]; +// int i; +// +// if (!fp_trace||level>level_trace) return; +// for (i=0;ing;i++) { +// time2str(nav->geph[i].toe,s1,0); +// time2str(nav->geph[i].tof,s2,0); +// satno2id(nav->geph[i].sat,id); +// fprintf(fp_trace,"(%3d) %-3s : %s %s %2d %2d %8.3f\n",i+1, +// id,s1,s2,nav->geph[i].frq,nav->geph[i].svh,nav->geph[i].taun*1e6); +// } +//} +//extern void tracehnav(int level, const nav_t *nav) +//{ +// char s1[64],s2[64],id[16]; +// int i; +// +// if (!fp_trace||level>level_trace) return; +// for (i=0;ins;i++) { +// time2str(nav->seph[i].t0,s1,0); +// time2str(nav->seph[i].tof,s2,0); +// satno2id(nav->seph[i].sat,id); +// fprintf(fp_trace,"(%3d) %-3s : %s %s %2d %2d\n",i+1, +// id,s1,s2,nav->seph[i].svh,nav->seph[i].sva); +// } +//} +//extern void tracepeph(int level, const nav_t *nav) +//{ +// char s[64],id[16]; +// int i,j; +// +// if (!fp_trace||level>level_trace) return; +// +// for (i=0;ine;i++) { +// time2str(nav->peph[i].time,s,0); +// for (j=0;jpeph[i].index,id, +// nav->peph[i].pos[j][0],nav->peph[i].pos[j][1], +// nav->peph[i].pos[j][2],nav->peph[i].pos[j][3]*1e9, +// nav->peph[i].std[j][0],nav->peph[i].std[j][1], +// nav->peph[i].std[j][2],nav->peph[i].std[j][3]*1e9); +// } +// } +//} +//extern void tracepclk(int level, const nav_t *nav) +//{ +// char s[64],id[16]; +// int i,j; +// +// if (!fp_trace||level>level_trace) return; +// +// for (i=0;inc;i++) { +// time2str(nav->pclk[i].time,s,0); +// for (j=0;jpclk[i].index,id, +// nav->pclk[i].clk[j][0]*1e9,nav->pclk[i].std[j][0]*1e9); +// } +// } +//} +//extern void traceb(int level, const unsigned char *p, int n) +//{ +// int i; +// if (!fp_trace||level>level_trace) return; +// for (i=0;i:error) + *-----------------------------------------------------------------------------*/ +int execcmd(const char *cmd) +{ + trace(3, "execcmd: cmd=%s\n", cmd); + return system(cmd); +} + + +/* create directory ------------------------------------------------------------ + * create directory if not exist + * args : char *path I file path to be saved + * return : none + * notes : not recursive. only one level + *-----------------------------------------------------------------------------*/ +void createdir(const char *path) +{ + char buff[1024], *p; + //tracet(3, "createdir: path=%s\n", path); + + strcpy(buff, path); + if (!(p = strrchr(buff, FILEPATHSEP))) return; + *p = '\0'; + + mkdir(buff, 0777); +} + + +/* replace string ------------------------------------------------------------*/ +int repstr(char *str, const char *pat, const char *rep) +{ + int len = (int)strlen(pat); + char buff[1024], *p, *q, *r; + + for (p = str, r = buff; *p; p = q+len) + { + if (!(q = strstr(p, pat))) break; + strncpy(r, p, q-p); + r += q-p; + r += sprintf(r, "%s", rep); + } + if (p <= str) return 0; + strcpy(r, p); + strcpy(str, buff); + return 1; +} + + +/* replace keywords in file path ----------------------------------------------- + * replace keywords in file path with date, time, rover and base station id + * args : char *path I file path (see below) + * char *rpath O file path in which keywords replaced (see below) + * gtime_t time I time (gpst) (time.time==0: not replaced) + * char *rov I rover id string ("": not replaced) + * char *base I base station id string ("": not replaced) + * return : status (1:keywords replaced, 0:no valid keyword in the path, + * -1:no valid time) + * notes : the following keywords in path are replaced by date, time and name + * %Y -> yyyy : year (4 digits) (1900-2099) + * %y -> yy : year (2 digits) (00-99) + * %m -> mm : month (01-12) + * %d -> dd : day of month (01-31) + * %h -> hh : hours (00-23) + * %M -> mm : minutes (00-59) + * %S -> ss : seconds (00-59) + * %n -> ddd : day of year (001-366) + * %W -> wwww : gps week (0001-9999) + * %D -> d : day of gps week (0-6) + * %H -> h : hour code (a=0,b=1,c=2,...,x=23) + * %ha-> hh : 3 hours (00,03,06,...,21) + * %hb-> hh : 6 hours (00,06,12,18) + * %hc-> hh : 12 hours (00,12) + * %t -> mm : 15 minutes (00,15,30,45) + * %r -> rrrr : rover id + * %b -> bbbb : base station id + *-----------------------------------------------------------------------------*/ +int reppath(const char *path, char *rpath, gtime_t time, const char *rov, + const char *base) +{ + double ep[6], ep0[6] = {2000, 1, 1, 0, 0, 0}; + int week, dow, doy, stat = 0; + char rep[64]; + + strcpy(rpath, path); + + if (!strstr(rpath, "%")) return 0; + if (*rov ) stat|=repstr(rpath, "%r", rov ); + if (*base) stat|=repstr(rpath, "%b", base); + if (time.time != 0) + { + time2epoch(time, ep); + ep0[0] = ep[0]; + dow = (int)floor(time2gpst(time, &week)/86400.0); + doy = (int)floor(timediff(time, epoch2time(ep0))/86400.0)+1; + sprintf(rep, "%02d", ((int)ep[3]/3)*3); stat|=repstr(rpath, "%ha", rep); + sprintf(rep, "%02d", ((int)ep[3]/6)*6); stat|=repstr(rpath, "%hb", rep); + sprintf(rep, "%02d", ((int)ep[3]/12)*12); stat|=repstr(rpath, "%hc", rep); + sprintf(rep, "%04.0f", ep[0]); stat|=repstr(rpath, "%Y", rep); + sprintf(rep, "%02.0f", fmod(ep[0], 100.0)); stat|=repstr(rpath, "%y", rep); + sprintf(rep, "%02.0f", ep[1]); stat|=repstr(rpath, "%m", rep); + sprintf(rep, "%02.0f", ep[2]); stat|=repstr(rpath, "%d", rep); + sprintf(rep, "%02.0f", ep[3]); stat|=repstr(rpath, "%h", rep); + sprintf(rep, "%02.0f", ep[4]); stat|=repstr(rpath, "%M", rep); + sprintf(rep, "%02.0f", floor(ep[5])); stat|=repstr(rpath, "%S", rep); + sprintf(rep, "%03d", doy); stat|=repstr(rpath, "%n", rep); + sprintf(rep, "%04d", week); stat|=repstr(rpath, "%W", rep); + sprintf(rep, "%d", dow); stat|=repstr(rpath, "%D", rep); + sprintf(rep, "%c", 'a'+(int)ep[3]); stat|=repstr(rpath, "%H", rep); + sprintf(rep, "%02d", ((int)ep[4]/15)*15); stat|=repstr(rpath, "%t", rep); + } + else if (strstr(rpath, "%ha") || strstr(rpath, "%hb") || strstr(rpath, "%hc") || + strstr(rpath, "%Y" ) || strstr(rpath, "%y" ) || strstr(rpath, "%m" ) || + strstr(rpath, "%d" ) || strstr(rpath, "%h" ) || strstr(rpath, "%M" ) || + strstr(rpath, "%S" ) || strstr(rpath, "%n" ) || strstr(rpath, "%W" ) || + strstr(rpath, "%D" ) || strstr(rpath, "%H" ) || strstr(rpath, "%t" )) + { + return -1; /* no valid time */ + } + return stat; +} + + +/* replace keywords in file path and generate multiple paths ------------------- + * replace keywords in file path with date, time, rover and base station id + * generate multiple keywords-replaced paths + * args : char *path I file path (see below) + * char *rpath[] O file paths in which keywords replaced + * int nmax I max number of output file paths + * gtime_t ts I time start (gpst) + * gtime_t te I time end (gpst) + * char *rov I rover id string ("": not replaced) + * char *base I base station id string ("": not replaced) + * return : number of replaced file paths + * notes : see reppath() for replacements of keywords. + * minimum interval of time replaced is 900s. + *-----------------------------------------------------------------------------*/ +int reppaths(const char *path, char *rpath[], int nmax, gtime_t ts, + gtime_t te, const char *rov, const char *base) +{ + gtime_t time; + double tow, tint = 86400.0; + int i, n = 0, week; + + trace(3, "reppaths: path =%s nmax=%d rov=%s base=%s\n", path, nmax, rov, base); + + if (ts.time == 0 || te.time == 0 || timediff(ts, te)>0.0) return 0; + + if (strstr(path, "%S") || strstr(path, "%M") || strstr(path, "%t")) tint = 900.0; + else if (strstr(path, "%h") || strstr(path, "%H")) tint = 3600.0; + + tow = time2gpst(ts, &week); + time = gpst2time(week, floor(tow/tint)*tint); + + while (timediff(time, te) <= 0.0 && nng; i++) + { + if (nav->geph[i].sat != sat) continue; + return SPEED_OF_LIGHT/(freq_glo[frq]+dfrq_glo[frq]*nav->geph[i].frq); + } + } + else if (frq == 2) + { /* L3 */ + return SPEED_OF_LIGHT/FREQ3_GLO; + } + } + else if (sys == SYS_BDS) + { + if (frq == 0) return SPEED_OF_LIGHT/FREQ1_BDS; /* B1 */ + else if (frq == 1) return SPEED_OF_LIGHT/FREQ2_BDS; /* B2 */ + else if (frq == 2) return SPEED_OF_LIGHT/FREQ3_BDS; /* B3 */ + } + else + { + if (frq == 0) return SPEED_OF_LIGHT/FREQ1; /* L1/E1 */ + else if (frq == 1) return SPEED_OF_LIGHT/FREQ2; /* L2 */ + else if (frq == 2) return SPEED_OF_LIGHT/FREQ5; /* L5/E5a */ + else if (frq == 3) return SPEED_OF_LIGHT/FREQ6; /* L6/LEX */ + else if (frq == 4) return SPEED_OF_LIGHT/FREQ7; /* E5b */ + else if (frq == 5) return SPEED_OF_LIGHT/FREQ8; /* E5a+b */ + else if (frq == 6) return SPEED_OF_LIGHT/FREQ9; /* S */ + } + return 0.0; +} + + +/* geometric distance ---------------------------------------------------------- + * compute geometric distance and receiver-to-satellite unit vector + * args : double *rs I satellilte position (ecef at transmission) (m) + * double *rr I receiver position (ecef at reception) (m) + * double *e O line-of-sight vector (ecef) + * return : geometric distance (m) (0>:error/no satellite position) + * notes : distance includes sagnac effect correction + *-----------------------------------------------------------------------------*/ +double geodist(const double *rs, const double *rr, double *e) +{ + double r; + int i; + + if (norm_rtk(rs, 3)-RE_WGS84) + { + ecef2enu(pos, e, enu); + az = dot(enu, enu, 2)<1e-12 ? 0.0 : atan2(enu[0], enu[1]); + if (az<0.0) az += 2*PI; + el = asin(enu[2]); + } + if (azel) {azel[0] = az; azel[1] = el;} + return el; +} + + +/* compute dops ---------------------------------------------------------------- + * compute DOP (dilution of precision) + * args : int ns I number of satellites + * double *azel I satellite azimuth/elevation angle (rad) + * double elmin I elevation cutoff angle (rad) + * double *dop O DOPs {GDOP,PDOP,HDOP,VDOP} + * return : none + * notes : dop[0]-[3] return 0 in case of dop computation error + *-----------------------------------------------------------------------------*/ +void dops(int ns, const double *azel, double elmin, double *dop) +{ + double H[4*MAXSAT], Q[16], cosel, sinel; + int i, n; + + for (i = 0; i<4; i++) dop[i] = 0.0; + for (i = n = 0; i 0.416) phi = 0.416; + else if (phi<-0.416) phi = -0.416; + lam = pos[1]/PI+psi*sin(azel[0])/cos(phi*PI); + + /* geomagnetic latitude (semi-circle) */ + phi += 0.064*cos((lam-1.617)*PI); + + /* local time (s) */ + tt = 43200.0*lam+time2gpst(t, &week); + tt -= floor(tt/86400.0)*86400.0; /* 0 <= tt<86400 */ + + /* slant factor */ + f = 1.0+16.0*pow(0.53-azel[1]/PI, 3.0); + + /* ionospheric delay */ + amp = ion[0]+phi*(ion[1]+phi*(ion[2]+phi*ion[3])); + per = ion[4]+phi*(ion[5]+phi*(ion[6]+phi*ion[7])); + amp = amp< 0.0 ? 0.0 : amp; + per = per<72000.0 ? 72000.0 : per; + x = 2.0*PI*(tt-50400.0)/per; + + return SPEED_OF_LIGHT*f*(fabs(x)<1.57 ? 5E-9+amp*(1.0+x*x*(-0.5+x*x/24.0)) : 5E-9); +} + + +/* ionosphere mapping function ------------------------------------------------- + * compute ionospheric delay mapping function by single layer model + * args : double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * return : ionospheric mapping function + *-----------------------------------------------------------------------------*/ +double ionmapf(const double *pos, const double *azel) +{ + if (pos[2] >= HION) return 1.0; + return 1.0/cos(asin((RE_WGS84+pos[2])/(RE_WGS84+HION)*sin(PI/2.0-azel[1]))); +} + + +/* ionospheric pierce point position ------------------------------------------- + * compute ionospheric pierce point (ipp) position and slant factor + * args : double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double re I earth radius (km) + * double hion I altitude of ionosphere (km) + * double *posp O pierce point position {lat,lon,h} (rad,m) + * return : slant factor + * notes : see ref [2], only valid on the earth surface + * fixing bug on ref [2] A.4.4.10.1 A-22,23 + *-----------------------------------------------------------------------------*/ +double ionppp(const double *pos, const double *azel, double re, + double hion, double *posp) +{ + double cosaz, rp, ap, sinap, tanap; + + rp = re/(re+hion)*cos(azel[1]); + ap = PI/2.0-azel[1]-asin(rp); + sinap = sin(ap); + tanap = tan(ap); + cosaz = cos(azel[0]); + posp[0] = asin(sin(pos[0])*cos(ap)+cos(pos[0])*sinap*cosaz); + + if ((pos[0]> 70.0*D2R && tanap*cosaz>tan(PI/2.0-pos[0])) || + (pos[0]<-70.0*D2R && -tanap*cosaz>tan(PI/2.0+pos[0]))) + { + posp[1] = pos[1]+PI-asin(sinap*sin(azel[0])/cos(posp[0])); + } + else + { + posp[1] = pos[1]+asin(sinap*sin(azel[0])/cos(posp[0])); + } + return 1.0/sqrt(1.0-rp*rp); +} + + +/* troposphere model ----------------------------------------------------------- + * compute tropospheric delay by standard atmosphere and saastamoinen model + * args : gtime_t time I time + * double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double humi I relative humidity + * return : tropospheric delay (m) + *-----------------------------------------------------------------------------*/ +double tropmodel(gtime_t time __attribute__((unused)), const double *pos, const double *azel, + double humi) +{ + const double temp0 = 15.0; /* temparature at sea level */ + double hgt, pres, temp, e, z, trph, trpw; + + if (pos[2]<-100.0 || 1e44) return coef[4]; + return coef[i-1]*(1.0-lat/15.0+i)+coef[i]*(lat/15.0-i); +} + + +double mapf(double el, double a, double b, double c) +{ + double sinel = sin(el); + return (1.0+a/(1.0+b/(1.0+c)))/(sinel+(a/(sinel+b/(sinel+c)))); +} + + +double nmf(gtime_t time, const double pos[], const double azel[], + double *mapfw) +{ + /* ref [5] table 3 */ + /* hydro-ave-a,b,c, hydro-amp-a,b,c, wet-a,b,c at latitude 15,30,45,60,75 */ + const double coef[][5] = { + { 1.2769934E-3, 1.2683230E-3, 1.2465397E-3, 1.2196049E-3, 1.2045996E-3}, + { 2.9153695E-3, 2.9152299E-3, 2.9288445E-3, 2.9022565E-3, 2.9024912E-3}, + { 62.610505E-3, 62.837393E-3, 63.721774E-3, 63.824265E-3, 64.258455E-3}, + + { 0.0000000E-0, 1.2709626E-5, 2.6523662E-5, 3.4000452E-5, 4.1202191e-5}, + { 0.0000000E-0, 2.1414979E-5, 3.0160779E-5, 7.2562722E-5, 11.723375E-5}, + { 0.0000000E-0, 9.0128400E-5, 4.3497037E-5, 84.795348E-5, 170.37206E-5}, + + { 5.8021897E-4, 5.6794847E-4, 5.8118019E-4, 5.9727542E-4, 6.1641693E-4}, + { 1.4275268E-3, 1.5138625E-3, 1.4572752E-3, 1.5007428E-3, 1.7599082E-3}, + { 4.3472961e-2, 4.6729510E-2, 4.3908931e-2, 4.4626982E-2, 5.4736038E-2} + }; + const double aht[] = { 2.53E-5, 5.49E-3, 1.14E-3}; /* height correction */ + + double y, cosy, ah[3], aw[3], dm, el = azel[1], lat = pos[0]*R2D, hgt = pos[2]; + int i; + + if (el <= 0.0) + { + if (mapfw) *mapfw = 0.0; + return 0.0; + } + /* year from doy 28, added half a year for southern latitudes */ + y = (time2doy(time)-28.0)/365.25+(lat<0.0 ? 0.5 : 0.0); + + cosy = cos(2.0*PI*y); + lat = fabs(lat); + + for (i = 0; i<3; i++) + { + ah[i] = interpc(coef[i ], lat)-interpc(coef[i+3], lat)*cosy; + aw[i] = interpc(coef[i+6], lat); + } + /* ellipsoidal height is used instead of height above sea level */ + dm = (1.0/sin(el)-mapf(el, aht[0], aht[1], aht[2]))*hgt/1e3; + + if (mapfw) *mapfw = mapf(el, aw[0], aw[1], aw[2]); + + return mapf(el, ah[0], ah[1], ah[2])+dm; +} +#endif /* !IERS_MODEL */ + + +/* troposphere mapping function ------------------------------------------------ + * compute tropospheric mapping function by NMF + * args : gtime_t t I time + * double *pos I receiver position {lat,lon,h} (rad,m) + * double *azel I azimuth/elevation angle {az,el} (rad) + * double *mapfw IO wet mapping function (NULL: not output) + * return : dry mapping function + * note : see ref [5] (NMF) and [9] (GMF) + * original JGR paper of [5] has bugs in eq.(4) and (5). the corrected + * paper is obtained from: + * ftp://web.haystack.edu/pub/aen/nmf/NMF_JGR.pdf + *-----------------------------------------------------------------------------*/ +double tropmapf(gtime_t time, const double pos[], const double azel[], + double *mapfw) +{ +#ifdef IERS_MODEL + const double ep[] = {2000, 1, 1, 12, 0, 0}; + double mjd, lat, lon, hgt, zd, gmfh, gmfw; +#endif + trace(4, "tropmapf: pos=%10.6f %11.6f %6.1f azel=%5.1f %4.1f\n", + pos[0]*R2D, pos[1]*R2D, pos[2], azel[0]*R2D, azel[1]*R2D); + + if (pos[2]<-1000.0 || pos[2]>20000.0) + { + if (mapfw) *mapfw = 0.0; + return 0.0; + } +#ifdef IERS_MODEL + mjd = 51544.5+(timediff(time, epoch2time(ep)))/86400.0; + lat = pos[0]; + lon = pos[1]; + hgt = pos[2]-geoidh(pos); /* height in m (mean sea level) */ + zd = PI/2.0-azel[1]; + + /* call GMF */ + gmf_(&mjd, &lat, &lon, &hgt, &zd, &gmfh, &gmfw); + + if (mapfw) *mapfw = gmfw; + return gmfh; +#else + return nmf(time, pos, azel, mapfw); /* NMF */ +#endif +} + + +/* interpolate antenna phase center variation --------------------------------*/ +double interpvar(double ang, const double *var) +{ + double a = ang/5.0; /* ang=0-90 */ + int i = (int)a; + if (i<0) return var[0]; else if (i >= 18) return var[18]; + return var[i]*(1.0-a+i)+var[i+1]*(a-i); +} + + +/* receiver antenna model ------------------------------------------------------ + * compute antenna offset by antenna phase center parameters + * args : pcv_t *pcv I antenna phase center parameters + * double *azel I azimuth/elevation for receiver {az,el} (rad) + * int opt I option (0:only offset,1:offset+pcv) + * double *dant O range offsets for each frequency (m) + * return : none + * notes : current version does not support azimuth dependent terms + *-----------------------------------------------------------------------------*/ +void antmodel(const pcv_t *pcv, const double *del, const double *azel, + int opt, double *dant) +{ + double e[3], off[3], cosel = cos(azel[1]); + int i, j; + + trace(4, "antmodel: azel=%6.1f %4.1f opt=%d\n", azel[0]*R2D, azel[1]*R2D, opt); + + e[0] = sin(azel[0])*cosel; + e[1] = cos(azel[0])*cosel; + e[2] = sin(azel[1]); + + for (i = 0;ioff[i][j]+del[j]; + + dant[i] = -dot(off, e, 3)+(opt ? interpvar(90.0-azel[1]*R2D, pcv->var[i]) : 0.0); + } + trace(5, "antmodel: dant=%6.3f %6.3f\n", dant[0], dant[1]); +} + + +/* satellite antenna model ------------------------------------------------------ + * compute satellite antenna phase center parameters + * args : pcv_t *pcv I antenna phase center parameters + * double nadir I nadir angle for satellite (rad) + * double *dant O range offsets for each frequency (m) + * return : none + *-----------------------------------------------------------------------------*/ +void antmodel_s(const pcv_t *pcv, double nadir, double *dant) +{ + int i; + + trace(4, "antmodel_s: nadir=%6.1f\n", nadir*R2D); + + for (i = 0; ivar[i]); + } + trace(5, "antmodel_s: dant=%6.3f %6.3f\n", dant[0], dant[1]); +} + + +/* sun and moon position in eci (ref [4] 5.1.1, 5.2.1) -----------------------*/ +void sunmoonpos_eci(gtime_t tut, double *rsun, double *rmoon) +{ + const double ep2000[] = {2000, 1, 1, 12, 0, 0}; + double t, f[5], eps, Ms, ls, rs, lm, pm, rm, sine, cose, sinp, cosp, sinl, cosl; + + trace(4, "sunmoonpos_eci: tut=%s\n", time_str(tut, 3)); + + t = timediff(tut, epoch2time(ep2000))/86400.0/36525.0; + + /* astronomical arguments */ + ast_args(t, f); + + /* obliquity of the ecliptic */ + eps = 23.439291-0.0130042*t; + sine = sin(eps*D2R); cose = cos(eps*D2R); + + /* sun position in eci */ + if (rsun) + { + Ms = 357.5277233+35999.05034*t; + ls = 280.460+36000.770*t+1.914666471*sin(Ms*D2R)+0.019994643*sin(2.0*Ms*D2R); + rs = AU*(1.000140612-0.016708617*cos(Ms*D2R)-0.000139589*cos(2.0*Ms*D2R)); + sinl = sin(ls*D2R); cosl = cos(ls*D2R); + rsun[0] = rs*cosl; + rsun[1] = rs*cose*sinl; + rsun[2] = rs*sine*sinl; + + trace(5, "rsun =%.3f %.3f %.3f\n", rsun[0], rsun[1], rsun[2]); + } + /* moon position in eci */ + if (rmoon) + { + lm = 218.32+481267.883*t+6.29*sin(f[0])-1.27*sin(f[0]-2.0*f[3])+ + 0.66*sin(2.0*f[3])+0.21*sin(2.0*f[0])-0.19*sin(f[1])-0.11*sin(2.0*f[2]); + pm = 5.13*sin(f[2])+0.28*sin(f[0]+f[2])-0.28*sin(f[2]-f[0])- + 0.17*sin(f[2]-2.0*f[3]); + rm = RE_WGS84/sin((0.9508+0.0518*cos(f[0])+0.0095*cos(f[0]-2.0*f[3])+ + 0.0078*cos(2.0*f[3])+0.0028*cos(2.0*f[0]))*D2R); + sinl = sin(lm*D2R); cosl = cos(lm*D2R); + sinp = sin(pm*D2R); cosp = cos(pm*D2R); + rmoon[0] = rm*cosp*cosl; + rmoon[1] = rm*(cose*cosp*sinl-sine*sinp); + rmoon[2] = rm*(sine*cosp*sinl+cose*sinp); + + trace(5, "rmoon=%.3f %.3f %.3f\n", rmoon[0], rmoon[1], rmoon[2]); + } +} + + +/* sun and moon position ------------------------------------------------------- + * get sun and moon position in ecef + * args : gtime_t tut I time in ut1 + * double *erpv I erp value {xp,yp,ut1_utc,lod} (rad,rad,s,s/d) + * double *rsun IO sun position in ecef (m) (NULL: not output) + * double *rmoon IO moon position in ecef (m) (NULL: not output) + * double *gmst O gmst (rad) + * return : none + *-----------------------------------------------------------------------------*/ +void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, + double *rmoon, double *gmst) +{ + gtime_t tut; + double rs[3], rm[3], U[9], gmst_; + + trace(4, "sunmoonpos: tutc=%s\n", time_str(tutc, 3)); + + tut = timeadd(tutc, erpv[2]); /* utc -> ut1 */ + + /* sun and moon position in eci */ + sunmoonpos_eci(tut, rsun ? rs : NULL, rmoon ? rm : NULL); + + /* eci to ecef transformation matrix */ + eci2ecef(tutc, erpv, U, &gmst_); + + /* sun and moon postion in ecef */ + if (rsun ) matmul("NN", 3, 1, 3, 1.0, U, rs, 0.0, rsun ); + if (rmoon) matmul("NN", 3, 1, 3, 1.0, U, rm, 0.0, rmoon); + if (gmst ) *gmst = gmst_; +} + + +/* carrier smoothing ----------------------------------------------------------- + * carrier smoothing by Hatch filter + * args : obs_t *obs IO raw observation data/smoothed observation data + * int ns I smoothing window size (epochs) + * return : none + *-----------------------------------------------------------------------------*/ +void csmooth(obs_t *obs, int ns) +{ + double Ps[2][MAXSAT][NFREQ] = {}, Lp[2][MAXSAT][NFREQ] = {}, dcp; + int i, j, s, r, n[2][MAXSAT][NFREQ] = {}; + obsd_t *p; + + trace(3, "csmooth: nobs=%d,ns=%d\n", obs->n, ns); + + for (i = 0; in; i++) + { + p = &obs->data[i]; s = p->sat; r = p->rcv; + for (j = 0; jP[j] == 0.0 || p->L[j] == 0.0) continue; + if (p->LLI[j]) n[r-1][s-1][j] = 0; + if (n[r-1][s-1][j] == 0) Ps[r-1][s-1][j] = p->P[j]; + else + { + dcp = lam_carr[j]*(p->L[j]-Lp[r-1][s-1][j]); + Ps[r-1][s-1][j] = p->P[j]/ns+(Ps[r-1][s-1][j]+dcp)*(ns-1)/ns; + } + if (++n[r-1][s-1][j]P[j] = 0.0; else p->P[j] = Ps[r-1][s-1][j]; + Lp[r-1][s-1][j] = p->L[j]; + } + } +} + + +/* uncompress file ------------------------------------------------------------- + * uncompress (uncompress/unzip/uncompact hatanaka-compression/tar) file + * args : char *file I input file + * char *uncfile O uncompressed file + * return : status (-1:error,0:not compressed file,1:uncompress completed) + * note : creates uncompressed file in tempolary directory + * gzip and crx2rnx commands have to be installed in commands path + *-----------------------------------------------------------------------------*/ +int rtk_uncompress(const char *file, char *uncfile) +{ + int stat = 0; + char *p, cmd[2048] = "", tmpfile[1024] = "", buff[1024], *fname, *dir = (char*)""; + + trace(3, "rtk_uncompress: file=%s\n", file); + + strcpy(tmpfile, file); + if (!(p = strrchr(tmpfile, '.'))) return 0; + + /* uncompress by gzip */ + if (!strcmp(p, ".z" ) || !strcmp(p, ".Z" ) || + !strcmp(p, ".gz" ) || !strcmp(p, ".GZ" ) || + !strcmp(p, ".zip") || !strcmp(p, ".ZIP")) + { + strcpy(uncfile, tmpfile); uncfile[p-tmpfile] = '\0'; + sprintf(cmd, "gzip -f -d -c \"%s\" > \"%s\"", tmpfile, uncfile); + + if (execcmd(cmd)) + { + remove(uncfile); + return -1; + } + strcpy(tmpfile, uncfile); + stat = 1; + } + /* extract tar file */ + if ((p = strrchr(tmpfile, '.')) && !strcmp(p, ".tar")) + { + strcpy(uncfile, tmpfile); uncfile[p-tmpfile] = '\0'; + strcpy(buff, tmpfile); + fname = buff; + if ((p = strrchr(buff, '/'))) + { + *p = '\0'; dir = fname; fname = p+1; + } + sprintf(cmd, "tar -C \"%s\" -xf \"%s\"", dir, tmpfile); + if (execcmd(cmd)) + { + if (stat) remove(tmpfile); + return -1; + } + if (stat) remove(tmpfile); + stat = 1; + } + /* extract hatanaka-compressed file by cnx2rnx */ + else if ((p = strrchr(tmpfile, '.')) && strlen(p)>3 && (*(p+3) == 'd' || *(p+3) == 'D')) + { + + strcpy(uncfile, tmpfile); + uncfile[p-tmpfile+3] = *(p+3) == 'D' ? 'O' : 'o'; + sprintf(cmd, "crx2rnx < \"%s\" > \"%s\"", tmpfile, uncfile); + + if (execcmd(cmd)) + { + remove(uncfile); + if (stat) remove(tmpfile); + return -1; + } + if (stat) remove(tmpfile); + stat = 1; + } + trace(3, "rtk_uncompress: stat=%d\n", stat); + return stat; +} + + +/* expand file path ------------------------------------------------------------ + * expand file path with wild-card (*) in file + * args : char *path I file path to expand (captal insensitive) + * char *paths O expanded file paths + * int nmax I max number of expanded file paths + * return : number of expanded file paths + * notes : the order of expanded files is alphabetical order + *-----------------------------------------------------------------------------*/ +int expath(const char *path, char *paths[], int nmax) +{ + int i, j, n = 0; + char tmp[1024]; + struct dirent *d; + DIR *dp; + const char *file = path; + char dir[1024] = "", s1[1024], s2[1024], *p, *q, *r; + + trace(3, "expath : path=%s nmax=%d\n", path, nmax); + + //TODO: Fix invalid conversion from ‘const char*’ to ‘char*’ + //if ((p=strrchr(path,'/')) || (p=strrchr(path,'\\'))) { + // file=p+1; strncpy(dir,path,p-path+1); dir[p-path+1]='\0'; + //} + if (!(dp = opendir(*dir ? dir : "."))) return 0; + while ((d = readdir(dp))) + { + if (*(d->d_name) == '.') continue; + sprintf(s1, "^%s$", d->d_name); + sprintf(s2, "^%s$", file); + for (p = s1; *p;p++) *p = (char)tolower((int)*p); + for (p = s2; *p;p++) *p = (char)tolower((int)*p); + + for (p = s1, q = strtok_r(s2, "*", &r); q; q = strtok_r(NULL, "*", &r)) + { + if ((p = strstr(p, q))) p += strlen(q); else break; + } + if (p && nd_name); + } + closedir(dp); + /* sort paths in alphabetical order */ + for (i = 0; i0) + { + strcpy(tmp, paths[i]); + strcpy(paths[i], paths[j]); + strcpy(paths[j], tmp); + } + } + } + for (i = 0; i 1.0) cosp = 1.0; + ph = acos(cosp)/2.0/PI; + cross3(ds, dr, drs); + if (dot(ek, drs, 3) < 0.0) ph = -ph; + + *phw = ph + floor(*phw - ph + 0.5); /* in cycle */ +} diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h new file mode 100644 index 000000000..df7247020 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h @@ -0,0 +1,271 @@ +/*! + * \file rtklib_rtkcmn.h + * \brief rtklib common functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * References : + * [1] IS-GPS-200D, Navstar GPS Space Segment/Navigation User Interfaces, + * 7 March, 2006 + * [2] RTCA/DO-229C, Minimum operational performanc standards for global + * positioning system/wide area augmentation system airborne equipment, + * RTCA inc, November 28, 2001 + * [3] M.Rothacher, R.Schmid, ANTEX: The Antenna Exchange Format Version 1.4, + * 15 September, 2010 + * [4] A.Gelb ed., Applied Optimal Estimation, The M.I.T Press, 1974 + * [5] A.E.Niell, Global mapping functions for the atmosphere delay at radio + * wavelengths, Jounal of geophysical research, 1996 + * [6] W.Gurtner and L.Estey, RINEX The Receiver Independent Exchange Format + * Version 3.00, November 28, 2007 + * [7] J.Kouba, A Guide to using International GNSS Service (IGS) products, + * May 2009 + * [8] China Satellite Navigation Office, BeiDou navigation satellite system + * signal in space interface control document, open service signal B1I + * (version 1.0), Dec 2012 + * [9] J.Boehm, A.Niell, P.Tregoning and H.Shuh, Global Mapping Function + * (GMF): A new empirical mapping function base on numerical weather + * model data, Geophysical Research Letters, 33, L07304, 2006 + * [10] GLONASS/GPS/Galileo/Compass/SBAS NV08C receiver series BINR interface + * protocol specification ver.1.3, August, 2012 + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_RTKCMN_H_ +#define GNSS_SDR_RTKLIB_RTKCMN_H_ + +#include "rtklib.h" +#include + +/* coordinate rotation matrix ------------------------------------------------*/ +#define Rx(t,X) do { \ + (X)[0]=1.0; (X)[1]=(X)[2]=(X)[3]=(X)[6]=0.0; \ + (X)[4]=(X)[8]=cos(t); (X)[7]=sin(t); (X)[5]=-(X)[7]; \ +} while (0) + +#define Ry(t,X) do { \ + (X)[4]=1.0; (X)[1]=(X)[3]=(X)[5]=(X)[7]=0.0; \ + (X)[0]=(X)[8]=cos(t); (X)[2]=sin(t); (X)[6]=-(X)[2]; \ +} while (0) + +#define Rz(t,X) do { \ + (X)[8]=1.0; (X)[2]=(X)[5]=(X)[6]=(X)[7]=0.0; \ + (X)[0]=(X)[4]=cos(t); (X)[3]=sin(t); (X)[1]=-(X)[3]; \ +} while (0) + + + + +void fatalerr(const char *format, ...); +int satno(int sys, int prn); +int satsys(int sat, int *prn); +int satid2no(const char *id); +void satno2id(int sat, char *id); +int satexclude(int sat, int svh, const prcopt_t *opt); +int testsnr(int base, int freq, double el, double snr,const snrmask_t *mask); +unsigned char obs2code(const char *obs, int *freq); +char *code2obs(unsigned char code, int *freq); +void setcodepri(int sys, int freq, const char *pri); +int getcodepri(int sys, unsigned char code, const char *opt); +unsigned int getbitu(const unsigned char *buff, int pos, int len); +int getbits(const unsigned char *buff, int pos, int len); +void setbitu(unsigned char *buff, int pos, int len, unsigned int data); +void setbits(unsigned char *buff, int pos, int len, int data); +unsigned int rtk_crc32(const unsigned char *buff, int len); +unsigned int rtk_crc24q(const unsigned char *buff, int len); +unsigned short rtk_crc16(const unsigned char *buff, int len); +int decode_word(unsigned int word, unsigned char *data); +double *mat(int n, int m); +int *imat(int n, int m); +double *zeros(int n, int m); +double *eye(int n); +double dot(const double *a, const double *b, int n); +double norm_rtk(const double *a, int n); +void cross3(const double *a, const double *b, double *c); +int normv3(const double *a, double *b); +void matcpy(double *A, const double *B, int n, int m); +void matmul(const char *tr, int n, int k, int m, double alpha, + const double *A, const double *B, double beta, double *C); +int matinv(double *A, int n); +int solve(const char *tr, const double *A, const double *Y, int n, + int m, double *X); +int lsq(const double *A, const double *y, int n, int m, double *x, + double *Q); +int filter_(const double *x, const double *P, const double *H, + const double *v, const double *R, int n, int m, + double *xp, double *Pp); +int filter(double *x, double *P, const double *H, const double *v, + const double *R, int n, int m); +int smoother(const double *xf, const double *Qf, const double *xb, + const double *Qb, int n, double *xs, double *Qs); +void matfprint(const double A[], int n, int m, int p, int q, FILE *fp); +void matprint(const double A[], int n, int m, int p, int q); +double str2num(const char *s, int i, int n); +int str2time(const char *s, int i, int n, gtime_t *t); +gtime_t epoch2time(const double *ep); +void time2epoch(gtime_t t, double *ep); +gtime_t gpst2time(int week, double sec); +double time2gpst(gtime_t t, int *week); +gtime_t gst2time(int week, double sec); +double time2gst(gtime_t t, int *week); +gtime_t bdt2time(int week, double sec); +double time2bdt(gtime_t t, int *week); +gtime_t timeadd(gtime_t t, double sec); +double timediff(gtime_t t1, gtime_t t2); +gtime_t timeget(void); +//void timeset(gtime_t t); +int read_leaps_text(FILE *fp); +int read_leaps_usno(FILE *fp); +int read_leaps(const char *file); +gtime_t gpst2utc(gtime_t t); +gtime_t utc2gpst(gtime_t t); +gtime_t gpst2bdt(gtime_t t); +gtime_t bdt2gpst(gtime_t t); +double time2sec(gtime_t time, gtime_t *day); +double utc2gmst(gtime_t t, double ut1_utc); +void time2str(gtime_t t, char *s, int n); +char *time_str(gtime_t t, int n); +double time2doy(gtime_t t); +int adjgpsweek(int week); +unsigned int tickget(void); +void sleepms(int ms); +void deg2dms(double deg, double *dms, int ndec); +double dms2deg(const double *dms); +void ecef2pos(const double *r, double *pos); +void pos2ecef(const double *pos, double *r); +void xyz2enu(const double *pos, double *E); +void ecef2enu(const double *pos, const double *r, double *e); +void enu2ecef(const double *pos, const double *e, double *r); +void covenu(const double *pos, const double *P, double *Q); +void covecef(const double *pos, const double *Q, double *P); + + +void ast_args(double t, double *f); +void nut_iau1980(double t, const double *f, double *dpsi, double *deps); +void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst); +int decodef(char *p, int n, double *v); +void addpcv(const pcv_t *pcv, pcvs_t *pcvs); +int readngspcv(const char *file, pcvs_t *pcvs); +int readantex(const char *file, pcvs_t *pcvs); +int readpcv(const char *file, pcvs_t *pcvs); +pcv_t *searchpcv(int sat, const char *type, gtime_t time, + const pcvs_t *pcvs); +void readpos(const char *file, const char *rcv, double *pos); +int readblqrecord(FILE *fp, double *odisp); +int readblq(const char *file, const char *sta, double *odisp); +int readerp(const char *file, erp_t *erp); +int geterp(const erp_t *erp, gtime_t time, double *erpv); +int cmpeph(const void *p1, const void *p2); +void uniqeph(nav_t *nav); +int cmpgeph(const void *p1, const void *p2); +void uniqgeph(nav_t *nav); +int cmpseph(const void *p1, const void *p2); +void uniqseph(nav_t *nav); +void uniqnav(nav_t *nav); +int cmpobs(const void *p1, const void *p2); +int sortobs(obs_t *obs); +int screent(gtime_t time, gtime_t ts, gtime_t te, double tint); +int readnav(const char *file, nav_t *nav); +int savenav(const char *file, const nav_t *nav); +void freeobs(obs_t *obs); +void freenav(nav_t *nav, int opt); + +//void traceopen(const char *file); +//void traceclose(void); +//void tracelevel(int level); +void trace (int level, const char *format, ...); +//void tracet (int level, const char *format, ...); +void tracemat(int level, const double *A, int n, int m, int p, int q); +void traceobs(int level, const obsd_t *obs, int n); +//void tracenav(int level, const nav_t *nav); +//void tracegnav(int level, const nav_t *nav); +//void tracehnav(int level, const nav_t *nav); +//void tracepeph(int level, const nav_t *nav); +//void tracepclk(int level, const nav_t *nav); +//void traceb (int level, const unsigned char *p, int n); + +int execcmd(const char *cmd); +void createdir(const char *path); +int repstr(char *str, const char *pat, const char *rep); +int reppath(const char *path, char *rpath, gtime_t time, const char *rov, + const char *base); +int reppaths(const char *path, char *rpath[], int nmax, gtime_t ts, + gtime_t te, const char *rov, const char *base); +double satwavelen(int sat, int frq, const nav_t *nav); +double geodist(const double *rs, const double *rr, double *e); +double satazel(const double *pos, const double *e, double *azel); + +//#define SQRT(x) ((x)<0.0?0.0:sqrt(x)) +void dops(int ns, const double *azel, double elmin, double *dop); +double ionmodel(gtime_t t, const double *ion, const double *pos, + const double *azel); +double ionmapf(const double *pos, const double *azel); +double ionppp(const double *pos, const double *azel, double re, + double hion, double *posp); +double tropmodel(gtime_t time, const double *pos, const double *azel, + double humi); +double interpc(const double coef[], double lat); +double mapf(double el, double a, double b, double c); +double nmf(gtime_t time, const double pos[], const double azel[], + double *mapfw); +double tropmapf(gtime_t time, const double pos[], const double azel[], + double *mapfw); +double interpvar(double ang, const double *var); + +void antmodel(const pcv_t *pcv, const double *del, const double *azel, + int opt, double *dant); + +void antmodel_s(const pcv_t *pcv, double nadir, double *dant); +void sunmoonpos_eci(gtime_t tut, double *rsun, double *rmoon); +void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, + double *rmoon, double *gmst); +void csmooth(obs_t *obs, int ns); +int rtk_uncompress(const char *file, char *uncfile); +int expath(const char *path, char *paths[], int nmax); +void windupcorr(gtime_t time, const double *rs, const double *rr, double *phw); + +#endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc new file mode 100644 index 000000000..437b75e1f --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -0,0 +1,2220 @@ +/*! + * \file rtklib_rtkpos.cc + * \brief rtklib ppp-related functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_rtkpos.h" +#include "rtklib_pntpos.h" +#include "rtklib_ephemeris.h" +#include "rtklib_ppp.h" +#include "rtklib_tides.h" +#include "rtklib_lambda.h" + +static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)), + const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)), + const double *azel __attribute((unused))) {return 0;} +static int resamb_TCAR(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)), + const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute((unused)), const nav_t *nav __attribute((unused)), + const double *azel __attribute((unused))) {return 0;} + +/* global variables ----------------------------------------------------------*/ +static int statlevel = 0; /* rtk status output level (0:off) */ +static FILE *fp_stat = NULL; /* rtk status file pointer */ +static char file_stat[1024] = ""; /* rtk status file original path */ +static gtime_t time_stat = {0, 0}; /* rtk status file time */ + +/* open solution status file --------------------------------------------------- + * open solution status file and set output level + * args : char *file I rtk status file + * int level I rtk status level (0: off) + * return : status (1:ok,0:error) + * notes : file can constain time keywords (%Y,%y,%m...) defined in reppath(). + * The time to replace keywords is based on UTC of CPU time. + * output : solution status file record format + * + * $POS,week,tow,stat,posx,posy,posz,posxf,posyf,poszf + * week/tow : gps week no/time of week (s) + * stat : solution status + * posx/posy/posz : position x/y/z ecef (m) float + * posxf/posyf/poszf : position x/y/z ecef (m) fixed + * + * $VELACC,week,tow,stat,vele,veln,velu,acce,accn,accu,velef,velnf,veluf,accef,accnf,accuf + * week/tow : gps week no/time of week (s) + * stat : solution status + * vele/veln/velu : velocity e/n/u (m/s) float + * acce/accn/accu : acceleration e/n/u (m/s^2) float + * velef/velnf/veluf : velocity e/n/u (m/s) fixed + * accef/accnf/accuf : acceleration e/n/u (m/s^2) fixed + * + * $CLK,week,tow,stat,clk1,clk2,clk3,clk4 + * week/tow : gps week no/time of week (s) + * stat : solution status + * clk1 : receiver clock bias GPS (ns) + * clk2 : receiver clock bias GLO-GPS (ns) + * clk3 : receiver clock bias GAL-GPS (ns) + * clk4 : receiver clock bias BDS-GPS (ns) + * + * $ION,week,tow,stat,sat,az,el,ion,ion-fixed + * week/tow : gps week no/time of week (s) + * stat : solution status + * sat : satellite id + * az/el : azimuth/elevation angle(deg) + * ion : vertical ionospheric delay L1 (m) float + * ion-fixed: vertical ionospheric delay L1 (m) fixed + * + * $TROP,week,tow,stat,rcv,ztd,ztdf + * week/tow : gps week no/time of week (s) + * stat : solution status + * rcv : receiver (1:rover,2:base station) + * ztd : zenith total delay (m) float + * ztdf : zenith total delay (m) fixed + * + * $HWBIAS,week,tow,stat,frq,bias,biasf + * week/tow : gps week no/time of week (s) + * stat : solution status + * frq : frequency (1:L1,2:L2,...) + * bias : h/w bias coefficient (m/MHz) float + * biasf : h/w bias coefficient (m/MHz) fixed + * + * $SAT,week,tow,sat,frq,az,el,resp,resc,vsat,snr,fix,slip,lock,outc,slipc,rejc + * week/tow : gps week no/time of week (s) + * sat/frq : satellite id/frequency (1:L1,2:L2,...) + * az/el : azimuth/elevation angle (deg) + * resp : pseudorange residual (m) + * resc : carrier-phase residual (m) + * vsat : valid data flag (0:invalid,1:valid) + * snr : signal strength (dbHz) + * fix : ambiguity flag (0:no data,1:float,2:fixed,3:hold,4:ppp) + * slip : cycle-slip flag (bit1:slip,bit2:parity unknown) + * lock : carrier-lock count + * outc : data outage count + * slipc : cycle-slip count + * rejc : data reject (outlier) count + * + *-----------------------------------------------------------------------------*/ +int rtkopenstat(const char *file, int level) +{ + gtime_t time = utc2gpst(timeget()); + char path[1024]; + + trace(3, "rtkopenstat: file=%s level=%d\n", file, level); + + if (level <= 0) return 0; + + reppath(file, path, time, "",""); + + if (!(fp_stat = fopen(path, "w"))) + { + trace(1, "rtkopenstat: file open error path=%s\n", path); + return 0; + } + strcpy(file_stat, file); + time_stat = time; + statlevel = level; + return 1; +} + + +/* close solution status file -------------------------------------------------- + * close solution status file + * args : none + * return : none + *-----------------------------------------------------------------------------*/ +void rtkclosestat(void) +{ + trace(3, "rtkclosestat:\n"); + + if (fp_stat) fclose(fp_stat); + fp_stat = NULL; + file_stat[0] = '\0'; + statlevel = 0; +} + + +/* write solution status to buffer -------------------------------------------*/ +void rtkoutstat(rtk_t *rtk, char *buff __attribute__((unused))) +{ + ssat_t *ssat; + double tow, pos[3], vel[3], acc[3], vela[3] = {0}, acca[3] = {0}, xa[3]; + int i, j, week, est, nfreq, nf = NF_RTK(&rtk->opt); + char id[32]; + + if (statlevel <= 0 || !fp_stat) return; + + trace(3, "outsolstat:\n"); + + /* swap solution status file */ + swapsolstat(); + + est = rtk->opt.mode >= PMODE_DGPS; + nfreq = est ? nf : 1; + tow = time2gpst(rtk->sol.time, &week); + + /* receiver position */ + if (est) + { + for (i = 0;i<3;i++) xa[i] = i < rtk->na ? rtk->xa[i] : 0.0; + fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, + rtk->sol.stat, rtk->x[0], rtk->x[1], rtk->x[2], xa[0], xa[1], xa[2]); + } + else + { + fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, + rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2], + 0.0, 0.0, 0.0); + } + /* receiver velocity and acceleration */ + if (est && rtk->opt.dynamics) + { + ecef2pos(rtk->sol.rr, pos); + ecef2enu(pos, rtk->x+3, vel); + ecef2enu(pos, rtk->x+6, acc); + if (rtk->na >= 6) ecef2enu(pos, rtk->xa+3, vela); + if (rtk->na >= 9) ecef2enu(pos, rtk->xa+6, acca); + fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[2], + vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]); + } + else + { + ecef2pos(rtk->sol.rr, pos); + ecef2enu(pos, rtk->sol.rr+3, vel); + fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + } + /* receiver clocks */ + fprintf(fp_stat, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", + week, tow, rtk->sol.stat, 1, rtk->sol.dtr[0]*1E9, rtk->sol.dtr[1]*1E9, + rtk->sol.dtr[2]*1E9, rtk->sol.dtr[3]*1E9); + + /* ionospheric parameters */ + if (est && rtk->opt.ionoopt == IONOOPT_EST) + { + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + j = II_RTK(i+1, &rtk->opt); + xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; + fprintf(fp_stat, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, rtk->sol.stat, + id, ssat->azel[0]*R2D, ssat->azel[1]*R2D, rtk->x[j], xa[0]); + } + } + /* tropospheric parameters */ + if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG)) + { + for (i = 0;i<2;i++) + { + j = IT_RTK(i, &rtk->opt); + xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; + fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, + i+1, rtk->x[j], xa[0]); + } + } + /* receiver h/w bias */ + if (est && rtk->opt.glomodear == 2) + { + for (i = 0;iopt); + xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; + fprintf(fp_stat, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, + i+1, rtk->x[j], xa[0]); + } + } + if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return; + + /* residuals and status */ + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + for (j = 0;jazel[0]*R2D, ssat->azel[1]*R2D, + ssat->resp [j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25, + ssat->fix [j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j], + ssat->slipc[j], ssat->rejc[j]); + } + } +} + + +/* swap solution status file -------------------------------------------------*/ +void swapsolstat(void) +{ + gtime_t time = utc2gpst(timeget()); + char path[1024]; + + if ((int)(time2gpst(time , NULL)/INT_SWAP_STAT) == + (int)(time2gpst(time_stat, NULL)/INT_SWAP_STAT)) + { + return; + } + time_stat = time; + + if (!reppath(file_stat, path, time, "","")) + { + return; + } + if (fp_stat) fclose(fp_stat); + + if (!(fp_stat = fopen(path, "w"))) + { + trace(2, "swapsolstat: file open error path=%s\n", path); + return; + } + trace(3, "swapsolstat: path=%s\n", path); +} + + +/* output solution status ----------------------------------------------------*/ +void outsolstat(rtk_t *rtk) +{ + ssat_t *ssat; + double tow, pos[3], vel[3], acc[3], vela[3] = {0}, acca[3] = {0}, xa[3]; + int i, j, week, est, nfreq, nf = NF_RTK(&rtk->opt); + char id[32]; + + if (statlevel <= 0 || !fp_stat) return; + + trace(3, "outsolstat:\n"); + + /* swap solution status file */ + swapsolstat(); + + est = rtk->opt.mode >= PMODE_DGPS; + nfreq = est ? nf : 1; + tow = time2gpst(rtk->sol.time, &week); + + /* receiver position */ + if (est) + { + for (i = 0;i<3;i++) xa[i] = i < rtk->na ? rtk->xa[i] : 0.0; + fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, + rtk->sol.stat, rtk->x[0], rtk->x[1], rtk->x[2], xa[0], xa[1], xa[2]); + } + else + { + fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow, + rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2], + 0.0, 0.0, 0.0); + } + /* receiver velocity and acceleration */ + if (est && rtk->opt.dynamics) + { + ecef2pos(rtk->sol.rr, pos); + ecef2enu(pos, rtk->x+3, vel); + ecef2enu(pos, rtk->x+6, acc); + if (rtk->na >= 6) ecef2enu(pos, rtk->xa+3, vela); + if (rtk->na >= 9) ecef2enu(pos, rtk->xa+6, acca); + fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[2], + vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]); + } + else + { + ecef2pos(rtk->sol.rr, pos); + ecef2enu(pos, rtk->sol.rr+3, vel); + fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", + week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + } + /* receiver clocks */ + fprintf(fp_stat, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", + week, tow, rtk->sol.stat, 1, rtk->sol.dtr[0]*1E9, rtk->sol.dtr[1]*1E9, + rtk->sol.dtr[2]*1E9, rtk->sol.dtr[3]*1E9); + + /* ionospheric parameters */ + if (est && rtk->opt.ionoopt == IONOOPT_EST) + { + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + j = II_RTK(i+1, &rtk->opt); + xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; + fprintf(fp_stat, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, rtk->sol.stat, + id, ssat->azel[0]*R2D, ssat->azel[1]*R2D, rtk->x[j], xa[0]); + } + } + /* tropospheric parameters */ + if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG)) + { + for (i = 0;i<2;i++) + { + j = IT_RTK(i, &rtk->opt); + xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; + fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, + i+1, rtk->x[j], xa[0]); + } + } + /* receiver h/w bias */ + if (est && rtk->opt.glomodear == 2) + { + for (i = 0;iopt); + xa[0] = j < rtk->na ? rtk->xa[j] : 0.0; + fprintf(fp_stat, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, + i+1, rtk->x[j], xa[0]); + } + } + if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return; + + /* residuals and status */ + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + for (j = 0;jazel[0]*R2D, ssat->azel[1]*R2D, + ssat->resp [j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25, + ssat->fix [j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j], + ssat->slipc[j], ssat->rejc[j]); + } + } +} + + +/* save error message --------------------------------------------------------*/ +void errmsg(rtk_t *rtk, const char *format, ...) +{ + char buff[256], tstr[32]; + int n; + va_list ap; + time2str(rtk->sol.time, tstr, 2); + n = sprintf(buff, "%s: ", tstr+11); + va_start(ap, format); + n += vsprintf(buff+n, format, ap); + va_end(ap); + n = nneb ? n : MAXERRMSG-rtk->neb; + memcpy(rtk->errbuf+rtk->neb, buff, n); + rtk->neb += n; + trace(2, "%s", buff); +} + + +/* single-differenced observable ---------------------------------------------*/ +double sdobs(const obsd_t *obs, int i, int j, int f) +{ + double pi = ferr[3]*bl/1e4, d = SPEED_OF_LIGHT * opt->sclkstab*dt, fact = 1.0; + double sinel = sin(el); + int i = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : 0), nf = NF_RTK(opt); + + /* extended error model */ + if (f >= nf && opt->exterr.ena[0]) + { /* code */ + a = opt->exterr.cerr[i][ (f-nf)*2]; + b = opt->exterr.cerr[i][1+(f-nf)*2]; + if (sys == SYS_SBS) {a *= EFACT_SBS; b *= EFACT_SBS;} + } + else if (fexterr.ena[1]) + { /* phase */ + a = opt->exterr.perr[i][ f*2]; + b = opt->exterr.perr[i][1+f*2]; + if (sys == SYS_SBS) {a *= EFACT_SBS; b *= EFACT_SBS;} + } + else + { /* normal error model */ + if (f >= nf) fact=opt->eratio[f-nf]; + if (fact <= 0.0) fact=opt->eratio[0]; + fact*=sys == SYS_GLO ? EFACT_GLO : (sys == SYS_SBS ? EFACT_SBS : EFACT_GPS); + a = fact*opt->err[1]; + b = fact*opt->err[2]; + } + return 2.0*(opt->ionoopt == IONOOPT_IFLC ? 3.0 : 1.0)*(a*a+b*b/sinel/sinel+c*c)+d*d; +} + + +/* baseline length -----------------------------------------------------------*/ +double baseline(const double *ru, const double *rb, double *dr) +{ + int i; + for (i = 0;i<3;i++) dr[i] = ru[i]-rb[i]; + return norm_rtk(dr, 3); +} + + +/* initialize state and covariance -------------------------------------------*/ +void initx_rtk(rtk_t *rtk, double xi, double var, int i) +{ + int j; + rtk->x[i] = xi; + for (j = 0;jnx;j++) + { + rtk->P[i+j*rtk->nx] = rtk->P[j+i*rtk->nx] = i == j ? var : 0.0; + } +} + + +/* select common satellites between rover and reference station --------------*/ +int selsat(const obsd_t *obs, double *azel, int nu, int nr, + const prcopt_t *opt, int *sat, int *iu, int *ir) +{ + int i, j, k = 0; + + trace(3, "selsat : nu=%d nr=%d\n", nu, nr); + + for (i = 0, j = nu;iobs[j].sat) i--; + else if (azel[1+j*2] >= opt->elmin) { /* elevation at base station */ + sat[k] = obs[i].sat; iu[k] = i; ir[k++] = j; + trace(4, "(%2d) sat=%3d iu=%2d ir=%2d\n", k-1, obs[i].sat, i, j); + } + } + return k; +} + + +/* temporal update of position/velocity/acceleration -------------------------*/ +void udpos(rtk_t *rtk, double tt) +{ + double *F, *FP, *xp, pos[3], Q[9] = {0}, Qv[9], var = 0.0; + int i, j; + + trace(3, "udpos : tt=%.3f\n", tt); + + /* fixed mode */ + if (rtk->opt.mode == PMODE_FIXED) + { + for (i = 0;i<3;i++) initx_rtk(rtk, rtk->opt.ru[i], 1E-8, i); + return; + } + /* initialize position for first epoch */ + if (norm_rtk(rtk->x, 3) <= 0.0) + { + for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i); + if (rtk->opt.dynamics) + { + for (i = 3;i<6;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_VEL, i); + for (i = 6;i<9;i++) initx_rtk(rtk, 1E-6, VAR_ACC, i); + } + } + /* static mode */ + if (rtk->opt.mode == PMODE_STATIC) return; + + /* kinmatic mode without dynamics */ + if (!rtk->opt.dynamics) + { + for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i); + return; + } + /* check variance of estimated postion */ + for (i = 0;i<3;i++) var += rtk->P[i+i*rtk->nx]; var/=3.0; + + if (var>VAR_POS) + { + /* reset position with large variance */ + for (i = 0;i<3;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_POS, i); + for (i = 3;i<6;i++) initx_rtk(rtk, rtk->sol.rr[i], VAR_VEL, i); + for (i = 6;i<9;i++) initx_rtk(rtk, 1E-6, VAR_ACC, i); + trace(2, "reset rtk position due to large variance: var=%.3f\n", var); + return; + } + /* state transition of position/velocity/acceleration */ + F = eye(rtk->nx); FP = mat(rtk->nx, rtk->nx); xp = mat(rtk->nx, 1); + + for (i = 0;i<6;i++) + { + F[i+(i+3)*rtk->nx] = tt; + } + /* x=F*x, P=F*P*F+Q */ + matmul("NN", rtk->nx, 1, rtk->nx, 1.0, F, rtk->x, 0.0, xp); + matcpy(rtk->x, xp, rtk->nx, 1); + matmul("NN", rtk->nx, rtk->nx, rtk->nx, 1.0, F, rtk->P, 0.0, FP); + matmul("NT", rtk->nx, rtk->nx, rtk->nx, 1.0, FP, F, 0.0, rtk->P); + + /* process noise added to only acceleration */ + Q[0] = Q[4] = std::pow(rtk->opt.prn[3], 2.0); Q[8] = std::pow(rtk->opt.prn[4], 2.0); + ecef2pos(rtk->x, pos); + covecef(pos, Q, Qv); + for (i = 0;i<3;i++) for (j = 0;j<3;j++) + { + rtk->P[i+6+(j+6)*rtk->nx] += Qv[i+j*3]; + } + free(F); free(FP); free(xp); +} + + +/* temporal update of ionospheric parameters ---------------------------------*/ +void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns) +{ + double el, fact; + int i, j; + + trace(3, "udion : tt=%.1f bl=%.0f ns=%d\n", tt, bl, ns); + + for (i = 1;i <= MAXSAT;i++) + { + j = II_RTK(i, &rtk->opt); + if (rtk->x[j] != 0.0 && + rtk->ssat[i-1].outc[0]>GAP_RESION && rtk->ssat[i-1].outc[1]>GAP_RESION) + rtk->x[j] = 0.0; + } + for (i = 0;iopt); + + if (rtk->x[j] == 0.0) + { + initx_rtk(rtk, 1E-6, std::pow(rtk->opt.std[1]*bl/1e4, 2.0), j); + } + else + { + /* elevation dependent factor of process noise */ + el = rtk->ssat[sat[i]-1].azel[1]; + fact = cos(el); + rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[1]*bl/1e4*fact, 2.0)*tt; + } + } +} + + +/* temporal update of tropospheric parameters --------------------------------*/ +void udtrop(rtk_t *rtk, double tt, double bl __attribute((unused))) +{ + int i, j, k; + + trace(3, "udtrop : tt=%.1f\n", tt); + + for (i = 0;i<2;i++) + { + j = IT_RTK(i, &rtk->opt); + + if (rtk->x[j] == 0.0) + { + initx_rtk(rtk, INIT_ZWD, std::pow(rtk->opt.std[2], 2.0), j); /* initial zwd */ + + if (rtk->opt.tropopt >= TROPOPT_ESTG) + { + for (k = 0;k<2;k++) initx_rtk(rtk, 1e-6, VAR_GRA, ++j); + } + } + else + { + rtk->P[j+j*rtk->nx] += std::pow(rtk->opt.prn[2], 2.0)*tt; + + if (rtk->opt.tropopt >= TROPOPT_ESTG) + { + for (k = 0;k<2;k++) + { + rtk->P[++j*(1+rtk->nx)]+=std::pow(rtk->opt.prn[2]*0.3, 2.0)*fabs(rtk->tt); + } + } + } + } +} + + +/* temporal update of receiver h/w biases ------------------------------------*/ +void udrcvbias(rtk_t *rtk, double tt) +{ + int i, j; + + trace(3, "udrcvbias: tt=%.1f\n", tt); + + for (i = 0;iopt); + + if (rtk->x[j] == 0.0) + { + initx_rtk(rtk, 1E-6, VAR_HWBIAS, j); + } + /* hold to fixed solution */ + else if (rtk->nfix >= rtk->opt.minfix && rtk->sol.ratio>rtk->opt.thresar[0]) + { + initx_rtk(rtk, rtk->xa[j], rtk->Pa[j+j*rtk->na], j); + } + else + { + rtk->P[j+j*rtk->nx]+=std::pow(PRN_HWBIAS, 2.0)*tt; + } + } +} + + +/* detect cycle slip by LLI --------------------------------------------------*/ +void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv) +{ + unsigned int slip, LLI; + int f, sat = obs[i].sat; + + trace(3, "detslp_ll: i=%d rcv=%d\n", i, rcv); + + for (f = 0;fopt.nf;f++) + { + + if (obs[i].L[f] == 0.0) continue; + + /* restore previous LLI */ + if (rcv == 1) LLI = getbitu(&rtk->ssat[sat-1].slip[f], 0, 2); /* rover */ + else LLI = getbitu(&rtk->ssat[sat-1].slip[f], 2, 2); /* base */ + + /* detect slip by cycle slip flag in LLI */ + if (rtk->tt >= 0.0) { /* forward */ + if (obs[i].LLI[f]&1) + { + errmsg(rtk, "slip detected forward (sat=%2d rcv=%d F=%d LLI=%x)\n", + sat, rcv, f+1, obs[i].LLI[f]); + } + slip = obs[i].LLI[f]; + } + else + { /* backward */ + if (LLI&1) + { + errmsg(rtk, "slip detected backward (sat=%2d rcv=%d F=%d LLI=%x)\n", + sat, rcv, f+1, LLI); + } + slip = LLI; + } + /* detect slip by parity unknown flag transition in LLI */ + if (((LLI&2) && !(obs[i].LLI[f]&2)) || (!(LLI&2) && (obs[i].LLI[f]&2))) + { + errmsg(rtk, "slip detected half-cyc (sat=%2d rcv=%d F=%d LLI=%x->%x)\n", + sat, rcv, f+1, LLI, obs[i].LLI[f]); + slip|=1; + } + /* save current LLI */ + if (rcv == 1) setbitu(&rtk->ssat[sat-1].slip[f], 0, 2, obs[i].LLI[f]); + else setbitu(&rtk->ssat[sat-1].slip[f], 2, 2, obs[i].LLI[f]); + + /* save slip and half-cycle valid flag */ + rtk->ssat[sat-1].slip[f]|=(unsigned char)slip; + rtk->ssat[sat-1].half[f] = (obs[i].LLI[f]&2) ? 0 : 1; + } +} + + +/* detect cycle slip by L1-L2 geometry free phase jump -----------------------*/ +void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j, + const nav_t *nav) +{ + int sat = obs[i].sat; + double g0, g1; + + trace(3, "detslp_gf_L1L2: i=%d j=%d\n", i, j); + + if (rtk->opt.nf <= 1 || (g1=gfobs_L1L2(obs, i, j, nav->lam[sat-1])) == 0.0) return; + + g0 = rtk->ssat[sat-1].gf; rtk->ssat[sat-1].gf = g1; + + if (g0 != 0.0 && fabs(g1-g0)>rtk->opt.thresslip) + { + + rtk->ssat[sat-1].slip[0]|=1; + rtk->ssat[sat-1].slip[1]|=1; + + errmsg(rtk, "slip detected (sat=%2d GF_L1_L2=%.3f %.3f)\n", sat, g0, g1); + } +} + + +/* detect cycle slip by L1-L5 geometry free phase jump -----------------------*/ +void detslp_gf_L1L5(rtk_t *rtk, const obsd_t *obs, int i, int j, + const nav_t *nav) +{ + int sat = obs[i].sat; + double g0, g1; + + trace(3, "detslp_gf_L1L5: i=%d j=%d\n", i, j); + + if (rtk->opt.nf <= 2 || (g1=gfobs_L1L5(obs, i, j, nav->lam[sat-1])) == 0.0) return; + + g0 = rtk->ssat[sat-1].gf2; rtk->ssat[sat-1].gf2 = g1; + + if (g0 != 0.0 && fabs(g1-g0)>rtk->opt.thresslip) + { + rtk->ssat[sat-1].slip[0]|=1; + rtk->ssat[sat-1].slip[2]|=1; + + errmsg(rtk, "slip detected (sat=%2d GF_L1_L5=%.3f %.3f)\n", sat, g0, g1); + } +} + + +/* detect cycle slip by doppler and phase difference -------------------------*/ +void detslp_dop(rtk_t *rtk __attribute__((unused)), const obsd_t *obs __attribute__((unused)), int i __attribute__((unused)), int rcv __attribute__((unused)), + const nav_t *nav __attribute__((unused))) +{ + /* detection with doppler disabled because of clock-jump issue (v.2.3.0) */ +#if 0 + int f,sat = obs[i].sat; + double tt,dph,dpt,lam,thres; + + trace(3,"detslp_dop: i=%d rcv=%d\n",i,rcv); + + for (f = 0;fopt.nf;f++) + { + if (obs[i].L[f] == 0.0 || obs[i].D[f] == 0.0 || rtk->ph[rcv-1][sat-1][f] == 0.0) + { + continue; + } + if (fabs(tt = timediff(obs[i].time,rtk->pt[rcv-1][sat-1][f]))lam[sat-1][f])<=0.0) continue; + + /* cycle slip threshold (cycle) */ + thres = MAXACC*tt*tt/2.0/lam+rtk->opt.err[4]*fabs(tt)*4.0; + + /* phase difference and doppler x time (cycle) */ + dph = obs[i].L[f]-rtk->ph[rcv-1][sat-1][f]; + dpt = -obs[i].D[f]*tt; + + if (fabs(dph-dpt)<=thres) continue; + + rtk->slip[sat-1][f]| = 1; + + errmsg(rtk,"slip detected (sat=%2d rcv=%d L%d=%.3f %.3f thres=%.3f)\n", + sat,rcv,f+1,dph,dpt,thres); + } +#endif +} + + +/* temporal update of phase biases -------------------------------------------*/ +void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat, + const int *iu, const int *ir, int ns, const nav_t *nav) +{ + double cp, pr, cp1, cp2, pr1, pr2, *bias, offset, lami, lam1, lam2, C1, C2; + int i, j, f, slip, reset, nf = NF_RTK(&rtk->opt); + + trace(3, "udbias : tt=%.1f ns=%d\n", tt, ns); + + for (i = 0;iopt.nf;f++) rtk->ssat[sat[i]-1].slip[f] &= 0xFC; + detslp_ll(rtk, obs, iu[i], 1); + detslp_ll(rtk, obs, ir[i], 2); + + /* detect cycle slip by geometry-free phase jump */ + detslp_gf_L1L2(rtk, obs, iu[i], ir[i], nav); + detslp_gf_L1L5(rtk, obs, iu[i], ir[i], nav); + + /* detect cycle slip by doppler and phase difference */ + detslp_dop(rtk, obs, iu[i], 1, nav); + detslp_dop(rtk, obs, ir[i], 2, nav); + + /* update half-cycle valid flag */ + for (f = 0;fssat[sat[i]-1].half[f] = + !((obs[iu[i]].LLI[f]&2) || (obs[ir[i]].LLI[f]&2)); + } + } + for (f = 0;fssat[i-1].outc[f]>(unsigned int)rtk->opt.maxout; + + if (rtk->opt.modear == ARMODE_INST && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) + { + initx_rtk(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt)); + } + else if (reset && rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) + { + initx_rtk(rtk, 0.0, 0.0, IB_RTK(i, f, &rtk->opt)); + trace(3, "udbias : obs outage counter overflow (sat=%3d L%d n=%d)\n", + i, f+1, rtk->ssat[i-1].outc[f]); + } + if (rtk->opt.modear != ARMODE_INST && reset) + { + rtk->ssat[i-1].lock[f] = -rtk->opt.minlock; + } + } + /* reset phase-bias if detecting cycle slip */ + for (i = 0;iopt); + rtk->P[j+j*rtk->nx]+=rtk->opt.prn[0]*rtk->opt.prn[0]*tt; + slip = rtk->ssat[sat[i]-1].slip[f]; + if (rtk->opt.ionoopt == IONOOPT_IFLC) slip|=rtk->ssat[sat[i]-1].slip[1]; + if (rtk->opt.modear == ARMODE_INST || !(slip&1)) continue; + rtk->x[j] = 0.0; + rtk->ssat[sat[i]-1].lock[f] = -rtk->opt.minlock; + } + bias = zeros(ns, 1); + + /* estimate approximate phase-bias by phase - code */ + for (i = j = 0, offset = 0.0;iopt.ionoopt != IONOOPT_IFLC) + { + cp = sdobs(obs, iu[i], ir[i], f); /* cycle */ + pr = sdobs(obs, iu[i], ir[i], f+NFREQ); + lami = nav->lam[sat[i]-1][f]; + if (cp == 0.0 || pr == 0.0 || lami <= 0.0) continue; + + bias[i] = cp-pr/lami; + } + else + { + cp1 = sdobs(obs, iu[i], ir[i], 0); + cp2 = sdobs(obs, iu[i], ir[i], 1); + pr1 = sdobs(obs, iu[i], ir[i], NFREQ); + pr2 = sdobs(obs, iu[i], ir[i], NFREQ+1); + lam1 = nav->lam[sat[i]-1][0]; + lam2 = nav->lam[sat[i]-1][1]; + if (cp1 == 0.0 || cp2 == 0.0 || pr1 == 0.0 || pr2 == 0.0 || lam1 <= 0.0 || lam2 <= 0.0) continue; + + C1 = std::pow(lam2, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0)); + C2 = -std::pow(lam1, 2.0)/(std::pow(lam2, 2.0)-std::pow(lam1, 2.0)); + bias[i] = (C1*lam1*cp1+C2*lam2*cp2)-(C1*pr1+C2*pr2); + } + if (rtk->x[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) + { + offset+=bias[i]-rtk->x[IB_RTK(sat[i], f, &rtk->opt)]; + j++; + } + } + /* correct phase-bias offset to enssure phase-code coherency */ + if (j>0) { + for (i = 1;i <= MAXSAT;i++) + { + if (rtk->x[IB_RTK(i, f, &rtk->opt)] != 0.0) rtk->x[IB_RTK(i, f, &rtk->opt)]+=offset/j; + } + } + /* set initial states of phase-bias */ + for (i = 0;ix[IB_RTK(sat[i], f, &rtk->opt)] != 0.0) continue; + initx_rtk(rtk, bias[i], std::pow(rtk->opt.std[0], 2.0), IB_RTK(sat[i], f, &rtk->opt)); + } + free(bias); + } +} + + +/* temporal update of states --------------------------------------------------*/ +void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat, + const int *iu, const int *ir, int ns, const nav_t *nav) +{ + double tt = fabs(rtk->tt), bl = 0.0, dr[3]; + + trace(3, "udstate : ns=%d\n", ns); + + /* temporal update of position/velocity/acceleration */ + udpos(rtk, tt); + + /* temporal update of ionospheric parameters */ + if (rtk->opt.ionoopt >= IONOOPT_EST) + { + bl = baseline(rtk->x, rtk->rb, dr); + udion(rtk, tt, bl, sat, ns); + } + /* temporal update of tropospheric parameters */ + if (rtk->opt.tropopt >= TROPOPT_EST) + { + udtrop(rtk, tt, bl); + } + /* temporal update of eceiver h/w bias */ + if (rtk->opt.glomodear == 2 && (rtk->opt.navsys&SYS_GLO)) + { + udrcvbias(rtk, tt); + } + /* temporal update of phase-bias */ + if (rtk->opt.mode>PMODE_DGPS) + { + udbias(rtk, tt, obs, sat, iu, ir, ns, nav); + } +} + + +/* undifferenced phase/code residual for satellite ---------------------------*/ +void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav, + const double *azel, const double *dant, + const prcopt_t *opt, double *y) +{ + const double *lam = nav->lam[obs->sat-1]; + double f1, f2, C1, C2, dant_if; + int i, nf = NF_RTK(opt); + + if (opt->ionoopt == IONOOPT_IFLC) + { /* iono-free linear combination */ + if (lam[0] == 0.0 || lam[1] == 0.0) return; + + if (testsnr(base, 0, azel[1], obs->SNR[0]*0.25, &opt->snrmask) || + testsnr(base, 1, azel[1], obs->SNR[1]*0.25, &opt->snrmask)) return; + + f1 = SPEED_OF_LIGHT/lam[0]; + f2 = SPEED_OF_LIGHT/lam[1]; + C1 = std::pow(f1, 2.0)/(std::pow(f1, 2.0)-std::pow(f2, 2.0)); + C2 = -std::pow(f2, 2.0)/(std::pow(f1, 2.0)-std::pow(f2, 2.0)); + dant_if = C1*dant[0]+C2*dant[1]; + + if (obs->L[0] != 0.0 && obs->L[1] != 0.0) + { + y[0] = C1*obs->L[0]*lam[0]+C2*obs->L[1]*lam[1]-r-dant_if; + } + if (obs->P[0] != 0.0 && obs->P[1] != 0.0) + { + y[1] = C1*obs->P[0]+C2*obs->P[1]-r-dant_if; + } + } + else + { + for (i = 0;iSNR[i]*0.25, &opt->snrmask)) + { + continue; + } + /* residuals = observable - pseudorange */ + if (obs->L[i] != 0.0) y[i ] = obs->L[i]*lam[i]-r-dant[i]; + if (obs->P[i] != 0.0) y[i+nf] = obs->P[i] -r-dant[i]; + } + } +} + + +/* undifferenced phase/code residuals ----------------------------------------*/ +int zdres(int base, const obsd_t *obs, int n, const double *rs, + const double *dts, const int *svh, const nav_t *nav, + const double *rr, const prcopt_t *opt, int index, double *y, + double *e, double *azel) +{ + double r, rr_[3], pos[3], dant[NFREQ] = {0}, disp[3]; + double zhd, zazel[] = {0.0, 90.0*D2R}; + int i, nf = NF_RTK(opt); + + trace(3, "zdres : n=%d\n", n); + + for (i = 0;itidecorr) + { + tidedisp(gpst2utc(obs[0].time), rr_, opt->tidecorr, &nav->erp, + opt->odisp[base], disp); + for (i = 0;i<3;i++) rr_[i]+=disp[i]; + } + ecef2pos(rr_, pos); + + for (i = 0;ielmin) continue; + + /* excluded satellite? */ + if (satexclude(obs[i].sat, svh[i], opt)) continue; + + /* satellite clock-bias */ + r+=-SPEED_OF_LIGHT*dts[i*2]; + + /* troposphere delay model (hydrostatic) */ + zhd = tropmodel(obs[0].time, pos, zazel, 0.0); + r+=tropmapf(obs[i].time, pos, azel+i*2, NULL)*zhd; + + /* receiver antenna phase center correction */ + antmodel(opt->pcvr+index, opt->antdel[index], azel+i*2, opt->posopt[1], + dant); + + /* undifferenced phase/code residual for satellite */ + zdres_sat(base, r, obs+i, nav, azel+i*2, dant, opt, y+i*nf*2); + } + trace(4, "rr_=%.3f %.3f %.3f\n", rr_[0], rr_[1], rr_[2]); + trace(4, "pos=%.9f %.9f %.3f\n", pos[0]*R2D, pos[1]*R2D, pos[2]); + for (i = 0;iopt.baseline[0] <= 0.0) return 0; + + /* time-adjusted baseline vector and length */ + for (i = 0;i<3;i++) + { + xb[i] = rtk->rb[i]+rtk->rb[i+3]*rtk->sol.age; + b[i] = x[i]-xb[i]; + } + bb = norm_rtk(b, 3); + + /* approximate variance of solution */ + if (P) + { + for (i = 0;i<3;i++) var+=P[i+i*rtk->nx]; + var/=3.0; + } + /* check nonlinearity */ + if (var>thres*thres*bb*bb) + { + trace(3, "constbl : equation nonlinear (bb=%.3f var=%.3f)\n", bb, var); + return 0; + } + /* constraint to baseline length */ + v[index] = rtk->opt.baseline[0]-bb; + if (H) + { + for (i = 0;i<3;i++) H[i+index*rtk->nx] = b[i]/bb; + } + Ri[index] = 0.0; + Rj[index] = std::pow(rtk->opt.baseline[1], 2.0); + + trace(4, "baseline len v=%13.3f R=%8.6f %8.6f\n", v[index], Ri[index], Rj[index]); + + return 1; +} + + +/* precise tropspheric model -------------------------------------------------*/ +double prectrop(gtime_t time, const double *pos, int r, + const double *azel, const prcopt_t *opt, const double *x, + double *dtdx) +{ + double m_w = 0.0, cotz, grad_n, grad_e; + int i = IT_RTK(r, opt); + + /* wet mapping function */ + tropmapf(time, pos, azel, &m_w); + + if (opt->tropopt >= TROPOPT_ESTG && azel[1]>0.0) + { + /* m_w=m_0+m_0*cot(el)*(Gn*cos(az)+Ge*sin(az)): ref [6] */ + cotz = 1.0/tan(azel[1]); + grad_n = m_w*cotz*cos(azel[0]); + grad_e = m_w*cotz*sin(azel[0]); + m_w+=grad_n*x[i+1]+grad_e*x[i+2]; + dtdx[1] = grad_n*x[i]; + dtdx[2] = grad_e*x[i]; + } + else dtdx[1] = dtdx[2] = 0.0; + dtdx[0] = m_w; + return m_w*x[i]; +} + + +/* glonass inter-channel bias correction -------------------------------------*/ +double gloicbcorr(int sat1 __attribute((unused)), int sat2 __attribute((unused)), const prcopt_t *opt, double lam1, + double lam2, int f) +{ + double dfreq; + + if (f>=NFREQGLO || f >= opt->nf || !opt->exterr.ena[2]) return 0.0; + + dfreq = (SPEED_OF_LIGHT/lam1-SPEED_OF_LIGHT/lam2)/(f == 0 ? DFRQ1_GLO : DFRQ2_GLO); + + return opt->exterr.gloicb[f]*0.01*dfreq; /* (m) */ +} + + +/* test navi system (m=0:gps/qzs/sbs,1:glo,2:gal,3:bds) ----------------------*/ +int test_sys(int sys, int m) +{ + switch (sys) + { + case SYS_GPS: return m == 0; + case SYS_QZS: return m == 0; + case SYS_SBS: return m == 0; + case SYS_GLO: return m == 1; + case SYS_GAL: return m == 2; + case SYS_BDS: return m == 3; + } + return 0; +} + + +/* double-differenced phase/code residuals -----------------------------------*/ +int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x, + const double *P, const int *sat, double *y, double *e, + double *azel, const int *iu, const int *ir, int ns, double *v, + double *H, double *R, int *vflg) +{ + prcopt_t *opt = &rtk->opt; + double bl, dr[3], posu[3], posr[3], didxi = 0.0, didxj = 0.0, *im; + double *tropr, *tropu, *dtdxr, *dtdxu, *Ri, *Rj, lami, lamj, fi, fj, df, *Hi = NULL; + int i, j, k, m, f, ff, nv = 0, nb[NFREQ*4*2+2] = {0}, b = 0, sysi, sysj, nf = NF_RTK(opt); + + trace(3, "ddres : dt=%.1f nx=%d ns=%d\n", dt, rtk->nx, ns); + + bl = baseline(x, rtk->rb, dr); + ecef2pos(x, posu); ecef2pos(rtk->rb, posr); + + Ri = mat(ns*nf*2+2, 1); Rj = mat(ns*nf*2+2, 1); im = mat(ns, 1); + tropu = mat(ns, 1); tropr = mat(ns, 1); dtdxu = mat(ns, 3); dtdxr = mat(ns, 3); + + for (i = 0;issat[i].resp[j] = rtk->ssat[i].resc[j] = 0.0; + } + /* compute factors of ionospheric and tropospheric delay */ + for (i = 0;iionoopt >= IONOOPT_EST) + { + im[i] = (ionmapf(posu, azel+iu[i]*2)+ionmapf(posr, azel+ir[i]*2))/2.0; + } + if (opt->tropopt >= TROPOPT_EST) + { + tropu[i] = prectrop(rtk->sol.time, posu, 0, azel+iu[i]*2, opt, x, dtdxu+i*3); + tropr[i] = prectrop(rtk->sol.time, posr, 1, azel+ir[i]*2, opt, x, dtdxr+i*3); + } + } + for (m = 0;m<4;m++) /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */ + + for (f = opt->mode>PMODE_DGPS ? 0 : nf;fssat[sat[j]-1].sys; + if (!test_sys(sysi, m)) continue; + if (!validobs(iu[j], ir[j], f, nf, y)) continue; + if (i<0 || azel[1+iu[j]*2] >= azel[1+iu[i]*2]) i = j; + } + if (i<0) continue; + + /* make double difference */ + for (j = 0;jssat[sat[i]-1].sys; + sysj = rtk->ssat[sat[j]-1].sys; + if (!test_sys(sysj, m)) continue; + if (!validobs(iu[j], ir[j], f, nf, y)) continue; + + ff = f%nf; + lami = nav->lam[sat[i]-1][ff]; + lamj = nav->lam[sat[j]-1][ff]; + if (lami <= 0.0 || lamj <= 0.0) continue; + if (H) + { + Hi = H+nv*rtk->nx; + for (k = 0;knx;k++) Hi[k] = 0.0; + } + /* double-differenced residual */ + v[nv] = (y[f+iu[i]*nf*2]-y[f+ir[i]*nf*2])- + (y[f+iu[j]*nf*2]-y[f+ir[j]*nf*2]); + + /* partial derivatives by rover position */ + if (H) + { + for (k = 0;k<3;k++) + { + Hi[k] = -e[k+iu[i]*3]+e[k+iu[j]*3]; + } + } + /* double-differenced ionospheric delay term */ + if (opt->ionoopt == IONOOPT_EST) + { + fi = lami/lam_carr[0]; fj = lamj/lam_carr[0]; + didxi = (ftropopt == TROPOPT_EST || opt->tropopt == TROPOPT_ESTG) + { + v[nv] -= (tropu[i]-tropu[j])-(tropr[i]-tropr[j]); + for (k = 0;k<(opt->tropoptionoopt != IONOOPT_IFLC) + { + v[nv] -= lami*x[IB_RTK(sat[i], f, opt)]-lamj*x[IB_RTK(sat[j], f, opt)]; + if (H) + { + Hi[IB_RTK(sat[i], f, opt)] = lami; + Hi[IB_RTK(sat[j], f, opt)] = -lamj; + } + } + else + { + v[nv] -= x[IB_RTK(sat[i], f, opt)]-x[IB_RTK(sat[j], f, opt)]; + if (H) + { + Hi[IB_RTK(sat[i], f, opt)] = 1.0; + Hi[IB_RTK(sat[j], f, opt)] = -1.0; + } + } + } + /* glonass receiver h/w bias term */ + if (rtk->opt.glomodear == 2 && sysi == SYS_GLO && sysj == SYS_GLO && ffopt, lami, lamj, f); + } + if (fssat[sat[j]-1].resc[f ] = v[nv]; + else rtk->ssat[sat[j]-1].resp[f-nf] = v[nv]; + + /* test innovation */ + if (opt->maxinno>0.0 && fabs(v[nv])>opt->maxinno) + { + if (fssat[sat[i]-1].rejc[f]++; + rtk->ssat[sat[j]-1].rejc[f]++; + } + errmsg(rtk, "outlier rejected (sat=%3d-%3d %s%d v=%.3f)\n", + sat[i], sat[j], fmode>PMODE_DGPS) + { + if (fssat[sat[i]-1].vsat[f] = rtk->ssat[sat[j]-1].vsat[f] = 1; + } + else + { + rtk->ssat[sat[i]-1].vsat[f-nf] = rtk->ssat[sat[j]-1].vsat[f-nf] = 1; + } + trace(4, "sat=%3d-%3d %s%d v=%13.3f R=%8.6f %8.6f\n", sat[i], + sat[j], fssat[j].resc[f]; + s/=nb[b]+1; + for (j=0;jssat[j].resc[f]!=0.0) rtk->ssat[j].resc[f]-=s; + } + } + else + { + for (j=0,s=0.0;jssat[j].resp[f-nf]; + s/=nb[b]+1; + for (j=0;jssat[j].resp[f-nf]!=0.0) + rtk->ssat[j].resp[f-nf]-=s; + } + } +#endif + b++; + } + /* end of system loop */ + + /* baseline length constraint for moving baseline */ + if (opt->mode == PMODE_MOVEB && constbl(rtk, x, P, v, H, Ri, Rj, nv)) + { + vflg[nv++] = 3<<4; + nb[b++]++; + } + if (H) {trace(5, "H=\n"); tracemat(5, H, rtk->nx, nv, 7, 4);} + + /* double-differenced measurement error covariance */ + ddcov(nb, b, Ri, Rj, nv, R); + + free(Ri); free(Rj); free(im); + free(tropu); free(tropr); free(dtdxu); free(dtdxr); + + return nv; +} + + +/* time-interpolation of residuals (for post-mission) ------------------------*/ +double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav, + rtk_t *rtk, double *y) +{ + static obsd_t obsb[MAXOBS]; + static double yb[MAXOBS*NFREQ*2], rs[MAXOBS*6], dts[MAXOBS*2], var[MAXOBS]; + static double e[MAXOBS*3], azel[MAXOBS*2]; + static int nb = 0, svh[MAXOBS*2]; + prcopt_t *opt = &rtk->opt; + double tt = timediff(time, obs[0].time), ttb, *p, *q; + int i, j, k, nf = NF_RTK(opt); + + trace(3, "intpres : n=%d tt=%.1f\n", n, tt); + + if (nb == 0 || fabs(tt)opt->maxtdiff*2.0 || ttb == tt) return tt; + + satposs(time, obsb, nb, nav, opt->sateph, rs, dts, var, svh); + + if (!zdres(1, obsb, nb, rs, dts, svh, nav, rtk->rb, opt, 1, yb, e, azel)) + { + return tt; + } + for (i = 0;i= nb) continue; + for (k = 0, p = y+i*nf*2, q = yb+j*nf*2;kfabs(tt) ? ttb : tt; +} + + +/* single to double-difference transformation matrix (D') --------------------*/ +int ddmat(rtk_t *rtk, double *D) +{ + int i, j, k, m, f, nb = 0, nx = rtk->nx, na = rtk->na, nf = NF_RTK(&rtk->opt), nofix; + + trace(3, "ddmat :\n"); + + for (i = 0;issat[i].fix[j] = 0; + } + for (i = 0;iopt.glomodear == 0) || (m == 3 && rtk->opt.bdsmodear == 0); + + for (f = 0, k = na;fx[i] == 0.0||!test_sys(rtk->ssat[i-k].sys,m)|| + !rtk->ssat[i-k].vsat[f]) + { +#else + if (rtk->x[i] == 0.0 || !test_sys(rtk->ssat[i-k].sys, m) || + !rtk->ssat[i-k].vsat[f] || !rtk->ssat[i-k].half[f]) + { +#endif + continue; + } + if (rtk->ssat[i-k].lock[f]>0 && !(rtk->ssat[i-k].slip[f]&2) && + rtk->ssat[i-k].azel[1] >= rtk->opt.elmaskar && !nofix) + { + rtk->ssat[i-k].fix[f] = 2; /* fix */ + break; + } + else rtk->ssat[i-k].fix[f] = 1; + } + for (j = k;jx[j] == 0.0 || !test_sys(rtk->ssat[j-k].sys, m) || + !rtk->ssat[j-k].vsat[f]) + { + continue; + } + if (rtk->ssat[j-k].lock[f]>0 && !(rtk->ssat[j-k].slip[f]&2) && + rtk->ssat[i-k].vsat[f] && + rtk->ssat[j-k].azel[1] >= rtk->opt.elmaskar && !nofix) + { + D[i+(na+nb)*nx] = 1.0; + D[j+(na+nb)*nx] = -1.0; + nb++; + rtk->ssat[j-k].fix[f] = 2; /* fix */ + } + else rtk->ssat[j-k].fix[f] = 1; + } + } + } + trace(5, "D=\n"); tracemat(5, D, nx, na+nb, 2, 0); + return nb; +} + + +/* restore single-differenced ambiguity --------------------------------------*/ +void restamb(rtk_t *rtk, const double *bias, int nb __attribute((unused)), double *xa) +{ + int i, n, m, f, index[MAXSAT], nv = 0, nf = NF_RTK(&rtk->opt); + + trace(3, "restamb :\n"); + + for (i = 0;inx;i++) xa[i] = rtk->x [i]; + for (i = 0;ina;i++) xa[i] = rtk->xa[i]; + + for (m = 0;m<4;m++) for (f = 0;fssat[i].sys, m) || rtk->ssat[i].fix[f] != 2) + { + continue; + } + index[n++] = IB_RTK(i+1, f, &rtk->opt); + } + if (n<2) continue; + + xa[index[0]] = rtk->x[index[0]]; + + for (i = 1;inx-rtk->na, nv = 0, nf = NF_RTK(&rtk->opt); + + trace(3, "holdamb :\n"); + + v = mat(nb, 1); H = zeros(nb, rtk->nx); + + for (m = 0;m<4;m++) for (f = 0;fssat[i].sys, m) || rtk->ssat[i].fix[f] != 2 || + rtk->ssat[i].azel[1]opt.elmaskhold) + { + continue; + } + index[n++] = IB_RTK(i+1, f, &rtk->opt); + rtk->ssat[i].fix[f] = 3; /* hold */ + } + /* constraint to fixed ambiguity */ + for (i = 1;ix[index[0]]-rtk->x[index[i]]); + + H[index[0]+nv*rtk->nx] = 1.0; + H[index[i]+nv*rtk->nx] = -1.0; + nv++; + } + } + if (nv>0) + { + R = zeros(nv, nv); + for (i = 0;ix, rtk->P, H, v, R, rtk->nx, nv))) + { + errmsg(rtk, "filter error (info=%d)\n", info); + } + free(R); + } + free(v); free(H); +} + + +/* resolve integer ambiguity by LAMBDA ---------------------------------------*/ +int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa) +{ + prcopt_t *opt = &rtk->opt; + int i, j, ny, nb, info, nx = rtk->nx, na = rtk->na; + double *D, *DP, *y, *Qy, *b, *db, *Qb, *Qab, *QQ, s[2]; + + trace(3, "resamb_LAMBDA : nx=%d\n", nx); + + rtk->sol.ratio = 0.0; + + if (rtk->opt.mode <= PMODE_DGPS || rtk->opt.modear == ARMODE_OFF || + rtk->opt.thresar[0]<1.0) + { + return 0; + } + /* single to double-difference transformation matrix (D') */ + D = zeros(nx, nx); + if ((nb = ddmat(rtk, D)) <= 0) + { + errmsg(rtk, "no valid double-difference\n"); + free(D); + return 0; + } + ny = na+nb; y = mat(ny, 1); Qy = mat(ny, ny); DP = mat(ny, nx); + b = mat(nb, 2); db = mat(nb, 1); Qb = mat(nb, nb); Qab = mat(na, nb); QQ = mat(na, nb); + + /* transform single to double-differenced phase-bias (y=D'*x, Qy=D'*P*D) */ + matmul("TN", ny, 1, nx, 1.0, D , rtk->x, 0.0, y ); + matmul("TN", ny, nx, nx, 1.0, D , rtk->P, 0.0, DP); + matmul("NN", ny, ny, nx, 1.0, DP, D , 0.0, Qy); + + /* phase-bias covariance (Qb) and real-parameters to bias covariance (Qab) */ + for (i = 0;ixa); + + /* covariance of fixed solution (Qa=Qa-Qab*Qb^-1*Qab') */ + matmul("NN", na, nb, nb, 1.0, Qab, Qb , 0.0, QQ); + matmul("NT", na, na, nb, -1.0, QQ , Qab, 1.0, rtk->Pa); + + trace(3, "resamb : validation ok (nb=%d ratio=%.2f s=%.2f/%.2f)\n", + nb, s[0] == 0.0 ? 0.0 : s[1]/s[0], s[0], s[1]); + + /* restore single-differenced ambiguity */ + restamb(rtk, bias, nb, xa); + } + else nb = 0; + } + else + { /* validation failed */ + errmsg(rtk, "ambiguity validation failed (nb=%d ratio=%.2f s=%.2f/%.2f)\n", + nb, s[1]/s[0], s[0], s[1]); + nb = 0; + } + } + else + { + errmsg(rtk, "lambda error (info=%d)\n", info); + } + free(D); free(y); free(Qy); free(DP); + free(b); free(db); free(Qb); free(Qab); free(QQ); + + return nb; /* number of ambiguities */ +} + + +/* validation of solution ----------------------------------------------------*/ +int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg, + int nv, double thres) +{ +#if 0 + prcopt_t *opt = &rtk->opt; + double vv = 0.0; +#endif + double fact = thres*thres; + int i, stat = 1, sat1, sat2, type, freq; + char stype; + + trace(3, "valpos : nv=%d thres=%.1f\n", nv, thres); + + /* post-fit residual test */ + for (i = 0;i>16)&0xFF; + sat2 = (vflg[i]>> 8)&0xFF; + type = (vflg[i]>> 4)&0xF; + freq = vflg[i]&0xF; + stype = type == 0 ? 'L' : (type == 1 ? 'L' : 'C'); + errmsg(rtk, "large residual (sat=%2d-%2d %s%d v=%6.3f sig=%.3f)\n", + sat1, sat2, stype, freq+1, v[i], std::sqrt(R[i+i*nv])); + } +#if 0 /* omitted v.2.4.0 */ + if (stat&&nv>NP(opt)) + { + /* chi-square validation */ + for (i = 0; i chisqr[nv-NP(opt)-1]) + { + errmsg(rtk,"residuals validation failed (nv=%d np=%d vv=%.2f cs=%.2f)\n", + nv, NP(opt), vv, chisqr[nv-NP(opt)-1]); + stat = 0; + } + else + { + trace(3,"valpos : validation ok (%s nv=%d np=%d vv=%.2f cs=%.2f)\n", + rtk->tstr, nv, NP(opt), vv, chisqr[nv-NP(opt)-1]); + } + } +#endif + return stat; +} + + +/* relative positioning ------------------------------------------------------*/ +int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, + const nav_t *nav) +{ + prcopt_t *opt = &rtk->opt; + gtime_t time = obs[0].time; + double *rs, *dts, *var, *y, *e, *azel, *v, *H, *R, *xp, *Pp, *xa, *bias, dt; + int i, j, f, n = nu+nr, ns, ny, nv, sat[MAXSAT], iu[MAXSAT], ir[MAXSAT], niter; + int info, vflg[MAXOBS*NFREQ*2+1], svh[MAXOBS*2]; + int stat = rtk->opt.mode <= PMODE_DGPS ? SOLQ_DGPS : SOLQ_FLOAT; + int nf = opt->ionoopt == IONOOPT_IFLC ? 1 : opt->nf; + + trace(3, "relpos : nx=%d nu=%d nr=%d\n", rtk->nx, nu, nr); + + dt = timediff(time, obs[nu].time); + + rs = mat(6, n); dts = mat(2, n); var = mat(1, n); y = mat(nf*2, n); e = mat(3, n); + azel = zeros(2, n); + + for (i = 0;issat[i].sys = satsys(i+1, NULL); + for (j = 0;jssat[i].vsat[j] = rtk->ssat[i].snr[j] = 0; + } + /* satellite positions/clocks */ + satposs(time, obs, n, nav, opt->sateph, rs, dts, var, svh); + + /* undifferenced residuals for base station */ + if (!zdres(1, obs+nu, nr, rs+nu*6, dts+nu*2, svh+nu, nav, rtk->rb, opt, 1, + y+nu*nf*2, e+nu*3, azel+nu*2)) + { + errmsg(rtk, "initial base station position error\n"); + + free(rs); free(dts); free(var); free(y); free(e); free(azel); + return 0; + } + /* time-interpolation of residuals (for post-processing) */ + if (opt->intpref) + { + dt = intpres(time, obs+nu, nr, nav, rtk, y+nu*nf*2); + } + /* select common satellites between rover and base-station */ + if ((ns = selsat(obs, azel, nu, nr, opt, sat, iu, ir)) <= 0) + { + errmsg(rtk, "no common satellite\n"); + + free(rs); free(dts); free(var); free(y); free(e); free(azel); + return 0; + } + /* temporal update of states */ + udstate(rtk, obs, sat, iu, ir, ns, nav); + + trace(4, "x(0)="); tracemat(4, rtk->x, 1, NR_RTK(opt), 13, 4); + + xp = mat(rtk->nx, 1); Pp = zeros(rtk->nx, rtk->nx); xa = mat(rtk->nx, 1); + matcpy(xp, rtk->x, rtk->nx, 1); + + ny = ns*nf*2+2; + v = mat(ny, 1); H = zeros(rtk->nx, ny); R = mat(ny, ny); bias = mat(rtk->nx, 1); + + /* add 2 iterations for baseline-constraint moving-base */ + niter = opt->niter+(opt->mode == PMODE_MOVEB && opt->baseline[0]>0.0 ? 2 : 0); + + for (i = 0;iP, rtk->nx, rtk->nx); + if ((info = filter(xp, Pp, H, v, R, rtk->nx, nv))) + { + errmsg(rtk, "filter error (info=%d)\n", info); + stat = SOLQ_NONE; + break; + } + trace(4, "x(%d)=", i+1); tracemat(4, xp, 1, NR_RTK(opt), 13, 4); + } + if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel)) + { + /* post-fit residuals for float solution */ + nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg); + + /* validation of float solution */ + if (valpos(rtk, v, R, vflg, nv, 4.0)) + { + /* update state and covariance matrix */ + matcpy(rtk->x, xp, rtk->nx, 1); + matcpy(rtk->P, Pp, rtk->nx, rtk->nx); + + /* update ambiguity control struct */ + rtk->sol.ns = 0; + for (i = 0;issat[sat[i]-1].vsat[f]) continue; + rtk->ssat[sat[i]-1].lock[f]++; + rtk->ssat[sat[i]-1].outc[f] = 0; + if (f == 0) rtk->sol.ns++; /* valid satellite count by L1 */ + } + /* lack of valid satellites */ + if (rtk->sol.ns<4) stat = SOLQ_NONE; + } + else stat = SOLQ_NONE; + } + /* resolve integer ambiguity by WL-NL */ + if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_WLNL) + { + if (resamb_WLNL(rtk, obs, sat, iu, ir, ns, nav, azel)) + { + stat = SOLQ_FIX; + } + } + /* resolve integer ambiguity by TCAR */ + else if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_TCAR) + { + if (resamb_TCAR(rtk, obs, sat, iu, ir, ns, nav, azel)) + { + stat = SOLQ_FIX; + } + } + /* resolve integer ambiguity by LAMBDA */ + else if (stat != SOLQ_NONE && resamb_LAMBDA(rtk, bias, xa)>1) + { + if (zdres(0, obs, nu, rs, dts, svh, nav, xa, opt, 0, y, e, azel)) + { + /* post-fit reisiduals for fixed solution */ + nv = ddres(rtk, nav, dt, xa, NULL, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg); + + /* validation of fixed solution */ + if (valpos(rtk, v, R, vflg, nv, 4.0)) + { + /* hold integer ambiguity */ + if (++rtk->nfix >= rtk->opt.minfix && + rtk->opt.modear == ARMODE_FIXHOLD) + { + holdamb(rtk, xa); + } + stat = SOLQ_FIX; + } + } + } + + /* save solution status */ + if (stat == SOLQ_FIX) + { + for (i = 0;i<3;i++) + { + rtk->sol.rr[i] = rtk->xa[i]; + rtk->sol.qr[i] = (float)rtk->Pa[i+i*rtk->na]; + } + rtk->sol.qr[3] = (float)rtk->Pa[1]; + rtk->sol.qr[4] = (float)rtk->Pa[1+2*rtk->na]; + rtk->sol.qr[5] = (float)rtk->Pa[2]; + } + else + { + for (i = 0;i<3;i++) + { + rtk->sol.rr[i] = rtk->x[i]; + rtk->sol.qr[i] = (float)rtk->P[i+i*rtk->nx]; + } + rtk->sol.qr[3] = (float)rtk->P[1]; + rtk->sol.qr[4] = (float)rtk->P[1+2*rtk->nx]; + rtk->sol.qr[5] = (float)rtk->P[2]; + rtk->nfix = 0; + } + for (i = 0;issat[obs[i].sat-1].pt[obs[i].rcv-1][j] = obs[i].time; + rtk->ssat[obs[i].sat-1].ph[obs[i].rcv-1][j] = obs[i].L[j]; + } + for (i = 0;issat[sat[i]-1].snr[j] = obs[iu[i]].SNR[j]; + } + for (i = 0;issat[i].fix[j] == 2 && stat != SOLQ_FIX) rtk->ssat[i].fix[j] = 1; + if (rtk->ssat[i].slip[j]&1) rtk->ssat[i].slipc[j]++; + } + free(rs); free(dts); free(var); free(y); free(e); free(azel); + free(xp); free(Pp); free(xa); free(v); free(H); free(R); free(bias); + + if (stat != SOLQ_NONE) rtk->sol.stat = stat; + + return stat != SOLQ_NONE; +} + + +/* initialize rtk control ------------------------------------------------------ + * initialize rtk control struct + * args : rtk_t *rtk IO rtk control/result struct + * prcopt_t *opt I positioning options (see rtklib.h) + * return : none + *-----------------------------------------------------------------------------*/ +void rtkinit(rtk_t *rtk, const prcopt_t *opt) +{ + sol_t sol0 = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; + ambc_t ambc0 = {{ {0, 0}, {0, 0}, {0, 0}, {0, 0} }, {}, {}, {} , 0, {}}; + ssat_t ssat0 = {0, 0, {0.0}, {0.0}, {0.0}, {'0'}, {'0'}, {'0'}, {'0'}, {'0'}, {}, {}, {}, {}, 0.0, 0.0, 0.0, 0.0, {{0,0},{0,0}}, {{},{}}}; + int i; + + trace(3, "rtkinit :\n"); + + rtk->sol = sol0; + for (i = 0;i<6;i++) rtk->rb[i] = 0.0; + rtk->nx = opt->mode <= PMODE_FIXED ? NX_RTK(opt) : pppnx(opt); + rtk->na = opt->mode <= PMODE_FIXED ? NR_RTK(opt) : pppnx(opt); + rtk->tt = 0.0; + rtk->x = zeros(rtk->nx, 1); + rtk->P = zeros(rtk->nx, rtk->nx); + rtk->xa = zeros(rtk->na, 1); + rtk->Pa = zeros(rtk->na, rtk->na); + rtk->nfix = rtk->neb = 0; + for (i = 0;iambc[i] = ambc0; + rtk->ssat[i] = ssat0; + } + for (i = 0;ierrbuf[i] = 0; + rtk->opt = *opt; +} + + +/* free rtk control ------------------------------------------------------------ + * free memory for rtk control struct + * args : rtk_t *rtk IO rtk control/result struct + * return : none + *-----------------------------------------------------------------------------*/ +void rtkfree(rtk_t *rtk) +{ + trace(3, "rtkfree :\n"); + + rtk->nx = rtk->na = 0; + free(rtk->x ); rtk->x = NULL; + free(rtk->P ); rtk->P = NULL; + free(rtk->xa); rtk->xa = NULL; + free(rtk->Pa); rtk->Pa = NULL; +} + + +/* precise positioning --------------------------------------------------------- + * input observation data and navigation message, compute rover position by + * precise positioning + * args : rtk_t *rtk IO rtk control/result struct + * rtk->sol IO solution + * .time O solution time + * .rr[] IO rover position/velocity + * (I:fixed mode,O:single mode) + * .dtr[0] O receiver clock bias (s) + * .dtr[1] O receiver glonass-gps time offset (s) + * .Qr[] O rover position covarinace + * .stat O solution status (SOLQ_???) + * .ns O number of valid satellites + * .age O age of differential (s) + * .ratio O ratio factor for ambiguity validation + * rtk->rb[] IO base station position/velocity + * (I:relative mode,O:moving-base mode) + * rtk->nx I number of all states + * rtk->na I number of integer states + * rtk->ns O number of valid satellite + * rtk->tt O time difference between current and previous (s) + * rtk->x[] IO float states pre-filter and post-filter + * rtk->P[] IO float covariance pre-filter and post-filter + * rtk->xa[] O fixed states after AR + * rtk->Pa[] O fixed covariance after AR + * rtk->ssat[s] IO sat(s+1) status + * .sys O system (SYS_???) + * .az [r] O azimuth angle (rad) (r=0:rover,1:base) + * .el [r] O elevation angle (rad) (r=0:rover,1:base) + * .vs [r] O data valid single (r=0:rover,1:base) + * .resp [f] O freq(f+1) pseudorange residual (m) + * .resc [f] O freq(f+1) carrier-phase residual (m) + * .vsat [f] O freq(f+1) data vaild (0:invalid,1:valid) + * .fix [f] O freq(f+1) ambiguity flag + * (0:nodata,1:float,2:fix,3:hold) + * .slip [f] O freq(f+1) slip flag + * (bit8-7:rcv1 LLI, bit6-5:rcv2 LLI, + * bit2:parity unknown, bit1:slip) + * .lock [f] IO freq(f+1) carrier lock count + * .outc [f] IO freq(f+1) carrier outage count + * .slipc[f] IO freq(f+1) cycle slip count + * .rejc [f] IO freq(f+1) data reject count + * .gf IO geometry-free phase (L1-L2) (m) + * .gf2 IO geometry-free phase (L1-L5) (m) + * rtk->nfix IO number of continuous fixes of ambiguity + * rtk->neb IO bytes of error message buffer + * rtk->errbuf IO error message buffer + * rtk->tstr O time string for debug + * rtk->opt I processing options + * obsd_t *obs I observation data for an epoch + * obs[i].rcv=1:rover,2:reference + * sorted by receiver and satellte + * int n I number of observation data + * nav_t *nav I navigation messages + * return : status (0:no solution,1:valid solution) + * notes : before calling function, base station position rtk->sol.rb[] should + * be properly set for relative mode except for moving-baseline + *-----------------------------------------------------------------------------*/ +int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) +{ + prcopt_t *opt = &rtk->opt; + sol_t solb = { {0, 0}, {}, {}, {}, '0', '0', '0', 0.0, 0.0, 0.0}; + gtime_t time; + int i, nu, nr; + char msg[128] = ""; + + trace(3, "rtkpos : time=%s n=%d\n", time_str(obs[0].time, 3), n); + trace(4, "obs=\n"); traceobs(4, obs, n); + /*trace(5,"nav=\n"); tracenav(5,nav);*/ + + /* set base staion position */ + if (opt->refpos <= POSOPT_RINEX && opt->mode != PMODE_SINGLE && + opt->mode != PMODE_MOVEB) + { + for (i = 0;i<6;i++) rtk->rb[i] = i < 3 ? opt->rb[i] : 0.0; + } + /* count rover/base station observations */ + for (nu = 0;nu sol.time; /* previous epoch */ + + /* rover position by single point positioning */ + if (!pntpos(obs, nu, nav, &rtk->opt, &rtk->sol, NULL, rtk->ssat, msg)) + { + errmsg(rtk, "point pos error (%s)\n", msg); + if (!rtk->opt.dynamics) + { + outsolstat(rtk); + return 0; + } + } + if (time.time != 0) rtk->tt = timediff(rtk->sol.time, time); + + /* single point positioning */ + if (opt->mode == PMODE_SINGLE) + { + outsolstat(rtk); + return 1; + } + /* suppress output of single solution */ + if (!opt->outsingle) + { + rtk->sol.stat = SOLQ_NONE; + } + /* precise point positioning */ + if (opt->mode >= PMODE_PPP_KINEMA) + { + pppos(rtk, obs, nu, nav); + outsolstat(rtk); + return 1; + } + /* check number of data of base station and age of differential */ + if (nr == 0) + { + errmsg(rtk, "no base station observation data for rtk\n"); + outsolstat(rtk); + return 1; + } + if (opt->mode == PMODE_MOVEB) + { /* moving baseline */ + + /* estimate position/velocity of base station */ + if (!pntpos(obs+nu, nr, nav, &rtk->opt, &solb, NULL, NULL, msg)) + { + errmsg(rtk, "base station position error (%s)\n", msg); + return 0; + } + rtk->sol.age = (float)timediff(rtk->sol.time, solb.time); + + if (fabs(rtk->sol.age)>TTOL_MOVEB) + { + errmsg(rtk, "time sync error for moving-base (age=%.1f)\n", rtk->sol.age); + return 0; + } + for (i = 0;i<6;i++) rtk->rb[i] = solb.rr[i]; + + /* time-synchronized position of base station */ + for (i = 0;i<3;i++) rtk->rb[i] += rtk->rb[i+3]*rtk->sol.age; + } + else + { + rtk->sol.age = (float)timediff(obs[0].time, obs[nu].time); + + if (fabs(rtk->sol.age) > opt->maxtdiff) + { + errmsg(rtk, "age of differential error (age=%.1f)\n", rtk->sol.age); + outsolstat(rtk); + return 1; + } + } + /* relative potitioning */ + relpos(rtk, obs, nu, nr, nav); + outsolstat(rtk); + + return 1; +} diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.h b/src/algorithms/libs/rtklib/rtklib_rtkpos.h new file mode 100644 index 000000000..82640eca9 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.h @@ -0,0 +1,202 @@ +/*! + * \file rtklib_rtkpos.h + * \brief rtklib ppp-related functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_RKTPOS_H_ +#define GNSS_SDR_RTKLIB_RKTPOS_H_ + +#include "rtklib.h" +#include "rtklib_rtkcmn.h" + +/* constants/macros ----------------------------------------------------------*/ +const double VAR_POS = std::pow(30.0, 2.0); /* initial variance of receiver pos (m^2) */ +const double VAR_VEL = std::pow(10.0, 2.0); /* initial variance of receiver vel ((m/s)^2) */ +const double VAR_ACC = std::pow(10.0, 2.0); /* initial variance of receiver acc ((m/ss)^2) */ +const double VAR_HWBIAS = std::pow(1.0, 2.0); /* initial variance of h/w bias ((m/MHz)^2) */ +const double VAR_GRA = std::pow(0.001, 2.0); /* initial variance of gradient (m^2) */ +const double INIT_ZWD = 0.15; /* initial zwd (m) */ + +const double PRN_HWBIA = 1E-6; /* process noise of h/w bias (m/MHz/sqrt(s)) */ +const double MAXAC = 30.0; /* max accel for doppler slip detection (m/s^2) */ + +const double VAR_HOLDAMB = 0.001; /* constraint to hold ambiguity (cycle^2) */ + +const double TTOL_MOVEB = (1.0+2*DTTOL); +/* time sync tolerance for moving-baseline (s) */ + +/* number of parameters (pos,ionos,tropos,hw-bias,phase-bias,real,estimated) */ +#define NF_RTK(opt) ((opt)->ionoopt==IONOOPT_IFLC?1:(opt)->nf) +#define NP_RTK(opt) ((opt)->dynamics==0?3:9) +#define NI_RTK(opt) ((opt)->ionoopt!=IONOOPT_EST?0:MAXSAT) +#define NT_RTK(opt) ((opt)->tropopttropoptglomodear!=2?0:NFREQGLO) +#define NB_RTK(opt) ((opt)->mode<=PMODE_DGPS?0:MAXSAT*NF_RTK(opt)) +#define NR_RTK(opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)+NL_RTK(opt)) +#define NX_RTK(opt) (NR_RTK(opt)+NB_RTK(opt)) + +/* state variable index */ +#define II_RTK(s,opt) (NP_RTK(opt)+(s)-1) /* ionos (s:satellite no) */ +#define IT_RTK(r,opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)/2*(r)) /* tropos (r:0=rov,1:ref) */ +#define IL_RTK(f,opt) (NP_RTK(opt)+NI_RTK(opt)+NT_RTK(opt)+(f)) /* receiver h/w bias */ +#define IB_RTK(s,f,opt) (NR_RTK(opt)+MAXSAT*(f)+(s)-1) /* phase bias (s:satno,f:freq) */ + +int rtkopenstat(const char *file, int level); + +void rtkclosestat(void); + +void rtkoutstat(rtk_t *rtk); + +void swapsolstat(void); + +void outsolstat(rtk_t *rtk); + +void errmsg(rtk_t *rtk, const char *format, ...); + +double sdobs(const obsd_t *obs, int i, int j, int f); + +double gfobs_L1L2(const obsd_t *obs, int i, int j, const double *lam); + +double gfobs_L1L5(const obsd_t *obs, int i, int j, const double *lam); + +double varerr(int sat, int sys, double el, double bl, double dt, int f, + const prcopt_t *opt); + + +double baseline(const double *ru, const double *rb, double *dr); + +void initx_rtk(rtk_t *rtk, double xi, double var, int i); + +int selsat(const obsd_t *obs, double *azel, int nu, int nr, + const prcopt_t *opt, int *sat, int *iu, int *ir); + +void udpos(rtk_t *rtk, double tt); + +void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns); + +void udtrop(rtk_t *rtk, double tt, double bl); + +void udrcvbias(rtk_t *rtk, double tt); + +void detslp_ll(rtk_t *rtk, const obsd_t *obs, int i, int rcv); +void detslp_gf_L1L2(rtk_t *rtk, const obsd_t *obs, int i, int j, + const nav_t *nav); + +void detslp_gf_L1L5(rtk_t *rtk, const obsd_t *obs, int i, int j, + const nav_t *nav); + +void detslp_dop(rtk_t *rtk, const obsd_t *obs, int i, int rcv, + const nav_t *nav); + +void udbias(rtk_t *rtk, double tt, const obsd_t *obs, const int *sat, + const int *iu, const int *ir, int ns, const nav_t *nav); + +void udstate(rtk_t *rtk, const obsd_t *obs, const int *sat, + const int *iu, const int *ir, int ns, const nav_t *nav); + +void zdres_sat(int base, double r, const obsd_t *obs, const nav_t *nav, + const double *azel, const double *dant, + const prcopt_t *opt, double *y); + +int zdres(int base, const obsd_t *obs, int n, const double *rs, + const double *dts, const int *svh, const nav_t *nav, + const double *rr, const prcopt_t *opt, int index, double *y, + double *e, double *azel); + +int validobs(int i, int j, int f, int nf, double *y); + +void ddcov(const int *nb, int n, const double *Ri, const double *Rj, + int nv, double *R); + +int constbl(rtk_t *rtk, const double *x, const double *P, double *v, + double *H, double *Ri, double *Rj, int index); + +double prectrop(gtime_t time, const double *pos, int r, + const double *azel, const prcopt_t *opt, const double *x, + double *dtdx); + +double gloicbcorr(int sat1, int sat2, const prcopt_t *opt, double lam1, + double lam2, int f); + +int test_sys(int sys, int m); + +int ddres(rtk_t *rtk, const nav_t *nav, double dt, const double *x, + const double *P, const int *sat, double *y, double *e, + double *azel, const int *iu, const int *ir, int ns, double *v, + double *H, double *R, int *vflg); + +double intpres(gtime_t time, const obsd_t *obs, int n, const nav_t *nav, + rtk_t *rtk, double *y); + + +int ddmat(rtk_t *rtk, double *D); + +void restamb(rtk_t *rtk, const double *bias, int nb, double *xa); + +void holdamb(rtk_t *rtk, const double *xa); + +int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa); + +int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg, + int nv, double thres); + +int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, + const nav_t *nav); + +void rtkinit(rtk_t *rtk, const prcopt_t *opt); + +void rtkfree(rtk_t *rtk); + +int rtkpos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav); + + + +#endif diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.cc b/src/algorithms/libs/rtklib/rtklib_sbas.cc new file mode 100644 index 000000000..ad081128f --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_sbas.cc @@ -0,0 +1,1006 @@ +/*! + * \file rtklib_sbas.cc + * \brief sbas functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * References : + * [1] RTCA/DO-229C, Minimum operational performanc standards for global + * positioning system/wide area augmentation system airborne equipment, + * RTCA inc, November 28, 2001 + * [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service + * Interface Specification for QZSS, Japan Aerospace Exploration Agency, + * July 31, 2009 + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_sbas.h" +#include "rtklib_rtkcmn.h" + +/* extract field from line ---------------------------------------------------*/ +char *getfield(char *p, int pos) +{ + for (pos--; pos > 0 ; pos--, p++) if (!(p = strchr(p, ','))) return NULL; + return p; +} + + +/* variance of fast correction (udre=UDRE+1) ---------------------------------*/ +double varfcorr(int udre) +{ + const double var[14] = { + 0.052, 0.0924, 0.1444, 0.283, 0.4678, 0.8315, 1.2992, 1.8709, 2.5465, 3.326, + 5.1968, 20.7870, 230.9661, 2078.695 + }; + return 0 < udre && udre <= 14 ? var[udre-1] : 0.0; +} + + +/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/ +double varicorr(int give) +{ + const double var[15] = { + 0.0084, 0.0333, 0.0749, 0.1331, 0.2079, 0.2994, 0.4075, 0.5322, 0.6735, 0.8315, + 1.1974, 1.8709, 3.326, 20.787, 187.0826 + }; + return 0 < give && give <= 15 ? var[give-1] : 0.0; +} + + +/* fast correction degradation -----------------------------------------------*/ +double degfcorr(int ai) +{ + const double degf[16] = { + 0.00000, 0.00005, 0.00009, 0.00012, 0.00015, 0.00020, 0.00030, 0.00045, + 0.00060, 0.00090, 0.00150, 0.00210, 0.00270, 0.00330, 0.00460, 0.00580 + }; + return 0 < ai && ai <= 15 ? degf[ai] : 0.0058; +} + + +/* decode type 1: prn masks --------------------------------------------------*/ +int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i, n, sat; + + trace(4, "decode_sbstype1:\n"); + + for (i = 1, n = 0;i<=210 && nmsg, 13+i, 1)) + { + if (i<= 37) sat = satno(SYS_GPS, i); /* 0- 37: gps */ + else if (i<= 61) sat = satno(SYS_GLO, i-37); /* 38- 61: glonass */ + else if (i<=119) sat = 0; /* 62-119: future gnss */ + else if (i<=138) sat = satno(SYS_SBS, i); /* 120-138: geo/waas */ + else if (i<=182) sat = 0; /* 139-182: reserved */ + else if (i<=192) sat = satno(SYS_SBS, i+10); /* 183-192: qzss ref [2] */ + else if (i<=202) sat = satno(SYS_QZS, i); /* 193-202: qzss ref [2] */ + else sat = 0; /* 203- : reserved */ + sbssat->sat[n++].sat = sat; + } + } + sbssat->iodp = getbitu(msg->msg, 224, 2); + sbssat->nsat = n; + + trace(5, "decode_sbstype1: nprn=%d iodp=%d\n", n, sbssat->iodp); + return 1; +} + + +/* decode type 2-5,0: fast corrections ---------------------------------------*/ +int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i, j, iodf, type, udre; + double prc, dt; + gtime_t t0; + + trace(4, "decode_sbstype2:\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg, 16, 2)) return 0; + + type = getbitu(msg->msg, 8, 6); + iodf = getbitu(msg->msg, 14, 2); + + for (i = 0; i < 13; i++) + { + if ((j = 13 * ((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat) break; + udre = getbitu(msg->msg, 174+4*i, 4); + t0 = sbssat->sat[j].fcorr.t0; + prc = sbssat->sat[j].fcorr.prc; + sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); + sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i * 12, 12) * 0.125f; + sbssat->sat[j].fcorr.udre = udre + 1; + dt = timediff(sbssat->sat[j].fcorr.t0, t0); + if (t0.time == 0||dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0) + { + sbssat->sat[j].fcorr.rrc = 0.0; + sbssat->sat[j].fcorr.dt = 0.0; + } + else + { + sbssat->sat[j].fcorr.rrc = (sbssat->sat[j].fcorr.prc-prc) / dt; + sbssat->sat[j].fcorr.dt = dt; + } + sbssat->sat[j].fcorr.iodf = iodf; + } + trace(5, "decode_sbstype2: type=%d iodf=%d\n", type, iodf); + return 1; +} + + +/* decode type 6: integrity info ---------------------------------------------*/ +int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i, iodf[4], udre; + + trace(4, "decode_sbstype6:\n"); + + for (i = 0;i<4;i++) + { + iodf[i] = getbitu(msg->msg, 14+i*2, 2); + } + for (i = 0;insat && isat[i].fcorr.iodf != iodf[i/13]) continue; + udre = getbitu(msg->msg, 22+i*4, 4); + sbssat->sat[i].fcorr.udre = udre+1; + } + trace(5, "decode_sbstype6: iodf=%d %d %d %d\n", iodf[0], iodf[1], iodf[2], iodf[3]); + return 1; +} + + +/* decode type 7: fast correction degradation factor -------------------------*/ +int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i; + + trace(4, "decode_sbstype7\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg, 18, 2)) return 0; + + sbssat->tlat = getbitu(msg->msg, 14, 4); + + for (i = 0;insat && isat[i].fcorr.ai = getbitu(msg->msg, 22+i*4, 4); + } + return 1; +} + + +/* decode type 9: geo navigation message -------------------------------------*/ +int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav) +{ + seph_t seph = { 0, {0,0}, {0,0}, 0, 0, {}, {}, {}, 0.0, 0.0 }; + int i, sat, t; + + trace(4, "decode_sbstype9:\n"); + + if (!(sat = satno(SYS_SBS, msg->prn))) + { + trace(2, "invalid prn in sbas type 9: prn=%3d\n", msg->prn); + return 0; + } + t = (int)getbitu(msg->msg, 22, 13)*16-(int)msg->tow%86400; + if (t<=-43200) t+=86400; + else if (t> 43200) t-=86400; + seph.sat = sat; + seph.t0 = gpst2time(msg->week, msg->tow+t); + seph.tof = gpst2time(msg->week, msg->tow); + seph.sva = getbitu(msg->msg, 35, 4); + seph.svh = seph.sva == 15 ? 1 : 0; /* unhealthy if ura==15 */ + + seph.pos[0] = getbits(msg->msg, 39, 30)*0.08; + seph.pos[1] = getbits(msg->msg, 69, 30)*0.08; + seph.pos[2] = getbits(msg->msg, 99, 25)*0.4; + seph.vel[0] = getbits(msg->msg, 124, 17)*0.000625; + seph.vel[1] = getbits(msg->msg, 141, 17)*0.000625; + seph.vel[2] = getbits(msg->msg, 158, 18)*0.004; + seph.acc[0] = getbits(msg->msg, 176, 10)*0.0000125; + seph.acc[1] = getbits(msg->msg, 186, 10)*0.0000125; + seph.acc[2] = getbits(msg->msg, 196, 10)*0.0000625; + + seph.af0 = getbits(msg->msg, 206, 12)*TWO_N31; + seph.af1 = getbits(msg->msg, 218, 8)*TWO_N39/2.0; + + i = msg->prn-MINPRNSBS; + if (!nav->seph||fabs(timediff(nav->seph[i].t0, seph.t0))<1e-3) + { /* not change */ + return 0; + } + nav->seph[NSATSBS+i] = nav->seph[i]; /* previous */ + nav->seph[i] = seph; /* current */ + + trace(5, "decode_sbstype9: prn=%d\n", msg->prn); + return 1; +} + + +/* decode type 18: ionospheric grid point masks ------------------------------*/ +int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion) +{ + const sbsigpband_t *p; + int i, j, n, m, band = getbitu(msg->msg, 18, 4); + + trace(4, "decode_sbstype18:\n"); + + if (0<=band && band<= 8) {p = igpband1[band ]; m = 8;} + else if (9<=band && band<=10) {p = igpband2[band-9]; m = 5;} + else return 0; + + sbsion[band].iodi = (short)getbitu(msg->msg, 22, 2); + + for (i = 1, n = 0; i<= 201;i++) + { + if (!getbitu(msg->msg , 23 + i, 1)) continue; + for (j = 0; j < m; j++) + { + if (i < p[j].bits || p[j].bite < i) continue; + sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x; + sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits]; + break; + } + } + sbsion[band].nigp = n; + + trace(5, "decode_sbstype18: band=%d nigp=%d\n", band, n); + return 1; +} + + +/* decode half long term correction (vel code=0) -----------------------------*/ +int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat) +{ + int i, n = getbitu(msg->msg, p, 6); + + trace(4, "decode_longcorr0:\n"); + + if (n == 0||n>MAXSAT) return 0; + + sbssat->sat[n-1].lcorr.iode = getbitu(msg->msg, p+6, 8); + + for (i = 0;i<3;i++) + { + sbssat->sat[n-1].lcorr.dpos[i] = getbits(msg->msg, p+14+9*i, 9)*0.125; + sbssat->sat[n-1].lcorr.dvel[i] = 0.0; + } + sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p + 41, 10) * TWO_N31; + sbssat->sat[n-1].lcorr.daf1 = 0.0; + sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow); + + trace(5, "decode_longcorr0:sat=%2d\n", sbssat->sat[n-1].sat); + return 1; +} + + +/* decode half long term correction (vel code=1) -----------------------------*/ +int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat) +{ + int i, n = getbitu(msg->msg, p, 6), t; + + trace(4, "decode_longcorr1:\n"); + + if (n == 0||n>MAXSAT) return 0; + + sbssat->sat[n-1].lcorr.iode = getbitu(msg->msg, p+6, 8); + + for (i = 0;i<3;i++) + { + sbssat->sat[n-1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + i * 11, 11) * 0.125; + sbssat->sat[n-1].lcorr.dvel[i] = getbits(msg->msg, p + 58 + i * 8, 8) * TWO_N11; + } + sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p+47, 11) * TWO_N31; + sbssat->sat[n-1].lcorr.daf1 = getbits(msg->msg, p+82, 8) * TWO_N39; + t = (int)getbitu(msg->msg, p+90, 13)*16-(int)msg->tow%86400; + if (t<=-43200) t+=86400; + else if (t> 43200) t-=86400; + sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow+t); + + trace(5, "decode_longcorr1: sat=%2d\n", sbssat->sat[n-1].sat); + return 1; +} + + +/* decode half long term correction ------------------------------------------*/ +int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat) +{ + trace(4, "decode_longcorrh:\n"); + + if (getbitu(msg->msg, p, 1) == 0) { /* vel code=0 */ + if (sbssat->iodp == (int)getbitu(msg->msg, p+103, 2)) + { + return decode_longcorr0(msg, p+ 1, sbssat) && + decode_longcorr0(msg, p+52, sbssat); + } + } + else if (sbssat->iodp == (int)getbitu(msg->msg, p+104, 2)) + { + return decode_longcorr1(msg, p+1, sbssat); + } + return 0; +} + + +/* decode type 24: mixed fast/long term correction ---------------------------*/ +int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + int i, j, iodf, blk, udre; + + trace(4, "decode_sbstype24:\n"); + + if (sbssat->iodp!=(int)getbitu(msg->msg, 110, 2)) return 0; /* check IODP */ + + blk = getbitu(msg->msg, 112, 2); + iodf = getbitu(msg->msg, 114, 2); + + for (i = 0;i<6;i++) + { + if ((j = 13*blk+i)>=sbssat->nsat) break; + udre = getbitu(msg->msg, 86+4*i, 4); + + sbssat->sat[j].fcorr.t0 = gpst2time(msg->week, msg->tow); + sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14+i*12, 12)*0.125f; + sbssat->sat[j].fcorr.udre = udre+1; + sbssat->sat[j].fcorr.iodf = iodf; + } + return decode_longcorrh(msg, 120, sbssat); +} + + +/* decode type 25: long term satellite error correction ----------------------*/ +int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat) +{ + trace(4, "decode_sbstype25:\n"); + + return decode_longcorrh(msg, 14, sbssat) && decode_longcorrh(msg, 120, sbssat); +} + + +/* decode type 26: ionospheric deley corrections -----------------------------*/ +int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion) +{ + int i, j, block, delay, give, band = getbitu(msg->msg, 14, 4); + + trace(4, "decode_sbstype26:\n"); + + if (band>MAXBAND || sbsion[band].iodi != (int)getbitu(msg->msg, 217, 2)) return 0; + + block = getbitu(msg->msg, 18, 4); + + for (i = 0;i<15;i++) + { + if ((j = block*15+i) >= sbsion[band].nigp) continue; + give = getbitu(msg->msg, 22+i*13+9, 4); + + delay = getbitu(msg->msg, 22+i*13, 9); + sbsion[band].igp[j].t0 = gpst2time(msg->week, msg->tow); + sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0f : delay * 0.125f; + sbsion[band].igp[j].give = give+1; + + if (sbsion[band].igp[j].give>=16) + { + sbsion[band].igp[j].give = 0; + } + } + trace(5, "decode_sbstype26: band=%d block=%d\n", band, block); + return 1; +} + + +/* update sbas corrections ----------------------------------------------------- + * update sbas correction parameters in navigation data with a sbas message + * args : sbsmg_t *msg I sbas message + * nav_t *nav IO navigation data + * return : message type (-1: error or not supported type) + * notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) + * seph[prn-MINPRNSBS+1] : sat prn current epehmeris + * seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris + *-----------------------------------------------------------------------------*/ +int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav) +{ + int type = getbitu(msg->msg, 8, 6), stat = -1; + + trace(3, "sbsupdatecorr: type=%d\n", type); + + if (msg->week == 0) return -1; + + switch (type) + { + case 0: stat = decode_sbstype2 (msg, &nav->sbssat); break; + case 1: stat = decode_sbstype1 (msg, &nav->sbssat); break; + case 2: + case 3: + case 4: + case 5: stat = decode_sbstype2 (msg, &nav->sbssat); break; + case 6: stat = decode_sbstype6 (msg, &nav->sbssat); break; + case 7: stat = decode_sbstype7 (msg, &nav->sbssat); break; + case 9: stat = decode_sbstype9 (msg, nav); break; + case 18: stat = decode_sbstype18(msg, nav ->sbsion); break; + case 24: stat = decode_sbstype24(msg, &nav->sbssat); break; + case 25: stat = decode_sbstype25(msg, &nav->sbssat); break; + case 26: stat = decode_sbstype26(msg, nav ->sbsion); break; + case 63: break; /* null message */ + + /*default: trace(2, "unsupported sbas message: type=%d\n", type); break;*/ + } + return stat ? type : -1; +} + + +/* read sbas log file --------------------------------------------------------*/ +void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te, + sbs_t *sbs) +{ + sbsmsg_t *sbs_msgs; + int i, week, prn, ch, msg; + unsigned int b; + double tow, ep[6] = {}; + char buff[256], *p; + gtime_t time; + FILE *fp; + + trace(3, "readmsgs: file=%s sel=%d\n", file, sel); + + if (!(fp = fopen(file, "r"))) + { + trace(2, "sbas message file open error: %s\n", file); + return; + } + while (fgets(buff, sizeof(buff), fp)) + { + if (sscanf(buff, "%d %lf %d", &week, &tow, &prn) == 3 && (p = strstr(buff, ": "))) + { + p+=2; /* rtklib form */ + } + else if (sscanf(buff, "%d %lf %lf %lf %lf %lf %lf %d", + &prn, ep, ep+1, ep+2, ep+3, ep+4, ep+5, &msg) == 8) + { + /* ems (EGNOS Message Service) form */ + ep[0] += ep[0] < 70.0 ? 2000.0 : 1900.0; + tow = time2gpst(epoch2time(ep), &week); + p = buff + (msg >= 10 ? 25 : 24); + } + else if (!strncmp(buff,"#RAWWAASFRAMEA",14)) + { /* NovAtel OEM4/V */ + if (!(p = getfield(buff,6))) continue; + if (sscanf(p,"%d,%lf", &week, &tow) < 2) continue; + if (!(p = strchr(++p, ';'))) continue; + if (sscanf(++p, "%d,%d", &ch, &prn) < 2) continue; + if (!(p = getfield(p, 4))) continue; + } + else if (!strncmp(buff,"$FRMA",5)) + { /* NovAtel OEM3 */ + if (!(p = getfield(buff,2))) continue; + if (sscanf(p, "%d,%lf,%d", &week, &tow, &prn) < 3) continue; + if (!(p = getfield(p, 6))) continue; + if (week < WEEKOFFSET) week += WEEKOFFSET; + } + else continue; + + if (sel != 0 && sel != prn) continue; + + time = gpst2time(week, tow); + + if (!screent(time, ts,te,0.0)) continue; + + if (sbs->n>=sbs->nmax) + { + sbs->nmax = sbs->nmax == 0 ? 1024 : sbs->nmax * 2; + if (!(sbs_msgs = (sbsmsg_t *)realloc(sbs->msgs, sbs->nmax * sizeof(sbsmsg_t)))) + { + trace(1, "readsbsmsg malloc error: nmax=%d\n", sbs->nmax); + free(sbs->msgs); sbs->msgs = NULL; sbs->n = sbs->nmax = 0; + return; + } + sbs->msgs = sbs_msgs; + } + sbs->msgs[sbs->n].week = week; + sbs->msgs[sbs->n].tow = (int)(tow + 0.5); + sbs->msgs[sbs->n].prn = prn; + for (i = 0; i < 29; i++) sbs->msgs[sbs->n].msg[i] = 0; + for (i = 0; *(p-1) && *p && i < 29; p += 2, i++) + { + if (sscanf(p, "%2X", &b) == 1) sbs->msgs[sbs->n].msg[i] = (unsigned char)b; + } + sbs->msgs[sbs->n++].msg[28] &= 0xC0; + } + fclose(fp); +} + + +/* compare sbas messages -----------------------------------------------------*/ +int cmpmsgs(const void *p1, const void *p2) +{ + sbsmsg_t *q1 = (sbsmsg_t *)p1,*q2 = (sbsmsg_t *)p2; + return q1->week != q2->week ? q1->week-q2->week : + (q1->tow < q2->tow ? -1 : (q1->tow > q2->tow ? 1 : q1->prn - q2->prn)); +} + + +/* read sbas message file ------------------------------------------------------ + * read sbas message file + * args : char *file I sbas message file (wind-card * is expanded) + * int sel I sbas satellite prn number selection (0:all) + * (gtime_t ts I start time) + * (gtime_t te I end time ) + * sbs_t *sbs IO sbas messages + * return : number of sbas messages + * notes : sbas message are appended and sorted. before calling the funciton, + * sbs->n, sbs->nmax and sbs->msgs must be set properly. (initially + * sbs->n=sbs->nmax=0, sbs->msgs=NULL) + * only the following file extentions after wild card expanded are valid + * to read. others are skipped + * .sbs, .SBS, .ems, .EMS + *-----------------------------------------------------------------------------*/ +int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te, + sbs_t *sbs) +{ + char *efiles[MAXEXFILE] = {},*ext; + int i,n; + + trace(3,"sbsreadmsgt: file=%s sel=%d\n",file,sel); + + for (i = 0; i < MAXEXFILE ; i++) + { + if (!(efiles[i] = (char *)malloc(1024))) + { + for (i--; i >= 0; i--) free(efiles[i]); + return 0; + } + } + /* expand wild card in file path */ + n = expath(file,efiles,MAXEXFILE); + + for (i = 0; i < n; i++) + { + if (!(ext = strrchr(efiles[i],'.'))) continue; + if (strcmp(ext, ".sbs") && strcmp(ext, ".SBS") && + strcmp(ext, ".ems") && strcmp(ext, ".EMS")) continue; + + readmsgs(efiles[i], sel, ts, te, sbs); + } + for (i = 0; i < MAXEXFILE; i++) free(efiles[i]); + + /* sort messages */ + if (sbs->n >0) + { + qsort(sbs->msgs, sbs->n, sizeof(sbsmsg_t), cmpmsgs); + } + return sbs->n; +} + + +int sbsreadmsg(const char *file, int sel, sbs_t *sbs) +{ + gtime_t ts = {0, 0}, te = {0, 0}; + + trace(3, "sbsreadmsg: file=%s sel=%d\n", file, sel); + + return sbsreadmsgt(file, sel, ts, te, sbs); +} + + +/* output sbas messages -------------------------------------------------------- + * output sbas message record to output file in rtklib sbas log format + * args : FILE *fp I output file pointer + * sbsmsg_t *sbsmsg I sbas messages + * return : none + *-----------------------------------------------------------------------------*/ +void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg) +{ + int i, type = sbsmsg->msg[1] >> 2; + + trace(4,"sbsoutmsg:\n"); + + fprintf(fp, "%4d %6d %3d %2d : ", sbsmsg->week, sbsmsg->tow, sbsmsg->prn, type); + for (i = 0; i < 29; i++) fprintf(fp, "%02X", sbsmsg->msg[i]); + fprintf(fp,"\n"); +} + + +/* search igps ---------------------------------------------------------------*/ +void searchigp(gtime_t time __attribute__((unused)), const double *pos, const sbsion_t *ion, + const sbsigp_t **igp, double *x, double *y) +{ + int i,latp[2],lonp[4]; + double lat = pos[0] * R2D,lon = pos[1] * R2D; + const sbsigp_t *p; + + trace(4,"searchigp: pos=%.3f %.3f\n",pos[0] * R2D, pos[1] * R2D); + + if (lon >= 180.0) lon -= 360.0; + if (-55.0 <= lat && lat < 55.0) + { + latp[0] = (int)floor(lat / 5.0) * 5; + latp[1] = latp[0] + 5; + lonp[0] = lonp[1] = (int)floor(lon / 5.0) * 5; + lonp[2] = lonp[3] = lonp[0] + 5; + *x = (lon - lonp[0]) / 5.0; + *y = (lat - latp[0]) / 5.0; + } + else + { + latp[0] = (int)floor((lat-5.0) / 10.0) * 10 + 5; + latp[1] = latp[0] + 10; + lonp[0] = lonp[1] = (int)floor(lon / 10.0) * 10; + lonp[2] = lonp[3] = lonp[0] + 10; + *x = (lon - lonp[0]) / 10.0; + *y = (lat - latp[0]) / 10.0; + if (75.0 <= lat && lat < 85.0) + { + lonp[1] = (int)floor(lon / 90.0) * 90; + lonp[3] = lonp[1] + 90; + } + else if (-85.0 <= lat && lat < -75.0) + { + lonp[0] = (int)floor((lon - 50.0) / 90.0) * 90 + 40; + lonp[2] = lonp[0] + 90; + } + else if (lat >= 85.0) + { + for (i = 0; i < 4; i++) lonp[i] = (int)floor(lon / 90.0) * 90; + } + else if (lat < -85.0) + { + for (i = 0; i < 4; i++) lonp[i] = (int)floor((lon - 50.0) / 90.0) * 90 + 40; + } + } + for (i = 0; i < 4; i++) if (lonp[i] == 180) lonp[i] = -180; + for (i = 0; i <= MAXBAND; i++) + { + for (p = ion[i].igp; p < ion[i].igp + ion[i].nigp; p++) + { + if (p->t0.time == 0) continue; + if (p->lat == latp[0] && p->lon == lonp[0] && p->give > 0) igp[0] = p; + else if (p->lat == latp[1] && p->lon == lonp[1] && p->give > 0) igp[1] = p; + else if (p->lat == latp[0] && p->lon == lonp[2] && p->give > 0) igp[2] = p; + else if (p->lat == latp[1] && p->lon == lonp[3] && p->give > 0) igp[3] = p; + if (igp[0] && igp[1] && igp[2] && igp[3]) return; + } + } +} + + +/* sbas ionospheric delay correction ------------------------------------------- + * compute sbas ionosphric delay correction + * args : gtime_t time I time + * nav_t *nav I navigation data + * double *pos I receiver position {lat,lon,height} (rad/m) + * double *azel I satellite azimuth/elavation angle (rad) + * double *delay O slant ionospheric delay (L1) (m) + * double *var O variance of ionospheric delay (m^2) + * return : status (1:ok, 0:no correction) + * notes : before calling the function, sbas ionosphere correction parameters + * in navigation data (nav->sbsion) must be set by callig + * sbsupdatecorr() + *-----------------------------------------------------------------------------*/ +int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, double *delay, double *var) +{ + const double re = 6378.1363,hion = 350.0; + int i,err = 0; + double fp,posp[2],x = 0.0,y = 0.0,t,w[4] = {}; + const sbsigp_t *igp[4] = {}; /* {ws,wn,es,en} */ + + trace(4,"sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D, + azel[0]*R2D,azel[1]*R2D); + + *delay = *var = 0.0; + if (pos[2]<-100.0||azel[1]<=0) return 1; + + /* ipp (ionospheric pierce point) position */ + fp = ionppp(pos,azel,re,hion,posp); + + /* search igps around ipp */ + searchigp(time,posp,nav->sbsion,igp,&x,&y); + + /* weight of igps */ + if (igp[0] && igp[1] && igp[2] && igp[3]) + { + w[0] = (1.0 - x) * (1.0 - y); + w[1] = (1.0 - x) * y; + w[2] = x * (1.0 - y); + w[3] = x * y; + } + else if (igp[0] && igp[1] && igp[2]) + { + w[1] = y; w[2] = x; + if ((w[0] = 1.0- w[1] - w[2]) < 0.0) err = 1; + } + else if (igp[0] && igp[2] && igp[3]) + { + w[0] = 1.0 - x; w[3] = y; + if ((w[2] = 1.0 - w[0] - w[3]) < 0.0) err = 1; + } + else if (igp[0] && igp[1] && igp[3]) + { + w[0] = 1.0 - y; w[3] = x; + if ((w[1] = 1.0 - w[0] - w[3]) < 0.0) err = 1; + } + else if (igp[1] && igp[2] && igp[3]) + { + w[1] = 1.0-x; w[2] = 1.0-y; + if ((w[3] = 1.0 - w[1] - w[2]) < 0.0) err = 1; + } + else err = 1; + + if (err) + { + trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f\n", posp[0] * R2D, + posp[1] * R2D); + return 0; + } + for (i = 0; i < 4; i++) + { + if (!igp[i]) continue; + t = timediff(time,igp[i]->t0); + *delay += w[i] * igp[i]->delay; + *var += w[i] * varicorr(igp[i]->give) * 9e-8 * fabs(t); + } + *delay *= fp; *var *= fp * fp; + + trace(5,"sbsioncorr: dion=%7.2f sig=%7.2f\n",*delay,sqrt(*var)); + return 1; +} + + +/* get meterological parameters ----------------------------------------------*/ +void getmet(double lat, double *met) +{ + static const double metprm[][10] = { /* lat=15,30,45,60,75 */ + {1013.25,299.65,26.31,6.30E-3,2.77, 0.00, 0.00,0.00,0.00E-3,0.00}, + {1017.25,294.15,21.79,6.05E-3,3.15, -3.75, 7.00,8.85,0.25E-3,0.33}, + {1015.75,283.15,11.66,5.58E-3,2.57, -2.25,11.00,7.24,0.32E-3,0.46}, + {1011.75,272.15, 6.78,5.39E-3,1.81, -1.75,15.00,5.36,0.81E-3,0.74}, + {1013.00,263.65, 4.11,4.53E-3,1.55, -0.50,14.50,3.39,0.62E-3,0.30} + }; + int i,j; + double a; + lat = fabs(lat); + if (lat <= 15.0) for (i = 0;i<10;i++) met[i] = metprm[0][i]; + else if (lat >= 75.0) for (i = 0;i<10;i++) met[i] = metprm[4][i]; + else + { + j = (int)(lat / 15.0); a = (lat - j * 15.0) / 15.0; + for (i = 0; i < 10; i++) met[i] = (1.0 - a) * metprm[j-1][i] + a * metprm[j][i]; + } +} + + +/* tropospheric delay correction ----------------------------------------------- + * compute sbas tropospheric delay correction (mops model) + * args : gtime_t time I time + * double *pos I receiver position {lat,lon,height} (rad/m) + * double *azel I satellite azimuth/elavation (rad) + * double *var O variance of troposphric error (m^2) + * return : slant tropospheric delay (m) + *-----------------------------------------------------------------------------*/ +double sbstropcorr(gtime_t time, const double *pos, const double *azel, + double *var) +{ + const double k1 = 77.604,k2 = 382000.0,rd = 287.054,gm = 9.784,g = 9.80665; + static double pos_[3] = {}, zh = 0.0, zw = 0.0; + int i; + double c, met[10], sinel = sin(azel[1]), h = pos[2], m; + + trace(4, "sbstropcorr: pos=%.3f %.3f azel=%.3f %.3f\n", pos[0] * R2D, pos[1] * R2D, + azel[0] * R2D, azel[1] * R2D); + + if (pos[2] < -100.0 || 10000.0 < pos[2] || azel[1] <= 0) + { + *var = 0.0; + return 0.0; + } + if (zh == 0.0 || fabs(pos[0] - pos_[0]) > 1e-7 || fabs(pos[1] - pos_[1]) > 1e-7 || + fabs(pos[2] - pos_[2]) > 1.0) + { + getmet(pos[0] * R2D,met); + c = cos(2.0 * PI * (time2doy(time) - (pos[0] >= 0.0 ? 28.0 : 211.0)) / 365.25); + for (i = 0; i < 5;i++) met[i] -= met[i+5] * c; + zh = 1e-6 * k1 * rd * met[0] / gm; + zw = 1e-6 * k2 * rd / (gm * (met[4] + 1.0) - met[3] * rd) * met[2] / met[1]; + zh *= pow(1.0 - met[3] * h / met[1], g / (rd * met[3])); + zw *= pow(1.0 - met[3] * h / met[1], (met[4] + 1.0) * g / (rd * met[3]) - 1.0); + for (i = 0; i < 3; i++) pos_[i] = pos[i]; + } + m = 1.001/sqrt(0.002001+sinel*sinel); + *var = 0.12*0.12*m*m; + return (zh+zw)*m; +} + + +/* long term correction ------------------------------------------------------*/ +int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat, + double *drs, double *ddts) +{ + const sbssatp_t *p; + double t; + int i; + + trace(3,"sbslongcorr: sat=%2d\n",sat); + + for (p = sbssat->sat;psat+sbssat->nsat;p++) + { + if (p->sat != sat || p->lcorr.t0.time == 0) continue; + t = timediff(time, p->lcorr.t0); + if (fabs(t) > MAXSBSAGEL) + { + trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n", + time_str(time,0), sat, t); + return 0; + } + for (i = 0; i < 3;i++) drs[i] = p->lcorr.dpos[i] + p->lcorr.dvel[i] * t; + *ddts = p->lcorr.daf0 + p->lcorr.daf1 * t; + + trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n", + sat, drs[0], drs[1], drs[2], *ddts * SPEED_OF_LIGHT); + + return 1; + } + /* if sbas satellite without correction, no correction applied */ + if (satsys(sat,NULL) == SYS_SBS) return 1; + + trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat); + return 0; +} + + +/* fast correction -----------------------------------------------------------*/ +int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat, + double *prc, double *var) +{ + const sbssatp_t *p; + double t; + + trace(3,"sbsfastcorr: sat=%2d\n",sat); + + for (p = sbssat->sat;psat+sbssat->nsat;p++) + { + if (p->sat!=sat) continue; + if (p->fcorr.t0.time == 0) break; + t = timediff(time, p->fcorr.t0) + sbssat->tlat; + + /* expire age of correction or UDRE==14 (not monitored) */ + if (fabs(t) > MAXSBSAGEF || p->fcorr.udre >= 15) continue; + *prc = p->fcorr.prc; +#ifdef RRCENA + if (p->fcorr.ai > 0 && fabs(t) <= 8.0 * p->fcorr.dt) + { + *prc += p->fcorr.rrc * t; + } +#endif + *var = varfcorr(p->fcorr.udre) + degfcorr(p->fcorr.ai) * t * t / 2.0; + + trace(5,"sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f\n",sat, + *prc, sqrt(*var), t); + return 1; + } + trace(2,"no sbas fast correction: %s sat=%2d\n", time_str(time, 0), sat); + return 0; +} + + +/* sbas satellite ephemeris and clock correction ------------------------------- + * correct satellite position and clock bias with sbas satellite corrections + * args : gtime_t time I reception time + * int sat I satellite + * nav_t *nav I navigation data + * double *rs IO sat position and corrected {x,y,z} (ecef) (m) + * double *dts IO sat clock bias and corrected (s) + * double *var O sat position and clock variance (m^2) + * return : status (1:ok,0:no correction) + * notes : before calling the function, sbas satellite correction parameters + * in navigation data (nav->sbssat) must be set by callig + * sbsupdatecorr(). + * satellite clock correction include long-term correction and fast + * correction. + * sbas clock correction is usually based on L1C/A code. TGD or DCB has + * to be considered for other codes + *-----------------------------------------------------------------------------*/ +int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs, + double *dts, double *var) +{ + double drs[3] = {}, dclk = 0.0, prc = 0.0; + int i; + + trace(3,"sbssatcorr : sat=%2d\n",sat); + + /* sbas long term corrections */ + if (!sbslongcorr(time, sat, &nav->sbssat, drs, &dclk)) + { + return 0; + } + /* sbas fast corrections */ + if (!sbsfastcorr(time, sat, &nav->sbssat, &prc, var)) + { + return 0; + } + for (i = 0; i < 3; i++) rs[i] += drs[i]; + + dts[0] += dclk + prc / SPEED_OF_LIGHT; + + trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f\n", + sat, drs[0], drs[1], drs[2], dclk, prc / SPEED_OF_LIGHT, *var); + + return 1; +} + + +/* decode sbas message --------------------------------------------------------- + * decode sbas message frame words and check crc + * args : gtime_t time I reception time + * int prn I sbas satellite prn number + * unsigned int *word I message frame words (24bit x 10) + * sbsmsg_t *sbsmsg O sbas message + * return : status (1:ok,0:crc error) + *-----------------------------------------------------------------------------*/ +int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words, + sbsmsg_t *sbsmsg) +{ + int i,j; + unsigned char f[29]; + double tow; + + trace(5,"sbsdecodemsg: prn=%d\n",prn); + + if (time.time == 0) return 0; + tow = time2gpst(time,&sbsmsg->week); + sbsmsg->tow = (int)(tow + DTTOL); + sbsmsg->prn = prn; + for (i = 0; i < 7; i++) for (j = 0; j < 4; j++) + { + sbsmsg->msg[i*4+j] = (unsigned char)(words[i] >> ((3-j)*8)); + } + sbsmsg->msg[28] = (unsigned char)(words[7] >> 18 ) & 0xC0; + for (i = 28; i > 0; i--) f[i] = (sbsmsg->msg[i] >> 6) + (sbsmsg->msg[i-1] << 2); + f[0] = sbsmsg->msg[0] >> 6; + + return rtk_crc24q(f, 29) == (words[7] & 0xFFFFFF); /* check crc */ +} diff --git a/src/algorithms/libs/rtklib/rtklib_sbas.h b/src/algorithms/libs/rtklib/rtklib_sbas.h new file mode 100644 index 000000000..62ea1dd31 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_sbas.h @@ -0,0 +1,165 @@ +/*! + * \file rtklib_sbas.h + * \brief sbas functions + * \authors
      + *
    • 2007-2013, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + * References : + * [1] RTCA/DO-229C, Minimum operational performanc standards for global + * positioning system/wide area augmentation system airborne equipment, + * RTCA inc, November 28, 2001 + * [2] IS-QZSS v.1.1, Quasi-Zenith Satellite System Navigation Service + * Interface Specification for QZSS, Japan Aerospace Exploration Agency, + * July 31, 2009 + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_SBAS_H_ +#define GNSS_SDR_RTKLIB_SBAS_H_ + +#include "rtklib.h" + +/* constants -----------------------------------------------------------------*/ + +const int WEEKOFFSET = 1024; /* gps week offset for NovAtel OEM-3 */ + +/* sbas igp definition -------------------------------------------------------*/ +static const short +x1[] = {-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15, 20, + 25, 30, 35, 40, 45, 50, 55, 65, 75, 85}, +x2[] = {-55, -50, -45, -40, -35, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, + 35, 40, 45, 50, 55}, +x3[] = {-75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15, 20, + 25, 30, 35, 40, 45, 50, 55, 65, 75}, +x4[] = {-85, -75, -65, -55, -50, -45, -40, -35, -30, -25, -20, -15, -10, - 5, 0, 5, 10, 15, + 20, 25, 30, 35, 40, 45, 50, 55, 65, 75}, +x5[] = {-180, -175, -170, -165, -160, -155, -150, -145, -140, -135, -130, -125, -120, -115, + -110, -105, -100, - 95, - 90, - 85, - 80, - 75, - 70, - 65, - 60, - 55, - 50, - 45, + - 40, - 35, - 30, - 25, - 20, - 15, - 10, - 5, 0, 5, 10, 15, 20, 25, + 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, + 100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165, + 170, 175}, +x6[] = {-180, -170, -160, -150, -140, -130, -120, -110, -100, - 90, - 80, - 70, - 60, - 50, + - 40, - 30, - 20, - 10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, + 100, 110, 120, 130, 140, 150, 160, 170}, +x7[] = {-180, -150, -120, - 90, - 60, - 30, 0, 30, 60, 90, 120, 150}, +x8[] = {-170, -140, -110, - 80, - 50, - 20, 10, 40, 70, 100, 130, 160}; + +const sbsigpband_t igpband1[9][8] = { /* band 0-8 */ + {{-180, x1, 1, 28}, {-175, x2, 29, 51}, {-170, x3, 52, 78}, {-165, x2, 79, 101}, + {-160, x3, 102, 128}, {-155, x2, 129, 151}, {-150, x3, 152, 178}, {-145, x2, 179, 201}}, + {{-140, x4, 1, 28}, {-135, x2, 29, 51}, {-130, x3, 52, 78}, {-125, x2, 79, 101}, + {-120, x3, 102, 128}, {-115, x2, 129, 151}, {-110, x3, 152, 178}, {-105, x2, 179, 201}}, + {{-100, x3, 1, 27}, {- 95,x2, 28, 50},{- 90, x1, 51, 78}, {- 85, x2, 79, 101}, + {- 80, x3, 102, 128}, {- 75, x2, 129, 151}, {- 70, x3, 152, 178}, {- 65, x2,179, 201}}, + {{- 60, x3, 1, 27}, {- 55, x2, 28, 50}, {- 50,x4, 51, 78}, {- 45, x2, 79, 101}, + {- 40, x3, 102, 128}, {- 35, x2, 129, 151}, {- 30, x3, 152, 178}, {- 25, x2, 179, 201}}, + {{- 20, x3, 1, 27}, {- 15, x2, 28, 50}, {- 10, x3, 51, 77}, {- 5, x2, 78, 100}, + { 0, x1, 101, 128}, { 5, x2, 129, 151}, { 10, x3, 152, 178}, { 15, x2, 179, 201}}, + {{ 20, x3, 1, 27}, { 25, x2, 28, 50}, { 30, x3, 51, 77}, { 35, x2, 78, 100}, + { 40, x4, 101, 128}, { 45, x2, 129, 151},{ 50, x3, 152, 178}, { 55, x2, 179, 201}}, + {{ 60, x3, 1, 27}, { 65, x2, 28, 50}, { 70, x3, 51, 77}, { 75, x2, 78, 100}, + { 80, x3, 101, 127}, { 85, x2, 128, 150}, { 90,x1, 151, 178}, { 95, x2, 179, 201}}, + {{ 100, x3, 1, 27}, { 105, x2, 28, 50}, { 110, x3, 51, 77}, { 115, x2, 78 ,100}, + { 120, x3, 101, 127}, { 125, x2, 128, 150}, { 130, x4, 151, 178}, { 135, x2, 179, 201}}, + {{ 140, x3, 1, 27}, { 145, x2, 28, 50}, { 150, x3, 51, 77}, { 155, x2, 78, 100}, + { 160, x3, 101, 127}, { 165, x2,128, 150}, { 170, x3,151,177}, { 175, x2, 178, 200}} +}; +const sbsigpband_t igpband2[2][5] = { /* band 9-10 */ + {{ 60, x5, 1, 72}, { 65, x6, 73, 108}, { 70, x6, 109, 144}, { 75, x6, 145, 180}, + { 85, x7, 181, 192}}, + {{- 60, x5, 1, 72}, {- 65, x6, 73, 108}, {- 70, x6, 109, 144}, {- 75 ,x6, 145, 180}, + {- 85, x8, 181, 192}} +}; + + +char *getfield(char *p, int pos); +double varfcorr(int udre); +double varicorr(int give); +double degfcorr(int ai); + +int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav); +int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion); +int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat); +int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat); +int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat); +int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat); +int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion); + +int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav); +void readmsgs(const char *file, int sel, gtime_t ts, gtime_t te,sbs_t *sbs); +int cmpmsgs(const void *p1, const void *p2); +int sbsreadmsgt(const char *file, int sel, gtime_t ts, gtime_t te, + sbs_t *sbs); +int sbsreadmsg(const char *file, int sel, sbs_t *sbs); +void sbsoutmsg(FILE *fp, sbsmsg_t *sbsmsg); +void searchigp(gtime_t time, const double *pos, const sbsion_t *ion, + const sbsigp_t **igp, double *x, double *y); +int sbsioncorr(gtime_t time, const nav_t *nav, const double *pos, + const double *azel, double *delay, double *var); + +void getmet(double lat, double *met); +double sbstropcorr(gtime_t time, const double *pos, const double *azel, + double *var); +int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat, + double *drs, double *ddts); +int sbsfastcorr(gtime_t time, int sat, const sbssat_t *sbssat, + double *prc, double *var); + +int sbssatcorr(gtime_t time, int sat, const nav_t *nav, double *rs, + double *dts, double *var); +int sbsdecodemsg(gtime_t time, int prn, const unsigned int *words, + sbsmsg_t *sbsmsg); + + +#endif /* GNSS_SDR_RTKLIB_SBAS_H_ */ diff --git a/src/algorithms/libs/rtklib/rtklib_tides.cc b/src/algorithms/libs/rtklib/rtklib_tides.cc new file mode 100644 index 000000000..96a23417a --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_tides.cc @@ -0,0 +1,321 @@ +/*! + * \file rtklib_tides.cc + * \brief Tidal displacement corrections + * \authors
      + *
    • 2007-2008, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2008, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_tides.h" +#include "rtklib_rtkcmn.h" + + + +/* solar/lunar tides (ref [2] 7) ---------------------------------------------*/ +//#ifndef IERS_MODEL +void tide_pl(const double *eu, const double *rp, double GMp, + const double *pos, double *dr) +{ + const double H3=0.292,L3=0.015; + double r,ep[3],latp,lonp,p,K2,K3,a,H2,L2,dp,du,cosp,sinl,cosl; + int i; + + trace(4,"tide_pl : pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); + + if ((r=norm_rtk(rp,3))<=0.0) return; + + for (i=0;i<3;i++) ep[i]=rp[i]/r; + + K2=GMp/GME*std::pow(RE_WGS84, 2.04)*std::pow(RE_WGS84, 2.0)/(r*r*r); + K3=K2*RE_WGS84/r; + latp=asin(ep[2]); lonp=atan2(ep[1],ep[0]); + cosp=cos(latp); sinl=sin(pos[0]); cosl=cos(pos[0]); + + /* step1 in phase (degree 2) */ + p=(3.0*sinl*sinl-1.0)/2.0; + H2=0.6078-0.0006*p; + L2=0.0847+0.0002*p; + a=dot(ep,eu,3); + dp=K2*3.0*L2*a; + du=K2*(H2*(1.5*a*a-0.5)-3.0*L2*a*a); + + /* step1 in phase (degree 3) */ + dp+=K3*L3*(7.5*a*a-1.5); + du+=K3*(H3*(2.5*a*a*a-1.5*a)-L3*(7.5*a*a-1.5)*a); + + /* step1 out-of-phase (only radial) */ + du+=3.0/4.0*0.0025*K2*sin(2.0*latp)*sin(2.0*pos[0])*sin(pos[1]-lonp); + du+=3.0/4.0*0.0022*K2*cosp*cosp*cosl*cosl*sin(2.0*(pos[1]-lonp)); + + dr[0]=dp*ep[0]+du*eu[0]; + dr[1]=dp*ep[1]+du*eu[1]; + dr[2]=dp*ep[2]+du*eu[2]; + + trace(5,"tide_pl : dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]); +} + + +/* displacement by solid earth tide (ref [2] 7) ------------------------------*/ +void tide_solid(const double *rsun, const double *rmoon, + const double *pos, const double *E, double gmst, int opt, + double *dr) +{ + double dr1[3],dr2[3],eu[3],du,dn,sinl,sin2l; + + trace(3,"tide_solid: pos=%.3f %.3f opt=%d\n",pos[0]*R2D,pos[1]*R2D,opt); + + /* step1: time domain */ + eu[0]=E[2]; eu[1]=E[5]; eu[2]=E[8]; + tide_pl(eu,rsun, GMS,pos,dr1); + tide_pl(eu,rmoon,GMM,pos,dr2); + + /* step2: frequency domain, only K1 radial */ + sin2l=sin(2.0*pos[0]); + du=-0.012*sin2l*sin(gmst+pos[1]); + + dr[0]=dr1[0]+dr2[0]+du*E[2]; + dr[1]=dr1[1]+dr2[1]+du*E[5]; + dr[2]=dr1[2]+dr2[2]+du*E[8]; + + /* eliminate permanent deformation */ + if (opt&8) + { + sinl=sin(pos[0]); + du=0.1196*(1.5*sinl*sinl-0.5); + dn=0.0247*sin2l; + dr[0]+=du*E[2]+dn*E[1]; + dr[1]+=du*E[5]+dn*E[4]; + dr[2]+=du*E[8]+dn*E[7]; + } + trace(5,"tide_solid: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]); +} +//#endif /* !IERS_MODEL */ + + +/* displacement by ocean tide loading (ref [2] 7) ----------------------------*/ +void tide_oload(gtime_t tut, const double *odisp, double *denu) +{ + const double args[][5]={ + {1.40519E-4, 2.0,-2.0, 0.0, 0.00}, /* M2 */ + {1.45444E-4, 0.0, 0.0, 0.0, 0.00}, /* S2 */ + {1.37880E-4, 2.0,-3.0, 1.0, 0.00}, /* N2 */ + {1.45842E-4, 2.0, 0.0, 0.0, 0.00}, /* K2 */ + {0.72921E-4, 1.0, 0.0, 0.0, 0.25}, /* K1 */ + {0.67598E-4, 1.0,-2.0, 0.0,-0.25}, /* O1 */ + {0.72523E-4,-1.0, 0.0, 0.0,-0.25}, /* P1 */ + {0.64959E-4, 1.0,-3.0, 1.0,-0.25}, /* Q1 */ + {0.53234E-5, 0.0, 2.0, 0.0, 0.00}, /* Mf */ + {0.26392E-5, 0.0, 1.0,-1.0, 0.00}, /* Mm */ + {0.03982E-5, 2.0, 0.0, 0.0, 0.00} /* Ssa */ + }; + const double ep1975[]={1975,1,1,0,0,0}; + double ep[6],fday,days,t,t2,t3,a[5],ang,dp[3]={0}; + int i,j; + + trace(3,"tide_oload:\n"); + + /* angular argument: see subroutine arg.f for reference [1] */ + time2epoch(tut,ep); + fday=ep[3]*3600.0+ep[4]*60.0+ep[5]; + ep[3]=ep[4]=ep[5]=0.0; + days=timediff(epoch2time(ep),epoch2time(ep1975))/86400.0+1.0; + t=(27392.500528+1.000000035*days)/36525.0; + t2=t*t; t3=t2*t; + + a[0]=fday; + a[1]=(279.69668+36000.768930485*t+3.03E-4*t2)*D2R; /* H0 */ + a[2]=(270.434358+481267.88314137*t-0.001133*t2+1.9E-6*t3)*D2R; /* S0 */ + a[3]=(334.329653+4069.0340329577*t-0.010325*t2-1.2E-5*t3)*D2R; /* P0 */ + a[4]=2.0*PI; + + /* displacements by 11 constituents */ + for (i=0;i<11;i++) + { + ang=0.0; + for (j=0;j<5;j++) ang+=a[j]*args[i][j]; + for (j=0;j<3;j++) dp[j]+=odisp[j+i*6]*cos(ang-odisp[j+3+i*6]*D2R); + } + denu[0]=-dp[1]; + denu[1]=-dp[2]; + denu[2]= dp[0]; + + trace(5,"tide_oload: denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]); +} + + +/* iers mean pole (ref [7] eq.7.25) ------------------------------------------*/ +void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar) +{ + const double ep2000[]={2000,1,1,0,0,0}; + double y,y2,y3; + + y=timediff(tut,epoch2time(ep2000))/86400.0/365.25; + + if (y<3653.0/365.25) + { /* until 2010.0 */ + y2=y*y; y3=y2*y; + *xp_bar= 55.974+1.8243*y+0.18413*y2+0.007024*y3; /* (mas) */ + *yp_bar=346.346+1.7896*y-0.10729*y2-0.000908*y3; + } + else + { /* after 2010.0 */ + *xp_bar= 23.513+7.6141*y; /* (mas) */ + *yp_bar=358.891-0.6287*y; + } +} + + +/* displacement by pole tide (ref [7] eq.7.26) --------------------------------*/ +void tide_pole(gtime_t tut, const double *pos, const double *erpv, + double *denu) +{ + double xp_bar,yp_bar,m1,m2,cosl,sinl; + + trace(3,"tide_pole: pos=%.3f %.3f\n",pos[0]*R2D,pos[1]*R2D); + + /* iers mean pole (mas) */ + iers_mean_pole(tut,&xp_bar,&yp_bar); + + /* ref [7] eq.7.24 */ + m1= erpv[0]/AS2R-xp_bar*1E-3; /* (as) */ + m2=-erpv[1]/AS2R+yp_bar*1E-3; + + /* sin(2*theta) = sin(2*phi), cos(2*theta)=-cos(2*phi) */ + cosl=cos(pos[1]); + sinl=sin(pos[1]); + denu[0]= 9E-3*sin(pos[0]) *(m1*sinl-m2*cosl); /* de= Slambda (m) */ + denu[1]= -9E-3*cos(2.0*pos[0])*(m1*cosl+m2*sinl); /* dn=-Stheta (m) */ + denu[2]=-33E-3*sin(2.0*pos[0])*(m1*cosl+m2*sinl); /* du= Sr (m) */ + + trace(5,"tide_pole : denu=%.3f %.3f %.3f\n",denu[0],denu[1],denu[2]); +} + + +/* tidal displacement ---------------------------------------------------------- + * displacements by earth tides + * args : gtime_t tutc I time in utc + * double *rr I site position (ecef) (m) + * int opt I options (or of the followings) + * 1: solid earth tide + * 2: ocean tide loading + * 4: pole tide + * 8: elimate permanent deformation + * double *erp I earth rotation parameters (NULL: not used) + * double *odisp I ocean loading parameters (NULL: not used) + * odisp[0+i*6]: consituent i amplitude radial(m) + * odisp[1+i*6]: consituent i amplitude west (m) + * odisp[2+i*6]: consituent i amplitude south (m) + * odisp[3+i*6]: consituent i phase radial (deg) + * odisp[4+i*6]: consituent i phase west (deg) + * odisp[5+i*6]: consituent i phase south (deg) + * (i=0:M2,1:S2,2:N2,3:K2,4:K1,5:O1,6:P1,7:Q1, + * 8:Mf,9:Mm,10:Ssa) + * double *dr O displacement by earth tides (ecef) (m) + * return : none + * notes : see ref [1], [2] chap 7 + * see ref [4] 5.2.1, 5.2.2, 5.2.3 + * ver.2.4.0 does not use ocean loading and pole tide corrections + *-----------------------------------------------------------------------------*/ +void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, + const double *odisp, double *dr) +{ + gtime_t tut; + double pos[2],E[9],drt[3],denu[3],rs[3],rm[3],gmst,erpv[5]={0}; + int i; +#ifdef IERS_MODEL + double ep[6],fhr; + int year,mon,day; +#endif + + trace(3,"tidedisp: tutc=%s\n",time_str(tutc,0)); + + if (erp) geterp(erp,tutc,erpv); + + tut=timeadd(tutc,erpv[2]); + + dr[0]=dr[1]=dr[2]=0.0; + + if (norm_rtk(rr,3)<=0.0) return; + + pos[0]=asin(rr[2]/norm_rtk(rr,3)); + pos[1]=atan2(rr[1],rr[0]); + xyz2enu(pos,E); + + if (opt&1) + { /* solid earth tides */ + + /* sun and moon position in ecef */ + sunmoonpos(tutc,erpv,rs,rm,&gmst); + +#ifdef IERS_MODEL + time2epoch(tutc,ep); + year=(int)ep[0]; + mon =(int)ep[1]; + day =(int)ep[2]; + fhr =ep[3]+ep[4]/60.0+ep[5]/3600.0; + + /* call DEHANTTIDEINEL */ + // dehanttideinel_((double *)rr,&year,&mon,&day,&fhr,rs,rm,drt); +#else + tide_solid(rs,rm,pos,E,gmst,opt,drt); +#endif + for (i=0;i<3;i++) dr[i]+=drt[i]; + } + if ((opt&2)&&odisp) + { /* ocean tide loading */ + tide_oload(tut,odisp,denu); + matmul("TN",3,1,3,1.0,E,denu,0.0,drt); + for (i=0;i<3;i++) dr[i]+=drt[i]; + } + if ((opt&4)&&erp) + { /* pole tide */ + tide_pole(tut,pos,erpv,denu); + matmul("TN",3,1,3,1.0,E,denu,0.0,drt); + for (i=0;i<3;i++) dr[i]+=drt[i]; + } + trace(5,"tidedisp: dr=%.3f %.3f %.3f\n",dr[0],dr[1],dr[2]); +} diff --git a/src/algorithms/libs/rtklib/rtklib_tides.h b/src/algorithms/libs/rtklib/rtklib_tides.h new file mode 100644 index 000000000..44124604a --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_tides.h @@ -0,0 +1,92 @@ +/*! + * \file rtklib_tides.h + * \brief Tidal displacement corrections + * \authors
      + *
    • 2015, T. Takasu + *
    • 2017, Javier Arribas + *
    • 2017, Carles Fernandez + *
    + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2015, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * References: + * [1] D.D.McCarthy, IERS Technical Note 21, IERS Conventions 1996, July 1996 + * [2] D.D.McCarthy and G.Petit, IERS Technical Note 32, IERS Conventions + * 2003, November 2003 + * [3] D.A.Vallado, Fundamentals of Astrodynamics and Applications 2nd ed, + * Space Technology Library, 2004 + * [4] J.Kouba, A Guide to using International GNSS Service (IGS) products, + * May 2009 + * [5] G.Petit and B.Luzum (eds), IERS Technical Note No. 36, IERS + * Conventions (2010), 2010 + *----------------------------------------------------------------------------*/ + + +#ifndef GNSS_SDR_RTKLIB_TIDES_H_ +#define GNSS_SDR_RTKLIB_TIDES_H_ + + +#include "rtklib.h" + + +const double GME = 3.986004415E+14; /* earth gravitational constant */ +const double GMS = 1.327124E+20; /* sun gravitational constant */ +const double GMM = 4.902801E+12; /* moon gravitational constant */ + +void tide_pl(const double *eu, const double *rp, double GMp, + const double *pos, double *dr); + +void tide_solid(const double *rsun, const double *rmoon, + const double *pos, const double *E, double gmst, int opt, + double *dr); + +void tide_oload(gtime_t tut, const double *odisp, double *denu); + +void iers_mean_pole(gtime_t tut, double *xp_bar, double *yp_bar); + + +void tide_pole(gtime_t tut, const double *pos, const double *erpv, + double *denu); + +void tidedisp(gtime_t tutc, const double *rr, int opt, const erp_t *erp, + const double *odisp, double *dr); +#endif diff --git a/src/algorithms/observables/adapters/CMakeLists.txt b/src/algorithms/observables/adapters/CMakeLists.txt index dcb40e527..087302d8c 100644 --- a/src/algorithms/observables/adapters/CMakeLists.txt +++ b/src/algorithms/observables/adapters/CMakeLists.txt @@ -17,10 +17,6 @@ # set(OBS_ADAPTER_SOURCES - gps_l1_ca_observables.cc - gps_l2c_observables.cc - galileo_e1_observables.cc - galileo_e5a_observables.cc hybrid_observables.cc ) diff --git a/src/algorithms/observables/adapters/galileo_e1_observables.cc b/src/algorithms/observables/adapters/galileo_e1_observables.cc deleted file mode 100644 index 28edee2d5..000000000 --- a/src/algorithms/observables/adapters/galileo_e1_observables.cc +++ /dev/null @@ -1,95 +0,0 @@ -/*! - * \file galileo_e1_observables.cc - * \brief Implementation of an adapter of a Galileo E1 observables block - * to a ObservablesInterface - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#include "galileo_e1_observables.h" -#include -#include "configuration_interface.h" -#include "Galileo_E1.h" - -using google::LogMessage; - -GalileoE1Observables::GalileoE1Observables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) -{ - std::string default_dump_filename = "./observables.dat"; - DLOG(INFO) << "role " << role; - dump_ = configuration->property(role + ".dump", false); - dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - unsigned int history_deep = configuration->property(role + ".averaging_depth", GALILEO_E1_HISTORY_DEEP); - observables_ = galileo_e1_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep); - DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; -} - - - - -GalileoE1Observables::~GalileoE1Observables() -{} - - - - -void GalileoE1Observables::connect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to connect internally - DLOG(INFO) << "nothing to connect internally"; -} - - - -void GalileoE1Observables::disconnect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to disconnect -} - - - - -gr::basic_block_sptr GalileoE1Observables::get_left_block() -{ - return observables_; -} - - - - -gr::basic_block_sptr GalileoE1Observables::get_right_block() -{ - return observables_; -} diff --git a/src/algorithms/observables/adapters/galileo_e1_observables.h b/src/algorithms/observables/adapters/galileo_e1_observables.h deleted file mode 100644 index 8aad5b142..000000000 --- a/src/algorithms/observables/adapters/galileo_e1_observables.h +++ /dev/null @@ -1,89 +0,0 @@ -/*! - * \file galileo_e1_observables.h - * \brief Implementation of an adapter of a Galileo E1 observables block - * to a ObservablesInterface - * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com - * \author Javier Arribas 2013. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#ifndef GNSS_SDR_GALILEO_E1_OBSERVABLES_H_ -#define GNSS_SDR_GALILEO_E1_OBSERVABLES_H_ - -#include -#include "observables_interface.h" -#include "galileo_e1_observables_cc.h" - - -class ConfigurationInterface; - -/*! - * \brief This class implements an ObservablesInterface for Galileo E1B - */ -class GalileoE1Observables : public ObservablesInterface -{ -public: - GalileoE1Observables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams); - virtual ~GalileoE1Observables(); - std::string role() - { - return role_; - } - - //! Returns "Galileo_E1B_Observables" - std::string implementation() - { - return "Galileo_E1B_Observables"; - } - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); - void reset() - { - return; - } - - //! All blocks must have an item_size() function implementation - size_t item_size() - { - return sizeof(gr_complex); - } - -private: - galileo_e1_observables_cc_sptr observables_; - bool dump_; - std::string dump_filename_; - std::string role_; - unsigned int in_streams_; - unsigned int out_streams_; -}; - -#endif diff --git a/src/algorithms/observables/adapters/galileo_e5a_observables.cc b/src/algorithms/observables/adapters/galileo_e5a_observables.cc deleted file mode 100644 index 239b4b17d..000000000 --- a/src/algorithms/observables/adapters/galileo_e5a_observables.cc +++ /dev/null @@ -1,96 +0,0 @@ -/*! - * \file galileo_e5a_observables.cc - * \brief Implementation of an adapter of a Galileo E5a observables block - * to a ObservablesInterface - * \author Carles Fernandez 2016. carles.fernandez(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2016 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - - #include "galileo_e5a_observables.h" - #include "configuration_interface.h" - #include - #include "Galileo_E5a.h" - - using google::LogMessage; - - GalileoE5aObservables::GalileoE5aObservables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) - { - std::string default_dump_filename = "./observables.dat"; - DLOG(INFO) << "role " << role; - dump_ = configuration->property(role + ".dump", false); - dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - unsigned int default_depth = GALILEO_E5A_HISTORY_DEEP; - unsigned int history_deep = configuration->property(role + ".averaging_depth", default_depth); - observables_ = hybrid_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep); - DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; - } - - - - - GalileoE5aObservables::~GalileoE5aObservables() - {} - - - - - void GalileoE5aObservables::connect(gr::top_block_sptr top_block) - { - if(top_block) { /* top_block is not null */}; - // Nothing to connect internally - DLOG(INFO) << "nothing to connect internally"; - } - - - - void GalileoE5aObservables::disconnect(gr::top_block_sptr top_block) - { - if(top_block) { /* top_block is not null */}; - // Nothing to disconnect - } - - - - - gr::basic_block_sptr GalileoE5aObservables::get_left_block() - { - return observables_; - } - - - - - gr::basic_block_sptr GalileoE5aObservables::get_right_block() - { - return observables_; - } diff --git a/src/algorithms/observables/adapters/galileo_e5a_observables.h b/src/algorithms/observables/adapters/galileo_e5a_observables.h deleted file mode 100644 index 89898c99a..000000000 --- a/src/algorithms/observables/adapters/galileo_e5a_observables.h +++ /dev/null @@ -1,88 +0,0 @@ -/*! - * \file galileo_e5a_observables.h - * \brief Implementation of an adapter of a Galileo E5a observables block - * to a ObservablesInterface - * \author Carles Fernandez 2016. carles.fernandez(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2016 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#ifndef GNSS_SDR_GALILEO_E5A_OBSERVABLES_H_ -#define GNSS_SDR_GALILEO_E5A_OBSERVABLES_H_ - -#include -#include "observables_interface.h" -#include "hybrid_observables_cc.h" - - -class ConfigurationInterface; - -/*! - * \brief This class implements an ObservablesInterface for Galileo E5A - */ -class GalileoE5aObservables : public ObservablesInterface -{ -public: - GalileoE5aObservables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams); - virtual ~GalileoE5aObservables(); - std::string role() - { - return role_; - } - - //! Returns "Galileo_E5A_Observables" - std::string implementation() - { - return "Galileo_E5A_Observables"; - } - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); - void reset() - { - return; - } - - //! All blocks must have an item_size() function implementation - size_t item_size() - { - return sizeof(gr_complex); - } - -private: - hybrid_observables_cc_sptr observables_; - bool dump_; - std::string dump_filename_; - std::string role_; - unsigned int in_streams_; - unsigned int out_streams_; -}; - -#endif diff --git a/src/algorithms/observables/adapters/gps_l1_ca_observables.cc b/src/algorithms/observables/adapters/gps_l1_ca_observables.cc deleted file mode 100644 index b75f1c3ef..000000000 --- a/src/algorithms/observables/adapters/gps_l1_ca_observables.cc +++ /dev/null @@ -1,95 +0,0 @@ -/*! - * \file gps_l1_ca_observables.cc - * \brief Implementation of an adapter of a GPS L1 C/A observables block - * to a ObservablesInterface - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#include "gps_l1_ca_observables.h" -#include "configuration_interface.h" -#include -#include "GPS_L1_CA.h" - -using google::LogMessage; - -GpsL1CaObservables::GpsL1CaObservables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) -{ - std::string default_dump_filename = "./observables.dat"; - DLOG(INFO) << "role " << role; - dump_ = configuration->property(role + ".dump", false); - dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - unsigned int history_deep = configuration->property(role + ".averaging_depth", GPS_L1_CA_HISTORY_DEEP); - observables_ = gps_l1_ca_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep); - DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; -} - - - - -GpsL1CaObservables::~GpsL1CaObservables() -{} - - - - -void GpsL1CaObservables::connect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to connect internally - DLOG(INFO) << "nothing to connect internally"; -} - - - -void GpsL1CaObservables::disconnect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to disconnect -} - - - - -gr::basic_block_sptr GpsL1CaObservables::get_left_block() -{ - return observables_; -} - - - - -gr::basic_block_sptr GpsL1CaObservables::get_right_block() -{ - return observables_; -} diff --git a/src/algorithms/observables/adapters/gps_l1_ca_observables.h b/src/algorithms/observables/adapters/gps_l1_ca_observables.h deleted file mode 100644 index cf83be10d..000000000 --- a/src/algorithms/observables/adapters/gps_l1_ca_observables.h +++ /dev/null @@ -1,89 +0,0 @@ -/*! - * \file gps_l1_ca_observables.h - * \brief Interface of an adapter of a GPS L1 C/A observables block - * to a ObservablesInterface - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#ifndef GNSS_SDR_GPS_L1_CA_OBSERVABLES_H_ -#define GNSS_SDR_GPS_L1_CA_OBSERVABLES_H_ - -#include -#include "observables_interface.h" -#include "gps_l1_ca_observables_cc.h" - - -class ConfigurationInterface; - -/*! - * \brief This class implements an ObservablesInterface for GPS L1 C/A - */ -class GpsL1CaObservables : public ObservablesInterface -{ -public: - GpsL1CaObservables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams); - virtual ~GpsL1CaObservables(); - std::string role() - { - return role_; - } - - //! Returns "GPS_L1_CA_Observables" - std::string implementation() - { - return "GPS_L1_CA_Observables"; - } - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); - void reset() - { - return; - } - - //! All blocks must have an item_size() function implementation - size_t item_size() - { - return sizeof(gr_complex); - } - -private: - gps_l1_ca_observables_cc_sptr observables_; - bool dump_; - //unsigned int fs_in_; - std::string dump_filename_; - std::string role_; - unsigned int in_streams_; - unsigned int out_streams_; -}; - -#endif diff --git a/src/algorithms/observables/adapters/gps_l2c_observables.cc b/src/algorithms/observables/adapters/gps_l2c_observables.cc deleted file mode 100644 index 4ceffd1a1..000000000 --- a/src/algorithms/observables/adapters/gps_l2c_observables.cc +++ /dev/null @@ -1,87 +0,0 @@ -/*! - * \file gps_l2c_observables.cc - * \brief Implementation of an adapter of a GPS L2 C(M) observables block - * to a ObservablesInterface - * \author Carles Fernandez 2016. carles.fernandez(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2016 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#include "gps_l2c_observables.h" -#include "configuration_interface.h" -#include -#include "GPS_L2C.h" - -using google::LogMessage; - -GpsL2CObservables::GpsL2CObservables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams) : - role_(role), - in_streams_(in_streams), - out_streams_(out_streams) -{ - std::string default_dump_filename = "./observables.dat"; - DLOG(INFO) << "role " << role; - dump_ = configuration->property(role + ".dump", false); - dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename); - unsigned int default_depth = GPS_L2C_HISTORY_DEEP; - unsigned int history_deep = configuration->property(role + ".averaging_depth", default_depth); - observables_ = hybrid_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep); - DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; -} - - -GpsL2CObservables::~GpsL2CObservables() -{} - - -void GpsL2CObservables::connect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to connect internally - DLOG(INFO) << "nothing to connect internally"; -} - - -void GpsL2CObservables::disconnect(gr::top_block_sptr top_block) -{ - if(top_block) { /* top_block is not null */}; - // Nothing to disconnect -} - - -gr::basic_block_sptr GpsL2CObservables::get_left_block() -{ - return observables_; -} - - -gr::basic_block_sptr GpsL2CObservables::get_right_block() -{ - return observables_; -} diff --git a/src/algorithms/observables/adapters/gps_l2c_observables.h b/src/algorithms/observables/adapters/gps_l2c_observables.h deleted file mode 100644 index aee598f73..000000000 --- a/src/algorithms/observables/adapters/gps_l2c_observables.h +++ /dev/null @@ -1,88 +0,0 @@ -/*! - * \file gps_l2c_observables.h - * \brief Implementation of an adapter of a GPS L2C(M) observables block - * to a ObservablesInterface - * \author Carles Fernandez 2016. carles.fernandez(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2016 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#ifndef GNSS_SDR_GPS_L2C_OBSERVABLES_H_ -#define GNSS_SDR_GPS_L2C_OBSERVABLES_H_ - -#include -#include "observables_interface.h" -#include "hybrid_observables_cc.h" - - -class ConfigurationInterface; - -/*! - * \brief This class implements an ObservablesInterface for GPS L2C - */ -class GpsL2CObservables : public ObservablesInterface -{ -public: - GpsL2CObservables(ConfigurationInterface* configuration, - std::string role, - unsigned int in_streams, - unsigned int out_streams); - virtual ~GpsL2CObservables(); - std::string role() - { - return role_; - } - - //! Returns "GPS_L2C_Observables" - std::string implementation() - { - return "GPS_L2C_Observables"; - } - void connect(gr::top_block_sptr top_block); - void disconnect(gr::top_block_sptr top_block); - gr::basic_block_sptr get_left_block(); - gr::basic_block_sptr get_right_block(); - void reset() - { - return; - } - - //! All blocks must have an item_size() function implementation - size_t item_size() - { - return sizeof(gr_complex); - } - -private: - hybrid_observables_cc_sptr observables_; - bool dump_; - std::string dump_filename_; - std::string role_; - unsigned int in_streams_; - unsigned int out_streams_; -}; - -#endif diff --git a/src/algorithms/observables/adapters/hybrid_observables.cc b/src/algorithms/observables/adapters/hybrid_observables.cc index e1d539d74..e076925b9 100644 --- a/src/algorithms/observables/adapters/hybrid_observables.cc +++ b/src/algorithms/observables/adapters/hybrid_observables.cc @@ -59,7 +59,7 @@ HybridObservables::HybridObservables(ConfigurationInterface* configuration, { default_depth = 100; } - unsigned int history_deep = configuration->property(role + ".averaging_depth", default_depth); + unsigned int history_deep = configuration->property(role + ".history_depth", default_depth); observables_ = hybrid_make_observables_cc(in_streams_, dump_, dump_filename_, history_deep); DLOG(INFO) << "pseudorange(" << observables_->unique_id() << ")"; } diff --git a/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt b/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt index aad359d18..ec5247aab 100644 --- a/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt +++ b/src/algorithms/observables/gnuradio_blocks/CMakeLists.txt @@ -17,8 +17,6 @@ # set(OBS_GR_BLOCKS_SOURCES - gps_l1_ca_observables_cc.cc - galileo_e1_observables_cc.cc hybrid_observables_cc.cc ) diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc deleted file mode 100644 index aab1e30c8..000000000 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.cc +++ /dev/null @@ -1,281 +0,0 @@ -/*! - * \file galileo_e1_observables_cc.cc - * \brief Implementation of the pseudorange computation block for Galileo E1 - * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com - * \author Javier Arribas 2013. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#include "galileo_e1_observables_cc.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "gnss_synchro.h" -#include "Galileo_E1.h" -#include "galileo_navigation_message.h" - - - -using google::LogMessage; - - -galileo_e1_observables_cc_sptr -galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) -{ - return galileo_e1_observables_cc_sptr(new galileo_e1_observables_cc(nchannels, dump, dump_filename, deep_history)); -} - - -galileo_e1_observables_cc::galileo_e1_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) : - gr::block("galileo_e1_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) -{ - // initialize internal vars - d_dump = dump; - d_nchannels = nchannels; - d_dump_filename = dump_filename; - history_deep = deep_history; - - for (unsigned int i = 0; i < d_nchannels; i++) - { - d_acc_carrier_phase_queue_rads.push_back(std::deque(d_nchannels)); - d_carrier_doppler_queue_hz.push_back(std::deque(d_nchannels)); - d_symbol_TOW_queue_s.push_back(std::deque(d_nchannels)); - } - - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - } - } - } -} - - - -galileo_e1_observables_cc::~galileo_e1_observables_cc() -{ - d_dump_file.close(); -} - - - -bool Galileo_pairCompare_gnss_synchro_Prn_delay_ms(const std::pair& a, const std::pair& b) -{ - return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms); -} - - - -bool Galileo_pairCompare_gnss_synchro_d_TOW_at_current_symbol(const std::pair& a, const std::pair& b) -{ - return (a.second.d_TOW_at_current_symbol) < (b.second.d_TOW_at_current_symbol); -} - - - -int galileo_e1_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ - Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer - Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // Get the output pointer - - Gnss_Synchro current_gnss_synchro[d_nchannels]; - std::map current_gnss_synchro_map; - std::map::iterator gnss_synchro_iter; - if (d_nchannels != ninput_items.size()) - { - LOG(WARNING) << "The Observables block is not well connected"; - } - - /* - * 1. Read the GNSS SYNCHRO objects from available channels - */ - for (unsigned int i = 0; i < d_nchannels; i++) - { - //Copy the telemetry decoder data to local copy - current_gnss_synchro[i] = in[i][0]; - /* - * 1.2 Assume no valid pseudoranges - */ - current_gnss_synchro[i].Flag_valid_pseudorange = false; - current_gnss_synchro[i].Pseudorange_m = 0.0; - - if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word - { - //record the word structure in a map for pseudorange computation - current_gnss_synchro_map.insert(std::pair(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - - //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE ####### - d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz); - d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads); - // save TOW history - d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol); - - if (d_carrier_doppler_queue_hz[i].size() > history_deep) - { - d_carrier_doppler_queue_hz[i].pop_front(); - } - if (d_acc_carrier_phase_queue_rads[i].size() > history_deep) - { - d_acc_carrier_phase_queue_rads[i].pop_front(); - } - if (d_symbol_TOW_queue_s[i].size() > history_deep) - { - d_symbol_TOW_queue_s[i].pop_front(); - } - } - else - { - // Clear the observables history for this channel - if (d_symbol_TOW_queue_s[i].size() > 0) - { - d_symbol_TOW_queue_s[i].clear(); - d_carrier_doppler_queue_hz[i].clear(); - d_acc_carrier_phase_queue_rads[i].clear(); - } - } - } - - /* - * 2. Compute RAW pseudoranges using COMMON RECEPTION TIME algorithm. Use only the valid channels (channels that are tracking a satellite) - */ - if(current_gnss_synchro_map.size() > 0) - { - /* - * 2.1 Use CURRENT set of measurements and find the nearest satellite - * common RX time algorithm - */ - // what is the most recent symbol TOW in the current set? -> this will be the reference symbol - gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), Galileo_pairCompare_gnss_synchro_d_TOW_at_current_symbol); - double d_TOW_reference = gnss_synchro_iter->second.d_TOW_at_current_symbol; - double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms; - //int reference_channel= gnss_synchro_iter->second.Channel_ID; - - // Now compute RX time differences due to the PRN alignment in the correlators - double traveltime_ms; - double pseudorange_m; - double delta_rx_time_ms; - arma::vec symbol_TOW_vec_s; - arma::vec dopper_vec_hz; - arma::vec dopper_vec_interp_hz; - arma::vec acc_phase_vec_rads; - arma::vec acc_phase_vec_interp_rads; - arma::vec desired_symbol_TOW(1); - for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++) - { - // compute the required symbol history shift in order to match the reference symbol - delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms; - //compute the pseudorange - traveltime_ms = (d_TOW_reference-gnss_synchro_iter->second.d_TOW_at_current_symbol) * 1000.0 + delta_rx_time_ms + GALILEO_STARTOFFSET_ms; - pseudorange_m = traveltime_ms * GALILEO_C_m_ms; // [m] - // update the pseudorange object - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference * 1000.0) / 1000.0 + GALILEO_STARTOFFSET_ms / 1000.0; - - if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep) - { - // compute interpolated observation values for Doppler and Accumulate carrier phase - symbol_TOW_vec_s = arma::vec(std::vector(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end())); - acc_phase_vec_rads = arma::vec(std::vector(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end())); - dopper_vec_hz = arma::vec(std::vector(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end())); - desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0; - // Curve fitting to cuadratic function - arma::mat A = arma::ones(history_deep, 2); - A.col(1) = symbol_TOW_vec_s; - //A.col(2)=symbol_TOW_vec_s % symbol_TOW_vec_s; - arma::mat coef_acc_phase(1,3); - arma::mat pinv_A = arma::pinv(A.t() * A) * A.t(); - coef_acc_phase = pinv_A * acc_phase_vec_rads; - arma::mat coef_doppler(1,3); - coef_doppler = pinv_A * dopper_vec_hz; - arma::vec acc_phase_lin; - arma::vec carrier_doppler_lin; - acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0];//+coef_acc_phase[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0]; - carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0];//+coef_doppler[2]*desired_symbol_TOW[0]*desired_symbol_TOW[0]; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = acc_phase_lin[0]; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz = carrier_doppler_lin[0]; - } - } - } - - if(d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels ; i++) - { - tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GALILEO_TWO_PI; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Pseudorange_m; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].PRN; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } - } - - consume_each(1); //one by one - for (unsigned int i = 0; i < d_nchannels ; i++) - { - *out[i] = current_gnss_synchro[i]; - } - if (noutput_items == 0) - { - LOG(WARNING) << "noutput_items = 0"; - } - return 1; -} diff --git a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h deleted file mode 100644 index 1cb3ef4c4..000000000 --- a/src/algorithms/observables/gnuradio_blocks/galileo_e1_observables_cc.h +++ /dev/null @@ -1,78 +0,0 @@ -/*! - * \file galileo_e1_observables_cc.h - * \brief Interface of the observables computation block for Galileo E1 - * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com - * \author Javier Arribas 2013. jarribas(at)cttc.es - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#ifndef GNSS_SDR_GALILEO_E1_OBSERVABLES_CC_H -#define GNSS_SDR_GALILEO_E1_OBSERVABLES_CC_H - - -#include -#include -#include - - -class galileo_e1_observables_cc; - -typedef boost::shared_ptr galileo_e1_observables_cc_sptr; - -galileo_e1_observables_cc_sptr -galileo_e1_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename, unsigned int deep_history); - -/*! - * \brief This class implements a block that computes Galileo observables - */ -class galileo_e1_observables_cc : public gr::block -{ -public: - ~galileo_e1_observables_cc (); - - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - -private: - friend galileo_e1_observables_cc_sptr - galileo_e1_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); - galileo_e1_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); - - //Tracking observable history - std::vector> d_acc_carrier_phase_queue_rads; - std::vector> d_carrier_doppler_queue_hz; - std::vector> d_symbol_TOW_queue_s; - - // class private vars - bool d_dump; - unsigned int d_nchannels; - unsigned int history_deep; - std::string d_dump_filename; - std::ofstream d_dump_file; -}; - -#endif diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc deleted file mode 100644 index db73b50dc..000000000 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ /dev/null @@ -1,278 +0,0 @@ -/*! - * \file gps_l1_ca_observables_cc.cc - * \brief Implementation of the pseudorange computation block for GPS L1 C/A - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "gps_l1_ca_observables_cc.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "control_message_factory.h" -#include "gnss_synchro.h" -#include "GPS_L1_CA.h" - - - -using google::LogMessage; - - -gps_l1_ca_observables_cc_sptr -gps_l1_ca_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) -{ - return gps_l1_ca_observables_cc_sptr(new gps_l1_ca_observables_cc(nchannels, dump, dump_filename, deep_history)); -} - - -gps_l1_ca_observables_cc::gps_l1_ca_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) : - gr::block("gps_l1_ca_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) -{ - - // initialize internal vars - d_dump = dump; - d_nchannels = nchannels; - d_dump_filename = dump_filename; - history_deep = deep_history; - - for (unsigned int i = 0; i < d_nchannels; i++) - { - d_acc_carrier_phase_queue_rads.push_back(std::deque(d_nchannels)); - d_carrier_doppler_queue_hz.push_back(std::deque(d_nchannels)); - d_symbol_TOW_queue_s.push_back(std::deque(d_nchannels)); - } - - // ############# ENABLE DATA FILE LOG ################# - if (d_dump == true) - { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - } - } - } -} - - - -gps_l1_ca_observables_cc::~gps_l1_ca_observables_cc() -{ - d_dump_file.close(); -} - -bool pairCompare_gnss_synchro_Prn_delay_ms(const std::pair& a, const std::pair& b) -{ - return (a.second.Prn_timestamp_ms) < (b.second.Prn_timestamp_ms); -} - - -bool pairCompare_gnss_synchro_d_TOW_at_current_symbol(const std::pair& a, const std::pair& b) -{ - return (a.second.d_TOW_at_current_symbol) < (b.second.d_TOW_at_current_symbol); -} - - -int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) -{ - Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer - Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // Get the output pointer - - Gnss_Synchro current_gnss_synchro[d_nchannels]; - std::map current_gnss_synchro_map; - std::map::iterator gnss_synchro_iter; - - if (d_nchannels != ninput_items.size()) - { - LOG(WARNING) << "The Observables block is not well connected"; - } - - /* - * 1. Read the GNSS SYNCHRO objects from available channels - */ - for (unsigned int i = 0; i < d_nchannels; i++) - { - //Copy the telemetry decoder data to local copy - current_gnss_synchro[i] = in[i][0]; - /* - * 1.2 Assume no valid pseudoranges - */ - current_gnss_synchro[i].Flag_valid_pseudorange = false; - current_gnss_synchro[i].Pseudorange_m = 0.0; - if (current_gnss_synchro[i].Flag_valid_word) //if this channel have valid word - { - //record the word structure in a map for pseudorange computation - current_gnss_synchro_map.insert(std::pair(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - - //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE ####### - d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz); - d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads); - // save TOW history - d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol); - - if (d_carrier_doppler_queue_hz[i].size() > history_deep) - { - d_carrier_doppler_queue_hz[i].pop_front(); - } - if (d_acc_carrier_phase_queue_rads[i].size() > history_deep) - { - d_acc_carrier_phase_queue_rads[i].pop_front(); - } - if (d_symbol_TOW_queue_s[i].size() > history_deep) - { - d_symbol_TOW_queue_s[i].pop_front(); - } - } - else - { - // Clear the observables history for this channel - if (d_symbol_TOW_queue_s[i].size() > 0) - { - d_symbol_TOW_queue_s[i].clear(); - d_carrier_doppler_queue_hz[i].clear(); - d_acc_carrier_phase_queue_rads[i].clear(); - } - } - } - - /* - * 2. Compute RAW pseudoranges using COMMON RECEPTION TIME algorithm. Use only the valid channels (channels that are tracking a satellite) - */ - if(current_gnss_synchro_map.size() > 0) - { - /* - * 2.1 Use CURRENT set of measurements and find the nearest satellite - * common RX time algorithm - */ - // what is the most recent symbol TOW in the current set? -> this will be the reference symbol - gnss_synchro_iter = max_element(current_gnss_synchro_map.begin(), current_gnss_synchro_map.end(), pairCompare_gnss_synchro_d_TOW_at_current_symbol); - double d_TOW_reference = gnss_synchro_iter->second.d_TOW_at_current_symbol; - double d_ref_PRN_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms; - - // Now compute RX time differences due to the PRN alignment in the correlators - double traveltime_ms; - double pseudorange_m; - double delta_rx_time_ms; - arma::vec symbol_TOW_vec_s; - arma::vec dopper_vec_hz; - arma::vec dopper_vec_interp_hz; - arma::vec acc_phase_vec_rads; - arma::vec acc_phase_vec_interp_rads; - arma::vec desired_symbol_TOW(1); - for(gnss_synchro_iter = current_gnss_synchro_map.begin(); gnss_synchro_iter != current_gnss_synchro_map.end(); gnss_synchro_iter++) - { - // compute the required symbol history shift in order to match the reference symbol - delta_rx_time_ms = gnss_synchro_iter->second.Prn_timestamp_ms - d_ref_PRN_rx_time_ms; - //compute the pseudorange - traveltime_ms = (d_TOW_reference - gnss_synchro_iter->second.d_TOW_at_current_symbol) * 1000.0 + delta_rx_time_ms + GPS_STARTOFFSET_ms; - //convert to meters and remove the receiver time offset in meters - pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] - // update the pseudorange object - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = d_TOW_reference + GPS_STARTOFFSET_ms / 1000.0; - - if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep) - { - // compute interpolated observation values for Doppler and Accumulate carrier phase - symbol_TOW_vec_s = arma::vec(std::vector(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end())); - acc_phase_vec_rads = arma::vec(std::vector(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end())); - dopper_vec_hz = arma::vec(std::vector(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end())); - - desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0; - // arma::interp1(symbol_TOW_vec_s,dopper_vec_hz,desired_symbol_TOW,dopper_vec_interp_hz); - // arma::interp1(symbol_TOW_vec_s,acc_phase_vec_rads,desired_symbol_TOW,acc_phase_vec_interp_rads); - // Curve fitting to quadratic function - arma::mat A = arma::ones (history_deep, 2); - A.col(1) = symbol_TOW_vec_s; - - arma::mat coef_acc_phase(1,3); - arma::mat pinv_A = arma::pinv(A.t() * A) * A.t(); - coef_acc_phase = pinv_A * acc_phase_vec_rads; - arma::mat coef_doppler(1,3); - coef_doppler = pinv_A * dopper_vec_hz; - arma::vec acc_phase_lin; - arma::vec carrier_doppler_lin; - acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0]; - carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0]; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = acc_phase_lin[0]; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz = carrier_doppler_lin[0]; - } - } - } - - if(d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try - { - double tmp_double; - for (unsigned int i = 0; i < d_nchannels; i++) - { - tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_Doppler_hz; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Carrier_phase_rads/GPS_TWO_PI; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].Pseudorange_m; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_gnss_synchro[i].PRN; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - } - } - catch (const std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); - } - } - - consume_each(1); //one by one - for (unsigned int i = 0; i < d_nchannels; i++) - { - *out[i] = current_gnss_synchro[i]; - } - if (noutput_items == 0) - { - LOG(WARNING) << "noutput_items = 0"; - } - return 1; -} diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h deleted file mode 100644 index 38a1e7f4d..000000000 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.h +++ /dev/null @@ -1,79 +0,0 @@ -/*! - * \file gps_l1_ca_observables_cc.h - * \brief Interface of the pseudorange computation block for GPS L1 C/A - * \author Javier Arribas, 2011. jarribas(at)cttc.es - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - - -#ifndef GNSS_SDR_GPS_L1_CA_OBSERVABLES_CC_H -#define GNSS_SDR_GPS_L1_CA_OBSERVABLES_CC_H - -#include -#include -#include -#include -#include -#include - - -class gps_l1_ca_observables_cc; - -typedef boost::shared_ptr gps_l1_ca_observables_cc_sptr; - -gps_l1_ca_observables_cc_sptr -gps_l1_ca_make_observables_cc(unsigned int n_channels, bool dump, std::string dump_filename, unsigned int deep_history); - -/*! - * \brief This class implements a block that computes GPS L1 C/A observables - */ -class gps_l1_ca_observables_cc : public gr::block -{ -public: - ~gps_l1_ca_observables_cc (); - //void set_fs_in(unsigned long int fs_in) {d_fs_in = fs_in;}; - int general_work (int noutput_items, gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, gr_vector_void_star &output_items); - -private: - friend gps_l1_ca_observables_cc_sptr - gps_l1_ca_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); - gps_l1_ca_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); - - - //Tracking observable history - std::vector> d_acc_carrier_phase_queue_rads; - std::vector> d_carrier_doppler_queue_hz; - std::vector> d_symbol_TOW_queue_s; - - // class private vars - bool d_dump; - unsigned int d_nchannels; - unsigned int history_deep; - std::string d_dump_filename; - std::ofstream d_dump_file; -}; - -#endif diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 4366093ba..87d1fe2b5 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -1,8 +1,7 @@ /*! * \file hybrid_observables_cc.cc * \brief Implementation of the pseudorange computation block for Galileo E1 - * \author Mara Branzanti 2013. mara.branzanti(at)gmail.com - * \author Javier Arribas 2013. jarribas(at)cttc.es + * \author Javier Arribas 2017. jarribas(at)cttc.es * * ------------------------------------------------------------------------- * @@ -34,12 +33,11 @@ #include #include #include -#include #include +#include #include #include #include -#include "gnss_synchro.h" #include "Galileo_E1.h" #include "GPS_L1_CA.h" @@ -56,39 +54,43 @@ hybrid_make_observables_cc(unsigned int nchannels, bool dump, std::string dump_f hybrid_observables_cc::hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history) : - gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), - gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) + gr::block("hybrid_observables_cc", gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro)), + gr::io_signature::make(nchannels, nchannels, sizeof(Gnss_Synchro))) { // initialize internal vars d_dump = dump; d_nchannels = nchannels; d_dump_filename = dump_filename; history_deep = deep_history; - + T_rx_s=0.0; + T_rx_step_s=1e-3;// todo: move to gnss-sdr config for (unsigned int i = 0; i < d_nchannels; i++) - { - d_acc_carrier_phase_queue_rads.push_back(std::deque(d_nchannels)); - d_carrier_doppler_queue_hz.push_back(std::deque(d_nchannels)); - d_symbol_TOW_queue_s.push_back(std::deque(d_nchannels)); - } + { + d_gnss_synchro_history_queue.push_back(std::deque()); + } + //todo: this is a gnuradio scheduler hack. + // Migrate the queues to gnuradio set_history to see if the scheduler can handle + // the multiple output flow + d_max_noutputs=100; + this->set_min_noutput_items(100); // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) + { + if (d_dump_file.is_open() == false) { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure & e) - { - LOG(WARNING) << "Exception opening observables dump file " << e.what(); - } - } + try + { + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit ); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Observables dump enabled Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure & e) + { + LOG(WARNING) << "Exception opening observables dump file " << e.what(); + } } + } } @@ -98,173 +100,254 @@ hybrid_observables_cc::~hybrid_observables_cc() } -bool Hybrid_pairCompare_gnss_synchro_d_TOW_hybrid_at_current_symbol(const std::pair& a, const std::pair& b) +bool Hybrid_pairCompare_gnss_synchro_sample_counter(const std::pair& a, const std::pair& b) { - return (a.second.d_TOW_hybrid_at_current_symbol) < (b.second.d_TOW_hybrid_at_current_symbol); + return (a.second.Tracking_sample_counter) < (b.second.Tracking_sample_counter); } +bool Hybrid_valueCompare_gnss_synchro_sample_counter(const Gnss_Synchro& a, unsigned long int b) +{ + return (a.Tracking_sample_counter) < (b); +} + +bool Hybrid_valueCompare_gnss_synchro_receiver_time(const Gnss_Synchro& a, double b) +{ + return (((double)a.Tracking_sample_counter+a.Code_phase_samples)/(double)a.fs) < (b); +} + +bool Hybrid_pairCompare_gnss_synchro_d_TOW(const std::pair& a, const std::pair& b) +{ + return (a.second.TOW_at_current_symbol_s) < (b.second.TOW_at_current_symbol_s); +} +bool Hybrid_valueCompare_gnss_synchro_d_TOW(const Gnss_Synchro& a, double b) +{ + return (a.TOW_at_current_symbol_s) < (b); +} int hybrid_observables_cc::general_work (int noutput_items, - gr_vector_int &ninput_items, - gr_vector_const_void_star &input_items, - gr_vector_void_star &output_items) + gr_vector_int &ninput_items, + gr_vector_const_void_star &input_items, + gr_vector_void_star &output_items) { Gnss_Synchro **in = (Gnss_Synchro **) &input_items[0]; // Get the input pointer Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // Get the output pointer + int n_outputs=0; + int n_consume[d_nchannels]; + double past_history_s=100e-3; Gnss_Synchro current_gnss_synchro[d_nchannels]; - std::map current_gnss_synchro_map; - std::map::iterator gnss_synchro_iter; - - if (d_nchannels != ninput_items.size()) - { - LOG(WARNING) << "The Observables block is not well connected"; - } /* - * 1. Read the GNSS SYNCHRO objects from available channels + * 1. Read the GNSS SYNCHRO objects from available channels. + * Multi-rate GNURADIO Block. Read how many input items are avaliable in each channel + * Record all synchronization data into queues */ for (unsigned int i = 0; i < d_nchannels; i++) + { + n_consume[i]=ninput_items[i];// full throttle + for (int j=0;j(current_gnss_synchro[i].Channel_ID, current_gnss_synchro[i])); - //################### SAVE DOPPLER AND ACC CARRIER PHASE HISTORIC DATA FOR INTERPOLATION IN OBSERVABLE MODULE ####### - d_carrier_doppler_queue_hz[i].push_back(current_gnss_synchro[i].Carrier_Doppler_hz); - d_acc_carrier_phase_queue_rads[i].push_back(current_gnss_synchro[i].Carrier_phase_rads); - // save TOW history - d_symbol_TOW_queue_s[i].push_back(current_gnss_synchro[i].d_TOW_at_current_symbol); - if (d_carrier_doppler_queue_hz[i].size() > history_deep) - { - d_carrier_doppler_queue_hz[i].pop_front(); - } - if (d_acc_carrier_phase_queue_rads[i].size() > history_deep) - { - d_acc_carrier_phase_queue_rads[i].pop_front(); - } - if (d_symbol_TOW_queue_s[i].size() > history_deep) - { - d_symbol_TOW_queue_s[i].pop_front(); - } - } - else - { - // Clear the observables history for this channel - if (d_symbol_TOW_queue_s[i].size() > 0) - { - d_symbol_TOW_queue_s[i].clear(); - d_carrier_doppler_queue_hz[i].clear(); - d_acc_carrier_phase_queue_rads[i].clear(); - } - } + d_gnss_synchro_history_queue[i].push_back(in[i][j]); } + //std::cout<<"push["<second.Channel_ID << " tracking GNSS System " - << gnss_synchro_iter->second.System << " has PRN start at= " << gnss_synchro_iter->second.Prn_timestamp_ms - << " [ms], d_TOW_at_current_symbol = " << (gnss_synchro_iter->second.d_TOW_at_current_symbol) * 1000 - << " [ms], d_TOW_hybrid_at_current_symbol = "<< (gnss_synchro_iter->second.d_TOW_hybrid_at_current_symbol) * 1000 - << "[ms], delta_rx_time_ms = " << delta_rx_time_ms << "[ms], travel_time = " << traveltime_ms - << ", pseudorange[m] = "<< pseudorange_m; - - // update the pseudorange object - //current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_hybrid_at_current_symbol = round(d_TOW_reference * 1000) / 1000 + start_offset_ms / 1000.0; - if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep) - { - // compute interpolated observation values for Doppler and Accumulate carrier phase - symbol_TOW_vec_s = arma::vec(std::vector(d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].begin(), d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].end())); - acc_phase_vec_rads = arma::vec(std::vector(d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].begin(), d_acc_carrier_phase_queue_rads[gnss_synchro_iter->second.Channel_ID].end())); - dopper_vec_hz = arma::vec(std::vector(d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].begin(), d_carrier_doppler_queue_hz[gnss_synchro_iter->second.Channel_ID].end())); - - desired_symbol_TOW[0] = symbol_TOW_vec_s[history_deep - 1] + delta_rx_time_ms / 1000.0; - - // Curve fitting to cuadratic function - arma::mat A = arma::ones (history_deep, 2); - A.col(1) = symbol_TOW_vec_s; - - arma::mat coef_acc_phase(1,3); - arma::mat pinv_A = arma::pinv(A.t() * A) * A.t(); - coef_acc_phase = pinv_A * acc_phase_vec_rads; - arma::mat coef_doppler(1,3); - coef_doppler = pinv_A * dopper_vec_hz; - arma::vec acc_phase_lin; - arma::vec carrier_doppler_lin; - acc_phase_lin = coef_acc_phase[0] + coef_acc_phase[1] * desired_symbol_TOW[0]; - carrier_doppler_lin = coef_doppler[0] + coef_doppler[1] * desired_symbol_TOW[0]; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_phase_rads = acc_phase_lin[0]; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Carrier_Doppler_hz = carrier_doppler_lin[0]; - } - } - } - - if(d_dump == true) - { - // MULTIPLEXED FILE RECORDING - Record results to file - try + if (d_gnss_synchro_history_queue[i].size()::iterator gnss_synchro_map_iter; + std::deque::iterator gnss_synchro_deque_iter; + + //1. If the RX time is not set, set the Rx time + if (T_rx_s==0) + { + //0. Read a gnss_synchro snapshot from the queue and store it in a map + std::map gnss_synchro_map; + for (unsigned int i = 0; i < d_nchannels; i++) + { + gnss_synchro_map.insert(std::pair( + d_gnss_synchro_history_queue[i].front().Channel_ID, + d_gnss_synchro_history_queue[i].front())); + + } + gnss_synchro_map_iter = min_element(gnss_synchro_map.begin(), + gnss_synchro_map.end(), + Hybrid_pairCompare_gnss_synchro_sample_counter); + T_rx_s = (double)gnss_synchro_map_iter->second.Tracking_sample_counter/(double)gnss_synchro_map_iter->second.fs; + T_rx_s = floor(T_rx_s*1000.0)/1000.0;// truncate to ms + T_rx_s +=past_history_s; //increase T_rx to have a minimum past history to interpolate + } + + //2. Realign RX time in all valid channels + std::map realigned_gnss_synchro_map;//container for the aligned set of observables for the selected T_rx + std::map adjacent_gnss_synchro_map; //container for the previous observable values to interpolate + //shift channels history to match the reference TOW + for (unsigned int i = 0; i < d_nchannels; i++) + { + gnss_synchro_deque_iter = std::lower_bound(d_gnss_synchro_history_queue[i].begin(), + d_gnss_synchro_history_queue[i].end(), + T_rx_s, + Hybrid_valueCompare_gnss_synchro_receiver_time); + if (gnss_synchro_deque_iter!=d_gnss_synchro_history_queue[i].end()) + { + if (gnss_synchro_deque_iter->Flag_valid_word==true) + { + double T_rx_channel=(double)gnss_synchro_deque_iter->Tracking_sample_counter/(double)gnss_synchro_deque_iter->fs; + double delta_T_rx_s=T_rx_channel-T_rx_s; + + //check that T_rx difference is less than a threshold (the correlation interval) + if (delta_T_rx_s*1000.0<(double)gnss_synchro_deque_iter->correlation_length_ms) { - tmp_double = current_gnss_synchro[i].d_TOW_at_current_symbol; + //record the word structure in a map for pseudorange computation + //save the previous observable + int distance=std::distance(d_gnss_synchro_history_queue[i].begin(), gnss_synchro_deque_iter); + if (distance>0) + { + double T_rx_channel_prev=(double)d_gnss_synchro_history_queue[i].at(distance-1).Tracking_sample_counter/(double)gnss_synchro_deque_iter->fs; + double delta_T_rx_s_prev=T_rx_channel_prev-T_rx_s; + if (fabs(delta_T_rx_s_prev)( + d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID, + d_gnss_synchro_history_queue[i].at(distance-1))); + adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, + *gnss_synchro_deque_iter)); + }else{ + realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, + *gnss_synchro_deque_iter)); + adjacent_gnss_synchro_map.insert(std::pair( + d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID, + d_gnss_synchro_history_queue[i].at(distance-1))); + } + }else{ + realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, + *gnss_synchro_deque_iter)); + } + + }else{ + //std::cout<<"ch["< this will be the reference symbol + gnss_synchro_map_iter = max_element(realigned_gnss_synchro_map.begin(), + realigned_gnss_synchro_map.end(), + Hybrid_pairCompare_gnss_synchro_d_TOW); + double ref_fs_hz=(double)gnss_synchro_map_iter->second.fs; + + // compute interpolated TOW value at T_rx_s + int ref_channel_key=gnss_synchro_map_iter->second.Channel_ID; + Gnss_Synchro adj_obs=adjacent_gnss_synchro_map.at(ref_channel_key); + double ref_adj_T_rx_s=(double)adj_obs.Tracking_sample_counter/ref_fs_hz + +adj_obs.Code_phase_samples/ref_fs_hz; + + double d_TOW_reference = gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + double d_ref_T_rx_s=(double)gnss_synchro_map_iter->second.Tracking_sample_counter/ref_fs_hz + +gnss_synchro_map_iter->second.Code_phase_samples/ref_fs_hz; + + double selected_T_rx_s=T_rx_s; + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + double ref_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s+(selected_T_rx_s-ref_adj_T_rx_s) + *(d_TOW_reference-adj_obs.TOW_at_current_symbol_s)/(d_ref_T_rx_s-ref_adj_T_rx_s); + + //std::cout<<"DELTA T REF:"<second.PRN<second.fs; + channel_TOW_s=gnss_synchro_map_iter->second.TOW_at_current_symbol_s; + channel_T_rx_s=(double)gnss_synchro_map_iter->second.Tracking_sample_counter/channel_fs_hz + +gnss_synchro_map_iter->second.Code_phase_samples/channel_fs_hz; + // compute interpolated observation values + // two points linear interpolation using adjacent (adj) values: y=y1+(x-x1)*(y2-y1)/(x2-x1) + // TOW at the selected receiver time T_rx_s + int element_key=gnss_synchro_map_iter->second.Channel_ID; + adj_obs=adjacent_gnss_synchro_map.at(element_key); + + double adj_T_rx_s=(double)adj_obs.Tracking_sample_counter/channel_fs_hz + +adj_obs.Code_phase_samples/channel_fs_hz; + + double channel_TOW_at_T_rx_s = adj_obs.TOW_at_current_symbol_s+(selected_T_rx_s-adj_T_rx_s) + *(channel_TOW_s-adj_obs.TOW_at_current_symbol_s)/(channel_T_rx_s-adj_T_rx_s); + + //Doppler and Accumulated carrier phase + double Carrier_phase_lin_rads = adj_obs.Carrier_phase_rads+(selected_T_rx_s-adj_T_rx_s) + *(gnss_synchro_map_iter->second.Carrier_phase_rads-adj_obs.Carrier_phase_rads)/(channel_T_rx_s-adj_T_rx_s); + double Carrier_Doppler_lin_hz = adj_obs.Carrier_Doppler_hz+(selected_T_rx_s-adj_T_rx_s) + *(gnss_synchro_map_iter->second.Carrier_Doppler_hz-adj_obs.Carrier_Doppler_hz)/(channel_T_rx_s-adj_T_rx_s); + + + //compute the pseudorange (no rx time offset correction) + traveltime_ms = (ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s) * 1000.0 + + GPS_STARTOFFSET_ms; + //convert to meters + pseudorange_m = traveltime_ms * GPS_C_m_ms; // [m] + // update the pseudorange object + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID] = gnss_synchro_map_iter->second; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Flag_valid_pseudorange = true; + // Save the estimated RX time (no RX clock offset correction yet!) + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].RX_time = ref_TOW_at_T_rx_s + GPS_STARTOFFSET_ms / 1000.0; + + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_phase_rads = Carrier_phase_lin_rads; + current_gnss_synchro[gnss_synchro_map_iter->second.Channel_ID].Carrier_Doppler_hz = Carrier_Doppler_lin_hz; + //debug + //double delta_T_rx_s_previous=((double)adjacent_gnss_synchro_map.at(gnss_synchro_map_iter->second.Channel_ID).Tracking_sample_counter/(double)gnss_synchro_map_iter->second.fs - T_rx_s); + +// std::cout<<"["<second.PRN<<"] delta_TOW at T_rx: "<<(ref_TOW_at_T_rx_s - channel_TOW_at_T_rx_s)*1000.0 +// <<" [ms] delta_TOW_ms: "<<(d_TOW_reference - gnss_synchro_map_iter->second.TOW_at_current_symbol_s) * 1000.0 +// <<" Pr: "<n_outputs); - consume_each(1); //consume one by one + //Multi-rate consume! + for (unsigned int i=0; i #include #include +#include "gnss_synchro.h" class hybrid_observables_cc; @@ -61,11 +62,11 @@ private: hybrid_observables_cc(unsigned int nchannels, bool dump, std::string dump_filename, unsigned int deep_history); //Tracking observable history - std::vector> d_acc_carrier_phase_queue_rads; - std::vector> d_carrier_doppler_queue_hz; - std::vector> d_symbol_TOW_queue_s; + std::vector> d_gnss_synchro_history_queue; - // class private vars + double T_rx_s; + double T_rx_step_s; + int d_max_noutputs; bool d_dump; unsigned int d_nchannels; unsigned int history_deep; diff --git a/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc index 508127cfa..053bce863 100644 --- a/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/galileo_e1b_telemetry_decoder.cc @@ -59,10 +59,6 @@ GalileoE1BTelemetryDecoder::GalileoE1BTelemetryDecoder(ConfigurationInterface* c telemetry_decoder_ = galileo_e1b_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")"; - //decimation factor - int decimation_factor = configuration->property(role + ".decimation_factor", 1); - telemetry_decoder_->set_decimation(decimation_factor); - channel_ = 0; } diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc index fe22b7a66..bc9792a7b 100644 --- a/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/gps_l1_ca_telemetry_decoder.cc @@ -58,9 +58,6 @@ GpsL1CaTelemetryDecoder::GpsL1CaTelemetryDecoder(ConfigurationInterface* configu telemetry_decoder_ = gps_l1_ca_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")"; - //decimation factor - int decimation_factor = configuration->property(role + ".decimation_factor", 1); - telemetry_decoder_->set_decimation(decimation_factor); DLOG(INFO) << "global navigation message queue assigned to telemetry_decoder ("<< telemetry_decoder_->unique_id() << ")"; channel_ = 0; } diff --git a/src/algorithms/telemetry_decoder/adapters/gps_l2c_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/gps_l2c_telemetry_decoder.cc index 2d352b24e..935998578 100644 --- a/src/algorithms/telemetry_decoder/adapters/gps_l2c_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/gps_l2c_telemetry_decoder.cc @@ -59,9 +59,6 @@ GpsL2CTelemetryDecoder::GpsL2CTelemetryDecoder(ConfigurationInterface* configura telemetry_decoder_ = gps_l2c_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")"; - //decimation factor - int decimation_factor = configuration->property(role + ".decimation_factor", 1); - telemetry_decoder_->set_decimation(decimation_factor); LOG(INFO) << "global navigation message queue assigned to telemetry_decoder (" << telemetry_decoder_->unique_id() << ")" << "role " << role; channel_ = 0; } diff --git a/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc b/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc index 0264acdba..41d2dcf38 100644 --- a/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc +++ b/src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc @@ -34,9 +34,6 @@ #include #include #include "concurrent_queue.h" -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" #include "sbas_ephemeris.h" #include "configuration_interface.h" #include "sbas_l1_telemetry_decoder_cc.h" diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc index 93e02e3fa..5f8e637a8 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e1b_telemetry_decoder_cc.cc @@ -158,7 +158,6 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc( d_stat = 0; d_preamble_index = 0; - d_preamble_time_seconds = 0; d_flag_frame_sync = false; d_flag_parity = false; @@ -169,10 +168,7 @@ galileo_e1b_telemetry_decoder_cc::galileo_e1b_telemetry_decoder_cc( flag_even_word_arrived = 0; d_flag_preamble = false; d_channel = 0; - Prn_timestamp_at_preamble_ms = 0.0; flag_TOW_set = false; - d_average_count = 0; - d_decimation_output_factor = 1; } @@ -267,16 +263,16 @@ void galileo_e1b_telemetry_decoder_cc::decode_word(double *page_part_symbols,int this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj)); //debug std::cout << "Galileo almanac received!" << std::endl; - LOG(INFO) << "GPS_to_Galileo time conversion:"; - LOG(INFO) << "A0G=" << tmp_obj->A_0G_10; - LOG(INFO) << "A1G=" << tmp_obj->A_1G_10; - LOG(INFO) << "T0G=" << tmp_obj->t_0G_10; - LOG(INFO) << "WN_0G_10=" << tmp_obj->WN_0G_10; - LOG(INFO) << "Current parameters:"; - LOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol; - LOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0; + DLOG(INFO) << "GPS_to_Galileo time conversion:"; + DLOG(INFO) << "A0G=" << tmp_obj->A_0G_10; + DLOG(INFO) << "A1G=" << tmp_obj->A_1G_10; + DLOG(INFO) << "T0G=" << tmp_obj->t_0G_10; + DLOG(INFO) << "WN_0G_10=" << tmp_obj->WN_0G_10; + DLOG(INFO) << "Current parameters:"; + DLOG(INFO) << "d_TOW_at_current_symbol=" << d_TOW_at_current_symbol; + DLOG(INFO) << "d_nav.WN_0=" << d_nav.WN_0; delta_t = tmp_obj->A_0G_10 + tmp_obj->A_1G_10 * (d_TOW_at_current_symbol - tmp_obj->t_0G_10 + 604800 * (fmod((d_nav.WN_0 - tmp_obj->WN_0G_10), 64))); - LOG(INFO) << "delta_t=" << delta_t << "[s]"; + DLOG(INFO) << "delta_t=" << delta_t << "[s]"; } } @@ -371,11 +367,11 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items __attribut d_CRC_error_counter = 0; d_flag_preamble = true; //valid preamble indicator (initialized to false every work()) d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P) - d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble if (!d_flag_frame_sync) { d_flag_frame_sync = true; - LOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]"; + DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " + << in[0][0].Tracking_sample_counter << " [samples]"; } } else @@ -407,7 +403,6 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items __attribut // Since we detected the preamble, then, we are in the last symbol of that preamble, or just at the start of the first page symbol. //flag preamble is true after the all page (even and odd) is received. I/NAV page period is 2 SECONDS { - Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; if(d_nav.flag_TOW_5 == true) //page 5 arrived and decoded, so we are in the odd page (since Tow refers to the even page, we have to add 1 sec) { //std::cout<< "Using TOW_5 for timestamping" << std::endl; @@ -458,53 +453,33 @@ int galileo_e1b_telemetry_decoder_cc::general_work (int noutput_items __attribut current_synchro_data.Flag_valid_word = false; } - current_synchro_data.d_TOW = d_TOW_at_Preamble; - current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; - current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol - delta_t; //delta_t = t_gal - t_gps ----> t_gps = t_gal -delta_t - current_synchro_data.Flag_preamble = d_flag_preamble; - current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0; - current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms; + current_synchro_data.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol*1000.0)/1000.0; + current_synchro_data.TOW_at_current_symbol_s -=delta_t; //Galileo to GPS TOW if(d_dump == true) { // MULTIPLEXED FILE RECORDING - Record results to file try { - double tmp_double; - tmp_double = d_TOW_at_current_symbol; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_synchro_data.Prn_timestamp_ms; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = d_TOW_at_Preamble; - d_dump_file.write((char*)&tmp_double, sizeof(double)); + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = d_TOW_at_current_symbol; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_ulong_int = current_synchro_data.Tracking_sample_counter; + d_dump_file.write((char*)&tmp_ulong_int, sizeof(unsigned long int)); + tmp_double = d_TOW_at_Preamble; + d_dump_file.write((char*)&tmp_double, sizeof(double)); } - catch (const std::ifstream::failure& e) + catch (const std::ifstream::failure & e) { LOG(WARNING) << "Exception writing observables dump file " << e.what(); } } - //todo: implement averaging - d_average_count++; + //3. Make the output (copy the object contents to the GNURadio reserved memory) + *out[0] = current_synchro_data; + //std::cout<<"GPS L1 TLM output on CH="<d_channel << " SAMPLE STAMP="<d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]"; - } + DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " + << in[0][0].Tracking_sample_counter << " [samples]"; } d_symbol_counter = 0; // d_page_symbols start right after preamble and finish at the end of next preamble. } else @@ -489,8 +486,6 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut //update TOW at the preamble instant //We expect a preamble each 10 seconds (FNAV page period) { - Prn_timestamp_at_preamble_ms = d_preamble_time_seconds * 1000; - //Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; if (d_nav.flag_TOW_1 == true) { d_TOW_at_Preamble = d_nav.FNAV_TOW_1; @@ -538,26 +533,23 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut current_synchro_data.Flag_valid_word = false; } - current_synchro_data.d_TOW = d_TOW_at_Preamble; - current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; - current_synchro_data.Flag_preamble = d_flag_preamble; - current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0; - current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms; + current_synchro_data.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol*1000.0)/1000.0; if(d_dump == true) { // MULTIPLEXED FILE RECORDING - Record results to file try { - double tmp_double; - tmp_double = d_TOW_at_current_symbol; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_synchro_data.Prn_timestamp_ms; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = d_TOW_at_Preamble; - d_dump_file.write((char*)&tmp_double, sizeof(double)); + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = d_TOW_at_current_symbol; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_ulong_int = current_synchro_data.Tracking_sample_counter; + d_dump_file.write((char*)&tmp_ulong_int, sizeof(unsigned long int)); + tmp_double = d_TOW_at_Preamble; + d_dump_file.write((char*)&tmp_double, sizeof(double)); } - catch (const std::ifstream::failure& e) + catch (const std::ifstream::failure & e) { LOG(WARNING) << "Exception writing observables dump file " << e.what(); } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h index 1168df64b..02e3c90dd 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h @@ -116,11 +116,9 @@ private: Gnss_Satellite d_satellite; int d_channel; - double d_preamble_time_seconds; - double d_TOW_at_Preamble; double d_TOW_at_current_symbol; - double Prn_timestamp_at_preamble_ms; + bool flag_TOW_set; std::string d_dump_filename; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 482e4b6af..ad7bcc655 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -95,7 +95,6 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_symbol_accumulator = 0; d_symbol_accumulator_counter = 0; d_frame_bit_index = 0; - d_preamble_time_seconds = 0; d_flag_frame_sync = false; d_GPS_frame_4bytes = 0; d_prev_GPS_frame_4bytes = 0; @@ -109,8 +108,9 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_word_number = 0; d_decimation_output_factor = 1; d_channel = 0; - Prn_timestamp_at_preamble_ms = 0.0; flag_PLL_180_deg_phase_locked = false; + //set minimum output buffer to avoid deadlock when combined with other GNSS systems or signals with slower symbol rates + this->set_min_output_buffer(3000); } @@ -181,8 +181,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ { d_GPS_FSM.Event_gps_word_preamble(); //record the preamble sample stamp - d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // record the preamble sample stamp - DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs=" << round(in[0][0].Tracking_timestamp_secs * 1000.0); + d_preamble_time_samples = in[0][0].Tracking_sample_counter; // record the preamble sample stamp + DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "in[0][0].Tracking_sample_counter=" << in[0][0].Tracking_sample_counter; //sync the symbol to bits integrator d_symbol_accumulator = 0; d_symbol_accumulator_counter = 0; @@ -191,17 +191,17 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ } else if (d_stat == 1) //check 6 seconds of preamble separation { - preamble_diff_ms = round((in[0][0].Tracking_timestamp_secs - d_preamble_time_seconds) * 1000.0); + preamble_diff_ms = round((((double)in[0][0].Tracking_sample_counter - d_preamble_time_samples)/(double)in[0][0].fs) * 1000.0); if (abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1) { - DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite << "in[0][0].Tracking_timestamp_secs=" << round(in[0][0].Tracking_timestamp_secs * 1000.0); + DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite; d_GPS_FSM.Event_gps_word_preamble(); d_flag_preamble = true; - d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // record the PRN start sample index associated to the preamble + d_preamble_time_samples = in[0][0].Tracking_sample_counter; // record the PRN start sample index associated to the preamble if (!d_flag_frame_sync) { // send asynchronous message to tracking to inform of frame sync and extend correlation time - pmt::pmt_t value = pmt::from_double(d_preamble_time_seconds - 0.001); + pmt::pmt_t value = pmt::from_double((double)d_preamble_time_samples/(double)in[0][0].fs - 0.001); this->message_port_pub(pmt::mp("preamble_timestamp_s"), value); d_flag_frame_sync = true; if (corr_value < 0) @@ -213,7 +213,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ { flag_PLL_180_deg_phase_locked = false; } - DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]"; + DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " << (double)d_preamble_time_samples/(double)in[0][0].fs << " [s]"; } } } @@ -222,7 +222,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ { if (d_stat == 1) { - preamble_diff_ms = round((in[0][0].Tracking_timestamp_secs - d_preamble_time_seconds) * 1000.0); + preamble_diff_ms = round((((double)in[0][0].Tracking_sample_counter - (double)d_preamble_time_samples)/(double)in[0][0].fs) * 1000.0); if (preamble_diff_ms > GPS_SUBFRAME_MS+1) { DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms; @@ -275,7 +275,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes)) { memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char)*4); - d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0; + //d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0; d_GPS_FSM.Event_gps_word_valid(); // send TLM data to PVT using asynchronous message queues if (d_GPS_FSM.d_flag_new_subframe == true) @@ -341,8 +341,8 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ { // update TOW at the preamble instant d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + GPS_L1_CA_CODE_PERIOD; - Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; - d_TOW_at_current_symbol = d_TOW_at_Preamble; + + d_TOW_at_current_symbol = floor(d_TOW_at_Preamble*1000.0)/1000.0; flag_TOW_set = true; d_flag_new_tow_available=false; } @@ -351,13 +351,9 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD; } - current_synchro_data.d_TOW = d_TOW_at_Preamble; - current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; - current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol; // to be used in the hybrid configuration - current_synchro_data.Flag_valid_word = flag_TOW_set;//(d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set == true); - current_synchro_data.Flag_preamble = d_flag_preamble; - current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0; - current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms; + current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; + current_synchro_data.Flag_valid_word = flag_TOW_set; + if (flag_PLL_180_deg_phase_locked == true) { @@ -371,10 +367,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ try { double tmp_double; + unsigned long int tmp_ulong_int; tmp_double = d_TOW_at_current_symbol; d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_synchro_data.Prn_timestamp_ms; - d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_ulong_int = current_synchro_data.Tracking_sample_counter; + d_dump_file.write((char*)&tmp_ulong_int, sizeof(unsigned long int)); tmp_double = d_TOW_at_Preamble; d_dump_file.write((char*)&tmp_double, sizeof(double)); } @@ -383,31 +380,11 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ LOG(WARNING) << "Exception writing observables dump file " << e.what(); } } - - //todo: implement averaging - - d_average_count++; - if (d_average_count == d_decimation_output_factor) - { - d_average_count = 0; - //3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_synchro_data; - //std::cout<<"GPS L1 TLM output on CH="<d_channel << " SAMPLE STAMP="<tow * 6000 + *pdelay * 20 + d_TOW_at_current_symbol=((double)msg.tow) * 6.0 + ((double)delay) * GPS_L2_M_PERIOD +12*GPS_L2_M_PERIOD; + d_TOW_at_current_symbol=floor(d_TOW_at_current_symbol*1000.0)/1000.0; d_flag_valid_word=true; } else { d_TOW_at_current_symbol +=GPS_L2_M_PERIOD; - current_synchro_data.d_TOW = d_TOW_at_Preamble; - current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; - current_synchro_data.d_TOW_hybrid_at_current_symbol = d_TOW_at_current_symbol; - current_synchro_data.Flag_preamble = false; - current_synchro_data.Prn_timestamp_ms = in[0].Tracking_timestamp_secs * 1000.0; if (current_synchro_data.Flag_valid_symbol_output==false) { d_flag_valid_word=false; } } - + current_synchro_data.TOW_at_current_symbol_s = d_TOW_at_current_symbol; current_synchro_data.Flag_valid_word=d_flag_valid_word; // if (flag_PLL_180_deg_phase_locked == true) @@ -187,13 +170,14 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( // MULTIPLEXED FILE RECORDING - Record results to file try { - double tmp_double; - tmp_double = d_TOW_at_current_symbol; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = current_synchro_data.Prn_timestamp_ms; - d_dump_file.write((char*)&tmp_double, sizeof(double)); - tmp_double = d_TOW_at_Preamble; - d_dump_file.write((char*)&tmp_double, sizeof(double)); + double tmp_double; + unsigned long int tmp_ulong_int; + tmp_double = d_TOW_at_current_symbol; + d_dump_file.write((char*)&tmp_double, sizeof(double)); + tmp_ulong_int = current_synchro_data.Tracking_sample_counter; + d_dump_file.write((char*)&tmp_ulong_int, sizeof(unsigned long int)); + tmp_double = d_TOW_at_Preamble; + d_dump_file.write((char*)&tmp_double, sizeof(double)); } catch (const std::ifstream::failure & e) { @@ -202,18 +186,9 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( } - d_average_count++; - if (d_average_count == d_decimation_output_factor) - { - d_average_count = 0; - //3. Make the output (copy the object contents to the GNURadio reserved memory) - out[0] = current_synchro_data; - return 1; - } - else - { - return 0; - } + //3. Make the output (copy the object contents to the GNURadio reserved memory) + out[0] = current_synchro_data; + return 1; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h index 3e93b0b3f..5d99e19c1 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.h @@ -69,7 +69,6 @@ public: ~gps_l2c_telemetry_decoder_cc(); void set_satellite(Gnss_Satellite satellite); //!< Set satellite PRN void set_channel(int channel); //!< Set receiver's channel - void set_decimation(int decimation); /*! * \brief This is where all signal processing takes place @@ -83,8 +82,6 @@ private: gps_l2c_make_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); gps_l2c_telemetry_decoder_cc(Gnss_Satellite satellite, bool dump); - - bool d_dump; Gnss_Satellite d_satellite; int d_channel; @@ -101,9 +98,6 @@ private: double d_TOW_at_Preamble; bool d_flag_valid_word; - int d_decimation_output_factor; - int d_average_count; - Gps_CNAV_Navigation_Message d_CNAV_Message; }; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc index c634a994e..591ccbe69 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc @@ -103,7 +103,7 @@ int sbas_l1_telemetry_decoder_cc::general_work (int noutput_items __attribute__( Gnss_Synchro *out = (Gnss_Synchro *) output_items[0]; // output // store the time stamp of the first sample in the processed sample block - double sample_stamp = in[0].Tracking_timestamp_secs; + double sample_stamp = in[0].Tracking_sample_counter/in[0].fs; // copy correlation samples into samples vector for (int i = 0; i < noutput_items; i++) @@ -140,7 +140,7 @@ int sbas_l1_telemetry_decoder_cc::general_work (int noutput_items __attribute__( // compute message sample stamp // and fill messages in SBAS raw message objects - std::vector sbas_raw_msgs; + //std::vector sbas_raw_msgs; for(std::vector::const_iterator it = valid_msgs.begin(); it != valid_msgs.end(); ++it) { @@ -156,17 +156,17 @@ int sbas_l1_telemetry_decoder_cc::general_work (int noutput_items __attribute__( << " relative_preamble_start=" << it->first << " message_sample_offset=" << message_sample_offset << ")"; - Sbas_Raw_Msg sbas_raw_msg(message_sample_stamp, this->d_satellite.get_PRN(), it->second); - sbas_raw_msgs.push_back(sbas_raw_msg); + //Sbas_Raw_Msg sbas_raw_msg(message_sample_stamp, this->d_satellite.get_PRN(), it->second); + //sbas_raw_msgs.push_back(sbas_raw_msg); } // parse messages // and send them to the SBAS raw message queue - for(std::vector::iterator it = sbas_raw_msgs.begin(); it != sbas_raw_msgs.end(); it++) - { - std::cout << "SBAS message type " << it->get_msg_type() << " from PRN" << it->get_prn() << " received" << std::endl; - sbas_telemetry_data.update(*it); - } + //for(std::vector::iterator it = sbas_raw_msgs.begin(); it != sbas_raw_msgs.end(); it++) + // { + //std::cout << "SBAS message type " << it->get_msg_type() << " from PRN" << it->get_prn() << " received" << std::endl; + //sbas_telemetry_data.update(*it); + // } // clear all processed samples in the input buffer d_sample_buf.clear(); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h index 6fc2d5699..84138491a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h @@ -41,7 +41,6 @@ #include #include "gnss_satellite.h" #include "viterbi_decoder.h" -#include "sbas_telemetry_data.h" class sbas_l1_telemetry_decoder_cc; @@ -159,8 +158,6 @@ private: void zerropad_back_and_convert_to_bytes(const std::vector msg_candidate, std::vector &bytes); } d_crc_verifier; - - Sbas_Telemetry_Data sbas_telemetry_data; }; #endif diff --git a/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc b/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc index f1db84183..45eefc1e2 100644 --- a/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc +++ b/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc @@ -238,7 +238,6 @@ GpsL1CaSubframeFsm::GpsL1CaSubframeFsm() d_nav.reset(); i_channel_ID = 0; i_satellite_PRN = 0; - d_preamble_time_ms = 0; d_subframe_ID=0; d_flag_new_subframe=false; initiate(); //start the FSM @@ -266,7 +265,6 @@ void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg() << Gnss_Satellite(std::string("GPS"), i_satellite_PRN) << std::endl; d_nav.i_satellite_PRN = i_satellite_PRN; d_nav.i_channel_ID = i_channel_ID; - d_nav.d_subframe_timestamp_ms = this->d_preamble_time_ms; d_flag_new_subframe=true; } diff --git a/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h b/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h index 0a577502b..34e40043f 100644 --- a/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h +++ b/src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h @@ -83,7 +83,7 @@ public: int d_subframe_ID; bool d_flag_new_subframe; char d_GPS_frame_4bytes[GPS_WORD_LENGTH]; - double d_preamble_time_ms; + //double d_preamble_time_ms; void gps_word_to_subframe(int position); //!< inserts the word in the correct position of the subframe diff --git a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc index 01afa145d..88b537478 100644 --- a/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l2_m_dll_pll_tracking.cc @@ -70,7 +70,7 @@ GpsL2MDllPllTracking::GpsL2MDllPllTracking( std::string default_dump_filename = "./track_ch"; dump_filename = configuration->property(role + ".dump_filename", default_dump_filename); //unused! - vector_length = std::round(fs_in / (GPS_L2_M_CODE_RATE_HZ / GPS_L2_M_CODE_LENGTH_CHIPS)); + vector_length = std::round((double)fs_in / ((double)GPS_L2_M_CODE_RATE_HZ / (double)GPS_L2_M_CODE_LENGTH_CHIPS)); //################# MAKE TRACKING GNURadio object ################### if (item_type.compare("gr_complex") == 0) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index 6f8db9cc7..78bba87c3 100755 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -289,8 +289,9 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri int acq_to_trk_delay_samples; acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_current_prn_length_samples - std::fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = std::round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_pull_in = false; @@ -335,7 +336,6 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri //Code phase accumulator double code_error_filt_secs; code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] - //code_error_filt_secs=T_prn_seconds*code_error_filt_chips*T_chip_seconds*static_cast(d_fs_in); //[seconds] d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### @@ -349,8 +349,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; T_prn_samples = T_prn_seconds * static_cast(d_fs_in); K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = std::round(K_blk_samples); //round to a discrete samples - //d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample + d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) @@ -393,7 +392,8 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri current_synchro_data.Prompt_I = static_cast((*d_Prompt).real()); current_synchro_data.Prompt_Q = static_cast((*d_Prompt).imag()); // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; //compute remnant code phase samples AFTER the Tracking timestamp d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; @@ -409,13 +409,15 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri *d_Prompt = gr_complex(0,0); *d_Late = gr_complex(0,0); // GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter; } //assign the GNURadio block output data current_synchro_data.System = {'E'}; std::string str_aux = "1B"; const char * str = str_aux.c_str(); // get a C style null terminated string std::memcpy((void*)current_synchro_data.Signal, str, 3); + + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; if(d_dump) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index 6b85f664f..7df873fb1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -284,7 +284,8 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items __attr acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((float)acq_to_trk_delay_samples, (float)d_current_prn_length_samples); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples d_pull_in = false; @@ -402,10 +403,9 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items __attr current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); // Tracking_timestamp_secs is aligned with the PRN start sample - //current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_next_prn_length_samples + (double)d_next_rem_code_phase_samples)/(double)d_fs_in; - current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_rem_code_phase_samples)/(double)d_fs_in; + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample - // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 current_synchro_data.Carrier_phase_rads = (double)d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = (double)d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = (double)d_CN0_SNV_dB_Hz; @@ -417,8 +417,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items __attr *d_Early = gr_complex(0,0); *d_Prompt = gr_complex(0,0); *d_Late = gr_complex(0,0); - - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; //! When tracking is disabled an array of 1's is sent to maintain the TCP connection boost::array tx_variables_array = {{1,1,1,1,1,1,1,1,1,1,1,1,0}}; d_tcp_com.send_receive_tcp_packet_galileo_e1(tx_variables_array, &tcp_data); @@ -429,6 +428,7 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items __attr const char * str = str_aux.c_str(); // get a C style null terminated string std::memcpy((void*)current_synchro_data.Signal, str, 3); + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; if(d_dump) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc index acf1e9bc6..d3b590de8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e5a_dll_pll_tracking_cc.cc @@ -399,9 +399,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute d_Prompt = gr_complex(0,0); d_Late = gr_complex(0,0); d_Prompt_data = gr_complex(0,0); - current_synchro_data.Tracking_timestamp_secs = static_cast(d_sample_counter) / static_cast(d_fs_in); - *out[0] = current_synchro_data; - + current_synchro_data.Tracking_sample_counter = d_sample_counter; break; } case 1: @@ -419,10 +417,10 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute // make an output to not stop the rest of the processing blocks current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_Q = 0.0; - current_synchro_data.Tracking_timestamp_secs = static_cast(d_sample_counter) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter; current_synchro_data.Carrier_phase_rads = 0.0; current_synchro_data.CN0_dB_hz = 0.0; - *out[0] = current_synchro_data; + current_synchro_data.fs=d_fs_in; consume_each(samples_offset); //shift input to perform alignment with local replica return 1; break; @@ -625,9 +623,8 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute { current_synchro_data.Prompt_I = static_cast((d_Prompt_data).real()); current_synchro_data.Prompt_Q = static_cast((d_Prompt_data).imag()); - // Tracking_timestamp_secs is aligned with the PRN start sample - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, thus, Code_phase_secs=0 + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -638,16 +635,19 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute // make an output to not stop the rest of the processing blocks current_synchro_data.Prompt_I = 0.0; current_synchro_data.Prompt_Q = 0.0; - current_synchro_data.Tracking_timestamp_secs = static_cast(d_sample_counter) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter; current_synchro_data.Carrier_phase_rads = 0.0; current_synchro_data.CN0_dB_hz = 0.0; } - *out[0] = current_synchro_data; + break; } } + current_synchro_data.fs=d_fs_in; + *out[0] = current_synchro_data; + if(d_dump) { // MULTIPLEXED FILE RECORDING - Record results to file diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc index 37f45fe3c..400749eda 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_cc.cc @@ -339,12 +339,13 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_length_samples)); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; d_sample_counter += samples_offset; // count for the processed samples d_pull_in = false; d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GPS_TWO_PI; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GPS_TWO_PI; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; consume_each(samples_offset); // shift input to perform alignment with local replica return 1; @@ -537,9 +538,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri // ########### Output the tracking data to navigation and PVT ########## current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -557,9 +557,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri { current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -573,10 +572,10 @@ int gps_l1_ca_dll_pll_c_aid_tracking_cc::general_work (int noutput_items __attri } current_synchro_data.System = {'G'}; - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; } //assign the GNURadio block output data + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; if(d_dump) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc index eaa4237c7..19cce7732 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc.cc @@ -332,12 +332,13 @@ int gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::general_work (int noutput_items __ acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_length_samples)); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; d_sample_counter += samples_offset; // count for the processed samples d_pull_in = false; d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GPS_TWO_PI; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GPS_TWO_PI; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; consume_each(samples_offset); // shift input to perform alignment with local replica multicorrelator_fpga_8sc.set_initial_sample(samples_offset); @@ -532,9 +533,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::general_work (int noutput_items __ // ########### Output the tracking data to navigation and PVT ########## current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -552,9 +552,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::general_work (int noutput_items __ { current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -568,9 +567,9 @@ int gps_l1_ca_dll_pll_c_aid_tracking_fpga_sc::general_work (int noutput_items __ } current_synchro_data.System = {'G'}; - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; } + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; if(d_dump) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc index 309fa2cbf..222986554 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_c_aid_tracking_sc.cc @@ -343,12 +343,13 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_length_samples)); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; d_sample_counter += samples_offset; // count for the processed samples d_pull_in = false; d_acc_carrier_phase_cycles -= d_carrier_phase_step_rad * samples_offset / GPS_TWO_PI; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_cycles * GPS_TWO_PI; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; consume_each(samples_offset); // shift input to perform alignment with local replica return 1; @@ -541,8 +542,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -560,9 +561,8 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri { current_synchro_data.Prompt_I = static_cast((d_correlator_outs_16sc[1]).real()); current_synchro_data.Prompt_Q = static_cast((d_correlator_outs_16sc[1]).imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + d_rem_code_phase_samples) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;// todo: project the carrier doppler current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -576,9 +576,9 @@ int gps_l1_ca_dll_pll_c_aid_tracking_sc::general_work (int noutput_items __attri } current_synchro_data.System = {'G'}; - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + d_correlation_length_samples + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; } + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; if(d_dump) { diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index deda7039e..a0a7b6743 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -82,9 +82,9 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required) { if (noutput_items != 0) - { - ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call - } + { + ninput_items_required[0] = static_cast(d_vector_length) * 2; //set the required available samples in each call + } } @@ -98,8 +98,8 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) : - gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + gr::block("Gps_L1_Ca_Dll_Pll_Tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry bit synchronization message port input this->message_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -129,9 +129,9 @@ Gps_L1_Ca_Dll_Pll_Tracking_cc::Gps_L1_Ca_Dll_Pll_Tracking_cc( d_n_correlator_taps = 3; // Early, Prompt, and Late d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_gnsssdr_get_alignment())); for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + { + d_correlator_outs[n] = gr_complex(0,0); + } d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(float), volk_gnsssdr_get_alignment())); // Set TAPs delay values [chips] d_local_code_shift_chips[0] = - d_early_late_spc_chips; @@ -218,9 +218,9 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() double corrected_acq_phase_samples, delay_correction_samples; corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); if (corrected_acq_phase_samples < 0) - { - corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; - } + { + corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; + } delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; d_acq_code_phase_samples = corrected_acq_phase_samples; @@ -237,9 +237,9 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::start_tracking() multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L1_CA_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + { + d_correlator_outs[n] = gr_complex(0,0); + } d_carrier_lock_fail_counter = 0; d_rem_code_phase_samples = 0; @@ -296,200 +296,203 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute__ Gnss_Synchro current_synchro_data = Gnss_Synchro(); if (d_enable_tracking == true) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Receiver signal alignment + if (d_pull_in == true) { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Receiver signal alignment - if (d_pull_in == true) - { - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; - acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples - d_pull_in = false; - // take into account the carrier cycles accumulated in the pull in signal alignment - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - *out[0] = current_synchro_data; - consume_each(samples_offset); // shift input to perform alignment with local replica - return 1; - } - - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_current_prn_length_samples); - - // ################## PLL ########################################################## - // PLL discriminator - // Update PLL discriminator [rads/Ti -> Secs/Ti] - carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; // prompt output - // Carrier discriminator filter - carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); - // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; - // New code Doppler frequency estimation - d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); - - // ################## DLL ########################################################## - // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late - // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] - double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; // [seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); - double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(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 = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; - d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); - // carrier phase accumulator - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; - - //################### DLL COMMANDS ################################################# - // code phase step (Code resampler phase increment per sample) [chips/sample] - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - // remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample - d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); - - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) - { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt - d_cn0_estimation_counter++; - } - else - { - d_cn0_estimation_counter = 0; - // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock - d_carrier_lock_fail_counter = 0; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - } - // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); - current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); - - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter + d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); + int samples_offset; + double acq_trk_shif_correction_samples; + int acq_to_trk_delay_samples; + acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples + d_pull_in = false; + // take into account the carrier cycles accumulated in the pull in signal alignment + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.fs=d_fs_in; current_synchro_data.correlation_length_ms = 1; + *out[0] = current_synchro_data; + consume_each(samples_offset); // shift input to perform alignment with local replica + return 1; } - else - { - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter + d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - current_synchro_data.Rem_code_phase_secs = d_rem_code_phase_samples / static_cast(d_fs_in); - current_synchro_data.System = {'G'}; + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); + + // ################## PLL ########################################################## + // PLL discriminator + // Update PLL discriminator [rads/Ti -> Secs/Ti] + carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_TWO_PI; // prompt output + // Carrier discriminator filter + carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); + // New carrier Doppler frequency estimation + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; + // New code Doppler frequency estimation + d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); + + // ################## DLL ########################################################## + // DLL discriminator + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] //early and late + // Code discriminator filter + code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); // [chips/second] + double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips*T_chip_seconds); //[seconds] + //double code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; // [seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + //double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + //double T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(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 = GPS_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; + + //################### DLL COMMANDS ################################################# + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); + + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES) + { + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; //prompt + d_cn0_estimation_counter++; } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L1_CA_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3)); // 3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine + } + } + // ########### Output the tracking data to navigation and PVT ########## + current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); + current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = 1; + } + else + { + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + + current_synchro_data.Tracking_sample_counter =d_sample_counter + d_current_prn_length_samples; + current_synchro_data.System = {'G'}; + current_synchro_data.correlation_length_ms = 1; + } //assign the GNURadio block output data + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; if(d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + unsigned long int tmp_long; + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); + try { - // MULTIPLEXED FILE RECORDING - Record results to file - float prompt_I; - float prompt_Q; - float tmp_E, tmp_P, tmp_L; - double tmp_double; - unsigned long int tmp_long; - prompt_I = d_correlator_outs[1].real(); - prompt_Q = d_correlator_outs[1].imag(); - tmp_E = std::abs(d_correlator_outs[0]); - tmp_P = std::abs(d_correlator_outs[1]); - tmp_L = std::abs(d_correlator_outs[2]); - try - { - // EPR - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), 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 = d_sample_counter + d_current_prn_length_samples; - d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); + // EPR + 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)); + // 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 = d_sample_counter + d_current_prn_length_samples; + d_dump_file.write(reinterpret_cast(&tmp_long), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - // PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); + // PLL commands + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&carr_error_filt_hz), sizeof(double)); - // DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + // DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - // AUX vars (for debug purposes) - tmp_double = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates d_sample_counter += d_current_prn_length_samples; // count for the processed samples - return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false } @@ -501,23 +504,23 @@ void Gps_L1_Ca_Dll_Pll_Tracking_cc::set_channel(unsigned int channel) LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) + { + if (d_dump_file.is_open() == false) { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_filename.append(boost::lexical_cast(d_channel)); - d_dump_filename.append(".dat"); - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); - } - catch (const std::ifstream::failure &e) - { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } - } + try + { + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (const std::ifstream::failure &e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } } + } } diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc index 7acfd7c21..014b08678 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_gpu_cc.cc @@ -317,10 +317,10 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items __attribu acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; acq_trk_shif_correction_samples = d_correlation_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_correlation_length_samples)); samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); - - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + current_synchro_data.fs=d_fs_in; + current_synchro_data.correlation_length_ms = 1; *out[0] = current_synchro_data; - d_sample_counter += samples_offset; //count for the processed samples d_pull_in = false; consume_each(samples_offset); //shift input to perform alignment with local replica @@ -436,8 +436,8 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items __attribu // ########### Output the tracking data to navigation and PVT ########## current_synchro_data.Prompt_I = static_cast((d_correlator_outs[1]).real()); current_synchro_data.Prompt_Q = static_cast((d_correlator_outs[1]).imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample (Hybridization OK!) - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + old_d_rem_code_phase_samples) / static_cast(d_fs_in); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; current_synchro_data.Carrier_phase_rads = GPS_TWO_PI * d_acc_carrier_phase_cycles; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; @@ -454,10 +454,12 @@ int Gps_L1_Ca_Dll_Pll_Tracking_GPU_cc::general_work (int noutput_items __attribu } current_synchro_data.System = {'G'}; - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + current_synchro_data.correlation_length_ms = 1; + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_correlation_length_samples; } //assign the GNURadio block output data + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; if(d_dump) diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index ce01dffc4..761d3c39a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -201,15 +201,6 @@ void Gps_L1_Ca_Tcp_Connector_Tracking_cc::start_tracking() long int acq_trk_diff_samples; float acq_trk_diff_seconds; - // jarribas: this patch correct a situation where the tracking sample counter - // is equal to 0 (remains in the initial state) at the first acquisition to tracking transition - // of the receiver operation when is connecting to simulink server. - // if (d_sample_countermessage_port_register_in(pmt::mp("preamble_timestamp_s")); @@ -110,6 +110,8 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc( d_vector_length = vector_length; d_dump_filename = dump_filename; + d_current_prn_length_samples = static_cast(d_vector_length); + // DLL/PLL filter initialization d_carrier_loop_filter=Tracking_2nd_PLL_filter(GPS_L2_M_PERIOD); d_code_loop_filter=Tracking_2nd_DLL_filter(GPS_L2_M_PERIOD); @@ -129,17 +131,16 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc( d_n_correlator_taps = 3; // Early, Prompt, and Late d_correlator_outs = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_gnsssdr_get_alignment())); for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + { + d_correlator_outs[n] = gr_complex(0,0); + } d_local_code_shift_chips = static_cast(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(float), volk_gnsssdr_get_alignment())); // Set TAPs delay values [chips] d_local_code_shift_chips[0] = - d_early_late_spc_chips; d_local_code_shift_chips[1] = 0.0; d_local_code_shift_chips[2] = d_early_late_spc_chips; - multicorrelator_cpu.init(2 * d_vector_length, d_n_correlator_taps); - + multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps); //--- Perform initializations ------------------------------ // define initial code frequency basis of NCO @@ -157,8 +158,6 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc( d_enable_tracking = false; d_pull_in = false; - d_current_prn_length_samples = static_cast(d_vector_length); - // CN0 estimation and lock detector buffers d_cn0_estimation_counter = 0; d_Prompt_buffer = new gr_complex[GPS_L2M_CN0_ESTIMATION_SAMPLES]; @@ -169,7 +168,6 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc( systemName["G"] = std::string("GPS"); - set_relative_rate(1.0/((double)d_vector_length*2)); //set_min_output_buffer((long int)300); d_acquisition_gnss_synchro = 0; @@ -184,7 +182,7 @@ gps_l2_m_dll_pll_tracking_cc::gps_l2_m_dll_pll_tracking_cc( d_code_phase_step_chips = 0.0; d_carrier_phase_step_rad = 0.0; - LOG(INFO) << "d_vector_length" << d_vector_length; + set_relative_rate(1.0 / static_cast(d_vector_length)); } @@ -195,12 +193,12 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() */ d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples; d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz; - d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; + d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples; long int acq_trk_diff_samples; - float acq_trk_diff_seconds; - acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp);//-d_vector_length; - LOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; + double acq_trk_diff_seconds; + acq_trk_diff_samples = static_cast(d_sample_counter) - static_cast(d_acq_sample_stamp); //-d_vector_length; + DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples; acq_trk_diff_seconds = static_cast(acq_trk_diff_samples) / static_cast(d_fs_in); // Doppler effect // Fd=(C/(C+Vr))*F @@ -213,23 +211,23 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() d_code_phase_step_chips = static_cast(d_code_freq_chips) / static_cast(d_fs_in); T_chip_mod_seconds = 1/d_code_freq_chips; T_prn_mod_seconds = T_chip_mod_seconds * GPS_L2_M_CODE_LENGTH_CHIPS; - T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); + T_prn_mod_samples = T_prn_mod_seconds * static_cast(d_fs_in); d_current_prn_length_samples = round(T_prn_mod_samples); double T_prn_true_seconds = GPS_L2_M_CODE_LENGTH_CHIPS / GPS_L2_M_CODE_RATE_HZ; - double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); + double T_prn_true_samples = T_prn_true_seconds * static_cast(d_fs_in); double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds; double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds; double corrected_acq_phase_samples, delay_correction_samples; - corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); + corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast(d_fs_in)), T_prn_true_samples); if (corrected_acq_phase_samples < 0) - { - corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; - } + { + corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples; + } delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples; - //TODO: debug the algorithm implementation and enable correction - //d_acq_code_phase_samples = corrected_acq_phase_samples; + + d_acq_code_phase_samples = corrected_acq_phase_samples; d_carrier_doppler_hz = d_acq_carrier_doppler_hz; d_carrier_phase_step_rad = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); @@ -243,15 +241,15 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() multicorrelator_cpu.set_local_code_and_taps(static_cast(GPS_L2_M_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips); for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } + { + d_correlator_outs[n] = gr_complex(0,0); + } d_carrier_lock_fail_counter = 0; d_rem_code_phase_samples = 0; - d_rem_carr_phase_rad = 0; + d_rem_carr_phase_rad = 0.0; d_rem_code_phase_chips = 0.0; - d_acc_carrier_phase_rad = 0; + d_acc_carrier_phase_rad = 0.0; d_code_phase_samples = d_acq_code_phase_samples; @@ -259,15 +257,14 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking() sys = sys_.substr(0,1); // DEBUG OUTPUT - std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " with Doppler=" << d_acq_carrier_doppler_hz << " [Hz]" << std::endl; - LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; - + std::cout << "Tracking GPS L2CM start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; + LOG(INFO) << "Starting GPS L2CM tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; // enable tracking d_pull_in = true; d_enable_tracking = true; - LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz + LOG(INFO) << "GPS L2CM PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz << " Code Phase correction [samples]=" << delay_correction_samples << " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples; } @@ -303,190 +300,196 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items __attribute__( Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; if (d_enable_tracking == true) + { + // Fill the acquisition data + current_synchro_data = *d_acquisition_gnss_synchro; + // Receiver signal alignment + if (d_pull_in == true) { - // Fill the acquisition data - current_synchro_data = *d_acquisition_gnss_synchro; - // Receiver signal alignment - if (d_pull_in == true) - { - int samples_offset; - double acq_trk_shif_correction_samples; - int acq_to_trk_delay_samples; - acq_to_trk_delay_samples = (d_sample_counter - (d_acq_sample_stamp-d_current_prn_length_samples)); - acq_trk_shif_correction_samples = -fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); - samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);//+(1.5*(d_fs_in/GPS_L2_M_CODE_RATE_HZ))); - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); - d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; - current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; - current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - *out[0] = current_synchro_data; - d_pull_in = false; - consume_each(samples_offset); //shift input to perform alignment with local replica - return 1; - } - - // ################# CARRIER WIPEOFF AND CORRELATORS ############################## - // perform carrier wipe-off and compute Early, Prompt and Late correlation - multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); - multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, - d_carrier_phase_step_rad, - d_rem_code_phase_chips, - d_code_phase_step_chips, - d_current_prn_length_samples); - - // ################## PLL ########################################################## - // PLL discriminator - carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_L2_TWO_PI; - // Carrier discriminator filter - carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); - // New carrier Doppler frequency estimation - d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; - // New code Doppler frequency estimation - d_code_freq_chips = GPS_L2_M_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L2_M_CODE_RATE_HZ) / GPS_L2_FREQ_HZ); - - // ################## DLL ########################################################## - // DLL discriminator - code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] - // Code discriminator filter - code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] - //Code phase accumulator - double code_error_filt_secs = (GPS_L2_M_PERIOD * code_error_filt_chips) / GPS_L2_M_CODE_RATE_HZ; //[seconds] - - // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### - // keep alignment parameters for the next input buffer - // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - double T_chip_seconds = 1.0 / d_code_freq_chips; - double T_prn_seconds = T_chip_seconds * GPS_L2_M_CODE_LENGTH_CHIPS; - double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); - double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); - d_current_prn_length_samples = round(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 = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); - // remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; - d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L2_TWO_PI); - // carrier phase accumulator - d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; - - //################### DLL COMMANDS ################################################# - // code phase step (Code resampler phase increment per sample) [chips/sample] - d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); - // remnant code phase [chips] - d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample - d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); - - // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### - if (d_cn0_estimation_counter < GPS_L2M_CN0_ESTIMATION_SAMPLES) - { - // fill buffer with prompt correlator output values - d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; - d_cn0_estimation_counter++; - } - else - { - d_cn0_estimation_counter = 0; - // Code lock indicator - d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, GPS_L2M_CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L2_M_CODE_LENGTH_CHIPS); - // Carrier lock indicator - d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, GPS_L2M_CN0_ESTIMATION_SAMPLES); - // Loss of lock detection - if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < GPS_L2M_MINIMUM_VALID_CN0) - { - d_carrier_lock_fail_counter++; - } - else - { - if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; - } - if (d_carrier_lock_fail_counter > GPS_L2M_MAXIMUM_LOCK_FAIL_COUNTER) - { - std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; - LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock - d_carrier_lock_fail_counter = 0; - d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine - } - } - // ########### Output the tracking data to navigation and PVT ########## - current_synchro_data.Prompt_I = static_cast(d_correlator_outs[1].real()); - current_synchro_data.Prompt_Q = static_cast(d_correlator_outs[1].imag()); - // Tracking_timestamp_secs is aligned with the CURRENT PRN start sample - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter + d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + int samples_offset; + double acq_trk_shif_correction_samples; + int acq_to_trk_delay_samples; + acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp; + acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast(acq_to_trk_delay_samples), static_cast(d_current_prn_length_samples)); + samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples); + current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset; + d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples + d_pull_in = false; + // take into account the carrier cycles accumulated in the pull in signal alignment + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; - current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; - current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.fs=d_fs_in; current_synchro_data.correlation_length_ms = 20; + *out[0] = current_synchro_data; + consume_each(samples_offset); // shift input to perform alignment with local replica + return 1; + } - } - else + // ################# CARRIER WIPEOFF AND CORRELATORS ############################## + // perform carrier wipe-off and compute Early, Prompt and Late correlation + multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in); + multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad, + d_carrier_phase_step_rad, + d_rem_code_phase_chips, + d_code_phase_step_chips, + d_current_prn_length_samples); + + // ################## PLL ########################################################## + // PLL discriminator + // Update PLL discriminator [rads/Ti -> Secs/Ti] + carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_L2_TWO_PI; + // Carrier discriminator filter + carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz); + // New carrier Doppler frequency estimation + d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz; + // New code Doppler frequency estimation + d_code_freq_chips = GPS_L2_M_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L2_M_CODE_RATE_HZ) / GPS_L2_FREQ_HZ); + + // ################## DLL ########################################################## + // DLL discriminator + code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti] + // Code discriminator filter + code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] + double T_chip_seconds = 1.0 / static_cast(d_code_freq_chips); + double T_prn_seconds = T_chip_seconds * GPS_L2_M_CODE_LENGTH_CHIPS; + double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips*T_chip_seconds); //[seconds] + //double code_error_filt_secs = (GPS_L2_M_PERIOD * code_error_filt_chips) / GPS_L2_M_CODE_RATE_HZ; //[seconds] + + // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### + // keep alignment parameters for the next input buffer + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + double T_prn_samples = T_prn_seconds * static_cast(d_fs_in); + double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast(d_fs_in); + d_current_prn_length_samples = round(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 = GPS_L2_TWO_PI * d_carrier_doppler_hz / static_cast(d_fs_in); + // remnant carrier phase to prevent overflow in the code NCO + d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples; + d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L2_TWO_PI); + // carrier phase accumulator + d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples; + + //################### DLL COMMANDS ################################################# + // code phase step (Code resampler phase increment per sample) [chips/sample] + d_code_phase_step_chips = d_code_freq_chips / static_cast(d_fs_in); + // remnant code phase [chips] + d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample + d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast(d_fs_in)); + + // ####### CN0 ESTIMATION AND LOCK DETECTORS ###### + if (d_cn0_estimation_counter < GPS_L2M_CN0_ESTIMATION_SAMPLES) { - for (int n = 0; n < d_n_correlator_taps; n++) - { - d_correlator_outs[n] = gr_complex(0,0); - } - current_synchro_data.Tracking_timestamp_secs = (static_cast(d_sample_counter + d_current_prn_length_samples) + static_cast(d_rem_code_phase_samples)) / static_cast(d_fs_in); + // fill buffer with prompt correlator output values + d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1]; + d_cn0_estimation_counter++; } + else + { + d_cn0_estimation_counter = 0; + // Code lock indicator + d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, GPS_L2M_CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L2_M_CODE_LENGTH_CHIPS); + // Carrier lock indicator + d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, GPS_L2M_CN0_ESTIMATION_SAMPLES); + // Loss of lock detection + if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < GPS_L2M_MINIMUM_VALID_CN0) + { + d_carrier_lock_fail_counter++; + } + else + { + if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--; + } + if (d_carrier_lock_fail_counter > GPS_L2M_MAXIMUM_LOCK_FAIL_COUNTER) + { + std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; + LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; + this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock + d_carrier_lock_fail_counter = 0; + d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine + } + } + // ########### Output the tracking data to navigation and PVT ########## + current_synchro_data.Prompt_I = static_cast(d_correlator_outs[1].real()); + current_synchro_data.Prompt_Q = static_cast(d_correlator_outs[1].imag()); + current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples; + current_synchro_data.Code_phase_samples = d_rem_code_phase_samples; + current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; + current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz; + current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz; + current_synchro_data.Flag_valid_symbol_output = true; + current_synchro_data.correlation_length_ms = 20; + + } + else + { + for (int n = 0; n < d_n_correlator_taps; n++) + { + d_correlator_outs[n] = gr_complex(0,0); + } + current_synchro_data.Tracking_sample_counter =d_sample_counter + d_current_prn_length_samples; + current_synchro_data.correlation_length_ms = 20; + } //assign the GNURadio block output data + current_synchro_data.fs=d_fs_in; *out[0] = current_synchro_data; if(d_dump) + { + // MULTIPLEXED FILE RECORDING - Record results to file + float prompt_I; + float prompt_Q; + float tmp_E, tmp_P, tmp_L; + double tmp_double; + prompt_I = d_correlator_outs[1].real(); + prompt_Q = d_correlator_outs[1].imag(); + tmp_E = std::abs(d_correlator_outs[0]); + tmp_P = std::abs(d_correlator_outs[1]); + tmp_L = std::abs(d_correlator_outs[2]); + try { - // MULTIPLEXED FILE RECORDING - Record results to file - float prompt_I; - float prompt_Q; - float tmp_E, tmp_P, tmp_L; - double tmp_double; - prompt_I = d_correlator_outs[1].real(); - prompt_Q = d_correlator_outs[1].imag(); - tmp_E = std::abs(d_correlator_outs[0]); - tmp_P = std::abs(d_correlator_outs[1]); - tmp_L = std::abs(d_correlator_outs[2]); - try - { - // EPR - d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_P), sizeof(float)); - d_dump_file.write(reinterpret_cast(&tmp_L), 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_float=(float)d_sample_counter; - d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); - // accumulated carrier phase - d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); + // EPR + 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)); + // 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_float=(float)d_sample_counter; + d_dump_file.write(reinterpret_cast(&d_sample_counter), sizeof(unsigned long int)); + // accumulated carrier phase + d_dump_file.write(reinterpret_cast(&d_acc_carrier_phase_rad), sizeof(double)); - // carrier and code frequency - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); + // carrier and code frequency + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_code_freq_chips), sizeof(double)); - //PLL commands - d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); + //PLL commands + d_dump_file.write(reinterpret_cast(&carr_error_hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_doppler_hz), sizeof(double)); - //DLL commands - d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); - d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); + //DLL commands + d_dump_file.write(reinterpret_cast(&code_error_chips), sizeof(double)); + d_dump_file.write(reinterpret_cast(&code_error_filt_chips), sizeof(double)); - // CN0 and carrier lock test - d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); - d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); + // CN0 and carrier lock test + d_dump_file.write(reinterpret_cast(&d_CN0_SNV_dB_Hz), sizeof(double)); + d_dump_file.write(reinterpret_cast(&d_carrier_lock_test), sizeof(double)); - // AUX vars (for debug purposes) - tmp_double = d_rem_code_phase_samples; - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - catch (std::ifstream::failure& e) - { - LOG(WARNING) << "Exception writing trk dump file " << e.what(); - } + // AUX vars (for debug purposes) + tmp_double = d_rem_code_phase_samples; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = static_cast(d_sample_counter + d_current_prn_length_samples); + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } + catch (std::ifstream::failure& e) + { + LOG(WARNING) << "Exception writing trk dump file " << e.what(); + } + } consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates d_sample_counter += d_current_prn_length_samples; // count for the processed samples return 1; // output tracking result ALWAYS even in the case of d_enable_tracking==false @@ -500,23 +503,23 @@ void gps_l2_m_dll_pll_tracking_cc::set_channel(unsigned int channel) LOG(INFO) << "Tracking Channel set to " << d_channel; // ############# ENABLE DATA FILE LOG ################# if (d_dump == true) + { + if (d_dump_file.is_open() == false) { - if (d_dump_file.is_open() == false) - { - try - { - d_dump_filename.append(boost::lexical_cast(d_channel)); - d_dump_filename.append(".dat"); - d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); - d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); - LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); - } - catch (std::ifstream::failure& e) - { - LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); - } - } + try + { + d_dump_filename.append(boost::lexical_cast(d_channel)); + d_dump_filename.append(".dat"); + d_dump_file.exceptions (std::ifstream::failbit | std::ifstream::badbit); + d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary); + LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str(); + } + catch (std::ifstream::failure& e) + { + LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what(); + } } + } } diff --git a/src/core/receiver/CMakeLists.txt b/src/core/receiver/CMakeLists.txt index fd1bfcca4..e4cc6896c 100644 --- a/src/core/receiver/CMakeLists.txt +++ b/src/core/receiver/CMakeLists.txt @@ -76,6 +76,7 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${ARMADILLO_INCLUDE_DIRS} ${GLOG_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 807c5deba..0e85d4f2d 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -90,14 +90,8 @@ #include "galileo_e1b_telemetry_decoder.h" #include "galileo_e5a_telemetry_decoder.h" #include "sbas_l1_telemetry_decoder.h" -#include "gps_l1_ca_observables.h" -#include "gps_l2c_observables.h" -#include "galileo_e1_observables.h" -#include "galileo_e5a_observables.h" #include "hybrid_observables.h" -#include "gps_l1_ca_pvt.h" -#include "galileo_e1_pvt.h" -#include "hybrid_pvt.h" +#include "rtklib_pvt.h" #if ENABLE_FPGA #include "gps_l1_ca_dll_pll_c_aid_tracking_fpga.h" @@ -227,7 +221,7 @@ std::unique_ptr GNSSBlockFactory::GetSignalConditioner( std::unique_ptr GNSSBlockFactory::GetObservables(std::shared_ptr configuration) { - std::string default_implementation = "GPS_L1_CA_Observables"; + std::string default_implementation = "Hybrid_Observables"; std::string implementation = configuration->property("Observables.implementation", default_implementation); LOG(INFO) << "Getting Observables with implementation " << implementation; unsigned int Galileo_channels = configuration->property("Channels_1B.count", 0); @@ -241,14 +235,14 @@ std::unique_ptr GNSSBlockFactory::GetObservables(std::shared std::unique_ptr GNSSBlockFactory::GetPVT(std::shared_ptr configuration) { - std::string default_implementation = "Pass_Through"; + std::string default_implementation = "RTKLIB_PVT"; std::string implementation = configuration->property("PVT.implementation", default_implementation); LOG(INFO) << "Getting PVT with implementation " << implementation; - unsigned int Galileo_channels =configuration->property("Channels_1B.count", 0); + unsigned int Galileo_channels = configuration->property("Channels_1B.count", 0); Galileo_channels += configuration->property("Channels_5X.count", 0); - unsigned int GPS_channels =configuration->property("Channels_1C.count", 0); + unsigned int GPS_channels = configuration->property("Channels_1C.count", 0); GPS_channels += configuration->property("Channels_2S.count", 0); - return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels, 1); + return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels, 0); } @@ -1087,52 +1081,17 @@ std::unique_ptr GNSSBlockFactory::GetBlock( } // OBSERVABLES ----------------------------------------------------------------- - else if (implementation.compare("GPS_L1_CA_Observables") == 0) - { - std::unique_ptr block_(new GpsL1CaObservables(configuration.get(), role, in_streams, - out_streams)); - block = std::move(block_); - } - else if (implementation.compare("GPS_L2C_Observables") == 0) - { - std::unique_ptr block_(new GpsL2CObservables(configuration.get(), role, in_streams, - out_streams)); - block = std::move(block_); - } - else if (implementation.compare("Galileo_E1B_Observables") == 0) - { - std::unique_ptr block_(new GalileoE1Observables(configuration.get(), role, in_streams, - out_streams)); - block = std::move(block_); - } - else if (implementation.compare("Galileo_E5A_Observables") == 0) - { - std::unique_ptr block_(new GalileoE5aObservables(configuration.get(), role, in_streams, - out_streams)); - block = std::move(block_); - } - else if (implementation.compare("Hybrid_Observables") == 0) + else if ((implementation.compare("Hybrid_Observables") == 0) || (implementation.compare("GPS_L1_CA_Observables") == 0) || (implementation.compare("GPS_L2C_Observables") == 0) || + (implementation.compare("Galileo_E5A_Observables") == 0)) { std::unique_ptr block_(new HybridObservables(configuration.get(), role, in_streams, out_streams)); block = std::move(block_); } // PVT ------------------------------------------------------------------------- - else if (implementation.compare("GPS_L1_CA_PVT") == 0) + else if ((implementation.compare("RTKLIB_PVT") == 0) || (implementation.compare("GPS_L1_CA_PVT") == 0) || (implementation.compare("Galileo_E1_PVT") == 0) || (implementation.compare("Hybrid_PVT") == 0)) { - std::unique_ptr block_(new GpsL1CaPvt(configuration.get(), role, in_streams, - out_streams)); - block = std::move(block_); - } - else if (implementation.compare("GALILEO_E1_PVT") == 0) - { - std::unique_ptr block_(new GalileoE1Pvt(configuration.get(), role, in_streams, - out_streams)); - block = std::move(block_); - } - else if (implementation.compare("Hybrid_PVT") == 0) - { - std::unique_ptr block_(new HybridPvt(configuration.get(), role, in_streams, + std::unique_ptr block_(new RtklibPvt(configuration.get(), role, in_streams, out_streams)); block = std::move(block_); } diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index a107eb4e5..f57f3ca04 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -48,6 +48,7 @@ #include "channel_interface.h" #include "gnss_block_factory.h" + #define GNSS_SDR_ARRAY_SIGNAL_CONDITIONER_CHANNELS 8 using google::LogMessage; @@ -70,20 +71,20 @@ GNSSFlowgraph::~GNSSFlowgraph() void GNSSFlowgraph::start() { if (running_) - { - LOG(WARNING) << "Already running"; - return; - } + { + LOG(WARNING) << "Already running"; + return; + } try { - top_block_->start(); + top_block_->start(); } catch (std::exception& e) { - LOG(WARNING) << "Unable to start flowgraph"; - LOG(ERROR) << e.what(); - return; + LOG(WARNING) << "Unable to start flowgraph"; + LOG(ERROR) << e.what(); + return; } running_ = true; @@ -111,253 +112,231 @@ void GNSSFlowgraph::connect() */ LOG(INFO) << "Connecting flowgraph"; if (connected_) - { - LOG(WARNING) << "flowgraph already connected"; - return; - } + { + LOG(WARNING) << "flowgraph already connected"; + return; + } for (int i = 0; i < sources_count_; i++) - { - if (configuration_->property(sig_source_.at(i)->role() + ".enable_FPGA", false)==false) { try { - sig_source_.at(i)->connect(top_block_); + sig_source_.at(i)->connect(top_block_); } catch (std::exception& e) { - LOG(INFO) << "Can't connect signal source block " << i << " internally"; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; + LOG(INFO) << "Can't connect signal source block " << i << " internally"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; } - }else{ - DLOG(INFO)<<"Disabled signal source "< Signal conditioner > for (unsigned int i = 0; i < sig_conditioner_.size(); i++) - { - if (configuration_->property(sig_conditioner_.at(i)->role() + ".enable_FPGA", false)==false) { try { - sig_conditioner_.at(i)->connect(top_block_); + sig_conditioner_.at(i)->connect(top_block_); } catch (std::exception& e) { - LOG(INFO) << "Can't connect signal conditioner block " << i << " internally"; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; + LOG(INFO) << "Can't connect signal conditioner block " << i << " internally"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; } - }else{ - DLOG(INFO)<<"Disabled signal conditioner "<connect(top_block_); + try + { + channels_.at(i)->connect(top_block_); + } + catch (std::exception& e) + { + LOG(WARNING) << "Can't connect channel " << i << " internally"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; + } } - catch (std::exception& e) - { - LOG(WARNING) << "Can't connect channel " << i << " internally"; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; - } - } try { - observables_->connect(top_block_); + observables_->connect(top_block_); } catch (std::exception& e) { - LOG(WARNING) << "Can't connect observables block internally"; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; + LOG(WARNING) << "Can't connect observables block internally"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; } // Signal Source > Signal conditioner >> Channels >> Observables > PVT try { - pvt_->connect(top_block_); + pvt_->connect(top_block_); } catch (std::exception& e) { - LOG(WARNING) << "Can't connect PVT block internally"; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; + LOG(WARNING) << "Can't connect PVT block internally"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; } DLOG(INFO) << "blocks connected internally"; - // Signal Source (i) > Signal conditioner (i) > int RF_Channels = 0; int signal_conditioner_ID = 0; for (int i = 0; i < sources_count_; i++) - { - - //FPGA Accelerators do not need signal sources or conditioners - //as the samples are feed directly to the FPGA fabric, so, if enabled, do not connect any source - if (configuration_->property(sig_source_.at(i)->role() + ".enable_FPGA", false)==false) { try { - //TODO: Remove this array implementation and create generic multistream connector - //(if a signal source has more than 1 stream, then connect it to the multistream signal conditioner) - if(sig_source_.at(i)->implementation().compare("Raw_Array_Signal_Source") == 0) - { - //Multichannel Array - std::cout << "ARRAY MODE" << std::endl; - for (int j = 0; j < GNSS_SDR_ARRAY_SIGNAL_CONDITIONER_CHANNELS; j++) - { - std::cout << "connecting ch " << j << std::endl; - top_block_->connect(sig_source_.at(i)->get_right_block(), j, sig_conditioner_.at(i)->get_left_block(), j); - } - } - else - { - //TODO: Create a class interface for SignalSources, derived from GNSSBlockInterface. - //Include GetRFChannels in the interface to avoid read config parameters here - //read the number of RF channels for each front-end - RF_Channels = configuration_->property(sig_source_.at(i)->role() + ".RF_channels", 1); - - for (int j = 0; j < RF_Channels; j++) - { - //Connect the multichannel signal source to multiple signal conditioners - // GNURADIO max_streams=-1 means infinite ports! - LOG(INFO) << "sig_source_.at(i)->get_right_block()->output_signature()->max_streams()=" << sig_source_.at(i)->get_right_block()->output_signature()->max_streams(); - LOG(INFO) << "sig_conditioner_.at(signal_conditioner_ID)->get_left_block()->input_signature()=" << sig_conditioner_.at(signal_conditioner_ID)->get_left_block()->input_signature()->max_streams(); - - if (sig_source_.at(i)->get_right_block()->output_signature()->max_streams() > 1) + //TODO: Remove this array implementation and create generic multistream connector + //(if a signal source has more than 1 stream, then connect it to the multistream signal conditioner) + if(sig_source_.at(i)->implementation().compare("Raw_Array_Signal_Source") == 0) { - - LOG(INFO) << "connecting sig_source_ " << i << " stream " << j << " to conditioner " << j; - top_block_->connect(sig_source_.at(i)->get_right_block(), j, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0); - + //Multichannel Array + std::cout << "ARRAY MODE" << std::endl; + for (int j = 0; j < GNSS_SDR_ARRAY_SIGNAL_CONDITIONER_CHANNELS; j++) + { + std::cout << "connecting ch " << j << std::endl; + top_block_->connect(sig_source_.at(i)->get_right_block(), j, sig_conditioner_.at(i)->get_left_block(), j); + } } - else + else { - if (j == 0) - { - // RF_channel 0 backward compatibility with single channel sources - LOG(INFO) << "connecting sig_source_ " << i << " stream " << 0 << " to conditioner " << j; - top_block_->connect(sig_source_.at(i)->get_right_block(), 0, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0); - } - else - { - // Multiple channel sources using multiple output blocks of single channel (requires RF_channel selector in call) - LOG(INFO) << "connecting sig_source_ " << i << " stream " << j << " to conditioner " << j; - top_block_->connect(sig_source_.at(i)->get_right_block(j), 0, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0); - } - } + //TODO: Create a class interface for SignalSources, derived from GNSSBlockInterface. + //Include GetRFChannels in the interface to avoid read config parameters here + //read the number of RF channels for each front-end + RF_Channels = configuration_->property(sig_source_.at(i)->role() + ".RF_channels", 1); - signal_conditioner_ID++; - } - } + for (int j = 0; j < RF_Channels; j++) + { + //Connect the multichannel signal source to multiple signal conditioners + // GNURADIO max_streams=-1 means infinite ports! + LOG(INFO) << "sig_source_.at(i)->get_right_block()->output_signature()->max_streams()=" << sig_source_.at(i)->get_right_block()->output_signature()->max_streams(); + LOG(INFO) << "sig_conditioner_.at(signal_conditioner_ID)->get_left_block()->input_signature()=" << sig_conditioner_.at(signal_conditioner_ID)->get_left_block()->input_signature()->max_streams(); + + if (sig_source_.at(i)->get_right_block()->output_signature()->max_streams() > 1) + { + + LOG(INFO) << "connecting sig_source_ " << i << " stream " << j << " to conditioner " << j; + top_block_->connect(sig_source_.at(i)->get_right_block(), j, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0); + } + else + { + if (j == 0) + { + // RF_channel 0 backward compatibility with single channel sources + LOG(INFO) << "connecting sig_source_ " << i << " stream " << 0 << " to conditioner " << j; + top_block_->connect(sig_source_.at(i)->get_right_block(), 0, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0); + } + else + { + // Multiple channel sources using multiple output blocks of single channel (requires RF_channel selector in call) + LOG(INFO) << "connecting sig_source_ " << i << " stream " << j << " to conditioner " << j; + top_block_->connect(sig_source_.at(i)->get_right_block(j), 0, sig_conditioner_.at(signal_conditioner_ID)->get_left_block(), 0); + } + } + signal_conditioner_ID++; + } + } } catch (std::exception& e) { - LOG(WARNING) << "Can't connect signal source " << i << " to signal conditioner " << i; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; + LOG(WARNING) << "Can't connect signal source " << i << " to signal conditioner " << i; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; } - }else{ - DLOG(INFO) << "Signal source "<> channels (i) (dependent of their associated SignalSource_ID) int selected_signal_conditioner_ID; for (unsigned int i = 0; i < channels_count_; i++) - { - - selected_signal_conditioner_ID = configuration_->property("Channel" + boost::lexical_cast(i) + ".RF_channel_ID", 0); - //FPGA Accelerators do not need signal sources or conditioners - //as the samples are feed directly to the FPGA fabric, so, if enabled, do not connect any source - if (configuration_->property(sig_conditioner_.at(selected_signal_conditioner_ID)->role() + ".enable_FPGA", false)==false) { + selected_signal_conditioner_ID = configuration_->property("Channel" + boost::lexical_cast(i) + ".RF_channel_ID", 0); try { - top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, - channels_.at(i)->get_left_block(), 0); + top_block_->connect(sig_conditioner_.at(selected_signal_conditioner_ID)->get_right_block(), 0, + channels_.at(i)->get_left_block(), 0); } catch (std::exception& e) { - LOG(WARNING) << "Can't connect signal conditioner " << selected_signal_conditioner_ID << " to channel " << i; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; + LOG(WARNING) << "Can't connect signal conditioner " << selected_signal_conditioner_ID << " to channel " << i; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; } DLOG(INFO) << "signal conditioner " << selected_signal_conditioner_ID << " connected to channel " << i; - }else{ - DLOG(INFO) << "signal conditioner disabled by FPGA in channel " << i; - } - // Signal Source > Signal conditioner >> Channels >> Observables - try - { - top_block_->connect(channels_.at(i)->get_right_block(), 0, - observables_->get_left_block(), i); - } - catch (std::exception& e) - { - LOG(WARNING) << "Can't connect channel " << i << " to observables"; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; - } - std::string gnss_signal = channels_.at(i)->get_signal().get_signal_str(); // use channel's implicit signal! - while (gnss_signal.compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) - { - available_GNSS_signals_.push_back(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - } - channels_.at(i)->set_signal(available_GNSS_signals_.front()); + // Signal Source > Signal conditioner >> Channels >> Observables + try + { + top_block_->connect(channels_.at(i)->get_right_block(), 0, + observables_->get_left_block(), i); + } + catch (std::exception& e) + { + LOG(WARNING) << "Can't connect channel " << i << " to observables"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; + } - if (channels_state_[i] == 1) - { - channels_.at(i)->start_acquisition(); - available_GNSS_signals_.pop_front(); - LOG(INFO) << "Channel " << i << " assigned to " << available_GNSS_signals_.front(); - LOG(INFO) << "Channel " << i << " connected to observables and ready for acquisition"; + std::string gnss_signal = channels_.at(i)->get_signal().get_signal_str(); // use channel's implicit signal! + while (gnss_signal.compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) + { + available_GNSS_signals_.push_back(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + } + channels_.at(i)->set_signal(available_GNSS_signals_.front()); + + if (channels_state_[i] == 1) + { + channels_.at(i)->start_acquisition(); + available_GNSS_signals_.pop_front(); + LOG(INFO) << "Channel " << i << " assigned to " << available_GNSS_signals_.front(); + LOG(INFO) << "Channel " << i << " connected to observables and ready for acquisition"; + } + else + { + LOG(INFO) << "Channel " << i << " connected to observables in standby mode"; + } + //connect the sample counter to the channel 0 + if (i == 0) + { + ch_out_sample_counter = gnss_sdr_make_sample_counter(); + top_block_->connect(channels_.at(i)->get_right_block(), 0, ch_out_sample_counter, 0); + + } } - else - { - LOG(INFO) << "Channel " << i << " connected to observables in standby mode"; - } - } /* * Connect the observables output of each channel to the PVT block */ try { - for (unsigned int i = 0; i < channels_count_; i++) - { - 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")); - } + for (unsigned int i = 0; i < channels_count_; i++) + { + 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")); + } } catch (std::exception& e) { - LOG(WARNING) << "Can't connect observables to PVT"; - LOG(ERROR) << e.what(); - top_block_->disconnect_all(); - return; + LOG(WARNING) << "Can't connect observables to PVT"; + LOG(ERROR) << e.what(); + top_block_->disconnect_all(); + return; } connected_ = true; @@ -369,10 +348,10 @@ void GNSSFlowgraph::connect() void GNSSFlowgraph::wait() { if (!running_) - { - LOG(WARNING) << "Can't apply wait. Flowgraph is not running"; - return; - } + { + LOG(WARNING) << "Can't apply wait. Flowgraph is not running"; + return; + } top_block_->wait(); DLOG(INFO) << "Flowgraph finished calculations"; running_ = false; @@ -387,6 +366,7 @@ bool GNSSFlowgraph::send_telemetry_msg(pmt::pmt_t msg) return true; } + /* * Applies an action to the flowgraph * @@ -404,13 +384,14 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) available_GNSS_signals_.push_back(channels_.at(who)->get_signal()); //TODO: Optimize the channel and signal matching! while ( channels_.at(who)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) - { - available_GNSS_signals_.push_back(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - } + { + available_GNSS_signals_.push_back(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + } channels_.at(who)->set_signal(available_GNSS_signals_.front()); available_GNSS_signals_.pop_front(); usleep(100); + LOG(INFO) << "Channel "<< who << " Starting acquisition " << channels_.at(who)->get_signal().get_satellite() << ", Signal " << channels_.at(who)->get_signal().get_signal_str(); channels_.at(who)->start_acquisition(); break; case 1: @@ -418,42 +399,42 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) channels_state_[who] = 2; acq_channels_count_--; if (!available_GNSS_signals_.empty() && acq_channels_count_ < max_acq_channels_) - { - for (unsigned int i = 0; i < channels_count_; i++) { - if (channels_state_[i] == 0) - { - channels_state_[i] = 1; - while (channels_.at(i)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) + for (unsigned int i = 0; i < channels_count_; i++) { - available_GNSS_signals_.push_back(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); + if (channels_state_[i] == 0) + { + channels_state_[i] = 1; + while (channels_.at(i)->get_signal().get_signal_str().compare(available_GNSS_signals_.front().get_signal_str()) != 0 ) + { + available_GNSS_signals_.push_back(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + } + channels_.at(i)->set_signal(available_GNSS_signals_.front()); + available_GNSS_signals_.pop_front(); + acq_channels_count_++; + channels_.at(i)->start_acquisition(); + break; + } + DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; } - channels_.at(i)->set_signal(available_GNSS_signals_.front()); - available_GNSS_signals_.pop_front(); - acq_channels_count_++; - channels_.at(i)->start_acquisition(); - break; - } - DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; } - } break; case 2: LOG(INFO) << "Channel " << who << " TRK FAILED satellite " << channels_.at(who)->get_signal().get_satellite(); if (acq_channels_count_ < max_acq_channels_) - { - channels_state_[who] = 1; - acq_channels_count_++; - channels_.at(who)->start_acquisition(); - } + { + channels_state_[who] = 1; + acq_channels_count_++; + channels_.at(who)->start_acquisition(); + } else - { - channels_state_[who] = 0; - available_GNSS_signals_.push_back( channels_.at(who)->get_signal() ); - } + { + channels_state_[who] = 0; + available_GNSS_signals_.push_back( channels_.at(who)->get_signal() ); + } // for (unsigned int i = 0; i < channels_count_; i++) // { @@ -472,15 +453,15 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) void GNSSFlowgraph::set_configuration(std::shared_ptr configuration) { if (running_) - { - LOG(WARNING) << "Unable to update configuration while flowgraph running"; - return; - } + { + LOG(WARNING) << "Unable to update configuration while flowgraph running"; + return; + } if (connected_) - { - LOG(WARNING) << "Unable to update configuration while flowgraph connected"; - } + { + LOG(WARNING) << "Unable to update configuration while flowgraph connected"; + } configuration_ = configuration; } @@ -500,58 +481,75 @@ void GNSSFlowgraph::init() int signal_conditioner_ID = 0; if (sources_count_ > 1) - { - for (int i = 0; i < sources_count_; i++) { - std::cout << "Creating source " << i << std::endl; - sig_source_.push_back(block_factory_->GetSignalSource(configuration_, queue_, i)); + for (int i = 0; i < sources_count_; i++) + { + std::cout << "Creating source " << i << std::endl; + sig_source_.push_back(block_factory_->GetSignalSource(configuration_, queue_, i)); + //TODO: Create a class interface for SignalSources, derived from GNSSBlockInterface. + //Include GetRFChannels in the interface to avoid read config parameters here + //read the number of RF channels for each front-end + RF_Channels = configuration_->property(sig_source_.at(i)->role() + ".RF_channels", 1); + std::cout << "RF Channels " << RF_Channels << std::endl; + for (int j = 0; j < RF_Channels; j++) + { + sig_conditioner_.push_back(block_factory_->GetSignalConditioner(configuration_, signal_conditioner_ID)); + signal_conditioner_ID++; + } + } + } + else + { + //backwards compatibility for old config files + sig_source_.push_back(block_factory_->GetSignalSource(configuration_, queue_, -1)); //TODO: Create a class interface for SignalSources, derived from GNSSBlockInterface. //Include GetRFChannels in the interface to avoid read config parameters here //read the number of RF channels for each front-end - RF_Channels = configuration_->property(sig_source_.at(i)->role() + ".RF_channels", 1); - std::cout << "RF Channels " << RF_Channels << std::endl; - for (int j = 0; j < RF_Channels; j++) - { - sig_conditioner_.push_back(block_factory_->GetSignalConditioner(configuration_, signal_conditioner_ID)); - signal_conditioner_ID++; - } + RF_Channels = configuration_->property(sig_source_.at(0)->role() + ".RF_channels", 0); + if (RF_Channels != 0) + { + for (int j = 0; j < RF_Channels; j++) + { + sig_conditioner_.push_back(block_factory_->GetSignalConditioner(configuration_, signal_conditioner_ID)); + signal_conditioner_ID++; + } + } + else + { + //old config file, single signal source and single channel, not specified + sig_conditioner_.push_back(block_factory_->GetSignalConditioner(configuration_, -1)); + } } - } - else - { - //backwards compatibility for old config files - sig_source_.push_back(block_factory_->GetSignalSource(configuration_, queue_, -1)); - //TODO: Create a class interface for SignalSources, derived from GNSSBlockInterface. - //Include GetRFChannels in the interface to avoid read config parameters here - //read the number of RF channels for each front-end - RF_Channels = configuration_->property(sig_source_.at(0)->role() + ".RF_channels", 0); - if (RF_Channels != 0) - { - for (int j = 0; j < RF_Channels; j++) - { - sig_conditioner_.push_back(block_factory_->GetSignalConditioner(configuration_, signal_conditioner_ID)); - signal_conditioner_ID++; - } - } - else - { - //old config file, single signal source and single channel, not specified - sig_conditioner_.push_back(block_factory_->GetSignalConditioner(configuration_, -1)); - } - } observables_ = block_factory_->GetObservables(configuration_); + // Mark old implementations as deprecated + std::string default_str("Default"); + std::string obs_implementation = configuration_->property("Observables.implementation", default_str); + if ((obs_implementation.compare("GPS_L1_CA_Observables") == 0) || (obs_implementation.compare("GPS_L2C_Observables") == 0) || + (obs_implementation.compare("Galileo_E1B_Observables") == 0) || (obs_implementation.compare("Galileo_E5A_Observables") == 0)) + { + std::cout << "WARNING: Implementation '" << obs_implementation << "' of the Observables block has been replaced by 'Hybrid_Observables'." << std::endl; + std::cout << "Please update your configuration file." << std::endl; + } + pvt_ = block_factory_->GetPVT(configuration_); + // Mark old implementations as deprecated + std::string pvt_implementation = configuration_->property("PVT.implementation", default_str); + if ((pvt_implementation.compare("GPS_L1_CA_PVT") == 0) || (pvt_implementation.compare("Galileo_E1_PVT") == 0) || (pvt_implementation.compare("Hybrid_PVT") == 0)) + { + std::cout << "WARNING: Implementation '" << pvt_implementation << "' of the PVT block has been replaced by 'RTKLIB_PVT'." << std::endl; + std::cout << "Please update your configuration file." << std::endl; + } std::shared_ptr>> channels = block_factory_->GetChannels(configuration_, queue_); //todo:check smart pointer coherence... channels_count_ = channels->size(); for (unsigned int i = 0; i < channels_count_; i++) - { - std::shared_ptr chan_ = std::move(channels->at(i)); - channels_.push_back(std::dynamic_pointer_cast(chan_)); - } + { + std::shared_ptr chan_ = std::move(channels->at(i)); + channels_.push_back(std::dynamic_pointer_cast(chan_)); + } top_block_ = gr::make_top_block("GNSSFlowgraph"); @@ -603,121 +601,121 @@ void GNSSFlowgraph::set_signals_list() std::string sv_list = configuration_->property("Galileo.prns", std::string("") ); if( sv_list.length() > 0 ) - { - // Reset the available prns: - std::set< unsigned int > tmp_set; - boost::tokenizer<> tok( sv_list ); - std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ), - boost::lexical_cast ); - - if( tmp_set.size() > 0 ) { - available_galileo_prn = tmp_set; + // Reset the available prns: + std::set< unsigned int > tmp_set; + boost::tokenizer<> tok( sv_list ); + std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ), + boost::lexical_cast ); + + if( tmp_set.size() > 0 ) + { + available_galileo_prn = tmp_set; + } } - } sv_list = configuration_->property("GPS.prns", std::string("") ); if( sv_list.length() > 0 ) - { - // Reset the available prns: - std::set< unsigned int > tmp_set; - boost::tokenizer<> tok( sv_list ); - std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ), - boost::lexical_cast ); - - if( tmp_set.size() > 0 ) { - available_gps_prn = tmp_set; + // Reset the available prns: + std::set< unsigned int > tmp_set; + boost::tokenizer<> tok( sv_list ); + std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ), + boost::lexical_cast ); + + if( tmp_set.size() > 0 ) + { + available_gps_prn = tmp_set; + } } - } sv_list = configuration_->property("SBAS.prns", std::string("") ); if( sv_list.length() > 0 ) - { - // Reset the available prns: - std::set< unsigned int > tmp_set; - boost::tokenizer<> tok( sv_list ); - std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ), - boost::lexical_cast ); - - if( tmp_set.size() > 0 ) { - available_sbas_prn = tmp_set; + // Reset the available prns: + std::set< unsigned int > tmp_set; + boost::tokenizer<> tok( sv_list ); + std::transform( tok.begin(), tok.end(), std::inserter( tmp_set, tmp_set.begin() ), + boost::lexical_cast ); + + if( tmp_set.size() > 0 ) + { + available_sbas_prn = tmp_set; + } } - } if (configuration_->property("Channels_1C.count", 0) > 0 ) - { - /* - * Loop to create GPS L1 C/A signals - */ - for (available_gnss_prn_iter = available_gps_prn.begin(); - available_gnss_prn_iter != available_gps_prn.end(); - available_gnss_prn_iter++) { - available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("GPS"), - *available_gnss_prn_iter), std::string("1C"))); + /* + * Loop to create GPS L1 C/A signals + */ + for (available_gnss_prn_iter = available_gps_prn.begin(); + available_gnss_prn_iter != available_gps_prn.end(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("GPS"), + *available_gnss_prn_iter), std::string("1C"))); + } } - } if (configuration_->property("Channels_2S.count", 0) > 0) - { - /* - * Loop to create GPS L2C M signals - */ - for (available_gnss_prn_iter = available_gps_prn.begin(); - available_gnss_prn_iter != available_gps_prn.end(); - available_gnss_prn_iter++) { - available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("GPS"), - *available_gnss_prn_iter), std::string("2S"))); + /* + * Loop to create GPS L2C M signals + */ + for (available_gnss_prn_iter = available_gps_prn.begin(); + available_gnss_prn_iter != available_gps_prn.end(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("GPS"), + *available_gnss_prn_iter), std::string("2S"))); + } } - } if (configuration_->property("Channels_SBAS.count", 0) > 0) - { - /* - * Loop to create SBAS L1 C/A signals - */ - for (available_gnss_prn_iter = available_sbas_prn.begin(); - available_gnss_prn_iter != available_sbas_prn.end(); - available_gnss_prn_iter++) { - available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("SBAS"), - *available_gnss_prn_iter), std::string("1C"))); + /* + * Loop to create SBAS L1 C/A signals + */ + for (available_gnss_prn_iter = available_sbas_prn.begin(); + available_gnss_prn_iter != available_sbas_prn.end(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("SBAS"), + *available_gnss_prn_iter), std::string("1C"))); + } } - } if (configuration_->property("Channels_1B.count", 0) > 0) - { - /* - * Loop to create the list of Galileo E1 B signals - */ - for (available_gnss_prn_iter = available_galileo_prn.begin(); - available_gnss_prn_iter != available_galileo_prn.end(); - available_gnss_prn_iter++) { - available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("Galileo"), - *available_gnss_prn_iter), std::string("1B"))); + /* + * Loop to create the list of Galileo E1 B signals + */ + for (available_gnss_prn_iter = available_galileo_prn.begin(); + available_gnss_prn_iter != available_galileo_prn.end(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("Galileo"), + *available_gnss_prn_iter), std::string("1B"))); + } } - } if (configuration_->property("Channels_5X.count", 0) > 0 ) - { - /* - * Loop to create the list of Galileo E1 B signals - */ - for (available_gnss_prn_iter = available_galileo_prn.begin(); - available_gnss_prn_iter != available_galileo_prn.end(); - available_gnss_prn_iter++) { - available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("Galileo"), - *available_gnss_prn_iter), std::string("5X"))); + /* + * Loop to create the list of Galileo E1 B signals + */ + for (available_gnss_prn_iter = available_galileo_prn.begin(); + available_gnss_prn_iter != available_galileo_prn.end(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal(Gnss_Satellite(std::string("Galileo"), + *available_gnss_prn_iter), std::string("5X"))); + } } - } /* * Ordering the list of signals from configuration file */ @@ -726,24 +724,24 @@ void GNSSFlowgraph::set_signals_list() // Pre-assignation if not defined at ChannelX.signal=1C ...? In what order? for (unsigned int i = 0; i < total_channels; i++) - { - std::string gnss_signal = (configuration_->property("Channel" + boost::lexical_cast(i) + ".signal", std::string("1C"))); - std::string gnss_system; - if((gnss_signal.compare("1C") == 0) or (gnss_signal.compare("2S") == 0) ) gnss_system = "GPS"; - if((gnss_signal.compare("1B") == 0) or (gnss_signal.compare("5X") == 0) ) gnss_system = "Galileo"; - unsigned int sat = configuration_->property("Channel" + boost::lexical_cast(i) + ".satellite", 0); - LOG(INFO) << "Channel " << i << " system " << gnss_system << ", signal " << gnss_signal <<", sat "<property("Channel" + boost::lexical_cast(i) + ".signal", std::string("1C"))); + std::string gnss_system; + if((gnss_signal.compare("1C") == 0) or (gnss_signal.compare("2S") == 0) ) gnss_system = "GPS"; + if((gnss_signal.compare("1B") == 0) or (gnss_signal.compare("5X") == 0) ) gnss_system = "Galileo"; + unsigned int sat = configuration_->property("Channel" + boost::lexical_cast(i) + ".satellite", 0); + LOG(INFO) << "Channel " << i << " system " << gnss_system << ", signal " << gnss_signal <<", sat "<::iterator available_gnss_list_iter; @@ -759,21 +757,21 @@ void GNSSFlowgraph::set_channels_state() { max_acq_channels_ = (configuration_->property("Channels.in_acquisition", channels_count_)); if (max_acq_channels_ > channels_count_) - { - max_acq_channels_ = channels_count_; - LOG(WARNING) << "Channels_in_acquisition is bigger than number of channels. Variable acq_channels_count_ is set to " << channels_count_; - } + { + max_acq_channels_ = channels_count_; + LOG(WARNING) << "Channels_in_acquisition is bigger than number of channels. Variable acq_channels_count_ is set to " << channels_count_; + } channels_state_.reserve(channels_count_); for (unsigned int i = 0; i < channels_count_; i++) - { - if (i < max_acq_channels_) { - channels_state_.push_back(1); + if (i < max_acq_channels_) + { + channels_state_.push_back(1); + } + else + channels_state_.push_back(0); + DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; } - else - channels_state_.push_back(0); - DLOG(INFO) << "Channel " << i << " in state " << channels_state_[i]; - } acq_channels_count_ = max_acq_channels_; DLOG(INFO) << acq_channels_count_ << " channels in acquisition state"; } diff --git a/src/core/receiver/gnss_flowgraph.h b/src/core/receiver/gnss_flowgraph.h index ad06267b9..d765d3cf4 100644 --- a/src/core/receiver/gnss_flowgraph.h +++ b/src/core/receiver/gnss_flowgraph.h @@ -45,6 +45,7 @@ #include #include "GPS_L1_CA.h" #include "gnss_signal.h" +#include "gnss_sdr_sample_counter.h" class GNSSBlockInterface; class ChannelInterface; @@ -136,6 +137,7 @@ private: std::shared_ptr pvt_; std::vector> channels_; + gnss_sdr_sample_counter_sptr ch_out_sample_counter; gr::top_block_sptr top_block_; boost::shared_ptr queue_; std::list available_GNSS_signals_; diff --git a/src/core/system_parameters/CMakeLists.txt b/src/core/system_parameters/CMakeLists.txt index a22c7d9c2..68b6862cf 100644 --- a/src/core/system_parameters/CMakeLists.txt +++ b/src/core/system_parameters/CMakeLists.txt @@ -33,9 +33,6 @@ set(SYSTEM_PARAMETERS_SOURCES galileo_iono.cc galileo_navigation_message.cc sbas_ephemeris.cc - sbas_ionospheric_correction.cc - sbas_satellite_correction.cc - sbas_telemetry_data.cc galileo_fnav_message.cc gps_cnav_ephemeris.cc gps_cnav_navigation_message.cc @@ -48,6 +45,8 @@ set(SYSTEM_PARAMETERS_SOURCES include_directories( $(CMAKE_CURRENT_SOURCE_DIR) ${CMAKE_SOURCE_DIR}/src/core/receiver + ${CMAKE_SOURCE_DIR}/src/algorithms/PVT/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/libs/rtklib ${GLOG_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${GFlags_INCLUDE_DIRS} @@ -58,6 +57,6 @@ file(GLOB SYSTEM_PARAMETERS_HEADERS "*.h") list(SORT SYSTEM_PARAMETERS_HEADERS) add_library(gnss_system_parameters ${SYSTEM_PARAMETERS_SOURCES} ${SYSTEM_PARAMETERS_HEADERS}) source_group(Headers FILES ${SYSTEM_PARAMETERS_HEADERS}) -add_dependencies(gnss_system_parameters glog-${glog_RELEASE}) -target_link_libraries(gnss_system_parameters ${Boost_LIBRARIES}) +add_dependencies(gnss_system_parameters rtklib_lib glog-${glog_RELEASE}) +target_link_libraries(gnss_system_parameters rtklib_lib ${Boost_LIBRARIES}) diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index ec6b79fe2..e72c076fb 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -35,19 +35,20 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants -const double GPS_C_m_s = 299792458.0; //!< The speed of light, [m/s] +const double GPS_C_m_s = SPEED_OF_LIGHT; //!< The speed of light, [m/s] const double GPS_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E const double GPS_TWO_PI = 6.283185307179586;//!< 2Pi as defined in IS-GPS-200E -const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s] +const double OMEGA_EARTH_DOT = DEFAULT_OMEGA_EARTH_DOT; //!< Earth rotation rate, [rad/s] const double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] const double F = -4.442807633e-10; //!< Constant, [s/(m)^(1/2)] // carrier and code frequencies -const double GPS_L1_FREQ_HZ = 1.57542e9; //!< L1 [Hz] +const double GPS_L1_FREQ_HZ = FREQ1; //!< L1 [Hz] const double GPS_L1_CA_CODE_RATE_HZ = 1.023e6; //!< GPS L1 C/A code rate [chips/s] const double GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0; //!< GPS L1 C/A code length [chips] const double GPS_L1_CA_CODE_PERIOD = 0.001; //!< GPS L1 C/A code period [seconds] @@ -82,7 +83,7 @@ const int GPS_WORD_LENGTH = 4; //!< CRC + GPS WORD (-2 -1 0 const int GPS_SUBFRAME_LENGTH = 40; //!< GPS_WORD_LENGTH x 10 = 40 bytes const int GPS_SUBFRAME_BITS = 300; //!< Number of bits per subframe in the NAV message [bits] const int GPS_SUBFRAME_SECONDS = 6; //!< Subframe duration [seconds] -const int GPS_SUBFRAME_MS = 6000; //!< Subframe duration [seconds] +const int GPS_SUBFRAME_MS = 6000; //!< Subframe duration [seconds] const int GPS_WORD_BITS = 30; //!< Number of bits per word in the NAV message [bits] // GPS NAVIGATION MESSAGE STRUCTURE diff --git a/src/core/system_parameters/GPS_L2C.h b/src/core/system_parameters/GPS_L2C.h index c6c703952..babbc59ce 100644 --- a/src/core/system_parameters/GPS_L2C.h +++ b/src/core/system_parameters/GPS_L2C.h @@ -36,6 +36,7 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants const double GPS_L2_C_m_s = 299792458.0; //!< The speed of light, [m/s] @@ -48,7 +49,7 @@ const double GPS_L2_F = -4.442807633e-10; //!< Constant, [s/(m)^(1 // carrier and code frequencies -const double GPS_L2_FREQ_HZ = 1.2276e9; //!< L2 [Hz] +const double GPS_L2_FREQ_HZ = FREQ2; //!< L2 [Hz] const double GPS_L2_M_CODE_RATE_HZ = 0.5115e6; //!< GPS L2 M code rate [chips/s] const int GPS_L2_M_CODE_LENGTH_CHIPS = 10230; //!< GPS L2 M code length [chips] diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index 258a9e954..8347393aa 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -37,6 +37,7 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" // Physical constants const double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD @@ -48,7 +49,7 @@ const double GALILEO_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] const double GALILEO_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)] // carrier and code frequencies -const double Galileo_E1_FREQ_HZ = 1.57542e9; //!< Galileo E1 carrier frequency [Hz] +const double Galileo_E1_FREQ_HZ = FREQ1; //!< Galileo E1 carrier frequency [Hz] const double Galileo_E1_CODE_CHIP_RATE_HZ = 1.023e6; //!< Galileo E1 code rate [chips/s] const double Galileo_E1_CODE_PERIOD = 0.004; //!< Galileo E1 code period [s] const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-carrier 'a' rate [Hz] diff --git a/src/core/system_parameters/Galileo_E5a.h b/src/core/system_parameters/Galileo_E5a.h index 70ec19a1e..09fb8b74b 100644 --- a/src/core/system_parameters/Galileo_E5a.h +++ b/src/core/system_parameters/Galileo_E5a.h @@ -35,19 +35,19 @@ #include #include // std::pair #include "MATH_CONSTANTS.h" +#include "gnss_frequencies.h" -// Physical constants already defined in E1 // Carrier and code frequencies -const double Galileo_E5a_FREQ_HZ = 1.176450e9; //!< Galileo E5a carrier frequency [Hz] +const double Galileo_E5a_FREQ_HZ = FREQ5; //!< Galileo E5a carrier frequency [Hz] const double Galileo_E5a_CODE_CHIP_RATE_HZ = 1.023e7; //!< Galileo E5a code rate [chips/s] -const double Galileo_E5a_I_TIERED_CODE_PERIOD = 0.020; //!< Galileo E5a-I tiered code period [s] -const double Galileo_E5a_Q_TIERED_CODE_PERIOD = 0.100; //!< Galileo E5a-Q tiered code period [s] -const int Galileo_E5a_CODE_LENGTH_CHIPS = 10230; //!< Galileo E5a primary code length [chips] -const int Galileo_E5a_I_SECONDARY_CODE_LENGTH = 20; //!< Galileo E5a-I secondary code length [chips] -const int Galileo_E5a_Q_SECONDARY_CODE_LENGTH = 100; //!< Galileo E5a-Q secondary code length [chips] +const double Galileo_E5a_I_TIERED_CODE_PERIOD = 0.020; //!< Galileo E5a-I tiered code period [s] +const double Galileo_E5a_Q_TIERED_CODE_PERIOD = 0.100; //!< Galileo E5a-Q tiered code period [s] +const int Galileo_E5a_CODE_LENGTH_CHIPS = 10230; //!< Galileo E5a primary code length [chips] +const int Galileo_E5a_I_SECONDARY_CODE_LENGTH = 20; //!< Galileo E5a-I secondary code length [chips] +const int Galileo_E5a_Q_SECONDARY_CODE_LENGTH = 100; //!< Galileo E5a-Q secondary code length [chips] const double GALILEO_E5a_CODE_PERIOD = 0.001; -const int Galileo_E5a_SYMBOL_RATE_BPS = 50; //!< Galileo E5a symbol rate [bits/second] +const int Galileo_E5a_SYMBOL_RATE_BPS = 50; //!< Galileo E5a symbol rate [bits/second] const int Galileo_E5a_NUMBER_OF_CODES = 50; diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 1ed316ed5..9be1a41cf 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -40,6 +40,9 @@ PI_TWO_PX ==> Pi*2^X ONE_PI_TWO_PX = (1/Pi)*2^X */ + +const double PI = 3.1415926535897932; //!< pi + const double TWO_P4 = (16); //!< 2^4 const double TWO_P11 = (2048); //!< 2^11 const double TWO_P12 = (4096); //!< 2^12 @@ -53,6 +56,7 @@ const double TWO_P57 = (1.441151880758559e+017); //!< 2^57 const double TWO_N2 = (0.25); //!< 2^-2 const double TWO_N5 = (0.03125); //!< 2^-5 +const double TWO_N6 = (0.015625); //!< 2^-6 const double TWO_N8 = (0.00390625); //!< 2^-8 const double TWO_N9 = (0.001953125); //!< 2^-9 const double TWO_N10 = (0.0009765625); //!< 2^-10 @@ -60,9 +64,11 @@ const double TWO_N11 = (4.882812500000000e-004); //!< 2^-11 const double TWO_N14 = (0.00006103515625); //!< 2^-14 const double TWO_N15 = (0.00003051757813); //!< 2^-15 const double TWO_N16 = (0.0000152587890625); //!< 2^-16 +const double TWO_N17 = (7.629394531250000e-006); //!< 2^-17 const double TWO_N19 = (1.907348632812500e-006); //!< 2^-19 const double TWO_N20 = (9.536743164062500e-007); //!< 2^-20 const double TWO_N21 = (4.768371582031250e-007); //!< 2^-21 +const double TWO_N23 = (1.192092895507810e-007); //!< 2^-23 const double TWO_N24 = (5.960464477539063e-008); //!< 2^-24 const double TWO_N25 = (2.980232238769531e-008); //!< 2^-25 const double TWO_N27 = (7.450580596923828e-009); //!< 2^-27 @@ -73,8 +79,9 @@ const double TWO_N32 = (2.328306436538696e-010); //!< 2^-32 const double TWO_N33 = (1.164153218269348e-010); //!< 2^-33 const double TWO_N34 = (5.82076609134674e-011); //!< 2^-34 const double TWO_N35 = (2.91038304567337e-011); //!< 2^-35 - const double TWO_N38 = (3.637978807091713e-012); //!< 2^-38 +const double TWO_N39 = (1.818989403545856e-012); //!< 2^-39 +const double TWO_N40 = (9.094947017729280e-013); //!< 2^-40 const double TWO_N43 = (1.136868377216160e-013); //!< 2^-43 const double TWO_N44 = (5.684341886080802e-14); //!< 2^-44 const double TWO_N46 = (1.4210854715202e-014); //!< 2^-46 @@ -95,4 +102,13 @@ const double PI_TWO_N31 = (1.462918079267160e-009); //!< Pi*2^-31 const double PI_TWO_N38 = (1.142904749427469e-011); //!< Pi*2^-38 const double PI_TWO_N23 = (3.745070282923929e-007); //!< Pi*2^-23 +const double D2R = (PI/180.0); //!< deg to rad +const double R2D = (180.0/PI); //!< rad to deg +const double SC2RAD = 3.1415926535898; //!< semi-circle to radian (IS-GPS) +const double AS2R = (D2R / 3600.0); //!< arc sec to radian + +const double DEFAULT_OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Default Earth rotation rate, [rad/s] +const double SPEED_OF_LIGHT = 299792458.0; //!< [m/s] +const double AU = 149597870691.0; //!< 1 Astronomical Unit AU (m) distance from Earth to the Sun. + #endif /* GNSS_SDR_MATH_CONSTANTS_H_ */ diff --git a/src/core/system_parameters/gnss_frequencies.h b/src/core/system_parameters/gnss_frequencies.h new file mode 100644 index 000000000..bd85aa414 --- /dev/null +++ b/src/core/system_parameters/gnss_frequencies.h @@ -0,0 +1,53 @@ +/*! + * \file gnss_frequencies.h + * \brief GNSS Frequencies + * \author Carles Fernandez, 2017. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GNSS_FREQUENCIES_H_ +#define GNSS_SDR_GNSS_FREQUENCIES_H_ + +const double FREQ1 = 1.57542e9; //!< L1/E1 frequency (Hz) +const double FREQ2 = 1.22760e9; //!< L2 frequency (Hz) +const double FREQ5 = 1.17645e9; //!< L5/E5a frequency (Hz) +const double FREQ6 = 1.27875e9; //!< E6/LEX frequency (Hz) +const double FREQ7 = 1.20714e9; //!< E5b frequency (Hz) +const double FREQ8 = 1.191795e9; //!< E5a+b frequency (Hz) +const double FREQ9 = 2.492028e9; //!< S frequency (Hz) +const double FREQ1_GLO = 1.60200e9; //!< GLONASS G1 base frequency (Hz) +const double DFRQ1_GLO = 0.56250e6; //!< GLONASS G1 bias frequency (Hz/n) +const double FREQ2_GLO = 1.24600e9; //!< GLONASS G2 base frequency (Hz) +const double DFRQ2_GLO = 0.43750e6; //!< GLONASS G2 bias frequency (Hz/n) +const double FREQ3_GLO = 1.202025e9; //!< GLONASS G3 frequency (Hz) +const double FREQ1_BDS = 1.561098e9; //!< BeiDou B1 frequency (Hz) +const double FREQ2_BDS = 1.20714e9; //!< BeiDou B2 frequency (Hz) +const double FREQ3_BDS = 1.26852e9; //!< BeiDou B3 frequency (Hz) + +#endif + diff --git a/src/core/system_parameters/gnss_obs_codes.h b/src/core/system_parameters/gnss_obs_codes.h new file mode 100644 index 000000000..1c17cafc9 --- /dev/null +++ b/src/core/system_parameters/gnss_obs_codes.h @@ -0,0 +1,97 @@ +/*! + * \file gnss_obs_codes.h + * \brief GNSS Observable codes + * \author Carles Fernandez, 2017. cfernandez(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_GNSS_OBS_CODES_H_ +#define GNSS_SDR_GNSS_OBS_CODES_H_ + + + +const unsigned int CODE_NONE = 0; //!< obs code: none or unknown +const unsigned int CODE_L1C = 1; //!< obs code: L1C/A,G1C/A,E1C (GPS,GLO,GAL,QZS,SBS) +const unsigned int CODE_L1P = 2; //!< obs code: L1P,G1P (GPS,GLO) +const unsigned int CODE_L1W = 3; //!< obs code: L1 Z-track (GPS) +const unsigned int CODE_L1Y = 4; //!< obs code: L1Y (GPS) +const unsigned int CODE_L1M = 5; //!< obs code: L1M (GPS) +const unsigned int CODE_L1N = 6; //!< obs code: L1codeless (GPS) +const unsigned int CODE_L1S = 7; //!< obs code: L1C(D) (GPS,QZS) +const unsigned int CODE_L1L = 8; //!< obs code: L1C(P) (GPS,QZS) +const unsigned int CODE_L1E = 9; //!< (not used) +const unsigned int CODE_L1A = 10; //!< obs code: E1A (GAL) +const unsigned int CODE_L1B = 11; //!< obs code: E1B (GAL) +const unsigned int CODE_L1X = 12; //!< obs code: E1B+C,L1C(D+P) (GAL,QZS) +const unsigned int CODE_L1Z = 13; //!< obs code: E1A+B+C,L1SAIF (GAL,QZS) +const unsigned int CODE_L2C = 14; //!< obs code: L2C/A,G1C/A (GPS,GLO) +const unsigned int CODE_L2D = 15; //!< obs code: L2 L1C/A-(P2-P1) (GPS) +const unsigned int CODE_L2S = 16; //!< obs code: L2C(M) (GPS,QZS) +const unsigned int CODE_L2L = 17; //!< obs code: L2C(L) (GPS,QZS) +const unsigned int CODE_L2X = 18; //!< obs code: L2C(M+L),B1I+Q (GPS,QZS,BDS) +const unsigned int CODE_L2P = 19; //!< obs code: L2P,G2P (GPS,GLO) +const unsigned int CODE_L2W = 20; //!< obs code: L2 Z-track (GPS) +const unsigned int CODE_L2Y = 21; //!< obs code: L2Y (GPS) +const unsigned int CODE_L2M = 22; //!< obs code: L2M (GPS) +const unsigned int CODE_L2N = 23; //!< obs code: L2codeless (GPS) +const unsigned int CODE_L5I = 24; //!< obs code: L5/E5aI (GPS,GAL,QZS,SBS) +const unsigned int CODE_L5Q = 25; //!< obs code: L5/E5aQ (GPS,GAL,QZS,SBS) +const unsigned int CODE_L5X = 26; //!< obs code: L5/E5aI+Q/L5B+C (GPS,GAL,QZS,IRN,SBS) +const unsigned int CODE_L7I = 27; //!< obs code: E5bI,B2I (GAL,BDS) +const unsigned int CODE_L7Q = 28; //!< obs code: E5bQ,B2Q (GAL,BDS) +const unsigned int CODE_L7X = 29; //!< obs code: E5bI+Q,B2I+Q (GAL,BDS) +const unsigned int CODE_L6A = 30; //!< obs code: E6A (GAL) +const unsigned int CODE_L6B = 31; //!< obs code: E6B (GAL) +const unsigned int CODE_L6C = 32; //!< obs code: E6C (GAL) +const unsigned int CODE_L6X = 33; //!< obs code: E6B+C,LEXS+L,B3I+Q (GAL,QZS,BDS) +const unsigned int CODE_L6Z = 34; //!< obs code: E6A+B+C (GAL) +const unsigned int CODE_L6S = 35; //!< obs code: LEXS (QZS) +const unsigned int CODE_L6L = 36; //!< obs code: LEXL (QZS) +const unsigned int CODE_L8I = 37; //!< obs code: E5(a+b)I (GAL) +const unsigned int CODE_L8Q = 38; //!< obs code: E5(a+b)Q (GAL) +const unsigned int CODE_L8X = 39; //!< obs code: E5(a+b)I+Q (GAL) +const unsigned int CODE_L2I = 40; //!< obs code: B1I (BDS) +const unsigned int CODE_L2Q = 41; //!< obs code: B1Q (BDS) +const unsigned int CODE_L6I = 42; //!< obs code: B3I (BDS) +const unsigned int CODE_L6Q = 43; //!< obs code: B3Q (BDS) +const unsigned int CODE_L3I = 44; //!< obs code: G3I (GLO) +const unsigned int CODE_L3Q = 45; //!< obs code: G3Q (GLO) +const unsigned int CODE_L3X = 46; //!< obs code: G3I+Q (GLO) +const unsigned int CODE_L1I = 47; //!< obs code: B1I (BDS) +const unsigned int CODE_L1Q = 48; //!< obs code: B1Q (BDS) +const unsigned int CODE_L5A = 49; //!< obs code: L5A SPS (IRN) +const unsigned int CODE_L5B = 50; //!< obs code: L5B RS(D) (IRN) +const unsigned int CODE_L5C = 51; //!< obs code: L5C RS(P) (IRN) +const unsigned int CODE_L9A = 52; //!< obs code: SA SPS (IRN) +const unsigned int CODE_L9B = 53; //!< obs code: SB RS(D) (IRN) +const unsigned int CODE_L9C = 54; //!< obs code: SC RS(P) (IRN) +const unsigned int CODE_L9X = 55; //!< obs code: SB+C (IRN) +const unsigned int MAXCODE = 55; //!< max number of obs code + + +#endif diff --git a/src/core/system_parameters/gnss_synchro.h b/src/core/system_parameters/gnss_synchro.h index 62974e73f..16a2287cf 100644 --- a/src/core/system_parameters/gnss_synchro.h +++ b/src/core/system_parameters/gnss_synchro.h @@ -52,30 +52,27 @@ public: unsigned long int Acq_samplestamp_samples; //!< Set by Acquisition processing block bool Flag_valid_acquisition; //!< Set by Acquisition processing block //Tracking + long int fs; //!< Set by Tracking processing block double Prompt_I; //!< Set by Tracking processing block double Prompt_Q; //!< Set by Tracking processing block double CN0_dB_hz; //!< Set by Tracking processing block double Carrier_Doppler_hz; //!< Set by Tracking processing block double Carrier_phase_rads; //!< Set by Tracking processing block - double Tracking_timestamp_secs; //!< Set by Tracking processing block - double Rem_code_phase_secs; //!< Set by Tracking processing block + double Code_phase_samples; //!< Set by Tracking processing block + unsigned long int Tracking_sample_counter; //!< Set by Tracking processing block bool Flag_valid_symbol_output; //!< Set by Tracking processing block int correlation_length_ms; //!< Set by Tracking processing block //Telemetry Decoder - double Prn_timestamp_ms; //!< Set by Telemetry Decoder processing block - double Prn_timestamp_at_preamble_ms; //!< Set by Telemetry Decoder processing block + bool Flag_valid_word; //!< Set by Telemetry Decoder processing block + double TOW_at_current_symbol_s; //!< Set by Telemetry Decoder processing block - bool Flag_valid_word; //!< Set by Telemetry Decoder processing block - bool Flag_preamble; //!< Set by Telemetry Decoder processing block - double d_TOW; //!< Set by Telemetry Decoder processing block - double d_TOW_at_current_symbol; - double d_TOW_hybrid_at_current_symbol; //Galileo TOW is expressed in the GPS time scale (it will be the same for any other constellation) + // Observables + double Pseudorange_m; //!< Set by Observables processing block + double RX_time; //!< Set by Observables processing block + bool Flag_valid_pseudorange; //!< Set by Observables processing block - // Pseudorange - double Pseudorange_m; - bool Flag_valid_pseudorange; }; #endif diff --git a/src/core/system_parameters/gps_cnav_ephemeris.cc b/src/core/system_parameters/gps_cnav_ephemeris.cc index a248fb66a..7669fb825 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.cc +++ b/src/core/system_parameters/gps_cnav_ephemeris.cc @@ -33,6 +33,7 @@ #include "gps_cnav_ephemeris.h" #include #include "GPS_L2C.h" +#include Gps_CNAV_Ephemeris::Gps_CNAV_Ephemeris() { @@ -119,6 +120,10 @@ double Gps_CNAV_Ephemeris::sv_clock_drift(double transmitTime) double dt; dt = check_t(transmitTime - d_Toc); d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime); + + //Correct satellite group delay + d_satClkDrift-=d_TGD; + return d_satClkDrift; } @@ -153,7 +158,7 @@ double Gps_CNAV_Ephemeris::sv_clock_relativistic_term(double transmitTime) M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi - M = fmod((M + 2.0 * GPS_L2_PI), (2.0 * GPS_L2_PI)); + //M = fmod((M + 2.0 * GPS_L2_PI), (2.0 * GPS_L2_PI)); // Initial guess of eccentric anomaly E = M; @@ -193,11 +198,13 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) double r; double i; double Omega; - const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 163 + + const double A_REF = 26559710.0; // See IS-GPS-200H, pp. 170 double d_sqrt_A = sqrt(A_REF + d_DELTA_A); + + const double GM = 3.986005e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] const double OMEGA_DOT_REF = -2.6e-9; // semicircles / s, see IS-GPS-200H pp. 164 - double d_OMEGA_DOT = OMEGA_DOT_REF + d_DELTA_OMEGA_DOT; const double OMEGA_EARTH_DOT = 7.2921151467e-5; //!< Earth rotation rate, [rad/s] // Find satellite's position ---------------------------------------------- @@ -210,14 +217,19 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) // Computed mean motion n0 = sqrt(GM / (a*a*a)); + // Mean motion difference from computed value + + double delta_n_a=d_Delta_n+0.5*d_DELTA_DOT_N*tk; + // Corrected mean motion - n = n0 + d_Delta_n; + n = n0 + delta_n_a; // Mean anomaly M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi - M = fmod((M + 2 * GPS_L2_PI), (2 * GPS_L2_PI)); + //M = fmod((M + 2 * GPS_L2_PI), (2 * GPS_L2_PI)); + // Initial guess of eccentric anomaly E = M; @@ -244,7 +256,7 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) phi = nu + d_OMEGA; // Reduce phi to between 0 and 2*pi rad - phi = fmod((phi), (2*GPS_L2_PI)); + //phi = fmod((phi), (2*GPS_L2_PI)); // Correct argument of latitude u = phi + d_Cuc * cos(2*phi) + d_Cus * sin(2*phi); @@ -256,10 +268,11 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) i = d_i_0 + d_IDOT * tk + d_Cic * cos(2*phi) + d_Cis * sin(2*phi); // Compute the angle between the ascending node and the Greenwich meridian + double d_OMEGA_DOT = OMEGA_DOT_REF*GPS_L2_PI + d_DELTA_OMEGA_DOT; Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe1; // Reduce to between 0 and 2*pi rad - Omega = fmod((Omega + 2*GPS_L2_PI), (2*GPS_L2_PI)); + //Omega = fmod((Omega + 2*GPS_L2_PI), (2*GPS_L2_PI)); // --- Compute satellite coordinates in Earth-fixed coordinates d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); @@ -281,5 +294,4 @@ double Gps_CNAV_Ephemeris::satellitePosition(double transmitTime) dtr_s -= 2.0 * sqrt(GM * a) * d_e_eccentricity * sin(E) / (GPS_L2_C_m_s * GPS_L2_C_m_s); return dtr_s; - } diff --git a/src/core/system_parameters/gps_cnav_ephemeris.h b/src/core/system_parameters/gps_cnav_ephemeris.h index dd0ea9a9b..df5d1c04f 100644 --- a/src/core/system_parameters/gps_cnav_ephemeris.h +++ b/src/core/system_parameters/gps_cnav_ephemeris.h @@ -32,6 +32,7 @@ #ifndef GNSS_SDR_GPS_CNAV_EPHEMERIS_H_ #define GNSS_SDR_GPS_CNAV_EPHEMERIS_H_ +#include "GPS_L2C.h" #include "boost/assign.hpp" #include @@ -153,9 +154,9 @@ public: archive & make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] archive & make_nvp("i_GPS_week", i_GPS_week); //!< GPS week number, aka WN [week] archive & make_nvp("d_TGD", d_TGD); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] - archive & make_nvp("d_DELTA_A", d_DELTA_A); - archive & make_nvp("d_A_DOT", d_A_DOT); - + archive & make_nvp("d_DELTA_A", d_DELTA_A); //!< Semi-major axis difference at reference time [m] + archive & make_nvp("d_A_DOT", d_A_DOT); //!< Change rate in semi-major axis [m/s] + archive & make_nvp("d_DELTA_OMEGA_DOT", d_DELTA_OMEGA_DOT); //!< Rate of Right Ascension difference [semi-circles/s] archive & make_nvp("d_A_f0", d_A_f0); //!< Coefficient 0 of code phase offset model [s] archive & make_nvp("d_A_f1", d_A_f1); //!< Coefficient 1 of code phase offset model [s/s] archive & make_nvp("d_A_f2", d_A_f2); //!< Coefficient 2 of code phase offset model [s/s^2] diff --git a/src/core/system_parameters/gps_ephemeris.cc b/src/core/system_parameters/gps_ephemeris.cc index 36ed12346..ac09ef955 100644 --- a/src/core/system_parameters/gps_ephemeris.cc +++ b/src/core/system_parameters/gps_ephemeris.cc @@ -117,14 +117,21 @@ double Gps_Ephemeris::check_t(double time) // 20.3.3.3.3.1 User Algorithm for SV Clock Correction. double Gps_Ephemeris::sv_clock_drift(double transmitTime) { +// double dt; +// dt = check_t(transmitTime - d_Toc); +// +// for (int i = 0; i < 2; i++) +// { +// dt -= d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); +// } +// d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); + + double dt; dt = check_t(transmitTime - d_Toc); - - for (int i = 0; i < 2; i++) - { - dt -= d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); - } - d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); + d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt) + sv_clock_relativistic_term(transmitTime); + //Correct satellite group delay + d_satClkDrift-=d_TGD; return d_satClkDrift; } @@ -156,7 +163,7 @@ double Gps_Ephemeris::sv_clock_relativistic_term(double transmitTime) M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi - M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI)); + //M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI)); // Initial guess of eccentric anomaly E = M; @@ -215,7 +222,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime) M = d_M_0 + n * tk; // Reduce mean anomaly to between 0 and 2pi - M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI)); + //M = fmod((M + 2.0 * GPS_PI), (2.0 * GPS_PI)); // Initial guess of eccentric anomaly E = M; @@ -242,7 +249,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime) phi = nu + d_OMEGA; // Reduce phi to between 0 and 2*pi rad - phi = fmod((phi), (2.0 * GPS_PI)); + //phi = fmod((phi), (2.0 * GPS_PI)); // Correct argument of latitude u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); @@ -257,7 +264,7 @@ double Gps_Ephemeris::satellitePosition(double transmitTime) Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT)*tk - OMEGA_EARTH_DOT * d_Toe; // Reduce to between 0 and 2*pi rad - Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI)); + //Omega = fmod((Omega + 2.0 * GPS_PI), (2.0 * GPS_PI)); // --- Compute satellite coordinates in Earth-fixed coordinates d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); diff --git a/src/core/system_parameters/gps_navigation_message.cc b/src/core/system_parameters/gps_navigation_message.cc index bc42aeecf..7a13109c1 100644 --- a/src/core/system_parameters/gps_navigation_message.cc +++ b/src/core/system_parameters/gps_navigation_message.cc @@ -262,141 +262,6 @@ signed long int Gps_Navigation_Message::read_navigation_signed(std::bitset half_week) - { - corrTime = time - 2 * half_week; - } - else if (time < -half_week) - { - corrTime = time + 2 * half_week; - } - return corrTime; -} - - - - -// 20.3.3.3.3.1 User Algorithm for SV Clock Correction. -double Gps_Navigation_Message::sv_clock_correction(double transmitTime) -{ - double dt; - dt = check_t(transmitTime - d_Toc); - d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr; - double correctedTime = transmitTime - d_satClkCorr; - return correctedTime; -} - - - - - -void Gps_Navigation_Message::satellitePosition(double transmitTime) -{ - double tk; - double a; - double n; - double n0; - double M; - double E; - double E_old; - double dE; - double nu; - double phi; - double u; - double r; - double i; - double Omega; - - // Find satellite's position ---------------------------------------------- - - // Restore semi-major axis - a = d_sqrt_A * d_sqrt_A; - - // Time from ephemeris reference epoch - tk = check_t(transmitTime - d_Toe); - - // Computed mean motion - n0 = sqrt(GM / (a * a * a)); - - // Corrected mean motion - n = n0 + d_Delta_n; - - // Mean anomaly - M = d_M_0 + n * tk; - - // Reduce mean anomaly to between 0 and 2pi - M = fmod((M + 2 * GPS_PI), (2 * GPS_PI)); - - // Initial guess of eccentric anomaly - E = M; - - // --- Iteratively compute eccentric anomaly ---------------------------- - for (int ii = 1; ii < 20; ii++) - { - E_old = E; - E = M + d_e_eccentricity * sin(E); - dE = fmod(E - E_old, 2 * GPS_PI); - if (fabs(dE) < 1e-12) - { - //Necessary precision is reached, exit from the loop - break; - } - } - - // Compute relativistic correction term - d_dtr = F * d_e_eccentricity * d_sqrt_A * sin(E); - - // Compute the true anomaly - double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); - double tmp_X = cos(E) - d_e_eccentricity; - nu = atan2(tmp_Y, tmp_X); - - // Compute angle phi (argument of Latitude) - phi = nu + d_OMEGA; - - // Reduce phi to between 0 and 2*pi rad - phi = fmod((phi), (2 * GPS_PI)); - - // Correct argument of latitude - u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi); - - // Correct radius - r = a * (1 - d_e_eccentricity * cos(E)) + d_Crc * cos(2 * phi) + d_Crs * sin(2 * phi); - - // Correct inclination - i = d_i_0 + d_IDOT * tk + d_Cic * cos(2 * phi) + d_Cis * sin(2 * phi); - - // Compute the angle between the ascending node and the Greenwich meridian - Omega = d_OMEGA0 + (d_OMEGA_DOT - OMEGA_EARTH_DOT) * tk - OMEGA_EARTH_DOT * d_Toe; - - // Reduce to between 0 and 2*pi rad - Omega = fmod((Omega + 2 * GPS_PI), (2 * GPS_PI)); - - // --- Compute satellite coordinates in Earth-fixed coordinates - d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); - d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); - d_satpos_Z = sin(u) * r * sin(i); - - // Satellite's velocity. Can be useful for Vector Tracking loops - double Omega_dot = d_OMEGA_DOT - OMEGA_EARTH_DOT; - d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); - d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); - d_satvel_Z = d_satpos_Y * sin(i); -} - - - - - int Gps_Navigation_Message::subframe_decoder(char *subframe) { int subframe_ID = 0; diff --git a/src/core/system_parameters/gps_navigation_message.h b/src/core/system_parameters/gps_navigation_message.h index aaded71a1..53482c905 100644 --- a/src/core/system_parameters/gps_navigation_message.h +++ b/src/core/system_parameters/gps_navigation_message.h @@ -58,15 +58,6 @@ private: signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); bool read_navigation_bool(std::bitset bits, const std::vector> parameter); void print_gps_word_bytes(unsigned int GPS_word); - /* - * Accounts for the beginning or end of week crossover - * - * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) - * \param[in] - time in seconds - * \param[out] - corrected time, in seconds - */ - double check_t(double time); - public: bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check //broadcast orbit 1 @@ -212,19 +203,6 @@ public: */ int subframe_decoder(char *subframe); - /*! - * \brief Computes the position of the satellite - * - * Implementation of Table 20-IV (IS-GPS-200E) - */ - void satellitePosition(double transmitTime); - - /*! - * \brief Sets (\a d_satClkCorr) according to the User Algorithm for SV Clock Correction - * and returns the corrected clock (IS-GPS-200E, 20.3.3.3.3.1) - */ - double sv_clock_correction(double transmitTime); - /*! * \brief Computes the Coordinated Universal Time (UTC) and * returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4) diff --git a/src/core/system_parameters/sbas_ionospheric_correction.cc b/src/core/system_parameters/sbas_ionospheric_correction.cc deleted file mode 100644 index 3243c32ed..000000000 --- a/src/core/system_parameters/sbas_ionospheric_correction.cc +++ /dev/null @@ -1,467 +0,0 @@ -/*! - * \file sbas_ionospheric_correction.cc - * \brief Implementation of the SBAS ionosphere correction set based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "sbas_ionospheric_correction.h" -#include -#include -#include - - -enum V_Log_Level {EVENT = 2, // logs important events which don't occur every update() call - FLOW = 3, // logs the function calls of block processing functions - MORE = 4}; // very detailed stuff - - -void Sbas_Ionosphere_Correction::print(std::ostream &out) -{ - for(std::vector::const_iterator it_band = d_bands.begin(); it_band != d_bands.end(); ++it_band) - { - int band = it_band - d_bands.begin(); - out << "<> Band" << band << ":" << std::endl; - for(std::vector::const_iterator it_igp = it_band->d_igps.begin(); it_igp != it_band->d_igps.end(); ++it_igp) - { - int igp = it_igp-it_band->d_igps.begin(); - out << "<> -IGP" << igp << ":"; - //std::cout << " valid=" << it_igp->d_valid; - out << " t0=" << it_igp->t0; - out << " lat=" << it_igp->d_latitude; - out << " lon=" << it_igp->d_longitude; - out << " give=" << it_igp->d_give; - out << " delay=" << it_igp->d_delay; - out << std::endl; - } - } -} - - -/* Applies SBAS ionosphric delay correction - * \param[out] delay Slant ionospheric delay (L1) (m) - * \param[out] var Variance of ionospheric delay (m^2) - * \param[in] sample_stamp Sample stamp of observable on which the correction will be applied - * \param[in] longitude_d Receiver's longitude in terms of WGS84 (degree) - * \param[in] latitude_d Receiver's latitude in terms of WGS84 (degree) - * \param[in] azimuth_d Satellite azimuth/elavation angle (rad). Azimuth is the angle of - * the satellite from the user�s location measured clockwise from north - * \param[in] elevation_d Elevation is the angle of the satellite from the user's location measured - * with respect to the local-tangent-plane - */ -bool Sbas_Ionosphere_Correction::apply(double sample_stamp, - double latitude_d, - double longitude_d, - double azimut_d, - double elevation_d, - double &delay, - double &var) -{ - const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E - int result; - double pos[3]; - double azel[2]; - - // convert receiver position from degrees to rad - pos[0] = latitude_d * GPS_PI / 180.0; - pos[1] = longitude_d * GPS_PI / 180.0; - pos[2] = 0; // is not used by sbsioncorr, for ionocorrection is a fixed earth radius assumed - - // convert satellite azimut and elevation from degrees to rad , use topocent to obtain it in pvt block - azel[0] = azimut_d * GPS_PI / 180.0; - azel[1] = elevation_d * GPS_PI / 180.0; - - result = sbsioncorr(sample_stamp, pos, azel, &delay, &var); - return (bool)result; -} - - - -/* geometric distance ---------------------------------------------------------- -* compute geometric distance and receiver-to-satellite unit vector -* args : double *rs I satellilte position (ecef at transmission) (m) -* double *rr I receiver position (ecef at reception) (m) -* double *e O line-of-sight vector (ecef) -* return : geometric distance (m) (0>:error/no satellite position) -* notes : distance includes sagnac effect correction -*-----------------------------------------------------------------------------*/ -//extern double geodist(const double *rs, const double *rr, double *e) -//{ -// double r; -// int i; -// -// if (norm(rs,3)= 0) c += a[n]*b[n]; - return c; -} - - - - -/* multiply matrix -----------------------------------------------------------*/ -void Sbas_Ionosphere_Correction::matmul(const char *tr, int n, int k, int m, double alpha, - const double *A, const double *B, double beta, double *C) -{ - double d; - int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4); - - for (i = 0; i < n; i++) for (j = 0; j < k; j++) - { - d = 0.0; - switch (f) - { - case 1: for (x = 0; x < m; x++) d += A[i + x*n]*B[x + j*m]; break; - case 2: for (x = 0; x < m; x++) d += A[i + x*n]*B[j + x*k]; break; - case 3: for (x = 0; x < m; x++) d += A[x + i*m]*B[x + j*m]; break; - case 4: for (x = 0; x < m; x++) d += A[x + i*m]*B[j + x*k]; break; - } - if (beta == 0.0) - { - C[i + j*n] = alpha*d; - } - else - { - C[i + j*n] = alpha*d + beta*C[i + j*n]; - } - } -} - - -/* ecef to local coordinate transfromation matrix ------------------------------ -* compute ecef to local coordinate transfromation matrix -* args : double *pos I geodetic position {lat,lon} (rad) -* double *E O ecef to local coord transformation matrix (3x3) -* return : none -* notes : matrix stored by column-major order (fortran convention) -*-----------------------------------------------------------------------------*/ -void Sbas_Ionosphere_Correction::xyz2enu(const double *pos, double *E) -{ - double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]); - E[0] = -sinl; E[3] = cosl; E[6] = 0.0; - E[1] = -sinp*cosl; E[4] = -sinp*sinl; E[7] = cosp; - E[2] = cosp*cosl; E[5] = cosp*sinl; E[8] = sinp; -} - - -/* transform ecef vector to local tangential coordinate ------------------------- -* transform ecef vector to local tangential coordinate -* args : double *pos I geodetic position {lat,lon} (rad) -* double *r I vector in ecef coordinate {x,y,z} -* double *e O vector in local tangental coordinate {e,n,u} -* return : none -*-----------------------------------------------------------------------------*/ -void Sbas_Ionosphere_Correction::ecef2enu(const double *pos, const double *r, double *e) -{ - double E[9]; - xyz2enu(pos, E); - matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e); -} - - - -const double PI = 3.1415926535897932; /* pi */ - - - -/* satellite azimuth/elevation angle ------------------------------------------- -* compute satellite azimuth/elevation angle -* args : double *pos I geodetic position {lat,lon,h} (rad,m) -* double *e I receiver-to-satellilte unit vevtor (ecef) -* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output) -* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2) -* return : elevation angle (rad) -*-----------------------------------------------------------------------------*/ -double Sbas_Ionosphere_Correction::satazel(const double *pos, const double *e, double *azel) -{ - const double RE_WGS84 = 6378137.0; /* earth semimajor axis (WGS84) (m) */ - - double az = 0.0, el = PI/2.0, enu[3]; - - if (pos[2] > -RE_WGS84) - { - ecef2enu(pos, e, enu); - az = dot(enu, enu, 2) < 1E-12 ? 0.0 : atan2(enu[0], enu[1]); - if (az < 0.0) az += 2*PI; - el = asin(enu[2]); - } - if (azel) - { - azel[0] = az; - azel[1] = el; - } - return el; -} - -/* debug trace function -----------------------------------------------------*/ -void Sbas_Ionosphere_Correction::trace(int level, const char *format, ...) -{ - va_list ap; - char str[1000]; - - va_start(ap,format); vsprintf(str,format,ap); va_end(ap); - VLOG(FLOW) << "<> " << std::string(str); -} - -/* ionospheric pierce point position ------------------------------------------- -* compute ionospheric pierce point (ipp) position and slant factor -* args : double *pos I receiver position {lat,lon,h} (rad,m) -* double *azel I azimuth/elevation angle {az,el} (rad) -* double re I earth radius (km) -* double hion I altitude of ionosphere (km) -* double *posp O pierce point position {lat,lon,h} (rad,m) -* return : slant factor -* notes : see ref [2], only valid on the earth surface -* fixing bug on ref [2] A.4.4.10.1 A-22,23 -*-----------------------------------------------------------------------------*/ -double Sbas_Ionosphere_Correction::ionppp(const double *pos, const double *azel, - double re, double hion, double *posp) -{ - double cosaz, rp, ap, sinap, tanap; - const double D2R = (PI/180.0); /* deg to rad */ - - rp = re/(re + hion)*cos(azel[1]); - ap = PI/2.0 - azel[1] - asin(rp); - sinap = sin(ap); - tanap = tan(ap); - cosaz = cos(azel[0]); - posp[0] = asin(sin(pos[0])*cos(ap) + cos(pos[0])*sinap*cosaz); - - if ((pos[0] > 70.0*D2R && tanap*cosaz > tan(PI/2.0 - pos[0])) || - (pos[0] < -70.0*D2R && - tanap*cosaz > tan(PI/2.0 + pos[0]))) - { - posp[1] = pos[1] + PI - asin(sinap*sin(azel[0])/cos(posp[0])); - } - else - { - posp[1] = pos[1] + asin(sinap*sin(azel[0])/cos(posp[0])); - } - return 1.0 / sqrt(1.0 - rp*rp); -} - - -/* variance of ionosphere correction (give=GIVEI) --------------------------*/ -double Sbas_Ionosphere_Correction::varicorr(int give) -{ - const double var[15]={ - 0.0084, 0.0333, 0.0749, 0.1331, 0.2079, 0.2994, 0.4075, 0.5322, 0.6735, 0.8315, - 1.1974, 1.8709, 3.326, 20.787, 187.0826 - }; - return 0 <= give && give < 15 ? var[give]:0.0; -} - - -/* search igps ---------------------------------------------------------------*/ -void Sbas_Ionosphere_Correction::searchigp(const double *pos, const Igp **igp, double *x, double *y) -{ - int i; - int latp[2]; - int lonp[4]; - const double R2D = (180.0/PI); /* rad to deg */ - - double lat = pos[0]*R2D; - double lon = pos[1]*R2D; - - trace(4,"searchigp: pos=%.3f %.3f",pos[0]*R2D, pos[1]*R2D); - - // round the pierce point position to the next IGP grid point - if (lon >= 180.0) lon -= 360.0; - if (-55.0 <= lat && lat < 55.0) - { - latp[0] = (int)floor(lat/5.0)*5; - latp[1] = latp[0] + 5; - lonp[0] = lonp[1] = (int)floor(lon/5.0)*5; - lonp[2] = lonp[3] = lonp[0] + 5; - *x = (lon - lonp[0])/5.0; - *y = (lat - latp[0])/5.0; - } - else - { - latp[0] = (int)floor((lat-5.0)/10.0)*10+5; - latp[1] = latp[0] + 10; - lonp[0] = lonp[1] = (int)floor(lon/10.0)*10; - lonp[2] = lonp[3] = lonp[0] + 10; - *x = (lon - lonp[0])/10.0; - *y = (lat - latp[0])/10.0; - if (75.0 <= lat && lat < 85.0) - { - lonp[1] = (int)floor(lon/90.0)*90; - lonp[3] = lonp[1] + 90; - } - else if (-85.0 <= lat && lat < -75.0) - { - lonp[0] = (int)floor((lon - 50.0)/90.0)*90 + 40; - lonp[2] = lonp[0] + 90; - } - else if (lat >= 85.0) - { - for (i = 0; i < 4; i++) lonp[i] = (int)floor(lon/90.0)*90; - } - else if (lat <- 85.0) - { - for (i = 0; i < 4; i++) lonp[i] = (int)floor((lon - 50.0)/90.0)*90 + 40; - } - } - - for (i = 0; i < 4; i++) if (lonp[i] == 180) lonp[i] = -180; - - // find the correction data for the grid points in latp[] and lonp[] - // iterate over bands - for (std::vector::const_iterator band_it = this->d_bands.begin(); band_it != d_bands.end(); ++band_it) - { - //VLOG(MORE) << "band=" << band_it-d_bands.begin() << std::endl; - // iterate over IGPs in band_it - for (std::vector::const_iterator igp_it = band_it->d_igps.begin(); igp_it != band_it->d_igps.end(); ++igp_it) - { - std::stringstream ss; - int give = igp_it->d_give; - ss << "IGP: give=" << give; - if(give < 15) // test if valid correction data is sent for current IGP - { - int lat = igp_it->d_latitude; - int lon = igp_it->d_longitude; - ss << " lat=" << lat << " lon=" << lon; - if (lat == latp[0] && lon == lonp[0]) igp[0] = igp_it.base(); - else if (lat == latp[1] && lon == lonp[1]) igp[1] = igp_it.base(); - else if (lat == latp[0] && lon == lonp[2]) igp[2] = igp_it.base(); - else if (lat == latp[1] && lon == lonp[3]) igp[3] = igp_it.base(); - } - //VLOG(MORE) << ss.str(); - } - } - //VLOG(MORE) << "igp[0:3]={" << igp[0] << "," << igp[1] << "," << igp[2] << "," << igp[3] << "}"; -} - - - -/* sbas ionospheric delay correction ------------------------------------------- -* compute sbas ionosphric delay correction -* args : long sample_stamp I sample stamp of observable on which the correction will be applied -* sbsion_t *ion I ionospheric correction data (implicit) -* double *pos I receiver position {lat,lon,height} (rad/m) -* double *azel I satellite azimuth/elavation angle (rad) -* double *delay O slant ionospheric delay (L1) (m) -* double *var O variance of ionospheric delay (m^2) -* return : status (1:ok, 0:no correction) -* notes : before calling the function, sbas ionosphere correction parameters -* in navigation data (nav->sbsion) must be set by calling -* sbsupdatecorr() -*-----------------------------------------------------------------------------*/ -int Sbas_Ionosphere_Correction::sbsioncorr(const double sample_stamp, const double *pos, - const double *azel, double *delay, double *var) -{ - const double re = 6378.1363; - const double hion = 350.0; - int err = 0; - double fp; - double posp[2]; - double x = 0.0; - double y = 0.0; - double t; - double w[4] = {0}; - const Igp *igp[4] = {0}; /* {ws,wn,es,en} */ - const double R2D = (180.0/PI); /* rad to deg */ - - trace(4, "sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f", pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D); - - *delay = *var = 0.0; - if (pos[2] < -100.0 || azel[1] <= 0) return 1; - - /* ipp (ionospheric pierce point) position */ - fp = ionppp(pos, azel, re, hion, posp); - - /* search igps around ipp */ - searchigp(posp, igp, &x, &y); - - VLOG(FLOW) << "<> SBAS iono correction:" << " igp[0]=" << igp[0] << " igp[1]=" << igp[1] - << " igp[2]=" << igp[2] << " igp[3]=" << igp[3] << " x=" << x << " y=" << y; - - /* weight of igps */ - if (igp[0] && igp[1] && igp[2] && igp[3]) - { - w[0] = (1.0 - x)*(1.0 - y); - w[1] = (1.0 - x)*y; - w[2] = x*(1.0 - y); - w[3] = x*y; - } - else if (igp[0] && igp[1] && igp[2]) - { - w[1] = y; - w[2] = x; - if ((w[0] = 1.0 - w[1] - w[2]) < 0.0) err = 1; - } - else if (igp[0] && igp[2] && igp[3]) - { - w[0] = 1.0 - x; - w[3] = y; - if ((w[2] = 1.0 - w[0] -w[3]) < 0.0) err = 1; - } - else if (igp[0] && igp[1] && igp[3]) - { - w[0] = 1.0 - y; - w[3] = x; - if ((w[1] = 1.0 - w[0] - w[3]) < 0.0) err = 1; - } - else if (igp[1]&&igp[2]&&igp[3]) - { - w[1] = 1.0 - x; - w[2] = 1.0 - y; - if ((w[3] = 1.0 - w[1] - w[2]) < 0.0) err = 1; - } - else err = 1; - - if (err) - { - trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f", posp[0]*R2D, posp[1]*R2D); - return 0; - } - for (int i = 0; i <4 ; i++) - { - if (!igp[i]) continue; - t = (sample_stamp - igp[i]->t0); // time diff between now and reception of the igp data in seconds - *delay += w[i]*igp[i]->d_delay; - *var += w[i] * varicorr(igp[i]->d_give) * 9E-8 * fabs(t); - } - *delay *= fp; - *var *= fp*fp; - - trace(5, "sbsioncorr: dion=%7.2f sig=%7.2f", *delay, sqrt(*var)); - return 1; -} diff --git a/src/core/system_parameters/sbas_ionospheric_correction.h b/src/core/system_parameters/sbas_ionospheric_correction.h deleted file mode 100644 index e2988fa18..000000000 --- a/src/core/system_parameters/sbas_ionospheric_correction.h +++ /dev/null @@ -1,203 +0,0 @@ -/*! - * \file sbas_ionospheric_correction.h - * \brief Interface of the SBAS ionosphere correction set based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef SBAS_IONOSPHERIC_CORRECTION_H_ -#define SBAS_IONOSPHERIC_CORRECTION_H_ - -#include -#include -#include -#include -#include -#include -#include -#include - - -/*! - * \brief Struct that represents a Ionospheric Grid Point (IGP) - */ -struct Igp -{ -public: - //bool d_valid; // valid==true indicates that the IGP can be used for corrections. it is set to false when a new IGP mask (MT18) has been received but no corresponding delays (MT26) - double t0; // time of reception, time of correction - int d_latitude; - int d_longitude; - int d_give; - double d_delay; -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version) - { - ar & t0; - ar & d_latitude; - ar & d_longitude; - ar & d_give; - ar & d_delay; - } -}; - - -/*! - * \brief Struct that represents the band of a Ionospheric Grid Point (IGP) - */ -struct Igp_Band -{ - //int d_iodi; - //int d_nigp; // number if IGPs in this band (defined by IGP mask from MT18) - std::vector d_igps; -private: - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version) - { - ar & d_igps; - } -}; - - - -/*! - * \brief Class that handles valid SBAS ionospheric correction for GPS - */ -class Sbas_Ionosphere_Correction -{ -private: - /* Inner product of vectors - * params : double *a,*b I vector a,b (n x 1) - * int n I size of vector a,b - * return : a'*b - */ - double dot(const double *a, const double *b, int n); - - /* Multiply matrix */ - void matmul(const char *tr, int n, int k, int m, double alpha, - const double *A, const double *B, double beta, double *C); - - /* EFEC to local coordinate transfomartion matrix - * Compute ecef to local coordinate transfomartion matrix - * params : double *pos I geodetic position {lat,lon} (rad) - * double *E O ecef to local coord transformation matrix (3x3) - * return : none - * notes : matrix stored by column-major order (fortran convention) - */ - void xyz2enu(const double *pos, double *E); - - /* Transforms ECEF vector into local tangential coordinates - * params : double *pos I geodetic position {lat,lon} (rad) - * double *r I vector in ecef coordinate {x,y,z} - * double *e O vector in local tangental coordinate {e,n,u} - * return : none - */ - void ecef2enu(const double *pos, const double *r, double *e); - - /* Compute satellite azimuth/elevation angle - * params : double *pos I geodetic position {lat,lon,h} (rad,m) - * double *e I receiver-to-satellilte unit vevtor (ecef) - * double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output) - * (0.0 <= azel[0] < 2*pi, -pi/2 <= azel[1] <= pi/2) - * return : elevation angle (rad) - */ - double satazel(const double *pos, const double *e, double *azel); - - /* Debug trace functions */ - void trace(int level, const char *format, ...); - - /* time difference ------------------------------------------------------------- - * difference between gtime_t structs - * args : gtime_t t1,t2 I gtime_t structs - * return : time difference (t1-t2) (s) - *-----------------------------------------------------------------------------*/ - //double timediff(gtime_t t1, gtime_t t2); - - /* Compute Ionospheric Pierce Point (IPP) position and slant factor - * params : double *pos I receiver position {lat,lon,h} (rad,m) - * double *azel I azimuth/elevation angle {az,el} (rad) - * double re I earth radius (km) - * double hion I altitude of ionosphere (km) - * double *posp O pierce point position {lat,lon,h} (rad,m) - * return : slant factor - * notes : see ref [2], only valid on the earth surface - * fixing bug on ref [2] A.4.4.10.1 A-22,23 - *-----------------------------------------------------------------------------*/ - double ionppp(const double *pos, const double *azel, double re, - double hion, double *posp); - - /* Variance of ionosphere correction (give = GIVEI + 1) */ - double varicorr(int give); - - /* Search igps */ - void searchigp(const double *pos, const Igp **igp, double *x, double *y); - - /* Compute sbas ionospheric delay correction - * params : long sample_stamp I sample stamp of observable on which the correction will be applied - * sbsion_t *ion I ionospheric correction data (implicit) - * double *pos I receiver position {lat,lon,height} (rad/m) - * double *azel I satellite azimuth/elavation angle (rad) - * double *delay O slant ionospheric delay (L1) (m) - * double *var O variance of ionospheric delay (m^2) - * return : status (1:ok, 0:no correction) - * notes : before calling the function, sbas ionosphere correction parameters - * in navigation data (nav->sbsion) must be set by callig - * sbsupdatecorr() - */ - int sbsioncorr(const double sample_stamp, const double *pos, - const double *azel, double *delay, double *var); - - friend class boost::serialization::access; - template - void serialize(Archive& ar, const unsigned int version){ar & d_bands;} - -public: - std::vector d_bands; - void print(std::ostream &out); - - /*! - * \brief Computes SBAS ionospheric delay correction. - * - * \param[out] delay Slant ionospheric delay (L1) (m) - * \param[out] var Variance of ionospheric delay (m^2) - * \param[in] sample_stamp Sample stamp of observable on which the correction will be applied - * \param[in] longitude_d Receiver's longitude in terms of WGS84 (degree) - * \param[in] latitude_d Receiver's latitude in terms of WGS84 (degree) - * \param[in] azimuth_d Satellite azimuth/elavation angle (rad). Azimuth is the angle of - * the satellite from the user's location measured clockwise from north - * \param[in] elevation_d Elevation is the angle of the satellite from the user's location measured - * with respect to the local-tangent-plane - */ - bool apply(double sample_stamp, double latitude_d, double longitude_d, - double azimut_d, double evaluation_d, double &delay, double &var); - -}; - - -#endif /* SBAS_IONOSPHERIC_CORRECTION_H_ */ diff --git a/src/core/system_parameters/sbas_satellite_correction.cc b/src/core/system_parameters/sbas_satellite_correction.cc deleted file mode 100644 index 106992364..000000000 --- a/src/core/system_parameters/sbas_satellite_correction.cc +++ /dev/null @@ -1,296 +0,0 @@ -/*! - * \file sbas_satellite_correction.cc - * \brief Implementation of the SBAS satellite correction set based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include "sbas_satellite_correction.h" -#include -#include -#include -#include -#include - - -#define EVENT 2 // logs important events which don't occur every update() call -#define FLOW 3 // logs the function calls of block processing functions - -#define CLIGHT 299792458.0 /* speed of light (m/s) */ -#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ -#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ - - -void Sbas_Satellite_Correction::print(std::ostream &out) -{ - out << "<> Sbas satellite corrections for PRN" << d_prn << ":" << std::endl; - print_fast_correction(out); - print_long_term_correction(out); -} - - -void Sbas_Satellite_Correction::print_fast_correction(std::ostream &out) -{ - Fast_Correction fcorr = d_fast_correction; - out << "<> Fast PRN" << d_prn << ":"; - if(fcorr.d_tof.is_related()) - { - int gps_week; - double gps_sec; - fcorr.d_tof.get_gps_time(gps_week, gps_sec); - out << " d_t0=(week=" << gps_week << ",sec=" << gps_sec << ")"; - } - else - { - out << " d_t0=" << fcorr.d_tof.get_time_stamp(); - } - out << " d_prc=" << fcorr.d_prc; - out << " d_rrc=" << fcorr.d_rrc; - out << " d_dt=" << fcorr.d_dt; - out << " d_udre=" << fcorr.d_udre; - out << " d_ai=" << fcorr.d_ai; - out << " d_tlat=" << fcorr.d_tlat; -} - - -void Sbas_Satellite_Correction::print_long_term_correction(std::ostream &out) -{ - Long_Term_Correction lcorr = d_long_term_correction; - out << "<> Long PRN" << d_prn << ":"; - out << " d_trx=" << lcorr.d_trx; - out << " i_tapp=" << lcorr.i_tapp; - out << " i_vel=" << lcorr.i_vel; - double *p = lcorr.d_dpos; - out << " d_dpos=(x=" << p[0] << ", y=" << p[1] << ", z=" << p[2] << ")" ; - double *v = lcorr.d_dvel; - out << " d_dvel=(x=" << v[0] << ", y=" << v[1] << ", z=" << v[2] << ")" ; - out << " d_daf0=" << lcorr.d_daf0; - out << " d_daf1=" << lcorr.d_daf1; -} - - -int Sbas_Satellite_Correction::apply_fast(double sample_stamp, double &pr, double &var) -{ - int result; - double prc = 0; // pseudo range correction - result = sbsfastcorr(sample_stamp, &prc, &var); - pr += prc; - VLOG(FLOW) << "<> fast correction applied: sample_stamp=" << sample_stamp << " prc=" << prc << " corr. pr=" << pr; - return result; -} - - - -int Sbas_Satellite_Correction::apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var) -{ - int result; - double drs[3] = {0}; - double ddts = 0; - result = sbslongcorr(sample_stamp, drs, &ddts); - for (int i = 0; i < 3; i++) sv_pos[i] += drs[i]; - VLOG(FLOW) << "<> long term sv pos correction applied: sample_stamp=" << sample_stamp << " drs=(x=" << drs[0] << " y=" << drs[1] << " z=" << drs[2] << ")"; - return result; -} - - - -int Sbas_Satellite_Correction::apply_long_term_sv_clk(double sample_stamp, double &dts, double &var) -{ - int result; - double drs[3] = {0}; - double ddts = 0; - result = sbslongcorr(sample_stamp, drs, &ddts); - dts += ddts; - VLOG(FLOW) << "<> long term sv clock correction correction applied: sample_stamp=" << sample_stamp << " ddts=" << ddts; - return result; -} - - -bool Sbas_Satellite_Correction::alarm() -{ - return this->d_fast_correction.d_udre == 16; -} - - - -/* debug trace function -----------------------------------------------------*/ -void Sbas_Satellite_Correction::trace(int level, const char *format, ...) -{ - va_list ap; - char str[1000]; - va_start(ap,format); - vsprintf(str,format,ap); - va_end(ap); - VLOG(FLOW) << "<> " << std::string(str); -} - - -/* variance of fast correction (udre=UDRE+1) ---------------------------------*/ -double Sbas_Satellite_Correction::varfcorr(int udre) -{ - const double var[14] = { - 0.052, 0.0924, 0.1444, 0.283, 0.4678, 0.8315, 1.2992, 1.8709, 2.5465, 3.326, - 5.1968, 20.7870, 230.9661, 2078.695 - }; - return 0 < udre && udre <= 14 ? var[udre - 1] : 0.0; -} - - -/* fast correction degradation -----------------------------------------------*/ -double Sbas_Satellite_Correction::degfcorr(int ai) -{ - const double degf[16] = { - 0.00000, 0.00005, 0.00009, 0.00012, 0.00015, 0.00020, 0.00030, 0.00045, - 0.00060, 0.00090, 0.00150, 0.00210, 0.00270, 0.00330, 0.00460, 0.00580 - }; - return 0 < ai && ai <= 15 ? degf[ai] : 0.0058; -} - - - -/* long term correction ------------------------------------------------------*/ -int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, double *ddts) -{ - double t = 0.0; - int i; - Long_Term_Correction lcorr = d_long_term_correction; - trace(3, "sbslongcorr: prn=%2d", this->d_prn); - // if (p->sat!=sat||p->lcorr.t0.time==0) continue; - // compute time of applicability - if(d_long_term_correction.i_vel == 1) - { - // time of applicability is the one sent, i.e., tapp - // TODO: adapt for vel==1 case - // t = tow-d_long_term_correction.i_tapp; - // vel=1 -> time of applicability is sent in message, needs to be corrected for rollover which can not be done here, since the absolute gps time is unknown. see IS-GPS-200G pdf page 116 for correction - /* t = (int)getbitu(msg->msg, p + 90, 13)*16 - (int)msg->tow%86400; - if (t <= -43200) t += 86400; - else if (t > 43200) t -= 86400; - sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/ - } - else - { - // time of applicability is time of reception - t = time_stamp - lcorr.d_trx; // should not have any impact if vel==0 since d_dvel and d_daf1 are zero, is only used to check outdating - } - //t=time_stamp-lcorr.d_t0; - if (fabs(t) > MAXSBSAGEL) - { - trace(2,"sbas long-term correction expired: sat=%2d time_stamp=%5.0f", d_prn, time_stamp); - return 0; - } - // sv position correction - for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*t; - // sv clock correction correction - *ddts = lcorr.d_daf0 + lcorr.d_daf1*t; - trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f", d_prn, drs[0], drs[1], drs[2], *ddts * CLIGHT); - return 1; - /* if sbas satellite without correction, no correction applied */ - //if (satsys(sat,NULL)==SYS_SBS) return 1; - //trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat); - //return 0; -} - - - - -/* fast correction -----------------------------------------------------------*/ -int Sbas_Satellite_Correction::sbsfastcorr(double time_stamp, double *prc, double *var) -#define RRCENA -{ - double t; - Fast_Correction fcorr = d_fast_correction; - trace(3, "sbsfastcorr: sat=%2d", this->d_prn); - //if (p->fcorr.t0.time==0) break; // time==0is only true if t0 hasn't been initialised -> it checks if the correction is valid - t = (time_stamp - fcorr.d_tof.get_time_stamp()) + fcorr.d_tlat; // delta t between now and tof - /* expire age of correction? */ - if (fabs(t) > MAXSBSAGEF) - { - trace(2, "no sbas fast correction (expired): time_stamp=%f prn=%2d", time_stamp, d_prn); - return 0; - } - /* UDRE==14 (not monitored)? */ - else if(fcorr.d_udre == 15) - { - trace(2,"no sbas fast correction (not monitored): time_stamp=%f prn=%2d", time_stamp, d_prn); - return 0; - } - else if(fcorr.d_udre == 16) - { - trace(2,"SV is marked as unhealthy: time_stamp=%f prn=%2d", time_stamp, d_prn); - return 0; - } - *prc = fcorr.d_prc; -#ifdef RRCENA - if (fcorr.d_ai > 0 && fabs(t) <= 8.0*fcorr.d_dt) - { - *prc += fcorr.d_rrc*t; - } -#endif - *var = varfcorr(fcorr.d_udre) + degfcorr(fcorr.d_ai) * t * t / 2.0; - trace(5, "sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f", d_prn, *prc, sqrt(*var), t); - return 1; -} - - - -/* sbas satellite ephemeris and clock correction ------------------------------- -* correct satellite position and clock bias with sbas satellite corrections -* args : long time_stamp I reception time stamp -* double *rs IO sat position and corrected {x,y,z} (ecef) (m) -* double *dts IO sat clock bias and corrected (s) -* double *var O sat position and clock variance (m^2) -* return : status (1:ok,0:no correction) -* notes : before calling the function, sbas satellite correction parameters -* in navigation data (nav->sbssat) must be set by callig -* sbsupdatecorr(). -* satellite clock correction include long-term correction and fast -* correction. -* sbas clock correction is usually based on L1C/A code. TGD or DCB has -* to be considered for other codes -*-----------------------------------------------------------------------------*/ -int Sbas_Satellite_Correction::sbssatcorr(double time_stamp, double *rs, double *dts, double *var) -{ - double drs[3] = {0}, dclk = 0.0, prc = 0.0; - int i; - trace(3,"sbssatcorr : sat=%2d",d_prn); - /* sbas long term corrections */ - if (!sbslongcorr(time_stamp, drs, &dclk)) - { - return 0; - } - /* sbas fast corrections */ - if (!sbsfastcorr(time_stamp, &prc, var)) - { - return 0; - } - for (i = 0; i < 3; i++) rs[i] += drs[i]; - dts[0] += dclk + prc/CLIGHT; - trace(5, "sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f", - d_prn, drs[0], drs[1], drs[2], dclk,prc/CLIGHT, *var); - return 1; -} - diff --git a/src/core/system_parameters/sbas_satellite_correction.h b/src/core/system_parameters/sbas_satellite_correction.h deleted file mode 100644 index 72c179594..000000000 --- a/src/core/system_parameters/sbas_satellite_correction.h +++ /dev/null @@ -1,107 +0,0 @@ -/*! - * \file sbas_satellite_correction.h - * \brief Interface of the SBAS satellite correction set based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_ -#define GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_ - -#include "sbas_time.h" - -struct Fast_Correction -{ - Sbas_Time d_tof; // for fast corrections the time of applicability (tof) is defined as the time when the corresponding message was send - double d_prc; - double d_rrc; - double d_dt; - int d_udre; // UDRE - int d_ai; - int d_tlat; -}; - - -struct Long_Term_Correction -{ - double d_trx; //!< Time when message was received - int i_tapp; //!< Time of applicability (only valid if vel=1, equals the sent t0) - int i_vel; //!< Use velocity corrections if vel=1 - int d_iode; - double d_dpos[3]; //!< position correction - double d_dvel[3]; //!< velocity correction - double d_daf0; //!< clock offset correction - double d_daf1; //!< clock drift correction -}; - - -/*! - * \brief Valid long and fast term SBAS corrections for one SV - */ -class Sbas_Satellite_Correction -{ -public: - int d_prn; - Fast_Correction d_fast_correction; - Long_Term_Correction d_long_term_correction; - void print(std::ostream &out); - void print_fast_correction(std::ostream &out); - void print_long_term_correction(std::ostream &out); - int apply_fast(double sample_stamp, double &pr, double &var); - int apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var); - int apply_long_term_sv_clk(double sample_stamp, double &dts, double &var); - bool alarm(); -private: - /* debug trace functions -----------------------------------------------------*/ - void trace(int level, const char *format, ...); - /* variance of fast correction (udre=UDRE+1) ---------------------------------*/ - double varfcorr(int udre); - /* fast correction degradation -----------------------------------------------*/ - double degfcorr(int ai); - /* long term correction ------------------------------------------------------*/ - int sbslongcorr(double time_stamp, double *drs, double *ddts); - /* fast correction -----------------------------------------------------------*/ - int sbsfastcorr(double time_stamp, double *prc, double *var); - /* sbas satellite ephemeris and clock correction ------------------------------- - * correct satellite position and clock bias with sbas satellite corrections - * args : long time_stamp I reception time stamp - * double *rs IO sat position and corrected {x,y,z} (ecef) (m) - * double *dts IO sat clock bias and corrected (s) - * double *var O sat position and clock variance (m^2) - * return : status (1:ok,0:no correction) - * notes : before calling the function, sbas satellite correction parameters - * in navigation data (nav->sbssat) must be set by callig - * sbsupdatecorr(). - * satellite clock correction include long-term correction and fast - * correction. - * sbas clock correction is usually based on L1C/A code. TGD or DCB has - * to be considered for other codes - *-----------------------------------------------------------------------------*/ - int sbssatcorr(double time_stamp, double *rs, double *dts, double *var); -}; - - -#endif /* GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_ */ diff --git a/src/core/system_parameters/sbas_telemetry_data.cc b/src/core/system_parameters/sbas_telemetry_data.cc deleted file mode 100644 index 599f03b55..000000000 --- a/src/core/system_parameters/sbas_telemetry_data.cc +++ /dev/null @@ -1,978 +0,0 @@ -/*! - * \file sbas_telemetry_data.cc - * \brief Implementation of the SBAS telemetry parser based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#include -#include -#include -#include -#include -#include -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" -#include "sbas_ephemeris.h" - - -// logging levels -#define EVENT 2 // logs important events which don't occur every update() call -#define FLOW 3 // logs the function calls of block processing functions -#define DETAIL 4 - - - -Sbas_Telemetry_Data::Sbas_Telemetry_Data() -{ - fp_trace = nullptr; // file pointer of trace - level_trace = 0; // level of trace - tick_trace = 0; // tick time at traceopen (ms) - - d_nav.sbssat.iodp = -1; // make sure that in any case iodp is not equal to the received one - prn_mask_changed(); // invalidate all satellite corrections - - for(size_t band = 0; band < sizeof(d_nav.sbsion)/sizeof(sbsion_t); band++) - { - d_nav.sbsion[band].iodi = -1; // make sure that in any case iodi is not equal to the received one - igp_mask_changed(band); - } -} - -Sbas_Telemetry_Data::~Sbas_Telemetry_Data() -{ - -} - - -int Sbas_Telemetry_Data::update(Sbas_Raw_Msg sbas_raw_msg) -{ - VLOG(FLOW) << "<> Sbas_Telemetry_Data.update():"; - int parsing_result; - - // if GPS time from MT12 is known (automatically handled by relate()): - // express the rx time in terms of GPS time - sbas_raw_msg.relate(mt12_time_ref); - - int mt = sbas_raw_msg.get_msg_type(); - // update internal state - if(mt == 12) parsing_result = decode_mt12(sbas_raw_msg); - //else if(mt == 9) parsing_result = parse_mt9(sbas_raw_msg); - else - { - // use RTKLIB to parse the message -> updates d_nav structure - sbsmsg_t sbas_raw_msg_rtklib; - std::vector msg_bytes = sbas_raw_msg.get_msg(); - // cast raw message to RTKLIB raw message struct - sbas_raw_msg_rtklib.prn = sbas_raw_msg.get_prn(); - //sbas_raw_msg_rtklib.tow = sbas_raw_msg.get_tow(); - //sbas_raw_msg_rtklib.week = sbas_raw_msg.get_week(); - sbas_raw_msg_rtklib.sample_stamp = sbas_raw_msg.get_sample_stamp(); - for (std::vector::const_iterator it = msg_bytes.begin(); it != msg_bytes.end() - 3; ++it) - { - int i = it - msg_bytes.begin(); - sbas_raw_msg_rtklib.msg[i] = *it; - } - parsing_result = sbsupdatecorr(&sbas_raw_msg_rtklib, &d_nav); - VLOG(FLOW) << "<> RTKLIB parsing result: " << parsing_result; - } - - // update gnss-sdr correction data sets from RTKLIB d_nav structure, emit SBAS data into queues - switch (parsing_result) - { - case -1: VLOG(FLOW) << "message parsing problem for MT" << sbas_raw_msg.get_msg_type(); break; - case 2: - case 3: - case 4: - case 5: - case 6: - case 7: - case 24: - case 25: updated_satellite_corrections(); break; - case 18: break; // new iono band mask received -> dont update iono corrections because delays are not - case 26: received_iono_correction(); break; - case 9: /*updated_sbas_ephemeris(sbas_raw_msg);*/ break; - - default: break; - } - - // send it to raw message queue - //TODO: update to new GNURadio msg queue - //if(raw_msg_queue != nullptr) raw_msg_queue->push(sbas_raw_msg); - return parsing_result; -} - - -unsigned int getbitu(const unsigned char *buff, int pos, int len); - - - -int getbits(const unsigned char *buff, int pos, int len); - - - -int Sbas_Telemetry_Data::decode_mt12(Sbas_Raw_Msg sbas_raw_msg) -{ - const double rx_delay = 38000.0/300000.0; // estimated sbas signal geosat to ground signal travel time - unsigned char * msg = sbas_raw_msg.get_msg().data(); - uint32_t gps_tow = getbitu(msg, 121, 20); - uint32_t gps_week = getbitu(msg, 141, 10) + 1024; // consider last gps time week overflow - double gps_tow_rx = double(gps_tow) + rx_delay; - mt12_time_ref = Sbas_Time_Relation(sbas_raw_msg.get_sample_stamp(), gps_week, gps_tow_rx); - VLOG(FLOW) << "<> extracted GPS time from MT12: gps_tow=" << gps_tow << " gps_week=" << gps_week; - return 12; -} - - - - -void Sbas_Telemetry_Data::updated_sbas_ephemeris(Sbas_Raw_Msg msg) -{ - VLOG(FLOW) << "<> updated_sbas_ephemeris():" << std::endl; - Sbas_Ephemeris seph; - int satidx = msg.get_prn() - MINPRNSBS; - seph_t seph_rtklib = d_nav.seph[satidx]; - // copy data - seph.i_prn = msg.get_prn(); - seph.i_t0 = seph_rtklib.t0; - seph.d_tof = seph_rtklib.tof; - seph.i_sv_ura = seph_rtklib.sva; - seph.b_sv_do_not_use = seph_rtklib.svh; - memcpy(seph.d_pos, seph_rtklib.pos, sizeof(seph.d_pos)); - memcpy(seph.d_vel, seph_rtklib.vel, sizeof(seph.d_vel)); - memcpy(seph.d_acc, seph_rtklib.acc, sizeof(seph.d_acc)); - seph.d_af0 = seph_rtklib.af0; - seph.d_af1 = seph_rtklib.af1; - // print ephemeris for debugging purposes - std::stringstream ss; - seph.print(ss); - VLOG(FLOW) << ss.str(); - - //todo_Update to new GNURadio msg queue - //if(ephemeris_queue != nullptr) ephemeris_queue->push(seph); -} - - - -void Sbas_Telemetry_Data::received_iono_correction() -{ - VLOG(FLOW) << "<> received_iono_correction():"; - std::stringstream ss; - - Sbas_Ionosphere_Correction iono_corr; - for (size_t i_band = 0; i_band < sizeof(d_nav.sbsion)/sizeof(sbsion_t); i_band++) - { - ss << "<> band=" << i_band - << " nigp=" << d_nav.sbsion[i_band].nigp << std::endl; - ss << "<> -> valid igps:"; - Igp_Band igp_band; - for (int i_igp = 0; i_igp < d_nav.sbsion[i_band].nigp; i_igp++) - { - if(d_nav.sbsion[i_band].igp[i_igp].valid) - { - // consider only valid IGPs, i.e, such ones which got updated at least once since instantiating sbas_telemtry_data - ss << " " << i_igp; - Igp igp; - igp.t0 = d_nav.sbsion[i_band].igp[i_igp].t0; - igp.d_latitude = d_nav.sbsion[i_band].igp[i_igp].lat; - igp.d_longitude = d_nav.sbsion[i_band].igp[i_igp].lon; - igp.d_give = d_nav.sbsion[i_band].igp[i_igp].give; - igp.d_delay = d_nav.sbsion[i_band].igp[i_igp].delay; - igp_band.d_igps.push_back(igp); - } - } - ss << std::endl; - iono_corr.d_bands.push_back(igp_band); - } - VLOG(DETAIL) << ss.str(); - ss.str(""); - iono_corr.print(ss); - VLOG(EVENT) << ss.str(); - - // send to SBAS ionospheric correction queue - //todo_Update to new GNURadio msg queue - //if(iono_queue != nullptr) iono_queue->push(iono_corr); -} - - -// helper for comparing two POD structures with undefined padding between members -// not guaranteed to work always properly -> don't use it for critical tasks -template -inline bool are_equal(const Struct &s1, const Struct &s2) -{ - const size_t struct_size = sizeof(Struct); - bool is_equal = true; - bool is_equal_manual = true; - std::stringstream ss; - - Struct *s1_; - Struct *s2_; - - // reserve zero initialised memory - s1_ = (Struct*) calloc (1, struct_size); - s2_ = (Struct*) calloc (1, struct_size); - - // use assignment constructor which doesn't copy paddings - *s1_ = s1; - *s2_ = s2; - - // compare struct memory byte-wise - is_equal_manual = true; - ss.str(); - ss << "\n<> compare objects of size=" << sizeof(Struct) << " (memcmp says is_equal=" << is_equal << ") :" << std::endl; - for (size_t i = 0; i < sizeof(Struct); ++i) - { - char byte1 = ((char *)s1_)[i]; - char byte2 = ((char *)s2_)[i]; - if(byte1 != byte2) is_equal_manual = false; - ss << "<> s1=" << std::hex << std::setw(4) << std::setfill('0'); - ss << (short)byte1; - ss << " s2=" << std::hex << std::setw(4) << std::setfill('0'); - ss << (short)byte2; - ss << " equal=" << is_equal_manual; - ss << std::endl; - } - - free(s1_); - free(s2_); - - return is_equal_manual; -} - - - -void Sbas_Telemetry_Data::updated_satellite_corrections() -{ - VLOG(FLOW) << "<> updated_satellite_corrections():"; - // for each satellite in the RTKLIB structure - for (int i_sat = 0; i_sat < d_nav.sbssat.nsat; i_sat++) - { - std::stringstream ss; - ss << "<> sat=" << d_nav.sbssat.sat[i_sat].sat - << " fastcorr.valid=" << d_nav.sbssat.sat[i_sat].fcorr.valid - << " lcorr.valid=" << d_nav.sbssat.sat[i_sat].lcorr.valid; - - if(is_rtklib_sat_correction_valid(i_sat)) // check if ever updated by a received message - { - int prn = d_nav.sbssat.sat[i_sat].sat; - - // get fast correction from RTKLIB structure - sbsfcorr_t fcorr_rtklib = d_nav.sbssat.sat[i_sat].fcorr; - Fast_Correction fcorr; - fcorr.d_tof = Sbas_Time(fcorr_rtklib.t0, mt12_time_ref); - fcorr.d_prc = fcorr_rtklib.prc; - fcorr.d_rrc = fcorr_rtklib.rrc; - fcorr.d_dt = fcorr_rtklib.dt; - fcorr.d_udre = fcorr_rtklib.udre; // UDRE - fcorr.d_ai = fcorr_rtklib.ai; - fcorr.d_tlat = d_nav.sbssat.tlat; - - // get long term correction from RTKLIB structure - sbslcorr_t lcorr_rtklib = d_nav.sbssat.sat[i_sat].lcorr; - Long_Term_Correction lcorr; - lcorr.d_trx = lcorr_rtklib.trx; - lcorr.i_tapp = lcorr_rtklib.tapp; - lcorr.i_vel = lcorr_rtklib.vel; - lcorr.d_iode = lcorr_rtklib.iode; - memcpy(lcorr.d_dpos, lcorr_rtklib.dpos, sizeof(lcorr.d_dpos)); - memcpy(lcorr.d_dvel, lcorr_rtklib.dvel, sizeof(lcorr.d_dvel)); - lcorr.d_daf0 = lcorr_rtklib.daf0; - lcorr.d_daf1= lcorr_rtklib.daf1; - - bool fast_correction_updated = false; - bool long_term_correction_updated = false; - - // check if fast corrections got updated - std::map::iterator it_old_fcorr = emitted_fast_corrections.find(prn); - if(it_old_fcorr == emitted_fast_corrections.end() || !are_equal < Fast_Correction>(fcorr, it_old_fcorr->second )) - { - // got updated - ss << " fast_correction_updated=" << true; - //ss << " not_found=" << (it_old_fcorr == emitted_fast_corrections.end()); - //ss << " not_equal=" << (!are_equal(fcorr, it_old_fcorr->second )); - fast_correction_updated = true; - emitted_fast_corrections[prn] = fcorr; - } - - // check if long term corrections got updated - std::map::iterator it_old_lcorr = emitted_long_term_corrections.find(prn); - if(it_old_lcorr == emitted_long_term_corrections.end() || !are_equal < Long_Term_Correction>(lcorr, it_old_lcorr->second )) - { - // got updated - ss << " long_term_correction_updated=" << true; - //ss << " not_found=" << (it_old_lcorr == emitted_long_term_corrections.end()); - //ss << " not_equal=" << (!are_equal(lcorr, it_old_lcorr->second )); - long_term_correction_updated = true; - emitted_long_term_corrections[prn] = lcorr; - } - - Sbas_Satellite_Correction sbas_satelite_correction; - sbas_satelite_correction.d_prn = prn; - sbas_satelite_correction.d_fast_correction = fcorr; - sbas_satelite_correction.d_long_term_correction = lcorr; - - if(fast_correction_updated) - { - ss << std::endl; - sbas_satelite_correction.print_fast_correction(ss << " "); - } - - if(long_term_correction_updated) - { - ss << std::endl; - sbas_satelite_correction.print_long_term_correction(ss << " "); - } - - if(fast_correction_updated || long_term_correction_updated) - { - //todo_Update to new GNURadio msg queue - //if(sat_corr_queue != nullptr) sat_corr_queue->push(sbas_satelite_correction); - } - } - VLOG(FLOW) << ss.str(); ss.str(""); - } -} - - - -const double Sbas_Telemetry_Data::gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */ - -/* debug trace function -----------------------------------------------------*/ -void Sbas_Telemetry_Data::trace(int level, const char *format, ...) -{ - va_list ap; - char str[1000]; - va_start(ap, format); - vsprintf(str, format, ap); - va_end(ap); - VLOG(FLOW) << "<> " << std::string(str); -} - - - -/* satellite system+prn/slot number to satellite number ------------------------ - * convert satellite system+prn/slot number to satellite number - * args : int sys I satellite system (SYS_GPS,SYS_GLO,...) - * int prn I satellite prn/slot number - * return : satellite number (0:error) - *-----------------------------------------------------------------------------*/ -int Sbas_Telemetry_Data::satno(int sys, int prn) -{ - if (prn <= 0) return 0; - switch (sys) - { - case SYS_GPS: - if (prn < MINPRNGPS || MAXPRNGPS < prn) return 0; - return prn - MINPRNGPS + 1; - case SYS_GLO: - if (prn < MINPRNGLO || MAXPRNGLO < prn) return 0; - return NSATGPS + prn - MINPRNGLO + 1; - case SYS_GAL: - if (prn < MINPRNGAL || MAXPRNGAL < prn) return 0; - return NSATGPS + NSATGLO + prn - MINPRNGAL + 1; - case SYS_QZS: - if (prn < MINPRNQZS || MAXPRNQZS < prn) return 0; - return NSATGPS + NSATGLO + NSATGAL + prn - MINPRNQZS + 1; - case SYS_CMP: - if (prn < MINPRNCMP || MAXPRNCMP < prn) return 0; - return NSATGPS + NSATGLO + NSATGAL + NSATQZS + prn - MINPRNCMP + 1; - case SYS_SBS: - if (prn < MINPRNSBS || MAXPRNSBS < prn) return 0; - return NSATGPS + NSATGLO + NSATGAL + NSATQZS + NSATCMP + prn - MINPRNSBS + 1; - } - return 0; -} - -/*! - * \brief Extracts unsigned/signed bits from byte data - * params : unsigned char *buff I byte data - * int pos I bit position from start of data (bits) - * int len I bit length (bits) (len<=32) - * return : extracted unsigned/signed bits - */ -unsigned int Sbas_Telemetry_Data::getbitu(const unsigned char *buff, int pos, int len) -{ - unsigned int bits = 0; - int i; - for (i = pos; i < pos + len; i++) bits = (bits << 1) + ((buff[i/8] >> (7 - i % 8)) & 1u); - return bits; -} - - - -int Sbas_Telemetry_Data::getbits(const unsigned char *buff, int pos, int len) -{ - unsigned int bits = getbitu(buff,pos,len); - if (len <= 0 || 32 <= len || !(bits & (1u << (len - 1)))) return (int)bits; - return (int)(bits|(~0u << len)); /* extend sign */ -} - - - -/* convert calendar day/time to time ------------------------------------------- - * convert calendar day/time to gtime_t struct - * args : double *ep I day/time {year,month,day,hour,min,sec} - * return : gtime_t struct - * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) - *-----------------------------------------------------------------------------*/ -Sbas_Telemetry_Data::gtime_t Sbas_Telemetry_Data::epoch2time(const double *ep) -{ - const int doy[] = {1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}; - gtime_t time = gtime_t(); - int days, sec, year = (int)ep[0], mon = (int)ep[1], day = (int)ep[2]; - - if (year < 1970 || 2099 < year || mon < 1 || 12 < mon) return time; - - /* leap year if year%4==0 in 1901-2099 */ - days = (year - 1970)*365 + (year - 1969)/4 + doy[mon - 1] + day - 2 + (year % 4 == 0 && mon >= 3 ? 1 : 0); - sec = (int)floor(ep[5]); - time.time = (time_t)days*86400 + (int)ep[3]*3600 + (int)ep[4]*60 + sec; - time.sec = ep[5] - sec; - return time; -} - - - - -/* time difference ------------------------------------------------------------- - * difference between gtime_t structs - * args : gtime_t t1,t2 I gtime_t structs - * return : time difference (t1-t2) (s) - *-----------------------------------------------------------------------------*/ -double Sbas_Telemetry_Data::timediff(gtime_t t1, gtime_t t2) -{ - return difftime(t1.time, t2.time) + t1.sec - t2.sec; -} - - - -/* gps time to time ------------------------------------------------------------ - * convert week and tow in gps time to gtime_t struct - * args : int week I week number in gps time - * double sec I time of week in gps time (s) - * return : gtime_t struct - *-----------------------------------------------------------------------------*/ -Sbas_Telemetry_Data::gtime_t Sbas_Telemetry_Data::gpst2time(int week, double sec) -{ - gtime_t t = epoch2time(gpst0); - if (sec < -1E9 || 1E9 < sec) sec = 0.0; - t.time += 86400*7*week + (int)sec; - t.sec = sec - (int)sec; - return t; -} - - - -/* sbas igp definition -------------------------------------------------------*/ -const short -Sbas_Telemetry_Data::x1[] = {-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20, - 25, 30, 35, 40, 45, 50, 55, 65, 75, 85}, -Sbas_Telemetry_Data::x2[] = {-55,-50,-45,-40,-35,-30,-25,-20,-15,-10, -5, 0, 5, 10, 15, 20, 25, 30, - 35, 40, 45, 50, 55}, -Sbas_Telemetry_Data::x3[] = {-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, 20, - 25, 30, 35, 40, 45, 50, 55, 65, 75}, -Sbas_Telemetry_Data::x4[] = {-85,-75,-65,-55,-50,-45,-40,-35,-30,-25,-20,-15,-10,- 5, 0, 5, 10, 15, - 20, 25, 30, 35, 40, 45, 50, 55, 65, 75}, -Sbas_Telemetry_Data::x5[] = {-180,-175,-170,-165,-160,-155,-150,-145,-140,-135,-130,-125,-120,-115, - -110,-105,-100,- 95,- 90,- 85,- 80,- 75,- 70,- 65,- 60,- 55,- 50,- 45, - - 40,- 35,- 30,- 25,- 20,- 15,- 10,- 5, 0, 5, 10, 15, 20, 25, - 30, 35, 40, 45, 50, 55, 60, 65, 70, 75, 80, 85, 90, 95, - 100, 105, 110, 115, 120, 125, 130, 135, 140, 145, 150, 155, 160, 165, - 170, 175}, -Sbas_Telemetry_Data::x6[] = {-180,-170,-160,-150,-140,-130,-120,-110,-100,- 90,- 80,- 70,- 60,- 50, - -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, - 100, 110, 120, 130, 140, 150, 160, 170}, -Sbas_Telemetry_Data::x7[] = {-180,-150,-120,- 90,- 60,- 30, 0, 30, 60, 90, 120, 150}, -Sbas_Telemetry_Data::x8[] = {-170,-140,-110,- 80,- 50,- 20, 10, 40, 70, 100, 130, 160}; - -const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband1[9][8] = { /* band 0-8 */ - {{-180,x1, 1, 28},{-175,x2, 29, 51},{-170,x3, 52, 78},{-165,x2, 79,101}, - {-160,x3,102,128},{-155,x2,129,151},{-150,x3,152,178},{-145,x2,179,201}}, - {{-140,x4, 1, 28},{-135,x2, 29, 51},{-130,x3, 52, 78},{-125,x2, 79,101}, - {-120,x3,102,128},{-115,x2,129,151},{-110,x3,152,178},{-105,x2,179,201}}, - {{-100,x3, 1, 27},{- 95,x2, 28, 50},{- 90,x1, 51, 78},{- 85,x2, 79,101}, - {- 80,x3,102,128},{- 75,x2,129,151},{- 70,x3,152,178},{- 65,x2,179,201}}, - {{- 60,x3, 1, 27},{- 55,x2, 28, 50},{- 50,x4, 51, 78},{- 45,x2, 79,101}, - {- 40,x3,102,128},{- 35,x2,129,151},{- 30,x3,152,178},{- 25,x2,179,201}}, - {{- 20,x3, 1, 27},{- 15,x2, 28, 50},{- 10,x3, 51, 77},{- 5,x2, 78,100}, - { 0,x1,101,128},{ 5,x2,129,151},{ 10,x3,152,178},{ 15,x2,179,201}}, - {{ 20,x3, 1, 27},{ 25,x2, 28, 50},{ 30,x3, 51, 77},{ 35,x2, 78,100}, - { 40,x4,101,128},{ 45,x2,129,151},{ 50,x3,152,178},{ 55,x2,179,201}}, - {{ 60,x3, 1, 27},{ 65,x2, 28, 50},{ 70,x3, 51, 77},{ 75,x2, 78,100}, - { 80,x3,101,127},{ 85,x2,128,150},{ 90,x1,151,178},{ 95,x2,179,201}}, - {{ 100,x3, 1, 27},{ 105,x2, 28, 50},{ 110,x3, 51, 77},{ 115,x2, 78,100}, - { 120,x3,101,127},{ 125,x2,128,150},{ 130,x4,151,178},{ 135,x2,179,201}}, - {{ 140,x3, 1, 27},{ 145,x2, 28, 50},{ 150,x3, 51, 77},{ 155,x2, 78,100}, - { 160,x3,101,127},{ 165,x2,128,150},{ 170,x3,151,177},{ 175,x2,178,200}} -}; - - - -const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband2[2][5] = { /* band 9-10 */ - {{ 60,x5, 1, 72},{ 65,x6, 73,108},{ 70,x6,109,144},{ 75,x6,145,180}, - { 85,x7,181,192}}, - {{- 60,x5, 1, 72},{- 65,x6, 73,108},{- 70,x6,109,144},{- 75,x6,145,180}, - {- 85,x8,181,192}} -}; - - -/* decode type 1: prn masks --------------------------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i, n, sat; - // see figure A-6: i corresponds to bit number (and for the GPS satellites is identically to the PRN), n to the PRN mask number - - trace(4, "decode_sbstype1:"); - - for (i = 1, n = 0; i <= 210 && n < MAXSAT; i++) - { - if (getbitu(msg->msg, 13 + i, 1)) - { - if (i <= 37) sat = satno(SYS_GPS, i); /* 0 - 37: gps */ - else if (i <= 61) sat = satno(SYS_GLO, i - 37); /* 38 - 61: glonass */ - else if (i <= 119) sat = 0; /* 62 - 119: future gnss */ - else if (i <= 138) sat = satno(SYS_SBS, i); /* 120 - 138: geo/waas */ - else if (i <= 182) sat = 0; /* 139 - 182: reserved */ - else if (i <= 192) sat = satno(SYS_SBS, i + 10); /* 183 - 192: qzss ref [2] */ - else if (i <= 202) sat = satno(SYS_QZS, i); /* 193 - 202: qzss ref [2] */ - else sat = 0; /* 203 - : reserved */ - sbssat->sat[n++].sat = sat; - } - } - // TODO consider the use of the old prn mask in the transition phase such that old data sets still can be used - int new_iodp = getbitu(msg->msg, 224, 2); - if (sbssat->iodp != new_iodp) prn_mask_changed(); // invalidate all satellite corrections - sbssat->iodp = new_iodp; - sbssat->nsat = n; - - trace(5, "decode_sbstype1: nprn=%d iodp=%d", n, sbssat->iodp); - return 1; -} - - -/* decode type 2-5,0: fast corrections ---------------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i, j, iodf, type, udrei; - double prc, dt; - double t0_past; - - trace(4,"decode_sbstype2:"); - - if (sbssat->iodp != (int)getbitu(msg->msg, 16, 2)) return 0; - - type = getbitu(msg->msg, 8, 6); - iodf = getbitu(msg->msg, 14, 2); - - for (i=0; i<13; i++) - { - if ((j = 13*((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat) break; - udrei = getbitu(msg->msg, 174 + 4*i, 4); - t0_past = sbssat->sat[j].fcorr.t0; - prc = sbssat->sat[j].fcorr.prc; - sbssat->sat[j].fcorr.t0 = msg->sample_stamp; - sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i*12, 12)*0.125f; - sbssat->sat[j].fcorr.udre = udrei + 1; - dt = sbssat->sat[j].fcorr.t0 - t0_past; - if (!sbssat->sat[j].fcorr.valid || dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0) - { - sbssat->sat[j].fcorr.rrc = 0.0; - sbssat->sat[j].fcorr.dt = 0.0; - } - else - { - sbssat->sat[j].fcorr.rrc = (sbssat->sat[j].fcorr.prc - prc)/dt; - sbssat->sat[j].fcorr.dt = dt; - } - sbssat->sat[j].fcorr.valid = true; - sbssat->sat[j].fcorr.iodf = iodf; - } - trace(5, "decode_sbstype2: type=%d iodf=%d", type, iodf); - return 1; -} - - - - -/* decode type 6: integrity info ---------------------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i, iodf[4], udrei; - - trace(4, "decode_sbstype6:"); - - if(sbssat->iodp < 0) return 0; - - for (i=0; i<4; i++) - { - iodf[i] = getbitu(msg->msg, 14 + i*2, 2); - } - for (i=0; i < sbssat->nsat && i < MAXSAT; i++) - { - if (!sbssat->sat[i].fcorr.valid || sbssat->sat[i].fcorr.iodf != iodf[i/13]) continue; - udrei = getbitu(msg->msg, 22 + i*4, 4); - sbssat->sat[i].fcorr.udre = udrei + 1; - } - trace(5, "decode_sbstype6: iodf=%d %d %d %d", iodf[0], iodf[1], iodf[2], iodf[3]); - return 1; -} - - - -/* decode type 7: fast correction degradation factor -------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i; - trace(4,"decode_sbstype7"); - if (sbssat->iodp != (int)getbitu(msg->msg, 18, 2)) return 0; - sbssat->tlat = getbitu(msg->msg, 14, 4); - for (i=0; i < sbssat->nsat && i < MAXSAT; i++) - { - sbssat->sat[i].fcorr.ai = getbitu(msg->msg, 22 + i*4, 4); - } - return 1; -} - - - -/* decode type 9: geo navigation message -------------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype9(const sbsmsg_t *msg, nav_t *nav) -{ - seph_t seph; - int i; - int sat; - - trace(4,"decode_sbstype9:"); - - if (!(sat = satno(SYS_SBS, msg->prn))) - { - trace(2, "invalid prn in sbas type 9: prn=%3d", msg->prn); - return 0; - } - /*t=(int)getbitu(msg->msg,22,13)*16-(int)msg->tow%86400; - if (t<=-43200) t+=86400; - else if (t> 43200) t-=86400;*/ - //seph.t0 =gpst2time(msg->week,msg->tow+t); - seph.sat = sat; - seph.t0 = getbitu(msg->msg, 22, 13)*16; - seph.tof = msg->sample_stamp; - seph.sva = getbitu(msg->msg, 35, 4); - seph.svh = seph.sva == 15 ? 1 : 0; /* unhealthy if ura==15 */ - - seph.pos[0] = getbits(msg->msg, 39, 30)*0.08; - seph.pos[1] = getbits(msg->msg, 69, 30)*0.08; - seph.pos[2] = getbits(msg->msg, 99, 25)*0.4; - seph.vel[0] = getbits(msg->msg, 124, 17)*0.000625; - seph.vel[1] = getbits(msg->msg, 141, 17)*0.000625; - seph.vel[2] = getbits(msg->msg, 158, 18)*0.004; - seph.acc[0] = getbits(msg->msg, 176, 10)*0.0000125; - seph.acc[1] = getbits(msg->msg, 186, 10)*0.0000125; - seph.acc[2] = getbits(msg->msg, 196, 10)*0.0000625; - - seph.af0 = getbits(msg->msg, 206, 12)*P2_31; - seph.af1 = getbits(msg->msg, 218, 8)*P2_39/2.0; - - i = msg->prn-MINPRNSBS; - if (std::abs(nav->seph[i].t0 - seph.t0) < 1E-3) - { /* not change */ - VLOG(FLOW) << "<> no change in ephemeris -> won't parse"; - return 0; - } - nav->seph[NSATSBS + i] = nav->seph[i]; /* previous */ - nav->seph[i] = seph; /* current */ - - trace(5, "decode_sbstype9: prn=%d", msg->prn); - return 1; -} - - - -/* decode type 18: ionospheric grid point masks ------------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion) -{ - const sbsigpband_t *p; - int i, j, n, m, band = getbitu(msg->msg, 18, 4); - - trace(4, "decode_sbstype18:"); - - if (0 <= band && band <= 8) {p = igpband1[band]; m = 8;} - else if (9 <= band && band <= 10) {p = igpband2[band - 9]; m = 5;} - else return 0; - - short iodi_new = (short)getbitu(msg->msg, 22, 2); - if(sbsion[band].iodi != iodi_new) - { - // IGP mask changed -> invalidate all IGPs in this band - igp_mask_changed(band); - } - sbsion[band].iodi = iodi_new; - - for (i=1, n=0; i <= 201; i++) - { - if (!getbitu(msg->msg, 23 + i, 1)) continue; - for (j = 0; j < m; j++) - { - if (i < p[j].bits || p[j].bite < i) continue; - sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x; - sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits]; - break; - } - } - sbsion[band].nigp = n; - - trace(5, "decode_sbstype18: band=%d nigp=%d", band, n); - return 1; -} - - - - -/* decode half long term correction (vel code=0) -----------------------------*/ -int Sbas_Telemetry_Data::decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat) -{ - int i, n = getbitu(msg->msg, p, 6); - - trace(4, "decode_longcorr0:"); - - if (n == 0 || n > MAXSAT) return 0; - - sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8); - - for (i = 0; i < 3; i++) - { - sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + 9*i, 9)*0.125; - sbssat->sat[n - 1].lcorr.dvel[i] = 0.0; - } - sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 41, 10)*P2_31; - sbssat->sat[n - 1].lcorr.daf1 = 0.0; - //sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow); - sbssat->sat[n - 1].lcorr.trx = msg->sample_stamp; // vel=0 -> time of applicability is reception time - sbssat->sat[n - 1].lcorr.tapp = 0; - sbssat->sat[n - 1].lcorr.valid = true; - - trace(5, "decode_longcorr0:sat=%2d", sbssat->sat[n - 1].sat); - return 1; -} - - - -/* decode half long term correction (vel code=1) -----------------------------*/ -int Sbas_Telemetry_Data::decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat) -{ - int i; - int n = getbitu(msg->msg, p, 6); - - trace(4,"decode_longcorr1:"); - - if (n == 0 || n > MAXSAT) return 0; - - sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8); - - for (i=0; i<3; i++) - { - sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + i*11, 11)*0.125; - sbssat->sat[n - 1].lcorr.dvel[i] = getbits(msg->msg, p + 58 + i*8, 8)*P2_11; - } - sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 47, 11)*P2_31; - sbssat->sat[n - 1].lcorr.daf1 = getbits(msg->msg, p + 82, 8)*P2_39; - // vel=1 -> time of applicability is sent in message, needs to be corrected for rollover which can not be done here, since the absolute gps time is unknown. see IS-GPS-200G pdf page 116 for correction - /*t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400; - if (t<=-43200) t+=86400; - else if (t> 43200) t-=86400; - sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t);*/ - sbssat->sat[n - 1].lcorr.trx = msg->sample_stamp; - sbssat->sat[n - 1].lcorr.tapp = (int)getbitu(msg->msg, p + 90, 13)*16; - sbssat->sat[n - 1].lcorr.vel = 1; - sbssat->sat[n - 1].lcorr.valid = true; - trace(5, "decode_longcorr1: sat=%2d", sbssat->sat[n - 1].sat); - return 1; -} - - - - -/* decode half long term correction ------------------------------------------*/ -int Sbas_Telemetry_Data::decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat) -{ - trace(4,"decode_longcorrh:"); - - if (getbitu(msg->msg, p, 1) == 0 ) - { - /* vel code=0 */ - if (sbssat->iodp == (int)getbitu(msg->msg, p + 103, 2)) - { - return decode_longcorr0(msg, p + 1, sbssat) && decode_longcorr0(msg, p + 52, sbssat); - } - } - else if (sbssat->iodp == (int)getbitu(msg->msg, p + 104, 2)) - { - return decode_longcorr1(msg, p + 1, sbssat); - } - return 0; -} - - - - -/* decode type 24: mixed fast/long term correction ---------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - int i, j, iodf, blk, udre; - - trace(4, "decode_sbstype24:"); - - if (sbssat->iodp != (int)getbitu(msg->msg, 110, 2)) return 0; /* check IODP */ - - blk = getbitu(msg->msg, 112, 2); - iodf =getbitu(msg->msg, 114, 2); - - for (i=0; i<6; i++) - { - if ((j = 13*blk+i) >= sbssat->nsat) break; - udre = getbitu(msg->msg, 86 + 4*i, 4); - - //sbssat->sat[j].fcorr.t0 =gpst2time(msg->week,msg->tow); - sbssat->sat[j].fcorr.t0 = msg->sample_stamp; - sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14 + i*12, 12)*0.125f; - sbssat->sat[j].fcorr.udre = udre + 1; - sbssat->sat[j].fcorr.iodf = iodf; - } - return decode_longcorrh(msg, 120, sbssat); -} - - - - -/* decode type 25: long term satellite error correction ----------------------*/ -int Sbas_Telemetry_Data::decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat) -{ - trace(4,"decode_sbstype25:"); - return decode_longcorrh(msg, 14, sbssat) && decode_longcorrh(msg, 120, sbssat); -} - - - - -/* decode type 26: ionospheric delay corrections -----------------------------*/ -int Sbas_Telemetry_Data::decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion) -{ - int i, j, block, delay, give, band = getbitu(msg->msg, 14, 4); - - trace(4, "decode_sbstype26:"); - - if (band > MAXBAND || sbsion[band].iodi != (int)getbitu(msg->msg, 217, 2)) return 0; - - block = getbitu(msg->msg, 18, 4); - - for (i = 0; i < 15; i++) - { - if ((j = block*15 + i) >= sbsion[band].nigp) continue; - give = getbitu(msg->msg, 2 + i*13 + 9, 4); - delay = getbitu(msg->msg, 22 + i*13, 9); - //sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow); - sbsion[band].igp[j].t0 = msg->sample_stamp; - sbsion[band].igp[j].valid = true; - sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0f : delay*0.125f; - sbsion[band].igp[j].give = give; - - if(sbsion[band].igp[j].give > 15) sbsion[band].igp[j].give = 15; // give is not higher than 15, but to be sure - } - trace(5, "decode_sbstype26: band=%d block=%d", band, block); - return 1; -} - - - - -/* update sbas corrections ----------------------------------------------------- - * update sbas correction parameters in navigation data with a sbas message - * args : sbsmg_t *msg I sbas message - * nav_t *nav IO navigation data - * return : message type (-1: error or not supported type) - * notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) - * seph[prn-MINPRNSBS+1] : sat prn current epehmeris - * seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris - *-----------------------------------------------------------------------------*/ -int Sbas_Telemetry_Data::sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav) -{ - int type = getbitu(msg->msg, 8, 6), stat = -1; - - trace(3,"sbsupdatecorr: type=%d",type); - - //if (msg->week==0) return -1; - - switch (type) - { - case 0: stat = decode_sbstype2(msg, &nav->sbssat); break; - case 1: stat = decode_sbstype1(msg, &nav->sbssat); break; - case 2: - case 3: - case 4: - case 5: stat = decode_sbstype2(msg, &nav->sbssat); break; - case 6: stat = decode_sbstype6(msg, &nav->sbssat); break; - case 7: stat = decode_sbstype7(msg, &nav->sbssat); break; - case 9: stat = decode_sbstype9(msg, nav); break; - case 10: trace(2, "unsupported sbas message: type=%d", type); break; - case 12: trace(2, "unsupported sbas message: type=%d", type); break; - case 17: trace(2, "unsupported sbas message: type=%d", type); break; - case 18: stat = decode_sbstype18(msg, nav ->sbsion); break; - case 24: stat = decode_sbstype24(msg, &nav->sbssat); break; - case 25: stat = decode_sbstype25(msg, &nav->sbssat); break; - case 26: stat = decode_sbstype26(msg, nav ->sbsion); break; - case 63: break; /* null message */ - - default: trace(2, "Unsupported SBAS message: type=%d", type); break; - } - return stat ? type : -1; -} - - -void Sbas_Telemetry_Data::prn_mask_changed() -{ - d_nav.sbssat.tlat = -1; - // for each satellite in the RTKLIB structure - for (int i_sat = 0; i_sat < d_nav.sbssat.nsat; i_sat++) - { - d_nav.sbssat.sat[i_sat].fcorr.valid = false; - d_nav.sbssat.sat[i_sat].lcorr.valid = false; - } -} - - -bool Sbas_Telemetry_Data::is_rtklib_sat_correction_valid(int sat) -{ - return d_nav.sbssat.tlat > -1 - && d_nav.sbssat.sat[sat].fcorr.valid - && d_nav.sbssat.sat[sat].lcorr.valid; -} - - -void Sbas_Telemetry_Data::igp_mask_changed(int band) -{ - for(int i_igp = 0; i_igp < d_nav.sbsion[band].nigp; i_igp++) - d_nav.sbsion[band].igp[i_igp].valid = false; -} diff --git a/src/core/system_parameters/sbas_telemetry_data.h b/src/core/system_parameters/sbas_telemetry_data.h deleted file mode 100644 index 9f75dee07..000000000 --- a/src/core/system_parameters/sbas_telemetry_data.h +++ /dev/null @@ -1,492 +0,0 @@ -/*! - * \file sbas_telemetry_data.h - * \brief Interface of the SBAS telemetry parser based on SBAS RTKLIB functions - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_SBAS_TELEMETRY_DATA_H_ -#define GNSS_SDR_SBAS_TELEMETRY_DATA_H_ - -#include -#include -#include -#include -#include -#include -#include -#include "boost/assign.hpp" -#include "concurrent_queue.h" -#include "sbas_time.h" - - -class Sbas_Ionosphere_Correction; -class Sbas_Satellite_Correction; -struct Fast_Correction; -struct Long_Term_Correction; -class Sbas_Ephemeris; - - -/*! - * \brief Represents a raw SBAS message of 250cbits + 6 bits padding - * (8b preamble + 6b message type + 212b data + 24b CRC + 6b zero padding) - */ -class Sbas_Raw_Msg -{ -public: - Sbas_Raw_Msg(){ rx_time = Sbas_Time(0); i_prn = -1; }; - Sbas_Raw_Msg(double sample_stamp, int prn, const std::vector msg) : rx_time(sample_stamp), i_prn(prn), d_msg(msg) {} - double get_sample_stamp() { return rx_time.get_time_stamp(); } //!< Time of reception sample stamp (first sample of preample) - void relate(Sbas_Time_Relation time_relation) - { - rx_time.relate(time_relation); - } - Sbas_Time get_rx_time_obj() const { return rx_time; } - int get_prn() const { return i_prn; } - std::vector get_msg() const { return d_msg; } - int get_preamble() - { - return d_msg[0]; - } - int get_msg_type() const - { - return d_msg[1] >> 2; - } - int get_crc() - { - unsigned char crc_last_byte = (d_msg[30] << 2) && (d_msg[31] >> 6); - unsigned char crc_middle_byte = (d_msg[29] << 2) && (d_msg[30] >> 6); - unsigned char crc_first_byte = (d_msg[28] << 2) && (d_msg[29] >> 6); - return ((unsigned int)(crc_first_byte) << 16) && ((unsigned int)(crc_middle_byte) << 8) && crc_last_byte; - } -private: - Sbas_Time rx_time; - int i_prn; /* SBAS satellite PRN number */ - std::vector d_msg; /* SBAS message (226 bit) padded by 0 */ -}; - - - - -/* - * \brief Holds an updated set of the telemetry data received from SBAS - */ -class Sbas_Telemetry_Data -{ -public: - int update(Sbas_Raw_Msg sbas_raw_msg); - - /*! - * Default constructor - */ - Sbas_Telemetry_Data(); - /*! - * Default deconstructor - */ - ~Sbas_Telemetry_Data(); - -private: - std::map emitted_fast_corrections; - std::map emitted_long_term_corrections; - - Sbas_Time_Relation mt12_time_ref; - - int decode_mt12(Sbas_Raw_Msg sbas_raw_msg); - - void updated_sbas_ephemeris(Sbas_Raw_Msg msg); - void received_iono_correction(); - void updated_satellite_corrections(); - - /////// rtklib.h - -#define SYS_NONE 0x00 /* navigation system: none */ -#define SYS_GPS 0x01 /* navigation system: GPS */ -#define SYS_SBS 0x02 /* navigation system: SBAS */ -#define SYS_GLO 0x04 /* navigation system: GLONASS */ -#define SYS_GAL 0x08 /* navigation system: Galileo */ -#define SYS_QZS 0x10 /* navigation system: QZSS */ -#define SYS_CMP 0x20 /* navigation system: BeiDou */ -#define SYS_ALL 0xFF /* navigation system: all */ - -#define TSYS_GPS 0 /* time system: GPS time */ -#define TSYS_UTC 1 /* time system: UTC */ -#define TSYS_GLO 2 /* time system: GLONASS time */ -#define TSYS_GAL 3 /* time system: Galileo time */ -#define TSYS_QZS 4 /* time system: QZSS time */ -#define TSYS_CMP 5 /* time system: BeiDou time */ - -#ifndef NFREQ -#define NFREQ 3 /* number of carrier frequencies */ -#endif -#define NFREQGLO 2 /* number of carrier frequencies of GLONASS */ - -#ifndef NEXOBS -#define NEXOBS 0 /* number of extended obs codes */ -#endif - -#define MINPRNGPS 1 /* min satellite PRN number of GPS */ -#define MAXPRNGPS 32 /* max satellite PRN number of GPS */ -#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */ -#define NSYSGPS 1 - -#ifdef ENAGLO -#define MINPRNGLO 1 /* min satellite slot number of GLONASS */ -#define MAXPRNGLO 24 /* max satellite slot number of GLONASS */ -#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */ -#define NSYSGLO 1 -#else -#define MINPRNGLO 0 -#define MAXPRNGLO 0 -#define NSATGLO 0 -#define NSYSGLO 0 -#endif -#ifdef ENAGAL -#define MINPRNGAL 1 /* min satellite PRN number of Galileo */ -#define MAXPRNGAL 27 /* max satellite PRN number of Galileo */ -#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */ -#define NSYSGAL 1 -#else -#define MINPRNGAL 0 -#define MAXPRNGAL 0 -#define NSATGAL 0 -#define NSYSGAL 0 -#endif -#ifdef ENAQZS -#define MINPRNQZS 193 /* min satellite PRN number of QZSS */ -#define MAXPRNQZS 195 /* max satellite PRN number of QZSS */ -#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */ -#define MAXPRNQZS_S 185 /* max satellite PRN number of QZSS SAIF */ -#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */ -#define NSYSQZS 1 -#else -#define MINPRNQZS 0 -#define MAXPRNQZS 0 -#define NSATQZS 0 -#define NSYSQZS 0 -#endif -#ifdef ENACMP -#define MINPRNCMP 1 /* min satellite sat number of BeiDou */ -#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */ -#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */ -#define NSYSCMP 1 -#else -#define MINPRNCMP 0 -#define MAXPRNCMP 0 -#define NSATCMP 0 -#define NSYSCMP 0 -#endif -#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP) /* number of systems */ - -#define MINPRNSBS 120 /* min satellite PRN number of SBAS */ -#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */ -#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */ - -#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATSBS) - /* max satellite number (1 to MAXSAT) */ -#ifndef MAXOBS -#define MAXOBS 64 /* max number of obs in an epoch */ -#endif -#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */ -#define MAXOBSTYPE 64 /* max number of obs type in RINEX */ -#define DTTOL 0.005 /* tolerance of time difference (s) */ -#if 0 -#define MAXDTOE 10800.0 /* max time difference to ephem Toe (s) for GPS */ -#else -#define MAXDTOE 7200.0 /* max time difference to ephem Toe (s) for GPS */ -#endif -#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */ -#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */ -#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */ -#define MAXGDOP 300.0 /* max GDOP */ - - //#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ - //#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ - //#define MAXSBSURA 8 /* max URA of SBAS satellite */ -#define MAXBAND 10 /* max SBAS band of IGP */ -#define MAXNIGP 201 /* max number of IGP in SBAS band */ - //#define MAXNGEO 4 /* max number of GEO satellites */ - - -#define P2_11 4.882812500000000E-04 /* 2^-11 */ -#define P2_31 4.656612873077393E-10 /* 2^-31 */ -#define P2_39 1.818989403545856E-12 /* 2^-39 */ - - /* type definitions ----------------------------------------------------------*/ - - typedef struct { /* time struct */ - time_t time; /* time (s) expressed by standard time_t */ - double sec; /* fraction of second under 1 s */ - } gtime_t; - - typedef struct { /* SBAS message type */ - //int week,tow; /* receiption time */ - double sample_stamp; - int prn; /* SBAS satellite PRN number */ - unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */ - } sbsmsg_t; - - typedef struct { /* SBAS messages type */ - int n,nmax; /* number of SBAS messages/allocated */ - sbsmsg_t *msgs; /* SBAS messages */ - } sbs_t; - - typedef struct { /* SBAS fast correction type */ - //gtime_t t0; /* time of applicability (TOF) */ - double t0; - bool valid; - double prc; /* pseudorange correction (PRC) (m) */ - double rrc; /* range-rate correction (RRC) (m/s) */ - double dt; /* range-rate correction delta-time (s) */ - int iodf; /* IODF (issue of date fast corr) */ - short udre; /* UDRE+1 */ - short ai; /* degradation factor indicator */ - } sbsfcorr_t; - - typedef struct { /* SBAS long term satellite error correction type */ - //gtime_t t0; /* correction time */ - double trx; /* time when message was received */ - int tapp; /* time of applicability (when vel=1 sent as t0) */ - int vel; /* use velocity if vel=1 */ - bool valid; - int iode; /* IODE (issue of date ephemeris) */ - double dpos[3]; /* delta position (m) (ecef) */ - double dvel[3]; /* delta velocity (m/s) (ecef) */ - double daf0,daf1; /* delta clock-offset/drift (s,s/s) */ - } sbslcorr_t; - - typedef struct { /* SBAS satellite correction type */ - int sat; /* satellite number */ - sbsfcorr_t fcorr; /* fast correction */ - sbslcorr_t lcorr; /* long term correction */ - } sbssatp_t; - - typedef struct { /* SBAS satellite corrections type */ - int iodp; /* IODP (issue of date mask) */ - int nsat; /* number of satellites */ - int tlat; /* system latency (s) */ - sbssatp_t sat[MAXSAT]; /* satellite correction */ - } sbssat_t; - - typedef struct { /* SBAS ionospheric correction type */ - //gtime_t t0; /* correction time */ - double t0; - bool valid; - short lat,lon; /* latitude/longitude (deg) */ - short give; /* GIVI+1 */ - float delay; /* vertical delay estimate (m) */ - } sbsigp_t; - - typedef struct { /* IGP band type */ - short x; /* longitude/latitude (deg) */ - const short *y; /* latitudes/longitudes (deg) */ - unsigned char bits; /* IGP mask start bit */ - unsigned char bite; /* IGP mask end bit */ - } sbsigpband_t; - - typedef struct { /* SBAS ionospheric corrections type */ - int iodi; /* IODI (issue of date ionos corr) */ - int nigp; /* number of igps */ - sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ - } sbsion_t; - - /* - * indicators - */ - - typedef struct { /* SBAS ephemeris type */ - int sat = 0; /* satellite number */ - //gtime_t t0; /* reference epoch time (GPST) */ - int t0 = 0; - //gtime_t tof; /* time of message frame (GPST) */ - double tof = 0; - int sva = 0; /* SV accuracy (URA index) */ - int svh = 0; /* SV health (0:ok) */ - double pos[3] = {0, 0, 0}; /* satellite position (m) (ecef) */ - double vel[3] = {0, 0, 0}; /* satellite velocity (m/s) (ecef) */ - double acc[3] = {0, 0, 0}; /* satellite acceleration (m/s^2) (ecef) */ - double af0 = 0; - double af1 = 0; /* satellite clock-offset/drift (s,s/s) */ - } seph_t; - - typedef struct { /* navigation data type */ - //int n,nmax; /* number of broadcast ephemeris */ - //int ng,ngmax; /* number of glonass ephemeris */ - //int ns,nsmax; /* number of sbas ephemeris */ - //int ne,nemax; /* number of precise ephemeris */ - //int nc,ncmax; /* number of precise clock */ - //int na,namax; /* number of almanac data */ - //int nt,ntmax; /* number of tec grid data */ - //int nn,nnmax; /* number of stec grid data */ - //eph_t *eph; /* GPS/QZS/GAL ephemeris */ - //geph_t *geph; /* GLONASS ephemeris */ - seph_t seph[2*NSATSBS]; /* SBAS ephemeris */ - // peph_t *peph; /* precise ephemeris */ - // pclk_t *pclk; /* precise clock */ - // alm_t *alm; /* almanac data */ - // tec_t *tec; /* tec grid data */ - // stec_t *stec; /* stec grid data */ - // erp_t erp; /* earth rotation parameters */ - - //double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */ - //double utc_glo[4]; /* GLONASS UTC GPS time parameters */ - //double utc_gal[4]; /* Galileo UTC GPS time parameters */ - //double utc_qzs[4]; /* QZS UTC GPS time parameters */ - //double utc_cmp[4]; /* BeiDou UTC parameters */ - //double utc_sbs[4]; /* SBAS UTC parameters */ - //double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ - //double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */ - //double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ - //double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */ - //int leaps; /* leap seconds (s) */ - //double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */ - //double cbias[MAXSAT][3]; /* code bias (0:p1-p2,1:p1-c1,2:p2-c2) (m) */ - //double wlbias[MAXSAT]; /* wide-lane bias (cycle) */ - //double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */ - //char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */ - // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ - sbssat_t sbssat; /* SBAS satellite corrections */ - sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */ - // dgps_t dgps[MAXSAT]; /* DGPS corrections */ - // ssr_t ssr[MAXSAT]; /* SSR corrections */ - // lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */ - // lexion_t lexion; /* LEX ionosphere correction */ - } nav_t; - - //// common - - static const double gpst0[]; /* gps time reference */ - - /* debug trace functions -----------------------------------------------------*/ - - FILE *fp_trace; /* file pointer of trace */ - int level_trace; /* level of trace */ - unsigned int tick_trace; /* tick time at traceopen (ms) */ - - void trace(int level, const char *format, ...); - - /* satellite system+prn/slot number to satellite number ------------------------ - * convert satellite system+prn/slot number to satellite number - * args : int sys I satellite system (SYS_GPS,SYS_GLO,...) - * int prn I satellite prn/slot number - * return : satellite number (0:error) - *-----------------------------------------------------------------------------*/ - int satno(int sys, int prn); - - /* extract unsigned/signed bits ------------------------------------------------ - * extract unsigned/signed bits from byte data - * args : unsigned char *buff I byte data - * int pos I bit position from start of data (bits) - * int len I bit length (bits) (len<=32) - * return : extracted unsigned/signed bits - *-----------------------------------------------------------------------------*/ - unsigned int getbitu(const unsigned char *buff, int pos, int len); - - int getbits(const unsigned char *buff, int pos, int len); - - /* convert calendar day/time to time ------------------------------------------- - * convert calendar day/time to gtime_t struct - * args : double *ep I day/time {year,month,day,hour,min,sec} - * return : gtime_t struct - * notes : proper in 1970-2037 or 1970-2099 (64bit time_t) - *-----------------------------------------------------------------------------*/ - gtime_t epoch2time(const double *ep); - - /* time difference ------------------------------------------------------------- - * difference between gtime_t structs - * args : gtime_t t1,t2 I gtime_t structs - * return : time difference (t1-t2) (s) - *-----------------------------------------------------------------------------*/ - double timediff(gtime_t t1, gtime_t t2); - - /* gps time to time ------------------------------------------------------------ - * convert week and tow in gps time to gtime_t struct - * args : int week I week number in gps time - * double sec I time of week in gps time (s) - * return : gtime_t struct - *-----------------------------------------------------------------------------*/ - gtime_t gpst2time(int week, double sec); - - - ////// sbas.c - - /* sbas igp definition -------------------------------------------------------*/ - static const short - x1[], - x2[], - x3[], - x4[], - x5[], - x6[], - x7[], - x8[]; - - static const sbsigpband_t igpband1[9][8]; /* band 0-8 */ - static const sbsigpband_t igpband2[2][5]; /* band 9-10 */ - - /* decode type 1: prn masks --------------------------------------------------*/ - int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 2-5,0: fast corrections ---------------------------------------*/ - int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 6: integrity info ---------------------------------------------*/ - int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 7: fast correction degradation factor -------------------------*/ - int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 9: geo navigation message -------------------------------------*/ - int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav); - /* decode type 18: ionospheric grid point masks ------------------------------*/ - int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion); - /* decode half long term correction (vel code=0) -----------------------------*/ - int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat); - /* decode half long term correction (vel code=1) -----------------------------*/ - int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat); - /* decode half long term correction ------------------------------------------*/ - int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat); - /* decode type 24: mixed fast/long term correction ---------------------------*/ - int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 25: long term satellite error correction ----------------------*/ - int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat); - /* decode type 26: ionospheric deley corrections -----------------------------*/ - int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion); - /* update sbas corrections ----------------------------------------------------- - * update sbas correction parameters in navigation data with a sbas message - * args : sbsmg_t *msg I sbas message - * nav_t *nav IO navigation data - * return : message type (-1: error or not supported type) - * notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) - * seph[prn-MINPRNSBS+1] : sat prn current epehmeris - * seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris - *-----------------------------------------------------------------------------*/ - int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav); - - void prn_mask_changed(); - bool is_rtklib_sat_correction_valid(int sat); - void igp_mask_changed(int band); - - // RTKLIB SBAS telemetry data representation - nav_t d_nav; -}; - -#endif diff --git a/src/core/system_parameters/sbas_time.h b/src/core/system_parameters/sbas_time.h deleted file mode 100644 index 3c86efa11..000000000 --- a/src/core/system_parameters/sbas_time.h +++ /dev/null @@ -1,193 +0,0 @@ -/*! - * \file sbas_time.h - * \brief Interface and implementation of classes to handle and relate sample stamp and GPS time based time scales - * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch - * - * ------------------------------------------------------------------------- - * - * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) - * - * GNSS-SDR is a software defined Global Navigation - * Satellite Systems receiver - * - * This file is part of GNSS-SDR. - * - * GNSS-SDR is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * GNSS-SDR is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with GNSS-SDR. If not, see . - * - * ------------------------------------------------------------------------- - */ - -#ifndef GNSS_SDR_SBAS_TIME_H_ -#define GNSS_SDR_SBAS_TIME_H_ - -#include -#include -#include - -#define EVENT 2 // logs important events which don't occur every update() call -#define FLOW 3 // logs the function calls of block processing functions - -class Sbas_Time_Relation -{ -public: - Sbas_Time_Relation() - { - i_gps_week = 0; - d_delta_sec = 0; - b_valid = false; - VLOG(FLOW) << "<> new invalid time relation: i_gps_week=" << i_gps_week << " d_delta_sec=" << d_delta_sec; - } - - Sbas_Time_Relation(double time_stamp_sec, int gps_week, double gps_sec) - { - i_gps_week = gps_week; - d_delta_sec = gps_sec - time_stamp_sec; - b_valid = true; - VLOG(FLOW) << "<> new time relation: i_gps_week=" << i_gps_week << " d_delta_sec=" << d_delta_sec; - } - - bool to_gps_time(double time_stamp_sec, int &gps_week, double &gps_sec) - { - int delta_weeks = int(trunc(time_stamp_sec + d_delta_sec))/604800; - gps_sec = time_stamp_sec + d_delta_sec - delta_weeks*604800; - gps_week = i_gps_week + delta_weeks; - VLOG(FLOW) << "<> to gps time: time_stamp_sec=" << time_stamp_sec << " gps_week=" << gps_week << " gps_sec=" << gps_sec; - return b_valid; - } - - bool to_sample_stamp(int gps_week, double gps_sec, double &time_stamp_sec) - { - time_stamp_sec = (gps_sec - d_delta_sec) + (gps_week - i_gps_week)*604800; - VLOG(FLOW) << "<> to gps time: gps_week=" << gps_week << " gps_sec=" << gps_sec << " time_stamp_sec=" << time_stamp_sec; - return b_valid; - } - - bool is_valid() - { - return b_valid; - } - -private: - int i_gps_week; - double d_delta_sec; - bool b_valid; -}; - - -/*! - * \brief Sbas_Time relates the relative sample stamp time scale with the absolute GPS time scale. - * There are three different states for a Sbas_Time object: - * - only relative time (sample stamp) is known - * - only absolute time (gps time) is known - * - absolute and relative time and their relation is known - */ -class Sbas_Time -{ -public: - enum Sbas_Time_State {RELATIVE, /*ABSOLUTE,*/ RELATED, UNDEFINED}; - - Sbas_Time() - { - e_state = UNDEFINED; - i_gps_week = 0; - d_gps_sec = 0; - d_time_stamp_sec = 0; - } - - Sbas_Time(double time_stamp_sec) - { - d_time_stamp_sec = time_stamp_sec; - i_gps_week = 0; - d_gps_sec = 0; - e_state = RELATIVE; - } - /* - Sbas_Time(int gps_week, double gps_sec) - { - i_gps_week = gps_week; - d_gps_sec = gps_sec; - d_time_stamp_sec = 0; - e_state = ABSOLUTE; - }*/ - Sbas_Time(double time_stamp_sec, Sbas_Time_Relation relation) - { - if(relation.is_valid()) - { // construct a RELATED object - d_time_stamp_sec = time_stamp_sec; - relation.to_gps_time(d_time_stamp_sec, i_gps_week, d_gps_sec); - e_state = RELATED; - } - else - { // construct a RELATIVE object - *this = Sbas_Time(time_stamp_sec); - VLOG(FLOW) << "<> create RELATIVE time (invalid relation): time_stamp_sec=" << time_stamp_sec; - } - } - /*Sbas_Time(int gps_week, double gps_sec, Sbas_Time_Relation relation) - { - i_gps_week = gps_week; - d_gps_sec = gps_sec; - relation.to_sample_stamp(gps_week, gps_sec, d_time_stamp_sec); - e_state = RELATED; - }*/ - - void relate(Sbas_Time_Relation sbas_time_realtion) - { - switch (e_state) - { - case RELATIVE: *this = Sbas_Time(d_time_stamp_sec, sbas_time_realtion); - break; - - //case ABSOLUTE: return Sbas_Time(i_gps_week, d_gps_sec, sbas_time_realtion); - break; - - case RELATED: LOG(INFO) << "Relating an already related Sbas_Time object is not possible"; - break; - - case UNDEFINED: std::cerr << "Sbas_Time object state undefined" << std::endl; - break; - - default: std::cerr << "Sbas_Time object state not known" << std::endl; - break; - } - return; - } - - double get_time_stamp() - { - return d_time_stamp_sec; - //return (e_state == RELATIVE || e_state == RELATED); - } - - bool get_gps_time(int &gps_week, double &gps_sec) - { - gps_week = i_gps_week; - gps_sec = d_gps_sec; - return (/*e_state == ABSOLUTE ||*/ e_state == RELATED); - } - - bool is_only_relativ() { return e_state == RELATIVE; } - //bool is_only_absolute() {return e_state == ABSOLUTE;} - bool is_related() { return e_state == RELATED; } - Sbas_Time_State get_state() { return e_state; } - -private: - Sbas_Time_State e_state; - double d_time_stamp_sec; - int i_gps_week; - double d_gps_sec; -}; - - -#endif /* GNSS_SDR_SBAS_TIME_H_ */ diff --git a/src/tests/CMakeLists.txt b/src/tests/CMakeLists.txt index ce1f45783..d48c6eda0 100644 --- a/src/tests/CMakeLists.txt +++ b/src/tests/CMakeLists.txt @@ -260,6 +260,8 @@ include_directories( ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/telemetry_decoder/libs + ${CMAKE_SOURCE_DIR}/src/algorithms/observables/adapters + ${CMAKE_SOURCE_DIR}/src/algorithms/observables/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/signal_source/adapters ${CMAKE_SOURCE_DIR}/src/algorithms/signal_source/gnuradio_blocks ${CMAKE_SOURCE_DIR}/src/algorithms/signal_generator/adapters diff --git a/src/tests/common-files/signal_generator_flags.h b/src/tests/common-files/signal_generator_flags.h index e35c62742..8ee56cf1c 100644 --- a/src/tests/common-files/signal_generator_flags.h +++ b/src/tests/common-files/signal_generator_flags.h @@ -41,8 +41,8 @@ DEFINE_string(static_position, "30.286502,120.032669,100", "Static receiver posi DEFINE_string(dynamic_position, "", "Observer positions file, in .csv or .nmea format"); DEFINE_string(filename_rinex_obs, "sim.16o", "Filename of output RINEX navigation file"); DEFINE_string(filename_raw_data, "signal_out.bin", "Filename of output raw data file"); -DEFINE_int32(fs_gen_hz, 2600000, "Samppling frequency [Hz]"); +DEFINE_int32(fs_gen_hz, 2600000, "Sampling frequency [sps]"); DEFINE_int32(test_satellite_PRN, 1, "PRN of the satellite under test (must be visible during the observation time)"); - +DEFINE_int32(test_satellite_PRN2, 2, "PRN of the satellite under test (must be visible during the observation time)"); #endif diff --git a/src/tests/single_test_main.cc b/src/tests/single_test_main.cc index 12b44d943..f9f91573b 100644 --- a/src/tests/single_test_main.cc +++ b/src/tests/single_test_main.cc @@ -48,10 +48,10 @@ #include "gps_ref_location.h" #include "gps_ref_time.h" #include "galileo_navigation_message.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_telemetry_data.h" -#include "sbas_ephemeris.h" -#include "sbas_satellite_correction.h" +//#include "sbas_ionospheric_correction.h" +//#include "sbas_telemetry_data.h" +//#include "sbas_ephemeris.h" +//#include "sbas_satellite_correction.h" concurrent_queue global_gps_acq_assist_queue; diff --git a/src/tests/system-tests/obs_gps_l1_system_test.cc b/src/tests/system-tests/obs_gps_l1_system_test.cc index 5a19f45d5..5a63924f3 100644 --- a/src/tests/system-tests/obs_gps_l1_system_test.cc +++ b/src/tests/system-tests/obs_gps_l1_system_test.cc @@ -206,8 +206,7 @@ int Obs_Gps_L1_System_Test::configure_receiver() const int extend_correlation_ms = 1; const int display_rate_ms = 500; - const int output_rate_ms = 1000; - const int averaging_depth = 1; + const int output_rate_ms = 100; config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(sampling_rate_internal)); @@ -300,15 +299,13 @@ int Obs_Gps_L1_System_Test::configure_receiver() config->set_property("TelemetryDecoder_1C.decimation_factor", std::to_string(decimation_factor)); // Set Observables - config->set_property("Observables.implementation", "GPS_L1_CA_Observables"); + config->set_property("Observables.implementation", "Hybrid_Observables"); config->set_property("Observables.dump", "false"); config->set_property("Observables.dump_filename", "./observables.dat"); config->set_property("Observables.averaging_depth", std::to_string(100)); // Set PVT - config->set_property("PVT.implementation", "GPS_L1_CA_PVT"); - config->set_property("PVT.averaging_depth", std::to_string(averaging_depth)); - config->set_property("PVT.flag_averaging", "true"); + config->set_property("PVT.implementation", "RTKLIB_PVT"); config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms)); config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms)); config->set_property("PVT.dump_filename", "./PVT"); @@ -596,7 +593,7 @@ void Obs_Gps_L1_System_Test::check_results() } double stdev_pr = compute_stdev(mean_pr_diff_v); std::cout << "Pseudorange diff error stdev = " << stdev_pr << " [m]" << std::endl; - ASSERT_LT(stdev_pr, 1.0); + ASSERT_LT(stdev_pr, 10.0); // Compute carrier phase error prn_id = 0; @@ -657,7 +654,7 @@ void Obs_Gps_L1_System_Test::check_results() double stdev_dp = compute_stdev(mean_doppler_v); std::cout << "Doppler error stdev = " << stdev_dp << " [Hz]" << std::endl; - ASSERT_LT(stdev_dp, 1.0); + ASSERT_LT(stdev_dp, 10.0); } @@ -672,7 +669,10 @@ TEST_F(Obs_Gps_L1_System_Test, Observables_system_test) configure_generator(); // Generate signal raw signal samples and observations RINEX file - generate_signal(); + if(!FLAGS_disable_generator) + { + generate_signal(); + } std::cout << "Validating generated reference RINEX obs file: " << FLAGS_filename_rinex_obs << " ..." << std::endl; bool is_gen_rinex_obs_valid = check_valid_rinex_obs( "./" + FLAGS_filename_rinex_obs); diff --git a/src/tests/system-tests/position_test.cc b/src/tests/system-tests/position_test.cc index d345788b6..d6f7fc5c8 100644 --- a/src/tests/system-tests/position_test.cc +++ b/src/tests/system-tests/position_test.cc @@ -30,6 +30,7 @@ */ #include +#include #include #include #include @@ -39,6 +40,7 @@ #include "concurrent_queue.h" #include "control_thread.h" #include "in_memory_configuration.h" +#include "MATH_CONSTANTS.h" #include "signal_generator_flags.h" @@ -56,7 +58,7 @@ public: std::string p4; std::string p5; - const double baseband_sampling_freq = 2.6e6; + const double baseband_sampling_freq = static_cast(FLAGS_fs_gen_hz); std::string filename_rinex_obs = FLAGS_filename_rinex_obs; std::string filename_raw_data = FLAGS_filename_raw_data; @@ -94,11 +96,15 @@ void Position_Gps_L1_System_Test::geodetic2Ecef(const double latitude, const dou double aux_x, aux_y, aux_z; // Convert to ECEF (See https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates ) - double N = std::pow(a, 2.0) / sqrt( std::pow(a, 2.0) * std::pow(cos(latitude), 2.0) + std::pow(b, 2.0) * std::pow(sin(latitude), 2.0)); + const double cLat = cos(latitude); + const double cLon = cos(longitude); + const double sLon = sin(longitude); + const double sLat = sin(latitude); + double N = std::pow(a, 2.0) / sqrt(std::pow(a, 2.0) * std::pow(cLat, 2.0) + std::pow(b, 2.0) * std::pow(sLat, 2.0)); - aux_x = (N + altitude) * cos(latitude) * cos(longitude); - aux_y = (N + altitude) * cos(latitude) * sin(longitude); - aux_z = ((std::pow(b, 2.0) / std::pow(a, 2.0)) * N + altitude) * sin(latitude); + aux_x = (N + altitude) * cLat * cLon; + aux_y = (N + altitude) * cLat * sLon; + aux_z = ((std::pow(b, 2.0) / std::pow(a, 2.0)) * N + altitude) * sLat; *x = aux_x; *y = aux_y; @@ -109,9 +115,8 @@ void Position_Gps_L1_System_Test::geodetic2Ecef(const double latitude, const dou void Position_Gps_L1_System_Test::geodetic2Enu(const double latitude, const double longitude, const double altitude, double* east, double* north, double* up) { - // Reference : https://github.com/ethz-asl/geodetic_utils/blob/master/include/geodetic_utils/geodetic_conv.hpp double x, y, z; - const double d2r = 3.1415926535898 / 180.0; + const double d2r = PI / 180.0; geodetic2Ecef(latitude * d2r, longitude * d2r, altitude, &x, &y, &z); @@ -163,9 +168,9 @@ double Position_Gps_L1_System_Test::compute_stdev_precision(const std::vector & vec, double ref) +double Position_Gps_L1_System_Test::compute_stdev_accuracy(const std::vector & vec, const double ref) { - double mean__ = ref; + const double mean__ = ref; double accum__ = 0.0; std::for_each (std::begin(vec), std::end(vec), [&](const double d) { accum__ += (d - mean__) * (d - mean__); @@ -261,7 +266,7 @@ int Position_Gps_L1_System_Test::configure_receiver() const float dll_bw_narrow_hz = 2.0; const int extend_correlation_ms = 1; - const int display_rate_ms = 500; + const int display_rate_ms = 1000; const int output_rate_ms = 1000; const int averaging_depth = 1; @@ -359,12 +364,10 @@ int Position_Gps_L1_System_Test::configure_receiver() config->set_property("Observables.implementation", "Hybrid_Observables"); config->set_property("Observables.dump", "false"); config->set_property("Observables.dump_filename", "./observables.dat"); - config->set_property("Observables.averaging_depth", std::to_string(100)); // Set PVT - config->set_property("PVT.implementation", "Hybrid_PVT"); - config->set_property("PVT.averaging_depth", std::to_string(averaging_depth)); - config->set_property("PVT.flag_averaging", "true"); + config->set_property("PVT.implementation", "RTKLIB_PVT"); + //config->set_property("PVT.implementation", "Hybrid_PVT"); config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms)); config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms)); config->set_property("PVT.dump_filename", "./PVT"); @@ -376,7 +379,10 @@ int Position_Gps_L1_System_Test::configure_receiver() config->set_property("PVT.rtcm_dump_devname", "/dev/pts/1"); config->set_property("PVT.dump", "false"); config->set_property("PVT.rinex_version", std::to_string(2)); - + config->set_property("PVT.positioning_mode", "PPP_Static"); + config->set_property("PVT.iono_model", "OFF"); + config->set_property("PVT.trop_model", "OFF"); +// config->set_property("PVT.AR_GPS", "PPP-AR"); return 0; } @@ -413,6 +419,7 @@ int Position_Gps_L1_System_Test::run_receiver() while (fgets(buffer, sizeof(buffer), fp) != NULL) { std::string aux = std::string(buffer); + EXPECT_EQ(aux.empty(), false); Position_Gps_L1_System_Test::generated_kml_file = aux.erase(aux.length() - 1, 1); } pclose(fp); @@ -480,6 +487,13 @@ void Position_Gps_L1_System_Test::check_results() double sigma_N_2_accuracy = std::pow(compute_stdev_accuracy(pos_n, 0.0), 2.0); double sigma_U_2_accuracy = std::pow(compute_stdev_accuracy(pos_u, 0.0), 2.0); + double sum__e = std::accumulate(pos_e.begin(), pos_e.end(), 0.0); + double mean__e = sum__e / pos_e.size(); + double sum__n = std::accumulate(pos_n.begin(), pos_n.end(), 0.0); + double mean__n = sum__n / pos_n.size(); + double sum__u = std::accumulate(pos_u.begin(), pos_u.end(), 0.0); + double mean__u = sum__u / pos_u.size(); + std::cout << "---- ACCURACY ----" << std::endl; std::cout << "2DRMS = " << 2 * sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; std::cout << "DRMS = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy) << " [m]" << std::endl; @@ -488,6 +502,8 @@ void Position_Gps_L1_System_Test::check_results() std::cout << "90% SAS = " << 0.833 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; std::cout << "MRSE = " << sqrt(sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; std::cout << "SEP = " << 0.51 * (sigma_E_2_accuracy + sigma_N_2_accuracy + sigma_U_2_accuracy) << " [m]" << std::endl; + std::cout << "Bias 2D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0)) << " [m]" << std::endl; + std::cout << "Bias 3D = " << sqrt(std::pow(mean__e, 2.0) + std::pow(mean__n, 2.0) + std::pow(mean__u, 2.0)) << " [m]" << std::endl; std::cout << std::endl; std::cout << "---- PRECISION ----" << std::endl; @@ -499,7 +515,6 @@ void Position_Gps_L1_System_Test::check_results() std::cout << "MRSE = " << sqrt(sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; std::cout << "SEP = " << 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision) << " [m]" << std::endl; - // Sanity Check double precision_SEP = 0.51 * (sigma_E_2_precision + sigma_N_2_precision + sigma_U_2_precision); ASSERT_LT(precision_SEP, 20.0); @@ -514,7 +529,7 @@ TEST_F(Position_Gps_L1_System_Test, Position_system_test) // Generate signal raw signal samples and observations RINEX file if(!FLAGS_disable_generator) { - generate_signal(); + generate_signal(); } // Configure receiver diff --git a/src/tests/system-tests/ttff_gps_l1.cc b/src/tests/system-tests/ttff_gps_l1.cc index e42274117..2d2155e4a 100644 --- a/src/tests/system-tests/ttff_gps_l1.cc +++ b/src/tests/system-tests/ttff_gps_l1.cc @@ -215,14 +215,12 @@ void TTFF_GPS_L1_CA_Test::config_1() config->set_property("TelemetryDecoder_1C.decimation_factor", std::to_string(decimation_factor)); // Set Observables - config->set_property("Observables.implementation", "GPS_L1_CA_Observables"); + config->set_property("Observables.implementation", "Hybrid_Observables"); config->set_property("Observables.dump", "false"); config->set_property("Observables.dump_filename", "./observables.dat"); // Set PVT - config->set_property("PVT.implementation", "GPS_L1_CA_PVT"); - config->set_property("PVT.averaging_depth", std::to_string(averaging_depth)); - config->set_property("PVT.flag_averaging", "true"); + config->set_property("PVT.implementation", "RTKLIB_PVT"); config->set_property("PVT.output_rate_ms", std::to_string(output_rate_ms)); config->set_property("PVT.display_rate_ms", std::to_string(display_rate_ms)); config->set_property("PVT.dump_filename", "./PVT"); @@ -279,8 +277,8 @@ void receive_msg() ttff_msg = msg.ttff; if( (ttff_msg != 0) && (ttff_msg != -1)) { - TTFF_v.push_back(ttff_msg / (1000.0 / decimation_factor) ); - LOG(INFO) << "Valid Time-To-First-Fix: " << ttff_msg / (1000.0 / decimation_factor ) << "[s]"; + TTFF_v.push_back(ttff_msg); + LOG(INFO) << "Valid Time-To-First-Fix: " << ttff_msg << "[s]"; // Stop the receiver while(((msqid_stop = msgget(key_stop, 0644))) == -1){} double msgsend_size = sizeof(msg_stop.ttff); diff --git a/src/tests/test_main.cc b/src/tests/test_main.cc index bafab092a..dec6bf9f0 100644 --- a/src/tests/test_main.cc +++ b/src/tests/test_main.cc @@ -58,12 +58,6 @@ #include "galileo_utc_model.h" #include "sbas_ephemeris.h" -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" -#include "sbas_time.h" - - using google::LogMessage; @@ -134,6 +128,7 @@ DECLARE_string(log_dir); #if MODERN_ARMADILLO #include "unit-tests/signal-processing-blocks/tracking/gps_l1_ca_dll_pll_tracking_test.cc" #include "unit-tests/signal-processing-blocks/telemetry_decoder/gps_l1_ca_telemetry_decoder_test.cc" +#include "unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc" #endif #endif diff --git a/src/tests/unit-tests/control-plane/control_thread_test.cc b/src/tests/unit-tests/control-plane/control_thread_test.cc index 7981b4d0e..accd4f24c 100644 --- a/src/tests/unit-tests/control-plane/control_thread_test.cc +++ b/src/tests/unit-tests/control-plane/control_thread_test.cc @@ -111,9 +111,9 @@ TEST_F(Control_Thread_Test, InstantiateRunControlMessages) config->set_property("Tracking_1C.item_type", "gr_complex"); config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); config->set_property("TelemetryDecoder_1C.item_type", "gr_complex"); - config->set_property("Observables.implementation", "GPS_L1_CA_Observables"); + config->set_property("Observables.implementation", "Hybrid_Observables"); config->set_property("Observables.item_type", "gr_complex"); - config->set_property("PVT.implementation", "GPS_L1_CA_PVT"); + config->set_property("PVT.implementation", "Hybrid_PVT"); config->set_property("PVT.item_type", "gr_complex"); std::shared_ptr control_thread = std::make_shared(config); @@ -171,9 +171,9 @@ TEST_F(Control_Thread_Test, InstantiateRunControlMessages2) config->set_property("Tracking_1C.item_type", "gr_complex"); config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); config->set_property("TelemetryDecoder_1C.item_type", "gr_complex"); - config->set_property("Observables.implementation", "GPS_L1_CA_Observables"); + config->set_property("Observables.implementation", "Hybrid_Observables"); config->set_property("Observables.item_type", "gr_complex"); - config->set_property("PVT.implementation", "GPS_L1_CA_PVT"); + config->set_property("PVT.implementation", "Hybrid_PVT"); config->set_property("PVT.item_type", "gr_complex"); std::unique_ptr control_thread2(new ControlThread(config)); @@ -235,9 +235,9 @@ TEST_F(Control_Thread_Test, StopReceiverProgrammatically) config->set_property("Tracking_1C.item_type", "gr_complex"); config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); config->set_property("TelemetryDecoder_1C.item_type", "gr_complex"); - config->set_property("Observables.implementation", "GPS_L1_CA_Observables"); + config->set_property("Observables.implementation", "Hybrid_Observables"); config->set_property("Observables.item_type", "gr_complex"); - config->set_property("PVT.implementation", "GPS_L1_CA_PVT"); + config->set_property("PVT.implementation", "Hybrid_PVT"); config->set_property("PVT.item_type", "gr_complex"); std::unique_ptr control_thread(new ControlThread(config)); diff --git a/src/tests/unit-tests/control-plane/gnss_block_factory_test.cc b/src/tests/unit-tests/control-plane/gnss_block_factory_test.cc index e5c9b473d..4477e56ac 100644 --- a/src/tests/unit-tests/control-plane/gnss_block_factory_test.cc +++ b/src/tests/unit-tests/control-plane/gnss_block_factory_test.cc @@ -304,11 +304,11 @@ TEST(GNSS_Block_Factory_Test, InstantiateObservables) TEST(GNSS_Block_Factory_Test, InstantiateGpsL1CaObservables) { std::shared_ptr configuration = std::make_shared(); - configuration->set_property("Observables.implementation", "GPS_L1_CA_Observables"); + configuration->set_property("Observables.implementation", "Hybrid_Observables"); std::unique_ptr factory; std::unique_ptr observables = factory->GetObservables(configuration); EXPECT_STREQ("Observables", observables->role().c_str()); - EXPECT_STREQ("GPS_L1_CA_Observables", observables->implementation().c_str()); + EXPECT_STREQ("Hybrid_Observables", observables->implementation().c_str()); } @@ -328,12 +328,12 @@ TEST(GNSS_Block_Factory_Test, InstantiatePvt) TEST(GNSS_Block_Factory_Test, InstantiateGpsL1CaPvt) { std::shared_ptr configuration = std::make_shared(); - configuration->set_property("PVT.implementation", "GPS_L1_CA_PVT"); + configuration->set_property("PVT.implementation", "Hybrid_PVT"); std::unique_ptr factory; std::shared_ptr pvt_ = factory->GetPVT(configuration); std::shared_ptr pvt = std::dynamic_pointer_cast(pvt_); EXPECT_STREQ("PVT", pvt->role().c_str()); - EXPECT_STREQ("GPS_L1_CA_PVT", pvt->implementation().c_str()); + EXPECT_STREQ("Hybrid_PVT", pvt->implementation().c_str()); } diff --git a/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc b/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc index 7719dc8d1..1dd59576e 100644 --- a/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc +++ b/src/tests/unit-tests/control-plane/gnss_flowgraph_test.cc @@ -64,8 +64,8 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStopOldNotation) config->set_property("Acquisition_1C.doppler_max", "5000"); config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking"); config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); - config->set_property("Observables.implementation", "GPS_L1_CA_Observables"); - config->set_property("PVT.implementation", "GPS_L1_CA_PVT"); + config->set_property("Observables.implementation", "Hybrid_Observables"); + config->set_property("PVT.implementation", "Hybrid_PVT"); std::shared_ptr flowgraph = std::make_shared(config, gr::msg_queue::make(0)); @@ -99,8 +99,8 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStop) config->set_property("Acquisition_1C.doppler_max", "5000"); config->set_property("Tracking_1C.implementation", "GPS_L1_CA_DLL_PLL_Tracking"); config->set_property("TelemetryDecoder_1C.implementation", "GPS_L1_CA_Telemetry_Decoder"); - config->set_property("Observables.implementation", "GPS_L1_CA_Observables"); - config->set_property("PVT.implementation", "GPS_L1_CA_PVT"); + config->set_property("Observables.implementation", "Hybrid_Observables"); + config->set_property("PVT.implementation", "Hybrid_PVT"); std::shared_ptr flowgraph = std::make_shared(config, gr::msg_queue::make(0)); @@ -133,8 +133,8 @@ TEST(GNSSFlowgraph, InstantiateConnectStartStopGalileoE1B) config->set_property("Acquisition_1B.doppler_max", "5000"); config->set_property("Tracking_1B.implementation", "Galileo_E1_DLL_PLL_VEML_Tracking"); config->set_property("TelemetryDecoder_1B.implementation", "Galileo_E1B_Telemetry_Decoder"); - config->set_property("Observables.implementation", "Galileo_E1B_Observables"); - config->set_property("PVT.implementation", "GALILEO_E1_PVT"); + config->set_property("Observables.implementation", "Hybrid_Observables"); + config->set_property("PVT.implementation", "Hybrid_PVT"); std::shared_ptr flowgraph = std::make_shared(config, gr::msg_queue::make(0)); diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt b/src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt index 8b4136e7d..aae96cedc 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt +++ b/src/tests/unit-tests/signal-processing-blocks/libs/CMakeLists.txt @@ -19,8 +19,10 @@ set(SIGNAL_PROCESSING_TESTING_LIB_SOURCES tracking_dump_reader.cc - tlm_dump_reader.cc + tlm_dump_reader.cc + observables_dump_reader.cc tracking_true_obs_reader.cc + true_observables_reader.cc ) include_directories( diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/observables_dump_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/observables_dump_reader.cc new file mode 100644 index 000000000..0773a5613 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/libs/observables_dump_reader.cc @@ -0,0 +1,133 @@ +/*! + * \file observables_dump_reader.cc + * \brief Helper file for unit testing + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "observables_dump_reader.h" + +bool observables_dump_reader::read_binary_obs() +{ + try + { + for(int i=0;i. + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_OBSERVABLES_DUMP_READER_H +#define GNSS_SDR_OBSERVABLES_DUMP_READER_H + +#include +#include +#include +#include + +class observables_dump_reader +{ +public: + observables_dump_reader(int n_channels); + ~observables_dump_reader(); + bool read_binary_obs(); + bool restart(); + long int num_epochs(); + bool open_obs_file(std::string out_file); + + + //dump variables + + double* RX_time; + double* TOW_at_current_symbol_s; + double* Carrier_Doppler_hz; + double* Acc_carrier_phase_hz; + double* Pseudorange_m; + double* PRN; + +private: + int n_channels; + std::string d_dump_filename; + std::ifstream d_dump_file; +}; + +#endif //GNSS_SDR_OBSERVABLES_DUMP_READER_H diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc index 40e522f20..e480699f7 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc @@ -35,7 +35,7 @@ bool tlm_dump_reader::read_binary_obs() try { d_dump_file.read((char *) &TOW_at_current_symbol, sizeof(double)); - d_dump_file.read((char *) &Prn_timestamp_ms, sizeof(double)); + d_dump_file.read((char *) &Tracking_sample_counter, sizeof(double)); d_dump_file.read((char *) &d_TOW_at_Preamble, sizeof(double)); } catch (const std::ifstream::failure &e) diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h b/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h index 7339b6e2e..1ffd3c217 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h @@ -47,7 +47,7 @@ public: //telemetry decoder dump variables double TOW_at_current_symbol; - double Prn_timestamp_ms; + unsigned long int Tracking_sample_counter; double d_TOW_at_Preamble; private: diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.cc new file mode 100644 index 000000000..fbe78cca7 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.cc @@ -0,0 +1,116 @@ +/*! + * \file true_observables_reader.cc + * \brief Helper file for unit testing + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "true_observables_reader.h" + +bool true_observables_reader::read_binary_obs() +{ + try + { + for(int i=0;i<12;i++) + { + d_dump_file.read((char *) &gps_time_sec[i], sizeof(double)); + d_dump_file.read((char *) &doppler_l1_hz, sizeof(double)); + d_dump_file.read((char *) &acc_carrier_phase_l1_cycles[i], sizeof(double)); + d_dump_file.read((char *) &dist_m[i], sizeof(double)); + d_dump_file.read((char *) &carrier_phase_l1_cycles[i], sizeof(double)); + d_dump_file.read((char *) &prn[i], sizeof(double)); + } + } + catch (const std::ifstream::failure &e) + { + return false; + } + return true; +} + +bool true_observables_reader::restart() +{ + if (d_dump_file.is_open()) + { + d_dump_file.clear(); + d_dump_file.seekg(0, std::ios::beg); + return true; + } + else + { + return false; + } +} + +long int true_observables_reader::num_epochs() +{ + std::ifstream::pos_type size; + int number_of_vars_in_epoch = 6*12; + int epoch_size_bytes = sizeof(double) * number_of_vars_in_epoch; + std::ifstream tmpfile( d_dump_filename.c_str(), std::ios::binary | std::ios::ate); + if (tmpfile.is_open()) + { + size = tmpfile.tellg(); + long int nepoch = size / epoch_size_bytes; + return nepoch; + } + else + { + return 0; + } +} + +bool true_observables_reader::open_obs_file(std::string out_file) +{ + if (d_dump_file.is_open() == false) + { + try + { + d_dump_filename=out_file; + d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit ); + d_dump_file.open(d_dump_filename.c_str(), std::ios::in | std::ios::binary); + std::cout << "True observables Log file opened: " << d_dump_filename.c_str() << std::endl; + return true; + } + catch (const std::ifstream::failure & e) + { + std::cout << "Problem opening True observables Log file: " << d_dump_filename.c_str() << std::endl; + return false; + } + } + else + { + return false; + } +} + +true_observables_reader::~true_observables_reader() +{ + if (d_dump_file.is_open() == true) + { + d_dump_file.close(); + } +} diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.h b/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.h new file mode 100644 index 000000000..a4fb8767c --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/libs/true_observables_reader.h @@ -0,0 +1,60 @@ +/*! + * \file tlm_dump_reader.h + * \brief Helper file for unit testing + * \author Javier Arribas, 2017. jarribas(at)cttc.es + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_TRUE_OBSERVABLES_READER_H +#define GNSS_SDR_TRUE_OBSERVABLES_READER_H + +#include +#include +#include +#include + +class true_observables_reader +{ +public: + ~true_observables_reader(); + bool read_binary_obs(); + bool restart(); + long int num_epochs(); + bool open_obs_file(std::string out_file); + + double gps_time_sec[12]; + double doppler_l1_hz[12]; + double acc_carrier_phase_l1_cycles[12]; + double dist_m[12]; + double carrier_phase_l1_cycles[12]; + double prn[12]; + +private: + std::string d_dump_filename; + std::ifstream d_dump_file; +}; + +#endif //GNSS_SDR_TRUE_OBSERVABLES_READER_H diff --git a/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc new file mode 100644 index 000000000..8af9e8480 --- /dev/null +++ b/src/tests/unit-tests/signal-processing-blocks/observables/hybrid_observables_test.cc @@ -0,0 +1,627 @@ +/*! + * \file hybrid_observables_test.cc + * \brief This class implements a tracking test for Galileo_E5a_DLL_PLL_Tracking + * implementation based on some input parameters. + * \author Javier Arribas, 2015. jarribas(at)cttc.es + * + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2012-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "GPS_L1_CA.h" +#include "gnss_satellite.h" +#include "gnss_block_factory.h" +#include "gnss_block_interface.h" +#include "tracking_interface.h" +#include "telemetry_decoder_interface.h" +#include "in_memory_configuration.h" +#include "gnss_synchro.h" +#include "gps_l1_ca_telemetry_decoder.h" +#include "tracking_true_obs_reader.h" +#include "true_observables_reader.h" +#include "tracking_dump_reader.h" +#include "observables_dump_reader.h" +#include "tlm_dump_reader.h" +#include "gps_l1_ca_dll_pll_tracking.h" +#include "gps_l1_ca_dll_pll_c_aid_tracking.h" +#include "hybrid_observables.h" +#include "signal_generator_flags.h" + + +// ######## GNURADIO BLOCK MESSAGE RECEVER FOR TRACKING MESSAGES ######### +class HybridObservablesTest_msg_rx; + +typedef boost::shared_ptr HybridObservablesTest_msg_rx_sptr; + +HybridObservablesTest_msg_rx_sptr HybridObservablesTest_msg_rx_make(); + +class HybridObservablesTest_msg_rx : public gr::block +{ +private: + friend HybridObservablesTest_msg_rx_sptr HybridObservablesTest_msg_rx_make(); + void msg_handler_events(pmt::pmt_t msg); + HybridObservablesTest_msg_rx(); + +public: + int rx_message; + ~HybridObservablesTest_msg_rx(); //!< Default destructor + +}; + +HybridObservablesTest_msg_rx_sptr HybridObservablesTest_msg_rx_make() +{ + return HybridObservablesTest_msg_rx_sptr(new HybridObservablesTest_msg_rx()); +} + +void HybridObservablesTest_msg_rx::msg_handler_events(pmt::pmt_t msg) +{ + try + { + long int message = pmt::to_long(msg); + rx_message = message; + } + catch(boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + rx_message = 0; + } +} + +HybridObservablesTest_msg_rx::HybridObservablesTest_msg_rx() : + gr::block("HybridObservablesTest_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + this->message_port_register_in(pmt::mp("events")); + this->set_msg_handler(pmt::mp("events"), boost::bind(&HybridObservablesTest_msg_rx::msg_handler_events, this, _1)); + rx_message = 0; +} + +HybridObservablesTest_msg_rx::~HybridObservablesTest_msg_rx() +{} + + +// ########################################################### + + +// ######## GNURADIO BLOCK MESSAGE RECEVER FOR TLM MESSAGES ######### +class HybridObservablesTest_tlm_msg_rx; + +typedef boost::shared_ptr HybridObservablesTest_tlm_msg_rx_sptr; + +HybridObservablesTest_tlm_msg_rx_sptr HybridObservablesTest_tlm_msg_rx_make(); + +class HybridObservablesTest_tlm_msg_rx : public gr::block +{ +private: + friend HybridObservablesTest_tlm_msg_rx_sptr HybridObservablesTest_tlm_msg_rx_make(); + void msg_handler_events(pmt::pmt_t msg); + HybridObservablesTest_tlm_msg_rx(); + +public: + int rx_message; + ~HybridObservablesTest_tlm_msg_rx(); //!< Default destructor + +}; + +HybridObservablesTest_tlm_msg_rx_sptr HybridObservablesTest_tlm_msg_rx_make() +{ + return HybridObservablesTest_tlm_msg_rx_sptr(new HybridObservablesTest_tlm_msg_rx()); +} + +void HybridObservablesTest_tlm_msg_rx::msg_handler_events(pmt::pmt_t msg) +{ + try + { + long int message = pmt::to_long(msg); + rx_message = message; + } + catch(boost::bad_any_cast& e) + { + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + rx_message = 0; + } +} + +HybridObservablesTest_tlm_msg_rx::HybridObservablesTest_tlm_msg_rx() : + gr::block("HybridObservablesTest_tlm_msg_rx", gr::io_signature::make(0, 0, 0), gr::io_signature::make(0, 0, 0)) +{ + this->message_port_register_in(pmt::mp("events")); + this->set_msg_handler(pmt::mp("events"), boost::bind(&HybridObservablesTest_tlm_msg_rx::msg_handler_events, this, _1)); + rx_message = 0; +} + +HybridObservablesTest_tlm_msg_rx::~HybridObservablesTest_tlm_msg_rx() +{} + + +// ########################################################### + + +class HybridObservablesTest: public ::testing::Test +{ + +public: + std::string generator_binary; + std::string p1; + std::string p2; + std::string p3; + std::string p4; + std::string p5; + + const int baseband_sampling_freq = FLAGS_fs_gen_hz; + + std::string filename_rinex_obs = FLAGS_filename_rinex_obs; + std::string filename_raw_data = FLAGS_filename_raw_data; + + int configure_generator(); + int generate_signal(); + void check_results( + arma::vec true_ch0_dist_m, arma::vec true_ch1_dist_m, + arma::vec true_ch0_tow_s, + arma::vec measuded_ch0_Pseudorange_m, + arma::vec measuded_ch1_Pseudorange_m, + arma::vec measuded_ch0_RX_time_s); + + HybridObservablesTest() + { + factory = std::make_shared(); + config = std::make_shared(); + item_size = sizeof(gr_complex); + gnss_synchro_ch0 = Gnss_Synchro(); + gnss_synchro_ch1 = Gnss_Synchro(); + } + + ~HybridObservablesTest() + {} + + void configure_receiver(); + + gr::top_block_sptr top_block; + std::shared_ptr factory; + std::shared_ptr config; + Gnss_Synchro gnss_synchro_ch0; + Gnss_Synchro gnss_synchro_ch1; + size_t item_size; +}; + +int HybridObservablesTest::configure_generator() +{ + // Configure signal generator + generator_binary = FLAGS_generator_binary; + + p1 = std::string("-rinex_nav_file=") + FLAGS_rinex_nav_file; + if(FLAGS_dynamic_position.empty()) + { + p2 = std::string("-static_position=") + FLAGS_static_position + std::string(",") + std::to_string(FLAGS_duration * 10); + } + else + { + p2 = std::string("-obs_pos_file=") + std::string(FLAGS_dynamic_position); + } + p3 = std::string("-rinex_obs_file=") + FLAGS_filename_rinex_obs; // RINEX 2.10 observation file output + p4 = std::string("-sig_out_file=") + FLAGS_filename_raw_data; // Baseband signal output file. Will be stored in int8_t IQ multiplexed samples + p5 = std::string("-sampling_freq=") + std::to_string(baseband_sampling_freq); //Baseband sampling frequency [MSps] + return 0; +} + + +int HybridObservablesTest::generate_signal() +{ + int child_status; + + char *const parmList[] = { &generator_binary[0], &generator_binary[0], &p1[0], &p2[0], &p3[0], &p4[0], &p5[0], NULL }; + + int pid; + if ((pid = fork()) == -1) + perror("fork err"); + else if (pid == 0) + { + execv(&generator_binary[0], parmList); + std::cout << "Return not expected. Must be an execv err." << std::endl; + std::terminate(); + } + + waitpid(pid, &child_status, 0); + + std::cout << "Signal and Observables RINEX and RAW files created." << std::endl; + return 0; +} + + +void HybridObservablesTest::configure_receiver() +{ + gnss_synchro_ch0.Channel_ID = 0; + gnss_synchro_ch0.System = 'G'; + std::string signal = "1C"; + signal.copy(gnss_synchro_ch0.Signal, 2, 0); + gnss_synchro_ch0.PRN = FLAGS_test_satellite_PRN; + + gnss_synchro_ch1.Channel_ID = 1; + gnss_synchro_ch1.System = 'G'; + signal.copy(gnss_synchro_ch1.Signal, 2, 0); + gnss_synchro_ch1.PRN = FLAGS_test_satellite_PRN2; + + + config->set_property("GNSS-SDR.internal_fs_hz", std::to_string(baseband_sampling_freq)); + + // Set Tracking + config->set_property("Tracking_1C.item_type", "gr_complex"); + config->set_property("Tracking_1C.if", "0"); + config->set_property("Tracking_1C.dump", "true"); + config->set_property("Tracking_1C.dump_filename", "./tracking_ch_"); + config->set_property("Tracking_1C.pll_bw_hz", "20.0"); + config->set_property("Tracking_1C.dll_bw_hz", "1.5"); + config->set_property("Tracking_1C.early_late_space_chips", "0.5"); + + + config->set_property("TelemetryDecoder_1C.dump","true"); + config->set_property("TelemetryDecoder_1C.decimation_factor","1"); + + config->set_property("Observables.history_depth","500"); + config->set_property("Observables.dump","true"); + + +} + +void HybridObservablesTest::check_results( + arma::vec true_ch0_dist_m, + arma::vec true_ch1_dist_m, + arma::vec true_ch0_tow_s, + arma::vec measuded_ch0_Pseudorange_m, + arma::vec measuded_ch1_Pseudorange_m, + arma::vec measuded_ch0_RX_time_s) +{ + //1. True value interpolation to match the measurement times + + arma::vec true_ch0_dist_interp; + arma::vec true_ch1_dist_interp; + arma::interp1(true_ch0_tow_s, true_ch0_dist_m, measuded_ch0_RX_time_s, true_ch0_dist_interp); + arma::interp1(true_ch0_tow_s, true_ch1_dist_m, measuded_ch0_RX_time_s, true_ch1_dist_interp); + + // generate delta pseudoranges + std::cout<<"s1:"< tracking_ch0 = std::make_shared(config.get(), "Tracking_1C", 1, 1); + //std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); + std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); + //std::shared_ptr tracking_ch1 = std::make_shared(config.get(), "Tracking_1C", 1, 1); + + boost::shared_ptr msg_rx_ch0 = HybridObservablesTest_msg_rx_make(); + boost::shared_ptr msg_rx_ch1 = HybridObservablesTest_msg_rx_make(); + + // load acquisition data based on the first epoch of the true observations + ASSERT_NO_THROW({ + if (true_obs_data_ch0.read_binary_obs() == false) + { + throw std::exception(); + }; + })<< "Failure reading true observables file" << std::endl; + + ASSERT_NO_THROW({ + if (true_obs_data_ch1.read_binary_obs() == false) + { + throw std::exception(); + }; + })<< "Failure reading true observables file" << std::endl; + + //restart the epoch counter + true_obs_data_ch0.restart(); + true_obs_data_ch1.restart(); + + std::cout << "Initial Doppler [Hz]=" << true_obs_data_ch0.doppler_l1_hz << " Initial code delay [Chips]=" << true_obs_data_ch0.prn_delay_chips << std::endl; + + gnss_synchro_ch0.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data_ch0.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; + gnss_synchro_ch0.Acq_doppler_hz = true_obs_data_ch0.doppler_l1_hz; + gnss_synchro_ch0.Acq_samplestamp_samples = 0; + + std::cout << "Initial Doppler [Hz]=" << true_obs_data_ch1.doppler_l1_hz << " Initial code delay [Chips]=" << true_obs_data_ch1.prn_delay_chips << std::endl; + + gnss_synchro_ch1.Acq_delay_samples = (GPS_L1_CA_CODE_LENGTH_CHIPS - true_obs_data_ch1.prn_delay_chips / GPS_L1_CA_CODE_LENGTH_CHIPS) * baseband_sampling_freq * GPS_L1_CA_CODE_PERIOD; + gnss_synchro_ch1.Acq_doppler_hz = true_obs_data_ch1.doppler_l1_hz; + gnss_synchro_ch1.Acq_samplestamp_samples = 0; + + //telemetry decoders + std::shared_ptr tlm_ch0(new GpsL1CaTelemetryDecoder(config.get(), "TelemetryDecoder_1C",1, 1)); + std::shared_ptr tlm_ch1(new GpsL1CaTelemetryDecoder(config.get(), "TelemetryDecoder_1C",1, 1)); + + + + ASSERT_NO_THROW( { + tlm_ch0->set_channel(0); + tlm_ch1->set_channel(1); + + tlm_ch0->set_satellite(Gnss_Satellite(std::string("GPS"),gnss_synchro_ch0.PRN)); + tlm_ch1->set_satellite(Gnss_Satellite(std::string("GPS"),gnss_synchro_ch1.PRN)); + }) << "Failure setting gnss_synchro." << std::endl; + + boost::shared_ptr tlm_msg_rx_ch1 = HybridObservablesTest_tlm_msg_rx_make(); + boost::shared_ptr tlm_msg_rx_ch2 = HybridObservablesTest_tlm_msg_rx_make(); + + //Observables + std::shared_ptr observables(new HybridObservables(config.get(), "Observables",2, 2)); + + + ASSERT_NO_THROW( { + tracking_ch0->set_channel(gnss_synchro_ch0.Channel_ID); + tracking_ch1->set_channel(gnss_synchro_ch1.Channel_ID); + }) << "Failure setting channel." << std::endl; + + ASSERT_NO_THROW( { + tracking_ch0->set_gnss_synchro(&gnss_synchro_ch0); + tracking_ch1->set_gnss_synchro(&gnss_synchro_ch1); + }) << "Failure setting gnss_synchro." << std::endl; + + ASSERT_NO_THROW( { + tracking_ch0->connect(top_block); + tracking_ch1->connect(top_block); + }) << "Failure connecting tracking to the top_block." << std::endl; + + ASSERT_NO_THROW( { + std::string file = "./" + filename_raw_data; + const char * file_name = file.c_str(); + gr::blocks::file_source::sptr file_source = gr::blocks::file_source::make(sizeof(int8_t), file_name, false); + gr::blocks::interleaved_char_to_complex::sptr gr_interleaved_char_to_complex = gr::blocks::interleaved_char_to_complex::make(); + gr::blocks::null_sink::sptr sink_ch0 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + gr::blocks::null_sink::sptr sink_ch1 = gr::blocks::null_sink::make(sizeof(Gnss_Synchro)); + top_block->connect(file_source, 0, gr_interleaved_char_to_complex, 0); + //ch0 + top_block->connect(gr_interleaved_char_to_complex, 0, tracking_ch0->get_left_block(), 0); + top_block->connect(tracking_ch0->get_right_block(), 0, tlm_ch0->get_left_block(), 0); + top_block->connect(tlm_ch0->get_right_block(), 0, observables->get_left_block(), 0); + top_block->msg_connect(tracking_ch0->get_right_block(), pmt::mp("events"), msg_rx_ch0, pmt::mp("events")); + //ch1 + top_block->connect(gr_interleaved_char_to_complex, 0, tracking_ch1->get_left_block(), 0); + top_block->connect(tracking_ch1->get_right_block(), 0, tlm_ch1->get_left_block(), 0); + top_block->connect(tlm_ch1->get_right_block(), 0, observables->get_left_block(), 1); + top_block->msg_connect(tracking_ch1->get_right_block(), pmt::mp("events"), msg_rx_ch1, pmt::mp("events")); + + top_block->connect(observables->get_right_block(), 0, sink_ch0, 0); + top_block->connect(observables->get_right_block(), 1, sink_ch1, 0); + + }) << "Failure connecting the blocks." << std::endl; + + tracking_ch0->start_tracking(); + tracking_ch1->start_tracking(); + + EXPECT_NO_THROW( { + gettimeofday(&tv, NULL); + begin = tv.tv_sec * 1000000 + tv.tv_usec; + top_block->run(); // Start threads and wait + gettimeofday(&tv, NULL); + end = tv.tv_sec * 1000000 + tv.tv_usec; + }) << "Failure running the top_block." << std::endl; + + //check results + //load the true values + + true_observables_reader true_observables; + + ASSERT_NO_THROW({ + if ( true_observables.open_obs_file(std::string("./obs_out.bin")) == false) + { + throw std::exception(); + }; + }) << "Failure opening true observables file" << std::endl; + + long int nepoch = true_observables.num_epochs(); + + std::cout << "True observation epochs=" << nepoch << std::endl; + arma::vec true_ch0_dist_m = arma::zeros(nepoch, 1); + arma::vec true_ch0_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); + arma::vec true_ch0_Doppler_Hz = arma::zeros(nepoch, 1); + arma::vec true_ch0_tow_s = arma::zeros(nepoch, 1); + arma::vec true_ch1_dist_m = arma::zeros(nepoch, 1); + arma::vec true_ch1_acc_carrier_phase_cycles = arma::zeros(nepoch, 1); + arma::vec true_ch1_Doppler_Hz = arma::zeros(nepoch, 1); + arma::vec true_ch1_tow_s = arma::zeros(nepoch, 1); + + true_observables.restart(); + long int epoch_counter = 0; + ASSERT_NO_THROW({ + while(true_observables.read_binary_obs()) + { + if (round(true_observables.prn[0])!=gnss_synchro_ch0.PRN) + { + std::cout<<"True observables SV PRN do not match"< #include #include "concurrent_map.h" +#include "concurrent_queue.h" #include "file_configuration.h" #include "gps_l1_ca_pcps_acquisition_fine_doppler.h" #include "gnss_signal.h" @@ -67,11 +68,7 @@ #include "galileo_almanac.h" #include "galileo_iono.h" #include "galileo_utc_model.h" -#include "sbas_telemetry_data.h" -#include "sbas_ionospheric_correction.h" -#include "sbas_satellite_correction.h" #include "sbas_ephemeris.h" -#include "sbas_time.h" #include "gnss_sdr_supl_client.h"