diff --git a/conf/gnss-sdr_BEIDOU_B1I_ishort.conf b/conf/gnss-sdr_BEIDOU_B1I_ishort.conf new file mode 100644 index 000000000..a4d10abaa --- /dev/null +++ b/conf/gnss-sdr_BEIDOU_B1I_ishort.conf @@ -0,0 +1,93 @@ +; This is a GNSS-SDR configuration file +; The configuration API is described at https://gnss-sdr.org/docs/sp-blocks/ + +; You can define your own receiver and invoke it by doing +; gnss-sdr --config_file=my_GNSS_SDR_configuration.conf +; + +[GNSS-SDR] + +;######### GLOBAL OPTIONS ################## +;internal_fs_sps: Internal signal sampling frequency after the signal conditioning stage [samples per second]. +GNSS-SDR.internal_fs_sps=25000000 + +;######### CONTROL_THREAD CONFIG ############ +ControlThread.wait_for_flowgraph=false + +;######### SIGNAL_SOURCE CONFIG ############ +SignalSource.implementation=File_Signal_Source +SignalSource.filename=/home/sergi/gnss-sdr/data/USRP_BDS_B1I_201805171123_fs_25e6_if0e3_ishort.bin +SignalSource.item_type=ishort +SignalSource.sampling_frequency=25000000 +SignalSource.samples=0 +SignalSource.repeat=false +SignalSource.dump=false +;SignalSource.dump_filename=../data/signal_source.dat +SignalSource.enable_throttle_control=false + + +;######### SIGNAL_CONDITIONER CONFIG ############ +SignalConditioner.implementation=Signal_Conditioner + +DataTypeAdapter.implementation=Ishort_To_Complex +InputFilter.implementation=Pass_Through +InputFilter.item_type=gr_complex +Resampler.implementation=Direct_Resampler +Resampler.sample_freq_in=25000000 +Resampler.sample_freq_out=25000000 +Resampler.item_type=gr_complex + +;######### CHANNELS GLOBAL CONFIG ############ +Channels_B1.count=8 +Channels.in_acquisition=1 +Channel.signal=B1 + + +;######### ACQUISITION GLOBAL CONFIG ############ +Acquisition_B1.implementation=BEIDOU_B1I_PCPS_Acquisition +Acquisition_B1.item_type=gr_complex +Acquisition_B1.coherent_integration_time_ms=1 +Acquisition_B1.threshold=0.008 +;Acquisition_B1.pfa=0.000001 +Acquisition_B1.doppler_max=10000 +Acquisition_B1.doppler_step=250 +Acquisition_B1.dump=false +Acquisition_B1.dump_filename=./acq_dump.dat +Acquisition_B1.blocking=false; + +;######### TRACKING GLOBAL CONFIG ############ +Tracking_B1.implementation=Pass_Through +Tracking_B1.item_type=gr_complex +Tracking_B1.pll_bw_hz=40.0; +Tracking_B1.dll_bw_hz=4.0; +Tracking_B1.order=3; +Tracking_B1.dump=false; +Tracking_B1.dump_filename=./epl_tracking_ch_ + + +;######### TELEMETRY DECODER GPS CONFIG ############ +TelemetryDecoder_B1.implementation=Pass_Through +TelemetryDecoder_B1.dump=false + + +;######### OBSERVABLES CONFIG ############ +Observables.implementation=Pass_Through +Observables.dump=true +Observables.dump_filename=./observables.dat + + +;######### PVT CONFIG ############ +PVT.implementation=Pass_Through +PVT.positioning_mode=PPP_Static ; options: Single, Static, Kinematic, PPP_Static, PPP_Kinematic +PVT.iono_model=Broadcast ; options: OFF, Broadcast, SBAS, Iono-Free-LC, Estimate_STEC, IONEX +PVT.trop_model=Saastamoinen ; options: OFF, Saastamoinen, SBAS, Estimate_ZTD, Estimate_ZTD_Grad +PVT.output_rate_ms=100 +PVT.display_rate_ms=500 +PVT.dump_filename=./PVT +PVT.nmea_dump_filename=./gnss_sdr_pvt.nmea; +PVT.flag_nmea_tty_port=false; +PVT.nmea_dump_devname=/dev/pts/4 +PVT.flag_rtcm_server=false +PVT.flag_rtcm_tty_port=false +PVT.rtcm_dump_devname=/dev/pts/1 +PVT.dump=false diff --git a/src/algorithms/acquisition/adapters/CMakeLists.txt b/src/algorithms/acquisition/adapters/CMakeLists.txt index 831601796..0244a612d 100644 --- a/src/algorithms/acquisition/adapters/CMakeLists.txt +++ b/src/algorithms/acquisition/adapters/CMakeLists.txt @@ -34,6 +34,7 @@ set(ACQ_ADAPTER_SOURCES galileo_e5a_pcps_acquisition.cc glonass_l1_ca_pcps_acquisition.cc glonass_l2_ca_pcps_acquisition.cc + beidou_b1i_pcps_acquisition.cc ) if(ENABLE_FPGA) diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc new file mode 100644 index 000000000..a38550c41 --- /dev/null +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.cc @@ -0,0 +1,330 @@ +/*! + * \file beidou_b1i_pcps_acquisition.cc + * \brief Adapts a PCPS acquisition block to an AcquisitionInterface for + * BeiDou B1I signals + * \authors + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "beidou_b1i_pcps_acquisition.h" +#include "configuration_interface.h" +#include "beidou_b1I_signal_processing.h" +#include "beidou_b1I.h" +#include "gnss_sdr_flags.h" +#include +#include + + +using google::LogMessage; + +BeidouB1iPcpsAcquisition::BeidouB1iPcpsAcquisition( + ConfigurationInterface* configuration, std::string role, + unsigned int in_streams, unsigned int out_streams) : role_(role), in_streams_(in_streams), out_streams_(out_streams) +{ + pcpsconf_t acq_parameters; + configuration_ = configuration; + std::string default_item_type = "gr_complex"; + std::string default_dump_filename = "./data/acquisition.dat"; + + DLOG(INFO) << "role " << role; + + item_type_ = configuration_->property(role + ".item_type", default_item_type); + + long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000); + fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated); + acq_parameters.fs_in = fs_in_; + dump_ = configuration_->property(role + ".dump", false); + acq_parameters.dump = dump_; + blocking_ = configuration_->property(role + ".blocking", true); + acq_parameters.blocking = blocking_; + doppler_max_ = configuration_->property(role + ".doppler_max", 5000); + if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max; + acq_parameters.doppler_max = doppler_max_; + sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1); + acq_parameters.sampled_ms = sampled_ms_; + bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false); + acq_parameters.bit_transition_flag = bit_transition_flag_; + use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions + acq_parameters.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_; + max_dwells_ = configuration_->property(role + ".max_dwells", 1); + acq_parameters.max_dwells = max_dwells_; + dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename); + acq_parameters.dump_filename = dump_filename_; + //--- Find number of samples per spreading code ------------------------- + code_length_ = static_cast(std::round(static_cast(fs_in_) / ( BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS))); + + vector_length_ = code_length_ * sampled_ms_; + + if (bit_transition_flag_) + { + vector_length_ *= 2; + } + + code_ = new gr_complex[vector_length_]; + + if (item_type_.compare("cshort") == 0) + { + item_size_ = sizeof(lv_16sc_t); + } + else + { + item_size_ = sizeof(gr_complex); + } + acq_parameters.it_size = item_size_; + acq_parameters.sampled_ms = sampled_ms_; + acq_parameters.samples_per_ms = code_length_; + acq_parameters.samples_per_code = code_length_; + acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4); + acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0); + acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false); + acquisition_ = pcps_make_acquisition(acq_parameters); + DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")"; + + stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_); + DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")"; + + if (item_type_.compare("cbyte") == 0) + { + cbyte_to_float_x2_ = make_complex_byte_to_float_x2(); + float_to_complex_ = gr::blocks::float_to_complex::make(); + } + + channel_ = 0; + threshold_ = 0.0; + doppler_step_ = 0; + gnss_synchro_ = 0; + if (in_streams_ > 1) + { + LOG(ERROR) << "This implementation only supports one input stream"; + } + if (out_streams_ > 0) + { + LOG(ERROR) << "This implementation does not provide an output stream"; + } +} + + +BeidouB1iPcpsAcquisition::~BeidouB1iPcpsAcquisition() +{ + delete[] code_; +} + + +void BeidouB1iPcpsAcquisition::set_channel(unsigned int channel) +{ + channel_ = channel; + acquisition_->set_channel(channel_); +} + + +void BeidouB1iPcpsAcquisition::set_threshold(float threshold) +{ + float pfa = configuration_->property(role_ + ".pfa", 0.0); + + if (pfa == 0.0) + { + threshold_ = threshold; + } + else + { + threshold_ = calculate_threshold(pfa); + } + + DLOG(INFO) << "Channel " << channel_ << " Threshold = " << threshold_; + + acquisition_->set_threshold(threshold_); +} + + +void BeidouB1iPcpsAcquisition::set_doppler_max(unsigned int doppler_max) +{ + doppler_max_ = doppler_max; + + acquisition_->set_doppler_max(doppler_max_); +} + + +void BeidouB1iPcpsAcquisition::set_doppler_step(unsigned int doppler_step) +{ + doppler_step_ = doppler_step; + + acquisition_->set_doppler_step(doppler_step_); +} + + +void BeidouB1iPcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro) +{ + gnss_synchro_ = gnss_synchro; + + acquisition_->set_gnss_synchro(gnss_synchro_); +} + + +signed int BeidouB1iPcpsAcquisition::mag() +{ + return acquisition_->mag(); +} + + +void BeidouB1iPcpsAcquisition::init() +{ + acquisition_->init(); + set_local_code(); +} + + +void BeidouB1iPcpsAcquisition::set_local_code() +{ + std::complex* code = new std::complex[code_length_]; + + beidou_b1i_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_, 0); + + for (unsigned int i = 0; i < sampled_ms_; i++) + { + memcpy(&(code_[i * code_length_]), code, + sizeof(gr_complex) * code_length_); + } + + acquisition_->set_local_code(code_); + delete[] code; +} + + +void BeidouB1iPcpsAcquisition::reset() +{ + acquisition_->set_active(true); +} + + +void BeidouB1iPcpsAcquisition::set_state(int state) +{ + acquisition_->set_state(state); +} + + +float BeidouB1iPcpsAcquisition::calculate_threshold(float pfa) +{ + //Calculate the threshold + unsigned int frequency_bins = 0; + /* + for (int doppler = (int)(-doppler_max_); doppler <= (int)doppler_max_; doppler += doppler_step_) + { + frequency_bins++; + } + */ + + frequency_bins = (2 * doppler_max_ + doppler_step_) / doppler_step_; + + DLOG(INFO) << "Channel " << channel_ << " Pfa = " << pfa; + unsigned int ncells = vector_length_ * frequency_bins; + double exponent = 1 / static_cast(ncells); + double val = pow(1.0 - pfa, exponent); + double lambda = static_cast(vector_length_); + boost::math::exponential_distribution mydist(lambda); + float threshold = static_cast(quantile(mydist, val)); + + return threshold; +} + + +void BeidouB1iPcpsAcquisition::connect(gr::top_block_sptr top_block) +{ + if (item_type_.compare("gr_complex") == 0) + { + top_block->connect(stream_to_vector_, 0, acquisition_, 0); + } + else if (item_type_.compare("cshort") == 0) + { + top_block->connect(stream_to_vector_, 0, acquisition_, 0); + } + else if (item_type_.compare("cbyte") == 0) + { + top_block->connect(cbyte_to_float_x2_, 0, float_to_complex_, 0); + top_block->connect(cbyte_to_float_x2_, 1, float_to_complex_, 1); + top_block->connect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->connect(stream_to_vector_, 0, acquisition_, 0); + } + else + { + LOG(WARNING) << item_type_ << " unknown acquisition item type"; + } +} + + +void BeidouB1iPcpsAcquisition::disconnect(gr::top_block_sptr top_block) +{ + if (item_type_.compare("gr_complex") == 0) + { + top_block->disconnect(stream_to_vector_, 0, acquisition_, 0); + } + else if (item_type_.compare("cshort") == 0) + { + top_block->disconnect(stream_to_vector_, 0, acquisition_, 0); + } + else if (item_type_.compare("cbyte") == 0) + { + // Since a byte-based acq implementation is not available, + // we just convert cshorts to gr_complex + top_block->disconnect(cbyte_to_float_x2_, 0, float_to_complex_, 0); + top_block->disconnect(cbyte_to_float_x2_, 1, float_to_complex_, 1); + top_block->disconnect(float_to_complex_, 0, stream_to_vector_, 0); + top_block->disconnect(stream_to_vector_, 0, acquisition_, 0); + } + else + { + LOG(WARNING) << item_type_ << " unknown acquisition item type"; + } +} + + +gr::basic_block_sptr BeidouB1iPcpsAcquisition::get_left_block() +{ + if (item_type_.compare("gr_complex") == 0) + { + return stream_to_vector_; + } + else if (item_type_.compare("cshort") == 0) + { + return stream_to_vector_; + } + else if (item_type_.compare("cbyte") == 0) + { + return cbyte_to_float_x2_; + } + else + { + LOG(WARNING) << item_type_ << " unknown acquisition item type"; + return nullptr; + } +} + + +gr::basic_block_sptr BeidouB1iPcpsAcquisition::get_right_block() +{ + return acquisition_; +} diff --git a/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h new file mode 100644 index 000000000..deac16c23 --- /dev/null +++ b/src/algorithms/acquisition/adapters/beidou_b1i_pcps_acquisition.h @@ -0,0 +1,170 @@ +/*! + * \file gps_l1_ca_pcps_acquisition.h + * \brief Adapts a PCPS acquisition block to an AcquisitionInterface for + * GPS L1 C/A signals + * \authors
    + *
  • Javier Arribas, 2011. jarribas(at)cttc.es + *
  • Luis Esteve, 2012. luis(at)epsilon-formacion.com + *
  • Marc Molina, 2013. marc.molina.pena(at)gmail.com + *
+ * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GNSS_SDR_BEIDOU_B1I_PCPS_ACQUISITION_H_ +#define GNSS_SDR_BEIDOU_B1I_PCPS_ACQUISITION_H_ + +#include "acquisition_interface.h" +#include "gnss_synchro.h" +#include "pcps_acquisition.h" +#include "complex_byte_to_float_x2.h" +#include +#include +#include +#include + + +class ConfigurationInterface; + +/*! + * \brief This class adapts a PCPS acquisition block to an AcquisitionInterface + * for GPS L1 C/A signals + */ +class BeidouB1iPcpsAcquisition : public AcquisitionInterface +{ +public: + BeidouB1iPcpsAcquisition(ConfigurationInterface* configuration, + std::string role, unsigned int in_streams, + unsigned int out_streams); + + virtual ~BeidouB1iPcpsAcquisition(); + + inline std::string role() override + { + return role_; + } + + /*! + * \brief Returns "BEIDOU_B1I_PCPS_Acquisition" + */ + inline std::string implementation() override + { + return "BEIDOU_B1I_PCPS_Acquisition"; + } + + inline size_t item_size() override + { + return item_size_; + } + + void connect(gr::top_block_sptr top_block) override; + void disconnect(gr::top_block_sptr top_block) override; + gr::basic_block_sptr get_left_block() override; + gr::basic_block_sptr get_right_block() override; + + /*! + * \brief Set acquisition/tracking common Gnss_Synchro object pointer + * to efficiently exchange synchronization data between acquisition and + * tracking blocks + */ + void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override; + + /*! + * \brief Set acquisition channel unique ID + */ + void set_channel(unsigned int channel) override; + + /*! + * \brief Set statistics threshold of PCPS algorithm + */ + void set_threshold(float threshold) override; + + /*! + * \brief Set maximum Doppler off grid search + */ + void set_doppler_max(unsigned int doppler_max) override; + + /*! + * \brief Set Doppler steps for the grid search + */ + void set_doppler_step(unsigned int doppler_step) override; + + /*! + * \brief Initializes acquisition algorithm. + */ + void init() override; + + /*! + * \brief Sets local code for GPS L1/CA PCPS acquisition algorithm. + */ + void set_local_code() override; + + /*! + * \brief Returns the maximum peak of grid search + */ + signed int mag() override; + + /*! + * \brief Restart acquisition algorithm + */ + void reset() override; + + /*! + * \brief If state = 1, it forces the block to start acquiring from the first sample + */ + void set_state(int state); + +private: + ConfigurationInterface* configuration_; + pcps_acquisition_sptr acquisition_; + gr::blocks::stream_to_vector::sptr stream_to_vector_; + gr::blocks::float_to_complex::sptr float_to_complex_; + complex_byte_to_float_x2_sptr cbyte_to_float_x2_; + size_t item_size_; + std::string item_type_; + unsigned int vector_length_; + unsigned int code_length_; + bool bit_transition_flag_; + bool use_CFAR_algorithm_flag_; + unsigned int channel_; + float threshold_; + unsigned int doppler_max_; + unsigned int doppler_step_; + unsigned int sampled_ms_; + unsigned int max_dwells_; + long fs_in_; + bool dump_; + bool blocking_; + std::string dump_filename_; + std::complex* code_; + Gnss_Synchro* gnss_synchro_; + std::string role_; + unsigned int in_streams_; + unsigned int out_streams_; + + float calculate_threshold(float pfa); +}; + +#endif /* GNSS_SDR_BEIDOU_B1I_PCPS_ACQUISITION_H_ */ diff --git a/src/algorithms/libs/CMakeLists.txt b/src/algorithms/libs/CMakeLists.txt index bdce5d1cf..c193cf28a 100644 --- a/src/algorithms/libs/CMakeLists.txt +++ b/src/algorithms/libs/CMakeLists.txt @@ -32,6 +32,7 @@ if(ENABLE_FPGA) glonass_l2_signal_processing.cc pass_through.cc galileo_e5_signal_processing.cc + beidou_b1I_signal_processing.cc complex_byte_to_float_x2.cc byte_x2_to_complex_byte.cc cshort_to_float_x2.cc @@ -52,6 +53,7 @@ else(ENABLE_FPGA) gps_sdr_signal_processing.cc glonass_l1_signal_processing.cc glonass_l2_signal_processing.cc + beidou_b1I_signal_processing.cc pass_through.cc galileo_e5_signal_processing.cc complex_byte_to_float_x2.cc diff --git a/src/algorithms/libs/beidou_b1I_signal_processing.cc b/src/algorithms/libs/beidou_b1I_signal_processing.cc new file mode 100644 index 000000000..c0cdaa0f7 --- /dev/null +++ b/src/algorithms/libs/beidou_b1I_signal_processing.cc @@ -0,0 +1,195 @@ +/*! + * \file beidou_b1I_signal_processing.cc + * \brief This class implements various functions for BeiDou B1I signal + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * Detailed description of the file here if needed. + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "beidou_b1I_signal_processing.h" + +auto auxCeil = [](float x) { return static_cast(static_cast((x) + 1)); }; + +void beidou_b1i_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shift) +{ + const unsigned int _code_length = 2046; + bool G1[_code_length]; + bool G2[_code_length]; + bool G1_register[11] = {0,1,0,1,0,1,0,1,0,1,0}; + bool G2_register[11] = {0,1,0,1,0,1,0,1,0,1,0}; + bool feedback1, feedback2; + bool aux; + unsigned int lcv, lcv2; + unsigned int delay; + signed int prn_idx; + + /* G2 Delays as defined in GPS-ISD-200D */ + const signed int delays[51] = {5 /*PRN1*/, 6, 7, 8, 17, 18, 139, 140, 141, 251, 252, 254, 255, 256, 257, 258, 469, 470, 471, 472, + 473, 474, 509, 512, 513, 514, 515, 516, 859, 860, 861, 862 /*PRN32*/, + 145 /*PRN120*/, 175, 52, 21, 237, 235, 886, 657, 634, 762, + 355, 1012, 176, 603, 130, 359, 595, 68, 386 /*PRN138*/}; + const signed int phase1[37] = {1, 1, 1, 1, 1, 1, 1, 1, 2, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 8, 8, 8, 9, 9, 10}; + const signed int phase2[37] = {3, 4, 5, 6, 8, 9, 10, 11, 7, 4, 5, 6, 8, 9, 10, 11, 5, 6, 8, 9, 10, 11, 6, 8, 9, 10, 11, 8, 9, 10, 11, 9, 10, 11, 10, 11, 11}; + // compute delay array index for given PRN number + if (120 <= _prn && _prn <= 138) + { + prn_idx = _prn - 88; // SBAS PRNs are at array indices 31 to 50 (offset: -120+33-1 =-88) + } + else + { + prn_idx = _prn - 1; + } + + /* A simple error check */ + if ((prn_idx < 0) || (prn_idx > 51)) + return; + + /*for (lcv = 0; lcv < 11; lcv++) + { + G1_register[lcv] = 1; + G2_register[lcv] = 1; + }*/ + + /* Generate G1 & G2 Register */ + for (lcv = 0; lcv < _code_length; lcv++) + { + G1[lcv] = G1_register[0]; + G2[lcv] = G2_register[phase1[prn_idx]] ^ G2_register[phase2[prn_idx]]; + + feedback1 = (G1_register[0] + G1_register[1] + G1_register[2] + G1_register[3] + G1_register[4] + G1_register[10]) & 0x1; + feedback2 = (G2_register[0] + G2_register[2] + G2_register[3] + G2_register[6] + G2_register[7] + G2_register[8] + G2_register[9] + G2_register[10]) & 0x1; + + for (lcv2 = 0; lcv2 < 10; lcv2++) + { + G1_register[lcv2] = G1_register[lcv2 + 1]; + G2_register[lcv2] = G2_register[lcv2 + 1]; + } + + G1_register[10] = feedback1; + G2_register[10] = feedback2; + } + + /* Set the delay */ + delay = _code_length /*- delays[prn_idx]*/; + delay += _chip_shift; + delay %= _code_length; + + /* Generate PRN from G1 and G2 Registers */ + for (lcv = 0; lcv < _code_length; lcv++) + { + aux = G1[(lcv + _chip_shift) % _code_length] ^ G2[delay]; + if (aux == true) + { + _dest[lcv] = 1; + } + else + { + _dest[lcv] = -1; + } + delay++; + delay %= _code_length; + } +} + + +void beidou_b1i_code_gen_float(float* _dest, signed int _prn, unsigned int _chip_shift) +{ + unsigned int _code_length = 2046; + int b1i_code_int[_code_length]; + + beidou_b1i_code_gen_int(b1i_code_int, _prn, _chip_shift); + + for (unsigned int ii = 0; ii < _code_length; ++ii) + { + _dest[ii] = static_cast(b1i_code_int[ii]); + } +} + + +void beidou_b1i_code_gen_complex(std::complex* _dest, signed int _prn, unsigned int _chip_shift) +{ + unsigned int _code_length = 2046; + int b1i_code_int[_code_length]; + + beidou_b1i_code_gen_int(b1i_code_int, _prn, _chip_shift); + + for (unsigned int ii = 0; ii < _code_length; ++ii) + { + _dest[ii] = std::complex(static_cast(b1i_code_int[ii]), 0.0f); + } +} + + +/* + * Generates complex GPS L1 C/A code for the desired SV ID and sampled to specific sampling frequency + */ +void beidou_b1i_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, int _fs, unsigned int _chip_shift) +{ + // This function is based on the GNU software GPS for MATLAB in the Kay Borre book + std::complex _code[2046]; + signed int _samplesPerCode, _codeValueIndex; + float _ts; + float _tc; + float aux; + const signed int _codeFreqBasis = 2046000; //Hz + const signed int _codeLength = 2046; + + //--- Find number of samples per spreading code ---------------------------- + _samplesPerCode = static_cast(static_cast(_fs) / static_cast(_codeFreqBasis / _codeLength)); + + //--- Find time constants -------------------------------------------------- + _ts = 1.0 / static_cast(_fs); // Sampling period in sec + _tc = 1.0 / static_cast(_codeFreqBasis); // C/A chip period in sec + beidou_b1i_code_gen_complex(_code, _prn, _chip_shift); //generate C/A code 1 sample per chip + + for (signed int i = 0; i < _samplesPerCode; i++) + { + //=== Digitizing ======================================================= + + //--- Make index array to read C/A code values ------------------------- + // The length of the index array depends on the sampling frequency - + // number of samples per millisecond (because one C/A code period is one + // millisecond). + + // _codeValueIndex = ceil((_ts * ((float)i + 1)) / _tc) - 1; + aux = (_ts * (i + 1)) / _tc; + _codeValueIndex = auxCeil(aux) - 1; + + //--- Make the digitized version of the C/A code ----------------------- + // The "upsampled" code is made by selecting values form the CA code + // chip array (caCode) for the time instances of each sample. + if (i == _samplesPerCode - 1) + { + //--- Correct the last index (due to number rounding issues) ----------- + _dest[i] = _code[_codeLength - 1]; + } + else + { + _dest[i] = _code[_codeValueIndex]; //repeat the chip -> upsample + } + } +} diff --git a/src/algorithms/libs/beidou_b1I_signal_processing.h b/src/algorithms/libs/beidou_b1I_signal_processing.h new file mode 100644 index 000000000..39af75758 --- /dev/null +++ b/src/algorithms/libs/beidou_b1I_signal_processing.h @@ -0,0 +1,53 @@ +/*! + * \file beidou_b1I_signal_processing.h + * \brief This class implements various functions for BeiDou B1I signals + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * Detailed description of the file here if needed. + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#ifndef BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_ +#define BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_ + +#include + +//!Generates int GPS L1 C/A code for the desired SV ID and code shift +void beidou_b1i_code_gen_int(int* _dest, signed int _prn, unsigned int _chip_shift); + +//!Generates float GPS L1 C/A code for the desired SV ID and code shift +void beidou_b1i_code_gen_float(float* _dest, signed int _prn, unsigned int _chip_shift); + +//!Generates complex GPS L1 C/A code for the desired SV ID and code shift, and sampled to specific sampling frequency +void beidou_b1i_code_gen_complex(std::complex* _dest, signed int _prn, unsigned int _chip_shift); + +//! Generates N complex GPS L1 C/A codes for the desired SV ID and code shift +void beidou_b1i_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, int _fs, unsigned int _chip_shift, unsigned int _ncodes); + +//! Generates complex GPS L1 C/A code for the desired SV ID and code shift +void beidou_b1i_code_gen_complex_sampled(std::complex* _dest, unsigned int _prn, int _fs, unsigned int _chip_shift); + +#endif /* BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_ */ diff --git a/src/algorithms/signal_generator/adapters/signal_generator.cc b/src/algorithms/signal_generator/adapters/signal_generator.cc index b5b7552f4..1651272d6 100644 --- a/src/algorithms/signal_generator/adapters/signal_generator.cc +++ b/src/algorithms/signal_generator/adapters/signal_generator.cc @@ -35,6 +35,7 @@ #include "Galileo_E1.h" #include "GPS_L1_CA.h" #include "Galileo_E5a.h" +#include "beidou_b1I.h" #include "GLONASS_L1_L2_CA.h" #include @@ -110,6 +111,12 @@ SignalGenerator::SignalGenerator(ConfigurationInterface* configuration, } } + else if (std::find(system.begin(), system.end(), "B") != system.end()) + { + vector_length = round(static_cast(fs_in) / (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS)); + } + + if (item_type_.compare("gr_complex") == 0) { item_size_ = sizeof(gr_complex); diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index f4bb4209e..e85fb90c1 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -81,6 +81,7 @@ #include "galileo_e5a_pcps_acquisition.h" #include "glonass_l1_ca_pcps_acquisition.h" #include "glonass_l2_ca_pcps_acquisition.h" +#include "beidou_b1i_pcps_acquisition.h" #include "gps_l1_ca_dll_pll_tracking.h" #include "gps_l1_ca_dll_pll_c_aid_tracking.h" #include "gps_l1_ca_tcp_connector_tracking.h" @@ -266,15 +267,18 @@ std::unique_ptr GNSSBlockFactory::GetObservables(std::shared GPS_channels += configuration->property("Channels_2S.count", 0); GPS_channels += configuration->property("Channels_L5.count", 0); unsigned int Glonass_channels = configuration->property("Channels_1G.count", 0); + unsigned int Beidou_channels = configuration->property("Channels_B1.count", 0); unsigned int extra_channels = 1; // For monitor channel sample counter return GetBlock(configuration, "Observables", implementation, Galileo_channels + GPS_channels + Glonass_channels + + Beidou_channels + extra_channels, Galileo_channels + GPS_channels + - Glonass_channels); + Glonass_channels + + Beidou_channels); } @@ -289,7 +293,8 @@ std::unique_ptr GNSSBlockFactory::GetPVT(std::shared_ptrproperty("Channels_2S.count", 0); GPS_channels += configuration->property("Channels_L5.count", 0); unsigned int Glonass_channels = configuration->property("Channels_1G.count", 0); - return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels + Glonass_channels, 0); + unsigned int Beidou_channels = configuration->property("Channels_B1.count", 0); + return GetBlock(configuration, "PVT", implementation, Galileo_channels + GPS_channels + Glonass_channels + Beidou_channels, 0); } @@ -769,6 +774,73 @@ std::unique_ptr GNSSBlockFactory::GetChannel_L5( } +//********* BeiDou B1I CHANNEL ***************** +std::unique_ptr GNSSBlockFactory::GetChannel_B1( + std::shared_ptr configuration, + std::string acq, std::string trk, std::string tlm, int channel, + gr::msg_queue::sptr queue) +{ + std::stringstream stream; + stream << channel; + std::string id = stream.str(); + LOG(INFO) << "Instantiating Channel " << id << " with Acquisition Implementation: " + << acq << ", Tracking Implementation: " << trk << ", Telemetry Decoder implementation: " << tlm; + std::string aux = configuration->property("Acquisition_B1" + std::to_string(channel) + ".implementation", std::string("W")); + std::string appendix1; + if (aux.compare("W") != 0) + { + appendix1 = std::to_string(channel); + } + else + { + appendix1 = ""; + } + aux = configuration->property("Tracking_B1" + std::to_string(channel) + ".implementation", std::string("W")); + std::string appendix2; + if (aux.compare("W") != 0) + { + appendix2 = std::to_string(channel); + } + else + { + appendix2 = ""; + } + aux = configuration->property("TelemetryDecoder_B1" + std::to_string(channel) + ".implementation", std::string("W")); + std::string appendix3; + if (aux.compare("W") != 0) + { + appendix3 = std::to_string(channel); + } + else + { + appendix3 = ""; + } + // Automatically detect input data type + std::shared_ptr config; + config = std::make_shared(); + std::string default_item_type = "gr_complex"; + std::string acq_item_type = configuration->property("Acquisition_B1" + appendix1 + ".item_type", default_item_type); + std::string trk_item_type = configuration->property("Tracking_B1" + appendix2 + ".item_type", default_item_type); + if (acq_item_type.compare(trk_item_type)) + { + LOG(ERROR) << "Acquisition and Tracking blocks must have the same input data type!"; + } + config->set_property("Channel.item_type", acq_item_type); + + std::unique_ptr pass_through_ = GetBlock(configuration, "Channel", "Pass_Through", 1, 1, queue); + std::unique_ptr acq_ = GetAcqBlock(configuration, "Acquisition_B1" + appendix1, acq, 1, 0); + std::unique_ptr trk_ = GetTrkBlock(configuration, "Tracking_B1" + appendix2, trk, 1, 1); + std::unique_ptr tlm_ = GetTlmBlock(configuration, "TelemetryDecoder_B1" + appendix3, tlm, 1, 1); + + std::unique_ptr channel_(new Channel(configuration.get(), channel, std::move(pass_through_), + std::move(acq_), + std::move(trk_), + std::move(tlm_), + "Channel", "B1", queue)); + + return channel_; +} + std::unique_ptr>> GNSSBlockFactory::GetChannels( std::shared_ptr configuration, gr::msg_queue::sptr queue) { @@ -793,6 +865,7 @@ std::unique_ptr>> GNSSBlockFacto Channels_2S_count + Channels_2G_count + Channels_5X_count + + Channels_L5_count + Channels_L5_count; std::unique_ptr>> channels(new std::vector>(total_channels)); @@ -1449,6 +1522,13 @@ std::unique_ptr GNSSBlockFactory::GetBlock( out_streams)); block = std::move(block_); } + else if (implementation.compare("BEIDOU_B1I_PCPS_Acquisition") == 0) + { + std::unique_ptr block_(new BeidouB1iPcpsAcquisition(configuration.get(), role, in_streams, + out_streams)); + block = std::move(block_); + } + // TRACKING BLOCKS ------------------------------------------------------------- else if (implementation.compare("GPS_L1_CA_DLL_PLL_Tracking") == 0) { diff --git a/src/core/receiver/gnss_block_factory.h b/src/core/receiver/gnss_block_factory.h index a66e6917e..ad014d356 100644 --- a/src/core/receiver/gnss_block_factory.h +++ b/src/core/receiver/gnss_block_factory.h @@ -106,6 +106,10 @@ private: std::string acq, std::string trk, std::string tlm, int channel, boost::shared_ptr queue); + std::unique_ptr GetChannel_B1(std::shared_ptr configuration, + std::string acq, std::string trk, std::string tlm, int channel, + boost::shared_ptr queue); + std::unique_ptr GetAcqBlock( std::shared_ptr configuration, std::string role, diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 83ad6807a..44b2e25d6 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -425,6 +425,7 @@ void GNSSFlowgraph::connect() if ((gnss_signal.compare("1C") == 0) or (gnss_signal.compare("2S") == 0) or (gnss_signal.compare("L5") == 0)) gnss_system = "GPS"; if ((gnss_signal.compare("1B") == 0) or (gnss_signal.compare("5X") == 0)) gnss_system = "Galileo"; if ((gnss_signal.compare("1G") == 0) or (gnss_signal.compare("2G") == 0)) gnss_system = "Glonass"; + if (gnss_signal.compare("B1") == 0) gnss_system = "Beidou"; Gnss_Signal signal_value = Gnss_Signal(Gnss_Satellite(gnss_system, sat), gnss_signal); available_GNSS_signals_.remove(signal_value); channels_.at(i)->set_signal(signal_value); @@ -970,6 +971,9 @@ void GNSSFlowgraph::set_signals_list() // Removing satellites sharing same frequency number(1 and 5, 2 and 6, 3 and 7, 4 and 6, 11 and 15, 12 and 16, 14 and 18, 17 and 21 std::set available_glonass_prn = {1, 2, 3, 4, 9, 10, 11, 12, 18, 19, 20, 21, 24}; + std::set available_beidou_prn = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, + 11, 12, 13, 17, 31, 32, 33, 34, 35}; + std::string sv_list = configuration_->property("Galileo.prns", std::string("")); if (sv_list.length() > 0) @@ -1033,6 +1037,22 @@ void GNSSFlowgraph::set_signals_list() available_glonass_prn = tmp_set; } } + sv_list = configuration_->property("Beidou.prns", std::string("")); + + if (sv_list.length() > 0) + { + // Reset the available prns: + std::set tmp_set; + boost::tokenizer<> tok(sv_list); + std::transform(tok.begin(), tok.end(), std::inserter(tmp_set, tmp_set.begin()), + boost::lexical_cast); + + if (tmp_set.size() > 0) + { + available_beidou_prn = tmp_set; + } + } + if (configuration_->property("Channels_1C.count", 0) > 0) { @@ -1153,6 +1173,22 @@ void GNSSFlowgraph::set_signals_list() std::string("2G"))); } } + + if (configuration_->property("Channels_B1.count", 0) > 0) + { + /* + * Loop to create the list of BeiDou B1C signals + */ + for (available_gnss_prn_iter = available_beidou_prn.begin(); + available_gnss_prn_iter != available_beidou_prn.end(); + available_gnss_prn_iter++) + { + available_GNSS_signals_.push_back(Gnss_Signal( + Gnss_Satellite(std::string("Beidou"), *available_gnss_prn_iter), + std::string("B1"))); + } + } + } diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 62090dd6e..b85643555 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -46,6 +46,7 @@ const double PI = 3.1415926535897932; //!< pi const double PI_2 = 2.0 * PI; //!< 2 * pi +const double TWO_P3 = (8); //!< 2^3 const double TWO_P4 = (16); //!< 2^4 const double TWO_P11 = (2048); //!< 2^11 const double TWO_P12 = (4096); //!< 2^12 @@ -97,6 +98,7 @@ const double TWO_N55 = (2.775557561562891e-017); //!< 2^-55 const double TWO_N57 = (6.938893903907228e-18); //!< 2^-57 const double TWO_N59 = (1.73472347597681e-018); //!< 2^-59 const double TWO_N60 = (8.673617379884036e-19); //!< 2^-60 +const double TWO_N66 = (1.3552527156068805425093160010874271392822265625e-20); //!< 2^-66 const double TWO_N68 = (3.388131789017201e-21); //!< 2^-68 diff --git a/src/core/system_parameters/beidou_b1I.h b/src/core/system_parameters/beidou_b1I.h new file mode 100644 index 000000000..e587eef4d --- /dev/null +++ b/src/core/system_parameters/beidou_b1I.h @@ -0,0 +1,250 @@ +/*! + * \file beidou_b1I.h + * \brief Defines system parameters for BeiDou B1I signal and D1 NAV data + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_BEIDOU_B1I_H_ +#define GNSS_SDR_BEIDOU_B1I_H_ + +#include +#include // std::pair +#include "MATH_CONSTANTS.h" + +// Physical constants +const double BEIDOU_C_m_s = 299792458.0; //!< The speed of light, [m/s] +const double BEIDOU_C_m_ms = 299792.4580; //!< The speed of light, [m/ms] +const double BEIDOU_PI = 3.1415926535898; //!< Pi +const double BEIDOU_TWO_PI = 6.283185307179586;//!< 2Pi +const double BEIDOU_OMEGA_EARTH_DOT = 7.2921150e-5; //!< Earth rotation rate, [rad/s] as defined in CGCS2000 +const double BEIDOU_GM = 3.986004418e14; //!< Universal gravitational constant times the mass of the Earth, [m^3/s^2] as defined in CGCS2000 +const double BEIDOU_F = -4.442807309e-10; //!< Constant, [s/(m)^(1/2)] F=-2(GM)^.5/C^2 + + +// carrier and code frequencies +const double BEIDOU_B1I_FREQ_HZ = 1.561098e9; //!< b1I [Hz] +const double BEIDOU_B1I_CODE_RATE_HZ = 2.046e6; //!< beidou b1I code rate [chips/s] +const double BEIDOU_B1I_CODE_LENGTH_CHIPS = 2046.0; //!< beidou b1I code length [chips] +const double BEIDOU_B1I_CODE_PERIOD = 0.001; //!< beidou b1I code period [seconds] +const double BEIDOU_B1I_CHIP_PERIOD = 4.8875e-07; //!< beidou b1I chip period [seconds] + +/*! + * \brief Maximum Time-Of-Arrival (TOA) difference between satellites for a receiver operated on Earth surface is 20 ms + * + * According to the GPS orbit model described in [1] Pag. 32. + * It should be taken into account to set the buffer size for the PRN start timestamp in the pseudoranges block. + * [1] J. Bao-Yen Tsui, Fundamentals of Global Positioning System Receivers. A Software Approach, John Wiley & Sons, + * Inc., Hoboken, NJ, 2nd edition, 2005. + */ +const double BEIDOU_MAX_TOA_DELAY_MS = 20; //****************** + +//#define NAVIGATION_SOLUTION_RATE_MS 1000 // this cannot go here +const double BEIDOU_STARTOFFSET_ms = 68.802; //**************[ms] Initial sign. travel time (this cannot go here) + + +// OBSERVABLE HISTORY DEEP FOR INTERPOLATION +const int BEIDOU_B1I_HISTORY_DEEP = 100; // **************** + +// NAVIGATION MESSAGE DEMODULATION AND DECODING + +#define BEIDOU_PREAMBLE {1, 1, 1, 0, 0, 0, 1, 0, 0, 1, 0} +const int BEIDOU_B1I_PREAMBLE_LENGTH_BITS = 11; +const int BEIDOU_B1I_PREAMBLE_LENGTH_SYMBOLS = 160; // ************** +const int BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND = 50; //!< D1 NAV message bit rate [bits/s] +const int BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT = 20; // ************* +const int BEIDOU_B1I_TELEMETRY_RATE_SYMBOLS_SECOND = BEIDOU_B1I_TELEMETRY_RATE_BITS_SECOND*BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //************!< NAV message bit rate [symbols/s] +const int BEIDOU_WORD_LENGTH = 4; //**************!< CRC + BEIDOU WORD (-2 -1 0 ... 29) Bits = 4 bytes +const int BEIDOU_SUBFRAME_LENGTH = 40; //**************!< BEIDOU_WORD_LENGTH x 10 = 40 bytes +const int BEIDOU_SUBFRAME_BITS = 300; //!< Number of bits per subframe in the NAV message [bits] +const int BEIDOU_SUBFRAME_SECONDS = 6; //!< Subframe duration [seconds] +const int BEIDOU_SUBFRAME_MS = 6000; //!< Subframe duration [miliseconds] +const int BEIDOU_WORD_BITS = 30; //!< Number of bits per word in the NAV message [bits] + + +// BEIDOU D1 NAVIGATION MESSAGE STRUCTURE +// GENERAL +const std::vector > D1_PRE( { {1,11} } ); +const std::vector > D1_FRAID( { {16,3} } ); +const std::vector > D1_SOW( { {19,8},{31,12} } ); +// SUBFRAME 1 +const std::vector > D1_SAT_H1( { {43,1} } ); +const std::vector > D1_AODC( { {44,5} } ); +const std::vector > D1_URAI( { {49,4} } ); +const std::vector > D1_WN( { {61,13} } ); +const std::vector > D1_TOC( { {74,9},{91,8} } ); +const double D1_TOC_LSB = TWO_P3; +const std::vector > D1_TGD1( { {99,10} } ); +const double D1_TGD1_LSB = 0.1; +const std::vector > D1_TGD2( { {121,6} } ); +const double D1_TGD2_LSB = 0.1; +const std::vector > D1_ALPHA0( { {127,8} } ); +const double D1_ALPHA0_LSB = TWO_N30; +const std::vector > D1_ALPHA1( { {135,8} } ); +const double D1_ALPHA1_LSB = TWO_N27; +const std::vector > D1_ALPHA2( { {151,8} } ); +const double D1_ALPHA2_LSB = TWO_N24; +const std::vector > D1_ALPHA3( { {159,8} } ); +const double D1_ALPHA3_LSB = TWO_N24; +const std::vector > D1_BETA0( { {167,6}, {181,2} } ); +const double D1_BETA0_LSB = TWO_P11; +const std::vector > D1_BETA1( { {183,8} } ); +const double D1_BETA1_LSB = TWO_P14; +const std::vector > D1_BETA2( { {191,8} } ); +const double D1_BETA2_LSB = TWO_P16; +const std::vector > D1_BETA3( { {199,4},{211,4} } ); +const double D1_BETA3_LSB = TWO_P16; +const std::vector > D1_A2( { {215,11} } ); +const double D1_A2_LSB = TWO_N66; +const std::vector > D1_A0( { {226,7},{241,17} } ); +const double D1_A0_LSB = TWO_N33; +const std::vector > D1_A1( { {258,5},{271,17} } ); +const double D1_A1_LSB = TWO_N50; +const std::vector > D1_AODE( { {288,5} } ); + +//SUBFRAME 2 +const std::vector > D1_DELTA_N( { {43,10},{61,6} } ); +const double D1_DELTA_N_LSB = PI_TWO_N43; +const std::vector > D1_CUC( { {67,16},{91,2} } ); +const double D1_CUC_LSB = TWO_N31; +const std::vector > D1_M0( { {93,20}, {121,12} } ); +const double D1_M0_LSB = PI_TWO_N31; +const std::vector > D1_E( { {133,10},{151,22} } ); +const double D1_E_LSB = TWO_N33; +const std::vector > D1_CUS( { {181,18} } ); +const double D1_CUS_LSB = TWO_N31; +const std::vector > D1_CRC( { {199,4},{211,14} } ); +const double D1_CRC_LSB = TWO_N6; +const std::vector > D1_CRS( { {225,8},{241,10} } ); +const double D1_CRS_LSB = TWO_N6; +const std::vector > D1_SQRT_A( { {251,12},{271,20} } ); +const double D1_SQRT_A_LSB = TWO_N19; +const std::vector > D1_TOE( { {291,2} } ); +const double D1_TOE_LSB = TWO_P3; + +//SUBFRAME 3 +const std::vector > D1_TOE_MSB2( { {43,10},{61,5} } ); +const std::vector > D1_I0( { {66,17},{91,15} } ); +const double D1_I0_LSB = PI_TWO_N31; +const std::vector > D1_CIC( { {106,7},{121,11} } ); +const double D1_CIC_LSB = TWO_N31; +const std::vector > D1_OMEGA_DOT( { {132,11},{151,13} } ); +const double D1_OMEGA_DOT_LSB = PI_TWO_N43; +const std::vector > D1_CIS( { {164,9},{181,9} } ); +const double D1_CIS_LSB = TWO_N31; +const std::vector > D1_IDOT( { {190,13},{211,1} } ); +const double D1_IDOT_LSB = PI_TWO_N43; +const std::vector > D1_OMEGA0( { {212,21},{241,11} } ); +const double D1_OMEGA0_LSB = PI_TWO_N31; +const std::vector > D1_OMEGA( { {252,11},{271,21} } ); +const double D1_OMEGA_LSB = PI_TWO_N31; + +//SUBFRAME 4 AND PAGES 1 THROUGH 6 IN SUBFRAME 5 +const std::vector > D1_SQRT_A_ALMANAC( { {51,2},{61,22} } ); +const double D1_SQRT_A_ALMANAC_LSB = TWO_N11; +const std::vector > D1_A1_ALMANAC( { {91,11} } ); +const double D1_A1_ALMANAC_LSB = TWO_N38; +const std::vector > D1_A0_ALMANAC( { {102,11} } ); +const double D1_A0_ALMANAC_LSB = TWO_N20; +const std::vector > D1_OMEGA0_ALMANAC( { {121,22},{151,2} } ); +const double D1_OMEGA0_ALMANAC_LSB = PI_TWO_N23; +const std::vector > D1_E_ALMANAC( { {153,17} } ); +const double D1_E_ALMANAC_LSB = TWO_N21; +const std::vector > D1_DELTA_I( { {170,3},{181,13} } ); +const double D1_DELTA_I_LSB = PI_TWO_N19; +const std::vector > D1_TOA( { {194,8} } ); +const double D1_TOA_LSB = TWO_P12; +const std::vector > D1_OMEGA_DOT_ALMANAC( { {202,1}, {211,16} } ); +const double D1_OMEGA_DOT_ALMANAC_LSB = PI_TWO_N38; +const std::vector > D1_OMEGA_ALMANAC( { {227,6},{241,18} } ); +const double D1_OMEGA_ALMANAC_LSB = PI_TWO_N23; +const std::vector > D1_M0_ALMANAC( { {259,4},{271,20} } ); +const double D1_M0_ALMANAC_LSB = PI_TWO_N23; + +//SUBFRAME 5 PAGE 7 +const std::vector > D1_HEA1( { {51,2},{61,7} } ); +const std::vector > D1_HEA2( { {68,9} } ); +const std::vector > D1_HEA3( { {77,6},{91,3} } ); +const std::vector > D1_HEA4( { {94,9} } ); +const std::vector > D1_HEA5( { {103,9} } ); +const std::vector > D1_HEA6( { {112,1},{121,8} } ); +const std::vector > D1_HEA7( { {129,9} } ); +const std::vector > D1_HEA8( { {138,5},{151,4} } ); +const std::vector > D1_HEA9( { {155,9} } ); +const std::vector > D1_HEA10( { {164,9} } ); +const std::vector > D1_HEA11( { {181,9} } ); +const std::vector > D1_HEA12( { {190,9} } ); +const std::vector > D1_HEA13( { {199,4},{211,5} } ); +const std::vector > D1_HEA14( { {216,9} } ); +const std::vector > D1_HEA15( { {225,8},{241,1} } ); +const std::vector > D1_HEA16( { {242,9} } ); +const std::vector > D1_HEA17( { {251,9} } ); +const std::vector > D1_HEA18( { {260,3},{271,6} } ); +const std::vector > D1_HEA19( { {277,9} } ); + + + +//SUBFRAME 5 PAGE 8 +const std::vector > D1_HEA20( { {51,2},{61,7} } ); +const std::vector > D1_HEA21( { {68,9} } ); +const std::vector > D1_HEA22( { {77,6},{91,3} } ); +const std::vector > D1_HEA23( { {94,9} } ); +const std::vector > D1_HEA24( { {103,9} } ); +const std::vector > D1_HEA25( { {112,1},{121,8} } ); +const std::vector > D1_HEA26( { {129,9} } ); +const std::vector > D1_HEA27( { {138,5},{151,4} } ); +const std::vector > D1_HEA28( { {155,9} } ); +const std::vector > D1_HEA29( { {164,9} } ); +const std::vector > D1_HEA30( { {181,9} } ); +const std::vector > D1_WNA( { {190,8} } ); +const std::vector > D1_TOA2( { {198,5},{211,3} } ); + +//SUBFRAME 5 PAGE 9 +const std::vector > D1_A0GPS( { {97,14} } ); +const double D1_A0GPS_LSB = 0.1; +const std::vector > D1_A1GPS( { {111,2},{121,14} } ); +const double D1_A1GPS_LSB = 0.1; +const std::vector > D1_A0GAL( { {135,8},{151,6} } ); +const double D1_A0GAL_LSB = 0.1; +const std::vector > D1_A1GAL( { {157,16} } ); +const double D1_A1GAL_LSB = 0.1; +const std::vector > D1_A0GLO( { {181,14} } ); +const double D1_A0GLO_LSB = 0.1; +const std::vector > D1_A1GLO( { {195,8},{211,8} } ); +const double D1_A1GLO_LSB = 0.1; + +//SUBFRAME 5 PAGE 10 +const std::vector > D1_DELTA_T_LS( { {51,2},{61,6} } ); +const std::vector > D1_DELTA_T_LSF( { {67,8} } ); +const std::vector > D1_WN_LSF( { {75,8} } ); +const std::vector > D1_A0UTC( { {91,22},{121,10} } ); +const double D1_A0UTC_LSB = TWO_N30; +const std::vector > D1_A1UTC( { {131,12},{151,12} } ); +const double D1_A1UTC_LSB = TWO_N50; +const std::vector > D1_DN( { {163,8} } ); + +#endif /* GNSS_SDR_BEIDOU_B1I_H_ */ diff --git a/src/core/system_parameters/beidou_ephemeris.cc b/src/core/system_parameters/beidou_ephemeris.cc new file mode 100644 index 000000000..297d94a36 --- /dev/null +++ b/src/core/system_parameters/beidou_ephemeris.cc @@ -0,0 +1,279 @@ +/*! + * \file beidou_ephemeris.cc + * \brief Interface of a BeiDou EPHEMERIS storage and orbital model functions + * + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "beidou_ephemeris.h" +#include +#include "beidou_b1I.h" +#include "gnss_satellite.h" + +Beidou_Ephemeris::Beidou_Ephemeris() +{ + i_satellite_PRN = 0; + d_TOW = 0; + d_Crs = 0; + d_Delta_n = 0; + d_M_0 = 0; + d_Cuc = 0; + d_e_eccentricity = 0; + d_Cus = 0; + d_sqrt_A = 0; + d_Toe = 0; + d_Toc = 0; + d_Cic = 0; + d_OMEGA0 = 0; + d_Cis = 0; + d_i_0 = 0; + d_Crc = 0; + d_OMEGA = 0; + d_OMEGA_DOT = 0; + d_IDOT = 0; + i_BEIDOU_week = 0; + i_SV_accuracy = 0; + i_SV_health = 0; + d_AODE = 0; + d_TGD1 = 0; + d_TGD2 = 0: + d_AODC = 0; // Issue of Data, Clock + i_AODO = 0; // Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s] + + b_fit_interval_flag = false; // indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours. + d_spare1 = 0; + d_spare2 = 0; + + d_A_f0 = 0; // Coefficient 0 of code phase offset model [s] + d_A_f1 = 0; // Coefficient 1 of code phase offset model [s/s] + d_A_f2 = 0; // Coefficient 2 of code phase offset model [s/s^2] + + b_integrity_status_flag = false; + b_alert_flag = false; // If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk. + b_antispoofing_flag = false; // If true, the AntiSpoofing mode is ON in that SV + + auto gnss_sat = Gnss_Satellite(); + std::string _system ("Beidou"); + for(unsigned int i = 1; i < 36; i++) + { + satelliteBlock[i] = gnss_sat.what_block(_system, i); + } + + d_satClkDrift = 0.0; + d_dtr = 0.0; + d_satpos_X = 0.0; + d_satpos_Y = 0.0; + d_satpos_Z = 0.0; + d_satvel_X = 0.0; + d_satvel_Y = 0.0; + d_satvel_Z = 0.0; +} + + +double Beidou_Ephemeris::check_t(double time) +{ + double corrTime; + double half_week = 302400.0; // seconds + corrTime = time; + if (time > half_week) + { + corrTime = time - 2.0 * half_week; + } + else if (time < -half_week) + { + corrTime = time + 2.0 * half_week; + } + return corrTime; +} + + +// 20.3.3.3.3.1 User Algorithm for SV Clock Correction. +double Beidou_Ephemeris::sv_clock_drift(double transmitTime) +{ + double dt; + dt = check_t(transmitTime - d_Toc); + + for (int i = 0; i < 2; i++) + { + dt -= d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); + } + d_satClkDrift = d_A_f0 + d_A_f1 * dt + d_A_f2 * (dt * dt); + + return d_satClkDrift; +} + + +// compute the relativistic correction term +double Beidou_Ephemeris::sv_clock_relativistic_term(double transmitTime) +{ + double tk; + double a; + double n; + double n0; + double E; + double E_old; + double dE; + double M; + + // Restore semi-major axis + a = d_sqrt_A * d_sqrt_A; + + // Time from ephemeris reference epoch + tk = check_t(transmitTime - d_Toe); + + // Computed mean motion + n0 = sqrt(BEIDOU_GM / (a * a * a)); + // Corrected mean motion + n = n0 + d_Delta_n; + // Mean anomaly + M = d_M_0 + n * tk; + + // Reduce mean anomaly to between 0 and 2pi + M = fmod((M + 2.0 * BEIDOU_PI), (2.0 * BEIDOU_PI)); + + // Initial guess of eccentric anomaly + E = M; + + // --- Iteratively compute eccentric anomaly ---------------------------- + for (int ii = 1; ii < 20; ii++) + { + E_old = E; + E = M + d_e_eccentricity * sin(E); + dE = fmod(E - E_old, 2.0 * BEIDOU_PI); + if (fabs(dE) < 1e-12) + { + //Necessary precision is reached, exit from the loop + break; + } + } + + // Compute relativistic correction term + d_dtr = BEIDOU_F * d_e_eccentricity * d_sqrt_A * sin(E); + return d_dtr; +} + + +double Beidou_Ephemeris::satellitePosition(double transmitTime) +{ + double tk; + double a; + double n; + double n0; + double M; + double E; + double E_old; + double dE; + double nu; + double phi; + double u; + double r; + double i; + double Omega; + + // Find satellite's position ---------------------------------------------- + + // Restore semi-major axis + a = d_sqrt_A * d_sqrt_A; + + // Time from ephemeris reference epoch + tk = check_t(transmitTime - d_Toe); + + // Computed mean motion + n0 = sqrt(BEIDOU_GM / (a * a * a)); + + // Corrected mean motion + n = n0 + d_Delta_n; + + // Mean anomaly + M = d_M_0 + n * tk; + + // Reduce mean anomaly to between 0 and 2pi + M = fmod((M + 2.0 * BEIDOU_PI), (2.0 * BEIDOU_PI)); + + // Initial guess of eccentric anomaly + E = M; + + // --- Iteratively compute eccentric anomaly ---------------------------- + for (int ii = 1; ii < 20; ii++) + { + E_old = E; + E = M + d_e_eccentricity * sin(E); + dE = fmod(E - E_old, 2.0 * BEIDOU_PI); + if (fabs(dE) < 1e-12) + { + //Necessary precision is reached, exit from the loop + break; + } + } + + // Compute the true anomaly + double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); + double tmp_X = cos(E) - d_e_eccentricity; + nu = atan2(tmp_Y, tmp_X); + + // Compute angle phi (argument of Latitude) + phi = nu + d_OMEGA; + + // Reduce phi to between 0 and 2*pi rad + phi = fmod((phi), (2.0 * BEIDOU_PI)); + + // Correct argument of latitude + u = phi + d_Cuc * cos(2.0 * phi) + d_Cus * sin(2.0 * phi); + + // Correct radius + r = a * (1.0 - d_e_eccentricity*cos(E)) + d_Crc * cos(2.0 * phi) + d_Crs * sin(2.0 * phi); + + // Correct inclination + i = d_i_0 + d_IDOT * tk + d_Cic * cos(2.0 * phi) + d_Cis * sin(2.0 * phi); + + // Compute the angle between the ascending node and the Greenwich meridian + Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT)*tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe; + + // Reduce to between 0 and 2*pi rad + Omega = fmod((Omega + 2.0 * BEIDOU_PI), (2.0 * BEIDOU_PI)); + + // --- Compute satellite coordinates in Earth-fixed coordinates + d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); + d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); + d_satpos_Z = sin(u) * r * sin(i); + + // Satellite's velocity. Can be useful for Vector Tracking loops + double Omega_dot = d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT; + d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); + d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); + d_satvel_Z = d_satpos_Y * sin(i); + + // Time from ephemeris reference clock + tk = check_t(transmitTime - d_Toc); + + double dtr_s = d_A_f0 + d_A_f1 * tk + d_A_f2 * tk * tk; + + /* relativity correction */ + dtr_s -= 2.0 * sqrt(BEIDOU_GM * a) * d_e_eccentricity * sin(E) / (BEIDOU_C_m_s * BEIDOU_C_m_s); + + return dtr_s; +} diff --git a/src/core/system_parameters/beidou_ephemeris.h b/src/core/system_parameters/beidou_ephemeris.h new file mode 100644 index 000000000..0d2b978ab --- /dev/null +++ b/src/core/system_parameters/beidou_ephemeris.h @@ -0,0 +1,205 @@ +/*! + * \file beidou_ephemeris.h + * \brief Interface of a BEIDOU EPHEMERIS storage + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_BEIDOU_EPHEMERIS_H_ +#define GNSS_SDR_BEIDOU_EPHEMERIS_H_ + + +#include +#include +#include "boost/assign.hpp" +#include + + + +/*! + * \brief This class is a storage and orbital model functions for the GPS SV ephemeris data as described in IS-GPS-200E + * + * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + */ +class Beidou_Ephemeris +{ +private: + /* + * Accounts for the beginning or end of week crossover + * + * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) + * \param[in] - time in seconds + * \param[out] - corrected time, in seconds + */ + double check_t(double time); +public: + unsigned int i_satellite_PRN; // SV PRN NUMBER + double d_TOW; //!< Time of BEIDOU Week of the ephemeris set (taken from subframes TOW) [s] + double d_Crs; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] + double d_Delta_n; //!< Mean Motion Difference From Computed Value [semi-circles/s] + double d_M_0; //!< Mean Anomaly at Reference Time [semi-circles] + double d_Cuc; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] + double d_e_eccentricity; //!< Eccentricity [dimensionless] + double d_Cus; //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] + double d_sqrt_A; //!< Square Root of the Semi-Major Axis [sqrt(m)] + double d_Toe; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + double d_Toc; //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] + double d_Cic; //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] + double d_OMEGA0; //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] + double d_Cis; //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] + double d_i_0; //!< Inclination Angle at Reference Time [semi-circles] + double d_Crc; //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] + double d_OMEGA; //!< Argument of Perigee [semi-cicles] + double d_OMEGA_DOT; //!< Rate of Right Ascension [semi-circles/s] + double d_IDOT; //!< Rate of Inclination Angle [semi-circles/s] + int i_BEIDOU_week; //!< BEIDOU week number, aka WN [week] + int i_SV_accuracy; //!< User Range Accuracy (URA) index of the SV (reference paragraph 6.2.1) for the standard positioning service user (Ref 20.3.3.3.1.3 IS-GPS-200E) + int i_SV_health; + double d_TGD1; //!< Estimated Group Delay Differential on B1I [s] + double d_TGD2; //!< Estimated Group Delay Differential on B2I [s] + double d_AODC; //!< Age of Data, Clock + double d_AODE; //!< Age of Data, Ephemeris + int i_AODO; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s] + + bool b_fit_interval_flag;//!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours. + double d_spare1; + double d_spare2; + + double d_A_f0; //!< Coefficient 0 of code phase offset model [s] + double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s] + double d_A_f2; //!< Coefficient 2 of code phase offset model [s/s^2] + + // Flags + + /*! \brief If true, enhanced level of integrity assurance. + * + * If false, indicates that the conveying signal is provided with the legacy level of integrity assurance. + * That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound + * value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less + * than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of + * integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73 + * times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an + * accompanying alert, is less than 1E-8 per hour. + */ + bool b_integrity_status_flag; + bool b_alert_flag; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk. + bool b_antispoofing_flag; //!< If true, the AntiSpoofing mode is ON in that SV + + // clock terms derived from ephemeris data + double d_satClkDrift; //!< GPS clock error + double d_dtr; //!< relativistic clock correction term + + // satellite positions + double d_satpos_X; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis. + double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system. + double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP). + + // Satellite velocity + double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m] + double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m] + double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m] + + std::map satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus + + template + + /*! + * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file. + */ + void serialize(Archive& archive, const unsigned int version) + { + using boost::serialization::make_nvp; + if(version){}; + + archive & make_nvp("i_satellite_PRN", i_satellite_PRN); // SV PRN NUMBER + archive & make_nvp("d_TOW", d_TOW); //!< Time of GPS Week of the ephemeris set (taken from subframes TOW) [s] + archive & make_nvp("d_AODE", d_AODE); + archive & make_nvp("d_Crs", d_Crs); //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] + archive & make_nvp("d_Delta_n", d_Delta_n); //!< Mean Motion Difference From Computed Value [semi-circles/s] + archive & make_nvp("d_M_0", d_M_0); //!< Mean Anomaly at Reference Time [semi-circles] + archive & make_nvp("d_Cuc", d_Cuc); //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] + archive & make_nvp("d_e_eccentricity", d_e_eccentricity); //!< Eccentricity [dimensionless] + archive & make_nvp("d_Cus", d_Cus); //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] + archive & make_nvp("d_sqrt_A", d_sqrt_A); //!< Square Root of the Semi-Major Axis [sqrt(m)] + archive & make_nvp("d_Toe", d_Toe); //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + archive & make_nvp("d_Toc", d_Toe); //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] + archive & make_nvp("d_Cic", d_Cic); //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] + archive & make_nvp("d_OMEGA0", d_OMEGA0); //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] + archive & make_nvp("d_Cis", d_Cis); //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] + archive & make_nvp("d_i_0", d_i_0); //!< Inclination Angle at Reference Time [semi-circles] + archive & make_nvp("d_Crc", d_Crc); //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] + archive & make_nvp("d_OMEGA", d_OMEGA); //!< Argument of Perigee [semi-cicles] + archive & make_nvp("d_OMEGA_DOT", d_OMEGA_DOT); //!< Rate of Right Ascension [semi-circles/s] + archive & make_nvp("d_IDOT", d_IDOT); //!< Rate of Inclination Angle [semi-circles/s] + archive & make_nvp("i_BEIDOU_week", i_GPS_week); //!< GPS week number, aka WN [week] + archive & make_nvp("i_SV_accuracy", i_SV_accuracy); //!< User Range Accuracy (URA) index of the SV (reference paragraph 6.2.1) for the standard positioning service user (Ref 20.3.3.3.1.3 IS-GPS-200E) + archive & make_nvp("i_SV_health", i_SV_health); + archive & make_nvp("d_IODC", d_IODC); //!< Issue of Data, Clock + archive & make_nvp("d_TGD1", d_TGD1); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive & make_nvp("d_TGD2", d_TGD2); //!< Estimated Group Delay Differential: L1-L2 correction term only for the benefit of "L1 P(Y)" or "L2 P(Y)" s users [s] + archive & make_nvp("i_AODO", i_AODO); //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s] + + archive & make_nvp("b_fit_interval_flag", b_fit_interval_flag);//!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours. + archive & make_nvp("d_spare1", d_spare1); + archive & make_nvp("d_spare2", d_spare2); + + archive & make_nvp("d_A_f0", d_A_f0); //!< Coefficient 0 of code phase offset model [s] + archive & make_nvp("d_A_f1", d_A_f1); //!< Coefficient 1 of code phase offset model [s/s] + archive & make_nvp("d_A_f2", d_A_f2); //!< Coefficient 2 of code phase offset model [s/s^2] + + archive & make_nvp("b_integrity_status_flag", b_integrity_status_flag); + archive & make_nvp("b_alert_flag", b_alert_flag); //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk. + archive & make_nvp("b_antispoofing_flag", b_antispoofing_flag); //!< If true, the AntiSpoofing mode is ON in that SV + } + + /*! + * \brief Compute the ECEF SV coordinates and ECEF velocity + * Implementation of Table 20-IV (IS-GPS-200E) + * and compute the clock bias term including relativistic effect (return value) + */ + double satellitePosition(double transmitTime); + + /*! + * \brief Sets (\a d_satClkDrift)and returns the clock drift in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_drift(double transmitTime); + + /*! + * \brief Sets (\a d_dtr) and returns the clock relativistic correction term in seconds according to the User Algorithm for SV Clock Correction + * (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_relativistic_term(double transmitTime); + + + /*! + * Default constructor + */ + Gps_Ephemeris(); +}; + +#endif diff --git a/src/core/system_parameters/beidou_iono.cc b/src/core/system_parameters/beidou_iono.cc new file mode 100644 index 000000000..f2312b79c --- /dev/null +++ b/src/core/system_parameters/beidou_iono.cc @@ -0,0 +1,46 @@ +/*! + * \file beidou_iono.cc + * \brief Interface of a BEIDOU IONOSPHERIC MODEL storage + * + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "beidou_iono.h" + +Beidou_Iono::Beidou_Iono() +{ + valid = false; + d_alpha0 = 0.0; + d_alpha1 = 0.0; + d_alpha2 = 0.0; + d_alpha3 = 0.0; + d_beta0 = 0.0; + d_beta1 = 0.0; + d_beta2 = 0.0; + d_beta3 = 0.0; +} + diff --git a/src/core/system_parameters/beidou_iono.h b/src/core/system_parameters/beidou_iono.h new file mode 100644 index 000000000..0c275541f --- /dev/null +++ b/src/core/system_parameters/beidou_iono.h @@ -0,0 +1,80 @@ +/*! + * \file beidou_iono.h + * \brief Interface of a BEIDOU IONOSPHERIC MODEL storage + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_BEIDOU_IONO_H_ +#define GNSS_SDR_BEIDOU_IONO_H_ + + +#include "boost/assign.hpp" +#include + + +/*! + * \brief This class is a storage for the BEIDOU IONOSPHERIC data as described in ICD v2.1 + * + */ +class Beidou_Iono +{ +public: + bool valid; //!< Valid flag + // Ionospheric parameters + double d_alpha0; //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s] + double d_alpha1; //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle] + double d_alpha2; //!< Coefficient 2 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^2] + double d_alpha3; //!< Coefficient 3 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^3] + double d_beta0; //!< Coefficient 0 of a cubic equation representing the period of the model [s] + double d_beta1; //!< Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle] + double d_beta2; //!< Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2] + double d_beta3; //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3] + + Beidou_Iono(); //!< Default constructor + + template + + /*! + * \brief Serialize is a boost standard method to be called by the boost XML serialization. Here is used to save the ephemeris data on disk file. + */ + void serialize(Archive& archive, const unsigned int version) + { + using boost::serialization::make_nvp; + if(version){}; + archive & make_nvp("d_alpha0",d_alpha0); + archive & make_nvp("d_alpha1",d_alpha1); + archive & make_nvp("d_alpha2",d_alpha2); + archive & make_nvp("d_alpha3",d_alpha3); + archive & make_nvp("d_beta0",d_beta0); + archive & make_nvp("d_beta1",d_beta1); + archive & make_nvp("d_beta2",d_beta2); + archive & make_nvp("d_beta3",d_beta3); + } +}; + +#endif diff --git a/src/core/system_parameters/beidou_navigation_message.cc b/src/core/system_parameters/beidou_navigation_message.cc new file mode 100644 index 000000000..d6b906dd9 --- /dev/null +++ b/src/core/system_parameters/beidou_navigation_message.cc @@ -0,0 +1,804 @@ +/*! +m * \file beidou_navigation_message.cc + * \brief Implementation of a BeiDou D1 NAV Data message decoder as described in BeiDou ICD Version 2.1 + * + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + +#include "beidou_navigation_message.h" +#include +#include +#include + + +void Beidou_Navigation_Message::reset() +{ + b_valid_ephemeris_set_flag = false; + d_TOW = 0; + d_TOW_SF1 = 0; + d_TOW_SF2 = 0; + d_TOW_SF3 = 0; + d_TOW_SF4 = 0; + d_TOW_SF5 = 0; + d_AODE = 0; + d_Crs = 0; + d_Delta_n = 0; + d_M_0 = 0; + d_Cuc = 0; + d_e_eccentricity = 0; + d_Cus = 0; + d_sqrt_A = 0; + d_Toe = 0; + d_Toc = 0; + d_Cic = 0; + d_OMEGA0 = 0; + d_Cis = 0; + d_i_0 = 0; + d_Crc = 0; + d_OMEGA = 0; + d_OMEGA_DOT = 0; + d_IDOT = 0; + i_BEIDOU_week = 0; + i_SV_accuracy = 0; + i_SV_health = 0; + d_TGD = 0; + d_AODC = -1; +// i_AODO = 0; + + b_fit_interval_flag = false; + d_spare1 = 0; + d_spare2 = 0; + + d_A_f0 = 0; + d_A_f1 = 0; + d_A_f2 = 0; + + //clock terms + //d_master_clock=0; + d_dtr = 0; + d_satClkCorr = 0; + d_satClkDrift = 0; + + // satellite positions + d_satpos_X = 0; + d_satpos_Y = 0; + d_satpos_Z = 0; + + // info + i_channel_ID = 0; + i_satellite_PRN = 0; + + // time synchro + d_subframe_timestamp_ms = 0; + + // flags + b_alert_flag = false; + b_integrity_status_flag = false; + b_antispoofing_flag = false; + + // Ionosphere and UTC + flag_iono_valid = false; + flag_utc_model_valid = false; + d_alpha0 = 0; + d_alpha1 = 0; + d_alpha2 = 0; + d_alpha3 = 0; + d_beta0 = 0; + d_beta1 = 0; + d_beta2 = 0; + d_beta3 = 0; + d_A1 = 0; + d_A0 = 0; + d_t_OT = 0; + i_WN_T = 0; + d_DeltaT_LS = 0; + i_WN_LSF = 0; + i_DN = 0; + d_DeltaT_LSF= 0; + + //Almanac + d_Toa = 0; + i_WN_A = 0; + for (int i=1; i < 36; i++ ) + { + almanacHealth[i] = 0; + } + + // Satellite velocity + d_satvel_X = 0; + d_satvel_Y = 0; + d_satvel_Z = 0; + + auto gnss_sat = Gnss_Satellite(); + std::string _system ("Beidou"); + for(unsigned int i = 1; i < 36; i++) + { + satelliteBlock[i] = gnss_sat.what_block(_system, i); + } +} + + + +Beidou_Navigation_Message::Gps_Navigation_Message() +{ + reset(); +} + + + +void Beidou_Navigation_Message::print_beidou_word_bytes(unsigned int BEIDOU_word) +{ + std::cout << " Word ="; + std::cout << std::bitset<32>(BEIDOU_word); + std::cout << std::endl; +} + + + +bool Beidou_Navigation_Message::read_navigation_bool(std::bitset bits, const std::vector> parameter) +{ + bool value; + + if (bits[BEIDOU_SUBFRAME_BITS - parameter[0].first] == 1) + { + value = true; + } + else + { + value = false; + } + return value; +} + + +unsigned long int Beidou_Navigation_Message::read_navigation_unsigned(std::bitset bits, const std::vector> parameter) +{ + unsigned long int value = 0; + int num_of_slices = parameter.size(); + for (int i = 0; i < num_of_slices; i++) + { + for (int j = 0; j < parameter[i].second; j++) + { + value <<= 1; //shift left + if (bits[BEIDOU_SUBFRAME_BITS - parameter[i].first - j] == 1) + { + value += 1; // insert the bit + } + } + } + return value; +} + +signed long int Beidou_Navigation_Message::read_navigation_signed(std::bitset bits, const std::vector> parameter) +{ + signed long int value = 0; + int num_of_slices = parameter.size(); + // Discriminate between 64 bits and 32 bits compiler + int long_int_size_bytes = sizeof(signed long int); + if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system + { + // read the MSB and perform the sign extension + if (bits[BEIDOU_SUBFRAME_BITS - parameter[0].first] == 1) + { + value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable + } + else + { + value &= 0; + } + + for (int i = 0; i < num_of_slices; i++) + { + for (int j = 0; j < parameter[i].second; j++) + { + value <<= 1; //shift left + value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable) + if (bits[BEIDOU_SUBFRAME_BITS - parameter[i].first - j] == 1) + { + value += 1; // insert the bit + } + } + } + } + else // we assume we are in a 32 bits system + { + // read the MSB and perform the sign extension + if (bits[BEIDOU_SUBFRAME_BITS - parameter[0].first] == 1) + { + value ^= 0xFFFFFFFF; + } + else + { + value &= 0; + } + + for (int i = 0; i < num_of_slices; i++) + { + for (int j = 0; j < parameter[i].second; j++) + { + value <<= 1; //shift left + value &= 0xFFFFFFFE; //reset the corresponding bit + if (bits[BEIDOU_SUBFRAME_BITS - parameter[i].first - j] == 1) + { + value += 1; // insert the bit + } + } + } + } + return value; +} + +double Beidou_Navigation_Message::check_t(double time) +{ + double corrTime; + double half_week = 302400; // seconds + corrTime = time; + if (time > half_week) + { + corrTime = time - 2 * half_week; + } + else if (time < -half_week) + { + corrTime = time + 2 * half_week; + } + return corrTime; +} + +// User Algorithm for SV Clock Correction. +double Beidou_Navigation_Message::sv_clock_correction(double transmitTime) +{ + double dt; + dt = check_t(transmitTime - d_Toc); + d_satClkCorr = (d_A_f2 * dt + d_A_f1) * dt + d_A_f0 + d_dtr; + double correctedTime = transmitTime - d_satClkCorr; + return correctedTime; +} + +void Beidou_Navigation_Message::satellitePosition(double transmitTime) +{ + double tk; + double a; + double n; + double n0; + double M; + double E; + double E_old; + double dE; + double nu; + double phi; + double u; + double r; + double i; + double Omega; + + // Find satellite's position ---------------------------------------------- + + // Restore semi-major axis + a = d_sqrt_A * d_sqrt_A; + + // Time from ephemeris reference epoch + tk = check_t(transmitTime - d_Toe); + + // Computed mean motion + n0 = sqrt(BEIDOU_GM / (a * a * a)); + + // Corrected mean motion + n = n0 + d_Delta_n; + + // Mean anomaly + M = d_M_0 + n * tk; + + // Reduce mean anomaly to between 0 and 2pi + M = fmod((M + 2 * BEIDOU_PI), (2 * BEIDOU_PI)); + + // Initial guess of eccentric anomaly + E = M; + + // --- Iteratively compute eccentric anomaly ---------------------------- + for (int ii = 1; ii < 20; ii++) + { + E_old = E; + E = M + d_e_eccentricity * sin(E); + dE = fmod(E - E_old, 2 * BEIDOU_PI); + if (fabs(dE) < 1e-12) + { + //Necessary precision is reached, exit from the loop + break; + } + } + + // Compute relativistic correction term + d_dtr = BEIDOU_F * d_e_eccentricity * d_sqrt_A * sin(E); + + // Compute the true anomaly + double tmp_Y = sqrt(1.0 - d_e_eccentricity * d_e_eccentricity) * sin(E); + double tmp_X = cos(E) - d_e_eccentricity; + nu = atan2(tmp_Y, tmp_X); + + // Compute angle phi (argument of Latitude) + phi = nu + d_OMEGA; + + // Reduce phi to between 0 and 2*pi rad + phi = fmod((phi), (2 * BEIDOU_PI)); + + // Correct argument of latitude + u = phi + d_Cuc * cos(2 * phi) + d_Cus * sin(2 * phi); + + // Correct radius + r = a * (1 - d_e_eccentricity * cos(E)) + d_Crc * cos(2 * phi) + d_Crs * sin(2 * phi); + + // Correct inclination + i = d_i_0 + d_IDOT * tk + d_Cic * cos(2 * phi) + d_Cis * sin(2 * phi); + + // Compute the angle between the ascending node and the Greenwich meridian + Omega = d_OMEGA0 + (d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT) * tk - BEIDOU_OMEGA_EARTH_DOT * d_Toe; + + // Reduce to between 0 and 2*pi rad + Omega = fmod((Omega + 2 * BEIDOU_PI), (2 * BEIDOU_PI)); + + // --- Compute satellite coordinates in Earth-fixed coordinates + d_satpos_X = cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega); + d_satpos_Y = cos(u) * r * sin(Omega) + sin(u) * r * cos(i) * cos(Omega); + d_satpos_Z = sin(u) * r * sin(i); + + // Satellite's velocity. Can be useful for Vector Tracking loops + double Omega_dot = d_OMEGA_DOT - BEIDOU_OMEGA_EARTH_DOT; + d_satvel_X = - Omega_dot * (cos(u) * r + sin(u) * r * cos(i)) + d_satpos_X * cos(Omega) - d_satpos_Y * cos(i) * sin(Omega); + d_satvel_Y = Omega_dot * (cos(u) * r * cos(Omega) - sin(u) * r * cos(i) * sin(Omega)) + d_satpos_X * sin(Omega) + d_satpos_Y * cos(i) * cos(Omega); + d_satvel_Z = d_satpos_Y * sin(i); +} + + + + + +int Beidou_Navigation_Message::subframe_decoder(char *subframe) +{ + int subframe_ID = 0; + + //double tmp_TOW; + + unsigned int beidou_word; + + // UNPACK BYTES TO BITS AND REMOVE THE CRC REDUNDANCE + std::bitset subframe_bits; + std::bitset word_bits; + for (int i = 0; i < 10; i++) + { + memcpy(&beidou_word, &subframe[i * 4], sizeof(char) * 4); + word_bits = std::bitset<(BEIDOU_WORD_BITS + 2) > (beidou_word); + for (int j = 0; j < BEIDOU_WORD_BITS; j++) + { + subframe_bits[BEIDOU_WORD_BITS * (9 - i) + j] = word_bits[j]; + } + } + + subframe_ID = static_cast(read_navigation_unsigned(subframe_bits, SUBFRAME_ID)); + + // Decode all 5 sub-frames + switch (subframe_ID) + { + //--- Decode the sub-frame id ------------------------------------------ + case 1: + //--- It is subframe 1 ------------------------------------- + // Compute the time of week (TOW) of the first sub-frames in the array ==== + // The transmitted TOW is actual TOW of the next subframe + // (the variable subframe at this point contains bits of the last subframe). + //TOW = bin2dec(subframe(31:47)) * 6; + d_TOW_SF1 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + //we are in the first subframe (the transmitted TOW is the start time of the next subframe) ! + d_TOW_SF1 = d_TOW_SF1 * 6; + d_TOW = d_TOW_SF1; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + i_GPS_week = static_cast(read_navigation_unsigned(subframe_bits, GPS_WEEK)); + i_SV_accuracy = static_cast(read_navigation_unsigned(subframe_bits, SV_ACCURACY)); // (20.3.3.3.1.3) + i_SV_health = static_cast(read_navigation_unsigned(subframe_bits, SV_HEALTH)); + b_L2_P_data_flag = read_navigation_bool(subframe_bits, L2_P_DATA_FLAG); // + i_code_on_L2 = static_cast(read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2)); + d_TGD = static_cast(read_navigation_signed(subframe_bits, T_GD)); + d_TGD = d_TGD * T_GD_LSB; + d_IODC = static_cast(read_navigation_unsigned(subframe_bits, IODC)); + d_Toc = static_cast(read_navigation_unsigned(subframe_bits, T_OC)); + d_Toc = d_Toc * T_OC_LSB; + d_A_f0 = static_cast(read_navigation_signed(subframe_bits, A_F0)); + d_A_f0 = d_A_f0 * A_F0_LSB; + d_A_f1 = static_cast(read_navigation_signed(subframe_bits, A_F1)); + d_A_f1 = d_A_f1 * A_F1_LSB; + d_A_f2 = static_cast(read_navigation_signed(subframe_bits, A_F2)); + d_A_f2 = d_A_f2 * A_F2_LSB; + + break; + + case 2: //--- It is subframe 2 ------------------- + d_TOW_SF2 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + d_TOW_SF2 = d_TOW_SF2 * 6; + d_TOW = d_TOW_SF2; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + d_IODE_SF2 = static_cast(read_navigation_unsigned(subframe_bits, IODE_SF2)); + d_Crs = static_cast(read_navigation_signed(subframe_bits, C_RS)); + d_Crs = d_Crs * C_RS_LSB; + d_Delta_n = static_cast(read_navigation_signed(subframe_bits, DELTA_N)); + d_Delta_n = d_Delta_n * DELTA_N_LSB; + d_M_0 = static_cast(read_navigation_signed(subframe_bits, M_0)); + d_M_0 = d_M_0 * M_0_LSB; + d_Cuc = static_cast(read_navigation_signed(subframe_bits, C_UC)); + d_Cuc = d_Cuc * C_UC_LSB; + d_e_eccentricity = static_cast(read_navigation_unsigned(subframe_bits, E)); + d_e_eccentricity = d_e_eccentricity * E_LSB; + d_Cus = static_cast(read_navigation_signed(subframe_bits, C_US)); + d_Cus = d_Cus * C_US_LSB; + d_sqrt_A = static_cast(read_navigation_unsigned(subframe_bits, SQRT_A)); + d_sqrt_A = d_sqrt_A * SQRT_A_LSB; + d_Toe = static_cast(read_navigation_unsigned(subframe_bits, T_OE)); + d_Toe = d_Toe * T_OE_LSB; + b_fit_interval_flag = read_navigation_bool(subframe_bits, FIT_INTERVAL_FLAG); + i_AODO = static_cast(read_navigation_unsigned(subframe_bits, AODO)); + i_AODO = i_AODO * AODO_LSB; + + break; + + case 3: // --- It is subframe 3 ------------------------------------- + d_TOW_SF3 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + d_TOW_SF3 = d_TOW_SF3 * 6; + d_TOW = d_TOW_SF3; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + d_Cic = static_cast(read_navigation_signed(subframe_bits, C_IC)); + d_Cic = d_Cic * C_IC_LSB; + d_OMEGA0 = static_cast(read_navigation_signed(subframe_bits, OMEGA_0)); + d_OMEGA0 = d_OMEGA0 * OMEGA_0_LSB; + d_Cis = static_cast(read_navigation_signed(subframe_bits, C_IS)); + d_Cis = d_Cis * C_IS_LSB; + d_i_0 = static_cast(read_navigation_signed(subframe_bits, I_0)); + d_i_0 = d_i_0 * I_0_LSB; + d_Crc = static_cast(read_navigation_signed(subframe_bits, C_RC)); + d_Crc = d_Crc * C_RC_LSB; + d_OMEGA = static_cast(read_navigation_signed(subframe_bits, OMEGA)); + d_OMEGA = d_OMEGA * OMEGA_LSB; + d_OMEGA_DOT = static_cast(read_navigation_signed(subframe_bits, OMEGA_DOT)); + d_OMEGA_DOT = d_OMEGA_DOT * OMEGA_DOT_LSB; + d_IODE_SF3 = static_cast(read_navigation_unsigned(subframe_bits, IODE_SF3)); + d_IDOT = static_cast(read_navigation_signed(subframe_bits, I_DOT)); + d_IDOT = d_IDOT * I_DOT_LSB; + + break; + + case 4: // --- It is subframe 4 ---------- Almanac, ionospheric model, UTC parameters, SV health (PRN: 25-32) + int SV_data_ID; + int SV_page; + d_TOW_SF4 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + d_TOW_SF4 = d_TOW_SF4 * 6; + d_TOW = d_TOW_SF4; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + SV_data_ID = static_cast(read_navigation_unsigned(subframe_bits, SV_DATA_ID)); + SV_page = static_cast(read_navigation_unsigned(subframe_bits, SV_PAGE)); + if (SV_page > 24 && SV_page < 33) // Page 4 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) + { + //! \TODO read almanac + if(SV_data_ID){} + } + + if (SV_page == 52) // Page 13 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) + { + //! \TODO read Estimated Range Deviation (ERD) values + } + + if (SV_page == 56) // Page 18 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) + { + // Page 18 - Ionospheric and UTC data + d_alpha0 = static_cast(read_navigation_signed(subframe_bits, ALPHA_0)); + d_alpha0 = d_alpha0 * ALPHA_0_LSB; + d_alpha1 = static_cast(read_navigation_signed(subframe_bits, ALPHA_1)); + d_alpha1 = d_alpha1 * ALPHA_1_LSB; + d_alpha2 = static_cast(read_navigation_signed(subframe_bits, ALPHA_2)); + d_alpha2 = d_alpha2 * ALPHA_2_LSB; + d_alpha3 = static_cast(read_navigation_signed(subframe_bits, ALPHA_3)); + d_alpha3 = d_alpha3 * ALPHA_3_LSB; + d_beta0 = static_cast(read_navigation_signed(subframe_bits, BETA_0)); + d_beta0 = d_beta0 * BETA_0_LSB; + d_beta1 = static_cast(read_navigation_signed(subframe_bits, BETA_1)); + d_beta1 = d_beta1 * BETA_1_LSB; + d_beta2 = static_cast(read_navigation_signed(subframe_bits, BETA_2)); + d_beta2 = d_beta2 * BETA_2_LSB; + d_beta3 = static_cast(read_navigation_signed(subframe_bits, BETA_3)); + d_beta3 = d_beta3 * BETA_3_LSB; + d_A1 = static_cast(read_navigation_signed(subframe_bits, A_1)); + d_A1 = d_A1 * A_1_LSB; + d_A0 = static_cast(read_navigation_signed(subframe_bits, A_0)); + d_A0 = d_A0 * A_0_LSB; + d_t_OT = static_cast(read_navigation_unsigned(subframe_bits, T_OT)); + d_t_OT = d_t_OT * T_OT_LSB; + i_WN_T = static_cast(read_navigation_unsigned(subframe_bits, WN_T)); + d_DeltaT_LS = static_cast(read_navigation_signed(subframe_bits, DELTAT_LS)); + i_WN_LSF = static_cast(read_navigation_unsigned(subframe_bits, WN_LSF)); + i_DN = static_cast(read_navigation_unsigned(subframe_bits, DN)); // Right-justified ? + d_DeltaT_LSF = static_cast(read_navigation_signed(subframe_bits, DELTAT_LSF)); + flag_iono_valid = true; + flag_utc_model_valid = true; + } + if (SV_page == 57) + { + // Reserved + } + + if (SV_page == 63) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) + { + // Page 25 Anti-Spoofing, SV config and almanac health (PRN: 25-32) + //! \TODO Read Anti-Spoofing, SV config + almanacHealth[25] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV25)); + almanacHealth[26] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV26)); + almanacHealth[27] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV27)); + almanacHealth[28] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV28)); + almanacHealth[29] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV29)); + almanacHealth[30] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV30)); + almanacHealth[31] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV31)); + almanacHealth[32] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV32)); + } + + break; + + case 5://--- It is subframe 5 -----------------almanac health (PRN: 1-24) and Almanac reference week number and time. + int SV_data_ID_5; + int SV_page_5; + d_TOW_SF5 = static_cast(read_navigation_unsigned(subframe_bits, TOW)); + d_TOW_SF5 = d_TOW_SF5 * 6; + d_TOW = d_TOW_SF5; // Set transmission time + b_integrity_status_flag = read_navigation_bool(subframe_bits, INTEGRITY_STATUS_FLAG); + b_alert_flag = read_navigation_bool(subframe_bits, ALERT_FLAG); + b_antispoofing_flag = read_navigation_bool(subframe_bits, ANTI_SPOOFING_FLAG); + SV_data_ID_5 = static_cast(read_navigation_unsigned(subframe_bits, SV_DATA_ID)); + SV_page_5 = static_cast(read_navigation_unsigned(subframe_bits, SV_PAGE)); + if (SV_page_5 < 25) + { + //! \TODO read almanac + if(SV_data_ID_5){} + } + if (SV_page_5 == 51) // Page 25 (from Table 20-V. Data IDs and SV IDs in Subframes 4 and 5, IS-GPS-200H, page 110) + { + d_Toa = static_cast(read_navigation_unsigned(subframe_bits, T_OA)); + d_Toa = d_Toa * T_OA_LSB; + i_WN_A = static_cast(read_navigation_unsigned(subframe_bits, WN_A)); + almanacHealth[1] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV1)); + almanacHealth[2] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV2)); + almanacHealth[3] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV3)); + almanacHealth[4] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV4)); + almanacHealth[5] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV5)); + almanacHealth[6] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV6)); + almanacHealth[7] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV7)); + almanacHealth[8] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV8)); + almanacHealth[9] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV9)); + almanacHealth[10] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV10)); + almanacHealth[11] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV11)); + almanacHealth[12] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV12)); + almanacHealth[13] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV13)); + almanacHealth[14] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV14)); + almanacHealth[15] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV15)); + almanacHealth[16] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV16)); + almanacHealth[17] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV17)); + almanacHealth[18] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV18)); + almanacHealth[19] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV19)); + almanacHealth[20] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV20)); + almanacHealth[21] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV21)); + almanacHealth[22] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV22)); + almanacHealth[23] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV23)); + almanacHealth[24] = static_cast(read_navigation_unsigned(subframe_bits, HEALTH_SV24)); + } + break; + + default: + break; + } // switch subframeID ... + + return subframe_ID; +} + + + + +double Beidou_Navigation_Message::utc_time(const double gpstime_corrected) const +{ + double t_utc; + double t_utc_daytime; + double Delta_t_UTC = d_DeltaT_LS + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast((i_GPS_week - i_WN_T))); + + // Determine if the effectivity time of the leap second event is in the past + int weeksToLeapSecondEvent = i_WN_LSF - i_GPS_week; + + if ((weeksToLeapSecondEvent) >= 0) // is not in the past + { + //Detect if the effectivity time and user's time is within six hours = 6 * 60 *60 = 21600 s + int secondOfLeapSecondEvent = i_DN * 24 * 60 * 60; + if (weeksToLeapSecondEvent > 0) + { + t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); + } + else //we are in the same week than the leap second event + { + if (std::abs(gpstime_corrected - secondOfLeapSecondEvent) > 21600) + { + /* 20.3.3.5.2.4a + * Whenever the effectivity time indicated by the WN_LSF and the DN values + * is not in the past (relative to the user's present time), and the user's + * present time does not fall in the time span which starts at six hours prior + * to the effectivity time and ends at six hours after the effectivity time, + * the UTC/GPS-time relationship is given by + */ + t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); + } + else + { + /* 20.3.3.5.2.4b + * Whenever the user's current time falls within the time span of six hours + * prior to the effectivity time to six hours after the effectivity time, + * proper accommodation of the leap second event with a possible week number + * transition is provided by the following expression for UTC: + */ + int W = fmod(gpstime_corrected - Delta_t_UTC - 43200, 86400) + 43200; + t_utc_daytime = fmod(W, 86400 + d_DeltaT_LSF - d_DeltaT_LS); + //implement something to handle a leap second event! + } + if ( (gpstime_corrected - secondOfLeapSecondEvent) > 21600) + { + Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast((i_GPS_week - i_WN_T))); + t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); + } + } + } + else // the effectivity time is in the past + { + /* 20.3.3.5.2.4c + * Whenever the effectivity time of the leap second event, as indicated by the + * WNLSF and DN values, is in the "past" (relative to the user's current time), + * and the user�s current time does not fall in the time span as given above + * in 20.3.3.5.2.4b,*/ + Delta_t_UTC = d_DeltaT_LSF + d_A0 + d_A1 * (gpstime_corrected - d_t_OT + 604800 * static_cast((i_GPS_week - i_WN_T))); + t_utc_daytime = fmod(gpstime_corrected - Delta_t_UTC, 86400); + } + + double secondsOfWeekBeforeToday = 43200 * floor(gpstime_corrected / 43200); + t_utc = secondsOfWeekBeforeToday + t_utc_daytime; + return t_utc; +} + + + +Gps_Ephemeris Beidou_Navigation_Message::get_ephemeris() +{ + Gps_Ephemeris ephemeris; + ephemeris.i_satellite_PRN = i_satellite_PRN; + ephemeris.d_TOW = d_TOW; + ephemeris.d_Crs = d_Crs; + ephemeris.d_Delta_n = d_Delta_n; + ephemeris.d_M_0 = d_M_0; + ephemeris.d_Cuc = d_Cuc; + ephemeris.d_e_eccentricity = d_e_eccentricity; + ephemeris.d_Cus = d_Cus; + ephemeris.d_sqrt_A = d_sqrt_A; + ephemeris.d_Toe = d_Toe; + ephemeris.d_Toc = d_Toc; + ephemeris.d_Cic = d_Cic; + ephemeris.d_OMEGA0 = d_OMEGA0; + ephemeris.d_Cis = d_Cis; + ephemeris.d_i_0 = d_i_0; + ephemeris.d_Crc = d_Crc; + ephemeris.d_OMEGA = d_OMEGA; + ephemeris.d_OMEGA_DOT = d_OMEGA_DOT; + ephemeris.d_IDOT = d_IDOT; + ephemeris.i_code_on_L2 = i_code_on_L2; + ephemeris.i_GPS_week = i_GPS_week; + ephemeris.b_L2_P_data_flag = b_L2_P_data_flag; + ephemeris.i_SV_accuracy = i_SV_accuracy; + ephemeris.i_SV_health = i_SV_health; + ephemeris.d_TGD = d_TGD; + ephemeris.d_IODC = d_IODC; + ephemeris.d_IODE_SF2 = d_IODE_SF2; + ephemeris.d_IODE_SF3 = d_IODE_SF3; + ephemeris.i_AODO = i_AODO; + ephemeris.b_fit_interval_flag = b_fit_interval_flag; + ephemeris.d_spare1 = d_spare1; + ephemeris.d_spare2 = d_spare2; + ephemeris.d_A_f0 = d_A_f0; + ephemeris.d_A_f1 = d_A_f1; + ephemeris.d_A_f2 = d_A_f2; + ephemeris.b_integrity_status_flag = b_integrity_status_flag; + ephemeris.b_alert_flag = b_alert_flag; + ephemeris.b_antispoofing_flag = b_antispoofing_flag; + ephemeris.d_satClkDrift = d_satClkDrift; + ephemeris.d_dtr = d_dtr; + ephemeris.d_satpos_X = d_satpos_X; + ephemeris.d_satpos_Y = d_satpos_Y; + ephemeris.d_satpos_Z = d_satpos_Z; + ephemeris.d_satvel_X = d_satvel_X; + ephemeris.d_satvel_Y = d_satvel_Y; + ephemeris.d_satvel_Z = d_satvel_Z; + + return ephemeris; +} + + +Gps_Iono Beidou_Navigation_Message::get_iono() +{ + Gps_Iono iono; + iono.d_alpha0 = d_alpha0; + iono.d_alpha1 = d_alpha1; + iono.d_alpha2 = d_alpha2; + iono.d_alpha3 = d_alpha3; + iono.d_beta0 = d_beta0; + iono.d_beta1 = d_beta1; + iono.d_beta2 = d_beta2; + iono.d_beta3 = d_beta3; + iono.valid = flag_iono_valid; + //WARNING: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue + flag_iono_valid = false; + return iono; +} + + +Gps_Utc_Model Beidou_Navigation_Message::get_utc_model() +{ + Gps_Utc_Model utc_model; + utc_model.valid = flag_utc_model_valid; + // UTC parameters + utc_model.d_A1 = d_A1; + utc_model.d_A0 = d_A0; + utc_model.d_t_OT = d_t_OT; + utc_model.i_WN_T = i_WN_T; + utc_model.d_DeltaT_LS = d_DeltaT_LS; + utc_model.i_WN_LSF = i_WN_LSF; + utc_model.i_DN = i_DN; + utc_model.d_DeltaT_LSF = d_DeltaT_LSF; + // warning: We clear flag_utc_model_valid in order to not re-send the same information to the ionospheric parameters queue + flag_utc_model_valid = false; + return utc_model; +} + + +bool Beidou_Navigation_Message::satellite_validation() +{ + bool flag_data_valid = false; + b_valid_ephemeris_set_flag = false; + + // First Step: + // check Issue Of Ephemeris Data (IODE IODC..) to find a possible interrupted reception + // and check if the data have been filled (!=0) + if (d_TOW_SF1 != 0 and d_TOW_SF2 != 0 and d_TOW_SF3 != 0) + { + if (d_IODE_SF2 == d_IODE_SF3 and d_IODC == d_IODE_SF2 and d_IODC!= -1) + { + flag_data_valid = true; + b_valid_ephemeris_set_flag = true; + } + } + return flag_data_valid; +} diff --git a/src/core/system_parameters/beidou_navigation_message.h b/src/core/system_parameters/beidou_navigation_message.h new file mode 100644 index 000000000..62e95d9b9 --- /dev/null +++ b/src/core/system_parameters/beidou_navigation_message.h @@ -0,0 +1,240 @@ +/*! + * \file beidou_navigation_message.h + * \brief Interface of a BeiDou D1 NAV Data message decoder + * \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com + * + * ------------------------------------------------------------------------- + * + * Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors) + * + * GNSS-SDR is a software defined Global Navigation + * Satellite Systems receiver + * + * This file is part of GNSS-SDR. + * + * GNSS-SDR is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * GNSS-SDR is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with GNSS-SDR. If not, see . + * + * ------------------------------------------------------------------------- + */ + + +#ifndef GNSS_SDR_BEIDOU_NAVIGATION_MESSAGE_H_ +#define GNSS_SDR_BEIDOU_NAVIGATION_MESSAGE_H_ + + +#include +#include +#include +#include +#include +#include "beidou_b1I.h" +#include "beidou_ephemeris.h" +#include "beidou_iono.h" +#include "beidou_almanac.h" +#include "beidou_utc_model.h" + + + +/*! + * \brief This class decodes a BeiDou D1 NAV Data message as described in IS-GPS-200E + * + * See http://www.gps.gov/technical/icwg/IS-GPS-200E.pdf Appendix II + */ +class Beidou_Navigation_Message_D1 +{ +private: + unsigned long int read_navigation_unsigned(std::bitset bits, const std::vector> parameter); + signed long int read_navigation_signed(std::bitset bits, const std::vector> parameter); + bool read_navigation_bool(std::bitset bits, const std::vector> parameter); + void print_beidou_word_bytes(unsigned int BEIDOU_word); + /* + * Accounts for the beginning or end of week crossover + * + * See paragraph 20.3.3.3.3.1 (IS-GPS-200E) + * \param[in] - time in seconds + * \param[out] - corrected time, in seconds + */ + double check_t(double time); + +public: + bool b_valid_ephemeris_set_flag; // flag indicating that this ephemeris set have passed the validation check + //broadcast orbit 1 + double d_TOW; //!< Time of BeiDou Week of the ephemeris set (taken from subframes TOW) [s] + double d_TOW_SF1; //!< Time of BeiDou Week from HOW word of Subframe 1 [s] + double d_TOW_SF2; //!< Time of BeiDou Week from HOW word of Subframe 2 [s] + double d_TOW_SF3; //!< Time of BeiDou Week from HOW word of Subframe 3 [s] + double d_TOW_SF4; //!< Time of BeiDou Week from HOW word of Subframe 4 [s] + double d_TOW_SF5; //!< Time of BeiDou Week from HOW word of Subframe 5 [s] + + double d_AODE; + double d_Crs; //!< Amplitude of the Sine Harmonic Correction Term to the Orbit Radius [m] + double d_Delta_n; //!< Mean Motion Difference From Computed Value [semi-circles/s] + double d_M_0; //!< Mean Anomaly at Reference Time [semi-circles] + //broadcast orbit 2 + double d_Cuc; //!< Amplitude of the Cosine Harmonic Correction Term to the Argument of Latitude [rad] + double d_e_eccentricity; //!< Eccentricity [dimensionless] + double d_Cus; //!< Amplitude of the Sine Harmonic Correction Term to the Argument of Latitude [rad] + double d_sqrt_A; //!< Square Root of the Semi-Major Axis [sqrt(m)] + //broadcast orbit 3 + double d_Toe; //!< Ephemeris data reference time of week (Ref. 20.3.3.4.3 IS-GPS-200E) [s] + double d_Toc; //!< clock data reference time (Ref. 20.3.3.3.3.1 IS-GPS-200E) [s] + double d_Cic; //!< Amplitude of the Cosine Harmonic Correction Term to the Angle of Inclination [rad] + double d_OMEGA0; //!< Longitude of Ascending Node of Orbit Plane at Weekly Epoch [semi-circles] + double d_Cis; //!< Amplitude of the Sine Harmonic Correction Term to the Angle of Inclination [rad] + //broadcast orbit 4 + double d_i_0; //!< Inclination Angle at Reference Time [semi-circles] + double d_Crc; //!< Amplitude of the Cosine Harmonic Correction Term to the Orbit Radius [m] + double d_OMEGA; //!< Argument of Perigee [semi-cicles] + double d_OMEGA_DOT; //!< Rate of Right Ascension [semi-circles/s] + //broadcast orbit 5 + double d_IDOT; //!< Rate of Inclination Angle [semi-circles/s] + int i_BEIDOU_week; //!< BeiDou week number, aka WN [week] + //broadcast orbit 6 + int i_SV_accuracy; //!< User Range Accuracy (URA) index of the SV + int i_SV_health; + double d_TGD1; //!< Estimated Group Delay Differential in B1 + double d_TGD2; //!< Estimated Group Delay Differential in B2 + double d_AODC; //!< Age of Data, Clock + //broadcast orbit 7 +// int i_AODO; //!< Age of Data Offset (AODO) term for the navigation message correction table (NMCT) contained in subframe 4 (reference paragraph 20.3.3.5.1.9) [s] + + bool b_fit_interval_flag;//!< indicates the curve-fit interval used by the CS (Block II/IIA/IIR/IIR-M/IIF) and SS (Block IIIA) in determining the ephemeris parameters, as follows: 0 = 4 hours, 1 = greater than 4 hours. + double d_spare1; + double d_spare2; + + double d_A_f0; //!< Coefficient 0 of code phase offset model [s] + double d_A_f1; //!< Coefficient 1 of code phase offset model [s/s] + double d_A_f2; //!< Coefficient 2 of code phase offset model [s/s^2] + + + // Almanac + double d_Toa; //!< Almanac reference time [s] + int i_WN_A; //!< Modulo 256 of the GPS week number to which the almanac reference time (d_Toa) is referenced + std::map almanacHealth; //!< Map that stores the health information stored in the almanac + + std::map satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus + + // Flags + + /*! \brief If true, enhanced level of integrity assurance. + * + * If false, indicates that the conveying signal is provided with the legacy level of integrity assurance. + * That is, the probability that the instantaneous URE of the conveying signal exceeds 4.42 times the upper bound + * value of the current broadcast URA index, for more than 5.2 seconds, without an accompanying alert, is less + * than 1E-5 per hour. If true, indicates that the conveying signal is provided with an enhanced level of + * integrity assurance. That is, the probability that the instantaneous URE of the conveying signal exceeds 5.73 + * times the upper bound value of the current broadcast URA index, for more than 5.2 seconds, without an + * accompanying alert, is less than 1E-8 per hour. + */ + bool b_integrity_status_flag; + bool b_alert_flag; //!< If true, indicates that the SV URA may be worse than indicated in d_SV_accuracy, use that SV at our own risk. + bool b_antispoofing_flag; //!< If true, the AntiSpoofing mode is ON in that SV + + // clock terms + //double d_master_clock; // GPS transmission time + double d_satClkCorr; // GPS clock error + double d_dtr; // relativistic clock correction term + double d_satClkDrift; + + // satellite positions + double d_satpos_X; //!< Earth-fixed coordinate x of the satellite [m]. Intersection of the IERS Reference Meridian (IRM) and the plane passing through the origin and normal to the Z-axis. + double d_satpos_Y; //!< Earth-fixed coordinate y of the satellite [m]. Completes a right-handed, Earth-Centered, Earth-Fixed orthogonal coordinate system. + double d_satpos_Z; //!< Earth-fixed coordinate z of the satellite [m]. The direction of the IERS (International Earth Rotation and Reference Systems Service) Reference Pole (IRP). + + // satellite identification info + int i_channel_ID; + unsigned int i_satellite_PRN; + + // time synchro + double d_subframe_timestamp_ms; //[ms] + + // Ionospheric parameters + bool flag_iono_valid; //!< If set, it indicates that the ionospheric parameters are filled (page 18 has arrived and decoded) + double d_alpha0; //!< Coefficient 0 of a cubic equation representing the amplitude of the vertical delay [s] + double d_alpha1; //!< Coefficient 1 of a cubic equation representing the amplitude of the vertical delay [s/semi-circle] + double d_alpha2; //!< Coefficient 2 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^2] + double d_alpha3; //!< Coefficient 3 of a cubic equation representing the amplitude of the vertical delay [s(semi-circle)^3] + double d_beta0; //!< Coefficient 0 of a cubic equation representing the period of the model [s] + double d_beta1; //!< Coefficient 1 of a cubic equation representing the period of the model [s/semi-circle] + double d_beta2; //!< Coefficient 2 of a cubic equation representing the period of the model [s(semi-circle)^2] + double d_beta3; //!< Coefficient 3 of a cubic equation representing the period of the model [s(semi-circle)^3] + + // UTC parameters + bool flag_utc_model_valid; //!< If set, it indicates that the UTC model parameters are filled + double d_A1; //!< 1st order term of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200E) [s/s] + double d_A0; //!< Constant of a model that relates GPS and UTC time (ref. 20.3.3.5.2.4 IS-GPS-200E) [s] + double d_t_OT; //!< Reference time for UTC data (reference 20.3.4.5 and 20.3.3.5.2.4 IS-GPS-200E) [s] + int i_WN_T; //!< UTC reference week number [weeks] + double d_DeltaT_LS; //!< delta time due to leap seconds [s]. Number of leap seconds since 6-Jan-1980 as transmitted by the GPS almanac. + int i_WN_LSF; //!< Week number at the end of which the leap second becomes effective [weeks] + int i_DN; //!< Day number (DN) at the end of which the leap second becomes effective [days] + double d_DeltaT_LSF; //!< Scheduled future or recent past (relative to NAV message upload) value of the delta time due to leap seconds [s] + + // Satellite velocity + double d_satvel_X; //!< Earth-fixed velocity coordinate x of the satellite [m] + double d_satvel_Y; //!< Earth-fixed velocity coordinate y of the satellite [m] + double d_satvel_Z; //!< Earth-fixed velocity coordinate z of the satellite [m] + + // public functions + void reset(); + + /*! + * \brief Obtain a GPS SV Ephemeris class filled with current SV data + */ + Gps_Ephemeris get_ephemeris(); + + /*! + * \brief Obtain a GPS ionospheric correction parameters class filled with current SV data + */ + Gps_Iono get_iono(); + + /*! + * \brief Obtain a GPS UTC model parameters class filled with current SV data + */ + Gps_Utc_Model get_utc_model(); + + + /*! + * \brief Decodes the GPS NAV message + */ + int subframe_decoder(char *subframe); + + /*! + * \brief Computes the position of the satellite + * + * Implementation of Table 20-IV (IS-GPS-200E) + */ + void satellitePosition(double transmitTime); + + /*! + * \brief Sets (\a d_satClkCorr) according to the User Algorithm for SV Clock Correction + * and returns the corrected clock (IS-GPS-200E, 20.3.3.3.3.1) + */ + double sv_clock_correction(double transmitTime); + + /*! + * \brief Computes the Coordinated Universal Time (UTC) and + * returns it in [s] (IS-GPS-200E, 20.3.3.5.2.4) + */ + double utc_time(const double gpstime_corrected) const; + + bool satellite_validation(); + + /*! + * Default constructor + */ + Beidou_Navigation_Message_D1(); +}; + +#endif