mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-03-14 07:28:17 +00:00
first evaluation beidou b1I
This commit is contained in:
Sergi Segura
parent
36ac696a46
commit
cd149895f0
93
conf/gnss-sdr_BEIDOU_B1I_ishort.conf
Normal file
93
conf/gnss-sdr_BEIDOU_B1I_ishort.conf
Normal file
@ -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
|
||||
@ -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)
|
||||
|
||||
@ -0,0 +1,330 @@
|
||||
/*!
|
||||
* \file beidou_b1i_pcps_acquisition.cc
|
||||
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
|
||||
* BeiDou B1I signals
|
||||
* \authors <ul>
|
||||
* <li> Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com
|
||||
* </ul>
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#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 <boost/math/distributions/exponential.hpp>
|
||||
#include <glog/logging.h>
|
||||
|
||||
|
||||
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<unsigned int>(std::round(static_cast<double>(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<float>* code = new std::complex<float>[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<double>(ncells);
|
||||
double val = pow(1.0 - pfa, exponent);
|
||||
double lambda = static_cast<double>(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist(lambda);
|
||||
float threshold = static_cast<float>(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_;
|
||||
}
|
||||
@ -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 <ul>
|
||||
* <li> Javier Arribas, 2011. jarribas(at)cttc.es
|
||||
* <li> Luis Esteve, 2012. luis(at)epsilon-formacion.com
|
||||
* <li> Marc Molina, 2013. marc.molina.pena(at)gmail.com
|
||||
* </ul>
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#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 <gnuradio/blocks/stream_to_vector.h>
|
||||
#include <gnuradio/blocks/float_to_complex.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include <string>
|
||||
|
||||
|
||||
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<float>* 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_ */
|
||||
@ -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
|
||||
|
||||
195
src/algorithms/libs/beidou_b1I_signal_processing.cc
Normal file
195
src/algorithms/libs/beidou_b1I_signal_processing.cc
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "beidou_b1I_signal_processing.h"
|
||||
|
||||
auto auxCeil = [](float x) { return static_cast<int>(static_cast<long>((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<float>(b1i_code_int[ii]);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1i_code_gen_complex(std::complex<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] = std::complex<float>(static_cast<float>(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<float>* _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<float> _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<signed int>(static_cast<double>(_fs) / static_cast<double>(_codeFreqBasis / _codeLength));
|
||||
|
||||
//--- Find time constants --------------------------------------------------
|
||||
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
|
||||
_tc = 1.0 / static_cast<float>(_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
|
||||
}
|
||||
}
|
||||
}
|
||||
53
src/algorithms/libs/beidou_b1I_signal_processing.h
Normal file
53
src/algorithms/libs/beidou_b1I_signal_processing.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_
|
||||
#define BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_
|
||||
|
||||
#include <complex>
|
||||
|
||||
//!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<float>* _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<float>* _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<float>* _dest, unsigned int _prn, int _fs, unsigned int _chip_shift);
|
||||
|
||||
#endif /* BEIDOU_B1I_SDR_SIGNAL_PROCESSING_H_ */
|
||||
@ -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 <glog/logging.h>
|
||||
|
||||
@ -110,6 +111,12 @@ SignalGenerator::SignalGenerator(ConfigurationInterface* configuration,
|
||||
}
|
||||
}
|
||||
|
||||
else if (std::find(system.begin(), system.end(), "B") != system.end())
|
||||
{
|
||||
vector_length = round(static_cast<float>(fs_in) / (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS));
|
||||
}
|
||||
|
||||
|
||||
if (item_type_.compare("gr_complex") == 0)
|
||||
{
|
||||
item_size_ = sizeof(gr_complex);
|
||||
|
||||
@ -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<GNSSBlockInterface> 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<GNSSBlockInterface> GNSSBlockFactory::GetPVT(std::shared_ptr<Con
|
||||
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);
|
||||
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<GNSSBlockInterface> GNSSBlockFactory::GetChannel_L5(
|
||||
}
|
||||
|
||||
|
||||
//********* BeiDou B1I CHANNEL *****************
|
||||
std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetChannel_B1(
|
||||
std::shared_ptr<ConfigurationInterface> 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<InMemoryConfiguration> config;
|
||||
config = std::make_shared<InMemoryConfiguration>();
|
||||
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<GNSSBlockInterface> pass_through_ = GetBlock(configuration, "Channel", "Pass_Through", 1, 1, queue);
|
||||
std::unique_ptr<AcquisitionInterface> acq_ = GetAcqBlock(configuration, "Acquisition_B1" + appendix1, acq, 1, 0);
|
||||
std::unique_ptr<TrackingInterface> trk_ = GetTrkBlock(configuration, "Tracking_B1" + appendix2, trk, 1, 1);
|
||||
std::unique_ptr<TelemetryDecoderInterface> tlm_ = GetTlmBlock(configuration, "TelemetryDecoder_B1" + appendix3, tlm, 1, 1);
|
||||
|
||||
std::unique_ptr<GNSSBlockInterface> 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<std::vector<std::unique_ptr<GNSSBlockInterface>>> GNSSBlockFactory::GetChannels(
|
||||
std::shared_ptr<ConfigurationInterface> configuration, gr::msg_queue::sptr queue)
|
||||
{
|
||||
@ -793,6 +865,7 @@ std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> GNSSBlockFacto
|
||||
Channels_2S_count +
|
||||
Channels_2G_count +
|
||||
Channels_5X_count +
|
||||
Channels_L5_count +
|
||||
Channels_L5_count;
|
||||
|
||||
std::unique_ptr<std::vector<std::unique_ptr<GNSSBlockInterface>>> channels(new std::vector<std::unique_ptr<GNSSBlockInterface>>(total_channels));
|
||||
@ -1449,6 +1522,13 @@ std::unique_ptr<GNSSBlockInterface> GNSSBlockFactory::GetBlock(
|
||||
out_streams));
|
||||
block = std::move(block_);
|
||||
}
|
||||
else if (implementation.compare("BEIDOU_B1I_PCPS_Acquisition") == 0)
|
||||
{
|
||||
std::unique_ptr<AcquisitionInterface> 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)
|
||||
{
|
||||
|
||||
@ -106,6 +106,10 @@ private:
|
||||
std::string acq, std::string trk, std::string tlm, int channel,
|
||||
boost::shared_ptr<gr::msg_queue> queue);
|
||||
|
||||
std::unique_ptr<GNSSBlockInterface> GetChannel_B1(std::shared_ptr<ConfigurationInterface> configuration,
|
||||
std::string acq, std::string trk, std::string tlm, int channel,
|
||||
boost::shared_ptr<gr::msg_queue> queue);
|
||||
|
||||
std::unique_ptr<AcquisitionInterface> GetAcqBlock(
|
||||
std::shared_ptr<ConfigurationInterface> configuration,
|
||||
std::string role,
|
||||
|
||||
@ -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<unsigned int> available_glonass_prn = {1, 2, 3, 4, 9, 10, 11, 12, 18, 19, 20, 21, 24};
|
||||
|
||||
std::set<unsigned int> 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<unsigned int> tmp_set;
|
||||
boost::tokenizer<> tok(sv_list);
|
||||
std::transform(tok.begin(), tok.end(), std::inserter(tmp_set, tmp_set.begin()),
|
||||
boost::lexical_cast<unsigned int, std::string>);
|
||||
|
||||
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")));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
|
||||
250
src/core/system_parameters/beidou_b1I.h
Normal file
250
src/core/system_parameters/beidou_b1I.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_B1I_H_
|
||||
#define GNSS_SDR_BEIDOU_B1I_H_
|
||||
|
||||
#include <vector>
|
||||
#include <utility> // 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<std::pair<int,int> > D1_PRE( { {1,11} } );
|
||||
const std::vector<std::pair<int,int> > D1_FRAID( { {16,3} } );
|
||||
const std::vector<std::pair<int,int> > D1_SOW( { {19,8},{31,12} } );
|
||||
// SUBFRAME 1
|
||||
const std::vector<std::pair<int,int> > D1_SAT_H1( { {43,1} } );
|
||||
const std::vector<std::pair<int,int> > D1_AODC( { {44,5} } );
|
||||
const std::vector<std::pair<int,int> > D1_URAI( { {49,4} } );
|
||||
const std::vector<std::pair<int,int> > D1_WN( { {61,13} } );
|
||||
const std::vector<std::pair<int,int> > D1_TOC( { {74,9},{91,8} } );
|
||||
const double D1_TOC_LSB = TWO_P3;
|
||||
const std::vector<std::pair<int,int> > D1_TGD1( { {99,10} } );
|
||||
const double D1_TGD1_LSB = 0.1;
|
||||
const std::vector<std::pair<int,int> > D1_TGD2( { {121,6} } );
|
||||
const double D1_TGD2_LSB = 0.1;
|
||||
const std::vector<std::pair<int,int> > D1_ALPHA0( { {127,8} } );
|
||||
const double D1_ALPHA0_LSB = TWO_N30;
|
||||
const std::vector<std::pair<int,int> > D1_ALPHA1( { {135,8} } );
|
||||
const double D1_ALPHA1_LSB = TWO_N27;
|
||||
const std::vector<std::pair<int,int> > D1_ALPHA2( { {151,8} } );
|
||||
const double D1_ALPHA2_LSB = TWO_N24;
|
||||
const std::vector<std::pair<int,int> > D1_ALPHA3( { {159,8} } );
|
||||
const double D1_ALPHA3_LSB = TWO_N24;
|
||||
const std::vector<std::pair<int,int> > D1_BETA0( { {167,6}, {181,2} } );
|
||||
const double D1_BETA0_LSB = TWO_P11;
|
||||
const std::vector<std::pair<int,int> > D1_BETA1( { {183,8} } );
|
||||
const double D1_BETA1_LSB = TWO_P14;
|
||||
const std::vector<std::pair<int,int> > D1_BETA2( { {191,8} } );
|
||||
const double D1_BETA2_LSB = TWO_P16;
|
||||
const std::vector<std::pair<int,int> > D1_BETA3( { {199,4},{211,4} } );
|
||||
const double D1_BETA3_LSB = TWO_P16;
|
||||
const std::vector<std::pair<int,int> > D1_A2( { {215,11} } );
|
||||
const double D1_A2_LSB = TWO_N66;
|
||||
const std::vector<std::pair<int,int> > D1_A0( { {226,7},{241,17} } );
|
||||
const double D1_A0_LSB = TWO_N33;
|
||||
const std::vector<std::pair<int,int> > D1_A1( { {258,5},{271,17} } );
|
||||
const double D1_A1_LSB = TWO_N50;
|
||||
const std::vector<std::pair<int,int> > D1_AODE( { {288,5} } );
|
||||
|
||||
//SUBFRAME 2
|
||||
const std::vector<std::pair<int,int> > D1_DELTA_N( { {43,10},{61,6} } );
|
||||
const double D1_DELTA_N_LSB = PI_TWO_N43;
|
||||
const std::vector<std::pair<int,int> > D1_CUC( { {67,16},{91,2} } );
|
||||
const double D1_CUC_LSB = TWO_N31;
|
||||
const std::vector<std::pair<int,int> > D1_M0( { {93,20}, {121,12} } );
|
||||
const double D1_M0_LSB = PI_TWO_N31;
|
||||
const std::vector<std::pair<int,int> > D1_E( { {133,10},{151,22} } );
|
||||
const double D1_E_LSB = TWO_N33;
|
||||
const std::vector<std::pair<int,int> > D1_CUS( { {181,18} } );
|
||||
const double D1_CUS_LSB = TWO_N31;
|
||||
const std::vector<std::pair<int,int> > D1_CRC( { {199,4},{211,14} } );
|
||||
const double D1_CRC_LSB = TWO_N6;
|
||||
const std::vector<std::pair<int,int> > D1_CRS( { {225,8},{241,10} } );
|
||||
const double D1_CRS_LSB = TWO_N6;
|
||||
const std::vector<std::pair<int,int> > D1_SQRT_A( { {251,12},{271,20} } );
|
||||
const double D1_SQRT_A_LSB = TWO_N19;
|
||||
const std::vector<std::pair<int,int> > D1_TOE( { {291,2} } );
|
||||
const double D1_TOE_LSB = TWO_P3;
|
||||
|
||||
//SUBFRAME 3
|
||||
const std::vector<std::pair<int,int> > D1_TOE_MSB2( { {43,10},{61,5} } );
|
||||
const std::vector<std::pair<int,int> > D1_I0( { {66,17},{91,15} } );
|
||||
const double D1_I0_LSB = PI_TWO_N31;
|
||||
const std::vector<std::pair<int,int> > D1_CIC( { {106,7},{121,11} } );
|
||||
const double D1_CIC_LSB = TWO_N31;
|
||||
const std::vector<std::pair<int,int> > D1_OMEGA_DOT( { {132,11},{151,13} } );
|
||||
const double D1_OMEGA_DOT_LSB = PI_TWO_N43;
|
||||
const std::vector<std::pair<int,int> > D1_CIS( { {164,9},{181,9} } );
|
||||
const double D1_CIS_LSB = TWO_N31;
|
||||
const std::vector<std::pair<int,int> > D1_IDOT( { {190,13},{211,1} } );
|
||||
const double D1_IDOT_LSB = PI_TWO_N43;
|
||||
const std::vector<std::pair<int,int> > D1_OMEGA0( { {212,21},{241,11} } );
|
||||
const double D1_OMEGA0_LSB = PI_TWO_N31;
|
||||
const std::vector<std::pair<int,int> > 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<std::pair<int,int> > D1_SQRT_A_ALMANAC( { {51,2},{61,22} } );
|
||||
const double D1_SQRT_A_ALMANAC_LSB = TWO_N11;
|
||||
const std::vector<std::pair<int,int> > D1_A1_ALMANAC( { {91,11} } );
|
||||
const double D1_A1_ALMANAC_LSB = TWO_N38;
|
||||
const std::vector<std::pair<int,int> > D1_A0_ALMANAC( { {102,11} } );
|
||||
const double D1_A0_ALMANAC_LSB = TWO_N20;
|
||||
const std::vector<std::pair<int,int> > D1_OMEGA0_ALMANAC( { {121,22},{151,2} } );
|
||||
const double D1_OMEGA0_ALMANAC_LSB = PI_TWO_N23;
|
||||
const std::vector<std::pair<int,int> > D1_E_ALMANAC( { {153,17} } );
|
||||
const double D1_E_ALMANAC_LSB = TWO_N21;
|
||||
const std::vector<std::pair<int,int> > D1_DELTA_I( { {170,3},{181,13} } );
|
||||
const double D1_DELTA_I_LSB = PI_TWO_N19;
|
||||
const std::vector<std::pair<int,int> > D1_TOA( { {194,8} } );
|
||||
const double D1_TOA_LSB = TWO_P12;
|
||||
const std::vector<std::pair<int,int> > D1_OMEGA_DOT_ALMANAC( { {202,1}, {211,16} } );
|
||||
const double D1_OMEGA_DOT_ALMANAC_LSB = PI_TWO_N38;
|
||||
const std::vector<std::pair<int,int> > D1_OMEGA_ALMANAC( { {227,6},{241,18} } );
|
||||
const double D1_OMEGA_ALMANAC_LSB = PI_TWO_N23;
|
||||
const std::vector<std::pair<int,int> > D1_M0_ALMANAC( { {259,4},{271,20} } );
|
||||
const double D1_M0_ALMANAC_LSB = PI_TWO_N23;
|
||||
|
||||
//SUBFRAME 5 PAGE 7
|
||||
const std::vector<std::pair<int,int> > D1_HEA1( { {51,2},{61,7} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA2( { {68,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA3( { {77,6},{91,3} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA4( { {94,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA5( { {103,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA6( { {112,1},{121,8} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA7( { {129,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA8( { {138,5},{151,4} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA9( { {155,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA10( { {164,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA11( { {181,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA12( { {190,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA13( { {199,4},{211,5} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA14( { {216,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA15( { {225,8},{241,1} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA16( { {242,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA17( { {251,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA18( { {260,3},{271,6} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA19( { {277,9} } );
|
||||
|
||||
|
||||
|
||||
//SUBFRAME 5 PAGE 8
|
||||
const std::vector<std::pair<int,int> > D1_HEA20( { {51,2},{61,7} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA21( { {68,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA22( { {77,6},{91,3} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA23( { {94,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA24( { {103,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA25( { {112,1},{121,8} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA26( { {129,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA27( { {138,5},{151,4} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA28( { {155,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA29( { {164,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_HEA30( { {181,9} } );
|
||||
const std::vector<std::pair<int,int> > D1_WNA( { {190,8} } );
|
||||
const std::vector<std::pair<int,int> > D1_TOA2( { {198,5},{211,3} } );
|
||||
|
||||
//SUBFRAME 5 PAGE 9
|
||||
const std::vector<std::pair<int,int> > D1_A0GPS( { {97,14} } );
|
||||
const double D1_A0GPS_LSB = 0.1;
|
||||
const std::vector<std::pair<int,int> > D1_A1GPS( { {111,2},{121,14} } );
|
||||
const double D1_A1GPS_LSB = 0.1;
|
||||
const std::vector<std::pair<int,int> > D1_A0GAL( { {135,8},{151,6} } );
|
||||
const double D1_A0GAL_LSB = 0.1;
|
||||
const std::vector<std::pair<int,int> > D1_A1GAL( { {157,16} } );
|
||||
const double D1_A1GAL_LSB = 0.1;
|
||||
const std::vector<std::pair<int,int> > D1_A0GLO( { {181,14} } );
|
||||
const double D1_A0GLO_LSB = 0.1;
|
||||
const std::vector<std::pair<int,int> > D1_A1GLO( { {195,8},{211,8} } );
|
||||
const double D1_A1GLO_LSB = 0.1;
|
||||
|
||||
//SUBFRAME 5 PAGE 10
|
||||
const std::vector<std::pair<int,int> > D1_DELTA_T_LS( { {51,2},{61,6} } );
|
||||
const std::vector<std::pair<int,int> > D1_DELTA_T_LSF( { {67,8} } );
|
||||
const std::vector<std::pair<int,int> > D1_WN_LSF( { {75,8} } );
|
||||
const std::vector<std::pair<int,int> > D1_A0UTC( { {91,22},{121,10} } );
|
||||
const double D1_A0UTC_LSB = TWO_N30;
|
||||
const std::vector<std::pair<int,int> > D1_A1UTC( { {131,12},{151,12} } );
|
||||
const double D1_A1UTC_LSB = TWO_N50;
|
||||
const std::vector<std::pair<int,int> > D1_DN( { {163,8} } );
|
||||
|
||||
#endif /* GNSS_SDR_BEIDOU_B1I_H_ */
|
||||
279
src/core/system_parameters/beidou_ephemeris.cc
Normal file
279
src/core/system_parameters/beidou_ephemeris.cc
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "beidou_ephemeris.h"
|
||||
#include <cmath>
|
||||
#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;
|
||||
}
|
||||
205
src/core/system_parameters/beidou_ephemeris.h
Normal file
205
src/core/system_parameters/beidou_ephemeris.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_EPHEMERIS_H_
|
||||
#define GNSS_SDR_BEIDOU_EPHEMERIS_H_
|
||||
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include "boost/assign.hpp"
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \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<int,std::string> satelliteBlock; //!< Map that stores to which block the PRN belongs http://www.navcen.uscg.gov/?Do=constellationStatus
|
||||
|
||||
template<class Archive>
|
||||
|
||||
/*!
|
||||
* \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
|
||||
46
src/core/system_parameters/beidou_iono.cc
Normal file
46
src/core/system_parameters/beidou_iono.cc
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
80
src/core/system_parameters/beidou_iono.h
Normal file
80
src/core/system_parameters/beidou_iono.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_IONO_H_
|
||||
#define GNSS_SDR_BEIDOU_IONO_H_
|
||||
|
||||
|
||||
#include "boost/assign.hpp"
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
|
||||
/*!
|
||||
* \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<class Archive>
|
||||
|
||||
/*!
|
||||
* \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
|
||||
804
src/core/system_parameters/beidou_navigation_message.cc
Normal file
804
src/core/system_parameters/beidou_navigation_message.cc
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "beidou_navigation_message.h"
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <gnss_satellite.h>
|
||||
|
||||
|
||||
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<BEIDOU_SUBFRAME_BITS> bits, const std::vector<std::pair<int,int>> 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<BEIDOU_SUBFRAME_BITS> bits, const std::vector<std::pair<int,int>> 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<BEIDOU_SUBFRAME_BITS> bits, const std::vector<std::pair<int,int>> 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<BEIDOU_SUBFRAME_BITS> subframe_bits;
|
||||
std::bitset<BEIDOU_WORD_BITS + 2> 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<int>(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<double>(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<int>(read_navigation_unsigned(subframe_bits, GPS_WEEK));
|
||||
i_SV_accuracy = static_cast<int>(read_navigation_unsigned(subframe_bits, SV_ACCURACY)); // (20.3.3.3.1.3)
|
||||
i_SV_health = static_cast<int>(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<int>(read_navigation_unsigned(subframe_bits, CA_OR_P_ON_L2));
|
||||
d_TGD = static_cast<double>(read_navigation_signed(subframe_bits, T_GD));
|
||||
d_TGD = d_TGD * T_GD_LSB;
|
||||
d_IODC = static_cast<double>(read_navigation_unsigned(subframe_bits, IODC));
|
||||
d_Toc = static_cast<double>(read_navigation_unsigned(subframe_bits, T_OC));
|
||||
d_Toc = d_Toc * T_OC_LSB;
|
||||
d_A_f0 = static_cast<double>(read_navigation_signed(subframe_bits, A_F0));
|
||||
d_A_f0 = d_A_f0 * A_F0_LSB;
|
||||
d_A_f1 = static_cast<double>(read_navigation_signed(subframe_bits, A_F1));
|
||||
d_A_f1 = d_A_f1 * A_F1_LSB;
|
||||
d_A_f2 = static_cast<double>(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<double>(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<double>(read_navigation_unsigned(subframe_bits, IODE_SF2));
|
||||
d_Crs = static_cast<double>(read_navigation_signed(subframe_bits, C_RS));
|
||||
d_Crs = d_Crs * C_RS_LSB;
|
||||
d_Delta_n = static_cast<double>(read_navigation_signed(subframe_bits, DELTA_N));
|
||||
d_Delta_n = d_Delta_n * DELTA_N_LSB;
|
||||
d_M_0 = static_cast<double>(read_navigation_signed(subframe_bits, M_0));
|
||||
d_M_0 = d_M_0 * M_0_LSB;
|
||||
d_Cuc = static_cast<double>(read_navigation_signed(subframe_bits, C_UC));
|
||||
d_Cuc = d_Cuc * C_UC_LSB;
|
||||
d_e_eccentricity = static_cast<double>(read_navigation_unsigned(subframe_bits, E));
|
||||
d_e_eccentricity = d_e_eccentricity * E_LSB;
|
||||
d_Cus = static_cast<double>(read_navigation_signed(subframe_bits, C_US));
|
||||
d_Cus = d_Cus * C_US_LSB;
|
||||
d_sqrt_A = static_cast<double>(read_navigation_unsigned(subframe_bits, SQRT_A));
|
||||
d_sqrt_A = d_sqrt_A * SQRT_A_LSB;
|
||||
d_Toe = static_cast<double>(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<int>(read_navigation_unsigned(subframe_bits, AODO));
|
||||
i_AODO = i_AODO * AODO_LSB;
|
||||
|
||||
break;
|
||||
|
||||
case 3: // --- It is subframe 3 -------------------------------------
|
||||
d_TOW_SF3 = static_cast<double>(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<double>(read_navigation_signed(subframe_bits, C_IC));
|
||||
d_Cic = d_Cic * C_IC_LSB;
|
||||
d_OMEGA0 = static_cast<double>(read_navigation_signed(subframe_bits, OMEGA_0));
|
||||
d_OMEGA0 = d_OMEGA0 * OMEGA_0_LSB;
|
||||
d_Cis = static_cast<double>(read_navigation_signed(subframe_bits, C_IS));
|
||||
d_Cis = d_Cis * C_IS_LSB;
|
||||
d_i_0 = static_cast<double>(read_navigation_signed(subframe_bits, I_0));
|
||||
d_i_0 = d_i_0 * I_0_LSB;
|
||||
d_Crc = static_cast<double>(read_navigation_signed(subframe_bits, C_RC));
|
||||
d_Crc = d_Crc * C_RC_LSB;
|
||||
d_OMEGA = static_cast<double>(read_navigation_signed(subframe_bits, OMEGA));
|
||||
d_OMEGA = d_OMEGA * OMEGA_LSB;
|
||||
d_OMEGA_DOT = static_cast<double>(read_navigation_signed(subframe_bits, OMEGA_DOT));
|
||||
d_OMEGA_DOT = d_OMEGA_DOT * OMEGA_DOT_LSB;
|
||||
d_IODE_SF3 = static_cast<double>(read_navigation_unsigned(subframe_bits, IODE_SF3));
|
||||
d_IDOT = static_cast<double>(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<double>(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<int>(read_navigation_unsigned(subframe_bits, SV_DATA_ID));
|
||||
SV_page = static_cast<int>(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<double>(read_navigation_signed(subframe_bits, ALPHA_0));
|
||||
d_alpha0 = d_alpha0 * ALPHA_0_LSB;
|
||||
d_alpha1 = static_cast<double>(read_navigation_signed(subframe_bits, ALPHA_1));
|
||||
d_alpha1 = d_alpha1 * ALPHA_1_LSB;
|
||||
d_alpha2 = static_cast<double>(read_navigation_signed(subframe_bits, ALPHA_2));
|
||||
d_alpha2 = d_alpha2 * ALPHA_2_LSB;
|
||||
d_alpha3 = static_cast<double>(read_navigation_signed(subframe_bits, ALPHA_3));
|
||||
d_alpha3 = d_alpha3 * ALPHA_3_LSB;
|
||||
d_beta0 = static_cast<double>(read_navigation_signed(subframe_bits, BETA_0));
|
||||
d_beta0 = d_beta0 * BETA_0_LSB;
|
||||
d_beta1 = static_cast<double>(read_navigation_signed(subframe_bits, BETA_1));
|
||||
d_beta1 = d_beta1 * BETA_1_LSB;
|
||||
d_beta2 = static_cast<double>(read_navigation_signed(subframe_bits, BETA_2));
|
||||
d_beta2 = d_beta2 * BETA_2_LSB;
|
||||
d_beta3 = static_cast<double>(read_navigation_signed(subframe_bits, BETA_3));
|
||||
d_beta3 = d_beta3 * BETA_3_LSB;
|
||||
d_A1 = static_cast<double>(read_navigation_signed(subframe_bits, A_1));
|
||||
d_A1 = d_A1 * A_1_LSB;
|
||||
d_A0 = static_cast<double>(read_navigation_signed(subframe_bits, A_0));
|
||||
d_A0 = d_A0 * A_0_LSB;
|
||||
d_t_OT = static_cast<double>(read_navigation_unsigned(subframe_bits, T_OT));
|
||||
d_t_OT = d_t_OT * T_OT_LSB;
|
||||
i_WN_T = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_T));
|
||||
d_DeltaT_LS = static_cast<double>(read_navigation_signed(subframe_bits, DELTAT_LS));
|
||||
i_WN_LSF = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_LSF));
|
||||
i_DN = static_cast<int>(read_navigation_unsigned(subframe_bits, DN)); // Right-justified ?
|
||||
d_DeltaT_LSF = static_cast<double>(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<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV25));
|
||||
almanacHealth[26] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV26));
|
||||
almanacHealth[27] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV27));
|
||||
almanacHealth[28] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV28));
|
||||
almanacHealth[29] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV29));
|
||||
almanacHealth[30] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV30));
|
||||
almanacHealth[31] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV31));
|
||||
almanacHealth[32] = static_cast<int>(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<double>(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<int>(read_navigation_unsigned(subframe_bits, SV_DATA_ID));
|
||||
SV_page_5 = static_cast<int>(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<double>(read_navigation_unsigned(subframe_bits, T_OA));
|
||||
d_Toa = d_Toa * T_OA_LSB;
|
||||
i_WN_A = static_cast<int>(read_navigation_unsigned(subframe_bits, WN_A));
|
||||
almanacHealth[1] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV1));
|
||||
almanacHealth[2] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV2));
|
||||
almanacHealth[3] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV3));
|
||||
almanacHealth[4] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV4));
|
||||
almanacHealth[5] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV5));
|
||||
almanacHealth[6] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV6));
|
||||
almanacHealth[7] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV7));
|
||||
almanacHealth[8] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV8));
|
||||
almanacHealth[9] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV9));
|
||||
almanacHealth[10] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV10));
|
||||
almanacHealth[11] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV11));
|
||||
almanacHealth[12] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV12));
|
||||
almanacHealth[13] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV13));
|
||||
almanacHealth[14] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV14));
|
||||
almanacHealth[15] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV15));
|
||||
almanacHealth[16] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV16));
|
||||
almanacHealth[17] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV17));
|
||||
almanacHealth[18] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV18));
|
||||
almanacHealth[19] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV19));
|
||||
almanacHealth[20] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV20));
|
||||
almanacHealth[21] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV21));
|
||||
almanacHealth[22] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV22));
|
||||
almanacHealth[23] = static_cast<int>(read_navigation_unsigned(subframe_bits, HEALTH_SV23));
|
||||
almanacHealth[24] = static_cast<int>(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<double>((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<double>((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<EFBFBD>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<double>((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;
|
||||
}
|
||||
240
src/core/system_parameters/beidou_navigation_message.h
Normal file
240
src/core/system_parameters/beidou_navigation_message.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_NAVIGATION_MESSAGE_H_
|
||||
#define GNSS_SDR_BEIDOU_NAVIGATION_MESSAGE_H_
|
||||
|
||||
|
||||
#include <bitset>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#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<BEIDOU_SUBFRAME_BITS> bits, const std::vector<std::pair<int,int>> parameter);
|
||||
signed long int read_navigation_signed(std::bitset<BEIDOU_SUBFRAME_BITS> bits, const std::vector<std::pair<int,int>> parameter);
|
||||
bool read_navigation_bool(std::bitset<BEIDOU_SUBFRAME_BITS> bits, const std::vector<std::pair<int,int>> 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<int,int> almanacHealth; //!< Map that stores the health information stored in the almanac
|
||||
|
||||
std::map<int,std::string> 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
|
||||
Loading…
x
Reference in New Issue
Block a user