mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2026-10-07 00:01:50 +00:00
Add BeiDou B1C (signal 1D) signal processing and CNAV1 support.
Integrate acquisition, pilot/data tracking, B-CNAV1 telemetry decoding, and PVT/RINEX hooks on upstream/next with minimal changes to shared blocks. Includes example configuration, code generators, and unit tests. Signed-off-by: OuWenhao16 <104730917+OuWenhao16@users.noreply.github.com> Signed-off-by: Vladslav P <vladisslav2011@gmail.com>
This commit is contained in:
@@ -0,0 +1,108 @@
|
||||
; BeiDou B1C (1D) file-input example (IF = +20 kHz, Fs = 18 MHz, ibyte)
|
||||
; Test data: https://drive.google.com/drive/folders/1UkjJwOMlKEFZiXYUdK4HKzTASYHXMsHL
|
||||
; SPDX-License-Identifier: GPL-3.0-or-later
|
||||
|
||||
[GNSS-SDR]
|
||||
|
||||
;######### GLOBAL OPTIONS ##################
|
||||
GNSS-SDR.internal_fs_sps=18000000
|
||||
GNSS-SDR.use_acquisition_resampler=true
|
||||
|
||||
ControlThread.wait_for_flowgraph=false
|
||||
|
||||
;######### SIGNAL_SOURCE CONFIG ############
|
||||
SignalSource.implementation=File_Signal_Source
|
||||
SignalSource.filename=./L1_IF20KHz_FS18MHz.bin
|
||||
SignalSource.item_type=ibyte
|
||||
SignalSource.sampling_frequency=18000000
|
||||
SignalSource.samples=0
|
||||
SignalSource.enable_throttle_control=false
|
||||
|
||||
;######### SIGNAL_CONDITIONER CONFIG ############
|
||||
SignalConditioner.implementation=Signal_Conditioner
|
||||
DataTypeAdapter.implementation=Ibyte_To_Complex
|
||||
DataTypeAdapter.item_type=ibyte
|
||||
|
||||
;######### INPUT_FILTER CONFIG ############
|
||||
InputFilter.implementation=Freq_Xlating_Fir_Filter
|
||||
InputFilter.input_item_type=gr_complex
|
||||
InputFilter.output_item_type=gr_complex
|
||||
InputFilter.taps_item_type=float
|
||||
InputFilter.number_of_taps=5
|
||||
InputFilter.number_of_bands=2
|
||||
InputFilter.band1_begin=0.0
|
||||
InputFilter.band1_end=0.45
|
||||
InputFilter.band2_begin=0.55
|
||||
InputFilter.band2_end=1.0
|
||||
InputFilter.ampl1_begin=1.0
|
||||
InputFilter.ampl1_end=1.0
|
||||
InputFilter.ampl2_begin=0.0
|
||||
InputFilter.ampl2_end=0.0
|
||||
InputFilter.band1_error=1.0
|
||||
InputFilter.band2_error=1.0
|
||||
InputFilter.filter_type=bandpass
|
||||
InputFilter.grid_density=16
|
||||
InputFilter.sampling_frequency=18000000
|
||||
InputFilter.IF=20000
|
||||
|
||||
Resampler.implementation=Pass_Through
|
||||
|
||||
;######### CHANNELS GLOBAL CONFIG ############
|
||||
Channels_1D.count=6
|
||||
Channels.in_acquisition=6
|
||||
|
||||
Channel.signal=1D
|
||||
|
||||
Channel0.satellite=23
|
||||
Channel1.satellite=27
|
||||
Channel2.satellite=28
|
||||
Channel3.satellite=37
|
||||
Channel4.satellite=43
|
||||
Channel5.satellite=46
|
||||
|
||||
;######### ACQUISITION GLOBAL CONFIG ############
|
||||
Acquisition_1D.implementation=BEIDOU_B1C_PCPS_Ambiguous_Acquisition
|
||||
Acquisition_1D.item_type=gr_complex
|
||||
Acquisition_1D.acquire_pilot=true
|
||||
Acquisition_1D.qmboc=true
|
||||
Acquisition_1D.pfa=0.005
|
||||
Acquisition_1D.doppler_max=5000
|
||||
Acquisition_1D.doppler_step=25
|
||||
|
||||
;######### TRACKING GLOBAL CONFIG ############
|
||||
Tracking_1D.implementation=BEIDOU_B1C_DLL_PLL_VEML_Tracking
|
||||
Tracking_1D.item_type=gr_complex
|
||||
Tracking_1D.track_pilot=true
|
||||
Tracking_1D.extend_correlation_symbols=4
|
||||
Tracking_1D.bit_synchronization_time_limit_s=90
|
||||
Tracking_1D.pull_in_time_s=5
|
||||
Tracking_1D.pll_bw_hz=10.0
|
||||
Tracking_1D.dll_bw_hz=0.5
|
||||
Tracking_1D.pll_bw_narrow_hz=5.0
|
||||
Tracking_1D.dll_bw_narrow_hz=0.2
|
||||
Tracking_1D.pll_filter_order=2
|
||||
Tracking_1D.early_late_space_chips=0.15
|
||||
Tracking_1D.very_early_late_space_chips=0.5
|
||||
Tracking_1D.cn0_min=20
|
||||
Tracking_1D.b1c_secondary_lock_ratio=0.92
|
||||
|
||||
;######### TELEMETRY DECODER CONFIG ############
|
||||
TelemetryDecoder_1D.implementation=BEIDOU_B1C_Telemetry_Decoder
|
||||
TelemetryDecoder_1D.dump=false
|
||||
|
||||
;######### OBSERVABLES CONFIG ############
|
||||
Observables.implementation=Hybrid_Observables
|
||||
|
||||
;######### PVT CONFIG ############
|
||||
PVT.implementation=RTKLIB_PVT
|
||||
PVT.positioning_mode=Single
|
||||
PVT.output_rate_ms=100
|
||||
PVT.display_rate_ms=500
|
||||
PVT.iono_model=Broadcast
|
||||
PVT.trop_model=Saastamoinen
|
||||
PVT.flag_rtcm_server=false
|
||||
|
||||
PVT.output_enabled=true
|
||||
PVT.rinex_output_enabled=true
|
||||
PVT.nmea_output_file_enabled=true
|
||||
PVT.gpx_output_enabled=true
|
||||
@@ -244,7 +244,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
|
||||
|
||||
int num_bands = 0;
|
||||
|
||||
if (signal_enabled_flags.check_any_enabled(GPS_1C, GAL_1B, GLO_1G, BDS_B1, QZS_J1))
|
||||
if (signal_enabled_flags.check_any_enabled(GPS_1C, GAL_1B, GLO_1G, BDS_B1, BDS_B1C, QZS_J1))
|
||||
{
|
||||
num_bands += 1;
|
||||
}
|
||||
@@ -396,7 +396,7 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
|
||||
{
|
||||
nsys += SYS_GLO;
|
||||
}
|
||||
if (signal_enabled_flags.check_any_enabled(BDS_B1, BDS_B3))
|
||||
if (signal_enabled_flags.check_any_enabled(BDS_B1, BDS_B3, BDS_B1C))
|
||||
{
|
||||
nsys += SYS_BDS;
|
||||
}
|
||||
|
||||
@@ -18,6 +18,10 @@
|
||||
#include "Galileo_CNAV.h"
|
||||
#include "MATH_CONSTANTS.h"
|
||||
#include "an_packet_printer.h"
|
||||
#include "beidou_cnav1_ephemeris.h"
|
||||
#include "beidou_cnav1_iono.h"
|
||||
#include "beidou_cnav1_navigation_message.h"
|
||||
#include "beidou_cnav1_utc_model.h"
|
||||
#include "beidou_dnav_almanac.h"
|
||||
#include "beidou_dnav_ephemeris.h"
|
||||
#include "beidou_dnav_iono.h"
|
||||
@@ -166,6 +170,10 @@ rtklib_pvt_gs::rtklib_pvt_gs(uint32_t nchannels,
|
||||
d_beidou_dnav_iono_sptr_type_hash_code(typeid(std::shared_ptr<Beidou_Dnav_Iono>).hash_code()),
|
||||
d_beidou_dnav_utc_model_sptr_type_hash_code(typeid(std::shared_ptr<Beidou_Dnav_Utc_Model>).hash_code()),
|
||||
d_beidou_dnav_almanac_sptr_type_hash_code(typeid(std::shared_ptr<Beidou_Dnav_Almanac>).hash_code()),
|
||||
d_beidou_cnav1_ephemeris_sptr_type_hash_code(typeid(std::shared_ptr<Beidou_Cnav1_Ephemeris>).hash_code()),
|
||||
d_beidou_cnav1_iono_sptr_type_hash_code(typeid(std::shared_ptr<Beidou_Cnav1_Iono>).hash_code()),
|
||||
d_beidou_cnav1_utc_model_sptr_type_hash_code(typeid(std::shared_ptr<Beidou_Cnav1_Utc_Model>).hash_code()),
|
||||
d_beidou_cnav1_page_data_sptr_type_hash_code(typeid(std::shared_ptr<Beidou_Cnav1_PageData_Message>).hash_code()),
|
||||
d_galileo_has_data_sptr_type_hash_code(typeid(std::shared_ptr<Galileo_HAS_data>).hash_code()),
|
||||
d_rinex_version(conf_.rinex_version),
|
||||
d_rx_time(0.0),
|
||||
@@ -1301,6 +1309,36 @@ rtklib_pvt_gs::~rtklib_pvt_gs()
|
||||
{
|
||||
LOG(INFO) << "Failed to save BeiDou DNAV UTC model parameters, not valid data";
|
||||
}
|
||||
|
||||
// save BeiDou B-CNAV1 ephemeris to XML file
|
||||
file_name = d_xml_base_path + "bds_cnav1_ephemeris.xml";
|
||||
if (d_internal_pvt_solver->beidou_cnav1_ephemeris_map.empty() == false)
|
||||
{
|
||||
std::ofstream ofs;
|
||||
try
|
||||
{
|
||||
ofs.open(file_name.c_str(), std::ofstream::trunc | std::ofstream::out);
|
||||
boost::archive::xml_oarchive xml(ofs);
|
||||
xml << boost::serialization::make_nvp("GNSS-SDR_bds_cnav1_ephemeris_map", d_internal_pvt_solver->beidou_cnav1_ephemeris_map);
|
||||
LOG(INFO) << "Saved BeiDou B-CNAV1 Ephemeris map data";
|
||||
}
|
||||
catch (const boost::archive::archive_exception& e)
|
||||
{
|
||||
LOG(WARNING) << e.what();
|
||||
}
|
||||
catch (const std::ofstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Problem opening output XML file";
|
||||
}
|
||||
catch (const std::exception& e)
|
||||
{
|
||||
LOG(WARNING) << e.what();
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(INFO) << "Failed to save BeiDou B-CNAV1 Ephemeris, map is empty";
|
||||
}
|
||||
}
|
||||
|
||||
if (d_log_timetag_file.is_open())
|
||||
@@ -1779,6 +1817,55 @@ void rtklib_pvt_gs::msg_handler_telemetry(const pmt::pmt_t& msg)
|
||||
}
|
||||
DLOG(INFO) << "New BeiDou DNAV almanac record has arrived";
|
||||
}
|
||||
else if (msg_type_hash_code == d_beidou_cnav1_ephemeris_sptr_type_hash_code)
|
||||
{
|
||||
const auto bds_cnav1_eph = wht::any_cast<std::shared_ptr<Beidou_Cnav1_Ephemeris>>(pmt::any_ref(msg));
|
||||
if (d_rinex_output_enabled && d_rp->is_rinex_header_written())
|
||||
{
|
||||
const auto eph_it = d_internal_pvt_solver->beidou_cnav1_ephemeris_map.find(bds_cnav1_eph->PRN);
|
||||
if (eph_it == d_internal_pvt_solver->beidou_cnav1_ephemeris_map.cend() || eph_it->second.toe != bds_cnav1_eph->toe)
|
||||
{
|
||||
d_rp->log_rinex_nav_bds_cnav1({{bds_cnav1_eph->PRN, *bds_cnav1_eph}});
|
||||
}
|
||||
}
|
||||
d_internal_pvt_solver->beidou_cnav1_ephemeris_map[bds_cnav1_eph->PRN] = *bds_cnav1_eph;
|
||||
if (d_enable_rx_clock_correction == true)
|
||||
{
|
||||
d_user_pvt_solver->beidou_cnav1_ephemeris_map[bds_cnav1_eph->PRN] = *bds_cnav1_eph;
|
||||
}
|
||||
DLOG(INFO) << "New BeiDou B-CNAV1 ephemeris record has arrived from SAT ID " << bds_cnav1_eph->PRN;
|
||||
}
|
||||
else if (msg_type_hash_code == d_beidou_cnav1_iono_sptr_type_hash_code)
|
||||
{
|
||||
const auto bds_cnav1_iono = wht::any_cast<std::shared_ptr<Beidou_Cnav1_Iono>>(pmt::any_ref(msg));
|
||||
d_internal_pvt_solver->beidou_cnav1_iono = *bds_cnav1_iono;
|
||||
if (d_enable_rx_clock_correction == true)
|
||||
{
|
||||
d_user_pvt_solver->beidou_cnav1_iono = *bds_cnav1_iono;
|
||||
}
|
||||
DLOG(INFO) << "New BeiDou B-CNAV1 IONO record has arrived";
|
||||
}
|
||||
else if (msg_type_hash_code == d_beidou_cnav1_utc_model_sptr_type_hash_code)
|
||||
{
|
||||
const auto bds_cnav1_utc = wht::any_cast<std::shared_ptr<Beidou_Cnav1_Utc_Model>>(pmt::any_ref(msg));
|
||||
d_internal_pvt_solver->beidou_cnav1_utc_model = *bds_cnav1_utc;
|
||||
if (d_enable_rx_clock_correction == true)
|
||||
{
|
||||
d_user_pvt_solver->beidou_cnav1_utc_model = *bds_cnav1_utc;
|
||||
}
|
||||
DLOG(INFO) << "New BeiDou B-CNAV1 UTC record has arrived";
|
||||
}
|
||||
else if (msg_type_hash_code == d_beidou_cnav1_page_data_sptr_type_hash_code)
|
||||
{
|
||||
const auto bds_cnav1_page = wht::any_cast<std::shared_ptr<Beidou_Cnav1_PageData_Message>>(pmt::any_ref(msg));
|
||||
d_internal_pvt_solver->beidou_cnav1_page_data_map[bds_cnav1_page->PRN] = bds_cnav1_page->page_data;
|
||||
if (d_enable_rx_clock_correction == true)
|
||||
{
|
||||
d_user_pvt_solver->beidou_cnav1_page_data_map[bds_cnav1_page->PRN] = bds_cnav1_page->page_data;
|
||||
}
|
||||
DLOG(INFO) << "New BeiDou B-CNAV1 page data record has arrived from SAT ID " << bds_cnav1_page->PRN
|
||||
<< " PageID=" << bds_cnav1_page->page_data.common.page_id;
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << "msg_handler_telemetry unknown object type!";
|
||||
@@ -2237,6 +2324,7 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
const auto tmp_eph_iter_cnav = d_internal_pvt_solver->gps_cnav_ephemeris_map.find(gnss_synchro.PRN);
|
||||
const auto tmp_eph_iter_glo_gnav = d_internal_pvt_solver->glonass_gnav_ephemeris_map.find(gnss_synchro.PRN);
|
||||
const auto tmp_eph_iter_bds_dnav = d_internal_pvt_solver->beidou_dnav_ephemeris_map.find(gnss_synchro.PRN);
|
||||
const auto tmp_eph_iter_bds_cnav1 = d_internal_pvt_solver->beidou_cnav1_ephemeris_map.find(gnss_synchro.PRN);
|
||||
|
||||
bool store_valid_observable = false;
|
||||
|
||||
@@ -2337,6 +2425,14 @@ int rtklib_pvt_gs::work(int noutput_items, gr_vector_const_void_star& input_item
|
||||
store_valid_observable = true;
|
||||
}
|
||||
}
|
||||
if (!d_osnma_strict && tmp_eph_iter_bds_cnav1 != d_internal_pvt_solver->beidou_cnav1_ephemeris_map.cend())
|
||||
{
|
||||
const uint32_t prn_aux = tmp_eph_iter_bds_cnav1->second.PRN;
|
||||
if ((prn_aux == gnss_synchro.PRN) && (std::string(gnss_synchro.Signal, 2) == std::string("1D")))
|
||||
{
|
||||
store_valid_observable = true;
|
||||
}
|
||||
}
|
||||
if (std::string(gnss_synchro.Signal, 2) == std::string("E6"))
|
||||
{
|
||||
if (d_osnma_strict)
|
||||
|
||||
@@ -85,7 +85,7 @@ rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels,
|
||||
class rtklib_pvt_gs : public gr::sync_block
|
||||
{
|
||||
public:
|
||||
~rtklib_pvt_gs(); //!< Default destructor
|
||||
~rtklib_pvt_gs() override; //!< Default destructor
|
||||
|
||||
/*!
|
||||
* \brief Get latest set of GPS ephemeris from PVT block
|
||||
@@ -133,7 +133,7 @@ public:
|
||||
time_t* UTC_time) const;
|
||||
|
||||
int work(int noutput_items, gr_vector_const_void_star& input_items,
|
||||
gr_vector_void_star& output_items); //!< PVT Signal Processing
|
||||
gr_vector_void_star& output_items) override; //!< PVT Signal Processing
|
||||
|
||||
private:
|
||||
friend rtklib_pvt_gs_sptr rtklib_make_pvt_gs(uint32_t nchannels,
|
||||
@@ -241,6 +241,10 @@ private:
|
||||
const size_t d_beidou_dnav_iono_sptr_type_hash_code;
|
||||
const size_t d_beidou_dnav_utc_model_sptr_type_hash_code;
|
||||
const size_t d_beidou_dnav_almanac_sptr_type_hash_code;
|
||||
const size_t d_beidou_cnav1_ephemeris_sptr_type_hash_code;
|
||||
const size_t d_beidou_cnav1_iono_sptr_type_hash_code;
|
||||
const size_t d_beidou_cnav1_utc_model_sptr_type_hash_code;
|
||||
const size_t d_beidou_cnav1_page_data_sptr_type_hash_code;
|
||||
const size_t d_galileo_has_data_sptr_type_hash_code;
|
||||
|
||||
const double d_rinex_version;
|
||||
|
||||
@@ -18,6 +18,7 @@
|
||||
|
||||
#include "rinex_printer.h"
|
||||
#include "GLONASS_L1_L2_CA.h"
|
||||
#include "beidou_cnav1_ephemeris.h"
|
||||
#include "beidou_dnav_ephemeris.h"
|
||||
#include "beidou_dnav_iono.h"
|
||||
#include "beidou_dnav_utc_model.h"
|
||||
@@ -98,6 +99,8 @@ std::string signal_flag_to_string(signal_flag flag)
|
||||
return "B1";
|
||||
case BDS_B3:
|
||||
return "B3";
|
||||
case BDS_B1C:
|
||||
return "1D";
|
||||
case QZS_J1:
|
||||
return "J1";
|
||||
case QZS_J5:
|
||||
@@ -112,7 +115,7 @@ std::map<std::string, signal_flag> string_to_signal_flag_map()
|
||||
{
|
||||
std::map<std::string, signal_flag> convertion_map;
|
||||
|
||||
for (const auto flag : {GPS_1C, GPS_2S, GPS_L5, GAL_1B, GAL_E5a, GAL_E5b, GAL_E6, GLO_1G, GLO_2G, BDS_B1, BDS_B3, QZS_J1, QZS_J5})
|
||||
for (const auto flag : {GPS_1C, GPS_2S, GPS_L5, GAL_1B, GAL_E5a, GAL_E5b, GAL_E6, GLO_1G, GLO_2G, BDS_B1, BDS_B3, BDS_B1C, QZS_J1, QZS_J5})
|
||||
{
|
||||
convertion_map[signal_flag_to_string(flag)] = flag;
|
||||
}
|
||||
@@ -132,7 +135,7 @@ std::map<char, std::set<signal_flag>> get_constel_signal_flags(const Signal_Enab
|
||||
{
|
||||
std::map<char, std::set<signal_flag>> constel_signal_flags;
|
||||
|
||||
for (const auto& it : std::map<char, std::set<signal_flag>>{{'G', {GPS_1C, GPS_2S, GPS_L5}}, {'E', {GAL_1B, GAL_E5a, GAL_E5b, GAL_E6}}, {'R', {GLO_1G, GLO_2G}}, {'C', {BDS_B1, BDS_B3}}, {'J', {QZS_J1, QZS_J5}}})
|
||||
for (const auto& it : std::map<char, std::set<signal_flag>>{{'G', {GPS_1C, GPS_2S, GPS_L5}}, {'E', {GAL_1B, GAL_E5a, GAL_E5b, GAL_E6}}, {'R', {GLO_1G, GLO_2G}}, {'C', {BDS_B1, BDS_B3, BDS_B1C}}, {'J', {QZS_J1, QZS_J5}}})
|
||||
{
|
||||
for (const auto flag : it.second)
|
||||
{
|
||||
@@ -342,6 +345,7 @@ std::map<std::string, std::string> getObservationCodes()
|
||||
{"BEIDOU_B1_I", "1I"},
|
||||
{"BEIDOU_B1_Q", "1Q"},
|
||||
{"BEIDOU_B1_IQ", "1X"},
|
||||
{"BEIDOU_B1C_D", "1D"},
|
||||
{"BEIDOU_B3_I", "6I"},
|
||||
{"BEIDOU_B3_Q", "6Q"},
|
||||
{"BEIDOU_B3_IQ", "6X"},
|
||||
@@ -2122,6 +2126,7 @@ void add_obs_sys_obs_type_beidou(std::fstream& out,
|
||||
{
|
||||
const std::map<uint32_t, std::string> signal_to_code_map = {
|
||||
{BDS_B1, "BEIDOU_B1_I"},
|
||||
{BDS_B1C, "BEIDOU_B1C_D"},
|
||||
{BDS_B3, "BEIDOU_B3_I"},
|
||||
};
|
||||
|
||||
@@ -2492,12 +2497,14 @@ void Rinex_Printer::print_rinex_annotation(const Rtklib_Solver* pvt_solver,
|
||||
const auto gps_cnav_ephemeris_iter = pvt_solver->gps_cnav_ephemeris_map.cbegin();
|
||||
const auto glonass_gnav_ephemeris_iter = pvt_solver->glonass_gnav_ephemeris_map.cbegin();
|
||||
const auto beidou_dnav_ephemeris_iter = pvt_solver->beidou_dnav_ephemeris_map.cbegin();
|
||||
const auto beidou_cnav1_ephemeris_iter = pvt_solver->beidou_cnav1_ephemeris_map.cbegin();
|
||||
|
||||
const bool has_gps_lnav_eph = !pvt_solver->gps_ephemeris_map.empty();
|
||||
const bool has_gps_cnav_eph = !pvt_solver->gps_cnav_ephemeris_map.empty();
|
||||
const bool has_galileo_eph = !pvt_solver->galileo_ephemeris_map.empty();
|
||||
const bool has_glonass_eph = !pvt_solver->glonass_gnav_ephemeris_map.empty();
|
||||
const bool has_beidou_dnav_eph = !pvt_solver->beidou_dnav_ephemeris_map.empty();
|
||||
const bool has_beidou_cnav1_eph = !pvt_solver->beidou_cnav1_ephemeris_map.empty();
|
||||
|
||||
// We require at least one ephemeris for an active signal of a constellation
|
||||
// Note: this is currently required because ephemeris are used when creating the headers,
|
||||
@@ -2520,7 +2527,7 @@ void Rinex_Printer::print_rinex_annotation(const Rtklib_Solver* pvt_solver,
|
||||
{
|
||||
return;
|
||||
}
|
||||
if (d_flags.has_beidou && !has_beidou_dnav_eph)
|
||||
if (d_flags.has_beidou && !has_beidou_dnav_eph && !has_beidou_cnav1_eph)
|
||||
{
|
||||
return;
|
||||
}
|
||||
@@ -2557,7 +2564,14 @@ void Rinex_Printer::print_rinex_annotation(const Rtklib_Solver* pvt_solver,
|
||||
}
|
||||
else if (d_flags.has_beidou)
|
||||
{
|
||||
system_time = Rinex_Printer::compute_BDS_time(beidou_dnav_ephemeris_iter->second, rx_time);
|
||||
if (has_beidou_cnav1_eph)
|
||||
{
|
||||
system_time = Rinex_Printer::compute_BDS_time(beidou_cnav1_ephemeris_iter->second, rx_time);
|
||||
}
|
||||
else
|
||||
{
|
||||
system_time = Rinex_Printer::compute_BDS_time(beidou_dnav_ephemeris_iter->second, rx_time);
|
||||
}
|
||||
seconds = fmod(rx_time, 60);
|
||||
system_time_str = "BDS";
|
||||
}
|
||||
@@ -2706,6 +2720,10 @@ void Rinex_Printer::print_rinex_annotation(const Rtklib_Solver* pvt_solver,
|
||||
{
|
||||
log_rinex_nav_bds_dnav(pvt_solver->beidou_dnav_ephemeris_map);
|
||||
}
|
||||
if (has_beidou_cnav1_eph)
|
||||
{
|
||||
log_rinex_nav_bds_cnav1(pvt_solver->beidou_cnav1_ephemeris_map);
|
||||
}
|
||||
|
||||
d_rinex_header_written = true;
|
||||
}
|
||||
@@ -3483,6 +3501,39 @@ void Rinex_Printer::log_rinex_nav_bds_dnav(const std::map<int32_t, Beidou_Dnav_E
|
||||
}
|
||||
|
||||
|
||||
void Rinex_Printer::log_rinex_nav_bds_cnav1(const std::map<int32_t, Beidou_Cnav1_Ephemeris>& new_bds_eph)
|
||||
{
|
||||
auto& out = navFile;
|
||||
const auto& sys_char = satelliteSystem.at("Beidou");
|
||||
|
||||
for (const auto& bds_ephemeris_iter : new_bds_eph)
|
||||
{
|
||||
const auto& eph = bds_ephemeris_iter.second;
|
||||
|
||||
const boost::posix_time::ptime p_utc_time = Rinex_Printer::compute_BDS_time(eph, eph.toc);
|
||||
out << get_nav_sv_epoch_svclk_line(p_utc_time, sys_char, eph.PRN, eph.af0, eph.af1, eph.af2) << '\n';
|
||||
|
||||
const auto iode_d = eph.IODE;
|
||||
out << get_nav_broadcast_orbit(&iode_d, &eph.Crs, &eph.delta_n, &eph.M_0) << '\n';
|
||||
out << get_nav_broadcast_orbit(&eph.Cuc, &eph.ecc, &eph.Cus, &eph.sqrtA) << '\n';
|
||||
|
||||
const auto toe_d = static_cast<double>(eph.toe);
|
||||
out << get_nav_broadcast_orbit(&toe_d, &eph.Cic, &eph.OMEGA_0, &eph.Cis) << '\n';
|
||||
out << get_nav_broadcast_orbit(&eph.i_0, &eph.Crc, &eph.omega, &eph.OMEGAdot) << '\n';
|
||||
|
||||
const auto wn_d = static_cast<double>(eph.WN);
|
||||
out << get_nav_broadcast_orbit(&eph.idot, nullptr, &wn_d, nullptr) << '\n';
|
||||
|
||||
const double zero_health = 0.0;
|
||||
out << get_nav_broadcast_orbit(&zero_health, &zero_health, &eph.TGD_B1Cp, &eph.TGD_B2ap) << '\n';
|
||||
|
||||
const auto tow_d = static_cast<double>(eph.tow);
|
||||
const auto iodc_d = eph.IODC;
|
||||
out << get_nav_broadcast_orbit(&tow_d, &iodc_d, nullptr, nullptr) << '\n';
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Rinex_Printer::rinex_nav_header(std::fstream& out,
|
||||
const std::vector<std::string>& iono_lines,
|
||||
const std::vector<std::string>& time_corr_lines,
|
||||
@@ -3828,6 +3879,15 @@ boost::posix_time::ptime Rinex_Printer::compute_BDS_time(const Beidou_Dnav_Ephem
|
||||
}
|
||||
|
||||
|
||||
boost::posix_time::ptime Rinex_Printer::compute_BDS_time(const Beidou_Cnav1_Ephemeris& eph, double obs_time) const
|
||||
{
|
||||
const double bds_t = obs_time;
|
||||
const boost::posix_time::time_duration t = boost::posix_time::milliseconds(static_cast<int64_t>((bds_t + 604800 * static_cast<double>(eph.WN % 8192)) * 1000));
|
||||
boost::posix_time::ptime p_time(boost::gregorian::date(2006, 1, 1), t);
|
||||
return p_time;
|
||||
}
|
||||
|
||||
|
||||
boost::posix_time::ptime Rinex_Printer::compute_GPS_time(const Gps_Ephemeris& eph, double obs_time) const
|
||||
{
|
||||
// The RINEX v2.11 v3.00 format uses GPS time for the observations epoch, not UTC time, thus, no leap seconds needed here.
|
||||
|
||||
@@ -55,6 +55,7 @@
|
||||
* \{ */
|
||||
|
||||
|
||||
class Beidou_Cnav1_Ephemeris;
|
||||
class Beidou_Dnav_Ephemeris;
|
||||
class Beidou_Dnav_Iono;
|
||||
class Beidou_Dnav_Utc_Model;
|
||||
@@ -134,6 +135,11 @@ public:
|
||||
*/
|
||||
void log_rinex_nav_bds_dnav(const std::map<int32_t, Beidou_Dnav_Ephemeris>& new_bds_eph);
|
||||
|
||||
/*!
|
||||
* \brief Print RINEX annotation for BeiDou B-CNAV1 message
|
||||
*/
|
||||
void log_rinex_nav_bds_cnav1(const std::map<int32_t, Beidou_Cnav1_Ephemeris>& new_bds_eph);
|
||||
|
||||
/*!
|
||||
* \brief Returns true is the RINEX file headers are already written
|
||||
*/
|
||||
@@ -200,6 +206,7 @@ private:
|
||||
* \param obs_time Observation time in BDT seconds of week
|
||||
*/
|
||||
boost::posix_time::ptime compute_BDS_time(const Beidou_Dnav_Ephemeris& eph, double obs_time) const;
|
||||
boost::posix_time::ptime compute_BDS_time(const Beidou_Cnav1_Ephemeris& eph, double obs_time) const;
|
||||
|
||||
/*
|
||||
* Computes the UTC time and returns a boost::posix_time::ptime object
|
||||
|
||||
@@ -73,6 +73,7 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
|
||||
d_rtklib_band_index["1C"] = 0;
|
||||
d_rtklib_band_index["1B"] = 0;
|
||||
d_rtklib_band_index["B1"] = 0;
|
||||
d_rtklib_band_index["1D"] = 0;
|
||||
d_rtklib_band_index["B3"] = 2;
|
||||
d_rtklib_band_index["2G"] = 1;
|
||||
d_rtklib_band_index["2S"] = 1;
|
||||
@@ -1798,6 +1799,24 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->first;
|
||||
}
|
||||
}
|
||||
if (sig_ == "1D")
|
||||
{
|
||||
const auto cnav1_iter = beidou_cnav1_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (cnav1_iter != beidou_cnav1_ephemeris_map.cend())
|
||||
{
|
||||
eph_data[valid_obs] = eph_to_rtklib(cnav1_iter->second);
|
||||
obsd_t newobs{};
|
||||
d_obs_data[valid_obs + glo_valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
cnav1_iter->second.WN + BEIDOU_DNAV_BDT2GPST_WEEK_NUM_OFFSET,
|
||||
d_rtklib_band_index[sig_]);
|
||||
valid_obs++;
|
||||
}
|
||||
else
|
||||
{
|
||||
DLOG(INFO) << "No B-CNAV1 ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
// BeiDou B3
|
||||
if (sig_ == "B3")
|
||||
{
|
||||
@@ -1926,6 +1945,18 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
|
||||
d_nav_data.ion_cmp[6] = beidou_dnav_iono.beta2;
|
||||
d_nav_data.ion_cmp[7] = beidou_dnav_iono.beta3;
|
||||
}
|
||||
else if (beidou_cnav1_iono.valid)
|
||||
{
|
||||
// B-CNAV1 BDGIM parameters mapped to RTKLIB ion_cmp slots (best-effort until BDGIM is native).
|
||||
d_nav_data.ion_cmp[0] = beidou_cnav1_iono.alpha1;
|
||||
d_nav_data.ion_cmp[1] = beidou_cnav1_iono.alpha2;
|
||||
d_nav_data.ion_cmp[2] = beidou_cnav1_iono.alpha3;
|
||||
d_nav_data.ion_cmp[3] = beidou_cnav1_iono.alpha4;
|
||||
d_nav_data.ion_cmp[4] = beidou_cnav1_iono.alpha5;
|
||||
d_nav_data.ion_cmp[5] = beidou_cnav1_iono.alpha6;
|
||||
d_nav_data.ion_cmp[6] = beidou_cnav1_iono.alpha7;
|
||||
d_nav_data.ion_cmp[7] = beidou_cnav1_iono.alpha8;
|
||||
}
|
||||
if (gps_utc_model.valid)
|
||||
{
|
||||
d_nav_data.utc_gps[0] = gps_utc_model.A0;
|
||||
@@ -1979,6 +2010,14 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
|
||||
d_nav_data.utc_cmp[3] = 0.0; // ??
|
||||
d_nav_data.leaps = beidou_dnav_utc_model.DeltaT_LS;
|
||||
}
|
||||
else if (beidou_cnav1_utc_model.valid)
|
||||
{
|
||||
d_nav_data.utc_cmp[0] = beidou_cnav1_utc_model.A0;
|
||||
d_nav_data.utc_cmp[1] = beidou_cnav1_utc_model.A1;
|
||||
d_nav_data.utc_cmp[2] = static_cast<double>(beidou_cnav1_utc_model.tot);
|
||||
d_nav_data.utc_cmp[3] = static_cast<double>(beidou_cnav1_utc_model.WN_t);
|
||||
d_nav_data.leaps = beidou_cnav1_utc_model.delta_t_LSF;
|
||||
}
|
||||
|
||||
/* update carrier wave length using native function call in RTKlib */
|
||||
for (int i = 0; i < MAXSAT; i++)
|
||||
|
||||
@@ -35,6 +35,10 @@
|
||||
#define GNSS_SDR_RTKLIB_SOLVER_H
|
||||
|
||||
|
||||
#include "beidou_cnav1_ephemeris.h"
|
||||
#include "beidou_cnav1_iono.h"
|
||||
#include "beidou_cnav1_navigation_message.h"
|
||||
#include "beidou_cnav1_utc_model.h"
|
||||
#include "beidou_dnav_almanac.h"
|
||||
#include "beidou_dnav_ephemeris.h"
|
||||
#include "beidou_dnav_iono.h"
|
||||
@@ -125,6 +129,7 @@ public:
|
||||
std::map<int, Gps_CNAV_Ephemeris> gps_cnav_ephemeris_map; //!< Map storing new GPS_CNAV_Ephemeris
|
||||
std::map<int, Glonass_Gnav_Ephemeris> glonass_gnav_ephemeris_map; //!< Map storing new GLONASS GNAV Ephemeris
|
||||
std::map<int, Beidou_Dnav_Ephemeris> beidou_dnav_ephemeris_map; //!< Map storing new BeiDou DNAV Ephmeris
|
||||
std::map<int, Beidou_Cnav1_Ephemeris> beidou_cnav1_ephemeris_map; //!< Map storing BeiDou B-CNAV1 ephemeris
|
||||
|
||||
Galileo_Utc_Model galileo_utc_model;
|
||||
Galileo_Iono galileo_iono;
|
||||
@@ -148,7 +153,10 @@ public:
|
||||
|
||||
Beidou_Dnav_Utc_Model beidou_dnav_utc_model;
|
||||
Beidou_Dnav_Iono beidou_dnav_iono;
|
||||
Beidou_Cnav1_Iono beidou_cnav1_iono;
|
||||
Beidou_Cnav1_Utc_Model beidou_cnav1_utc_model;
|
||||
std::map<int, Beidou_Dnav_Almanac> beidou_dnav_almanac_map;
|
||||
std::map<int, Bds3_B1c_PageData> beidou_cnav1_page_data_map;
|
||||
|
||||
private:
|
||||
friend class GalileoEphemerisSourceTest_E6SlotsFollowRtklibGalileoPolicy_Test;
|
||||
|
||||
@@ -36,6 +36,7 @@ uint32_t flags_from_config(const ConfigurationInterface* configuration)
|
||||
{GLO_2G, "Channels_2G.count"},
|
||||
{BDS_B1, "Channels_B1.count"},
|
||||
{BDS_B3, "Channels_B3.count"},
|
||||
{BDS_B1C, "Channels_1D.count"},
|
||||
{QZS_J1, "Channels_J1.count"},
|
||||
{QZS_J5, "Channels_J5.count"}};
|
||||
|
||||
@@ -66,7 +67,7 @@ Signal_Enabled_Flags::Signal_Enabled_Flags(uint32_t flags_)
|
||||
has_gps(check_any_enabled(GPS_1C, GPS_2S, GPS_L5)),
|
||||
has_galileo(check_any_enabled(GAL_1B, GAL_E5a, GAL_E5b, GAL_E6)),
|
||||
has_glonass(check_any_enabled(GLO_1G, GLO_2G)),
|
||||
has_beidou(check_any_enabled(BDS_B1, BDS_B3)),
|
||||
has_beidou(check_any_enabled(BDS_B1, BDS_B3, BDS_B1C)),
|
||||
has_qzss(check_any_enabled(QZS_J1, QZS_J5)),
|
||||
only_gps(has_gps && !(has_galileo || has_glonass || has_beidou || has_qzss)),
|
||||
only_galileo(has_galileo && !(has_gps || has_glonass || has_beidou || has_qzss)),
|
||||
|
||||
@@ -8,11 +8,15 @@
|
||||
set(ACQ_ADAPTER_SOURCES
|
||||
pcps_acquisition_adapter.cc
|
||||
pcps_acquisition_adapter_custom.cc
|
||||
base_pcps_acquisition.cc
|
||||
beidou_b1c_pcps_ambiguous_acquisition.cc
|
||||
)
|
||||
|
||||
set(ACQ_ADAPTER_HEADERS
|
||||
pcps_acquisition_adapter.h
|
||||
pcps_acquisition_adapter_custom.h
|
||||
base_pcps_acquisition.h
|
||||
beidou_b1c_pcps_ambiguous_acquisition.h
|
||||
)
|
||||
|
||||
if(ENABLE_FPGA)
|
||||
|
||||
@@ -0,0 +1,222 @@
|
||||
/*!
|
||||
* \file base_ca_pcps_acquisition.h
|
||||
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface
|
||||
* \authors <ul>
|
||||
* <li> Mathieu Favreau, 2025. favreau.mathieu(at)hotmail.com
|
||||
* </ul>
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2025 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "base_pcps_acquisition.h"
|
||||
#include "acq_conf.h"
|
||||
#include "configuration_interface.h"
|
||||
#include "gnss_sdr_flags.h"
|
||||
|
||||
#if USE_GLOG_AND_GFLAGS
|
||||
#include <glog/logging.h>
|
||||
#else
|
||||
#include <absl/log/log.h>
|
||||
#endif
|
||||
|
||||
#if HAS_STD_SPAN
|
||||
#include <span>
|
||||
namespace own = std;
|
||||
#else
|
||||
#include <gsl-lite/gsl-lite.hpp>
|
||||
namespace own = gsl_lite;
|
||||
#endif
|
||||
|
||||
namespace
|
||||
{
|
||||
Acq_Conf get_acq_conf(const ConfigurationInterface* configuration, const std::string& role, double chip_rate, double opt_freq, uint32_t ms_per_code)
|
||||
{
|
||||
Acq_Conf acq_parameters;
|
||||
acq_parameters.ms_per_code = ms_per_code;
|
||||
acq_parameters.sampled_ms = ms_per_code; // Set as default value
|
||||
acq_parameters.SetFromConfiguration(configuration, role, chip_rate, opt_freq);
|
||||
|
||||
#if USE_GLOG_AND_GFLAGS
|
||||
if (FLAGS_doppler_max != 0)
|
||||
{
|
||||
acq_parameters.doppler_max = FLAGS_doppler_max;
|
||||
}
|
||||
if (FLAGS_doppler_step != 0)
|
||||
{
|
||||
acq_parameters.doppler_step = FLAGS_doppler_step;
|
||||
}
|
||||
#else
|
||||
if (absl::GetFlag(FLAGS_doppler_max) != 0)
|
||||
{
|
||||
acq_parameters.doppler_max = absl::GetFlag(FLAGS_doppler_max);
|
||||
}
|
||||
if (absl::GetFlag(FLAGS_doppler_step) != 0)
|
||||
{
|
||||
acq_parameters.doppler_step = absl::GetFlag(FLAGS_doppler_step);
|
||||
}
|
||||
#endif
|
||||
|
||||
return acq_parameters;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
BasePcpsAcquisition::BasePcpsAcquisition(
|
||||
const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams,
|
||||
double chip_rate,
|
||||
double opt_freq,
|
||||
double code_length_chips,
|
||||
uint32_t ms_per_code) : acq_parameters_(get_acq_conf(configuration, role, chip_rate, opt_freq, ms_per_code)),
|
||||
gnss_synchro_(nullptr),
|
||||
role_(role),
|
||||
vector_length_(std::floor(acq_parameters_.sampled_ms * acq_parameters_.samples_per_ms) * (acq_parameters_.bit_transition_flag ? 2.0 : 1.0)),
|
||||
code_length_(static_cast<unsigned int>(std::floor(static_cast<double>(acq_parameters_.resampled_fs) / (chip_rate / code_length_chips)))),
|
||||
code_(vector_length_),
|
||||
acquisition_(pcps_make_acquisition(acq_parameters_))
|
||||
{
|
||||
DLOG(INFO) << "role " << role;
|
||||
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
|
||||
|
||||
if (acq_parameters_.item_type == "cbyte")
|
||||
{
|
||||
cbyte_to_float_x2_ = make_complex_byte_to_float_x2();
|
||||
float_to_complex_ = gr::blocks::float_to_complex::make();
|
||||
}
|
||||
|
||||
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";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void BasePcpsAcquisition::stop_acquisition()
|
||||
{
|
||||
acquisition_->set_active(false);
|
||||
}
|
||||
|
||||
|
||||
void BasePcpsAcquisition::set_doppler_center(int doppler_center)
|
||||
{
|
||||
acquisition_->set_doppler_center(doppler_center);
|
||||
}
|
||||
|
||||
|
||||
void BasePcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
|
||||
{
|
||||
gnss_synchro_ = gnss_synchro;
|
||||
acquisition_->set_gnss_synchro(gnss_synchro_);
|
||||
}
|
||||
|
||||
|
||||
signed int BasePcpsAcquisition::mag()
|
||||
{
|
||||
return acquisition_->mag();
|
||||
}
|
||||
|
||||
|
||||
void BasePcpsAcquisition::reset()
|
||||
{
|
||||
acquisition_->set_active(true);
|
||||
}
|
||||
|
||||
|
||||
void BasePcpsAcquisition::connect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if (acq_parameters_.item_type == "gr_complex" || acq_parameters_.item_type == "cshort")
|
||||
{
|
||||
// nothing to connect
|
||||
}
|
||||
else if (acq_parameters_.item_type == "cbyte")
|
||||
{
|
||||
// Since a byte-based acq implementation is not available,
|
||||
// we just convert cshorts to gr_complex
|
||||
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, acquisition_, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << acq_parameters_.item_type << " unknown acquisition item type";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void BasePcpsAcquisition::disconnect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if (acq_parameters_.item_type == "gr_complex" || acq_parameters_.item_type == "cshort")
|
||||
{
|
||||
// nothing to disconnect
|
||||
}
|
||||
else if (acq_parameters_.item_type == "cbyte")
|
||||
{
|
||||
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, acquisition_, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << acq_parameters_.item_type << " unknown acquisition item type";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr BasePcpsAcquisition::get_left_block()
|
||||
{
|
||||
if (acq_parameters_.item_type == "gr_complex" || acq_parameters_.item_type == "cshort")
|
||||
{
|
||||
return acquisition_;
|
||||
}
|
||||
if (acq_parameters_.item_type == "cbyte")
|
||||
{
|
||||
return cbyte_to_float_x2_;
|
||||
}
|
||||
|
||||
LOG(WARNING) << acq_parameters_.item_type << " unknown acquisition item type";
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr BasePcpsAcquisition::get_right_block()
|
||||
{
|
||||
return acquisition_;
|
||||
}
|
||||
|
||||
|
||||
void BasePcpsAcquisition::set_resampler_latency(uint32_t latency_samples)
|
||||
{
|
||||
acquisition_->set_resampler_latency(latency_samples);
|
||||
}
|
||||
|
||||
|
||||
void BasePcpsAcquisition::set_local_code()
|
||||
{
|
||||
volk_gnsssdr::vector<std::complex<float>> code(code_length_);
|
||||
|
||||
const auto sampling_freq = acq_parameters_.use_automatic_resampler ? acq_parameters_.resampled_fs : acq_parameters_.fs_in;
|
||||
code_gen_complex_sampled(code, gnss_synchro_->PRN, sampling_freq);
|
||||
|
||||
const auto num_codes = acq_parameters_.sampled_ms / acq_parameters_.ms_per_code;
|
||||
|
||||
own::span<gr_complex> code_span(code_.data(), vector_length_);
|
||||
for (unsigned int i = 0; i < num_codes; i++)
|
||||
{
|
||||
std::copy_n(code.data(), code_length_, code_span.subspan(i * code_length_, code_length_).data());
|
||||
}
|
||||
|
||||
acquisition_->set_local_code(code_.data());
|
||||
}
|
||||
@@ -0,0 +1,146 @@
|
||||
/*!
|
||||
* \file base_ca_pcps_acquisition.h
|
||||
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface
|
||||
* \authors <ul>
|
||||
* <li> Mathieu Favreau, 2025. favreau.mathieu(at)hotmail.com
|
||||
* </ul>
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2025 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BASE_PCPS_ACQUISITION_H
|
||||
#define GNSS_SDR_BASE_PCPS_ACQUISITION_H
|
||||
|
||||
#include "acq_conf.h"
|
||||
#include "channel_fsm.h"
|
||||
#include "complex_byte_to_float_x2.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "pcps_acquisition.h"
|
||||
#include <gnuradio/blocks/float_to_complex.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr_alloc.h>
|
||||
|
||||
/** \addtogroup Acquisition
|
||||
* Classes for GNSS signal acquisition
|
||||
* \{ */
|
||||
/** \addtogroup Acq_adapters acquisition_adapters
|
||||
* Wrap GNU Radio acquisition blocks with an AcquisitionInterface
|
||||
* \{ */
|
||||
|
||||
|
||||
class ConfigurationInterface;
|
||||
|
||||
/*!
|
||||
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
|
||||
*/
|
||||
class BasePcpsAcquisition : public AcquisitionInterface
|
||||
{
|
||||
public:
|
||||
BasePcpsAcquisition(
|
||||
const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams,
|
||||
double chip_rate,
|
||||
double opt_freq,
|
||||
double code_length_chips,
|
||||
uint32_t ms_per_code);
|
||||
|
||||
~BasePcpsAcquisition() override = default;
|
||||
|
||||
inline std::string role() override
|
||||
{
|
||||
return role_;
|
||||
}
|
||||
|
||||
inline size_t item_size() override
|
||||
{
|
||||
return acq_parameters_.it_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
|
||||
*/
|
||||
inline void set_channel(unsigned int channel) override
|
||||
{
|
||||
acquisition_->set_channel(channel);
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Set channel fsm associated to this acquisition instance
|
||||
*/
|
||||
inline void set_channel_fsm(std::weak_ptr<ChannelFsm> channel_fsm) override
|
||||
{
|
||||
acquisition_->set_channel_fsm(std::move(channel_fsm));
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Set Doppler center for the grid search
|
||||
*/
|
||||
void set_doppler_center(int doppler_center) override;
|
||||
|
||||
/*!
|
||||
* \brief Returns the maximum peak of grid search
|
||||
*/
|
||||
signed int mag() override;
|
||||
|
||||
/*!
|
||||
* \brief Restart acquisition algorithm
|
||||
*/
|
||||
void reset() override;
|
||||
|
||||
/*!
|
||||
* \brief Stop running acquisition
|
||||
*/
|
||||
void stop_acquisition() override;
|
||||
|
||||
/*!
|
||||
* \brief Sets the resampler latency to account it in the acquisition code delay estimation
|
||||
*/
|
||||
void set_resampler_latency(uint32_t latency_samples) override;
|
||||
|
||||
/*!
|
||||
* \brief Sets local code
|
||||
*/
|
||||
void set_local_code() override;
|
||||
|
||||
private:
|
||||
/*!
|
||||
* \brief Generate code
|
||||
*/
|
||||
virtual void code_gen_complex_sampled(own::span<std::complex<float>> dest, uint32_t prn, int32_t sampling_freq) = 0;
|
||||
|
||||
const Acq_Conf acq_parameters_;
|
||||
gr::blocks::float_to_complex::sptr float_to_complex_;
|
||||
complex_byte_to_float_x2_sptr cbyte_to_float_x2_;
|
||||
Gnss_Synchro* gnss_synchro_;
|
||||
const std::string role_;
|
||||
const unsigned int vector_length_;
|
||||
const unsigned int code_length_;
|
||||
volk_gnsssdr::vector<std::complex<float>> code_;
|
||||
pcps_acquisition_sptr acquisition_;
|
||||
};
|
||||
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_BASE_PCPS_ACQUISITION_H
|
||||
@@ -0,0 +1,64 @@
|
||||
/*!
|
||||
* \file beidou_b1c_pcps_ambiguous_acquisition.cc
|
||||
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
|
||||
* BeiDou B1C Signals
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "beidou_b1c_pcps_ambiguous_acquisition.h"
|
||||
#include "Beidou_B1C.h"
|
||||
#include "beidou_b1c_signal_replica.h"
|
||||
|
||||
BeidouB1cPcpsAmbiguousAcquisition::BeidouB1cPcpsAmbiguousAcquisition(
|
||||
const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams)
|
||||
: BasePcpsAcquisition(configuration,
|
||||
role,
|
||||
in_streams,
|
||||
out_streams,
|
||||
BEIDOU_B1C_CODE_RATE_CPS,
|
||||
BEIDOU_B1C_OPT_ACQ_FS_SPS,
|
||||
BEIDOU_B1C_CODE_LENGTH_CHIPS,
|
||||
BEIDOU_B1C_CODE_PERIOD_MS),
|
||||
acquire_pilot_(configuration->property(role + ".acquire_pilot", true)),
|
||||
qmboc_(configuration->property(role + ".qmboc", true)),
|
||||
gnss_synchro_(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void BeidouB1cPcpsAmbiguousAcquisition::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
|
||||
{
|
||||
gnss_synchro_ = p_gnss_synchro;
|
||||
BasePcpsAcquisition::set_gnss_synchro(p_gnss_synchro);
|
||||
}
|
||||
|
||||
|
||||
void BeidouB1cPcpsAmbiguousAcquisition::code_gen_complex_sampled(own::span<std::complex<float>> dest, uint32_t prn, int32_t sampling_freq)
|
||||
{
|
||||
if (acquire_pilot_)
|
||||
{
|
||||
const std::array<char, 3> pilot_signal = {{'1', 'P', '\0'}};
|
||||
beidou_b1c_code_gen_complex_sampled(dest, pilot_signal, qmboc_, prn, sampling_freq, 0, false);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::array<char, 3> signal_id{};
|
||||
signal_id[0] = gnss_synchro_->Signal[0];
|
||||
signal_id[1] = gnss_synchro_->Signal[1];
|
||||
signal_id[2] = '\0';
|
||||
beidou_b1c_code_gen_complex_sampled(dest, signal_id, qmboc_, prn, sampling_freq, 0, false);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
/*!
|
||||
* \file beidou_b1c_pcps_ambiguous_acquisition.h
|
||||
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
|
||||
* BeiDou B1C Signals
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_B1C_PCPS_AMBIGUOUS_ACQUISITION_H
|
||||
#define GNSS_SDR_BEIDOU_B1C_PCPS_AMBIGUOUS_ACQUISITION_H
|
||||
|
||||
#include "base_pcps_acquisition.h"
|
||||
|
||||
/** \addtogroup Acquisition
|
||||
* \{ */
|
||||
/** \addtogroup Acq_adapters
|
||||
* \{ */
|
||||
|
||||
class BeidouB1cPcpsAmbiguousAcquisition : public BasePcpsAcquisition
|
||||
{
|
||||
public:
|
||||
BeidouB1cPcpsAmbiguousAcquisition(
|
||||
const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams);
|
||||
|
||||
~BeidouB1cPcpsAmbiguousAcquisition() override = default;
|
||||
|
||||
inline std::string implementation() override
|
||||
{
|
||||
return "BEIDOU_B1C_PCPS_Ambiguous_Acquisition";
|
||||
}
|
||||
|
||||
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override;
|
||||
|
||||
private:
|
||||
void code_gen_complex_sampled(own::span<std::complex<float>> dest, uint32_t prn, int32_t sampling_freq) override;
|
||||
|
||||
const bool acquire_pilot_;
|
||||
const bool qmboc_;
|
||||
Gnss_Synchro* gnss_synchro_;
|
||||
};
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_BEIDOU_B1C_PCPS_AMBIGUOUS_ACQUISITION_H
|
||||
@@ -9,6 +9,7 @@ add_subdirectory(rtklib)
|
||||
|
||||
set(GNSS_SPLIBS_SOURCES
|
||||
beidou_b1i_signal_replica.cc
|
||||
beidou_b1c_signal_replica.cc
|
||||
beidou_b3i_signal_replica.cc
|
||||
galileo_e1_signal_replica.cc
|
||||
galileo_e5_signal_replica.cc
|
||||
@@ -37,6 +38,7 @@ set(GNSS_SPLIBS_SOURCES
|
||||
|
||||
set(GNSS_SPLIBS_HEADERS
|
||||
beidou_b1i_signal_replica.h
|
||||
beidou_b1c_signal_replica.h
|
||||
beidou_b3i_signal_replica.h
|
||||
galileo_e1_signal_replica.h
|
||||
galileo_e5_signal_replica.h
|
||||
|
||||
@@ -0,0 +1,246 @@
|
||||
/*!
|
||||
* \file beidou_b1c_signal_replica.cc
|
||||
* \brief Library for BeiDou B1C signal replica generation
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* Primary codes loaded from precomputed hex tables (Galileo E1 style).
|
||||
* Pilot secondary codes from per-PRN binary strings (Galileo E5a-Q style).
|
||||
* BDS-SIS-ICD-B1C-1.0 §5.2.
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "beidou_b1c_signal_replica.h"
|
||||
#include "Beidou_B1C.h"
|
||||
#include "gnss_signal_replica.h"
|
||||
#include <cmath>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace
|
||||
{
|
||||
void hex_primary_to_chip_int(own::span<int32_t> dest, const char* hex_code)
|
||||
{
|
||||
int32_t index = 0;
|
||||
for (size_t i = 0; i < BEIDOU_B1C_PRIMARY_CODE_STR_LENGTH - 1; i++)
|
||||
{
|
||||
std::array<int32_t, 4> bipolar{};
|
||||
hex_to_binary_converter(bipolar, hex_code[i]);
|
||||
for (int32_t j = 0; j < 4; j++)
|
||||
{
|
||||
dest[index++] = (bipolar[j] == 1) ? 0 : 1;
|
||||
}
|
||||
}
|
||||
std::array<int32_t, 4> last{};
|
||||
hex_to_binary_converter(last, hex_code[BEIDOU_B1C_PRIMARY_CODE_STR_LENGTH - 1]);
|
||||
dest[index++] = (last[0] == 1) ? 0 : 1;
|
||||
dest[index++] = (last[1] == 1) ? 0 : 1;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
|
||||
void beidou_b1c_code_gen_int(own::span<int32_t> dest, const std::array<char, 3>& signal_id, int32_t prn)
|
||||
{
|
||||
if (prn < 1 || prn > BEIDOU_B1C_NUMBER_OF_PRNS)
|
||||
{
|
||||
return;
|
||||
}
|
||||
const int32_t prn_index = prn - 1;
|
||||
const std::string sig = signal_id.data();
|
||||
const char* hex_code = nullptr;
|
||||
if (sig.rfind("1P") != std::string::npos)
|
||||
{
|
||||
hex_code = BEIDOU_B1C_PILOT_PRIMARY_CODE[prn_index];
|
||||
}
|
||||
else
|
||||
{
|
||||
hex_code = BEIDOU_B1C_DATA_PRIMARY_CODE[prn_index];
|
||||
}
|
||||
hex_primary_to_chip_int(dest, hex_code);
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_sinboc_11_gen_int(own::span<int32_t> dest, own::span<const int32_t> prn)
|
||||
{
|
||||
constexpr auto length_in = static_cast<uint32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS);
|
||||
const auto period = static_cast<uint32_t>(dest.size() / length_in);
|
||||
for (uint32_t i = 0; i < length_in; i++)
|
||||
{
|
||||
for (uint32_t j = 0; j < (period / 2); j++)
|
||||
{
|
||||
dest[i * period + j] = prn[i];
|
||||
}
|
||||
for (uint32_t j = (period / 2); j < period; j++)
|
||||
{
|
||||
dest[i * period + j] = -prn[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_sinboc_61_gen_int(own::span<int32_t> dest, own::span<const int32_t> prn)
|
||||
{
|
||||
constexpr auto length_in = static_cast<uint32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS);
|
||||
const auto period = static_cast<uint32_t>(dest.size() / length_in);
|
||||
for (uint32_t i = 0; i < length_in; i++)
|
||||
{
|
||||
for (uint32_t j = 0; j < period; j += 2)
|
||||
{
|
||||
dest[i * period + j] = prn[i];
|
||||
}
|
||||
for (uint32_t j = 1; j < period; j += 2)
|
||||
{
|
||||
dest[i * period + j] = -prn[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_qmboc_gen_float(own::span<float> dest, own::span<const int32_t> prn, bool pilot)
|
||||
{
|
||||
const auto code_length = dest.size();
|
||||
const float alpha = std::sqrt(29.0F / 44.0F);
|
||||
const float beta = std::sqrt(4.0F / 44.0F);
|
||||
std::vector<int32_t> sinboc_11(code_length);
|
||||
std::vector<int32_t> sinboc_61(code_length);
|
||||
beidou_b1c_sinboc_11_gen_int(sinboc_11, prn);
|
||||
beidou_b1c_sinboc_61_gen_int(sinboc_61, prn);
|
||||
if (pilot)
|
||||
{
|
||||
for (size_t i = 0; i < code_length; i++)
|
||||
{
|
||||
dest[i] = alpha * static_cast<float>(sinboc_11[i]) + beta * static_cast<float>(sinboc_61[i]);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
for (size_t i = 0; i < code_length; i++)
|
||||
{
|
||||
dest[i] = static_cast<float>(sinboc_11[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_code_gen_sinboc11_float(own::span<float> dest, const std::array<char, 3>& signal_id, uint32_t prn)
|
||||
{
|
||||
std::array<int32_t, 10230> primary_code{};
|
||||
beidou_b1c_code_gen_int(primary_code, signal_id, static_cast<int32_t>(prn));
|
||||
const auto code_length = static_cast<uint32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS);
|
||||
for (uint32_t i = 0; i < code_length; i++)
|
||||
{
|
||||
const float chip = primary_code[i] == 0 ? 1.0F : -1.0F;
|
||||
dest[2 * i] = chip;
|
||||
dest[2 * i + 1] = -chip;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_code_gen_float_sampled(own::span<float> dest, const std::array<char, 3>& signal_id,
|
||||
bool qmboc, uint32_t prn, int32_t sampling_freq, uint32_t chip_shift, bool secondary_flag)
|
||||
{
|
||||
constexpr auto code_freq_basis = static_cast<int32_t>(BEIDOU_B1C_CODE_RATE_CPS);
|
||||
const int32_t samples_per_chip = qmboc ? 12 : 2;
|
||||
const uint32_t code_length = samples_per_chip * static_cast<uint32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS);
|
||||
const std::string b1c_signal = signal_id.data();
|
||||
const bool is_pilot = b1c_signal.rfind("1P") != std::string::npos;
|
||||
auto samples_per_code = static_cast<uint32_t>(static_cast<double>(sampling_freq) /
|
||||
(static_cast<double>(code_freq_basis) / BEIDOU_B1C_CODE_LENGTH_CHIPS));
|
||||
const uint32_t delay = ((static_cast<int32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS) - static_cast<int32_t>(chip_shift)) %
|
||||
static_cast<int32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS)) *
|
||||
samples_per_code / static_cast<uint32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS);
|
||||
|
||||
std::array<int32_t, 10230> primary_code{};
|
||||
beidou_b1c_code_gen_int(primary_code, signal_id, static_cast<int32_t>(prn));
|
||||
|
||||
std::array<int32_t, 10230> primary_bipolar{};
|
||||
for (uint32_t i = 0; i < static_cast<uint32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS); i++)
|
||||
{
|
||||
primary_bipolar[i] = primary_code[i] == 0 ? 1 : -1;
|
||||
}
|
||||
|
||||
std::vector<float> signal_b1c(code_length);
|
||||
if (qmboc && is_pilot)
|
||||
{
|
||||
beidou_b1c_qmboc_gen_float(signal_b1c, primary_bipolar, true);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<int32_t> signal_b1c_int(code_length);
|
||||
beidou_b1c_sinboc_11_gen_int(signal_b1c_int, primary_bipolar);
|
||||
for (uint32_t ii = 0; ii < code_length; ++ii)
|
||||
{
|
||||
signal_b1c[ii] = static_cast<float>(signal_b1c_int[ii]);
|
||||
}
|
||||
}
|
||||
|
||||
if (sampling_freq != samples_per_chip * code_freq_basis)
|
||||
{
|
||||
std::vector<float> resampled_signal(samples_per_code);
|
||||
resampler(signal_b1c, resampled_signal, static_cast<float>(samples_per_chip * code_freq_basis), sampling_freq);
|
||||
signal_b1c = std::move(resampled_signal);
|
||||
}
|
||||
else
|
||||
{
|
||||
samples_per_code = code_length;
|
||||
}
|
||||
|
||||
if (is_pilot && secondary_flag)
|
||||
{
|
||||
const int32_t prn_index = static_cast<int32_t>(prn) - 1;
|
||||
const char* sec_code = BEIDOU_B1C_PILOT_SECONDARY_CODE[prn_index];
|
||||
|
||||
std::vector<float> signal_with_secondary(BEIDOU_B1C_PILOT_SECONDARY_CODE_LENGTH * samples_per_code);
|
||||
for (int32_t i = 0; i < BEIDOU_B1C_PILOT_SECONDARY_CODE_LENGTH; i++)
|
||||
{
|
||||
const float sec_chip = sec_code[i] == '0' ? 1.0F : -1.0F;
|
||||
for (uint32_t k = 0; k < samples_per_code; k++)
|
||||
{
|
||||
signal_with_secondary[static_cast<size_t>(i) * samples_per_code + k] = signal_b1c[k] * sec_chip;
|
||||
}
|
||||
}
|
||||
samples_per_code *= static_cast<uint32_t>(BEIDOU_B1C_PILOT_SECONDARY_CODE_LENGTH);
|
||||
signal_b1c = std::move(signal_with_secondary);
|
||||
}
|
||||
|
||||
for (uint32_t i = 0; i < samples_per_code; i++)
|
||||
{
|
||||
dest[(i + delay) % samples_per_code] = signal_b1c[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_code_gen_complex_sampled(own::span<std::complex<float>> dest, const std::array<char, 3>& signal_id,
|
||||
bool qmboc, uint32_t prn, int32_t sampling_freq, uint32_t chip_shift, bool secondary_flag)
|
||||
{
|
||||
const std::string b1c_signal = signal_id.data();
|
||||
const bool is_pilot = b1c_signal.rfind("1P") != std::string::npos;
|
||||
auto samples_per_code = static_cast<uint32_t>(static_cast<double>(sampling_freq) /
|
||||
(BEIDOU_B1C_CODE_RATE_CPS / BEIDOU_B1C_CODE_LENGTH_CHIPS));
|
||||
if (is_pilot && secondary_flag)
|
||||
{
|
||||
samples_per_code *= static_cast<uint32_t>(BEIDOU_B1C_PILOT_SECONDARY_CODE_LENGTH);
|
||||
}
|
||||
|
||||
std::vector<float> real_code(samples_per_code);
|
||||
beidou_b1c_code_gen_float_sampled(real_code, signal_id, qmboc, prn, sampling_freq, chip_shift, secondary_flag);
|
||||
for (uint32_t ii = 0; ii < samples_per_code; ++ii)
|
||||
{
|
||||
dest[ii] = std::complex<float>(real_code[ii], 0.0F);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_code_gen_complex_sampled(own::span<std::complex<float>> dest, const std::array<char, 3>& signal_id,
|
||||
bool qmboc, uint32_t prn, int32_t sampling_freq, uint32_t chip_shift)
|
||||
{
|
||||
beidou_b1c_code_gen_complex_sampled(dest, signal_id, qmboc, prn, sampling_freq, chip_shift, false);
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
/*!
|
||||
* \file beidou_b1c_signal_replica.h
|
||||
* \brief Library for BeiDou B1C signal replica generation
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_B1C_SIGNAL_REPLICA_H
|
||||
#define GNSS_SDR_BEIDOU_B1C_SIGNAL_REPLICA_H
|
||||
|
||||
#include <array>
|
||||
#include <complex>
|
||||
#include <cstdint>
|
||||
#if HAS_STD_SPAN
|
||||
#include <span>
|
||||
namespace own = std;
|
||||
#else
|
||||
#include <gsl-lite/gsl-lite.hpp>
|
||||
namespace own = gsl_lite;
|
||||
#endif
|
||||
|
||||
/** \addtogroup Algorithms_Library
|
||||
* \{ */
|
||||
/** \addtogroup Algorithm_libs algorithms_libs
|
||||
* \{ */
|
||||
|
||||
void beidou_b1c_code_gen_int(own::span<int32_t> dest, const std::array<char, 3>& signal_id, int32_t prn);
|
||||
void beidou_b1c_code_gen_sinboc11_float(own::span<float> dest, const std::array<char, 3>& signal_id, uint32_t prn);
|
||||
void beidou_b1c_code_gen_float_sampled(own::span<float> dest, const std::array<char, 3>& signal_id,
|
||||
bool qmboc, uint32_t prn, int32_t sampling_freq, uint32_t chip_shift, bool secondary_flag);
|
||||
void beidou_b1c_code_gen_complex_sampled(own::span<std::complex<float>> dest, const std::array<char, 3>& signal_id,
|
||||
bool qmboc, uint32_t prn, int32_t sampling_freq, uint32_t chip_shift, bool secondary_flag);
|
||||
void beidou_b1c_code_gen_complex_sampled(own::span<std::complex<float>> dest, const std::array<char, 3>& signal_id,
|
||||
bool qmboc, uint32_t prn, int32_t sampling_freq, uint32_t chip_shift);
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_BEIDOU_B1C_SIGNAL_REPLICA_H
|
||||
@@ -16,6 +16,7 @@
|
||||
|
||||
#include "rtklib_conversions.h"
|
||||
#include "MATH_CONSTANTS.h" // for GNSS_PI, TWO_PI
|
||||
#include "beidou_cnav1_ephemeris.h" // for Beidou_Cnav1_Ephemeris
|
||||
#include "beidou_dnav_ephemeris.h" // for Beidou_Dnav_Ephemeris
|
||||
#include "galileo_almanac.h" // for Galileo_Almanac
|
||||
#include "galileo_ephemeris.h" // for Galileo_Ephemeris
|
||||
@@ -227,6 +228,10 @@ obsd_t insert_obs_to_rtklib(obsd_t& rtklib_obs,
|
||||
{
|
||||
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L6I);
|
||||
}
|
||||
else if (sig_ == "1D")
|
||||
{
|
||||
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L1D);
|
||||
}
|
||||
|
||||
break;
|
||||
case 'J':
|
||||
@@ -709,6 +714,70 @@ eph_t eph_to_rtklib(const Beidou_Dnav_Ephemeris& bei_eph)
|
||||
}
|
||||
|
||||
|
||||
eph_t eph_to_rtklib(const Beidou_Cnav1_Ephemeris& bei_eph)
|
||||
{
|
||||
eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, {}, {}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, false, 0, 0, 0, 0, 0.0, -1, 0};
|
||||
rtklib_sat.sat = bei_eph.PRN + NSATGPS + NSATGLO + NSATGAL + NSATQZS;
|
||||
rtklib_sat.A = bei_eph.sqrtA * bei_eph.sqrtA;
|
||||
rtklib_sat.M0 = bei_eph.M_0;
|
||||
rtklib_sat.deln = bei_eph.delta_n;
|
||||
rtklib_sat.OMG0 = bei_eph.OMEGA_0;
|
||||
rtklib_sat.OMGd = bei_eph.OMEGAdot;
|
||||
rtklib_sat.omg = bei_eph.omega;
|
||||
rtklib_sat.i0 = bei_eph.i_0;
|
||||
rtklib_sat.idot = bei_eph.idot;
|
||||
rtklib_sat.e = bei_eph.ecc;
|
||||
rtklib_sat.svh = 0;
|
||||
rtklib_sat.sva = 0;
|
||||
rtklib_sat.code = bei_eph.sig_type;
|
||||
rtklib_sat.flag = bei_eph.nav_type;
|
||||
rtklib_sat.iode = static_cast<int32_t>(bei_eph.IODE);
|
||||
rtklib_sat.iodc = static_cast<int32_t>(bei_eph.IODC);
|
||||
rtklib_sat.week = bei_eph.WN;
|
||||
rtklib_sat.cic = bei_eph.Cic;
|
||||
rtklib_sat.cis = bei_eph.Cis;
|
||||
rtklib_sat.cuc = bei_eph.Cuc;
|
||||
rtklib_sat.cus = bei_eph.Cus;
|
||||
rtklib_sat.crc = bei_eph.Crc;
|
||||
rtklib_sat.crs = bei_eph.Crs;
|
||||
rtklib_sat.f0 = bei_eph.af0;
|
||||
rtklib_sat.f1 = bei_eph.af1;
|
||||
rtklib_sat.f2 = bei_eph.af2;
|
||||
rtklib_sat.tgd[0] = bei_eph.TGD_B1Cp;
|
||||
rtklib_sat.tgd[1] = bei_eph.TGD_B2ap;
|
||||
rtklib_sat.tgd[2] = bei_eph.ISC_B1Cd;
|
||||
rtklib_sat.toes = bei_eph.toe;
|
||||
rtklib_sat.toe = bdt2gpst(bdt2time(rtklib_sat.week, bei_eph.toe));
|
||||
rtklib_sat.toc = bdt2gpst(bdt2time(rtklib_sat.week, bei_eph.toc));
|
||||
rtklib_sat.ttr = bdt2gpst(bdt2time(rtklib_sat.week, bei_eph.tow));
|
||||
|
||||
double tow = time2gpst(rtklib_sat.ttr, &rtklib_sat.week);
|
||||
const double toc = time2gpst(rtklib_sat.toc, nullptr);
|
||||
const double toe = time2gpst(rtklib_sat.toe, nullptr);
|
||||
if (rtklib_sat.toes < tow - 302400.0)
|
||||
{
|
||||
rtklib_sat.week++;
|
||||
tow -= 604800.0;
|
||||
}
|
||||
else if (rtklib_sat.toes > tow + 302400.0)
|
||||
{
|
||||
rtklib_sat.week--;
|
||||
tow += 604800.0;
|
||||
}
|
||||
rtklib_sat.toe = gpst2time(rtklib_sat.week, toe);
|
||||
rtklib_sat.toc = gpst2time(rtklib_sat.week, toc);
|
||||
rtklib_sat.ttr = gpst2time(rtklib_sat.week, tow);
|
||||
|
||||
rtklib_sat.has_orbit_radial_correction_m = 0.0;
|
||||
rtklib_sat.has_orbit_in_track_correction_m = 0.0;
|
||||
rtklib_sat.has_orbit_cross_track_correction_m = 0.0;
|
||||
rtklib_sat.has_clock_correction_m = 0.0;
|
||||
rtklib_sat.apply_has_corrections = false;
|
||||
return rtklib_sat;
|
||||
}
|
||||
|
||||
|
||||
eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph)
|
||||
{
|
||||
eph_t rtklib_sat = {0, 0, 0, 0, 0, 0, 0, 0, {0, 0}, {0, 0}, {0, 0}, 0.0, 0.0, 0.0, 0.0, 0.0,
|
||||
|
||||
@@ -28,6 +28,7 @@
|
||||
* \{ */
|
||||
|
||||
|
||||
class Beidou_Cnav1_Ephemeris;
|
||||
class Beidou_Dnav_Ephemeris;
|
||||
class Galileo_Almanac;
|
||||
class Galileo_Ephemeris;
|
||||
@@ -102,6 +103,8 @@ eph_t eph_to_rtklib(const Gps_CNAV_Ephemeris& gps_cnav_eph);
|
||||
|
||||
eph_t eph_to_rtklib(const Beidou_Dnav_Ephemeris& bei_eph);
|
||||
|
||||
eph_t eph_to_rtklib(const Beidou_Cnav1_Ephemeris& bei_eph);
|
||||
|
||||
alm_t alm_to_rtklib(const Gps_Almanac& gps_alm);
|
||||
alm_t alm_to_rtklib(const Galileo_Almanac& gal_alm);
|
||||
|
||||
|
||||
@@ -108,7 +108,7 @@ char obscodes[][3] = {
|
||||
"2W", "2Y", "2M", "2N", "5I", "5Q", "5X", "7I", "7Q", "7X", /* 20-29 */
|
||||
"6A", "6B", "6C", "6X", "6Z", "6S", "6L", "8L", "8Q", "8X", /* 30-39 */
|
||||
"2I", "2Q", "6I", "6Q", "3I", "3Q", "3X", "1I", "1Q", "5A", /* 40-49 */
|
||||
"5B", "5C", "9A", "9B", "9C", "9X", "", "", "", "" /* 50-59 */
|
||||
"5B", "5C", "9A", "9B", "9C", "9X", "1D", "", "", "" /* 50-59 */
|
||||
};
|
||||
|
||||
|
||||
@@ -119,7 +119,7 @@ unsigned char obsfreqs[] = {
|
||||
2, 2, 2, 2, 3, 3, 3, 5, 5, 5, /* 20-29 */
|
||||
4, 4, 4, 4, 4, 4, 4, 6, 6, 6, /* 30-39 */
|
||||
2, 2, 4, 4, 3, 3, 3, 1, 1, 3, /* 40-49 */
|
||||
3, 3, 7, 7, 7, 7, 0, 0, 0, 0 /* 50-59 */
|
||||
3, 3, 7, 7, 7, 7, 1, 0, 0, 0 /* 50-59 */
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -7,10 +7,14 @@
|
||||
|
||||
set(TELEMETRY_DECODER_ADAPTER_SOURCES
|
||||
telemetry_decoder_adapter.cc
|
||||
telemetry_decoder_adapter_base.cc
|
||||
beidou_b1c_telemetry_decoder.cc
|
||||
)
|
||||
|
||||
set(TELEMETRY_DECODER_ADAPTER_HEADERS
|
||||
telemetry_decoder_adapter.h
|
||||
telemetry_decoder_adapter_base.h
|
||||
beidou_b1c_telemetry_decoder.h
|
||||
)
|
||||
|
||||
list(SORT TELEMETRY_DECODER_ADAPTER_HEADERS)
|
||||
|
||||
@@ -0,0 +1,17 @@
|
||||
/*!
|
||||
* \file beidou_b1c_telemetry_decoder.cc
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#include "beidou_b1c_telemetry_decoder.h"
|
||||
#include "beidou_b1c_telemetry_decoder_gs.h"
|
||||
|
||||
BeidouB1cTelemetryDecoder::BeidouB1cTelemetryDecoder(
|
||||
const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams)
|
||||
: TelemetryDecoderAdapterBase(configuration, role, in_streams, out_streams)
|
||||
{
|
||||
InitializeDecoder(beidou_b1c_make_telemetry_decoder_gs(satellite(), tlm_parameters_));
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
/*!
|
||||
* \file beidou_b1c_telemetry_decoder.h
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_B1C_TELEMETRY_DECODER_H
|
||||
#define GNSS_SDR_BEIDOU_B1C_TELEMETRY_DECODER_H
|
||||
|
||||
#include "telemetry_decoder_adapter_base.h"
|
||||
|
||||
class BeidouB1cTelemetryDecoder : public TelemetryDecoderAdapterBase
|
||||
{
|
||||
public:
|
||||
BeidouB1cTelemetryDecoder(const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams);
|
||||
|
||||
inline std::string implementation() override
|
||||
{
|
||||
return "BEIDOU_B1C_Telemetry_Decoder";
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,135 @@
|
||||
/*!
|
||||
* \file telemetry_decoder_adapter_base.cc
|
||||
* \brief Common functionality for telemetry decoder adapters
|
||||
* \author Carles Fernandez-Prades, 2025 cfernandez@cttc.es
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2025 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include "telemetry_decoder_adapter_base.h"
|
||||
#include <utility>
|
||||
|
||||
#if USE_GLOG_AND_GFLAGS
|
||||
#include <glog/logging.h>
|
||||
#else
|
||||
#include <absl/log/log.h>
|
||||
#endif
|
||||
|
||||
|
||||
TelemetryDecoderAdapterBase::TelemetryDecoderAdapterBase(const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams) : role_(role),
|
||||
in_streams_(in_streams),
|
||||
out_streams_(out_streams)
|
||||
{
|
||||
DLOG(INFO) << "role " << role_;
|
||||
if (configuration != nullptr)
|
||||
{
|
||||
tlm_parameters_.SetFromConfiguration(configuration, role_);
|
||||
}
|
||||
if (in_streams_ > 1)
|
||||
{
|
||||
LOG(ERROR) << "This implementation only supports one input stream ("
|
||||
<< in_streams_ << " provided)";
|
||||
}
|
||||
if (out_streams_ > 1)
|
||||
{
|
||||
LOG(ERROR) << "This implementation only supports one output stream ("
|
||||
<< out_streams_ << " provided)";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void TelemetryDecoderAdapterBase::InitializeDecoder(telemetry_impl_interface_sptr decoder)
|
||||
{
|
||||
telemetry_decoder_ = std::move(decoder);
|
||||
DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")";
|
||||
}
|
||||
|
||||
|
||||
void TelemetryDecoderAdapterBase::connect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if (top_block)
|
||||
{
|
||||
/* top_block is not null */
|
||||
}
|
||||
DLOG(INFO) << "nothing to connect internally";
|
||||
}
|
||||
|
||||
|
||||
void TelemetryDecoderAdapterBase::disconnect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if (top_block)
|
||||
{
|
||||
/* top_block is not null */
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr TelemetryDecoderAdapterBase::get_left_block()
|
||||
{
|
||||
return telemetry_decoder_;
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr TelemetryDecoderAdapterBase::get_right_block()
|
||||
{
|
||||
return telemetry_decoder_;
|
||||
}
|
||||
|
||||
|
||||
void TelemetryDecoderAdapterBase::set_satellite(const Gnss_Satellite& satellite)
|
||||
{
|
||||
satellite_ = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
if (telemetry_decoder_)
|
||||
{
|
||||
telemetry_decoder_->set_satellite(satellite_);
|
||||
DLOG(INFO) << "TELEMETRY DECODER: satellite set to " << satellite_;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
std::string TelemetryDecoderAdapterBase::role()
|
||||
{
|
||||
return role_;
|
||||
}
|
||||
|
||||
|
||||
void TelemetryDecoderAdapterBase::set_channel(int channel)
|
||||
{
|
||||
if (telemetry_decoder_)
|
||||
{
|
||||
telemetry_decoder_->set_channel(channel);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void TelemetryDecoderAdapterBase::reset()
|
||||
{
|
||||
if (telemetry_decoder_)
|
||||
{
|
||||
telemetry_decoder_->reset();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
size_t TelemetryDecoderAdapterBase::item_size()
|
||||
{
|
||||
return sizeof(Gnss_Synchro);
|
||||
}
|
||||
|
||||
|
||||
const Gnss_Satellite& TelemetryDecoderAdapterBase::satellite() const
|
||||
{
|
||||
return satellite_;
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
/*!
|
||||
* \file telemetry_decoder_adapter_base.h
|
||||
* \brief Common functionality for telemetry decoder adapters
|
||||
* \author Carles Fernandez-Prades, 2025 cfernandez@cttc.es
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2025 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_TELEMETRY_DECODER_ADAPTER_BASE_CLASS_H
|
||||
#define GNSS_SDR_TELEMETRY_DECODER_ADAPTER_BASE_CLASS_H
|
||||
|
||||
#include "configuration_interface.h"
|
||||
#include "gnss_satellite.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "telemetry_decoder_interface.h"
|
||||
#include "telemetry_impl_interface.h"
|
||||
#include "tlm_conf.h"
|
||||
#include <gnuradio/runtime_types.h>
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
|
||||
class ConfigurationInterface;
|
||||
|
||||
/** \addtogroup Telemetry_Decoder
|
||||
* \{
|
||||
*/
|
||||
/** \addtogroup Telemetry_Decoder_adapters
|
||||
* \{
|
||||
*/
|
||||
|
||||
/*!
|
||||
* \brief Base class for Telemetry Decoder adapters
|
||||
*/
|
||||
class TelemetryDecoderAdapterBase : public TelemetryDecoderInterface
|
||||
{
|
||||
public:
|
||||
TelemetryDecoderAdapterBase(const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams);
|
||||
|
||||
~TelemetryDecoderAdapterBase() override = default;
|
||||
|
||||
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;
|
||||
|
||||
void set_satellite(const Gnss_Satellite& satellite) override;
|
||||
|
||||
std::string role() override;
|
||||
|
||||
void set_channel(int channel) override;
|
||||
|
||||
void reset() override;
|
||||
|
||||
size_t item_size() override;
|
||||
|
||||
protected:
|
||||
void InitializeDecoder(telemetry_impl_interface_sptr decoder);
|
||||
|
||||
const Gnss_Satellite& satellite() const;
|
||||
|
||||
Tlm_Conf tlm_parameters_;
|
||||
|
||||
private:
|
||||
telemetry_impl_interface_sptr telemetry_decoder_;
|
||||
Gnss_Satellite satellite_;
|
||||
std::string role_;
|
||||
unsigned int in_streams_ = 0;
|
||||
unsigned int out_streams_ = 0;
|
||||
};
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
|
||||
#endif // GNSS_SDR_TELEMETRY_DECODER_ADAPTER_BASE_CLASS_H
|
||||
@@ -14,6 +14,7 @@ set(TELEMETRY_DECODER_GR_BLOCKS_SOURCES
|
||||
glonass_gnav_telemetry_decoder_gs.cc
|
||||
galileo_telemetry_decoder_gs.cc
|
||||
beidou_dnav_telemetry_decoder_gs.cc
|
||||
beidou_b1c_telemetry_decoder_gs.cc
|
||||
)
|
||||
|
||||
set(TELEMETRY_DECODER_GR_BLOCKS_HEADERS
|
||||
@@ -25,6 +26,7 @@ set(TELEMETRY_DECODER_GR_BLOCKS_HEADERS
|
||||
glonass_gnav_telemetry_decoder_gs.h
|
||||
galileo_telemetry_decoder_gs.h
|
||||
beidou_dnav_telemetry_decoder_gs.h
|
||||
beidou_b1c_telemetry_decoder_gs.h
|
||||
)
|
||||
|
||||
list(SORT TELEMETRY_DECODER_GR_BLOCKS_HEADERS)
|
||||
|
||||
@@ -0,0 +1,719 @@
|
||||
/*!
|
||||
* \file beidou_b1c_telemetry_decoder_gs.cc
|
||||
* \brief BeiDou B1C B-CNAV1 telemetry decoder block
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#include "beidou_b1c_telemetry_decoder_gs.h"
|
||||
#include "Beidou_B1C.h"
|
||||
#include "Beidou_B1C_codes.h"
|
||||
#include "Beidou_CNAV1.h"
|
||||
#include "Beidou_DNAV.h"
|
||||
#include "MATH_CONSTANTS.h"
|
||||
#include "beidou_cnav1_ephemeris.h"
|
||||
#include "beidou_cnav1_iono.h"
|
||||
#include "beidou_cnav1_utc_model.h"
|
||||
#include "display.h"
|
||||
#include "gnss_sdr_make_unique.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "tlm_crc_stats.h"
|
||||
#include "tlm_utils.h"
|
||||
#include "tow_to_trk.h"
|
||||
#include <pmt/pmt.h>
|
||||
#include <pmt/pmt_sugar.h>
|
||||
#include <algorithm>
|
||||
#include <complex>
|
||||
#include <iomanip>
|
||||
#include <iostream>
|
||||
#include <numeric>
|
||||
#include <vector>
|
||||
|
||||
#if USE_GLOG_AND_GFLAGS
|
||||
#include <glog/logging.h>
|
||||
#else
|
||||
#include <absl/log/log.h>
|
||||
#endif
|
||||
|
||||
#define CRC_ERROR_LIMIT 8
|
||||
|
||||
namespace
|
||||
{
|
||||
// SOH/HOW in CNAV1 refers to the start of the 1800-symbol frame in the decode buffer.
|
||||
// Current symbol is the newest entry (index BEIDOU_CNAV1_FRAME_SYMBOLS - 1), matching
|
||||
// BeiDou B1I (preamble + required_symbols) and Galileo INAV (preamble + page offset) TOW tagging.
|
||||
int32_t resolve_b1c_frame_soh_offset(int32_t matched_offset, int32_t prev_candidate_offset)
|
||||
{
|
||||
if (matched_offset >= 0)
|
||||
{
|
||||
return matched_offset;
|
||||
}
|
||||
if (matched_offset == -2 && prev_candidate_offset >= 0)
|
||||
{
|
||||
return prev_candidate_offset;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
uint32_t compute_b1c_tow_ms_at_current_symbol(
|
||||
double tow_soh_bdt_s,
|
||||
int32_t frame_soh_offset,
|
||||
int32_t symbol_duration_ms)
|
||||
{
|
||||
const int32_t clamped_offset = std::max(0, std::min(frame_soh_offset, BEIDOU_CNAV1_FRAME_SYMBOLS - 1));
|
||||
const int32_t symbols_since_soh = (BEIDOU_CNAV1_FRAME_SYMBOLS - 1) - clamped_offset;
|
||||
const double tow_gpst_s = tow_soh_bdt_s + static_cast<double>(BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) +
|
||||
static_cast<double>(symbols_since_soh) * static_cast<double>(symbol_duration_ms) / 1000.0;
|
||||
const double tow_ms = std::fmod(tow_gpst_s * 1000.0, 604800000.0);
|
||||
if (tow_ms < 0.0)
|
||||
{
|
||||
return static_cast<uint32_t>(tow_ms + 604800000.0);
|
||||
}
|
||||
return static_cast<uint32_t>(tow_ms);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
beidou_b1c_telemetry_decoder_gs_sptr beidou_b1c_make_telemetry_decoder_gs(
|
||||
const Gnss_Satellite& satellite,
|
||||
const Tlm_Conf& conf)
|
||||
{
|
||||
return beidou_b1c_telemetry_decoder_gs_sptr(new beidou_b1c_telemetry_decoder_gs(satellite, conf));
|
||||
}
|
||||
|
||||
|
||||
beidou_b1c_telemetry_decoder_gs::beidou_b1c_telemetry_decoder_gs(
|
||||
const Gnss_Satellite& satellite,
|
||||
const Tlm_Conf& conf)
|
||||
: telemetry_impl_interface("beidou_b1c_telemetry_decoder_gs",
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))),
|
||||
d_dump_filename(conf.dump_filename),
|
||||
|
||||
d_symbol_duration_ms(BEIDOU_B1C_CODE_PERIOD_MS),
|
||||
d_dump(conf.dump),
|
||||
d_dump_mat(conf.dump_mat),
|
||||
d_remove_dat(conf.remove_dat),
|
||||
d_enable_navdata_monitor(conf.enable_navdata_monitor),
|
||||
d_dump_crc_stats(conf.dump_crc_stats),
|
||||
d_tow_to_trk(conf.tow_to_trk)
|
||||
{
|
||||
configure_basic_outputs();
|
||||
d_symbol_history.set_capacity(BEIDOU_CNAV1_FRAME_SYMBOLS);
|
||||
d_symbol_history_q.set_capacity(BEIDOU_CNAV1_FRAME_SYMBOLS);
|
||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
DLOG(INFO) << "Initializing BeiDou B1C B-CNAV1 telemetry decoder for satellite " << d_satellite;
|
||||
if (d_enable_navdata_monitor)
|
||||
{
|
||||
message_port_register_out(pmt::mp("Nav_msg_from_TLM"));
|
||||
d_nav_msg_packet.system = std::string("C");
|
||||
d_nav_msg_packet.signal = std::string("1D");
|
||||
}
|
||||
configure_dump_file(d_channel, d_dump, d_dump_filename, d_dump_file);
|
||||
configure_crc_stats_channel(d_channel, d_dump_crc_stats, d_Tlm_CRC_Stats);
|
||||
}
|
||||
|
||||
|
||||
beidou_b1c_telemetry_decoder_gs::~beidou_b1c_telemetry_decoder_gs()
|
||||
{
|
||||
if (d_dump_file.is_open())
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_telemetry_decoder_gs::set_satellite(const Gnss_Satellite& satellite)
|
||||
{
|
||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
DLOG(INFO) << "BeiDou B1C telemetry decoder set to satellite " << d_satellite;
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_telemetry_decoder_gs::set_channel(int channel)
|
||||
{
|
||||
d_channel = channel;
|
||||
DLOG(INFO) << "BeiDou B1C B-CNAV1 telemetry channel set to " << channel;
|
||||
configure_dump_file(d_channel, d_dump, d_dump_filename, d_dump_file);
|
||||
configure_crc_stats_channel(d_channel, d_dump_crc_stats, d_Tlm_CRC_Stats);
|
||||
}
|
||||
|
||||
|
||||
void beidou_b1c_telemetry_decoder_gs::reset()
|
||||
{
|
||||
d_sample_counter = 0;
|
||||
d_frame_sync_index = 0;
|
||||
d_TOW_at_current_symbol_ms = 0;
|
||||
d_flag_valid_word = false;
|
||||
d_flag_frame_sync = false;
|
||||
d_flag_PLL_180_deg_phase_locked = false;
|
||||
d_stat = 0;
|
||||
d_CRC_error_counter = 0;
|
||||
d_symbol_history.clear();
|
||||
d_symbol_history_q.clear();
|
||||
d_prev_candidate_frame.clear();
|
||||
d_prev_candidate_offset = -1;
|
||||
d_have_prev_candidate_frame = false;
|
||||
d_prev_valid_symbol_output = false;
|
||||
d_await_post_lock_frame_decode = false;
|
||||
d_post_lock_valid_symbols = 0U;
|
||||
d_nav.clear_flags();
|
||||
DLOG(INFO) << "BeiDou B1C telemetry decoder reset for satellite " << d_satellite;
|
||||
}
|
||||
|
||||
|
||||
bool beidou_b1c_telemetry_decoder_gs::decode_frame_from_history(bool invert)
|
||||
{
|
||||
std::vector<float> frame(d_symbol_history.begin(), d_symbol_history.end());
|
||||
if (invert)
|
||||
{
|
||||
for (auto& sample : frame)
|
||||
{
|
||||
sample = -sample;
|
||||
}
|
||||
}
|
||||
return d_nav.decode_frame_symbols(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS, d_satellite.get_PRN());
|
||||
}
|
||||
|
||||
|
||||
bool beidou_b1c_telemetry_decoder_gs::decode_frame_from_history_with_offset(int32_t start_offset, bool invert)
|
||||
{
|
||||
std::vector<float> history(d_symbol_history.begin(), d_symbol_history.end());
|
||||
if (history.size() < static_cast<size_t>(BEIDOU_CNAV1_FRAME_SYMBOLS))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
std::vector<float> frame(BEIDOU_CNAV1_FRAME_SYMBOLS);
|
||||
const int32_t n = BEIDOU_CNAV1_FRAME_SYMBOLS;
|
||||
for (int32_t i = 0; i < n; i++)
|
||||
{
|
||||
float sample = history[static_cast<size_t>((start_offset + i) % n)];
|
||||
if (invert)
|
||||
{
|
||||
sample = -sample;
|
||||
}
|
||||
frame[static_cast<size_t>(i)] = sample;
|
||||
}
|
||||
return d_nav.decode_frame_symbols(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS, d_satellite.get_PRN());
|
||||
}
|
||||
|
||||
|
||||
bool beidou_b1c_telemetry_decoder_gs::probe_frame_from_history_with_offset(int32_t start_offset, bool invert) const
|
||||
{
|
||||
std::vector<float> history(d_symbol_history.begin(), d_symbol_history.end());
|
||||
if (history.size() < static_cast<size_t>(BEIDOU_CNAV1_FRAME_SYMBOLS))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
std::vector<float> frame(BEIDOU_CNAV1_FRAME_SYMBOLS);
|
||||
const int32_t n = BEIDOU_CNAV1_FRAME_SYMBOLS;
|
||||
for (int32_t i = 0; i < n; i++)
|
||||
{
|
||||
float sample = history[static_cast<size_t>((start_offset + i) % n)];
|
||||
if (invert)
|
||||
{
|
||||
sample = -sample;
|
||||
}
|
||||
frame[static_cast<size_t>(i)] = sample;
|
||||
}
|
||||
return d_nav.probe_subframe1_prn(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS, d_satellite.get_PRN());
|
||||
}
|
||||
|
||||
|
||||
namespace
|
||||
{
|
||||
bool decode_frame_from_buffer_with_offset(
|
||||
const boost::circular_buffer<float>& symbol_history,
|
||||
Beidou_Cnav1_Navigation_Message& nav,
|
||||
int32_t expected_prn,
|
||||
int32_t start_offset,
|
||||
bool invert,
|
||||
float cn0_db_hz,
|
||||
int32_t* fail_stage = nullptr)
|
||||
{
|
||||
std::vector<float> history(symbol_history.begin(), symbol_history.end());
|
||||
if (history.size() < static_cast<size_t>(BEIDOU_CNAV1_FRAME_SYMBOLS))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
std::vector<float> frame(BEIDOU_CNAV1_FRAME_SYMBOLS);
|
||||
const int32_t n = BEIDOU_CNAV1_FRAME_SYMBOLS;
|
||||
for (int32_t i = 0; i < n; i++)
|
||||
{
|
||||
float sample = history[static_cast<size_t>((start_offset + i) % n)];
|
||||
if (invert)
|
||||
{
|
||||
sample = -sample;
|
||||
}
|
||||
frame[static_cast<size_t>(i)] = sample;
|
||||
}
|
||||
|
||||
// LLR calibration: normalize by frame RMS and scale by CN0-derived symbol SNR.
|
||||
double rms_acc = 0.0;
|
||||
for (const float v : frame)
|
||||
{
|
||||
rms_acc += static_cast<double>(v) * static_cast<double>(v);
|
||||
}
|
||||
const double rms = std::sqrt(rms_acc / static_cast<double>(n)) + 1e-6;
|
||||
const double cn0_linear = std::pow(10.0, static_cast<double>(cn0_db_hz) / 10.0);
|
||||
const double snr_sym = std::max(cn0_linear * (BEIDOU_B1C_CODE_PERIOD_MS * 1e-3), 1e-4);
|
||||
const double llr_scale = std::min(std::max(std::sqrt(snr_sym) / rms, 0.05), 50.0);
|
||||
for (auto& v : frame)
|
||||
{
|
||||
v = static_cast<float>(static_cast<double>(v) * llr_scale);
|
||||
}
|
||||
return nav.decode_frame_symbols(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS, expected_prn, fail_stage);
|
||||
}
|
||||
|
||||
bool build_frame_from_buffer_with_offset(
|
||||
const boost::circular_buffer<float>& symbol_history,
|
||||
int32_t start_offset,
|
||||
bool invert,
|
||||
std::vector<float>& frame_out)
|
||||
{
|
||||
std::vector<float> history(symbol_history.begin(), symbol_history.end());
|
||||
if (history.size() < static_cast<size_t>(BEIDOU_CNAV1_FRAME_SYMBOLS))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
const int32_t n = BEIDOU_CNAV1_FRAME_SYMBOLS;
|
||||
frame_out.resize(static_cast<size_t>(n));
|
||||
for (int32_t i = 0; i < n; i++)
|
||||
{
|
||||
float sample = history[static_cast<size_t>((start_offset + i) % n)];
|
||||
if (invert)
|
||||
{
|
||||
sample = -sample;
|
||||
}
|
||||
frame_out[static_cast<size_t>(i)] = sample;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool probe_frame_from_buffer_with_offset(
|
||||
const boost::circular_buffer<float>& symbol_history,
|
||||
const Beidou_Cnav1_Navigation_Message& nav,
|
||||
int32_t expected_prn,
|
||||
int32_t start_offset,
|
||||
bool invert)
|
||||
{
|
||||
std::vector<float> history(symbol_history.begin(), symbol_history.end());
|
||||
if (history.size() < static_cast<size_t>(BEIDOU_CNAV1_FRAME_SYMBOLS))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
std::vector<float> frame(BEIDOU_CNAV1_FRAME_SYMBOLS);
|
||||
const int32_t n = BEIDOU_CNAV1_FRAME_SYMBOLS;
|
||||
for (int32_t i = 0; i < n; i++)
|
||||
{
|
||||
float sample = history[static_cast<size_t>((start_offset + i) % n)];
|
||||
if (invert)
|
||||
{
|
||||
sample = -sample;
|
||||
}
|
||||
frame[static_cast<size_t>(i)] = sample;
|
||||
}
|
||||
return nav.probe_subframe1_prn(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS, expected_prn);
|
||||
}
|
||||
|
||||
std::pair<int32_t, std::vector<int32_t>> find_secondary_code_candidates(
|
||||
const boost::circular_buffer<float>& pilot_symbol_history,
|
||||
int32_t prn)
|
||||
{
|
||||
std::vector<float> history(pilot_symbol_history.begin(), pilot_symbol_history.end());
|
||||
if (history.size() < static_cast<size_t>(BEIDOU_CNAV1_FRAME_SYMBOLS) ||
|
||||
prn < 1 || prn > static_cast<int32_t>(BEIDOU_B1C_NUMBER_OF_PRNS))
|
||||
{
|
||||
return {0, {}};
|
||||
}
|
||||
|
||||
const char* sec_code = BEIDOU_B1C_PILOT_SECONDARY_CODE[static_cast<size_t>(prn - 1)];
|
||||
const int32_t n = BEIDOU_CNAV1_FRAME_SYMBOLS;
|
||||
int32_t best_corr = 0;
|
||||
int32_t best_lag = 0;
|
||||
std::vector<int32_t> candidates;
|
||||
for (int32_t lag = 0; lag < n; lag++)
|
||||
{
|
||||
int32_t corr = 0;
|
||||
for (int32_t i = 0; i < n; i++)
|
||||
{
|
||||
const float sym = history[static_cast<size_t>((lag + i) % n)];
|
||||
const int32_t sym_sign = (sym >= 0.0F) ? 1 : -1;
|
||||
const int32_t sec_sign = (sec_code[static_cast<size_t>(i)] == '0') ? 1 : -1;
|
||||
corr += sym_sign * sec_sign;
|
||||
}
|
||||
if (std::abs(corr) > std::abs(best_corr))
|
||||
{
|
||||
best_corr = corr;
|
||||
best_lag = lag;
|
||||
}
|
||||
if (std::abs(corr) >= 1799)
|
||||
{
|
||||
candidates.push_back(lag);
|
||||
}
|
||||
}
|
||||
|
||||
if (candidates.empty() && std::abs(best_corr) >= 1650)
|
||||
{
|
||||
candidates.push_back(best_lag);
|
||||
}
|
||||
return {best_corr, candidates};
|
||||
}
|
||||
|
||||
bool build_rotated_bpsk_frame_from_iq(
|
||||
const boost::circular_buffer<float>& symbol_history_i,
|
||||
const boost::circular_buffer<float>& symbol_history_q,
|
||||
int32_t start_offset,
|
||||
bool invert,
|
||||
std::vector<float>& frame_out)
|
||||
{
|
||||
std::vector<float> history_i(symbol_history_i.begin(), symbol_history_i.end());
|
||||
std::vector<float> history_q(symbol_history_q.begin(), symbol_history_q.end());
|
||||
if (history_i.size() < static_cast<size_t>(BEIDOU_CNAV1_FRAME_SYMBOLS) ||
|
||||
history_q.size() < static_cast<size_t>(BEIDOU_CNAV1_FRAME_SYMBOLS))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const int32_t n = BEIDOU_CNAV1_FRAME_SYMBOLS;
|
||||
std::vector<std::complex<float>> z(static_cast<size_t>(n));
|
||||
std::complex<float> sum_z2(0.0F, 0.0F);
|
||||
for (int32_t i = 0; i < n; i++)
|
||||
{
|
||||
const auto idx = static_cast<size_t>((start_offset + i) % n);
|
||||
const std::complex<float> zi(history_i[idx], history_q[idx]);
|
||||
z[static_cast<size_t>(i)] = zi;
|
||||
sum_z2 += zi * zi;
|
||||
}
|
||||
|
||||
const float theta = 0.5F * std::arg(sum_z2);
|
||||
const std::complex<float> rot(std::cos(theta), -std::sin(theta));
|
||||
frame_out.resize(static_cast<size_t>(n));
|
||||
for (int32_t i = 0; i < n; i++)
|
||||
{
|
||||
float sample = (z[static_cast<size_t>(i)] * rot).real();
|
||||
if (invert)
|
||||
{
|
||||
sample = -sample;
|
||||
}
|
||||
frame_out[static_cast<size_t>(i)] = sample;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool probe_frame_from_iq_buffers_with_offset(
|
||||
const boost::circular_buffer<float>& symbol_history_i,
|
||||
const boost::circular_buffer<float>& symbol_history_q,
|
||||
const Beidou_Cnav1_Navigation_Message& nav,
|
||||
int32_t expected_prn,
|
||||
int32_t start_offset,
|
||||
bool invert)
|
||||
{
|
||||
std::vector<float> frame;
|
||||
if (!build_rotated_bpsk_frame_from_iq(symbol_history_i, symbol_history_q, start_offset, invert, frame))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return nav.probe_subframe1_prn(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS, expected_prn);
|
||||
}
|
||||
|
||||
bool decode_frame_from_iq_buffers_with_offset(
|
||||
const boost::circular_buffer<float>& symbol_history_i,
|
||||
const boost::circular_buffer<float>& symbol_history_q,
|
||||
Beidou_Cnav1_Navigation_Message& nav,
|
||||
int32_t expected_prn,
|
||||
int32_t start_offset,
|
||||
bool invert,
|
||||
float cn0_db_hz,
|
||||
int32_t* fail_stage = nullptr)
|
||||
{
|
||||
std::vector<float> frame;
|
||||
if (!build_rotated_bpsk_frame_from_iq(symbol_history_i, symbol_history_q, start_offset, invert, frame))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
double rms_acc = 0.0;
|
||||
for (const float v : frame)
|
||||
{
|
||||
rms_acc += static_cast<double>(v) * static_cast<double>(v);
|
||||
}
|
||||
const double rms = std::sqrt(rms_acc / static_cast<double>(BEIDOU_CNAV1_FRAME_SYMBOLS)) + 1e-6;
|
||||
const double cn0_linear = std::pow(10.0, static_cast<double>(cn0_db_hz) / 10.0);
|
||||
const double snr_sym = std::max(cn0_linear * (BEIDOU_B1C_CODE_PERIOD_MS * 1e-3), 1e-4);
|
||||
const double llr_scale = std::min(std::max(std::sqrt(snr_sym) / rms, 0.05), 50.0);
|
||||
for (auto& v : frame)
|
||||
{
|
||||
v = static_cast<float>(static_cast<double>(v) * llr_scale);
|
||||
}
|
||||
return nav.decode_frame_symbols(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS, expected_prn, fail_stage);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
|
||||
void beidou_b1c_telemetry_decoder_gs::publish_navigation(double cn0_db_hz)
|
||||
{
|
||||
if (d_nav.have_new_ephemeris())
|
||||
{
|
||||
auto eph = std::make_shared<Beidou_Cnav1_Ephemeris>(d_nav.get_ephemeris());
|
||||
eph->PRN = d_satellite.get_PRN();
|
||||
message_port_pub(pmt::mp("telemetry"), pmt::make_any(eph));
|
||||
#if __cplusplus == 201103L
|
||||
const int default_precision = std::cout.precision();
|
||||
#else
|
||||
const auto default_precision{std::cout.precision()};
|
||||
#endif
|
||||
std::cout << TEXT_BLUE << "New BeiDou B-CNAV1 ephemeris in channel " << d_channel
|
||||
<< " from satellite " << d_satellite
|
||||
<< " with CN0=" << std::setprecision(2) << cn0_db_hz << std::setprecision(default_precision)
|
||||
<< " dB-Hz" << TEXT_RESET << std::endl;
|
||||
LOG(INFO) << "New BeiDou B-CNAV1 ephemeris from PRN " << d_satellite.get_PRN()
|
||||
<< " in channel " << d_channel << " with CN0=" << cn0_db_hz << " dB-Hz";
|
||||
}
|
||||
if (d_nav.have_new_iono())
|
||||
{
|
||||
auto iono = std::make_shared<Beidou_Cnav1_Iono>(d_nav.get_iono());
|
||||
iono->valid = true;
|
||||
message_port_pub(pmt::mp("telemetry"), pmt::make_any(iono));
|
||||
DLOG(INFO) << "New BeiDou B-CNAV1 iono model from PRN " << d_satellite.get_PRN()
|
||||
<< " in channel " << d_channel;
|
||||
}
|
||||
if (d_nav.have_new_utc_model())
|
||||
{
|
||||
auto utc = std::make_shared<Beidou_Cnav1_Utc_Model>(d_nav.get_utc_model());
|
||||
utc->valid = true;
|
||||
message_port_pub(pmt::mp("telemetry"), pmt::make_any(utc));
|
||||
DLOG(INFO) << "New BeiDou B-CNAV1 UTC model from PRN " << d_satellite.get_PRN()
|
||||
<< " in channel " << d_channel;
|
||||
}
|
||||
if (d_nav.have_new_page_data())
|
||||
{
|
||||
Beidou_Cnav1_PageData_Message page_msg{};
|
||||
page_msg.PRN = d_satellite.get_PRN();
|
||||
page_msg.page_data = d_nav.get_page_data();
|
||||
message_port_pub(pmt::mp("telemetry"), pmt::make_any(std::make_shared<Beidou_Cnav1_PageData_Message>(page_msg)));
|
||||
DLOG(INFO) << "New BeiDou B-CNAV1 page data from PRN " << d_satellite.get_PRN()
|
||||
<< " in channel " << d_channel
|
||||
<< " PageID=" << d_nav.get_page_data().common.page_id;
|
||||
}
|
||||
d_nav.clear_flags();
|
||||
d_prev_candidate_frame.clear();
|
||||
d_prev_candidate_offset = -1;
|
||||
d_have_prev_candidate_frame = false;
|
||||
}
|
||||
|
||||
|
||||
int beidou_b1c_telemetry_decoder_gs::general_work(
|
||||
int noutput_items,
|
||||
gr_vector_int& ninput_items,
|
||||
gr_vector_const_void_star& input_items,
|
||||
gr_vector_void_star& output_items)
|
||||
{
|
||||
const auto* in = reinterpret_cast<const Gnss_Synchro*>(input_items[0]);
|
||||
auto* out = reinterpret_cast<Gnss_Synchro*>(output_items[0]);
|
||||
if (ninput_items[0] <= 0 || noutput_items <= 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
Gnss_Synchro current_symbol = in[0];
|
||||
current_symbol.PRN = d_satellite.get_PRN();
|
||||
d_sample_counter++;
|
||||
consume_each(1);
|
||||
d_symbol_history.push_back(current_symbol.Prompt_I);
|
||||
d_symbol_history_q.push_back(current_symbol.Prompt_Q);
|
||||
const bool just_locked_event = current_symbol.Flag_valid_symbol_output && !d_prev_valid_symbol_output;
|
||||
d_prev_valid_symbol_output = current_symbol.Flag_valid_symbol_output;
|
||||
|
||||
if (current_symbol.Flag_valid_symbol_output && d_await_post_lock_frame_decode && !just_locked_event)
|
||||
{
|
||||
d_post_lock_valid_symbols++;
|
||||
}
|
||||
|
||||
// After pilot-secondary lock (first valid symbol): decode once if 18 s history is ready.
|
||||
// While tracking: only decode on 18 s frame boundaries (no periodic LDPC polling).
|
||||
const bool at_frame_boundary =
|
||||
d_flag_frame_sync &&
|
||||
(d_sample_counter > d_frame_sync_index) &&
|
||||
((d_sample_counter - d_frame_sync_index) % static_cast<uint64_t>(BEIDOU_CNAV1_FRAME_SYMBOLS) == 0U);
|
||||
const bool post_lock_frame_ready =
|
||||
d_await_post_lock_frame_decode &&
|
||||
d_post_lock_valid_symbols >= static_cast<uint32_t>(BEIDOU_CNAV1_FRAME_SYMBOLS);
|
||||
bool frame_decoded = false;
|
||||
if (static_cast<int32_t>(d_symbol_history.size()) >= BEIDOU_CNAV1_FRAME_SYMBOLS)
|
||||
{
|
||||
const bool should_try_decode =
|
||||
current_symbol.Flag_valid_symbol_output &&
|
||||
((d_stat == 0 && just_locked_event) ||
|
||||
(d_stat == 0 && post_lock_frame_ready && !just_locked_event) ||
|
||||
(d_stat == 2 && at_frame_boundary &&
|
||||
d_sample_counter >= d_frame_sync_index + static_cast<uint64_t>(BEIDOU_CNAV1_FRAME_SYMBOLS)));
|
||||
|
||||
if (should_try_decode)
|
||||
{
|
||||
bool invert = false;
|
||||
int32_t matched_offset = -1;
|
||||
if (d_stat == 0 && just_locked_event)
|
||||
{
|
||||
// One-shot sync-decode on pilot-secondary lock (no periodic LDPC polling).
|
||||
const int32_t expected_prn_scan = d_satellite.get_PRN();
|
||||
const auto sec_candidates =
|
||||
find_secondary_code_candidates(d_symbol_history_q, expected_prn_scan);
|
||||
const auto& candidate_offsets = sec_candidates.second;
|
||||
constexpr int32_t local_offset_span = 32;
|
||||
const auto cn0_db_hz = static_cast<float>(current_symbol.CN0_dB_hz);
|
||||
|
||||
if (!candidate_offsets.empty())
|
||||
{
|
||||
for (const int32_t candidate_offset : candidate_offsets)
|
||||
{
|
||||
for (int32_t delta = -local_offset_span; delta <= local_offset_span; delta++)
|
||||
{
|
||||
const int32_t start_offset =
|
||||
(candidate_offset + delta + BEIDOU_CNAV1_FRAME_SYMBOLS) % BEIDOU_CNAV1_FRAME_SYMBOLS;
|
||||
for (const bool inv : {false, true})
|
||||
{
|
||||
if (!probe_frame_from_iq_buffers_with_offset(
|
||||
d_symbol_history, d_symbol_history_q, d_nav, expected_prn_scan, start_offset, inv))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
int32_t fail_stage = -1;
|
||||
if (decode_frame_from_iq_buffers_with_offset(
|
||||
d_symbol_history, d_symbol_history_q, d_nav, expected_prn_scan, start_offset, inv,
|
||||
cn0_db_hz, &fail_stage))
|
||||
{
|
||||
frame_decoded = true;
|
||||
invert = inv;
|
||||
matched_offset = start_offset;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (frame_decoded)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (frame_decoded)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (frame_decoded)
|
||||
{
|
||||
d_await_post_lock_frame_decode = false;
|
||||
d_post_lock_valid_symbols = 0U;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_await_post_lock_frame_decode = true;
|
||||
d_post_lock_valid_symbols = 1U;
|
||||
}
|
||||
}
|
||||
else if (d_stat == 0 && post_lock_frame_ready)
|
||||
{
|
||||
frame_decoded = decode_frame_from_history(false);
|
||||
if (!frame_decoded)
|
||||
{
|
||||
frame_decoded = decode_frame_from_history(true);
|
||||
}
|
||||
if (frame_decoded)
|
||||
{
|
||||
d_await_post_lock_frame_decode = false;
|
||||
d_post_lock_valid_symbols = 0U;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
frame_decoded = decode_frame_from_history(false);
|
||||
if (!frame_decoded)
|
||||
{
|
||||
frame_decoded = decode_frame_from_history(true);
|
||||
invert = frame_decoded;
|
||||
}
|
||||
}
|
||||
|
||||
if (frame_decoded)
|
||||
{
|
||||
d_CRC_error_counter = 0;
|
||||
d_flag_PLL_180_deg_phase_locked = invert;
|
||||
d_frame_sync_index = d_sample_counter;
|
||||
const int32_t frame_soh_offset = resolve_b1c_frame_soh_offset(matched_offset, d_prev_candidate_offset);
|
||||
d_TOW_at_current_symbol_ms = compute_b1c_tow_ms_at_current_symbol(
|
||||
d_nav.get_tow_s(), frame_soh_offset, d_symbol_duration_ms);
|
||||
d_flag_valid_word = true;
|
||||
publish_navigation(current_symbol.CN0_dB_hz);
|
||||
|
||||
if (!d_flag_frame_sync)
|
||||
{
|
||||
d_flag_frame_sync = true;
|
||||
d_stat = 2;
|
||||
LOG(INFO) << "Successful frame synchronization in channel " << d_channel
|
||||
<< " for satellite " << d_satellite
|
||||
<< " with CN0=" << current_symbol.CN0_dB_hz << " dB-Hz";
|
||||
#if __cplusplus == 201103L
|
||||
const int default_precision = std::cout.precision();
|
||||
#else
|
||||
const auto default_precision{std::cout.precision()};
|
||||
#endif
|
||||
std::cout << TEXT_GREEN << "B-CNAV1 frame sync on channel " << d_channel
|
||||
<< ", satellite " << d_satellite
|
||||
<< ", TOW=" << std::setprecision(3) << d_nav.get_tow_s()
|
||||
<< std::setprecision(default_precision) << " s" << TEXT_RESET << std::endl;
|
||||
}
|
||||
}
|
||||
else if (d_stat == 2)
|
||||
{
|
||||
d_CRC_error_counter++;
|
||||
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
|
||||
{
|
||||
DLOG(INFO) << "B-CNAV1 frame sync lost for satellite " << d_satellite
|
||||
<< " in channel " << d_channel;
|
||||
d_flag_frame_sync = false;
|
||||
d_flag_valid_word = false;
|
||||
d_stat = 0;
|
||||
d_CRC_error_counter = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (d_flag_valid_word && !frame_decoded)
|
||||
{
|
||||
d_TOW_at_current_symbol_ms += d_symbol_duration_ms;
|
||||
if (!current_symbol.Flag_valid_symbol_output)
|
||||
{
|
||||
d_flag_valid_word = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (d_flag_PLL_180_deg_phase_locked)
|
||||
{
|
||||
current_symbol.Carrier_phase_rads += GNSS_PI;
|
||||
current_symbol.Flag_PLL_180_deg_phase_locked = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
current_symbol.Flag_PLL_180_deg_phase_locked = false;
|
||||
}
|
||||
|
||||
if (d_flag_valid_word)
|
||||
{
|
||||
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
|
||||
current_symbol.Flag_valid_word = true;
|
||||
if (d_tow_to_trk)
|
||||
{
|
||||
const auto tow_obj = std::make_shared<TOW_to_trk>(TOW_to_trk(
|
||||
std::string(current_symbol.Signal),
|
||||
d_channel,
|
||||
d_TOW_at_current_symbol_ms,
|
||||
current_symbol.Tracking_sample_counter,
|
||||
d_nav.get_ephemeris().WN,
|
||||
d_satellite.get_PRN()));
|
||||
message_port_pub(pmt::mp("telemetry_to_trk"), pmt::make_any(tow_obj));
|
||||
}
|
||||
out[0] = std::move(current_symbol);
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
/*!
|
||||
* \file beidou_b1c_telemetry_decoder_gs.h
|
||||
* \brief BeiDou B1C B-CNAV1 telemetry decoder block
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_B1C_TELEMETRY_DECODER_GS_H
|
||||
#define GNSS_SDR_BEIDOU_B1C_TELEMETRY_DECODER_GS_H
|
||||
|
||||
#include "beidou_cnav1_navigation_message.h"
|
||||
#include "nav_message_packet.h"
|
||||
#include "telemetry_impl_interface.h"
|
||||
#include "tlm_conf.h"
|
||||
#include <boost/circular_buffer.hpp>
|
||||
#include <vector>
|
||||
|
||||
class beidou_b1c_telemetry_decoder_gs;
|
||||
using beidou_b1c_telemetry_decoder_gs_sptr = gnss_shared_ptr<beidou_b1c_telemetry_decoder_gs>;
|
||||
|
||||
beidou_b1c_telemetry_decoder_gs_sptr beidou_b1c_make_telemetry_decoder_gs(
|
||||
const Gnss_Satellite& satellite,
|
||||
const Tlm_Conf& conf);
|
||||
|
||||
class beidou_b1c_telemetry_decoder_gs : public telemetry_impl_interface
|
||||
{
|
||||
public:
|
||||
~beidou_b1c_telemetry_decoder_gs() override;
|
||||
void set_satellite(const Gnss_Satellite& satellite) override;
|
||||
void set_channel(int channel) override;
|
||||
void reset() override;
|
||||
int general_work(int noutput_items, gr_vector_int& ninput_items,
|
||||
gr_vector_const_void_star& input_items, gr_vector_void_star& output_items) override;
|
||||
|
||||
private:
|
||||
friend beidou_b1c_telemetry_decoder_gs_sptr beidou_b1c_make_telemetry_decoder_gs(
|
||||
const Gnss_Satellite& satellite, const Tlm_Conf& conf);
|
||||
|
||||
beidou_b1c_telemetry_decoder_gs(const Gnss_Satellite& satellite, const Tlm_Conf& conf);
|
||||
bool decode_frame_from_history(bool invert);
|
||||
bool decode_frame_from_history_with_offset(int32_t start_offset, bool invert);
|
||||
bool probe_frame_from_history_with_offset(int32_t start_offset, bool invert) const;
|
||||
void publish_navigation(double cn0_db_hz);
|
||||
|
||||
Beidou_Cnav1_Navigation_Message d_nav;
|
||||
Nav_Message_Packet d_nav_msg_packet;
|
||||
Gnss_Satellite d_satellite;
|
||||
boost::circular_buffer<float> d_symbol_history;
|
||||
boost::circular_buffer<float> d_symbol_history_q;
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
std::unique_ptr<Tlm_CRC_Stats> d_Tlm_CRC_Stats;
|
||||
uint64_t d_sample_counter{};
|
||||
uint64_t d_frame_sync_index{};
|
||||
int32_t d_channel{};
|
||||
int32_t d_stat{};
|
||||
int32_t d_CRC_error_counter{};
|
||||
uint32_t d_TOW_at_current_symbol_ms{};
|
||||
uint32_t d_symbol_duration_ms{};
|
||||
bool d_flag_valid_word{false};
|
||||
bool d_flag_frame_sync{false};
|
||||
bool d_flag_PLL_180_deg_phase_locked{false};
|
||||
bool d_dump{false};
|
||||
bool d_dump_mat{false};
|
||||
bool d_remove_dat{false};
|
||||
bool d_enable_navdata_monitor{false};
|
||||
bool d_dump_crc_stats{false};
|
||||
bool d_tow_to_trk{false};
|
||||
bool d_prev_valid_symbol_output{false};
|
||||
bool d_await_post_lock_frame_decode{false};
|
||||
uint32_t d_post_lock_valid_symbols{0U};
|
||||
std::vector<float> d_prev_candidate_frame;
|
||||
int32_t d_prev_candidate_offset{-1};
|
||||
bool d_have_prev_candidate_frame{false};
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -28,6 +28,8 @@ endif()
|
||||
|
||||
set(TRACKING_ADAPTER_SOURCES
|
||||
dll_pll_tracking_adapter.cc
|
||||
base_dll_pll_tracking.cc
|
||||
beidou_b1c_dll_pll_veml_tracking.cc
|
||||
galileo_e1_tcp_connector_tracking.cc
|
||||
gps_l1_ca_tcp_connector_tracking.cc
|
||||
gps_l1_ca_gaussian_tracking.cc
|
||||
@@ -37,6 +39,8 @@ set(TRACKING_ADAPTER_SOURCES
|
||||
|
||||
set(TRACKING_ADAPTER_HEADERS
|
||||
dll_pll_tracking_adapter.h
|
||||
base_dll_pll_tracking.h
|
||||
beidou_b1c_dll_pll_veml_tracking.h
|
||||
galileo_e1_tcp_connector_tracking.h
|
||||
gps_l1_ca_tcp_connector_tracking.h
|
||||
gps_l1_ca_gaussian_tracking.h
|
||||
|
||||
@@ -0,0 +1,98 @@
|
||||
/*!
|
||||
* \file base_dll_pll_tracking.cc
|
||||
* \brief Base class providing shared logic for DLL+PLL VEML tracking adapters.
|
||||
* \authors Carles Fernandez, 2025. carles.fernandez(at)cttc.cat
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2025 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "base_dll_pll_tracking.h"
|
||||
#include "configuration_interface.h"
|
||||
|
||||
#if USE_GLOG_AND_GFLAGS
|
||||
#include <glog/logging.h>
|
||||
#else
|
||||
#include <absl/log/log.h>
|
||||
#endif
|
||||
|
||||
BaseDllPllTracking::BaseDllPllTracking(
|
||||
const ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams)
|
||||
: role_(std::move(role)),
|
||||
item_size_(sizeof(gr_complex))
|
||||
{
|
||||
trk_params_.SetFromConfiguration(configuration, role_);
|
||||
|
||||
if (in_streams > 1)
|
||||
{
|
||||
LOG(ERROR) << "Only one input stream is supported.";
|
||||
}
|
||||
if (out_streams > 1)
|
||||
{
|
||||
LOG(ERROR) << "Only one output stream is supported.";
|
||||
}
|
||||
|
||||
DLOG(INFO) << "role " << role_;
|
||||
}
|
||||
|
||||
|
||||
void BaseDllPllTracking::connect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if (top_block)
|
||||
{ /* no connection needed */
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void BaseDllPllTracking::disconnect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if (top_block)
|
||||
{ /* no disconnection needed */
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr BaseDllPllTracking::get_left_block()
|
||||
{
|
||||
return tracking_sptr_;
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr BaseDllPllTracking::get_right_block()
|
||||
{
|
||||
return tracking_sptr_;
|
||||
}
|
||||
|
||||
|
||||
void BaseDllPllTracking::set_channel(unsigned int channel)
|
||||
{
|
||||
tracking_sptr_->set_channel(channel);
|
||||
}
|
||||
|
||||
|
||||
void BaseDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
|
||||
{
|
||||
tracking_sptr_->set_gnss_synchro(p_gnss_synchro);
|
||||
}
|
||||
|
||||
|
||||
void BaseDllPllTracking::start_tracking()
|
||||
{
|
||||
tracking_sptr_->start_tracking();
|
||||
}
|
||||
|
||||
|
||||
void BaseDllPllTracking::stop_tracking()
|
||||
{
|
||||
tracking_sptr_->stop_tracking();
|
||||
}
|
||||
@@ -0,0 +1,126 @@
|
||||
/*!
|
||||
* \file base_dll_pll_tracking.h
|
||||
* \brief Base class providing shared logic for DLL+PLL VEML tracking adapters.
|
||||
* \authors Carles Fernandez, 2025. carles.fernandez(at)cttc.cat
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2025 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BASE_DLL_PLL_TRACKING_H
|
||||
#define GNSS_SDR_BASE_DLL_PLL_TRACKING_H
|
||||
|
||||
#include "dll_pll_conf.h"
|
||||
#include "dll_pll_veml_tracking.h"
|
||||
#include "tracking_interface.h"
|
||||
#include <cstddef>
|
||||
#include <string>
|
||||
|
||||
/** \addtogroup Tracking
|
||||
* Classes for GNSS signal tracking.
|
||||
* \{ */
|
||||
/** \addtogroup Tracking_adapters tracking_adapters
|
||||
* Wrap GNU Radio blocks for GNSS signal tracking with a TrackingInterface
|
||||
* \{ */
|
||||
|
||||
class ConfigurationInterface;
|
||||
|
||||
/*!
|
||||
* \brief Base class providing shared logic for DLL+PLL tracking loop adapters
|
||||
* for GNSS signals.
|
||||
*/
|
||||
class BaseDllPllTracking : public TrackingInterface
|
||||
{
|
||||
public:
|
||||
/*!
|
||||
* \brief Base constructor of a Tracking block adapter
|
||||
*/
|
||||
explicit BaseDllPllTracking(const ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams);
|
||||
|
||||
/*!
|
||||
* \brief Default destructor of the Tracking block adapter
|
||||
*/
|
||||
~BaseDllPllTracking() override = default;
|
||||
|
||||
/*!
|
||||
* \brief Get role from the Tracking block adapter
|
||||
*/
|
||||
inline std::string role() final { return role_; }
|
||||
|
||||
/*!
|
||||
* \brief Get item_size from the Tracking block adapter
|
||||
*/
|
||||
inline size_t item_size() final { return item_size_; }
|
||||
|
||||
/*!
|
||||
* \brief Connect the Tracking block adapter
|
||||
*/
|
||||
void connect(gr::top_block_sptr top_block) final;
|
||||
|
||||
/*!
|
||||
* \brief Disconnect the sTracking block adapter
|
||||
*/
|
||||
void disconnect(gr::top_block_sptr top_block) final;
|
||||
|
||||
/*!
|
||||
* \brief Get left block from the Tracking block adapter
|
||||
*/
|
||||
gr::basic_block_sptr get_left_block() final;
|
||||
|
||||
/*!
|
||||
* \brief Get right block from the Tracking block adapter
|
||||
*/
|
||||
gr::basic_block_sptr get_right_block() final;
|
||||
|
||||
/*!
|
||||
* \brief Set tracking channel unique ID
|
||||
*/
|
||||
void set_channel(unsigned int channel) final;
|
||||
|
||||
/*!
|
||||
* \brief Set acquisition Gnss_Synchro object pointer
|
||||
* to exchange synchronization data between acquisition and tracking blocks
|
||||
*/
|
||||
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) final;
|
||||
|
||||
/*!
|
||||
* \brief Start the Tracking block
|
||||
*/
|
||||
void start_tracking() final;
|
||||
|
||||
/*!
|
||||
* \brief Stop the Tracking block
|
||||
*/
|
||||
void stop_tracking() final;
|
||||
|
||||
protected:
|
||||
// Can be used by each derived class
|
||||
inline Dll_Pll_Conf& config_params() { return trk_params_; }
|
||||
inline const Dll_Pll_Conf& config_params() const { return trk_params_; }
|
||||
inline void set_item_size(size_t item_size) { item_size_ = item_size; }
|
||||
|
||||
// Must be implemented / filled by each derived class
|
||||
virtual void configure_tracking_parameters(const ConfigurationInterface* configuration) = 0;
|
||||
virtual void create_tracking_block() = 0;
|
||||
dll_pll_veml_tracking_sptr tracking_sptr_;
|
||||
|
||||
private:
|
||||
// Managed by the base class
|
||||
Dll_Pll_Conf trk_params_;
|
||||
const std::string role_;
|
||||
size_t item_size_;
|
||||
};
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_BASE_DLL_PLL_TRACKING_H
|
||||
@@ -0,0 +1,101 @@
|
||||
/*!
|
||||
* \file beidou_b1c_dll_pll_veml_tracking.cc
|
||||
* \brief Adapts a DLL+PLL VEML tracking loop block to a TrackingInterface
|
||||
* for BeiDou B1C signals
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "beidou_b1c_dll_pll_veml_tracking.h"
|
||||
#include "Beidou_B1C.h"
|
||||
#include "configuration_interface.h"
|
||||
#include "display.h"
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
|
||||
#if USE_GLOG_AND_GFLAGS
|
||||
#include <glog/logging.h>
|
||||
#else
|
||||
#include <absl/log/log.h>
|
||||
#endif
|
||||
|
||||
BeidouB1cDllPllVemlTracking::BeidouB1cDllPllVemlTracking(
|
||||
const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams)
|
||||
: BaseDllPllTracking(configuration, role, in_streams, out_streams)
|
||||
{
|
||||
configure_tracking_parameters(configuration);
|
||||
create_tracking_block();
|
||||
}
|
||||
|
||||
|
||||
void BeidouB1cDllPllVemlTracking::configure_tracking_parameters(
|
||||
const ConfigurationInterface* configuration [[maybe_unused]])
|
||||
{
|
||||
config_params().vector_length = static_cast<int>(std::round(
|
||||
config_params().fs_in / (BEIDOU_B1C_CODE_RATE_CPS / BEIDOU_B1C_CODE_LENGTH_CHIPS)));
|
||||
config_params().system = 'C';
|
||||
const std::array<char, 3> sig{'1', 'D', '\0'};
|
||||
std::copy_n(sig.data(), 3, config_params().signal);
|
||||
const bool acq_qmboc = configuration->property("Acquisition_1D.qmboc", config_params().b1c_qmboc_tracking);
|
||||
// Keep tracking-side local replica modulation aligned with acquisition by default.
|
||||
config_params().b1c_qmboc_tracking = configuration->property(role() + ".b1c_qmboc_tracking", acq_qmboc);
|
||||
|
||||
if (config_params().extend_correlation_symbols < 1)
|
||||
{
|
||||
config_params().extend_correlation_symbols = 1;
|
||||
std::cout << TEXT_RED << "WARNING: BeiDou B1C. extend_correlation_symbols must be bigger than 0. Coherent integration has been set to 1 symbol (10 ms)" << TEXT_RESET << '\n';
|
||||
}
|
||||
else if (!config_params().track_pilot && config_params().extend_correlation_symbols > 1)
|
||||
{
|
||||
config_params().extend_correlation_symbols = 1;
|
||||
std::cout << TEXT_RED << "WARNING: BeiDou B1C. Extended coherent integration is not allowed when tracking the data component. Coherent integration has been set to 10 ms (1 symbol)" << TEXT_RESET << '\n';
|
||||
}
|
||||
|
||||
// B1C pilot: 1800-chip secondary sync spans one B-CNAV1 frame (18 s).
|
||||
// Keep coherent integration at 1 symbol (10 ms); extended correlator is not used.
|
||||
if (config_params().track_pilot)
|
||||
{
|
||||
config_params().extend_correlation_symbols = 1;
|
||||
const auto min_bit_sync_limit_s = static_cast<uint32_t>(
|
||||
BEIDOU_B1C_FRAME_PERIOD_S + 30);
|
||||
if (config_params().bit_synchronization_time_limit_s < min_bit_sync_limit_s)
|
||||
{
|
||||
config_params().bit_synchronization_time_limit_s = min_bit_sync_limit_s;
|
||||
}
|
||||
}
|
||||
|
||||
if ((config_params().extend_correlation_symbols > 1) &&
|
||||
(config_params().pll_bw_narrow_hz > config_params().pll_bw_hz ||
|
||||
config_params().dll_bw_narrow_hz > config_params().dll_bw_hz))
|
||||
{
|
||||
std::cout << TEXT_RED << "WARNING: BeiDou B1C. PLL or DLL narrow tracking bandwidth is higher than wide tracking one" << TEXT_RESET << '\n';
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void BeidouB1cDllPllVemlTracking::create_tracking_block()
|
||||
{
|
||||
if (config_params().item_type == "gr_complex")
|
||||
{
|
||||
tracking_sptr_ = dll_pll_veml_make_tracking(config_params());
|
||||
DLOG(INFO) << "tracking(" << tracking_sptr_->unique_id() << ")";
|
||||
}
|
||||
else
|
||||
{
|
||||
set_item_size(0);
|
||||
tracking_sptr_ = nullptr;
|
||||
LOG(WARNING) << config_params().item_type << " unknown tracking item type.";
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
/*!
|
||||
* \file beidou_b1c_dll_pll_veml_tracking.h
|
||||
* \brief Adapts a DLL+PLL VEML tracking loop block to a TrackingInterface
|
||||
* for BeiDou B1C signals
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_B1C_DLL_PLL_VEML_TRACKING_H
|
||||
#define GNSS_SDR_BEIDOU_B1C_DLL_PLL_VEML_TRACKING_H
|
||||
|
||||
#include "base_dll_pll_tracking.h"
|
||||
|
||||
class BeidouB1cDllPllVemlTracking : public BaseDllPllTracking
|
||||
{
|
||||
public:
|
||||
BeidouB1cDllPllVemlTracking(const ConfigurationInterface* configuration,
|
||||
const std::string& role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams);
|
||||
|
||||
inline std::string implementation() override
|
||||
{
|
||||
return "BEIDOU_B1C_DLL_PLL_VEML_Tracking";
|
||||
}
|
||||
|
||||
private:
|
||||
void configure_tracking_parameters(const ConfigurationInterface* configuration) override;
|
||||
void create_tracking_block() override;
|
||||
};
|
||||
|
||||
#endif // GNSS_SDR_BEIDOU_B1C_DLL_PLL_VEML_TRACKING_H
|
||||
@@ -22,6 +22,7 @@
|
||||
*/
|
||||
|
||||
#include "dll_pll_veml_tracking.h"
|
||||
#include "Beidou_B1C.h"
|
||||
#include "Beidou_B1I.h"
|
||||
#include "Beidou_B3I.h"
|
||||
#include "GLONASS_L1_L2_CA.h"
|
||||
@@ -33,6 +34,7 @@
|
||||
#include "Galileo_E5b.h"
|
||||
#include "Galileo_E6.h"
|
||||
#include "MATH_CONSTANTS.h"
|
||||
#include "beidou_b1c_signal_replica.h"
|
||||
#include "beidou_b1i_signal_replica.h"
|
||||
#include "beidou_b3i_signal_replica.h"
|
||||
#include "galileo_e1_signal_replica.h"
|
||||
@@ -180,6 +182,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_)
|
||||
map_signal_pretty_name["7X"] = "E5b";
|
||||
map_signal_pretty_name["L5"] = "L5";
|
||||
map_signal_pretty_name["B1"] = "B1I";
|
||||
map_signal_pretty_name["1D"] = "B1C";
|
||||
map_signal_pretty_name["B3"] = "B3I";
|
||||
map_signal_pretty_name["E6"] = "E6";
|
||||
map_signal_pretty_name["J1"] = "L1 C/A";
|
||||
@@ -449,6 +452,31 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_)
|
||||
d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
|
||||
d_data_secondary_code_string = BEIDOU_B3I_SECONDARY_CODE_STR;
|
||||
}
|
||||
else if (d_signal_type == "1D")
|
||||
{
|
||||
d_signal_carrier_freq = BEIDOU_B1C_FREQ_HZ;
|
||||
d_code_period = BEIDOU_B1C_CODE_PERIOD_S;
|
||||
d_code_chip_rate = BEIDOU_B1C_CODE_RATE_CPS;
|
||||
d_code_length_chips = static_cast<int32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS);
|
||||
d_symbols_per_bit = BEIDOU_B1C_SYMBOLS_PER_BIT;
|
||||
d_correlation_length_ms = static_cast<int32_t>(BEIDOU_B1C_CODE_PERIOD_MS);
|
||||
d_code_samples_per_chip = d_trk_parameters.b1c_qmboc_tracking ? 12 : 2;
|
||||
d_veml = true;
|
||||
d_trk_parameters.spc = d_trk_parameters.early_late_space_chips;
|
||||
d_trk_parameters.slope = static_cast<float>(-CalculateSlopeAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc));
|
||||
d_trk_parameters.y_intercept = static_cast<float>(GetYInterceptAbs(&SinBocCorrelationFunction<1, 1>, d_trk_parameters.spc));
|
||||
if (d_trk_parameters.track_pilot)
|
||||
{
|
||||
d_secondary = true;
|
||||
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1C_PILOT_SECONDARY_CODE_LENGTH);
|
||||
d_signal_pretty_name = d_signal_pretty_name + "P";
|
||||
}
|
||||
else
|
||||
{
|
||||
d_secondary = false;
|
||||
d_signal_pretty_name = d_signal_pretty_name + "D";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << "Invalid Signal argument when instantiating tracking blocks";
|
||||
@@ -609,8 +637,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_)
|
||||
d_carrier_loop_filter.set_params(d_trk_parameters.fll_bw_hz, d_trk_parameters.pll_bw_hz, d_trk_parameters.pll_filter_order);
|
||||
|
||||
// Initialization of local code replica
|
||||
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
|
||||
d_tracking_code.resize(2 * d_code_length_chips, 0.0);
|
||||
d_tracking_code.resize(static_cast<size_t>(d_code_samples_per_chip) * static_cast<size_t>(d_code_length_chips), 0.0);
|
||||
// correlator outputs (scalar)
|
||||
if (d_veml)
|
||||
{
|
||||
@@ -668,7 +695,7 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_)
|
||||
// Enable Data component prompt correlator (slave to Pilot prompt) if tracking uses Pilot signal
|
||||
if (d_trk_parameters.track_pilot)
|
||||
{
|
||||
d_data_code.resize(2 * d_code_length_chips, 0.0);
|
||||
d_data_code.resize(static_cast<size_t>(d_code_samples_per_chip) * static_cast<size_t>(d_code_length_chips), 0.0);
|
||||
}
|
||||
|
||||
// --- Initializations ---
|
||||
@@ -806,6 +833,7 @@ void dll_pll_veml_tracking::start_tracking()
|
||||
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
|
||||
d_carrier_phase_step_rad = TWO_PI * d_carrier_doppler_hz / d_trk_parameters.fs_in;
|
||||
d_carrier_phase_rate_step_rad = 0.0;
|
||||
d_b1c_prelock_output_pending = false;
|
||||
d_carr_ph_history.clear();
|
||||
d_code_ph_history.clear();
|
||||
std::array<char, 3> Signal_{};
|
||||
@@ -950,6 +978,32 @@ void dll_pll_veml_tracking::start_tracking()
|
||||
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
|
||||
}
|
||||
}
|
||||
else if (d_systemName == "Beidou" && d_signal_type == "1D")
|
||||
{
|
||||
const int32_t prn_index = static_cast<int32_t>(d_acquisition_gnss_synchro->PRN) - 1;
|
||||
const auto b1c_replica_fs = static_cast<int32_t>(d_code_samples_per_chip * BEIDOU_B1C_CODE_RATE_CPS);
|
||||
if (d_trk_parameters.track_pilot)
|
||||
{
|
||||
const std::array<char, 3> pilot_signal = {{'1', 'P', '\0'}};
|
||||
beidou_b1c_code_gen_float_sampled(
|
||||
d_tracking_code, pilot_signal, d_trk_parameters.b1c_qmboc_tracking,
|
||||
d_acquisition_gnss_synchro->PRN, b1c_replica_fs, 0, false);
|
||||
// Data component remains BOC(1,1) even when pilot is QMBOC.
|
||||
beidou_b1c_code_gen_float_sampled(
|
||||
d_data_code, Signal_, d_trk_parameters.b1c_qmboc_tracking,
|
||||
d_acquisition_gnss_synchro->PRN, b1c_replica_fs, 0, false);
|
||||
d_Prompt_Data[0] = gr_complex(0.0, 0.0);
|
||||
d_multicorrelator_cpu.set_data_code_and_prompt_tap(
|
||||
d_code_samples_per_chip * d_code_length_chips, d_data_code.data(), d_prompt_data_shift);
|
||||
d_secondary_code_string = BEIDOU_B1C_PILOT_SECONDARY_CODE[prn_index];
|
||||
}
|
||||
else
|
||||
{
|
||||
beidou_b1c_code_gen_float_sampled(
|
||||
d_tracking_code, Signal_, d_trk_parameters.b1c_qmboc_tracking,
|
||||
d_acquisition_gnss_synchro->PRN, b1c_replica_fs, 0, false);
|
||||
}
|
||||
}
|
||||
|
||||
else if (d_systemName == "Beidou" && d_signal_type == "B3")
|
||||
{
|
||||
@@ -1170,7 +1224,15 @@ bool dll_pll_veml_tracking::acquire_secondary()
|
||||
}
|
||||
}
|
||||
|
||||
if (abs(corr_value) == static_cast<int32_t>(d_secondary_code_length))
|
||||
auto lock_threshold = static_cast<int32_t>(d_secondary_code_length);
|
||||
if (d_systemName == "Beidou" && d_signal_type == "1D")
|
||||
{
|
||||
const float ratio = std::min(std::max(d_trk_parameters.b1c_secondary_lock_ratio, 0.5F), 1.0F);
|
||||
lock_threshold = std::max(1, static_cast<int32_t>(std::lround(
|
||||
ratio * static_cast<float>(d_secondary_code_length))));
|
||||
}
|
||||
|
||||
if (abs(corr_value) >= lock_threshold)
|
||||
{
|
||||
if (corr_value < 0)
|
||||
{
|
||||
@@ -1397,6 +1459,7 @@ void dll_pll_veml_tracking::clear_tracking_vars()
|
||||
d_code_error_filt_chips = 0.0;
|
||||
d_current_symbol = 0;
|
||||
d_current_data_symbol = 0;
|
||||
d_b1c_prelock_output_pending = false;
|
||||
d_Prompt_circular_buffer.clear();
|
||||
d_carrier_phase_rate_step_rad = 0.0;
|
||||
d_code_phase_rate_step_chips = 0.0;
|
||||
@@ -1628,6 +1691,61 @@ void dll_pll_veml_tracking::save_correlation_results()
|
||||
}
|
||||
|
||||
|
||||
void dll_pll_veml_tracking::map_correlator_to_iq(const gr_complex &c, float &out_i, float &out_q) const
|
||||
{
|
||||
if (d_interchange_iq)
|
||||
{
|
||||
out_i = c.imag();
|
||||
out_q = c.real();
|
||||
}
|
||||
else
|
||||
{
|
||||
out_i = c.real();
|
||||
out_q = c.imag();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void dll_pll_veml_tracking::assign_correlators_to_synchro(Gnss_Synchro &synchro) const
|
||||
{
|
||||
if (d_systemName == "Beidou" && d_signal_type == "1D" && d_trk_parameters.track_pilot)
|
||||
{
|
||||
const gr_complex pilot_raw = *d_Prompt;
|
||||
gr_complex pilot_ref = (d_state == 2) ? pilot_raw : d_P_accu;
|
||||
// Do not apply pilot-secondary sign during pre-lock (phase unknown until acquire_secondary()).
|
||||
|
||||
const float pilot_abs = std::abs(pilot_ref);
|
||||
gr_complex data_aligned = d_P_data_accu;
|
||||
if (pilot_abs > 1e-6F)
|
||||
{
|
||||
data_aligned = d_P_data_accu * std::conj(pilot_ref) / pilot_abs;
|
||||
}
|
||||
|
||||
float data_prompt = d_trk_parameters.b1c_prompt_use_data_q ? data_aligned.imag() : data_aligned.real();
|
||||
float pilot_i_for_sync = 0.0F;
|
||||
float pilot_q_unused = 0.0F;
|
||||
map_correlator_to_iq(pilot_raw, pilot_i_for_sync, pilot_q_unused);
|
||||
float pilot_prompt = pilot_i_for_sync;
|
||||
if (d_trk_parameters.b1c_prompt_normalize_power)
|
||||
{
|
||||
data_prompt *= d_trk_parameters.b1c_data_prompt_scale;
|
||||
pilot_prompt *= d_trk_parameters.b1c_pilot_prompt_scale;
|
||||
}
|
||||
synchro.Prompt_I = static_cast<double>(data_prompt);
|
||||
synchro.Prompt_Q = static_cast<double>(pilot_prompt);
|
||||
return;
|
||||
}
|
||||
|
||||
float prompt_i = 0.0F;
|
||||
float prompt_q = 0.0F;
|
||||
|
||||
map_correlator_to_iq(d_P_data_accu, prompt_i, prompt_q);
|
||||
|
||||
synchro.Prompt_I = static_cast<double>(prompt_i);
|
||||
synchro.Prompt_Q = static_cast<double>(prompt_q);
|
||||
}
|
||||
|
||||
|
||||
void dll_pll_veml_tracking::log_data()
|
||||
{
|
||||
if (d_dump)
|
||||
@@ -2055,6 +2173,29 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
|
||||
// enable write dump file this cycle (valid DLL/PLL cycle)
|
||||
log_data();
|
||||
|
||||
// Feed 10 ms B1C data symbols to telemetry while secondary lock is pending,
|
||||
// so telemetry can build a rolling 18 s frame history before the first valid symbol.
|
||||
if (d_systemName == "Beidou" && d_signal_type == "1D" && d_trk_parameters.track_pilot &&
|
||||
!d_pull_in_transitory && d_current_data_symbol == 0)
|
||||
{
|
||||
// State 2 does not update d_P_data_accu in save_correlation_results().
|
||||
// Use the instantaneous 10 ms data prompt and assign_correlators_to_synchro()
|
||||
// to publish data (Prompt_I) and pilot (Prompt_Q) to telemetry.
|
||||
d_P_data_accu = d_Prompt_Data[0];
|
||||
current_synchro_data = *d_acquisition_gnss_synchro;
|
||||
assign_correlators_to_synchro(current_synchro_data);
|
||||
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
|
||||
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
|
||||
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
|
||||
current_synchro_data.correlation_length_ms = d_correlation_length_ms;
|
||||
// Cache pre-lock symbols for telemetry history, but mark them invalid so
|
||||
// telemetry can avoid running BCH/LDPC before secondary alignment is confirmed.
|
||||
current_synchro_data.Flag_valid_symbol_output = false;
|
||||
d_b1c_prelock_output_pending = true;
|
||||
d_P_data_accu = gr_complex(0.0, 0.0);
|
||||
}
|
||||
|
||||
if (!d_pull_in_transitory)
|
||||
{
|
||||
if (d_secondary)
|
||||
@@ -2203,16 +2344,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
|
||||
// ########### Output the tracking results to Telemetry block ##########
|
||||
// Fill the acquisition data
|
||||
current_synchro_data = *d_acquisition_gnss_synchro;
|
||||
if (d_interchange_iq)
|
||||
{
|
||||
current_synchro_data.Prompt_I = static_cast<double>(d_P_data_accu.imag());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>(d_P_data_accu.real());
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Prompt_I = static_cast<double>(d_P_data_accu.real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>(d_P_data_accu.imag());
|
||||
}
|
||||
assign_correlators_to_synchro(current_synchro_data);
|
||||
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
|
||||
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
|
||||
@@ -2255,16 +2387,7 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
|
||||
// ########### Output the tracking results to Telemetry block ##########
|
||||
// Fill the acquisition data
|
||||
current_synchro_data = *d_acquisition_gnss_synchro;
|
||||
if (d_interchange_iq)
|
||||
{
|
||||
current_synchro_data.Prompt_I = static_cast<double>(d_P_data_accu.imag());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>(d_P_data_accu.real());
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Prompt_I = static_cast<double>(d_P_data_accu.real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>(d_P_data_accu.imag());
|
||||
}
|
||||
assign_correlators_to_synchro(current_synchro_data);
|
||||
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
|
||||
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
|
||||
@@ -2322,11 +2445,24 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
|
||||
|
||||
consume_each(d_current_prn_length_samples);
|
||||
// d_sample_counter += static_cast<uint64_t>(d_current_prn_length_samples);
|
||||
if (current_synchro_data.Flag_valid_symbol_output || loss_of_lock)
|
||||
if (current_synchro_data.Flag_valid_symbol_output || loss_of_lock || d_b1c_prelock_output_pending)
|
||||
{
|
||||
current_synchro_data.fs = static_cast<int64_t>(d_trk_parameters.fs_in);
|
||||
current_synchro_data.Tracking_sample_counter = this->nitems_read(0);
|
||||
current_synchro_data.Flag_valid_symbol_output = !loss_of_lock;
|
||||
if (loss_of_lock)
|
||||
{
|
||||
current_synchro_data.Flag_valid_symbol_output = false;
|
||||
d_b1c_prelock_output_pending = false;
|
||||
}
|
||||
else if (d_b1c_prelock_output_pending)
|
||||
{
|
||||
current_synchro_data.Flag_valid_symbol_output = false;
|
||||
d_b1c_prelock_output_pending = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Flag_valid_symbol_output = true;
|
||||
}
|
||||
current_synchro_data.Flag_PLL_180_deg_phase_locked = d_Flag_PLL_180_deg_phase_locked;
|
||||
|
||||
// generate new tag associated with gnss-synchro object
|
||||
|
||||
@@ -85,6 +85,8 @@ private:
|
||||
void clear_tracking_vars();
|
||||
void save_correlation_results();
|
||||
void log_data();
|
||||
void map_correlator_to_iq(const gr_complex &c, float &out_i, float &out_q) const;
|
||||
void assign_correlators_to_synchro(Gnss_Synchro &synchro) const;
|
||||
void configure_bit_synchronizer();
|
||||
bool cn0_and_tracking_lock_status(double coh_integration_time_s);
|
||||
bool acquire_secondary();
|
||||
@@ -219,6 +221,7 @@ private:
|
||||
bool d_Flag_PLL_180_deg_phase_locked;
|
||||
bool d_use_histogram_bit_sync;
|
||||
bool d_wait_for_bit_edge{false};
|
||||
bool d_b1c_prelock_output_pending{false};
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -159,4 +159,11 @@ void Dll_Pll_Conf::SetFromConfiguration(const ConfigurationInterface *configurat
|
||||
bs_transition_window_epochs = configuration->property(role + ".bs_transition_window_epochs", bs_transition_window_epochs);
|
||||
bs_tentative_events_required = configuration->property(role + ".bs_tentative_events_required", bs_tentative_events_required);
|
||||
bs_use_phase_dot_detector = configuration->property(role + ".bs_use_phase_dot_detector", bs_use_phase_dot_detector);
|
||||
|
||||
b1c_qmboc_tracking = configuration->property(role + ".b1c_qmboc_tracking", b1c_qmboc_tracking);
|
||||
b1c_prompt_use_data_q = configuration->property(role + ".b1c_prompt_use_data_q", b1c_prompt_use_data_q);
|
||||
b1c_prompt_normalize_power = configuration->property(role + ".b1c_prompt_normalize_power", b1c_prompt_normalize_power);
|
||||
b1c_data_prompt_scale = configuration->property(role + ".b1c_data_prompt_scale", b1c_data_prompt_scale);
|
||||
b1c_pilot_prompt_scale = configuration->property(role + ".b1c_pilot_prompt_scale", b1c_pilot_prompt_scale);
|
||||
b1c_secondary_lock_ratio = configuration->property(role + ".b1c_secondary_lock_ratio", b1c_secondary_lock_ratio);
|
||||
}
|
||||
|
||||
@@ -90,6 +90,13 @@ public:
|
||||
bool dump_mat{true};
|
||||
bool tow_to_trk{false};
|
||||
bool bs_use_phase_dot_detector{true};
|
||||
// BeiDou B1C-specific options.
|
||||
bool b1c_qmboc_tracking{true};
|
||||
bool b1c_prompt_use_data_q{true};
|
||||
bool b1c_prompt_normalize_power{true};
|
||||
float b1c_data_prompt_scale{1.7320508F}; // sqrt(3), compensates 1:3 data:pilot power.
|
||||
float b1c_pilot_prompt_scale{1.0F};
|
||||
float b1c_secondary_lock_ratio{0.88F};
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -28,6 +28,10 @@
|
||||
#include "acquisition_interface.h"
|
||||
#include "array_signal_conditioner.h"
|
||||
#include "beamformer_filter.h"
|
||||
#include "beidou_b1c_dll_pll_veml_tracking.h"
|
||||
#include "beidou_b1c_pcps_ambiguous_acquisition.h"
|
||||
#include "beidou_b1c_telemetry_decoder.h"
|
||||
#include "beidou_b1c_telemetry_decoder_gs.h"
|
||||
#include "beidou_dnav_telemetry_decoder_gs.h"
|
||||
#include "byte_to_short.h"
|
||||
#include "channel.h"
|
||||
@@ -209,6 +213,7 @@ const auto signal_mapping = std::vector<std::pair<std::string, std::string>>{
|
||||
{"1G", "GLONASS L1 C/A"},
|
||||
{"2G", "GLONASS L2 C/A"},
|
||||
{"B1", "BEIDOU B1I"},
|
||||
{"1D", "BEIDOU B1C"},
|
||||
{"B3", "BEIDOU B3I"},
|
||||
{"7X", "GALILEO E5b I (I/NAV OS)"},
|
||||
{"J1", "QZSS L1 C/A"},
|
||||
@@ -492,6 +497,10 @@ std::unique_ptr<AcquisitionInterface> get_acq_block(
|
||||
{
|
||||
return std::make_unique<PcpsAcquisitionAdapter>(configuration, role, implementation, in_streams, out_streams, BDS_B1);
|
||||
}
|
||||
else if (implementation == "BEIDOU_B1C_PCPS_Ambiguous_Acquisition")
|
||||
{
|
||||
return std::make_unique<BeidouB1cPcpsAmbiguousAcquisition>(configuration, role, in_streams, out_streams);
|
||||
}
|
||||
else if (implementation == "BEIDOU_B3I_PCPS_Acquisition")
|
||||
{
|
||||
return std::make_unique<PcpsAcquisitionAdapter>(configuration, role, implementation, in_streams, out_streams, BDS_B3);
|
||||
@@ -604,6 +613,10 @@ std::unique_ptr<TrackingInterface> get_trk_block(
|
||||
{
|
||||
return std::make_unique<DllPllTrackingAdapter>(configuration, role, implementation, in_streams, out_streams, BDS_B1);
|
||||
}
|
||||
else if (implementation == "BEIDOU_B1C_DLL_PLL_VEML_Tracking")
|
||||
{
|
||||
return std::make_unique<BeidouB1cDllPllVemlTracking>(configuration, role, in_streams, out_streams);
|
||||
}
|
||||
else if (implementation == "BEIDOU_B3I_DLL_PLL_Tracking")
|
||||
{
|
||||
return std::make_unique<DllPllTrackingAdapter>(configuration, role, implementation, in_streams, out_streams, BDS_B3);
|
||||
@@ -683,6 +696,11 @@ std::unique_ptr<TelemetryDecoderInterface> get_tlm_block(
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams)
|
||||
{
|
||||
if (implementation == "BEIDOU_B1C_Telemetry_Decoder")
|
||||
{
|
||||
return std::make_unique<BeidouB1cTelemetryDecoder>(configuration, role, in_streams, out_streams);
|
||||
}
|
||||
|
||||
telemetry_impl_interface_sptr telemetry;
|
||||
|
||||
if (implementation == "GPS_L1_CA_Telemetry_Decoder")
|
||||
|
||||
@@ -20,6 +20,7 @@
|
||||
*/
|
||||
|
||||
#include "gnss_flowgraph.h"
|
||||
#include "Beidou_B1C.h"
|
||||
#include "Beidou_B1I.h"
|
||||
#include "GLONASS_L1_L2_CA.h"
|
||||
#include "GPS_L1_CA.h"
|
||||
@@ -91,6 +92,7 @@ const auto signal_mapping = std::unordered_map<std::string, std::pair<std::strin
|
||||
{"7X", {"Galileo", "E5b"}},
|
||||
{"E6", {"Galileo", "E6"}},
|
||||
{"B1", {"Beidou", "B1"}},
|
||||
{"1D", {"Beidou", "B1C"}},
|
||||
{"B3", {"Beidou", "B3"}},
|
||||
{"1G", {"Glonass", "L1"}},
|
||||
{"2G", {"Glonass", "L2"}},
|
||||
@@ -258,6 +260,7 @@ void GNSSFlowgraph::init()
|
||||
mapStringValues_["1G"] = evGLO_1G;
|
||||
mapStringValues_["2G"] = evGLO_2G;
|
||||
mapStringValues_["B1"] = evBDS_B1;
|
||||
mapStringValues_["1D"] = evBDS_B1C;
|
||||
mapStringValues_["B3"] = evBDS_B3;
|
||||
mapStringValues_["J1"] = evQZS_J1;
|
||||
mapStringValues_["J5"] = evQZS_J5;
|
||||
@@ -1176,6 +1179,9 @@ int GNSSFlowgraph::connect_signal_conditioners_to_channels()
|
||||
case evBDS_B1:
|
||||
acq_fs = BEIDOU_B1I_OPT_ACQ_FS_SPS;
|
||||
break;
|
||||
case evBDS_B1C:
|
||||
acq_fs = BEIDOU_B1C_OPT_ACQ_FS_SPS;
|
||||
break;
|
||||
case evGLO_1G:
|
||||
case evGLO_2G:
|
||||
case evBDS_B3:
|
||||
@@ -2182,6 +2188,13 @@ void GNSSFlowgraph::set_signals_list()
|
||||
|
||||
for (const auto& prn : available_prn_map.at(gnss_system_str))
|
||||
{
|
||||
if (signal_str == "1D")
|
||||
{
|
||||
if ((prn >= 1U && prn <= 5U) || (prn >= 59U && prn <= 63U))
|
||||
{
|
||||
continue; // GEO satellites do not broadcast B1C (ICD section 3.1)
|
||||
}
|
||||
}
|
||||
if (signal_str == "J5" && prn > QZSS_L5_MAX_PRN)
|
||||
{
|
||||
// QZSS L1 C/B PRNs (203-206) do not transmit an L5 signal
|
||||
|
||||
@@ -264,6 +264,7 @@ private:
|
||||
evGLO_1G,
|
||||
evGLO_2G,
|
||||
evBDS_B1,
|
||||
evBDS_B1C,
|
||||
evBDS_B3,
|
||||
evQZS_J1,
|
||||
evQZS_J5
|
||||
|
||||
@@ -0,0 +1,46 @@
|
||||
/*!
|
||||
* \file Beidou_B1C.h
|
||||
* \brief Defines system parameters for BeiDou B1C open service signal
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* Parameter values from BDS-SIS-ICD-B1C-1.0 (2017-12).
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_B1C_H
|
||||
#define GNSS_SDR_BEIDOU_B1C_H
|
||||
|
||||
#include "Beidou_B1C_codes.h"
|
||||
#include "gnss_frequencies.h"
|
||||
#include <cstdint>
|
||||
|
||||
/** \addtogroup Core
|
||||
* \{ */
|
||||
/** \addtogroup System_Parameters
|
||||
* \{ */
|
||||
|
||||
constexpr double BEIDOU_B1C_FREQ_HZ = FREQ1; //!< §4.2.2, Table 4-1 [Hz]
|
||||
constexpr double BEIDOU_B1C_CODE_RATE_CPS = 1.023e6; //!< §5.2.1 [chips/s]
|
||||
constexpr double BEIDOU_B1C_CODE_LENGTH_CHIPS = 10230.0; //!< §5.2.1 [chips]
|
||||
constexpr double BEIDOU_B1C_CODE_PERIOD_S = 0.01; //!< §5.2.1, 10 ms [s]
|
||||
constexpr uint32_t BEIDOU_B1C_CODE_PERIOD_MS = 10; //!< §5.2.1 [ms]
|
||||
constexpr uint32_t BEIDOU_B1C_OPT_ACQ_FS_SPS = 2000000; //!< 2 samples/chip at 1.023 Mcps
|
||||
constexpr int32_t BEIDOU_B1C_PILOT_SECONDARY_CODE_LENGTH = 1800; //!< §5.2.2 [chips]
|
||||
constexpr int32_t BEIDOU_B1C_TELEMETRY_RATE_SPS = 100; //!< §6.2 [symbols/s]
|
||||
constexpr int32_t BEIDOU_B1C_SYMBOLS_PER_BIT = 1; //!< 100 sps, 10 ms/code period
|
||||
constexpr int32_t BEIDOU_B1C_FRAME_LENGTH_SYMBOLS = 1800; //!< §6.2 [symbols]
|
||||
constexpr int32_t BEIDOU_B1C_FRAME_PERIOD_S = 18; //!< §6.2 [s]
|
||||
constexpr int32_t BEIDOU_B1C_NUMBER_OF_PRNS = 63; //!< §5.2.1
|
||||
|
||||
/** \} */
|
||||
/** \} */
|
||||
#endif // GNSS_SDR_BEIDOU_B1C_H
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,403 @@
|
||||
/*!
|
||||
* \file Beidou_B1C_prn.h
|
||||
* \brief B1C PRN Weil code parameters from BDS-SIS-ICD-B1C-1.0 Table 5-2/5-3/5-4
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
#ifndef GNSS_SDR_BEIDOU_B1C_PRN_H
|
||||
#define GNSS_SDR_BEIDOU_B1C_PRN_H
|
||||
#include <cstdint>
|
||||
constexpr int32_t BEIDOU_B1C_DATA_PRN_W[63] = {
|
||||
2678,
|
||||
4802,
|
||||
958,
|
||||
859,
|
||||
3843,
|
||||
2232,
|
||||
124,
|
||||
4352,
|
||||
1816,
|
||||
1126,
|
||||
1860,
|
||||
4800,
|
||||
2267,
|
||||
424,
|
||||
4192,
|
||||
4333,
|
||||
2656,
|
||||
4148,
|
||||
243,
|
||||
1330,
|
||||
1593,
|
||||
1470,
|
||||
882,
|
||||
3202,
|
||||
5095,
|
||||
2546,
|
||||
1733,
|
||||
4795,
|
||||
4577,
|
||||
1627,
|
||||
3638,
|
||||
2553,
|
||||
3646,
|
||||
1087,
|
||||
1843,
|
||||
216,
|
||||
2245,
|
||||
726,
|
||||
1966,
|
||||
670,
|
||||
4130,
|
||||
53,
|
||||
4830,
|
||||
182,
|
||||
2181,
|
||||
2006,
|
||||
1080,
|
||||
2288,
|
||||
2027,
|
||||
271,
|
||||
915,
|
||||
497,
|
||||
139,
|
||||
3693,
|
||||
2054,
|
||||
4342,
|
||||
3342,
|
||||
2592,
|
||||
1007,
|
||||
310,
|
||||
4203,
|
||||
455,
|
||||
4318,
|
||||
};
|
||||
constexpr int32_t BEIDOU_B1C_DATA_PRN_P[63] = {
|
||||
699,
|
||||
694,
|
||||
7318,
|
||||
2127,
|
||||
715,
|
||||
6682,
|
||||
7850,
|
||||
5495,
|
||||
1162,
|
||||
7682,
|
||||
6792,
|
||||
9973,
|
||||
6596,
|
||||
2092,
|
||||
19,
|
||||
10151,
|
||||
6297,
|
||||
5766,
|
||||
2359,
|
||||
7136,
|
||||
1706,
|
||||
2128,
|
||||
6827,
|
||||
693,
|
||||
9729,
|
||||
1620,
|
||||
6805,
|
||||
534,
|
||||
712,
|
||||
1929,
|
||||
5355,
|
||||
6139,
|
||||
6339,
|
||||
1470,
|
||||
6867,
|
||||
7851,
|
||||
1162,
|
||||
7659,
|
||||
1156,
|
||||
2672,
|
||||
6043,
|
||||
2862,
|
||||
180,
|
||||
2663,
|
||||
6940,
|
||||
1645,
|
||||
1582,
|
||||
951,
|
||||
6878,
|
||||
7701,
|
||||
1823,
|
||||
2391,
|
||||
2606,
|
||||
822,
|
||||
6403,
|
||||
239,
|
||||
442,
|
||||
6769,
|
||||
2560,
|
||||
2502,
|
||||
5072,
|
||||
7268,
|
||||
341,
|
||||
};
|
||||
constexpr int32_t BEIDOU_B1C_PILOT_PRN_W[63] = {
|
||||
796,
|
||||
156,
|
||||
4198,
|
||||
3941,
|
||||
1374,
|
||||
1338,
|
||||
1833,
|
||||
2521,
|
||||
3175,
|
||||
168,
|
||||
2715,
|
||||
4408,
|
||||
3160,
|
||||
2796,
|
||||
459,
|
||||
3594,
|
||||
4813,
|
||||
586,
|
||||
1428,
|
||||
2371,
|
||||
2285,
|
||||
3377,
|
||||
4965,
|
||||
3779,
|
||||
4547,
|
||||
1646,
|
||||
1430,
|
||||
607,
|
||||
2118,
|
||||
4709,
|
||||
1149,
|
||||
3283,
|
||||
2473,
|
||||
1006,
|
||||
3670,
|
||||
1817,
|
||||
771,
|
||||
2173,
|
||||
740,
|
||||
1433,
|
||||
2458,
|
||||
3459,
|
||||
2155,
|
||||
1205,
|
||||
413,
|
||||
874,
|
||||
2463,
|
||||
1106,
|
||||
1590,
|
||||
3873,
|
||||
4026,
|
||||
4272,
|
||||
3556,
|
||||
128,
|
||||
1200,
|
||||
130,
|
||||
4494,
|
||||
1871,
|
||||
3073,
|
||||
4386,
|
||||
4098,
|
||||
1923,
|
||||
1176,
|
||||
};
|
||||
constexpr int32_t BEIDOU_B1C_PILOT_PRN_P[63] = {
|
||||
7575,
|
||||
2369,
|
||||
5688,
|
||||
539,
|
||||
2270,
|
||||
7306,
|
||||
6457,
|
||||
6254,
|
||||
5644,
|
||||
7119,
|
||||
1402,
|
||||
5557,
|
||||
5764,
|
||||
1073,
|
||||
7001,
|
||||
5910,
|
||||
10060,
|
||||
2710,
|
||||
1546,
|
||||
6887,
|
||||
1883,
|
||||
5613,
|
||||
5062,
|
||||
1038,
|
||||
10170,
|
||||
6484,
|
||||
1718,
|
||||
2535,
|
||||
1158,
|
||||
526,
|
||||
7331,
|
||||
5844,
|
||||
6423,
|
||||
6968,
|
||||
1280,
|
||||
1838,
|
||||
1989,
|
||||
6468,
|
||||
2091,
|
||||
1581,
|
||||
1453,
|
||||
6252,
|
||||
7122,
|
||||
7711,
|
||||
7216,
|
||||
2113,
|
||||
1095,
|
||||
1628,
|
||||
1713,
|
||||
6102,
|
||||
6123,
|
||||
6070,
|
||||
1115,
|
||||
8047,
|
||||
6795,
|
||||
2575,
|
||||
53,
|
||||
1729,
|
||||
6388,
|
||||
682,
|
||||
5565,
|
||||
7160,
|
||||
2277,
|
||||
};
|
||||
constexpr int32_t BEIDOU_B1C_PILOT_SECONDARY_W[63] = {
|
||||
269,
|
||||
1448,
|
||||
1028,
|
||||
1324,
|
||||
822,
|
||||
5,
|
||||
155,
|
||||
458,
|
||||
310,
|
||||
959,
|
||||
1238,
|
||||
1180,
|
||||
1288,
|
||||
334,
|
||||
885,
|
||||
1362,
|
||||
181,
|
||||
1648,
|
||||
838,
|
||||
313,
|
||||
750,
|
||||
225,
|
||||
1477,
|
||||
309,
|
||||
108,
|
||||
1457,
|
||||
149,
|
||||
322,
|
||||
271,
|
||||
576,
|
||||
1103,
|
||||
450,
|
||||
399,
|
||||
241,
|
||||
1045,
|
||||
164,
|
||||
513,
|
||||
687,
|
||||
422,
|
||||
303,
|
||||
324,
|
||||
495,
|
||||
725,
|
||||
780,
|
||||
367,
|
||||
882,
|
||||
631,
|
||||
37,
|
||||
647,
|
||||
1043,
|
||||
24,
|
||||
120,
|
||||
134,
|
||||
136,
|
||||
158,
|
||||
214,
|
||||
335,
|
||||
340,
|
||||
661,
|
||||
889,
|
||||
929,
|
||||
1002,
|
||||
1149,
|
||||
};
|
||||
constexpr int32_t BEIDOU_B1C_PILOT_SECONDARY_P[63] = {
|
||||
1889,
|
||||
1268,
|
||||
1593,
|
||||
1186,
|
||||
1239,
|
||||
1930,
|
||||
176,
|
||||
1696,
|
||||
26,
|
||||
1344,
|
||||
1271,
|
||||
1182,
|
||||
1381,
|
||||
1604,
|
||||
1333,
|
||||
1185,
|
||||
31,
|
||||
704,
|
||||
1190,
|
||||
1646,
|
||||
1385,
|
||||
113,
|
||||
860,
|
||||
1656,
|
||||
1921,
|
||||
1173,
|
||||
1928,
|
||||
57,
|
||||
150,
|
||||
1214,
|
||||
1148,
|
||||
1458,
|
||||
1519,
|
||||
1635,
|
||||
1257,
|
||||
1687,
|
||||
1382,
|
||||
1514,
|
||||
1,
|
||||
1583,
|
||||
1806,
|
||||
1664,
|
||||
1338,
|
||||
1111,
|
||||
1706,
|
||||
1543,
|
||||
1813,
|
||||
228,
|
||||
2871,
|
||||
2884,
|
||||
1823,
|
||||
75,
|
||||
11,
|
||||
63,
|
||||
1937,
|
||||
22,
|
||||
1768,
|
||||
1526,
|
||||
1402,
|
||||
1445,
|
||||
1680,
|
||||
1290,
|
||||
1245,
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,82 @@
|
||||
/*!
|
||||
* \file Beidou_CNAV1.h
|
||||
* \brief B-CNAV1 navigation message constants (BDS-SIS-ICD-B1C-1.0 §6.2, §7)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_CNAV1_H
|
||||
#define GNSS_SDR_BEIDOU_CNAV1_H
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
|
||||
#ifndef GNSS_SDR_TWO_N8
|
||||
constexpr double GNSS_SDR_TWO_N8 = 0.00390625;
|
||||
#endif
|
||||
#ifndef GNSS_SDR_TWO_N9
|
||||
constexpr double GNSS_SDR_TWO_N9 = 0.001953125;
|
||||
#endif
|
||||
#ifndef GNSS_SDR_TWO_N30
|
||||
constexpr double GNSS_SDR_TWO_N30 = 9.313225746154785e-10;
|
||||
#endif
|
||||
#ifndef GNSS_SDR_TWO_N32
|
||||
constexpr double GNSS_SDR_TWO_N32 = 2.3283064365386963e-10;
|
||||
#endif
|
||||
#ifndef GNSS_SDR_TWO_N34
|
||||
constexpr double GNSS_SDR_TWO_N34 = 5.820766091053686e-11;
|
||||
#endif
|
||||
#ifndef GNSS_SDR_TWO_N44
|
||||
constexpr double GNSS_SDR_TWO_N44 = 5.684341886080802e-14;
|
||||
#endif
|
||||
#ifndef GNSS_SDR_TWO_N50
|
||||
constexpr double GNSS_SDR_TWO_N50 = 8.881784197001252e-16;
|
||||
#endif
|
||||
#ifndef GNSS_SDR_TWO_N57
|
||||
constexpr double GNSS_SDR_TWO_N57 = 6.938893903907228e-18;
|
||||
#endif
|
||||
#ifndef GNSS_SDR_TWO_N66
|
||||
constexpr double GNSS_SDR_TWO_N66 = 1.3552527156068805e-20;
|
||||
#endif
|
||||
|
||||
constexpr int32_t BEIDOU_CNAV1_FRAME_SYMBOLS = 1800;
|
||||
constexpr int32_t BEIDOU_CNAV1_FRAME_PERIOD_S = 18;
|
||||
constexpr int32_t BEIDOU_CNAV1_SYMBOL_RATE_SPS = 100;
|
||||
constexpr int32_t BEIDOU_CNAV1_SUBFRAME1_SYMBOLS = 72;
|
||||
constexpr int32_t BEIDOU_CNAV1_SUBFRAME2_SYMBOLS = 1200;
|
||||
constexpr int32_t BEIDOU_CNAV1_SUBFRAME3_SYMBOLS = 528;
|
||||
constexpr int32_t BEIDOU_CNAV1_SOH_LSB_S = 18;
|
||||
constexpr int32_t BEIDOU_CNAV1_INTERLEAVED_SYMBOLS = 1728;
|
||||
constexpr int32_t BEIDOU_CNAV1_INTERLEAVE_ROWS = 36;
|
||||
constexpr int32_t BEIDOU_CNAV1_INTERLEAVE_COLS = 48;
|
||||
constexpr int32_t BEIDOU_CNAV1_SF2_DATA_BITS = 600;
|
||||
constexpr int32_t BEIDOU_CNAV1_SF3_DATA_BITS = 264;
|
||||
constexpr int32_t BEIDOU_CNAV1_CRC_BITS = 24;
|
||||
|
||||
constexpr int32_t BEIDOU_CNAV1_PAGE_IONO_UTC = 1;
|
||||
|
||||
constexpr double BEIDOU_CNAV1_A_REF_MEO = 27906100.0;
|
||||
constexpr double BEIDOU_CNAV1_A_REF_IGSO = 42162200.0;
|
||||
constexpr double BEIDOU_CNAV1_TOE_TOC_LSB = 300.0;
|
||||
constexpr double BEIDOU_CNAV1_DELTA_A_LSB = GNSS_SDR_TWO_N9;
|
||||
constexpr double BEIDOU_CNAV1_A_DOT_LSB = 3.725290298461914e-9; //!< 2^-25 m/s, Table 7-8
|
||||
constexpr double BEIDOU_CNAV1_DELTA_N0_LSB = GNSS_SDR_TWO_N44 * M_PI;
|
||||
constexpr double BEIDOU_CNAV1_DELTA_N0_DOT_LSB = GNSS_SDR_TWO_N57 * M_PI;
|
||||
constexpr double BEIDOU_CNAV1_M0_LSB = GNSS_SDR_TWO_N32 * M_PI;
|
||||
constexpr double BEIDOU_CNAV1_E_LSB = GNSS_SDR_TWO_N34;
|
||||
constexpr double BEIDOU_CNAV1_OMEGA_LSB = GNSS_SDR_TWO_N32 * M_PI;
|
||||
constexpr double BEIDOU_CNAV1_I0_LSB = GNSS_SDR_TWO_N32 * M_PI;
|
||||
constexpr double BEIDOU_CNAV1_OMEGADOT_LSB = GNSS_SDR_TWO_N44 * M_PI;
|
||||
constexpr double BEIDOU_CNAV1_IDOT_LSB = GNSS_SDR_TWO_N44 * M_PI;
|
||||
constexpr double BEIDOU_CNAV1_CIS_LSB = GNSS_SDR_TWO_N30;
|
||||
constexpr double BEIDOU_CNAV1_CIC_LSB = GNSS_SDR_TWO_N30;
|
||||
constexpr double BEIDOU_CNAV1_CRS_LSB = GNSS_SDR_TWO_N8;
|
||||
constexpr double BEIDOU_CNAV1_CRC_LSB = GNSS_SDR_TWO_N8;
|
||||
constexpr double BEIDOU_CNAV1_CUS_LSB = GNSS_SDR_TWO_N30;
|
||||
constexpr double BEIDOU_CNAV1_CUC_LSB = GNSS_SDR_TWO_N30;
|
||||
constexpr double BEIDOU_CNAV1_AF0_LSB = GNSS_SDR_TWO_N34;
|
||||
constexpr double BEIDOU_CNAV1_AF1_LSB = GNSS_SDR_TWO_N50;
|
||||
constexpr double BEIDOU_CNAV1_AF2_LSB = GNSS_SDR_TWO_N66;
|
||||
constexpr double BEIDOU_CNAV1_TGD_LSB = GNSS_SDR_TWO_N34;
|
||||
constexpr double BEIDOU_CNAV1_ISC_LSB = GNSS_SDR_TWO_N34;
|
||||
|
||||
#endif // GNSS_SDR_BEIDOU_CNAV1_H
|
||||
@@ -24,6 +24,9 @@ set(SYSTEM_PARAMETERS_SOURCES
|
||||
galileo_reduced_ced.cc
|
||||
beidou_dnav_navigation_message.cc
|
||||
beidou_dnav_ephemeris.cc
|
||||
beidou_cnav1_ephemeris.cc
|
||||
beidou_cnav1_navigation_message.cc
|
||||
beidou_cnav1_ldpc.cc
|
||||
sbas_ephemeris.cc
|
||||
gps_cnav_navigation_message.cc
|
||||
glonass_gnav_ephemeris.cc
|
||||
@@ -90,6 +93,15 @@ set(SYSTEM_PARAMETERS_HEADERS
|
||||
GPS_L2C.h
|
||||
GPS_L5.h
|
||||
Beidou_B1I.h
|
||||
Beidou_B1C.h
|
||||
Beidou_B1C_codes.h
|
||||
Beidou_B1C_prn.h
|
||||
Beidou_CNAV1.h
|
||||
beidou_cnav1_ldpc.h
|
||||
beidou_cnav1_ephemeris.h
|
||||
beidou_cnav1_navigation_message.h
|
||||
beidou_cnav1_iono.h
|
||||
beidou_cnav1_utc_model.h
|
||||
Beidou_B3I.h
|
||||
Beidou_DNAV.h
|
||||
MATH_CONSTANTS.h
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
#include "beidou_cnav1_ephemeris.h"
|
||||
@@ -0,0 +1,69 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_ephemeris.h
|
||||
* \brief BeiDou B-CNAV1 ephemeris storage (BDS-SIS-ICD-B1C-1.0 §7)
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_CNAV1_EPHEMERIS_H
|
||||
#define GNSS_SDR_BEIDOU_CNAV1_EPHEMERIS_H
|
||||
|
||||
#include "gnss_ephemeris.h"
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
class Beidou_Cnav1_Ephemeris : public Gnss_Ephemeris
|
||||
{
|
||||
public:
|
||||
Beidou_Cnav1_Ephemeris() = default;
|
||||
|
||||
double TGD_B1Cp{}; //!< §7.6 B1C pilot group delay
|
||||
double TGD_B2ap{}; //!< §7.6 B2a pilot group delay
|
||||
double ISC_B1Cd{}; //!< §7.6 B1C data-to-pilot intra-frequency delay correction
|
||||
double IODC{}; //!< §7.4.2
|
||||
double IODE{}; //!< §7.4.1
|
||||
int32_t sig_type{7}; //!< B1C data component identifier for PVT
|
||||
int32_t nav_type{1}; //!< 0: GEO, 1: MEO/IGSO
|
||||
|
||||
template <class Archive>
|
||||
void serialize(Archive& archive, const unsigned int version)
|
||||
{
|
||||
if (version)
|
||||
{
|
||||
};
|
||||
archive& BOOST_SERIALIZATION_NVP(PRN);
|
||||
archive& BOOST_SERIALIZATION_NVP(M_0);
|
||||
archive& BOOST_SERIALIZATION_NVP(delta_n);
|
||||
archive& BOOST_SERIALIZATION_NVP(ecc);
|
||||
archive& BOOST_SERIALIZATION_NVP(sqrtA);
|
||||
archive& BOOST_SERIALIZATION_NVP(OMEGA_0);
|
||||
archive& BOOST_SERIALIZATION_NVP(i_0);
|
||||
archive& BOOST_SERIALIZATION_NVP(omega);
|
||||
archive& BOOST_SERIALIZATION_NVP(OMEGAdot);
|
||||
archive& BOOST_SERIALIZATION_NVP(idot);
|
||||
archive& BOOST_SERIALIZATION_NVP(Cuc);
|
||||
archive& BOOST_SERIALIZATION_NVP(Cus);
|
||||
archive& BOOST_SERIALIZATION_NVP(Crc);
|
||||
archive& BOOST_SERIALIZATION_NVP(Crs);
|
||||
archive& BOOST_SERIALIZATION_NVP(Cic);
|
||||
archive& BOOST_SERIALIZATION_NVP(Cis);
|
||||
archive& BOOST_SERIALIZATION_NVP(toe);
|
||||
archive& BOOST_SERIALIZATION_NVP(toc);
|
||||
archive& BOOST_SERIALIZATION_NVP(af0);
|
||||
archive& BOOST_SERIALIZATION_NVP(af1);
|
||||
archive& BOOST_SERIALIZATION_NVP(af2);
|
||||
archive& BOOST_SERIALIZATION_NVP(WN);
|
||||
archive& BOOST_SERIALIZATION_NVP(tow);
|
||||
archive& BOOST_SERIALIZATION_NVP(TGD_B1Cp);
|
||||
archive& BOOST_SERIALIZATION_NVP(TGD_B2ap);
|
||||
archive& BOOST_SERIALIZATION_NVP(ISC_B1Cd);
|
||||
archive& BOOST_SERIALIZATION_NVP(IODC);
|
||||
archive& BOOST_SERIALIZATION_NVP(IODE);
|
||||
archive& BOOST_SERIALIZATION_NVP(sig_type);
|
||||
archive& BOOST_SERIALIZATION_NVP(nav_type);
|
||||
}
|
||||
};
|
||||
|
||||
#endif // GNSS_SDR_BEIDOU_CNAV1_EPHEMERIS_H
|
||||
@@ -0,0 +1,25 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_iono.h
|
||||
* \brief BeiDou B-CNAV1 ionospheric model (§7.8)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_CNAV1_IONO_H
|
||||
#define GNSS_SDR_BEIDOU_CNAV1_IONO_H
|
||||
|
||||
class Beidou_Cnav1_Iono
|
||||
{
|
||||
public:
|
||||
double alpha1{};
|
||||
double alpha2{};
|
||||
double alpha3{};
|
||||
double alpha4{};
|
||||
double alpha5{};
|
||||
double alpha6{};
|
||||
double alpha7{};
|
||||
double alpha8{};
|
||||
double alpha9{};
|
||||
bool valid{};
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,703 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_ldpc.cc
|
||||
* \brief B-CNAV1 LDPC graph and non-binary BP decoder over GF(64)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#include "beidou_cnav1_ldpc.h"
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
namespace
|
||||
{
|
||||
constexpr int32_t BITS_PER_SYMBOL = 6;
|
||||
constexpr int32_t Q = 64;
|
||||
constexpr int32_t NM = BEIDOU_CNAV1_LDPC_NM;
|
||||
constexpr int32_t FP_CANDIDATES = 8 + 2 * NM;
|
||||
constexpr float LLR_FALLBACK_GAP = 25.0F;
|
||||
|
||||
struct TruncMessage
|
||||
{
|
||||
std::array<uint8_t, NM> sym{};
|
||||
std::array<float, NM> llr{};
|
||||
};
|
||||
|
||||
struct FixedPathCandidate
|
||||
{
|
||||
uint8_t sym = 0;
|
||||
float llr = std::numeric_limits<float>::max();
|
||||
};
|
||||
|
||||
uint8_t bits_to_symbol_msb_first(const float* bit_llr, int32_t bit_offset)
|
||||
{
|
||||
uint8_t symbol = 0;
|
||||
for (int32_t bit = 0; bit < BITS_PER_SYMBOL; bit++)
|
||||
{
|
||||
symbol = static_cast<uint8_t>((symbol << 1U) | (bit_llr[bit_offset + bit] >= 0.0F ? 1U : 0U));
|
||||
}
|
||||
return symbol;
|
||||
}
|
||||
|
||||
void normalize_message(TruncMessage& msg)
|
||||
{
|
||||
const float min_llr = msg.llr[0];
|
||||
for (int32_t i = 0; i < NM; i++)
|
||||
{
|
||||
msg.llr[i] -= min_llr;
|
||||
}
|
||||
}
|
||||
|
||||
float lookup_message_llr_var_domain(const TruncMessage& c2v_msg, uint8_t h_inv, uint8_t x_var)
|
||||
{
|
||||
for (int32_t i = 0; i < NM; i++)
|
||||
{
|
||||
const uint8_t mapped = GaloisField64::mul(h_inv, c2v_msg.sym[i]);
|
||||
if (mapped == x_var)
|
||||
{
|
||||
return c2v_msg.llr[i];
|
||||
}
|
||||
}
|
||||
return c2v_msg.llr[NM - 1] + LLR_FALLBACK_GAP;
|
||||
}
|
||||
|
||||
void truncate_metric_vector(const std::array<float, Q>& metric, TruncMessage& out)
|
||||
{
|
||||
std::array<bool, Q> used{};
|
||||
for (int32_t kept = 0; kept < NM; kept++)
|
||||
{
|
||||
float best_llr = std::numeric_limits<float>::max();
|
||||
int32_t best_sym = 0;
|
||||
for (int32_t x = 0; x < Q; x++)
|
||||
{
|
||||
if (!used[static_cast<size_t>(x)] && metric[static_cast<size_t>(x)] < best_llr)
|
||||
{
|
||||
best_llr = metric[static_cast<size_t>(x)];
|
||||
best_sym = x;
|
||||
}
|
||||
}
|
||||
used[static_cast<size_t>(best_sym)] = true;
|
||||
out.sym[kept] = static_cast<uint8_t>(best_sym);
|
||||
out.llr[kept] = best_llr;
|
||||
}
|
||||
normalize_message(out);
|
||||
}
|
||||
|
||||
uint8_t row_syndrome(const BeidouCnav1LdpcGraph& graph, int32_t check_index, const std::vector<uint8_t>& codeword)
|
||||
{
|
||||
uint8_t syndrome = GaloisField64::kZero;
|
||||
const uint32_t begin = graph.check_offsets[static_cast<size_t>(check_index)];
|
||||
const uint32_t end = graph.check_offsets[static_cast<size_t>(check_index + 1)];
|
||||
for (uint32_t edge = begin; edge < end; edge++)
|
||||
{
|
||||
const uint16_t var = graph.check_to_var[edge];
|
||||
const uint8_t h_ij = graph.check_to_h[edge];
|
||||
syndrome = GaloisField64::add(syndrome, GaloisField64::mul(h_ij, codeword[var]));
|
||||
}
|
||||
return syndrome;
|
||||
}
|
||||
|
||||
bool syndrome_is_zero(const BeidouCnav1LdpcGraph& graph, const std::vector<uint8_t>& codeword)
|
||||
{
|
||||
for (int32_t row = 0; row < graph.num_checks; row++)
|
||||
{
|
||||
if (row_syndrome(graph, row, codeword) != GaloisField64::kZero)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void symbols_to_bits(const std::vector<uint8_t>& symbols, int32_t num_info_symbols, uint8_t* info_bits)
|
||||
{
|
||||
for (int32_t symbol_index = 0; symbol_index < num_info_symbols; symbol_index++)
|
||||
{
|
||||
const uint8_t symbol = symbols[static_cast<size_t>(symbol_index)];
|
||||
for (int32_t bit = 0; bit < BITS_PER_SYMBOL; bit++)
|
||||
{
|
||||
info_bits[symbol_index * BITS_PER_SYMBOL + bit] = static_cast<uint8_t>((symbol >> (5 - bit)) & 1U);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool graph_is_valid(const BeidouCnav1LdpcGraph& graph)
|
||||
{
|
||||
if (graph.num_checks <= 0 || graph.num_variables <= 0 || graph.row_weight <= 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
const int32_t expected_edges = graph.num_checks * graph.row_weight;
|
||||
return graph.check_offsets.size() == static_cast<size_t>(graph.num_checks + 1) &&
|
||||
graph.var_offsets.size() == static_cast<size_t>(graph.num_variables + 1) &&
|
||||
graph.check_to_var.size() == static_cast<size_t>(expected_edges) &&
|
||||
graph.check_to_h.size() == static_cast<size_t>(expected_edges) &&
|
||||
graph.var_to_check.size() == static_cast<size_t>(expected_edges) &&
|
||||
graph.var_to_edge.size() == static_cast<size_t>(expected_edges) &&
|
||||
graph.var_to_h.size() == static_cast<size_t>(expected_edges) &&
|
||||
graph.var_to_h_inv.size() == static_cast<size_t>(expected_edges);
|
||||
}
|
||||
|
||||
void initialize_channel_llr(
|
||||
const float* bit_llr,
|
||||
int32_t num_symbols,
|
||||
std::vector<uint8_t>& hard_symbol,
|
||||
std::vector<float>& channel_llr)
|
||||
{
|
||||
hard_symbol.assign(static_cast<size_t>(num_symbols), 0U);
|
||||
channel_llr.assign(static_cast<size_t>(num_symbols) * Q, 0.0F);
|
||||
|
||||
for (int32_t j = 0; j < num_symbols; j++)
|
||||
{
|
||||
const int32_t bit_offset = j * BITS_PER_SYMBOL;
|
||||
const uint8_t hard = bits_to_symbol_msb_first(bit_llr, bit_offset);
|
||||
hard_symbol[static_cast<size_t>(j)] = hard;
|
||||
std::array<float, BITS_PER_SYMBOL> abs_bit_llr{};
|
||||
for (int32_t b = 0; b < BITS_PER_SYMBOL; b++)
|
||||
{
|
||||
abs_bit_llr[b] = std::fabs(bit_llr[bit_offset + b]);
|
||||
}
|
||||
for (int32_t x = 0; x < Q; x++)
|
||||
{
|
||||
float metric = 0.0F;
|
||||
const auto diff = static_cast<uint8_t>(x ^ hard);
|
||||
for (int32_t b = 0; b < BITS_PER_SYMBOL; b++)
|
||||
{
|
||||
const auto bit_mask = static_cast<uint8_t>(1U << (BITS_PER_SYMBOL - 1 - b));
|
||||
if ((diff & bit_mask) != 0U)
|
||||
{
|
||||
metric += abs_bit_llr[b];
|
||||
}
|
||||
}
|
||||
channel_llr[static_cast<size_t>(j) * Q + static_cast<size_t>(x)] = metric;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void initialize_v2c_messages(
|
||||
const BeidouCnav1LdpcGraph& graph,
|
||||
const std::vector<float>& channel_llr,
|
||||
std::vector<TruncMessage>& v2c)
|
||||
{
|
||||
const int32_t edge_count = graph.num_checks * graph.row_weight;
|
||||
for (int32_t edge = 0; edge < edge_count; edge++)
|
||||
{
|
||||
const uint16_t var = graph.check_to_var[static_cast<size_t>(edge)];
|
||||
const uint8_t h_ij = graph.check_to_h[static_cast<size_t>(edge)];
|
||||
std::array<float, Q> mapped_metric{};
|
||||
for (int32_t x = 0; x < Q; x++)
|
||||
{
|
||||
const uint8_t y = GaloisField64::mul(h_ij, static_cast<uint8_t>(x));
|
||||
mapped_metric[static_cast<size_t>(y)] =
|
||||
channel_llr[static_cast<size_t>(var) * Q + static_cast<size_t>(x)];
|
||||
}
|
||||
truncate_metric_vector(mapped_metric, v2c[static_cast<size_t>(edge)]);
|
||||
}
|
||||
}
|
||||
|
||||
void variable_node_update(
|
||||
const BeidouCnav1LdpcGraph& graph,
|
||||
const std::vector<float>& channel_llr,
|
||||
std::vector<TruncMessage>& v2c,
|
||||
const std::vector<TruncMessage>& c2v,
|
||||
std::vector<uint8_t>& hard_codeword)
|
||||
{
|
||||
std::array<float, Q> posterior{};
|
||||
std::array<float, Q> outgoing_metric{};
|
||||
|
||||
for (int32_t var = 0; var < graph.num_variables; var++)
|
||||
{
|
||||
const uint32_t begin = graph.var_offsets[static_cast<size_t>(var)];
|
||||
const uint32_t end = graph.var_offsets[static_cast<size_t>(var + 1)];
|
||||
|
||||
for (int32_t x = 0; x < Q; x++)
|
||||
{
|
||||
float metric = channel_llr[static_cast<size_t>(var) * Q + static_cast<size_t>(x)];
|
||||
for (uint32_t p = begin; p < end; p++)
|
||||
{
|
||||
const uint32_t edge = graph.var_to_edge[p];
|
||||
const uint8_t h_fj_inv = graph.var_to_h_inv[p];
|
||||
metric += lookup_message_llr_var_domain(c2v[edge], h_fj_inv, static_cast<uint8_t>(x));
|
||||
}
|
||||
posterior[static_cast<size_t>(x)] = metric;
|
||||
}
|
||||
|
||||
int32_t hard_symbol = 0;
|
||||
float best_hard_llr = posterior[0];
|
||||
for (int32_t x = 1; x < Q; x++)
|
||||
{
|
||||
if (posterior[static_cast<size_t>(x)] < best_hard_llr)
|
||||
{
|
||||
best_hard_llr = posterior[static_cast<size_t>(x)];
|
||||
hard_symbol = x;
|
||||
}
|
||||
}
|
||||
hard_codeword[static_cast<size_t>(var)] = static_cast<uint8_t>(hard_symbol);
|
||||
|
||||
for (uint32_t p = begin; p < end; p++)
|
||||
{
|
||||
const uint32_t edge_out = graph.var_to_edge[p];
|
||||
const uint8_t h_ij = graph.var_to_h[p];
|
||||
for (int32_t y = 0; y < Q; y++)
|
||||
{
|
||||
outgoing_metric[static_cast<size_t>(y)] = std::numeric_limits<float>::max();
|
||||
}
|
||||
for (int32_t x = 0; x < Q; x++)
|
||||
{
|
||||
float metric = channel_llr[static_cast<size_t>(var) * Q + static_cast<size_t>(x)];
|
||||
for (uint32_t q = begin; q < end; q++)
|
||||
{
|
||||
const uint32_t edge_in = graph.var_to_edge[q];
|
||||
if (edge_in == edge_out)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
const uint8_t h_fj_inv = graph.var_to_h_inv[q];
|
||||
metric += lookup_message_llr_var_domain(c2v[edge_in], h_fj_inv, static_cast<uint8_t>(x));
|
||||
}
|
||||
const uint8_t y_out = GaloisField64::mul(h_ij, static_cast<uint8_t>(x));
|
||||
outgoing_metric[static_cast<size_t>(y_out)] = metric;
|
||||
}
|
||||
truncate_metric_vector(outgoing_metric, v2c[edge_out]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void build_fixed_path_candidates(
|
||||
const std::array<TruncMessage, 4>& in,
|
||||
std::array<FixedPathCandidate, FP_CANDIDATES>& cand)
|
||||
{
|
||||
for (auto& c : cand)
|
||||
{
|
||||
c.sym = 0U;
|
||||
c.llr = std::numeric_limits<float>::max();
|
||||
}
|
||||
|
||||
const uint8_t s0 = in[0].sym[0];
|
||||
const uint8_t s1 = in[1].sym[0];
|
||||
const uint8_t s2 = in[2].sym[0];
|
||||
const uint8_t s3 = in[3].sym[0];
|
||||
const float r0 = in[0].llr[0];
|
||||
const float r1 = in[1].llr[0];
|
||||
const float r2 = in[2].llr[0];
|
||||
const float r3 = in[3].llr[0];
|
||||
const uint8_t base_sym = GaloisField64::add(GaloisField64::add(s0, s1), GaloisField64::add(s2, s3));
|
||||
const float base_llr = r0 + r1 + r2 + r3;
|
||||
|
||||
int32_t idx = 0;
|
||||
cand[idx++] = {base_sym, base_llr};
|
||||
|
||||
for (int32_t l = 0; l < 4; l++)
|
||||
{
|
||||
const uint8_t sym = GaloisField64::add(base_sym, GaloisField64::add(in[l].sym[0], in[l].sym[1]));
|
||||
const float llr = base_llr - in[l].llr[0] + in[l].llr[1];
|
||||
cand[idx++] = {sym, llr};
|
||||
}
|
||||
|
||||
constexpr std::array<std::pair<int32_t, int32_t>, 6> pairs = {
|
||||
std::pair<int32_t, int32_t>{0, 1},
|
||||
{0, 2},
|
||||
{0, 3},
|
||||
{1, 2},
|
||||
{1, 3},
|
||||
{2, 3},
|
||||
};
|
||||
for (const auto& pair : pairs)
|
||||
{
|
||||
const int32_t a = pair.first;
|
||||
const int32_t b = pair.second;
|
||||
const uint8_t sym = GaloisField64::add(
|
||||
base_sym,
|
||||
GaloisField64::add(
|
||||
GaloisField64::add(in[a].sym[0], in[a].sym[1]),
|
||||
GaloisField64::add(in[b].sym[0], in[b].sym[1])));
|
||||
const float llr = base_llr - in[a].llr[0] - in[b].llr[0] + in[a].llr[1] + in[b].llr[1];
|
||||
cand[idx++] = {sym, llr};
|
||||
}
|
||||
|
||||
for (int32_t rank = 2; rank < NM && idx < FP_CANDIDATES; rank++)
|
||||
{
|
||||
for (int32_t l = 0; l < 4 && idx < FP_CANDIDATES; l++)
|
||||
{
|
||||
const uint8_t sym = GaloisField64::add(base_sym, GaloisField64::add(in[l].sym[0], in[l].sym[rank]));
|
||||
const float llr = base_llr - in[l].llr[0] + in[l].llr[rank];
|
||||
cand[idx++] = {sym, llr};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void fixed_path_check_update(
|
||||
const BeidouCnav1LdpcGraph& graph,
|
||||
const std::vector<TruncMessage>& v2c,
|
||||
std::vector<TruncMessage>& c2v)
|
||||
{
|
||||
std::array<int32_t, 4> edge_idx{};
|
||||
std::array<int32_t, 4> order{};
|
||||
std::array<TruncMessage, 4> in_sorted{};
|
||||
std::array<FixedPathCandidate, FP_CANDIDATES> candidates{};
|
||||
std::array<uint8_t, FP_CANDIDATES> T{};
|
||||
std::array<uint8_t, FP_CANDIDATES> Tbar{};
|
||||
|
||||
for (int32_t check = 0; check < graph.num_checks; check++)
|
||||
{
|
||||
const uint32_t begin = graph.check_offsets[static_cast<size_t>(check)];
|
||||
const uint32_t end = graph.check_offsets[static_cast<size_t>(check + 1)];
|
||||
if (static_cast<int32_t>(end - begin) != 4)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
for (int32_t l = 0; l < 4; l++)
|
||||
{
|
||||
edge_idx[l] = static_cast<int32_t>(begin) + l;
|
||||
order[l] = l;
|
||||
}
|
||||
std::sort(order.begin(), order.end(), [&](int32_t a, int32_t b) {
|
||||
return v2c[static_cast<size_t>(edge_idx[a])].llr[1] < v2c[static_cast<size_t>(edge_idx[b])].llr[1];
|
||||
});
|
||||
for (int32_t l = 0; l < 4; l++)
|
||||
{
|
||||
in_sorted[l] = v2c[static_cast<size_t>(edge_idx[order[l]])];
|
||||
}
|
||||
|
||||
build_fixed_path_candidates(in_sorted, candidates);
|
||||
|
||||
int32_t theta = 0;
|
||||
int32_t beta = 1;
|
||||
float min2 = in_sorted[0].llr[1];
|
||||
float second2 = in_sorted[1].llr[1];
|
||||
if (second2 < min2)
|
||||
{
|
||||
std::swap(min2, second2);
|
||||
std::swap(theta, beta);
|
||||
}
|
||||
for (int32_t l = 2; l < 4; l++)
|
||||
{
|
||||
const float v = in_sorted[l].llr[1];
|
||||
if (v < min2)
|
||||
{
|
||||
second2 = min2;
|
||||
beta = theta;
|
||||
min2 = v;
|
||||
theta = l;
|
||||
}
|
||||
else if (v < second2)
|
||||
{
|
||||
second2 = v;
|
||||
beta = l;
|
||||
}
|
||||
}
|
||||
const int32_t ref_idx = NM / 2;
|
||||
const float theta_ref = in_sorted[theta].llr[ref_idx];
|
||||
const float beta_ref = in_sorted[beta].llr[ref_idx];
|
||||
for (int32_t k = 0; k < FP_CANDIDATES; k++)
|
||||
{
|
||||
T[k] = (candidates[k].llr <= theta_ref) ? 1U : 0U;
|
||||
Tbar[k] = (candidates[k].llr <= beta_ref) ? 1U : 0U;
|
||||
}
|
||||
|
||||
for (int32_t l = 0; l < 4; l++)
|
||||
{
|
||||
TruncMessage out{};
|
||||
int32_t filled = 0;
|
||||
for (int32_t k = 0; k < FP_CANDIDATES && filled < NM; k++)
|
||||
{
|
||||
bool pass_gate = false;
|
||||
if (l == 0)
|
||||
{
|
||||
pass_gate = (T[k] != 0U);
|
||||
}
|
||||
else if (l == theta)
|
||||
{
|
||||
pass_gate = (Tbar[k] != 0U);
|
||||
}
|
||||
else
|
||||
{
|
||||
pass_gate = (T[k] != 0U) || (Tbar[k] != 0U);
|
||||
}
|
||||
if (!pass_gate)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
out.sym[filled] = GaloisField64::add(in_sorted[l].sym[0], candidates[k].sym);
|
||||
out.llr[filled] = candidates[k].llr - in_sorted[l].llr[0];
|
||||
filled++;
|
||||
}
|
||||
while (filled < NM)
|
||||
{
|
||||
out.sym[filled] = out.sym[filled - 1];
|
||||
out.llr[filled] = out.llr[filled - 1] + 1.0F;
|
||||
filled++;
|
||||
}
|
||||
normalize_message(out);
|
||||
c2v[static_cast<size_t>(edge_idx[order[l]])] = out;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void reorder_icd_column_bands_to_check_rows(
|
||||
int32_t num_checks,
|
||||
int32_t row_weight,
|
||||
int32_t num_entries,
|
||||
const uint16_t* icd_index,
|
||||
const uint8_t* icd_element,
|
||||
std::vector<uint16_t>& reordered_index,
|
||||
std::vector<uint8_t>& reordered_element)
|
||||
{
|
||||
// ICD publishes a compact matrix with 4-column bands; decoding should read each band
|
||||
// top-to-bottom ("按栏读取"), then move left-to-right to the next band.
|
||||
const int32_t base_rows = num_checks / row_weight;
|
||||
const int32_t cols_per_base_row = row_weight * row_weight;
|
||||
|
||||
reordered_index.assign(static_cast<size_t>(num_entries), 0U);
|
||||
reordered_element.assign(static_cast<size_t>(num_entries), 0U);
|
||||
|
||||
for (int32_t band = 0; band < row_weight; band++)
|
||||
{
|
||||
for (int32_t row = 0; row < base_rows; row++)
|
||||
{
|
||||
const int32_t src_offset = row * cols_per_base_row + band * row_weight;
|
||||
const int32_t check_row = band * base_rows + row;
|
||||
const int32_t dst_offset = check_row * row_weight;
|
||||
for (int32_t k = 0; k < row_weight; k++)
|
||||
{
|
||||
reordered_index[static_cast<size_t>(dst_offset + k)] =
|
||||
icd_index[static_cast<size_t>(src_offset + k)];
|
||||
reordered_element[static_cast<size_t>(dst_offset + k)] =
|
||||
icd_element[static_cast<size_t>(src_offset + k)];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool decode_block(
|
||||
const BeidouCnav1LdpcGraph& graph,
|
||||
const float* bit_llr,
|
||||
int32_t num_bits,
|
||||
uint8_t* info_bits,
|
||||
uint8_t* codeword_bits)
|
||||
{
|
||||
if (!graph_is_valid(graph))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const int32_t n = graph.num_variables;
|
||||
const int32_t k = n - graph.num_checks;
|
||||
if (num_bits < n * BITS_PER_SYMBOL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<uint8_t> hard_codeword(static_cast<size_t>(n), 0U);
|
||||
std::vector<uint8_t> hard_symbol;
|
||||
std::vector<float> channel_llr;
|
||||
initialize_channel_llr(bit_llr, n, hard_symbol, channel_llr);
|
||||
hard_codeword = hard_symbol;
|
||||
|
||||
const int32_t edge_count = graph.num_checks * graph.row_weight;
|
||||
std::vector<TruncMessage> v2c(static_cast<size_t>(edge_count));
|
||||
std::vector<TruncMessage> c2v(static_cast<size_t>(edge_count));
|
||||
initialize_v2c_messages(graph, channel_llr, v2c);
|
||||
c2v = v2c;
|
||||
|
||||
for (int32_t iteration = 0; iteration < BEIDOU_CNAV1_LDPC_MAX_ITER; iteration++)
|
||||
{
|
||||
variable_node_update(graph, channel_llr, v2c, c2v, hard_codeword);
|
||||
if (syndrome_is_zero(graph, hard_codeword))
|
||||
{
|
||||
if (info_bits != nullptr)
|
||||
{
|
||||
symbols_to_bits(hard_codeword, k, info_bits);
|
||||
}
|
||||
if (codeword_bits != nullptr)
|
||||
{
|
||||
symbols_to_bits(hard_codeword, n, codeword_bits);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
fixed_path_check_update(graph, v2c, c2v);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool init_graph_impl(
|
||||
int32_t num_checks,
|
||||
int32_t num_variables,
|
||||
int32_t row_weight,
|
||||
const uint16_t* icd_index,
|
||||
const uint8_t* icd_element,
|
||||
int32_t num_entries,
|
||||
BeidouCnav1LdpcGraph& graph)
|
||||
{
|
||||
if (num_checks <= 0 || num_variables <= 0 || row_weight <= 0 || icd_index == nullptr || icd_element == nullptr)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (num_entries != num_checks * row_weight)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (num_checks % row_weight != 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const int32_t base_rows = num_checks / row_weight;
|
||||
const int32_t cols_per_base_row = row_weight * row_weight;
|
||||
if (base_rows * cols_per_base_row != num_entries)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
std::vector<uint16_t> reordered_index;
|
||||
std::vector<uint8_t> reordered_element;
|
||||
reorder_icd_column_bands_to_check_rows(
|
||||
num_checks,
|
||||
row_weight,
|
||||
num_entries,
|
||||
icd_index,
|
||||
icd_element,
|
||||
reordered_index,
|
||||
reordered_element);
|
||||
const uint16_t* check_index = reordered_index.data();
|
||||
const uint8_t* check_element = reordered_element.data();
|
||||
|
||||
graph.num_checks = num_checks;
|
||||
graph.num_variables = num_variables;
|
||||
graph.row_weight = row_weight;
|
||||
graph.check_offsets.assign(static_cast<size_t>(num_checks + 1), 0U);
|
||||
graph.check_to_var.assign(static_cast<size_t>(num_entries), 0U);
|
||||
graph.check_to_h.assign(static_cast<size_t>(num_entries), 0U);
|
||||
graph.var_offsets.assign(static_cast<size_t>(num_variables + 1), 0U);
|
||||
graph.var_to_check.assign(static_cast<size_t>(num_entries), 0U);
|
||||
graph.var_to_edge.assign(static_cast<size_t>(num_entries), 0U);
|
||||
graph.var_to_h.assign(static_cast<size_t>(num_entries), 0U);
|
||||
graph.var_to_h_inv.assign(static_cast<size_t>(num_entries), 0U);
|
||||
|
||||
for (int32_t i = 0; i < num_checks; i++)
|
||||
{
|
||||
graph.check_offsets[static_cast<size_t>(i + 1)] =
|
||||
graph.check_offsets[static_cast<size_t>(i)] + static_cast<uint32_t>(row_weight);
|
||||
}
|
||||
|
||||
std::vector<uint32_t> var_degree(static_cast<size_t>(num_variables), 0U);
|
||||
for (int32_t edge = 0; edge < num_entries; edge++)
|
||||
{
|
||||
const uint16_t var = check_index[edge];
|
||||
const uint8_t h_ij = check_element[edge];
|
||||
if (var >= static_cast<uint16_t>(num_variables) ||
|
||||
!GaloisField64::valid_symbol(h_ij) ||
|
||||
h_ij == GaloisField64::kZero)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
graph.check_to_var[static_cast<size_t>(edge)] = var;
|
||||
graph.check_to_h[static_cast<size_t>(edge)] = h_ij;
|
||||
var_degree[static_cast<size_t>(var)]++;
|
||||
}
|
||||
|
||||
for (int32_t j = 0; j < num_variables; j++)
|
||||
{
|
||||
graph.var_offsets[static_cast<size_t>(j + 1)] =
|
||||
graph.var_offsets[static_cast<size_t>(j)] + var_degree[static_cast<size_t>(j)];
|
||||
}
|
||||
|
||||
std::vector<uint32_t> cursor = graph.var_offsets;
|
||||
for (int32_t i = 0; i < num_checks; i++)
|
||||
{
|
||||
const uint32_t begin = graph.check_offsets[static_cast<size_t>(i)];
|
||||
const uint32_t end = graph.check_offsets[static_cast<size_t>(i + 1)];
|
||||
for (uint32_t edge = begin; edge < end; edge++)
|
||||
{
|
||||
const uint16_t var = graph.check_to_var[edge];
|
||||
const uint8_t h_ij = graph.check_to_h[edge];
|
||||
const uint32_t pos = cursor[static_cast<size_t>(var)]++;
|
||||
graph.var_to_check[pos] = static_cast<uint16_t>(i);
|
||||
graph.var_to_edge[pos] = edge;
|
||||
graph.var_to_h[pos] = h_ij;
|
||||
graph.var_to_h_inv[pos] = GaloisField64::inv(h_ij);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
bool beidou_cnav1_ldpc_init_graph(
|
||||
int32_t num_checks,
|
||||
int32_t num_variables,
|
||||
int32_t row_weight,
|
||||
const uint16_t* icd_index,
|
||||
const uint8_t* icd_element,
|
||||
int32_t num_entries,
|
||||
BeidouCnav1LdpcGraph& graph)
|
||||
{
|
||||
return init_graph_impl(num_checks, num_variables, row_weight, icd_index, icd_element, num_entries, graph);
|
||||
}
|
||||
|
||||
const BeidouCnav1LdpcGraph& beidou_cnav1_ldpc_graph_200_100()
|
||||
{
|
||||
static const BeidouCnav1LdpcGraph graph = []() {
|
||||
BeidouCnav1LdpcGraph out;
|
||||
const bool ok = beidou_cnav1_ldpc_init_graph(
|
||||
BEIDOU_CNAV1_LDPC_M200,
|
||||
BEIDOU_CNAV1_LDPC_N200,
|
||||
BEIDOU_CNAV1_LDPC_DC200,
|
||||
BEIDOU_CNAV1_H100_200_INDEX,
|
||||
BEIDOU_CNAV1_H100_200_ELEMENT,
|
||||
BEIDOU_CNAV1_LDPC_M200 * BEIDOU_CNAV1_LDPC_DC200,
|
||||
out);
|
||||
if (!ok)
|
||||
{
|
||||
return BeidouCnav1LdpcGraph{};
|
||||
}
|
||||
return out;
|
||||
}();
|
||||
return graph;
|
||||
}
|
||||
|
||||
const BeidouCnav1LdpcGraph& beidou_cnav1_ldpc_graph_88_44()
|
||||
{
|
||||
static const BeidouCnav1LdpcGraph graph = []() {
|
||||
BeidouCnav1LdpcGraph out;
|
||||
const bool ok = beidou_cnav1_ldpc_init_graph(
|
||||
BEIDOU_CNAV1_LDPC_M88,
|
||||
BEIDOU_CNAV1_LDPC_N88,
|
||||
BEIDOU_CNAV1_LDPC_DC88,
|
||||
BEIDOU_CNAV1_H44_88_INDEX,
|
||||
BEIDOU_CNAV1_H44_88_ELEMENT,
|
||||
BEIDOU_CNAV1_LDPC_M88 * BEIDOU_CNAV1_LDPC_DC88,
|
||||
out);
|
||||
if (!ok)
|
||||
{
|
||||
return BeidouCnav1LdpcGraph{};
|
||||
}
|
||||
return out;
|
||||
}();
|
||||
return graph;
|
||||
}
|
||||
|
||||
bool beidou_cnav1_ldpc_decode_200_100(const float* symbol_llr, int32_t num_bits, uint8_t* info_bits600)
|
||||
{
|
||||
return decode_block(beidou_cnav1_ldpc_graph_200_100(), symbol_llr, num_bits, info_bits600, nullptr);
|
||||
}
|
||||
|
||||
bool beidou_cnav1_ldpc_decode_88_44(const float* symbol_llr, int32_t num_bits, uint8_t* info_bits264)
|
||||
{
|
||||
return decode_block(beidou_cnav1_ldpc_graph_88_44(), symbol_llr, num_bits, info_bits264, nullptr);
|
||||
}
|
||||
|
||||
bool beidou_cnav1_ldpc_decode_200_100_codeword(const float* symbol_llr, int32_t num_bits, uint8_t* codeword_bits1200)
|
||||
{
|
||||
return decode_block(beidou_cnav1_ldpc_graph_200_100(), symbol_llr, num_bits, nullptr, codeword_bits1200);
|
||||
}
|
||||
|
||||
bool beidou_cnav1_ldpc_decode_88_44_codeword(const float* symbol_llr, int32_t num_bits, uint8_t* codeword_bits528)
|
||||
{
|
||||
return decode_block(beidou_cnav1_ldpc_graph_88_44(), symbol_llr, num_bits, nullptr, codeword_bits528);
|
||||
}
|
||||
@@ -0,0 +1,183 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_ldpc.h
|
||||
* \brief B-CNAV1 NB-LDPC decoder (BDS-SIS-ICD-B1C-1.0 §6.2.2, Appendix)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_CNAV1_LDPC_DECODER_H
|
||||
#define GNSS_SDR_BEIDOU_CNAV1_LDPC_DECODER_H
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_N200 = 200;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_K200 = 100;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_M200 = 100;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_DC200 = 4;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_N88 = 88;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_K88 = 44;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_M88 = 44;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_DC88 = 4;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_NM = 8;
|
||||
constexpr int32_t BEIDOU_CNAV1_LDPC_MAX_ITER = 15;
|
||||
|
||||
/*
|
||||
* GF(2^6) LUTs follow BDS-SIS-ICD-B1C-1.0 Appendix mapping tables.
|
||||
* Primitive polynomial: p(x)=x^6+x+1.
|
||||
*/
|
||||
constexpr uint16_t BEIDOU_CNAV1_H100_200_INDEX[400] = {
|
||||
11, 62, 102, 150, 9, 60, 100, 148, 0, 51, 142, 197, 22, 80, 116, 154, 4, 90,
|
||||
131, 177, 47, 95, 138, 191, 51, 79, 146, 195, 44, 75, 142, 190, 13, 57, 135,
|
||||
198, 24, 65, 120, 173, 6, 88, 129, 179, 7, 89, 130, 176, 6, 58, 106, 158, 8,
|
||||
60, 108, 160, 44, 92, 139, 188, 4, 56, 104, 156, 10, 61, 101, 149, 39, 87, 123,
|
||||
168, 15, 67, 105, 167, 50, 78, 145, 194, 17, 98, 151, 187, 46, 94, 137, 190,
|
||||
14, 66, 104, 166, 7, 59, 107, 159, 21, 83, 119, 153, 31, 87, 114, 167, 2, 49,
|
||||
140, 199, 12, 64, 106, 164, 40, 53, 132, 159, 19, 96, 149, 185, 16, 68, 112,
|
||||
168, 14, 58, 132, 199, 34, 69, 125, 162, 23, 75, 119, 175, 42, 96, 144, 192,
|
||||
8, 63, 103, 151, 23, 81, 117, 155, 24, 93, 111, 182, 20, 72, 116, 172, 17, 69,
|
||||
113, 169, 34, 82, 130, 182, 1, 53, 101, 153, 46, 73, 140, 188, 13, 65, 107,
|
||||
165, 2, 54, 102, 154, 18, 70, 114, 170, 26, 67, 122, 175, 29, 77, 125, 177,
|
||||
36, 84, 120, 169, 25, 94, 108, 183, 39, 89, 137, 185, 21, 73, 117, 173, 28,
|
||||
76, 124, 176, 36, 90, 138, 186, 33, 68, 124, 161, 12, 56, 134, 197, 29, 85,
|
||||
112, 165, 45, 93, 136, 189, 27, 64, 123, 172, 28, 84, 115, 164, 25, 66, 121,
|
||||
174, 37, 85, 121, 170, 3, 50, 141, 196, 48, 76, 147, 192, 35, 70, 126, 163,
|
||||
32, 80, 128, 180, 0, 52, 100, 152, 43, 52, 135, 158, 35, 83, 131, 183, 10,
|
||||
62, 110, 162, 19, 71, 115, 171, 15, 59, 133, 196, 33, 81, 129, 181, 41, 54,
|
||||
133, 156, 20, 82, 118, 152, 38, 86, 122, 171, 30, 78, 126, 178, 9, 61, 109,
|
||||
161, 26, 95, 109, 180, 45, 72, 143, 191, 1, 48, 143, 198, 40, 98, 146, 194,
|
||||
18, 99, 148, 184, 5, 57, 105, 157, 41, 99, 147, 195, 31, 79, 127, 179, 3, 55,
|
||||
103, 155, 22, 74, 118, 174, 37, 91, 139, 187, 5, 91, 128, 178, 30, 86, 113,
|
||||
166, 43, 97, 145, 193, 16, 97, 150, 186, 11, 63, 111, 163, 32, 71, 127, 160,
|
||||
42, 55, 134, 157, 38, 88, 136, 184, 47, 74, 141, 189, 49, 77, 144, 193, 27,
|
||||
92, 110, 181};
|
||||
constexpr uint8_t BEIDOU_CNAV1_H100_200_ELEMENT[400] = {
|
||||
35, 13, 51, 60, 1, 44, 53, 24, 1, 45, 15, 6, 45, 15, 6, 1, 1, 44, 53, 24, 1,
|
||||
45, 15, 6, 35, 46, 56, 15, 6, 1, 45, 15, 15, 6, 1, 45, 44, 53, 24, 1, 24, 1,
|
||||
44, 30, 1, 45, 15, 6, 30, 24, 1, 44, 24, 1, 44, 30, 45, 15, 6, 1, 17, 38, 49,
|
||||
11, 24, 1, 44, 30, 24, 1, 44, 53, 24, 1, 44, 53, 30, 24, 1, 44, 33, 42, 14,
|
||||
24, 33, 42, 14, 24, 45, 15, 6, 1, 1, 45, 15, 6, 30, 24, 1, 44, 24, 1, 44, 53,
|
||||
1, 44, 30, 24, 57, 25, 9, 41, 1, 45, 15, 6, 1, 45, 15, 6, 42, 36, 12, 57, 6,
|
||||
1, 45, 15, 24, 1, 44, 53, 24, 1, 44, 30, 1, 45, 15, 6, 1, 45, 15, 6, 44, 53,
|
||||
24, 1, 30, 24, 1, 44, 1, 44, 30, 24, 53, 24, 1, 44, 1, 44, 53, 24, 27, 28,
|
||||
30, 31, 53, 24, 1, 44, 24, 1, 44, 30, 45, 15, 6, 1, 30, 24, 1, 44, 1, 45, 15,
|
||||
6, 26, 22, 14, 2, 35, 13, 18, 60, 45, 15, 6, 1, 30, 1, 44, 7, 6, 1, 45, 15,
|
||||
6, 1, 45, 15, 53, 24, 1, 44, 24, 1, 44, 53, 30, 24, 1, 44, 1, 44, 30, 24, 44,
|
||||
53, 24, 1, 53, 24, 1, 44, 44, 30, 24, 1, 30, 24, 1, 44, 1, 44, 30, 24, 1, 44,
|
||||
30, 24, 41, 16, 29, 51, 1, 44, 30, 24, 38, 23, 22, 7, 44, 53, 24, 1, 1, 45,
|
||||
15, 6, 30, 24, 1, 44, 53, 24, 1, 44, 6, 1, 45, 15, 24, 1, 44, 53, 35, 46, 56,
|
||||
15, 5, 33, 42, 14, 54, 7, 38, 23, 1, 45, 15, 6, 44, 30, 24, 1, 6, 1, 45, 15,
|
||||
53, 24, 1, 44, 44, 53, 24, 1, 1, 44, 53, 24, 1, 44, 30, 24, 44, 30, 24, 1, 1,
|
||||
44, 53, 24, 45, 15, 6, 1, 6, 1, 45, 15, 1, 44, 53, 24, 42, 47, 37, 32, 51, 60,
|
||||
35, 13, 29, 28, 30, 31, 6, 1, 45, 15, 24, 1, 44, 53, 44, 53, 24, 1, 44, 30,
|
||||
24, 1, 38, 49, 11, 17, 44, 30, 24, 1, 24, 1, 44, 30, 24, 1, 44, 30, 1, 44, 53,
|
||||
24, 53, 24, 1, 44};
|
||||
constexpr uint16_t BEIDOU_CNAV1_H44_88_INDEX[176] = {
|
||||
14, 35, 56, 70, 11, 29, 55, 73, 13, 39, 53, 69, 15, 34, 57, 71, 1, 27, 45, 54,
|
||||
23, 41, 63, 87, 2, 20, 46, 68, 6, 24, 50, 61, 2, 26, 61, 79, 9, 33, 59, 77, 4,
|
||||
30, 48, 74, 22, 42, 59, 76, 12, 38, 52, 68, 23, 43, 58, 77, 19, 21, 63, 64, 11,
|
||||
25, 65, 82, 17, 39, 44, 75, 9, 35, 49, 72, 19, 29, 66, 84, 13, 36, 56, 82, 17,
|
||||
43, 67, 81, 22, 40, 62, 86, 3, 21, 47, 69, 10, 24, 64, 83, 0, 37, 70, 86, 5, 31,
|
||||
49, 75, 4, 40, 53, 84, 5, 41, 52, 85, 18, 28, 67, 85, 0, 26, 44, 55, 10, 28, 54,
|
||||
72, 7, 30, 50, 81, 1, 36, 71, 87, 16, 38, 45, 74, 8, 34, 48, 73, 8, 32, 58, 76,
|
||||
12, 37, 57, 83, 6, 31, 51, 80, 15, 33, 47, 79, 16, 42, 66, 80, 7, 25, 51, 60, 3,
|
||||
27, 60, 78, 14, 32, 46, 78, 18, 20, 62, 65};
|
||||
constexpr uint8_t BEIDOU_CNAV1_H44_88_ELEMENT[176] = {
|
||||
30, 24, 1, 44, 24, 1, 44, 30, 40, 32, 61, 18, 53, 24, 1, 44, 51, 60, 35, 13, 18,
|
||||
15, 32, 61, 15, 6, 1, 45, 30, 24, 1, 44, 6, 1, 45, 15, 45, 15, 6, 1, 1, 45, 15,
|
||||
6, 1, 44, 53, 24, 24, 1, 44, 53, 44, 30, 24, 1, 34, 33, 45, 36, 55, 9, 34, 3,
|
||||
1, 44, 53, 24, 61, 47, 20, 8, 53, 24, 1, 44, 15, 6, 1, 45, 13, 18, 60, 35, 45,
|
||||
15, 6, 1, 24, 1, 44, 53, 37, 32, 52, 47, 44, 53, 24, 1, 39, 36, 34, 33, 44, 35,
|
||||
31, 50, 12, 25, 36, 14, 15, 35, 46, 56, 53, 24, 1, 44, 1, 44, 53, 24, 24, 1, 44,
|
||||
30, 44, 30, 24, 1, 15, 6, 1, 45, 30, 24, 1, 44, 2, 50, 22, 14, 33, 42, 14, 5, 34,
|
||||
3, 55, 9, 44, 35, 61, 50, 15, 6, 1, 45, 45, 15, 6, 1, 1, 44, 30, 24, 6, 1, 45,
|
||||
15, 1, 44, 53, 24};
|
||||
constexpr uint8_t BEIDOU_CNAV1_GF64_EXP[64] = {
|
||||
1, 2, 4, 8, 16, 32, 3, 6, 12, 24, 48, 35, 5, 10, 20, 40, 19, 38, 15, 30, 60, 59, 53,
|
||||
41, 17, 34, 7, 14, 28, 56, 51, 37, 9, 18, 36, 11, 22, 44, 27, 54, 47, 29, 58, 55,
|
||||
45, 25, 50, 39, 13, 26, 52, 43, 21, 42, 23, 46, 31, 62, 63, 61, 57, 49, 33, 1};
|
||||
constexpr int8_t BEIDOU_CNAV1_GF64_LOG[64] = {
|
||||
0, 0, 1, 6, 2, 12, 7, 26, 3, 32, 13, 35, 8, 48, 27, 18, 4, 24, 33, 16, 14, 52, 36,
|
||||
54, 9, 45, 49, 38, 28, 41, 19, 56, 5, 62, 25, 11, 34, 31, 17, 47, 15, 23, 53, 51,
|
||||
37, 44, 55, 40, 10, 61, 46, 30, 50, 22, 39, 43, 29, 60, 42, 21, 20, 59, 57, 58};
|
||||
|
||||
bool beidou_cnav1_ldpc_decode_200_100(const float* symbol_llr, int32_t num_bits, uint8_t* info_bits600);
|
||||
bool beidou_cnav1_ldpc_decode_88_44(const float* symbol_llr, int32_t num_bits, uint8_t* info_bits264);
|
||||
bool beidou_cnav1_ldpc_decode_200_100_codeword(const float* symbol_llr, int32_t num_bits, uint8_t* codeword_bits1200);
|
||||
bool beidou_cnav1_ldpc_decode_88_44_codeword(const float* symbol_llr, int32_t num_bits, uint8_t* codeword_bits528);
|
||||
|
||||
namespace GaloisField64
|
||||
{
|
||||
constexpr uint8_t kZero = 0U;
|
||||
constexpr uint8_t kOrder = 63U;
|
||||
constexpr uint8_t kFieldSize = 64U;
|
||||
|
||||
// LUT values come from BDS-SIS-ICD-B1C-1.0 Appendix GF(2^6) mapping rules.
|
||||
inline bool valid_symbol(uint8_t x)
|
||||
{
|
||||
return x < kFieldSize;
|
||||
}
|
||||
|
||||
inline uint8_t add(uint8_t a, uint8_t b)
|
||||
{
|
||||
if (!valid_symbol(a) || !valid_symbol(b))
|
||||
{
|
||||
return kZero;
|
||||
}
|
||||
return static_cast<uint8_t>(a ^ b);
|
||||
}
|
||||
|
||||
inline uint8_t mul(uint8_t a, uint8_t b)
|
||||
{
|
||||
if (!valid_symbol(a) || !valid_symbol(b) || a == kZero || b == kZero)
|
||||
{
|
||||
return kZero;
|
||||
}
|
||||
const int32_t log_sum = static_cast<int32_t>(BEIDOU_CNAV1_GF64_LOG[a]) +
|
||||
static_cast<int32_t>(BEIDOU_CNAV1_GF64_LOG[b]);
|
||||
return BEIDOU_CNAV1_GF64_EXP[log_sum % kOrder];
|
||||
}
|
||||
|
||||
inline uint8_t inv(uint8_t a)
|
||||
{
|
||||
if (!valid_symbol(a) || a == kZero)
|
||||
{
|
||||
return kZero;
|
||||
}
|
||||
const int32_t exponent = static_cast<int32_t>(kOrder) - static_cast<int32_t>(BEIDOU_CNAV1_GF64_LOG[a]);
|
||||
return BEIDOU_CNAV1_GF64_EXP[exponent % kOrder];
|
||||
}
|
||||
} // namespace GaloisField64
|
||||
|
||||
struct BeidouCnav1LdpcGraph
|
||||
{
|
||||
int32_t num_checks = 0;
|
||||
int32_t num_variables = 0;
|
||||
int32_t row_weight = 0;
|
||||
|
||||
// Check-node (CSR-like) adjacency: [check_offsets[i], check_offsets[i+1]).
|
||||
std::vector<uint32_t> check_offsets;
|
||||
std::vector<uint16_t> check_to_var;
|
||||
std::vector<uint8_t> check_to_h;
|
||||
|
||||
// Variable-node (CSC-like) adjacency: [var_offsets[j], var_offsets[j+1]).
|
||||
std::vector<uint32_t> var_offsets;
|
||||
std::vector<uint16_t> var_to_check;
|
||||
std::vector<uint32_t> var_to_edge;
|
||||
std::vector<uint8_t> var_to_h;
|
||||
std::vector<uint8_t> var_to_h_inv;
|
||||
};
|
||||
|
||||
bool beidou_cnav1_ldpc_init_graph(
|
||||
int32_t num_checks,
|
||||
int32_t num_variables,
|
||||
int32_t row_weight,
|
||||
const uint16_t* icd_index,
|
||||
const uint8_t* icd_element,
|
||||
int32_t num_entries,
|
||||
BeidouCnav1LdpcGraph& graph);
|
||||
|
||||
const BeidouCnav1LdpcGraph& beidou_cnav1_ldpc_graph_200_100();
|
||||
const BeidouCnav1LdpcGraph& beidou_cnav1_ldpc_graph_88_44();
|
||||
|
||||
#endif // GNSS_SDR_BEIDOU_CNAV1_LDPC_DECODER_H
|
||||
@@ -0,0 +1,884 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_navigation_message.cc
|
||||
* \brief B-CNAV1 navigation message parser (BDS-SIS-ICD-B1C-1.0 §6.2)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#include "beidou_cnav1_navigation_message.h"
|
||||
#include "Beidou_B1C.h"
|
||||
#include "Beidou_CNAV1.h"
|
||||
#include "beidou_cnav1_ldpc.h"
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
namespace
|
||||
{
|
||||
struct BchCodebook
|
||||
{
|
||||
int32_t n = 0;
|
||||
int32_t k = 0;
|
||||
int32_t corr_threshold = 0;
|
||||
std::vector<int8_t> bipolar_codewords;
|
||||
std::vector<uint16_t> messages;
|
||||
};
|
||||
|
||||
std::vector<int8_t> encode_bch_hypothesis_bipolar(
|
||||
uint16_t msg_bits,
|
||||
int32_t n,
|
||||
int32_t k,
|
||||
const std::vector<int32_t>& feedback_pos_1based)
|
||||
{
|
||||
std::vector<int8_t> register_state(static_cast<size_t>(k), 1);
|
||||
std::vector<int8_t> encoded(static_cast<size_t>(n), 1);
|
||||
|
||||
for (int32_t i = 0; i < k; i++)
|
||||
{
|
||||
const auto bit = static_cast<int32_t>((msg_bits >> static_cast<uint32_t>(k - 1 - i)) & 1U);
|
||||
register_state[static_cast<size_t>(k - 1 - i)] = (bit == 0) ? 1 : -1;
|
||||
}
|
||||
|
||||
for (int32_t ind = 0; ind < n; ind++)
|
||||
{
|
||||
encoded[static_cast<size_t>(ind)] = register_state.back();
|
||||
int8_t feedback = 1;
|
||||
for (const int32_t pos : feedback_pos_1based)
|
||||
{
|
||||
feedback = static_cast<int8_t>(feedback * register_state[static_cast<size_t>(pos - 1)]);
|
||||
}
|
||||
for (int32_t j = k - 1; j > 0; j--)
|
||||
{
|
||||
register_state[static_cast<size_t>(j)] = register_state[static_cast<size_t>(j - 1)];
|
||||
}
|
||||
register_state[0] = feedback;
|
||||
}
|
||||
|
||||
return encoded;
|
||||
}
|
||||
|
||||
BchCodebook build_bch_codebook(
|
||||
int32_t n,
|
||||
int32_t k,
|
||||
const std::vector<int32_t>& feedback_pos_1based,
|
||||
int32_t corr_threshold)
|
||||
{
|
||||
BchCodebook cb{};
|
||||
cb.n = n;
|
||||
cb.k = k;
|
||||
cb.corr_threshold = corr_threshold;
|
||||
const uint32_t num_messages = 1U << static_cast<uint32_t>(k);
|
||||
cb.bipolar_codewords.resize(static_cast<size_t>(num_messages) * static_cast<size_t>(n));
|
||||
cb.messages.resize(num_messages);
|
||||
for (uint32_t m = 0; m < num_messages; m++)
|
||||
{
|
||||
const auto encoded = encode_bch_hypothesis_bipolar(static_cast<uint16_t>(m), n, k, feedback_pos_1based);
|
||||
std::copy(encoded.begin(), encoded.end(),
|
||||
cb.bipolar_codewords.begin() + static_cast<ptrdiff_t>(m) * static_cast<ptrdiff_t>(n));
|
||||
cb.messages[m] = static_cast<uint16_t>(m);
|
||||
}
|
||||
return cb;
|
||||
}
|
||||
|
||||
bool decode_bch_codeword(const int32_t* in, int32_t* out_msg, const BchCodebook& cb)
|
||||
{
|
||||
int32_t best_corr = std::numeric_limits<int32_t>::min();
|
||||
uint16_t best_message = 0U;
|
||||
for (size_t i = 0; i < cb.messages.size(); i++)
|
||||
{
|
||||
int32_t corr = 0;
|
||||
const auto* cw = cb.bipolar_codewords.data() + static_cast<ptrdiff_t>(i) * cb.n;
|
||||
for (int32_t j = 0; j < cb.n; j++)
|
||||
{
|
||||
const int32_t bit = in[j] > 0 ? 1 : 0;
|
||||
const int32_t bipolar = (bit == 0) ? 1 : -1;
|
||||
corr += static_cast<int32_t>(cw[j]) * bipolar;
|
||||
}
|
||||
if (corr > best_corr)
|
||||
{
|
||||
best_corr = corr;
|
||||
best_message = cb.messages[i];
|
||||
}
|
||||
}
|
||||
if (best_corr < cb.corr_threshold)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
for (int32_t bit = 0; bit < cb.k; bit++)
|
||||
{
|
||||
out_msg[bit] = static_cast<int32_t>((best_message >> static_cast<uint32_t>(cb.k - 1 - bit)) & 1U);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static bool decode_bch_21_6(const int32_t* in, int32_t* out_msg)
|
||||
{
|
||||
static const std::vector<int32_t> feedback_pos = {2, 4, 5, 6};
|
||||
static const BchCodebook cb = build_bch_codebook(21, 6, feedback_pos, 20);
|
||||
return decode_bch_codeword(in, out_msg, cb);
|
||||
}
|
||||
|
||||
|
||||
static bool decode_bch_51_8(const int32_t* in, int32_t* out_msg)
|
||||
{
|
||||
static const std::vector<int32_t> feedback_pos = {1, 4, 5, 6, 7, 8};
|
||||
static const BchCodebook cb = build_bch_codebook(51, 8, feedback_pos, 50);
|
||||
return decode_bch_codeword(in, out_msg, cb);
|
||||
}
|
||||
|
||||
|
||||
uint32_t bits_to_unsigned(const int32_t* bits, int32_t length)
|
||||
{
|
||||
uint32_t value = 0;
|
||||
for (int32_t i = 0; i < length; i++)
|
||||
{
|
||||
value = (value << 1) | static_cast<uint32_t>(bits[i] & 1);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
template <typename UIntT>
|
||||
UIntT read_bits_be(const uint8_t* bits, int32_t offset, int32_t length)
|
||||
{
|
||||
UIntT value = 0;
|
||||
for (int32_t i = 0; i < length; i++)
|
||||
{
|
||||
value = static_cast<UIntT>((value << 1U) | static_cast<UIntT>(bits[offset + i] & 0x1U));
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
template <typename IntT>
|
||||
IntT sign_extend(uint64_t raw, int32_t bit_width)
|
||||
{
|
||||
const uint64_t sign_mask = 1ULL << static_cast<uint64_t>(bit_width - 1);
|
||||
if ((raw & sign_mask) == 0ULL)
|
||||
{
|
||||
return static_cast<IntT>(raw);
|
||||
}
|
||||
const uint64_t extend_mask = ~((1ULL << static_cast<uint64_t>(bit_width)) - 1ULL);
|
||||
return static_cast<IntT>(raw | extend_mask);
|
||||
}
|
||||
|
||||
double twos_pow(int32_t exponent)
|
||||
{
|
||||
return std::ldexp(1.0, exponent);
|
||||
}
|
||||
|
||||
|
||||
void deinterleave_sf2_sf3(const float* interleaved, float* sf2, float* sf3)
|
||||
{
|
||||
int32_t sf2_idx = 0;
|
||||
int32_t sf3_idx = 0;
|
||||
for (int32_t in_idx = 0; in_idx < BEIDOU_CNAV1_INTERLEAVED_SYMBOLS; in_idx++)
|
||||
{
|
||||
// Received stream is column-major readout of a 36x48 block.
|
||||
const int32_t row = in_idx % BEIDOU_CNAV1_INTERLEAVE_ROWS;
|
||||
const int32_t col = in_idx / BEIDOU_CNAV1_INTERLEAVE_ROWS;
|
||||
if (row < 33)
|
||||
{
|
||||
const int32_t group = row / 3; // 0..10
|
||||
const int32_t rem = row % 3; // 0,1 => SF2 rows ; 2 => SF3 row
|
||||
if (rem < 2)
|
||||
{
|
||||
const int32_t sf2_row = group * 2 + rem; // 0..21
|
||||
sf2[sf2_row * BEIDOU_CNAV1_INTERLEAVE_COLS + col] = interleaved[in_idx];
|
||||
sf2_idx++;
|
||||
}
|
||||
else
|
||||
{
|
||||
const int32_t sf3_row = group; // 0..10
|
||||
sf3[sf3_row * BEIDOU_CNAV1_INTERLEAVE_COLS + col] = interleaved[in_idx];
|
||||
sf3_idx++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
const int32_t sf2_row = 22 + (row - 33); // 22..24
|
||||
sf2[sf2_row * BEIDOU_CNAV1_INTERLEAVE_COLS + col] = interleaved[in_idx];
|
||||
sf2_idx++;
|
||||
}
|
||||
}
|
||||
(void)sf2_idx;
|
||||
(void)sf3_idx;
|
||||
}
|
||||
|
||||
uint64_t read_unsigned(const uint8_t* bits, int32_t offset, int32_t length)
|
||||
{
|
||||
return read_bits_be<uint64_t>(bits, offset, length);
|
||||
}
|
||||
|
||||
int64_t read_signed(const uint8_t* bits, int32_t offset, int32_t length)
|
||||
{
|
||||
const uint64_t raw = read_unsigned(bits, offset, length);
|
||||
return sign_extend<int64_t>(raw, length);
|
||||
}
|
||||
uint32_t crc24q(const uint8_t* bits, int32_t num_bits)
|
||||
{
|
||||
// 寄存器初始值设为全 0
|
||||
uint32_t crc = 0;
|
||||
|
||||
// 生成多项式去除了最高位 x^24 后的低 24 位掩码 (0x1864CFB & 0xFFFFFF)
|
||||
const uint32_t POLY = 0x864CFBU;
|
||||
|
||||
for (int32_t i = 0; i < num_bits; i++)
|
||||
{
|
||||
// 提取当前最高位(第24位,索引为23)
|
||||
uint32_t msb = (crc >> 23) & 1U;
|
||||
uint32_t bit = (bits[i] != 0U) ? 1U : 0U;
|
||||
|
||||
// 寄存器左移,保持 24 位位宽
|
||||
crc = (crc << 1) & 0xFFFFFFU;
|
||||
|
||||
// MSB 反馈与输入信息比特异或,决定是否执行多项式除法(异或生成多项式)
|
||||
if (msb ^ bit)
|
||||
{
|
||||
crc ^= POLY;
|
||||
}
|
||||
}
|
||||
|
||||
// 依次输出构成 CRC 校验序列
|
||||
return crc;
|
||||
}
|
||||
|
||||
bool verify_crc24q(const uint8_t* bits, int32_t data_bits)
|
||||
{
|
||||
const uint32_t computed = crc24q(bits, data_bits);
|
||||
const auto received = static_cast<uint32_t>(read_unsigned(bits, data_bits, BEIDOU_CNAV1_CRC_BITS));
|
||||
return computed == received;
|
||||
}
|
||||
|
||||
double a_ref_from_sat_type(uint32_t sat_type)
|
||||
{
|
||||
if (sat_type == 0b01U || sat_type == 0b10U)
|
||||
{
|
||||
return BEIDOU_CNAV1_A_REF_IGSO;
|
||||
}
|
||||
return BEIDOU_CNAV1_A_REF_MEO;
|
||||
}
|
||||
|
||||
int32_t nav_type_from_sat_type(uint32_t sat_type)
|
||||
{
|
||||
if (sat_type == 0b01U)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
void parse_subframe2(const uint8_t* bits, Beidou_Cnav1_Ephemeris& eph, double soh_seconds)
|
||||
{
|
||||
int32_t offset = 0;
|
||||
eph.WN = static_cast<int32_t>(read_unsigned(bits, offset, 13));
|
||||
offset += 13;
|
||||
const auto how = static_cast<int32_t>(read_unsigned(bits, offset, 8));
|
||||
offset += 8;
|
||||
eph.IODC = static_cast<double>(read_unsigned(bits, offset, 10));
|
||||
offset += 10;
|
||||
eph.IODE = static_cast<double>(read_unsigned(bits, offset, 8));
|
||||
offset += 8;
|
||||
|
||||
const auto toe_raw = static_cast<int32_t>(read_unsigned(bits, offset, 11));
|
||||
offset += 11;
|
||||
const auto sat_type = static_cast<uint32_t>(read_unsigned(bits, offset, 2));
|
||||
offset += 2;
|
||||
const auto delta_a = static_cast<double>(read_signed(bits, offset, 26)) * BEIDOU_CNAV1_DELTA_A_LSB;
|
||||
offset += 26;
|
||||
const auto a_dot = static_cast<double>(read_signed(bits, offset, 25)) * BEIDOU_CNAV1_A_DOT_LSB;
|
||||
offset += 25;
|
||||
eph.delta_n = static_cast<double>(read_signed(bits, offset, 17)) * BEIDOU_CNAV1_DELTA_N0_LSB;
|
||||
offset += 17;
|
||||
const auto delta_n_dot = static_cast<double>(read_signed(bits, offset, 23)) * BEIDOU_CNAV1_DELTA_N0_DOT_LSB;
|
||||
offset += 23;
|
||||
eph.M_0 = static_cast<double>(read_signed(bits, offset, 33)) * BEIDOU_CNAV1_M0_LSB;
|
||||
offset += 33;
|
||||
eph.ecc = static_cast<double>(read_unsigned(bits, offset, 33)) * BEIDOU_CNAV1_E_LSB;
|
||||
offset += 33;
|
||||
eph.omega = static_cast<double>(read_signed(bits, offset, 33)) * BEIDOU_CNAV1_OMEGA_LSB;
|
||||
offset += 33;
|
||||
|
||||
eph.OMEGA_0 = static_cast<double>(read_signed(bits, offset, 33)) * BEIDOU_CNAV1_OMEGA_LSB;
|
||||
offset += 33;
|
||||
eph.i_0 = static_cast<double>(read_signed(bits, offset, 33)) * BEIDOU_CNAV1_I0_LSB;
|
||||
offset += 33;
|
||||
eph.OMEGAdot = static_cast<double>(read_signed(bits, offset, 19)) * BEIDOU_CNAV1_OMEGADOT_LSB;
|
||||
offset += 19;
|
||||
eph.idot = static_cast<double>(read_signed(bits, offset, 15)) * BEIDOU_CNAV1_IDOT_LSB;
|
||||
offset += 15;
|
||||
eph.Cis = static_cast<double>(read_signed(bits, offset, 16)) * BEIDOU_CNAV1_CIS_LSB;
|
||||
offset += 16;
|
||||
eph.Cic = static_cast<double>(read_signed(bits, offset, 16)) * BEIDOU_CNAV1_CIC_LSB;
|
||||
offset += 16;
|
||||
eph.Crs = static_cast<double>(read_signed(bits, offset, 24)) * BEIDOU_CNAV1_CRS_LSB;
|
||||
offset += 24;
|
||||
eph.Crc = static_cast<double>(read_signed(bits, offset, 24)) * BEIDOU_CNAV1_CRC_LSB;
|
||||
offset += 24;
|
||||
eph.Cus = static_cast<double>(read_signed(bits, offset, 21)) * BEIDOU_CNAV1_CUS_LSB;
|
||||
offset += 21;
|
||||
eph.Cuc = static_cast<double>(read_signed(bits, offset, 21)) * BEIDOU_CNAV1_CUC_LSB;
|
||||
offset += 21;
|
||||
|
||||
eph.toc = static_cast<int32_t>(read_unsigned(bits, offset, 11));
|
||||
offset += 11;
|
||||
eph.af0 = static_cast<double>(read_signed(bits, offset, 25)) * BEIDOU_CNAV1_AF0_LSB;
|
||||
offset += 25;
|
||||
eph.af1 = static_cast<double>(read_signed(bits, offset, 22)) * BEIDOU_CNAV1_AF1_LSB;
|
||||
offset += 22;
|
||||
eph.af2 = static_cast<double>(read_signed(bits, offset, 11)) * BEIDOU_CNAV1_AF2_LSB;
|
||||
offset += 11;
|
||||
eph.TGD_B2ap = static_cast<double>(read_signed(bits, offset, 12)) * BEIDOU_CNAV1_TGD_LSB;
|
||||
offset += 12;
|
||||
eph.ISC_B1Cd = static_cast<double>(read_signed(bits, offset, 12)) * BEIDOU_CNAV1_ISC_LSB;
|
||||
offset += 12;
|
||||
eph.TGD_B1Cp = static_cast<double>(read_signed(bits, offset, 12)) * BEIDOU_CNAV1_TGD_LSB;
|
||||
offset += 12;
|
||||
(void)a_dot;
|
||||
(void)delta_n_dot;
|
||||
|
||||
const double a_ref = a_ref_from_sat_type(sat_type);
|
||||
eph.sqrtA = std::sqrt(a_ref + delta_a);
|
||||
eph.toe = static_cast<int32_t>(toe_raw * BEIDOU_CNAV1_TOE_TOC_LSB);
|
||||
eph.toc = static_cast<int32_t>(eph.toc * BEIDOU_CNAV1_TOE_TOC_LSB);
|
||||
eph.tow = static_cast<int32_t>(how * 3600 + soh_seconds);
|
||||
eph.nav_type = nav_type_from_sat_type(sat_type);
|
||||
eph.sig_type = 7;
|
||||
}
|
||||
|
||||
void parse_page_common(
|
||||
const uint8_t* bits,
|
||||
bool has_sisaioe,
|
||||
bool has_sisaioc,
|
||||
Bds3_B1c_PageData& page_data,
|
||||
int32_t& offset)
|
||||
{
|
||||
offset = 0;
|
||||
page_data.common.page_id = static_cast<int32_t>(read_unsigned(bits, offset, 6));
|
||||
offset += 6;
|
||||
page_data.common.hs = static_cast<int32_t>(read_unsigned(bits, offset, 2));
|
||||
offset += 2;
|
||||
page_data.common.dif = read_unsigned(bits, offset, 1) != 0U;
|
||||
offset += 1;
|
||||
page_data.common.sif = read_unsigned(bits, offset, 1) != 0U;
|
||||
offset += 1;
|
||||
page_data.common.aif = read_unsigned(bits, offset, 1) != 0U;
|
||||
offset += 1;
|
||||
page_data.common.sismai = static_cast<int32_t>(read_unsigned(bits, offset, 4));
|
||||
offset += 4;
|
||||
page_data.sisaioe = 0;
|
||||
page_data.sisaioc = 0;
|
||||
if (has_sisaioe)
|
||||
{
|
||||
page_data.sisaioe = static_cast<int32_t>(read_unsigned(bits, offset, 5));
|
||||
offset += 5;
|
||||
}
|
||||
if (has_sisaioc)
|
||||
{
|
||||
page_data.sisaioc = static_cast<int32_t>(read_unsigned(bits, offset, 22));
|
||||
offset += 22;
|
||||
}
|
||||
}
|
||||
|
||||
void parse_reduced_almanac(const uint8_t* bits, int32_t& offset, Bds3_B1c_AlmanacReduced& almanac)
|
||||
{
|
||||
almanac.prn = static_cast<int32_t>(read_unsigned(bits, offset, 6));
|
||||
offset += 6;
|
||||
almanac.sat_type = static_cast<int32_t>(read_unsigned(bits, offset, 2));
|
||||
offset += 2;
|
||||
almanac.delta_a_m = static_cast<double>(read_signed(bits, offset, 8)) * twos_pow(9);
|
||||
offset += 8;
|
||||
almanac.omega0_rad = static_cast<double>(read_signed(bits, offset, 7)) * twos_pow(-6) * M_PI;
|
||||
offset += 7;
|
||||
almanac.phi0_rad = static_cast<double>(read_signed(bits, offset, 7)) * twos_pow(-6) * M_PI;
|
||||
offset += 7;
|
||||
almanac.health = static_cast<int32_t>(read_unsigned(bits, offset, 8));
|
||||
offset += 8;
|
||||
}
|
||||
|
||||
void parse_medium_almanac(const uint8_t* bits, int32_t& offset, Bds3_B1c_AlmanacMedium& almanac)
|
||||
{
|
||||
almanac.prn = static_cast<int32_t>(read_unsigned(bits, offset, 6));
|
||||
offset += 6;
|
||||
almanac.sat_type = static_cast<int32_t>(read_unsigned(bits, offset, 2));
|
||||
offset += 2;
|
||||
almanac.wna = static_cast<int32_t>(read_unsigned(bits, offset, 13));
|
||||
offset += 13;
|
||||
almanac.toa_s = static_cast<int32_t>(read_unsigned(bits, offset, 8)) * 4096;
|
||||
offset += 8;
|
||||
almanac.eccentricity = static_cast<double>(read_unsigned(bits, offset, 11)) * twos_pow(-16);
|
||||
offset += 11;
|
||||
almanac.delta_i_rad = static_cast<double>(read_signed(bits, offset, 11)) * twos_pow(-14) * M_PI;
|
||||
offset += 11;
|
||||
almanac.sqrt_a_m_sqrt = static_cast<double>(read_unsigned(bits, offset, 17)) * twos_pow(-4);
|
||||
offset += 17;
|
||||
almanac.omega0_rad = static_cast<double>(read_signed(bits, offset, 16)) * twos_pow(-15) * M_PI;
|
||||
offset += 16;
|
||||
almanac.omega_dot_rad_s = static_cast<double>(read_signed(bits, offset, 11)) * twos_pow(-33) * M_PI;
|
||||
offset += 11;
|
||||
almanac.omega_rad = static_cast<double>(read_signed(bits, offset, 16)) * twos_pow(-15) * M_PI;
|
||||
offset += 16;
|
||||
almanac.m0_rad = static_cast<double>(read_signed(bits, offset, 16)) * twos_pow(-15) * M_PI;
|
||||
offset += 16;
|
||||
almanac.af0_s = static_cast<double>(read_signed(bits, offset, 11)) * twos_pow(-20);
|
||||
offset += 11;
|
||||
almanac.af1_s_s = static_cast<double>(read_signed(bits, offset, 10)) * twos_pow(-37);
|
||||
offset += 10;
|
||||
almanac.health = static_cast<int32_t>(read_unsigned(bits, offset, 8));
|
||||
offset += 8;
|
||||
}
|
||||
|
||||
void parse_eop(const uint8_t* bits, int32_t& offset, Bds3_B1c_Eop& eop)
|
||||
{
|
||||
eop.t_eop_s = static_cast<int32_t>(read_unsigned(bits, offset, 16)) * 16;
|
||||
offset += 16;
|
||||
eop.pm_x_arcsec = static_cast<double>(read_signed(bits, offset, 21)) * twos_pow(-20);
|
||||
offset += 21;
|
||||
eop.pm_x_dot_arcsec_day = static_cast<double>(read_signed(bits, offset, 15)) * twos_pow(-21);
|
||||
offset += 15;
|
||||
eop.pm_y_arcsec = static_cast<double>(read_signed(bits, offset, 21)) * twos_pow(-20);
|
||||
offset += 21;
|
||||
eop.pm_y_dot_arcsec_day = static_cast<double>(read_signed(bits, offset, 15)) * twos_pow(-21);
|
||||
offset += 15;
|
||||
eop.delta_ut1_s = static_cast<double>(read_signed(bits, offset, 31)) * twos_pow(-24);
|
||||
offset += 31;
|
||||
eop.delta_ut1_dot_s_day = static_cast<double>(read_signed(bits, offset, 19)) * twos_pow(-25);
|
||||
offset += 19;
|
||||
}
|
||||
|
||||
void parse_bgto(const uint8_t* bits, int32_t& offset, Bds3_B1c_Bgto& bgto)
|
||||
{
|
||||
bgto.gnss_id = static_cast<int32_t>(read_unsigned(bits, offset, 3));
|
||||
offset += 3;
|
||||
bgto.wn0_bgto = static_cast<int32_t>(read_unsigned(bits, offset, 13));
|
||||
offset += 13;
|
||||
bgto.t0_bgto_s = static_cast<int32_t>(read_unsigned(bits, offset, 16)) * 16;
|
||||
offset += 16;
|
||||
bgto.a0_bgto_s = static_cast<double>(read_signed(bits, offset, 16)) * twos_pow(-35);
|
||||
offset += 16;
|
||||
bgto.a1_bgto_s_s = static_cast<double>(read_signed(bits, offset, 13)) * twos_pow(-51);
|
||||
offset += 13;
|
||||
bgto.a2_bgto_s_s2 = static_cast<double>(read_signed(bits, offset, 7)) * twos_pow(-68);
|
||||
offset += 7;
|
||||
}
|
||||
|
||||
void parse_page1(
|
||||
const uint8_t* bits,
|
||||
Beidou_Cnav1_Iono& iono,
|
||||
Beidou_Cnav1_Utc_Model& utc,
|
||||
Bds3_B1c_PageData& page_data)
|
||||
{
|
||||
int32_t offset = 0;
|
||||
parse_page_common(bits, true, true, page_data, offset);
|
||||
iono.alpha1 = static_cast<double>(read_signed(bits, offset, 10)) * 0.125;
|
||||
offset += 10;
|
||||
iono.alpha2 = static_cast<double>(read_signed(bits, offset, 8)) * 0.125;
|
||||
offset += 8;
|
||||
iono.alpha3 = static_cast<double>(read_signed(bits, offset, 8)) * 0.125;
|
||||
offset += 8;
|
||||
iono.alpha4 = static_cast<double>(read_signed(bits, offset, 8)) * 0.125;
|
||||
offset += 8;
|
||||
iono.alpha5 = static_cast<double>(read_signed(bits, offset, 8)) * 0.125;
|
||||
offset += 8;
|
||||
iono.alpha6 = static_cast<double>(read_signed(bits, offset, 8)) * 0.125;
|
||||
offset += 8;
|
||||
iono.alpha7 = static_cast<double>(read_signed(bits, offset, 8)) * 0.125;
|
||||
offset += 8;
|
||||
iono.alpha8 = static_cast<double>(read_signed(bits, offset, 8)) * 0.125;
|
||||
offset += 8;
|
||||
iono.alpha9 = static_cast<double>(read_signed(bits, offset, 8)) * 0.125;
|
||||
offset += 8;
|
||||
|
||||
utc.A0 = static_cast<double>(read_signed(bits, offset, 16)) * GNSS_SDR_TWO_N34;
|
||||
offset += 16;
|
||||
utc.A1 = static_cast<double>(read_signed(bits, offset, 13)) * GNSS_SDR_TWO_N50;
|
||||
offset += 13;
|
||||
utc.A2 = static_cast<double>(read_signed(bits, offset, 7)) * GNSS_SDR_TWO_N66;
|
||||
offset += 7;
|
||||
utc.delta_t_LSF = static_cast<int32_t>(read_signed(bits, offset, 8));
|
||||
offset += 8;
|
||||
utc.tot = static_cast<int32_t>(read_unsigned(bits, offset, 16));
|
||||
offset += 16;
|
||||
utc.WN_t = static_cast<int32_t>(read_unsigned(bits, offset, 13));
|
||||
offset += 13;
|
||||
utc.WN_LSF = static_cast<int32_t>(read_unsigned(bits, offset, 13));
|
||||
offset += 13;
|
||||
utc.DN = static_cast<int32_t>(read_unsigned(bits, offset, 3));
|
||||
offset += 3;
|
||||
(void)read_signed(bits, offset, 8);
|
||||
}
|
||||
|
||||
void parse_page2(const uint8_t* bits, Bds3_B1c_PageData& page_data)
|
||||
{
|
||||
int32_t offset = 0;
|
||||
parse_page_common(bits, false, true, page_data, offset);
|
||||
page_data.wna = static_cast<int32_t>(read_unsigned(bits, offset, 13));
|
||||
offset += 13;
|
||||
page_data.toa_s = static_cast<int32_t>(read_unsigned(bits, offset, 8)) * 4096;
|
||||
offset += 8;
|
||||
for (auto& alm : page_data.reduced_almanac)
|
||||
{
|
||||
parse_reduced_almanac(bits, offset, alm);
|
||||
}
|
||||
}
|
||||
|
||||
void parse_page3(const uint8_t* bits, Bds3_B1c_PageData& page_data)
|
||||
{
|
||||
int32_t offset = 0;
|
||||
parse_page_common(bits, true, false, page_data, offset);
|
||||
parse_eop(bits, offset, page_data.eop);
|
||||
parse_bgto(bits, offset, page_data.bgto);
|
||||
// Consume trailing reserved block and CRC for layout completeness.
|
||||
(void)read_unsigned(bits, offset, 14);
|
||||
offset += 14;
|
||||
(void)read_unsigned(bits, offset, 24);
|
||||
}
|
||||
|
||||
void parse_page4(const uint8_t* bits, Bds3_B1c_PageData& page_data)
|
||||
{
|
||||
int32_t offset = 0;
|
||||
parse_page_common(bits, false, true, page_data, offset);
|
||||
parse_medium_almanac(bits, offset, page_data.medium_almanac);
|
||||
// Consume trailing reserved block and CRC for layout completeness.
|
||||
(void)read_unsigned(bits, offset, 47);
|
||||
offset += 47;
|
||||
(void)read_unsigned(bits, offset, 24);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
|
||||
bool Beidou_Cnav1_Navigation_Message::decode_frame(
|
||||
const float* symbols,
|
||||
int32_t num_symbols,
|
||||
int32_t expected_prn,
|
||||
int32_t* fail_stage)
|
||||
{
|
||||
auto set_fail = [&](int32_t stage) {
|
||||
if (fail_stage != nullptr)
|
||||
{
|
||||
*fail_stage = stage;
|
||||
}
|
||||
};
|
||||
|
||||
if (fail_stage != nullptr)
|
||||
{
|
||||
*fail_stage = -1;
|
||||
}
|
||||
if (num_symbols < BEIDOU_CNAV1_FRAME_SYMBOLS)
|
||||
{
|
||||
set_fail(0);
|
||||
return false;
|
||||
}
|
||||
|
||||
std::array<float, BEIDOU_CNAV1_FRAME_SYMBOLS> bit_llr{};
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_FRAME_SYMBOLS; i++)
|
||||
{
|
||||
bit_llr[i] = symbols[i];
|
||||
}
|
||||
|
||||
std::array<int32_t, BEIDOU_CNAV1_SUBFRAME1_SYMBOLS> hard_bits{};
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_SUBFRAME1_SYMBOLS; i++)
|
||||
{
|
||||
hard_bits[i] = (symbols[i] >= 0.0F) ? 1 : 0;
|
||||
}
|
||||
|
||||
int32_t prn_bits[6];
|
||||
int32_t soh_bits[8];
|
||||
if (!decode_bch_21_6(hard_bits.data(), prn_bits))
|
||||
{
|
||||
set_fail(1);
|
||||
return false;
|
||||
}
|
||||
if (!decode_bch_51_8(hard_bits.data() + 21, soh_bits))
|
||||
{
|
||||
set_fail(2);
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint32_t prn = bits_to_unsigned(prn_bits, 6);
|
||||
if (prn < 1U || prn > static_cast<uint32_t>(BEIDOU_B1C_NUMBER_OF_PRNS))
|
||||
{
|
||||
set_fail(3);
|
||||
return false;
|
||||
}
|
||||
if (expected_prn > 0 && static_cast<uint32_t>(expected_prn) != prn)
|
||||
{
|
||||
set_fail(4);
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint32_t soh = bits_to_unsigned(soh_bits, 8);
|
||||
const auto soh_seconds = static_cast<double>(soh * BEIDOU_CNAV1_SOH_LSB_S);
|
||||
ephemeris_.PRN = static_cast<int32_t>(prn);
|
||||
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> sf2_llr{};
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> sf3_llr{};
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> sf2_llr_inv{};
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> sf2_llr_bitrev{};
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> sf2_llr_bitrev_inv{};
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> sf3_llr_inv{};
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> sf3_llr_bitrev{};
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> sf3_llr_bitrev_inv{};
|
||||
deinterleave_sf2_sf3(bit_llr.data() + BEIDOU_CNAV1_SUBFRAME1_SYMBOLS, sf2_llr.data(), sf3_llr.data());
|
||||
// LDPC works on GF(64): 6 bit-LLRs per symbol. Build four polarity/order variants
|
||||
// (normal, inverted, per-symbol bit-reversed, both) and try each below.
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_SUBFRAME2_SYMBOLS; i++)
|
||||
{
|
||||
sf2_llr_inv[static_cast<size_t>(i)] = -sf2_llr[static_cast<size_t>(i)];
|
||||
}
|
||||
// Reverse bit order within each 6-LLR GF(64) symbol; inv applies the same polarity flip.
|
||||
for (int32_t symbol = 0; symbol < BEIDOU_CNAV1_SUBFRAME2_SYMBOLS / 6; symbol++)
|
||||
{
|
||||
const int32_t base = symbol * 6;
|
||||
for (int32_t bit = 0; bit < 6; bit++)
|
||||
{
|
||||
sf2_llr_bitrev[static_cast<size_t>(base + bit)] =
|
||||
sf2_llr[static_cast<size_t>(base + (5 - bit))];
|
||||
sf2_llr_bitrev_inv[static_cast<size_t>(base + bit)] =
|
||||
-sf2_llr_bitrev[static_cast<size_t>(base + bit)];
|
||||
}
|
||||
}
|
||||
// SF3: same four LLR variants as SF2.
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_SUBFRAME3_SYMBOLS; i++)
|
||||
{
|
||||
sf3_llr_inv[static_cast<size_t>(i)] = -sf3_llr[static_cast<size_t>(i)];
|
||||
}
|
||||
for (int32_t symbol = 0; symbol < BEIDOU_CNAV1_SUBFRAME3_SYMBOLS / 6; symbol++)
|
||||
{
|
||||
const int32_t base = symbol * 6;
|
||||
for (int32_t bit = 0; bit < 6; bit++)
|
||||
{
|
||||
sf3_llr_bitrev[static_cast<size_t>(base + bit)] =
|
||||
sf3_llr[static_cast<size_t>(base + (5 - bit))];
|
||||
sf3_llr_bitrev_inv[static_cast<size_t>(base + bit)] =
|
||||
-sf3_llr_bitrev[static_cast<size_t>(base + bit)];
|
||||
}
|
||||
}
|
||||
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SF2_DATA_BITS> sf2_data{};
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SF3_DATA_BITS> sf3_data{};
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> sf2_codeword_bits{};
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> sf3_codeword_bits{};
|
||||
bool sf2_ldpc_ok = beidou_cnav1_ldpc_decode_200_100_codeword(
|
||||
sf2_llr.data(), BEIDOU_CNAV1_SUBFRAME2_SYMBOLS, sf2_codeword_bits.data());
|
||||
if (!sf2_ldpc_ok)
|
||||
{
|
||||
sf2_ldpc_ok = beidou_cnav1_ldpc_decode_200_100_codeword(
|
||||
sf2_llr_inv.data(), BEIDOU_CNAV1_SUBFRAME2_SYMBOLS, sf2_codeword_bits.data());
|
||||
}
|
||||
if (!sf2_ldpc_ok)
|
||||
{
|
||||
sf2_ldpc_ok = beidou_cnav1_ldpc_decode_200_100_codeword(
|
||||
sf2_llr_bitrev.data(), BEIDOU_CNAV1_SUBFRAME2_SYMBOLS, sf2_codeword_bits.data());
|
||||
}
|
||||
if (!sf2_ldpc_ok)
|
||||
{
|
||||
sf2_ldpc_ok = beidou_cnav1_ldpc_decode_200_100_codeword(
|
||||
sf2_llr_bitrev_inv.data(), BEIDOU_CNAV1_SUBFRAME2_SYMBOLS, sf2_codeword_bits.data());
|
||||
}
|
||||
if (!sf2_ldpc_ok)
|
||||
{
|
||||
// SF2 LDPC fallback: after normal/inverted/bit-reversed attempts all fail,
|
||||
// hard-slice the deinterleaved LLRs (sign of sf2_llr) as information bits.
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_SF2_DATA_BITS; i++)
|
||||
{
|
||||
sf2_data[static_cast<size_t>(i)] = (sf2_llr[static_cast<size_t>(i)] >= 0.0F) ? 1U : 0U;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// Some matrix descriptions/orderings are not guaranteed to keep information symbols
|
||||
// in the first K symbols. Prefer the half that satisfies CRC if available.
|
||||
std::copy_n(sf2_codeword_bits.begin(), BEIDOU_CNAV1_SF2_DATA_BITS, sf2_data.begin());
|
||||
if (!verify_crc24q(sf2_data.data(), BEIDOU_CNAV1_SF2_DATA_BITS - BEIDOU_CNAV1_CRC_BITS))
|
||||
{
|
||||
std::copy_n(
|
||||
sf2_codeword_bits.begin() + BEIDOU_CNAV1_SF2_DATA_BITS,
|
||||
BEIDOU_CNAV1_SF2_DATA_BITS,
|
||||
sf2_data.begin());
|
||||
}
|
||||
}
|
||||
bool sf3_ldpc_ok = beidou_cnav1_ldpc_decode_88_44_codeword(
|
||||
sf3_llr.data(), BEIDOU_CNAV1_SUBFRAME3_SYMBOLS, sf3_codeword_bits.data());
|
||||
if (!sf3_ldpc_ok)
|
||||
{
|
||||
sf3_ldpc_ok = beidou_cnav1_ldpc_decode_88_44_codeword(
|
||||
sf3_llr_inv.data(), BEIDOU_CNAV1_SUBFRAME3_SYMBOLS, sf3_codeword_bits.data());
|
||||
}
|
||||
if (!sf3_ldpc_ok)
|
||||
{
|
||||
sf3_ldpc_ok = beidou_cnav1_ldpc_decode_88_44_codeword(
|
||||
sf3_llr_bitrev.data(), BEIDOU_CNAV1_SUBFRAME3_SYMBOLS, sf3_codeword_bits.data());
|
||||
}
|
||||
if (!sf3_ldpc_ok)
|
||||
{
|
||||
sf3_ldpc_ok = beidou_cnav1_ldpc_decode_88_44_codeword(
|
||||
sf3_llr_bitrev_inv.data(), BEIDOU_CNAV1_SUBFRAME3_SYMBOLS, sf3_codeword_bits.data());
|
||||
}
|
||||
if (!sf3_ldpc_ok)
|
||||
{
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_SF3_DATA_BITS; i++)
|
||||
{
|
||||
sf3_data[static_cast<size_t>(i)] = (sf3_llr[static_cast<size_t>(i)] >= 0.0F) ? 1U : 0U;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
std::copy_n(sf3_codeword_bits.begin(), BEIDOU_CNAV1_SF3_DATA_BITS, sf3_data.begin());
|
||||
if (!verify_crc24q(sf3_data.data(), BEIDOU_CNAV1_SF3_DATA_BITS - BEIDOU_CNAV1_CRC_BITS))
|
||||
{
|
||||
std::copy_n(
|
||||
sf3_codeword_bits.begin() + BEIDOU_CNAV1_SF3_DATA_BITS,
|
||||
BEIDOU_CNAV1_SF3_DATA_BITS,
|
||||
sf3_data.begin());
|
||||
}
|
||||
}
|
||||
|
||||
const bool sf2_crc_ok = verify_crc24q(sf2_data.data(), BEIDOU_CNAV1_SF2_DATA_BITS - BEIDOU_CNAV1_CRC_BITS);
|
||||
const bool sf3_crc_ok = verify_crc24q(sf3_data.data(), BEIDOU_CNAV1_SF3_DATA_BITS - BEIDOU_CNAV1_CRC_BITS);
|
||||
|
||||
if (!sf2_crc_ok)
|
||||
{
|
||||
set_fail(sf2_ldpc_ok ? 6 : 5);
|
||||
return false;
|
||||
}
|
||||
|
||||
parse_subframe2(sf2_data.data(), ephemeris_, soh_seconds);
|
||||
if (ephemeris_.sqrtA < 5000.0 || ephemeris_.sqrtA > 6700.0)
|
||||
{
|
||||
set_fail(7);
|
||||
return false;
|
||||
}
|
||||
|
||||
ephemeris_.PRN = static_cast<int32_t>(prn);
|
||||
tow_s_ = static_cast<double>(ephemeris_.tow);
|
||||
flag_new_ephemeris_ = true;
|
||||
|
||||
if (sf3_crc_ok)
|
||||
{
|
||||
const auto page_id = static_cast<int32_t>(read_unsigned(sf3_data.data(), 0, 6));
|
||||
switch (page_id)
|
||||
{
|
||||
case 1:
|
||||
parse_page1(sf3_data.data(), iono_, utc_model_, page_data_);
|
||||
flag_new_iono_ = true;
|
||||
flag_new_utc_ = true;
|
||||
flag_new_page_data_ = true;
|
||||
break;
|
||||
case 2:
|
||||
parse_page2(sf3_data.data(), page_data_);
|
||||
flag_new_page_data_ = true;
|
||||
break;
|
||||
case 3:
|
||||
parse_page3(sf3_data.data(), page_data_);
|
||||
flag_new_page_data_ = true;
|
||||
break;
|
||||
case 4:
|
||||
parse_page4(sf3_data.data(), page_data_);
|
||||
flag_new_page_data_ = true;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool Beidou_Cnav1_Navigation_Message::decode_frame_symbols(
|
||||
const float* symbols,
|
||||
int32_t num_symbols,
|
||||
int32_t expected_prn,
|
||||
int32_t* fail_stage)
|
||||
{
|
||||
return decode_frame(symbols, num_symbols, expected_prn, fail_stage);
|
||||
}
|
||||
|
||||
|
||||
bool Beidou_Cnav1_Navigation_Message::probe_subframe1_prn(const float* symbols, int32_t num_symbols, int32_t expected_prn) const
|
||||
{
|
||||
if (num_symbols < BEIDOU_CNAV1_SUBFRAME1_SYMBOLS)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
std::array<int32_t, BEIDOU_CNAV1_SUBFRAME1_SYMBOLS> hard_bits{};
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_SUBFRAME1_SYMBOLS; i++)
|
||||
{
|
||||
hard_bits[i] = (symbols[i] >= 0.0F) ? 1 : 0;
|
||||
}
|
||||
|
||||
int32_t prn_bits[6];
|
||||
int32_t soh_bits[8];
|
||||
if (!decode_bch_21_6(hard_bits.data(), prn_bits))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
if (!decode_bch_51_8(hard_bits.data() + 21, soh_bits))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const uint32_t prn = bits_to_unsigned(prn_bits, 6);
|
||||
if (prn < 1U || prn > static_cast<uint32_t>(BEIDOU_B1C_NUMBER_OF_PRNS))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return expected_prn <= 0 || static_cast<uint32_t>(expected_prn) == prn;
|
||||
}
|
||||
|
||||
|
||||
bool Beidou_Cnav1_Navigation_Message::have_new_ephemeris() const
|
||||
{
|
||||
return flag_new_ephemeris_;
|
||||
}
|
||||
|
||||
bool Beidou_Cnav1_Navigation_Message::have_new_iono() const
|
||||
{
|
||||
return flag_new_iono_;
|
||||
}
|
||||
|
||||
bool Beidou_Cnav1_Navigation_Message::have_new_utc_model() const
|
||||
{
|
||||
return flag_new_utc_;
|
||||
}
|
||||
|
||||
bool Beidou_Cnav1_Navigation_Message::have_new_page_data() const
|
||||
{
|
||||
return flag_new_page_data_;
|
||||
}
|
||||
|
||||
void Beidou_Cnav1_Navigation_Message::clear_flags()
|
||||
{
|
||||
flag_new_ephemeris_ = false;
|
||||
flag_new_iono_ = false;
|
||||
flag_new_utc_ = false;
|
||||
flag_new_page_data_ = false;
|
||||
}
|
||||
|
||||
const Beidou_Cnav1_Ephemeris& Beidou_Cnav1_Navigation_Message::get_ephemeris() const
|
||||
{
|
||||
return ephemeris_;
|
||||
}
|
||||
|
||||
const Beidou_Cnav1_Iono& Beidou_Cnav1_Navigation_Message::get_iono() const
|
||||
{
|
||||
return iono_;
|
||||
}
|
||||
|
||||
const Beidou_Cnav1_Utc_Model& Beidou_Cnav1_Navigation_Message::get_utc_model() const
|
||||
{
|
||||
return utc_model_;
|
||||
}
|
||||
|
||||
const Bds3_B1c_PageData& Beidou_Cnav1_Navigation_Message::get_page_data() const
|
||||
{
|
||||
return page_data_;
|
||||
}
|
||||
|
||||
double Beidou_Cnav1_Navigation_Message::get_tow_s() const
|
||||
{
|
||||
return tow_s_;
|
||||
}
|
||||
@@ -0,0 +1,122 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_navigation_message.h
|
||||
* \brief B-CNAV1 navigation message parser (§6.2)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_CNAV1_NAVIGATION_MESSAGE_H
|
||||
#define GNSS_SDR_BEIDOU_CNAV1_NAVIGATION_MESSAGE_H
|
||||
|
||||
#include "beidou_cnav1_ephemeris.h"
|
||||
#include "beidou_cnav1_iono.h"
|
||||
#include "beidou_cnav1_utc_model.h"
|
||||
#include <array>
|
||||
|
||||
struct Bds3_B1c_PageCommon
|
||||
{
|
||||
int32_t page_id{};
|
||||
int32_t sismai{};
|
||||
int32_t hs{};
|
||||
bool dif{};
|
||||
bool sif{};
|
||||
bool aif{};
|
||||
};
|
||||
|
||||
struct Bds3_B1c_AlmanacReduced
|
||||
{
|
||||
int32_t prn{};
|
||||
int32_t sat_type{};
|
||||
double delta_a_m{};
|
||||
double omega0_rad{};
|
||||
double phi0_rad{};
|
||||
int32_t health{};
|
||||
};
|
||||
|
||||
struct Bds3_B1c_AlmanacMedium
|
||||
{
|
||||
int32_t prn{};
|
||||
int32_t sat_type{};
|
||||
int32_t wna{};
|
||||
int32_t toa_s{};
|
||||
double eccentricity{};
|
||||
double delta_i_rad{};
|
||||
double sqrt_a_m_sqrt{};
|
||||
double omega0_rad{};
|
||||
double omega_dot_rad_s{};
|
||||
double omega_rad{};
|
||||
double m0_rad{};
|
||||
double af0_s{};
|
||||
double af1_s_s{};
|
||||
int32_t health{};
|
||||
};
|
||||
|
||||
struct Bds3_B1c_Eop
|
||||
{
|
||||
int32_t t_eop_s{};
|
||||
double pm_x_arcsec{};
|
||||
double pm_x_dot_arcsec_day{};
|
||||
double pm_y_arcsec{};
|
||||
double pm_y_dot_arcsec_day{};
|
||||
double delta_ut1_s{};
|
||||
double delta_ut1_dot_s_day{};
|
||||
};
|
||||
|
||||
struct Bds3_B1c_Bgto
|
||||
{
|
||||
int32_t gnss_id{};
|
||||
int32_t wn0_bgto{};
|
||||
int32_t t0_bgto_s{};
|
||||
double a0_bgto_s{};
|
||||
double a1_bgto_s_s{};
|
||||
double a2_bgto_s_s2{};
|
||||
};
|
||||
|
||||
struct Bds3_B1c_PageData
|
||||
{
|
||||
Bds3_B1c_PageCommon common{};
|
||||
int32_t sisaioc{};
|
||||
int32_t sisaioe{};
|
||||
int32_t wna{};
|
||||
int32_t toa_s{};
|
||||
std::array<Bds3_B1c_AlmanacReduced, 4> reduced_almanac{};
|
||||
Bds3_B1c_Eop eop{};
|
||||
Bds3_B1c_Bgto bgto{};
|
||||
Bds3_B1c_AlmanacMedium medium_almanac{};
|
||||
};
|
||||
|
||||
struct Beidou_Cnav1_PageData_Message
|
||||
{
|
||||
int32_t PRN{};
|
||||
Bds3_B1c_PageData page_data{};
|
||||
};
|
||||
|
||||
class Beidou_Cnav1_Navigation_Message
|
||||
{
|
||||
public:
|
||||
bool decode_frame(const float* symbols, int32_t num_symbols, int32_t expected_prn = 0, int32_t* fail_stage = nullptr);
|
||||
bool decode_frame_symbols(const float* symbols, int32_t num_symbols, int32_t expected_prn = 0, int32_t* fail_stage = nullptr);
|
||||
bool probe_subframe1_prn(const float* symbols, int32_t num_symbols, int32_t expected_prn) const;
|
||||
bool have_new_ephemeris() const;
|
||||
bool have_new_iono() const;
|
||||
bool have_new_utc_model() const;
|
||||
bool have_new_page_data() const;
|
||||
void clear_flags();
|
||||
const Beidou_Cnav1_Ephemeris& get_ephemeris() const;
|
||||
const Beidou_Cnav1_Iono& get_iono() const;
|
||||
const Beidou_Cnav1_Utc_Model& get_utc_model() const;
|
||||
const Bds3_B1c_PageData& get_page_data() const;
|
||||
double get_tow_s() const;
|
||||
|
||||
private:
|
||||
Beidou_Cnav1_Ephemeris ephemeris_{};
|
||||
Beidou_Cnav1_Iono iono_{};
|
||||
Beidou_Cnav1_Utc_Model utc_model_{};
|
||||
bool flag_new_ephemeris_{false};
|
||||
bool flag_new_iono_{false};
|
||||
bool flag_new_utc_{false};
|
||||
bool flag_new_page_data_{false};
|
||||
double tow_s_{0.0};
|
||||
Bds3_B1c_PageData page_data_{};
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,24 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_utc_model.h
|
||||
* \brief BeiDou B-CNAV1 UTC model (§7.12)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_BEIDOU_CNAV1_UTC_MODEL_H
|
||||
#define GNSS_SDR_BEIDOU_CNAV1_UTC_MODEL_H
|
||||
|
||||
class Beidou_Cnav1_Utc_Model
|
||||
{
|
||||
public:
|
||||
double A0{};
|
||||
double A1{};
|
||||
double A2{};
|
||||
int32_t tot{};
|
||||
int32_t WN_t{};
|
||||
int32_t WN_LSF{};
|
||||
int32_t DN{};
|
||||
int32_t delta_t_LSF{};
|
||||
bool valid{};
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -57,6 +57,7 @@ const std::unordered_map<std::string, double> SIGNAL_FREQ_MAP = {
|
||||
{"B1", FREQ1_BDS},
|
||||
{"B2", FREQ2_BDS},
|
||||
{"B3", FREQ3_BDS},
|
||||
{"1D", FREQ1},
|
||||
{"J1", FREQ1},
|
||||
{"J5", FREQ5},
|
||||
};
|
||||
|
||||
@@ -83,7 +83,8 @@ constexpr uint32_t CODE_L9A = 52; //!< obs code: SA SPS (IRN)
|
||||
constexpr uint32_t CODE_L9B = 53; //!< obs code: SB RS(D) (IRN)
|
||||
constexpr uint32_t CODE_L9C = 54; //!< obs code: SC RS(P) (IRN)
|
||||
constexpr uint32_t CODE_L9X = 55; //!< obs code: SB+C (IRN)
|
||||
constexpr int32_t MAXCODE = 55; //!< max number of obs code
|
||||
constexpr uint32_t CODE_L1D = 56; //!< obs code: B1C data (BDS)
|
||||
constexpr int32_t MAXCODE = 56; //!< max number of obs code
|
||||
|
||||
|
||||
/** \} */
|
||||
|
||||
@@ -49,7 +49,7 @@ public:
|
||||
* - GPS: "1C" (L1 C/A), "2S" (L2C), "L5"
|
||||
* - GLONASS: "1G" (L1 C/A), "2G" (L2 C/A)
|
||||
* - Galileo: "1B" (E1B), "5X" (E5a), "7X" (E5b), "6C" (E6C)
|
||||
* - BeiDou: "B1" (B1I), "B3" (B3I)
|
||||
* - BeiDou: "B1" (B1I), "B3" (B3I), "1D" (B1C)
|
||||
*/
|
||||
std::string get_signal_str() const;
|
||||
|
||||
@@ -59,8 +59,8 @@ public:
|
||||
friend std::ostream& operator<<(std::ostream& /*out*/, const Gnss_Signal& /*sig*/); //!< operator<< for pretty printing
|
||||
|
||||
private:
|
||||
Gnss_Satellite satellite{};
|
||||
std::string signal{};
|
||||
Gnss_Satellite satellite;
|
||||
std::string signal;
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -34,6 +34,7 @@ enum signal_flag : uint32_t
|
||||
GLO_2G = 0x1 << 8,
|
||||
BDS_B1 = 0x1 << 9,
|
||||
BDS_B3 = 0x1 << 10,
|
||||
BDS_B1C = 0x1 << 13,
|
||||
QZS_J1 = 0x1 << 11,
|
||||
QZS_J5 = 0x1 << 12
|
||||
};
|
||||
|
||||
@@ -235,6 +235,11 @@ private:
|
||||
#if EXTRA_TESTS
|
||||
#include "unit-tests/signal-processing-blocks/acquisition/acq_performance_test.cc"
|
||||
// #include "unit-tests/signal-processing-blocks/acquisition/beidou_b1i_pcps_acquisition_test.cc"
|
||||
#include "unit-tests/signal-processing-blocks/acquisition/beidou_b1c_pcps_acquisition_test.cc"
|
||||
#include "unit-tests/signal-processing-blocks/libs/beidou_b1c_signal_replica_test.cc"
|
||||
#include "unit-tests/signal-processing-blocks/tracking/beidou_b1c_dll_pll_veml_tracking_test.cc"
|
||||
#include "unit-tests/system-parameters/beidou_cnav1_ldpc_test.cc"
|
||||
#include "unit-tests/system-parameters/beidou_cnav1_navigation_message_test.cc"
|
||||
// #include "unit-tests/signal-processing-blocks/acquisition/beidou_b3i_pcps_acquisition_test.cc"
|
||||
#ifndef EXCLUDE_TESTS_REQUIRING_BINARIES
|
||||
#include "unit-tests/signal-processing-blocks/acquisition/glonass_l1_ca_pcps_acquisition_test.cc"
|
||||
|
||||
+35
@@ -0,0 +1,35 @@
|
||||
/*!
|
||||
* \file beidou_b1c_pcps_acquisition_test.cc
|
||||
* \brief Unit tests for BeidouB1cPcpsAmbiguousAcquisition
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "acquisition_interface.h"
|
||||
#include "beidou_b1c_pcps_ambiguous_acquisition.h"
|
||||
#include "gnss_block_factory.h"
|
||||
#include "gnss_block_interface.h"
|
||||
#include "in_memory_configuration.h"
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
|
||||
TEST(BeidouB1cPcpsAcquisitionTest, Instantiate)
|
||||
{
|
||||
auto configuration = std::make_shared<InMemoryConfiguration>();
|
||||
configuration->set_property("Acquisition_1D.implementation", "BEIDOU_B1C_PCPS_Ambiguous_Acquisition");
|
||||
configuration->set_property("Acquisition_1D.item_type", "gr_complex");
|
||||
auto factory = std::make_unique<GNSSBlockFactory>();
|
||||
std::shared_ptr<GNSSBlockInterface> acq_ = factory->GetBlock(configuration.get(), "Acquisition_1D", 1, 0);
|
||||
auto acquisition = std::dynamic_pointer_cast<AcquisitionInterface>(acq_);
|
||||
ASSERT_NE(acquisition, nullptr);
|
||||
EXPECT_STREQ("BEIDOU_B1C_PCPS_Ambiguous_Acquisition", acquisition->implementation().c_str());
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
/*!
|
||||
* \file beidou_b1c_signal_replica_test.cc
|
||||
* \brief Unit tests for BeiDou B1C signal replica generation
|
||||
* \author GNSS-SDR contributors
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*
|
||||
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* Copyright (C) 2010-2020 (see AUTHORS file for a list of contributors)
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*
|
||||
* -----------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "Beidou_B1C.h"
|
||||
#include "beidou_b1c_signal_replica.h"
|
||||
#include <gtest/gtest.h>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <complex>
|
||||
#include <vector>
|
||||
|
||||
TEST(BeidouB1cSignalReplicaTest, SampledCodeLength)
|
||||
{
|
||||
constexpr int32_t fs = 2046000;
|
||||
const auto samples_per_chip = static_cast<uint32_t>(std::lround(static_cast<double>(fs) / BEIDOU_B1C_CODE_RATE_CPS));
|
||||
const auto expected_length = static_cast<size_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS * samples_per_chip);
|
||||
std::vector<std::complex<float>> code(expected_length);
|
||||
const std::array<char, 3> data_signal = {{'1', 'D', '\0'}};
|
||||
beidou_b1c_code_gen_complex_sampled(code, data_signal, false, 1, fs, 0);
|
||||
EXPECT_EQ(code.size(), expected_length);
|
||||
}
|
||||
|
||||
|
||||
TEST(BeidouB1cSignalReplicaTest, Prn1DataFirst24ChipsMatchIcd)
|
||||
{
|
||||
std::array<int32_t, 10230> chips{};
|
||||
const std::array<char, 3> data_signal = {{'1', 'D', '\0'}};
|
||||
beidou_b1c_code_gen_int(chips, data_signal, 1);
|
||||
const char* expected = "101011111111011001001110";
|
||||
for (int i = 0; i < 24; i++)
|
||||
{
|
||||
EXPECT_EQ(chips[i], expected[i] - '0') << "chip index " << i;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
TEST(BeidouB1cSignalReplicaTest, SinBoc11AutocorrelationShape)
|
||||
{
|
||||
std::array<float, 20460> code{};
|
||||
const std::array<char, 3> data_signal = {{'1', 'D', '\0'}};
|
||||
beidou_b1c_code_gen_sinboc11_float(code, data_signal, 1);
|
||||
|
||||
double peak = 0.0;
|
||||
double side = 0.0;
|
||||
const int32_t samples_per_chip = 2;
|
||||
const int32_t code_samples = static_cast<int32_t>(BEIDOU_B1C_CODE_LENGTH_CHIPS) * samples_per_chip;
|
||||
for (int32_t lag = -code_samples; lag <= code_samples; lag++)
|
||||
{
|
||||
double corr = 0.0;
|
||||
for (int32_t i = 0; i < code_samples; i++)
|
||||
{
|
||||
const int32_t j = (i + lag + code_samples) % code_samples;
|
||||
corr += static_cast<double>(code[i]) * static_cast<double>(code[j]);
|
||||
}
|
||||
corr /= static_cast<double>(code_samples);
|
||||
if (lag == 0)
|
||||
{
|
||||
peak = std::abs(corr);
|
||||
}
|
||||
else if (std::abs(lag) == samples_per_chip)
|
||||
{
|
||||
side = std::max(side, std::abs(corr));
|
||||
}
|
||||
}
|
||||
EXPECT_GT(peak, 0.0);
|
||||
EXPECT_GT(side, 0.0);
|
||||
EXPECT_LT(side, peak);
|
||||
}
|
||||
+25
@@ -0,0 +1,25 @@
|
||||
/*!
|
||||
* \file beidou_b1c_dll_pll_veml_tracking_test.cc
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#include "beidou_b1c_dll_pll_veml_tracking.h"
|
||||
#include "gnss_block_factory.h"
|
||||
#include "gnss_block_interface.h"
|
||||
#include "in_memory_configuration.h"
|
||||
#include "tracking_interface.h"
|
||||
#include <gtest/gtest.h>
|
||||
#include <memory>
|
||||
|
||||
TEST(BeidouB1cDllPllVemlTrackingTest, Instantiate)
|
||||
{
|
||||
auto configuration = std::make_shared<InMemoryConfiguration>();
|
||||
configuration->set_property("Tracking_1D.implementation", "BEIDOU_B1C_DLL_PLL_VEML_Tracking");
|
||||
configuration->set_property("Tracking_1D.item_type", "gr_complex");
|
||||
configuration->set_property("GNSS-SDR.internal_fs_sps", "2046000");
|
||||
auto factory = std::make_unique<GNSSBlockFactory>();
|
||||
std::shared_ptr<GNSSBlockInterface> trk_ = factory->GetBlock(configuration.get(), "Tracking_1D", 1, 1);
|
||||
auto tracking = std::dynamic_pointer_cast<TrackingInterface>(trk_);
|
||||
ASSERT_NE(tracking, nullptr);
|
||||
EXPECT_STREQ("BEIDOU_B1C_DLL_PLL_VEML_Tracking", tracking->implementation().c_str());
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_ldpc_test.cc
|
||||
* \brief Unit tests for B-CNAV1 NB-LDPC decoder
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#include "Beidou_CNAV1.h"
|
||||
#include "beidou_cnav1_ldpc.h"
|
||||
#include <gtest/gtest.h>
|
||||
#include <array>
|
||||
#include <cstring>
|
||||
|
||||
namespace
|
||||
{
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> make_bit_llrs(const uint8_t* bits, float magnitude)
|
||||
{
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> llr{};
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_SUBFRAME2_SYMBOLS; i++)
|
||||
{
|
||||
llr[static_cast<size_t>(i)] = (bits[i] != 0U) ? magnitude : -magnitude;
|
||||
}
|
||||
return llr;
|
||||
}
|
||||
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> make_sf3_bit_llrs(const uint8_t* bits, float magnitude)
|
||||
{
|
||||
std::array<float, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> llr{};
|
||||
for (int32_t i = 0; i < BEIDOU_CNAV1_SUBFRAME3_SYMBOLS; i++)
|
||||
{
|
||||
llr[static_cast<size_t>(i)] = (bits[i] != 0U) ? magnitude : -magnitude;
|
||||
}
|
||||
return llr;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST(BeidouCnav1LdpcTest, DecodeZeroCodeword200_100)
|
||||
{
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> bits{};
|
||||
const auto llr = make_bit_llrs(bits.data(), 4.0F);
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SF2_DATA_BITS> info{};
|
||||
EXPECT_TRUE(beidou_cnav1_ldpc_decode_200_100(llr.data(), BEIDOU_CNAV1_SUBFRAME2_SYMBOLS, info.data()));
|
||||
for (const auto bit : info)
|
||||
{
|
||||
EXPECT_EQ(bit, 0U);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(BeidouCnav1LdpcTest, DecodeZeroCodeword88_44)
|
||||
{
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> bits{};
|
||||
const auto llr = make_sf3_bit_llrs(bits.data(), 4.0F);
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SF3_DATA_BITS> info{};
|
||||
EXPECT_TRUE(beidou_cnav1_ldpc_decode_88_44(llr.data(), BEIDOU_CNAV1_SUBFRAME3_SYMBOLS, info.data()));
|
||||
for (const auto bit : info)
|
||||
{
|
||||
EXPECT_EQ(bit, 0U);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(BeidouCnav1LdpcTest, CorrectSingleSymbolError200_100)
|
||||
{
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SUBFRAME2_SYMBOLS> bits{};
|
||||
const uint8_t wrong_symbol = 5U;
|
||||
for (int32_t bit = 0; bit < 6; bit++)
|
||||
{
|
||||
bits[static_cast<size_t>(bit)] = static_cast<uint8_t>((wrong_symbol >> (5 - bit)) & 1U);
|
||||
}
|
||||
|
||||
const auto llr = make_bit_llrs(bits.data(), 4.0F);
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SF2_DATA_BITS> info{};
|
||||
EXPECT_TRUE(beidou_cnav1_ldpc_decode_200_100(llr.data(), BEIDOU_CNAV1_SUBFRAME2_SYMBOLS, info.data()));
|
||||
for (const auto bit : info)
|
||||
{
|
||||
EXPECT_EQ(bit, 0U);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(BeidouCnav1LdpcTest, CorrectSingleSymbolError88_44)
|
||||
{
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SUBFRAME3_SYMBOLS> bits{};
|
||||
const uint8_t wrong_symbol = 17U;
|
||||
for (int32_t bit = 0; bit < 6; bit++)
|
||||
{
|
||||
bits[static_cast<size_t>(bit)] = static_cast<uint8_t>((wrong_symbol >> (5 - bit)) & 1U);
|
||||
}
|
||||
|
||||
const auto llr = make_sf3_bit_llrs(bits.data(), 4.0F);
|
||||
std::array<uint8_t, BEIDOU_CNAV1_SF3_DATA_BITS> info{};
|
||||
EXPECT_TRUE(beidou_cnav1_ldpc_decode_88_44(llr.data(), BEIDOU_CNAV1_SUBFRAME3_SYMBOLS, info.data()));
|
||||
for (const auto bit : info)
|
||||
{
|
||||
EXPECT_EQ(bit, 0U);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
/*!
|
||||
* \file beidou_cnav1_navigation_message_test.cc
|
||||
* \brief Unit tests for B-CNAV1 navigation message parser
|
||||
* SPDX-License-Identifier: GPL-3.0-or-later
|
||||
*/
|
||||
|
||||
#include "Beidou_CNAV1.h"
|
||||
#include "beidou_cnav1_navigation_message.h"
|
||||
#include "rtklib_conversions.h"
|
||||
#include <gtest/gtest.h>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
|
||||
namespace
|
||||
{
|
||||
void append_bch21_6(std::vector<float>& frame, uint32_t prn)
|
||||
{
|
||||
std::array<int32_t, 6> msg{};
|
||||
for (int32_t bit = 5; bit >= 0; bit--)
|
||||
{
|
||||
msg[5 - bit] = static_cast<int32_t>((prn >> bit) & 1U);
|
||||
}
|
||||
static const int32_t g[7] = {1, 0, 1, 0, 1, 1, 1};
|
||||
std::array<int32_t, 21> codeword{};
|
||||
for (int32_t i = 0; i < 6; i++)
|
||||
{
|
||||
codeword[i] = msg[i];
|
||||
}
|
||||
for (int32_t i = 0; i < 6; i++)
|
||||
{
|
||||
if (codeword[i] != 0)
|
||||
{
|
||||
for (int32_t j = 0; j < 7; j++)
|
||||
{
|
||||
codeword[i + j] ^= g[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (const auto bit : codeword)
|
||||
{
|
||||
frame.push_back(bit != 0 ? 1.0F : -1.0F);
|
||||
}
|
||||
}
|
||||
|
||||
void append_bch51_8(std::vector<float>& frame, uint32_t soh)
|
||||
{
|
||||
std::array<int32_t, 8> msg{};
|
||||
for (int32_t bit = 7; bit >= 0; bit--)
|
||||
{
|
||||
msg[7 - bit] = static_cast<int32_t>((soh >> bit) & 1U);
|
||||
}
|
||||
static const int32_t g[9] = {1, 1, 0, 0, 1, 1, 1, 1, 1};
|
||||
std::array<int32_t, 51> codeword{};
|
||||
for (int32_t i = 0; i < 8; i++)
|
||||
{
|
||||
codeword[i] = msg[i];
|
||||
}
|
||||
for (int32_t i = 0; i < 8; i++)
|
||||
{
|
||||
if (codeword[i] != 0)
|
||||
{
|
||||
for (int32_t j = 0; j < 9; j++)
|
||||
{
|
||||
codeword[i + j] ^= g[j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (const auto bit : codeword)
|
||||
{
|
||||
frame.push_back(bit != 0 ? 1.0F : -1.0F);
|
||||
}
|
||||
}
|
||||
|
||||
void deinterleave_like_icd(const std::vector<float>& sf2_bits, const std::vector<float>& sf3_bits, std::vector<float>& interleaved)
|
||||
{
|
||||
std::array<float, BEIDOU_CNAV1_INTERLEAVE_ROWS * BEIDOU_CNAV1_INTERLEAVE_COLS> grid{};
|
||||
int32_t sf2_idx = 0;
|
||||
int32_t sf3_idx = 0;
|
||||
int32_t row = 0;
|
||||
while (row < 33)
|
||||
{
|
||||
for (int32_t r = 0; r < 2; r++)
|
||||
{
|
||||
for (int32_t col = 0; col < BEIDOU_CNAV1_INTERLEAVE_COLS; col++)
|
||||
{
|
||||
grid[static_cast<size_t>(row) * BEIDOU_CNAV1_INTERLEAVE_COLS + col] = sf2_bits[static_cast<size_t>(sf2_idx++)];
|
||||
}
|
||||
row++;
|
||||
}
|
||||
for (int32_t col = 0; col < BEIDOU_CNAV1_INTERLEAVE_COLS; col++)
|
||||
{
|
||||
grid[static_cast<size_t>(row) * BEIDOU_CNAV1_INTERLEAVE_COLS + col] = sf3_bits[static_cast<size_t>(sf3_idx++)];
|
||||
}
|
||||
row++;
|
||||
}
|
||||
for (int32_t r = 0; r < 3; r++)
|
||||
{
|
||||
for (int32_t col = 0; col < BEIDOU_CNAV1_INTERLEAVE_COLS; col++)
|
||||
{
|
||||
grid[static_cast<size_t>(row) * BEIDOU_CNAV1_INTERLEAVE_COLS + col] = sf2_bits[static_cast<size_t>(sf2_idx++)];
|
||||
}
|
||||
row++;
|
||||
}
|
||||
|
||||
interleaved.clear();
|
||||
for (int32_t col = 0; col < BEIDOU_CNAV1_INTERLEAVE_COLS; col++)
|
||||
{
|
||||
for (int32_t row_idx = 0; row_idx < BEIDOU_CNAV1_INTERLEAVE_ROWS; row_idx++)
|
||||
{
|
||||
interleaved.push_back(grid[static_cast<size_t>(row_idx) * BEIDOU_CNAV1_INTERLEAVE_COLS + col]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<float> encode_bits_to_llr(const std::vector<uint8_t>& bits)
|
||||
{
|
||||
std::vector<float> llr;
|
||||
llr.reserve(bits.size());
|
||||
for (const auto bit : bits)
|
||||
{
|
||||
llr.push_back(bit != 0U ? 4.0F : -4.0F);
|
||||
}
|
||||
return llr;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST(BeidouCnav1NavigationMessageTest, DecodeSubframe1OnlyFrameFailsWithoutSf2Crc)
|
||||
{
|
||||
std::vector<float> frame;
|
||||
append_bch21_6(frame, 19U);
|
||||
append_bch51_8(frame, 1U);
|
||||
frame.insert(frame.end(), BEIDOU_CNAV1_INTERLEAVED_SYMBOLS, 4.0F);
|
||||
|
||||
Beidou_Cnav1_Navigation_Message nav;
|
||||
EXPECT_FALSE(nav.decode_frame_symbols(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS));
|
||||
}
|
||||
|
||||
TEST(BeidouCnav1NavigationMessageTest, DecodeEphemerisFromZeroSf2Payload)
|
||||
{
|
||||
const auto sf2_llr = encode_bits_to_llr(std::vector<uint8_t>(BEIDOU_CNAV1_SUBFRAME2_SYMBOLS, 0U));
|
||||
const auto sf3_llr = encode_bits_to_llr(std::vector<uint8_t>(BEIDOU_CNAV1_SUBFRAME3_SYMBOLS, 0U));
|
||||
EXPECT_EQ(sf2_llr.size(), static_cast<size_t>(BEIDOU_CNAV1_SUBFRAME2_SYMBOLS));
|
||||
EXPECT_EQ(sf3_llr.size(), static_cast<size_t>(BEIDOU_CNAV1_SUBFRAME3_SYMBOLS));
|
||||
|
||||
std::vector<float> interleaved;
|
||||
deinterleave_like_icd(sf2_llr, sf3_llr, interleaved);
|
||||
EXPECT_EQ(interleaved.size(), static_cast<size_t>(BEIDOU_CNAV1_INTERLEAVED_SYMBOLS));
|
||||
|
||||
std::vector<float> frame;
|
||||
append_bch21_6(frame, 19U);
|
||||
append_bch51_8(frame, 1U);
|
||||
frame.insert(frame.end(), interleaved.begin(), interleaved.end());
|
||||
|
||||
Beidou_Cnav1_Navigation_Message nav;
|
||||
ASSERT_TRUE(nav.decode_frame_symbols(frame.data(), BEIDOU_CNAV1_FRAME_SYMBOLS, 19));
|
||||
EXPECT_TRUE(nav.have_new_ephemeris());
|
||||
EXPECT_EQ(nav.get_ephemeris().PRN, 19);
|
||||
EXPECT_DOUBLE_EQ(nav.get_tow_s(), 18.0);
|
||||
|
||||
const auto rtklib_eph = eph_to_rtklib(nav.get_ephemeris());
|
||||
EXPECT_EQ(rtklib_eph.iode, 0);
|
||||
EXPECT_EQ(rtklib_eph.iodc, 0);
|
||||
EXPECT_NEAR(rtklib_eph.A, BEIDOU_CNAV1_A_REF_MEO, 1.0);
|
||||
}
|
||||
@@ -0,0 +1,103 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Generate Beidou_B1C_codes.h from ICD Weil parameters in Beidou_B1C_prn.h."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
PRN_HEADER = ROOT / "src/core/system_parameters/Beidou_B1C_prn.h"
|
||||
OUT_HEADER = ROOT / "src/core/system_parameters/Beidou_B1C_codes.h"
|
||||
|
||||
|
||||
def extract(name: str, text: str) -> list[int]:
|
||||
match = re.search(rf"{name}\[63\]\s*=\s*\{{([^}}]+)\}}", text, re.S)
|
||||
if not match:
|
||||
raise RuntimeError(f"Could not parse {name}")
|
||||
return [int(x.strip()) for x in match.group(1).split(",") if x.strip()]
|
||||
|
||||
|
||||
def legendre(n: int) -> list[int]:
|
||||
seq = [0] * n
|
||||
for x in range(1, n):
|
||||
seq[(x * x) % n] = 1
|
||||
return seq
|
||||
|
||||
|
||||
def weil_code(L: list[int], weil_len: int, w: int, p: int, out_len: int) -> list[int]:
|
||||
W = [L[k] ^ L[(k + w) % weil_len] for k in range(weil_len)]
|
||||
return [W[(n + p - 1) % weil_len] for n in range(out_len)]
|
||||
|
||||
|
||||
def chips_to_hex(chips: list[int]) -> str:
|
||||
out = ""
|
||||
for i in range(0, len(chips), 4):
|
||||
chunk = chips[i : i + 4]
|
||||
value = 0
|
||||
for bit in chunk:
|
||||
value = (value << 1) | bit
|
||||
while len(chunk) < 4:
|
||||
value <<= 1
|
||||
chunk.append(0)
|
||||
out += format(value, "X")
|
||||
return out
|
||||
|
||||
|
||||
def chips_to_binstr(chips: list[int]) -> str:
|
||||
return "".join(str(c) for c in chips)
|
||||
|
||||
|
||||
def emit_array(name: str, rows: list[str]) -> str:
|
||||
lines = [f"constexpr char {name}[63][{len(rows[0]) + 1}] = {{"]
|
||||
lines.extend(f' "{row}",' for row in rows)
|
||||
lines.append("};")
|
||||
return "\n".join(lines)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
text = PRN_HEADER.read_text(encoding="utf-8")
|
||||
data_w = extract("BEIDOU_B1C_DATA_PRN_W", text)
|
||||
data_p = extract("BEIDOU_B1C_DATA_PRN_P", text)
|
||||
pilot_w = extract("BEIDOU_B1C_PILOT_PRN_W", text)
|
||||
pilot_p = extract("BEIDOU_B1C_PILOT_PRN_P", text)
|
||||
sec_w = extract("BEIDOU_B1C_PILOT_SECONDARY_W", text)
|
||||
sec_p = extract("BEIDOU_B1C_PILOT_SECONDARY_P", text)
|
||||
|
||||
L10243 = legendre(10243)
|
||||
L3607 = legendre(3607)
|
||||
|
||||
data_hex = [
|
||||
chips_to_hex(weil_code(L10243, 10243, data_w[i], data_p[i], 10230)) for i in range(63)
|
||||
]
|
||||
pilot_hex = [
|
||||
chips_to_hex(weil_code(L10243, 10243, pilot_w[i], pilot_p[i], 10230)) for i in range(63)
|
||||
]
|
||||
sec_bin = [chips_to_binstr(weil_code(L3607, 3607, sec_w[i], sec_p[i], 1800)) for i in range(63)]
|
||||
|
||||
expected = "101011111111011001001110"
|
||||
assert "".join(str(b) for b in weil_code(L10243, 10243, data_w[0], data_p[0], 10230)[:24]) == expected
|
||||
|
||||
header = f"""/* Auto-generated by {Path(__file__).name}. Do not edit manually. */
|
||||
#ifndef GNSS_SDR_BEIDOU_B1C_CODES_H
|
||||
#define GNSS_SDR_BEIDOU_B1C_CODES_H
|
||||
|
||||
#include <cstddef>
|
||||
|
||||
constexpr size_t BEIDOU_B1C_PRIMARY_CODE_STR_LENGTH = 2558;
|
||||
constexpr size_t BEIDOU_B1C_PILOT_SECONDARY_CODE_STR_LENGTH = 1800;
|
||||
|
||||
{emit_array("BEIDOU_B1C_DATA_PRIMARY_CODE", data_hex)}
|
||||
|
||||
{emit_array("BEIDOU_B1C_PILOT_PRIMARY_CODE", pilot_hex)}
|
||||
|
||||
{emit_array("BEIDOU_B1C_PILOT_SECONDARY_CODE", sec_bin)}
|
||||
|
||||
#endif
|
||||
"""
|
||||
OUT_HEADER.write_text(header, encoding="utf-8")
|
||||
print(f"Wrote {OUT_HEADER} ({len(header)} bytes)")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,145 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Generate Beidou_CNAV1_ldpc.h from BDS-SIS-ICD-B1C-1.0 PDF text."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import subprocess
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
ICD_PDF = ROOT / "docs/article/北斗卫星导航系统-空间信号接口控制文件-公开服务信号B1C(1.0版).pdf"
|
||||
OUT = ROOT / "src/core/system_parameters/Beidou_CNAV1_ldpc.h"
|
||||
|
||||
|
||||
def icd_text() -> str:
|
||||
proc = subprocess.run(
|
||||
["pdftotext", "-layout", str(ICD_PDF), "-"],
|
||||
check=True,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
return proc.stdout
|
||||
|
||||
|
||||
def is_numline(line: str) -> bool:
|
||||
stripped = line.strip()
|
||||
return bool(stripped) and all(part.isdigit() for part in stripped.split())
|
||||
|
||||
|
||||
def extract_numeric_lines(
|
||||
text: str,
|
||||
start_marker: str,
|
||||
end_marker: str,
|
||||
numbers_per_line: int,
|
||||
) -> list[list[int]]:
|
||||
section = text.split(start_marker, 1)[1].split(end_marker, 1)[0]
|
||||
lines = section.splitlines()
|
||||
rows: list[list[int]] = []
|
||||
for line in lines:
|
||||
if not is_numline(line):
|
||||
continue
|
||||
nums = [int(part) for part in line.split()]
|
||||
if len(nums) == numbers_per_line:
|
||||
rows.append(nums)
|
||||
return rows
|
||||
|
||||
|
||||
def parse_matrix(
|
||||
text: str,
|
||||
start_marker: str,
|
||||
end_marker: str,
|
||||
expected_rows: int,
|
||||
row_weight: int,
|
||||
) -> list[tuple[int, ...]]:
|
||||
lines = extract_numeric_lines(text, start_marker, end_marker, 16)
|
||||
if len(lines) * 16 != expected_rows * row_weight:
|
||||
raise RuntimeError(
|
||||
f"{start_marker}: expected {expected_rows * row_weight} values, got {len(lines) * 16}"
|
||||
)
|
||||
values: list[int] = []
|
||||
for line in lines:
|
||||
values.extend(line)
|
||||
rows: list[tuple[int, ...]] = []
|
||||
for row in range(expected_rows):
|
||||
base = row * row_weight
|
||||
rows.append(tuple(values[base + j] for j in range(row_weight)))
|
||||
return rows
|
||||
|
||||
|
||||
def flatten_rows(rows: list[tuple[int, ...]]) -> list[int]:
|
||||
flat: list[int] = []
|
||||
for row in rows:
|
||||
flat.extend(row)
|
||||
return flat
|
||||
|
||||
|
||||
def gf64_tables() -> tuple[list[int], list[int]]:
|
||||
def gf_mul(a: int, b: int) -> int:
|
||||
product = 0
|
||||
for _ in range(6):
|
||||
if b & 1:
|
||||
product ^= a
|
||||
carry = a & 0x20
|
||||
a = (a << 1) & 0x3F
|
||||
if carry:
|
||||
a ^= 0x03
|
||||
b >>= 1
|
||||
return product
|
||||
|
||||
exp = [0] * 64
|
||||
log = [0] * 64
|
||||
value = 1
|
||||
for i in range(63):
|
||||
exp[i] = value
|
||||
log[value] = i
|
||||
value = gf_mul(value, 2)
|
||||
exp[63] = exp[0]
|
||||
return exp, log
|
||||
|
||||
|
||||
def emit_array(name: str, c_type: str, values: list[int]) -> str:
|
||||
return f"constexpr {c_type} {name}[{len(values)}] = {{{','.join(map(str, values))}}};"
|
||||
|
||||
|
||||
def main() -> None:
|
||||
text = icd_text()
|
||||
idx100 = parse_matrix(text, "H100,200,index=[", "H100,200,element=[", expected_rows=100, row_weight=4)
|
||||
el100 = parse_matrix(text, "H100,200,element=[", "H44,88,index=[", expected_rows=100, row_weight=4)
|
||||
idx44 = parse_matrix(text, "H44,88,index=[", "H44,88,element=[", expected_rows=44, row_weight=4)
|
||||
el44 = parse_matrix(text, "H44,88,element=[", "H44,88 的读取规则", expected_rows=44, row_weight=4)
|
||||
exp, log = gf64_tables()
|
||||
|
||||
header: list[str] = [
|
||||
"/* Auto-generated by utils/generate_beidou_cnav1_ldpc.py. Do not edit manually. */",
|
||||
"#ifndef GNSS_SDR_BEIDOU_CNAV1_LDPC_MATRICES_H",
|
||||
"#define GNSS_SDR_BEIDOU_CNAV1_LDPC_MATRICES_H",
|
||||
"#include <cstdint>",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_N200 = 200;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_K200 = 100;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_M200 = 100;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_DC200 = 4;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_N88 = 88;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_K88 = 44;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_M88 = 44;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_DC88 = 4;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_NM = 8;",
|
||||
"constexpr int32_t BEIDOU_CNAV1_LDPC_MAX_ITER = 15;",
|
||||
"",
|
||||
"/*",
|
||||
" * GF(2^6) LUTs follow BDS-SIS-ICD-B1C-1.0 Appendix mapping tables.",
|
||||
" * Primitive polynomial: p(x)=x^6+x+1.",
|
||||
" */",
|
||||
]
|
||||
header.append(emit_array("BEIDOU_CNAV1_H100_200_INDEX", "uint16_t", flatten_rows(idx100)))
|
||||
header.append(emit_array("BEIDOU_CNAV1_H100_200_ELEMENT", "uint8_t", flatten_rows(el100)))
|
||||
header.append(emit_array("BEIDOU_CNAV1_H44_88_INDEX", "uint16_t", flatten_rows(idx44)))
|
||||
header.append(emit_array("BEIDOU_CNAV1_H44_88_ELEMENT", "uint8_t", flatten_rows(el44)))
|
||||
header.append(emit_array("BEIDOU_CNAV1_GF64_EXP", "uint8_t", exp))
|
||||
header.append(emit_array("BEIDOU_CNAV1_GF64_LOG", "int8_t", log))
|
||||
header.append("#endif // GNSS_SDR_BEIDOU_CNAV1_LDPC_MATRICES_H")
|
||||
OUT.write_text("\n".join(header) + "\n", encoding="utf-8")
|
||||
print(f"Wrote {OUT}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user