From 7a2a15b37dc93dc6741357054e998461812f6c3e Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Fri, 12 May 2017 17:58:04 +0200 Subject: [PATCH] Adding the path of Galileo E5 observations to RTKLIB solver. Some bug fixes. Work with Galileo in progress --- ...el_GPS_L2_M_Flexiband_bin_file_III_1b.conf | 117 +- .../PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 1788 +++++++++-------- src/algorithms/PVT/libs/rtklib_solver.cc | 403 ++-- .../gnuradio_blocks/hybrid_observables_cc.cc | 2 +- .../galileo_e5a_telemetry_decoder_cc.cc | 30 +- .../gps_l1_ca_telemetry_decoder_cc.cc | 2 +- .../galileo_e1_dll_pll_veml_tracking_cc.cc | 2 +- .../galileo_e5a_dll_pll_tracking_cc.cc | 1 + src/core/system_parameters/Galileo_E1.h | 1 + src/core/system_parameters/Galileo_E5a.h | 3 +- 10 files changed, 1237 insertions(+), 1112 deletions(-) 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 36a78f77b..e278865e9 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=/home/javier/signals/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 @@ -38,7 +38,7 @@ SignalSource.item_type=gr_complex 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=3 +SignalSource.RF_channels=1 ;#frontend channels gain. Not usable yet! SignalSource.gain1=0 @@ -324,9 +324,10 @@ Resampler2.implementation=Pass_Through ;######### CHANNELS GLOBAL CONFIG ############ ;#count: Number of available GPS satellite channels. -Channels_1C.count=1 -Channels_2S.count=1 -Channels_5X.count=2 +Channels_1C.count=0 +Channels_1B.count=10 +Channels_2S.count=0 +Channels_5X.count=0 ;#GPS.prns=7,8 @@ -343,30 +344,50 @@ Channels.in_acquisition=1 ;# CHANNEL CONNECTION Channel0.RF_channel_ID=0 -Channel1.RF_channel_ID=1 -Channel2.RF_channel_ID=2 -Channel3.RF_channel_ID=2 +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 - +Channel10.RF_channel_ID=0 +Channel11.RF_channel_ID=0 +Channel12.RF_channel_ID=0 +Channel13.RF_channel_ID=0 +Channel14.RF_channel_ID=0 +Channel15.RF_channel_ID=0 +Channel16.RF_channel_ID=0 +Channel17.RF_channel_ID=0 +Channel18.RF_channel_ID=0 +Channel19.RF_channel_ID=0 +Channel20.RF_channel_ID=0 +Channel21.RF_channel_ID=0 +Channel22.RF_channel_ID=0 +Channel23.RF_channel_ID=0 +Channel24.RF_channel_ID=0 +Channel25.RF_channel_ID=0 +Channel26.RF_channel_ID=0 +Channel27.RF_channel_ID=0 +Channel28.RF_channel_ID=0 +Channel29.RF_channel_ID=0 +Channel30.RF_channel_ID=2 +Channel31.RF_channel_ID=2 +Channel32.RF_channel_ID=2 +Channel33.RF_channel_ID=2 +Channel34.RF_channel_ID=2 +Channel35.RF_channel_ID=2 +Channel36.RF_channel_ID=2 +Channel37.RF_channel_ID=2 +Channel38.RF_channel_ID=2 +Channel39.RF_channel_ID=2 ;######### ACQUISITION GENERIC CONFIG ###### ;#The following options are specific to each channel and overwrite the generic options +;# GPS L1 CA Acquisition_1C.dump=false Acquisition_1C.dump_filename=./acq_dump.dat Acquisition_1C.item_type=gr_complex @@ -379,6 +400,30 @@ Acquisition_1C.doppler_step=250 Acquisition_1C.bit_transition_flag=false Acquisition_1C.max_dwells=1 +;# Galileo E1 +;#dump: Enable or disable the acquisition internal data file logging [true] or [false] +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=gr_complex +;#if: Signal intermediate frequency in [Hz] +Acquisition_1B.if=0 +;#sampled_ms: Signal block duration for the acquisition signal detection [ms] +Acquisition_1B.sampled_ms=4 +;#implementation: Acquisition algorithm selection for this channel: [GPS_L1_CA_PCPS_Acquisition] or [Galileo_E1_PCPS_Ambiguous_Acquisition] +Acquisition_1B.implementation=Galileo_E1_PCPS_Ambiguous_Acquisition +;#threshold: Acquisition threshold +;Acquisition_1B.threshold=0 +;#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_1B.pfa=0.0000002 +;#doppler_max: Maximum expected Doppler shift [Hz] +Acquisition_1B.doppler_max=5000 +;#doppler_max: Doppler step in the grid search [Hz] +Acquisition_1B.doppler_step=125 + + + ;# GPS L2C M Acquisition_2S.dump=false Acquisition_2S.dump_filename=./acq_dump.dat @@ -399,16 +444,14 @@ Acquisition_5X.item_type=gr_complex Acquisition_5X.if=0 Acquisition_5X.coherent_integration_time_ms=1 Acquisition_5X.implementation=Galileo_E5a_Noncoherent_IQ_Acquisition_CAF -Acquisition_5X.threshold=0.008 -Acquisition_5X.doppler_max=10000 -Acquisition_5X.doppler_step=250 +Acquisition_5X.threshold=0.009 +Acquisition_5X.doppler_max=5000 +Acquisition_5X.doppler_step=125 Acquisition_5X.bit_transition_flag=false Acquisition_5X.max_dwells=1 Acquisition_5X.CAF_window_hz=0 ; **Only for E5a** Resolves doppler ambiguity averaging the specified BW in the winner code delay. If set to 0 CAF filter is desactivated. Recommended value 3000 Hz Acquisition_5X.Zero_padding=0 ; **Only for E5a** Avoids power loss and doppler ambiguity in bit transitions by correlating one code with twice the input data length, ensuring that at least one full code is present without transitions. If set to 1 it is ON, if set to 0 it is OFF. - - ;######### TRACKING CONFIG ############ ;######### GPS L1 C/A GENERIC TRACKING CONFIG ############ @@ -422,6 +465,28 @@ Tracking_1C.dll_bw_hz=3.0; Tracking_1C.order=3; Tracking_1C.early_late_space_chips=0.5; +;######### GALILEO E1 TRK 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_1B.implementation=Galileo_E1_DLL_PLL_VEML_Tracking +;#item_type: Type and resolution for each of the signal samples. Use only [gr_complex] in this version. +Tracking_1B.item_type=gr_complex +;#sampling_frequency: Signal Intermediate Frequency in [Hz] +Tracking_1B.if=0 +;#dump: Enable or disable the Tracking internal binary data file logging [true] or [false] +Tracking_1B.dump=false +;#dump_filename: Log path and filename. Notice that the tracking channel will add "x.dat" where x is the channel number. +Tracking_1B.dump_filename=../data/veml_tracking_ch_ +;#pll_bw_hz: PLL loop filter bandwidth [Hz] +Tracking_1B.pll_bw_hz=15.0; +;#dll_bw_hz: DLL loop filter bandwidth [Hz] +Tracking_1B.dll_bw_hz=2.0; +;#order: PLL/DLL loop filter order [2] or [3] +Tracking_1B.order=3; +;#early_late_space_chips: correlator early-late space [chips]. Use [0.5] for GPS and [0.15] for Galileo +Tracking_1B.early_late_space_chips=0.15; +;#very_early_late_space_chips: only for [Galileo_E1_DLL_PLL_VEML_Tracking], correlator very early-late space [chips]. Use [0.6] +Tracking_1B.very_early_late_space_chips=0.6; ;######### GPS L2C GENERIC TRACKING CONFIG ############ Tracking_2S.implementation=GPS_L2_M_DLL_PLL_Tracking @@ -434,6 +499,7 @@ Tracking_2S.dll_bw_hz=0.25; Tracking_2S.order=2; Tracking_2S.early_late_space_chips=0.5; +;######### GALILEO E5 TRK CONFIG ############ Tracking_5X.implementation=Galileo_E5a_DLL_PLL_Tracking Tracking_5X.item_type=gr_complex Tracking_5X.if=0 @@ -451,6 +517,9 @@ Tracking_5X.early_late_space_chips=0.5; TelemetryDecoder_1C.implementation=GPS_L1_CA_Telemetry_Decoder TelemetryDecoder_1C.dump=false +TelemetryDecoder_1B.implementation=Galileo_E1B_Telemetry_Decoder +TelemetryDecoder_1B.dump=false + TelemetryDecoder_2S.implementation=GPS_L2C_Telemetry_Decoder TelemetryDecoder_2S.dump=false diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 3bd5e23db..e23dca100 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -86,108 +86,111 @@ 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 "; - } + //************* GPS telemetry ***************** + 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) ) + { + // ### 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 "; + } - 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 "; - } + //**************** Galileo telemetry ******************** + else 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!"; - } + + else + { + LOG(WARNING) << "msg_handler_telemetry unknown object type!"; + } } catch(boost::bad_any_cast& e) { - LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; + LOG(WARNING) << "msg_handler_telemetry Bad any cast!"; } } @@ -203,8 +206,8 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump 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)) + 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; @@ -238,39 +241,39 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump 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]; - } + { + 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 - } + { + 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]; - } + { + 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 - } + { + 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]; - } + { + 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 - } + { + 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]; - } + { + 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 - } + { + 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"); @@ -297,31 +300,31 @@ rtklib_pvt_cc::rtklib_pvt_cc(unsigned int nchannels, bool dump, std::string dump // ############# 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) << "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(); - } - } + 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(); - } + { + 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; } @@ -335,70 +338,70 @@ rtklib_pvt_cc::~rtklib_pvt_cc() std::string file_name="eph_GPS_L2CM.xml"; if (d_ls_pvt->gps_cnav_ephemeris_map.size() > 0) + { + try { - 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(); - } + 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"; - } + { + 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 { - 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(); - } + 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"; - } + { + 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 { - 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(); - } + 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"; - } + { + LOG(WARNING) << "Failed to save Galileo E1 Ephemeris, map is empty"; + } } @@ -429,735 +432,738 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite 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++) { - 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_cnav->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("2S") == 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_gal->second.i_satellite_PRN == in[i][epoch].PRN) && (std::string(in[i][epoch].Signal).compare("5X") == 0))) { - 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 - } - } - } + // store valid observables in a map. + gnss_observables_map.insert(std::pair(i, in[i][epoch])); } - // ############ 2 COMPUTE THE PVT ################################ - if (gnss_observables_map.size() > 0) + if(d_ls_pvt->gps_ephemeris_map.size() > 0) { - double current_RX_time = gnss_observables_map.begin()->second.RX_time; + 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 + } + } + } + } - if (std::fabs(current_RX_time - d_rx_time) * 1000.0 >= static_cast(d_output_rate_ms)) + + // ############ 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 { - flag_compute_pvt_output = true; - d_rx_time = current_RX_time; + 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 + } } - // compute on the fly PVT solution - if (flag_compute_pvt_output == true) + if(type_of_rx == 9) // GPS L1 C/A + Galileo E1B { - bool pvt_result; - pvt_result = d_ls_pvt->get_PVT(gnss_observables_map, d_rx_time, false); + 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(); - if (pvt_result == true) + // 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()) { - 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; - } - } + 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; } - // DEBUG MESSAGE: Display position in console output - if( (d_ls_pvt->b_valid_position == true) && (flag_display_pvt == true) ) + if((type_of_rx == 4) || (type_of_rx == 5) || (type_of_rx == 6) || (type_of_rx == 14) || (type_of_rx == 15)) // Galileo { - 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; + 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); + } - 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::map::iterator gal_ephemeris_iter = d_ls_pvt->galileo_ephemeris_map.begin(); - /* std::cout << "Dilution of Precision at " << boost::posix_time::to_simple_string(d_ls_pvt->d_position_UTC_time) + 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(); - } - } + // 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/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 526392885..053b8a926 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -74,21 +74,21 @@ rtklib_solver::rtklib_solver(int nchannels, std::string dump_filename, bool flag // ############# ENABLE DATA FILE LOG ################# if (d_flag_dump_enabled == 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) << "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(); - } - } + 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(); + } } + } } @@ -118,194 +118,243 @@ bool rtklib_solver::get_PVT(std::map gnss_observables_map, dou for(gnss_observables_iter = gnss_observables_map.begin(); gnss_observables_iter != gnss_observables_map.end(); gnss_observables_iter++) + { + switch(gnss_observables_iter->second.System) { - switch(gnss_observables_iter->second.System) + case 'E': + { + std::string sig_(gnss_observables_iter->second.Signal); + // Galileo E1 + if(sig_.compare("1B") == 0) { - 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()) { - // 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; + } + + } + // Galileo E5 + if(sig_.compare("5X") == 0) + { + + // 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()) + { + bool found_E1_obs=false; + for (int i = 0; i < valid_obs; i++) + { + if (eph_data[i].sat == (static_cast(gnss_observables_iter->second.PRN+NSATGPS+NSATGLO))) { - //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, + obs_data[i] = insert_obs_to_rtklib(obs_data[i], gnss_observables_iter->second, galileo_ephemeris_iter->second.WN_5, - 0); - valid_obs++; + 2);//Band 3 (L5/E5) + found_E1_obs=true; + break; } - else // the ephemeris are not available for this SV - { - DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; - } - break; + } + if (!found_E1_obs) + { + //insert Galileo E5 obs as new obs and also insert its ephemeris + //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, + 2); //Band 3 (L5/E5) + valid_obs++; + } } - case 'G': + else // the ephemeris are not available for this SV { - // 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; + DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN; } - default : - DLOG(INFO) << "Hybrid observables: Unknown GNSS"; - break; + } + break; + } + case 'G': + { + // GPS L1 + // 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; + } + } + //GPS L2 + 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[i] = insert_obs_to_rtklib(obs_data[i], + 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****************************************************** - // ********************************************************************** + // ********************************************************************** + // ****** SOLVE PVT****************************************************** + // ********************************************************************** - b_valid_position = false; - if (valid_obs > 0) + 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 */ - } + { + 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; /* L5/E5 */ + } 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; - } + { + 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) { - 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(); - } - } + // 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; -} + return b_valid_position; + } diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index ee7f8e00b..7093407b4 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -147,6 +147,7 @@ int hybrid_observables_cc::general_work (int noutput_items, */ for (unsigned int i = 0; i < d_nchannels; i++) { + current_gnss_synchro[i].Flag_valid_pseudorange=false; n_consume[i] = ninput_items[i];// full throttle for (int j = 0; j < n_consume[i]; j++) { @@ -186,7 +187,6 @@ int hybrid_observables_cc::general_work (int noutput_items, 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(), diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc index 03247cadc..16a9c0eb5 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc @@ -188,10 +188,9 @@ void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols,int fram } galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc( - Gnss_Satellite satellite, - bool dump) : - gr::block("galileo_e5a_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), - gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) + Gnss_Satellite satellite, bool dump) : gr::block("galileo_e5a_telemetry_decoder_cc", + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)), + gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) { // Telemetry Bit transition synchronization port out this->message_port_register_out(pmt::mp("preamble_timestamp_s")); @@ -245,10 +244,8 @@ galileo_e5a_telemetry_decoder_cc::~galileo_e5a_telemetry_decoder_cc() int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - // - const Gnss_Synchro **in = (const Gnss_Synchro **) &input_items[0]; //Get the input samples pointer - Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; - + const Gnss_Synchro *in = (const Gnss_Synchro *) input_items[0]; // input + Gnss_Synchro *out = (Gnss_Synchro *) output_items[0]; // output /* Terminology: Prompt: output from tracking Prompt correlator (Prompt samples) * Symbol: encoded navigation bits. 1 symbol = 20 samples in E5a * Bit: decoded navigation bits forming words as described in Galileo ICD @@ -260,9 +257,9 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut { case 0: { - if (in[0][0].Prompt_I != 0) + if (in[0].Prompt_I != 0) { - d_current_symbol += in[0][0].Prompt_I; + d_current_symbol += in[0].Prompt_I; if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1) { if (d_current_symbol > 0) @@ -290,7 +287,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut } case 1: { - d_current_symbol += in[0][0].Prompt_I; + d_current_symbol += in[0].Prompt_I; if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1) { if (d_current_symbol > 0) @@ -354,7 +351,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut } case 2: { - d_current_symbol += in[0][0].Prompt_I; + d_current_symbol += in[0].Prompt_I; if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1) { if (d_current_symbol > 0) @@ -415,7 +412,7 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut { d_flag_frame_sync = true; DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " - << in[0][0].Tracking_sample_counter << " [samples]"; } + << in[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 @@ -443,12 +440,12 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut break; } } - consume_each(1); + // UPDATE GNSS SYNCHRO DATA Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block //1. Copy the current tracking output - current_synchro_data = in[0][0]; + current_synchro_data = in[0]; //2. Add the telemetry decoder information if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true) //update TOW at the preamble instant @@ -524,7 +521,8 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut } d_sample_counter++; //count for the processed samples //3. Make the output (copy the object contents to the GNURadio reserved memory) - *out[0] = current_synchro_data; + out[0] = current_synchro_data; + consume_each(1); return 1; } 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 ad7bcc655..6db371746 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 @@ -110,7 +110,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( d_channel = 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); + this->set_min_output_buffer(5000); } 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 78bba87c3..daca4bb28 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 @@ -400,7 +400,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items __attri 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 = 4; + current_synchro_data.correlation_length_ms = Galileo_E1_CODE_PERIOD_MS; } else 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 789da2f3f..040aef9ea 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 @@ -646,6 +646,7 @@ int Galileo_E5a_Dll_Pll_Tracking_cc::general_work (int noutput_items __attribute } current_synchro_data.fs=d_fs_in; + current_synchro_data.correlation_length_ms=GALILEO_E5a_CODE_PERIOD_MS; *out[0] = current_synchro_data; if(d_dump) diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index 8347393aa..22260a145 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -52,6 +52,7 @@ const double GALILEO_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)] 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 int Galileo_E1_CODE_PERIOD_MS = 4; //!< Galileo E1 code period [ms] const double Galileo_E1_SUB_CARRIER_A_RATE_HZ = 1.023e6; //!< Galileo E1 sub-carrier 'a' rate [Hz] const double Galileo_E1_SUB_CARRIER_B_RATE_HZ = 6.138e6; //!< Galileo E1 sub-carrier 'b' rate [Hz] const double Galileo_E1_B_CODE_LENGTH_CHIPS = 4092.0; //!< Galileo E1-B code length [chips] diff --git a/src/core/system_parameters/Galileo_E5a.h b/src/core/system_parameters/Galileo_E5a.h index b8eed1fdd..d38a2ed14 100644 --- a/src/core/system_parameters/Galileo_E5a.h +++ b/src/core/system_parameters/Galileo_E5a.h @@ -46,7 +46,8 @@ const double Galileo_E5a_Q_TIERED_CODE_PERIOD = 0.100; //!< Galileo E5a-Q tie 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 double GALILEO_E5a_CODE_PERIOD = 0.001; //!< Galileo E1 primary code period [s] +const int GALILEO_E5a_CODE_PERIOD_MS = 1; //!< Galileo E1 primary code period [ms] const int Galileo_E5a_SYMBOL_RATE_BPS = 50; //!< Galileo E5a symbol rate [bits/second] const int Galileo_E5a_NUMBER_OF_CODES = 50;