mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 04:00:34 +00:00
Code Issues Releases Wiki Activity

Fix Beidou. Disabled symbol integration in tracking for Beidou and disabled experimental tracking carrier Doppler correction for all systems

Browse Source
This commit is contained in:
Javier Arribas 2019-07-05 20:30:34 +02:00
parent 38b91bec13
commit 0d3299f29b
7 changed files with 400 additions and 403 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

315
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc
View File

@ -29,10 +29,10 @@
*
* -------------------------------------------------------------------------
*/
#include "beidou_b1i_telemetry_decoder_gs.h"
#include "Beidou_B1I.h"
#include "Beidou_DNAV.h"
#include "beidou_dnav_almanac.h"
#include "beidou_dnav_ephemeris.h"
#include "beidou_dnav_iono.h"
#include "beidou_dnav_utc_model.h"
@ -57,6 +57,7 @@ beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump)
return beidou_b1i_telemetry_decoder_gs_sptr(new beidou_b1i_telemetry_decoder_gs(satellite, dump));
}
beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs(
const Gnss_Satellite &satellite,
bool dump) : gr::block("beidou_b1i_telemetry_decoder_gs",
@ -74,38 +75,63 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs(
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "Initializing BeiDou B1I Telemetry Decoding for satellite " << this->d_satellite;
d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS;
d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D1NAV_SYMBOL_RATE_SPS;
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol;
d_secondary_code_symbols = static_cast<int32_t *>(volk_gnsssdr_malloc(BEIDOU_B1I_SECONDARY_CODE_LENGTH * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS;
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol;
// Setting samples of preamble code
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
// Setting samples of secondary code
for (int32_t i = 0; i < BEIDOU_B1I_SECONDARY_CODE_LENGTH; i++)
{
if (BEIDOU_DNAV_PREAMBLE.at(i) == '1')
if (BEIDOU_B1I_SECONDARY_CODE.at(i) == '1')
{
d_preamble_samples[i] = 1;
d_secondary_code_symbols[i] = 1;
}
else
{
d_preamble_samples[i] = -1;
d_secondary_code_symbols[i] = -1;
}
}
d_subframe_symbols = static_cast<float *>(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols);
// Setting samples of preamble code
int32_t n = 0;
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
{
int32_t m = 0;
if (BEIDOU_DNAV_PREAMBLE.at(i) == '1')
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = d_secondary_code_symbols[m];
n++;
m++;
m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH;
}
}
else
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = -d_secondary_code_symbols[m];
n++;
m++;
m = m % BEIDOU_B1I_SECONDARY_CODE_LENGTH;
}
}
}
d_subframe_symbols = static_cast<double *>(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols + 1);
d_last_valid_preamble = 0;
d_sent_tlm_failed_msg = false;
d_flag_valid_word = false;
// Generic settings
d_sample_counter = 0;
d_stat = 0;
d_preamble_index = 0;
d_flag_frame_sync = false;
d_TOW_at_current_symbol_ms = 0U;
d_TOW_at_current_symbol_ms = 0;
d_TOW_at_Preamble_ms = 0U;
Flag_valid_word = false;
d_CRC_error_counter = 0;
@ -118,6 +144,7 @@ beidou_b1i_telemetry_decoder_gs::beidou_b1i_telemetry_decoder_gs(
beidou_b1i_telemetry_decoder_gs::~beidou_b1i_telemetry_decoder_gs()
{
volk_gnsssdr_free(d_preamble_samples);
volk_gnsssdr_free(d_secondary_code_symbols);
volk_gnsssdr_free(d_subframe_symbols);
if (d_dump_file.is_open() == true)
@ -165,7 +192,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, in
void beidou_b1i_telemetry_decoder_gs::decode_word(
int32_t word_counter,
const float *enc_word_symbols,
const double *enc_word_symbols,
int32_t *dec_word_symbols)
{
int32_t bitsbch[30], first_branch[15], second_branch[15];
@ -205,7 +232,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_word(
}
void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
void beidou_b1i_telemetry_decoder_gs::decode_subframe(double *frame_symbols)
{
// 1. Transform from symbols to bits
std::string data_bits;
@ -236,13 +263,11 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
// 3. Check operation executed correctly
if (d_nav.flag_crc_test == true)
{
DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel
<< " from satellite " << d_satellite;
DLOG(INFO) << "BeiDou DNAV CRC correct in channel " << d_channel << " from satellite " << d_satellite;
}
else
{
DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel
<< " from satellite " << d_satellite;
DLOG(INFO) << "BeiDou DNAV CRC error in channel " << d_channel << " from satellite " << d_satellite;
}
// 4. Push the new navigation data to the queues
if (d_nav.have_new_ephemeris() == true)
@ -297,59 +322,41 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell
{
// Clear values from previous declaration
volk_gnsssdr_free(d_preamble_samples);
volk_gnsssdr_free(d_secondary_code_symbols);
volk_gnsssdr_free(d_subframe_symbols);
d_samples_per_symbol = (BEIDOU_B1I_CODE_RATE_HZ / BEIDOU_B1I_CODE_LENGTH_CHIPS) / BEIDOU_D2NAV_SYMBOL_RATE_SPS;
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol;
d_secondary_code_symbols = nullptr;
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS;
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol;
// Setting samples of preamble code
int32_t n = 0;
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
{
if (BEIDOU_DNAV_PREAMBLE.at(i) == '1')
{
d_preamble_samples[i] = 1;
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = 1;
n++;
}
}
else
{
d_preamble_samples[i] = -1;
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = -1;
n++;
}
}
}
d_symbol_duration_ms = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS;
d_subframe_symbols = static_cast<float *>(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols);
}
else
{
// Clear values from previous declaration
volk_gnsssdr_free(d_preamble_samples);
volk_gnsssdr_free(d_subframe_symbols);
//back to normal satellites
d_symbol_duration_ms = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B1I_CODE_PERIOD_MS;
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS;
// Setting samples of preamble code
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
{
if (BEIDOU_DNAV_PREAMBLE.at(i) == '1')
{
d_preamble_samples[i] = 1;
}
else
{
d_preamble_samples[i] = -1;
}
}
d_subframe_symbols = static_cast<float *>(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols);
d_subframe_symbols = static_cast<double *>(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double), volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol + d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols + 1);
}
}
@ -380,15 +387,6 @@ void beidou_b1i_telemetry_decoder_gs::set_channel(int32_t channel)
}
}
void beidou_b1i_telemetry_decoder_gs::reset()
{
d_last_valid_preamble = d_sample_counter;
d_TOW_at_current_symbol_ms = 0;
d_sent_tlm_failed_msg = false;
d_flag_valid_word = false;
DLOG(INFO) << "Beidou B1I Telemetry decoder reset for satellite " << d_satellite;
return;
}
int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
@ -405,9 +403,10 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue
d_sample_counter++; // count for the processed samples
consume_each(1);
d_flag_preamble = false;
if (d_symbol_history.size() >= d_required_symbols)
if (d_symbol_history.size() > d_required_symbols)
{
//******* preamble correlation ********
for (int32_t i = 0; i < d_samples_per_preamble; i++)
@ -422,6 +421,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
}
}
}
//******* frame sync ******************
if (d_stat == 0) // no preamble information
{
@ -429,7 +429,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
{
// Record the preamble sample stamp
d_preamble_index = d_sample_counter;
DLOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite;
LOG(INFO) << "Preamble detection for BEIDOU B1I SAT " << this->d_satellite;
// Enter into frame pre-detection status
d_stat = 1;
}
@ -443,54 +443,9 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
if (abs(preamble_diff - d_preamble_period_samples) == 0)
{
// try to decode frame
DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite;
LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B1I SAT " << this->d_satellite;
d_preamble_index = d_sample_counter; //record the preamble sample stamp
d_stat = 2;
// ******* SAMPLES TO SYMBOLS *******
if (corr_value > 0) //normal PLL lock
{
for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++)
{
d_subframe_symbols[i] = d_symbol_history.at(i);
}
}
else // 180 deg. inverted carrier phase PLL lock
{
for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++)
{
d_subframe_symbols[i] = -d_symbol_history.at(i);
}
}
// call the decoder
decode_subframe(d_subframe_symbols);
if (d_nav.flag_crc_test == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
DLOG(INFO) << "BeiDou DNAV frame sync found for SAT " << this->d_satellite;
}
}
else
{
d_CRC_error_counter++;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
{
DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite;
d_flag_frame_sync = false;
d_stat = 0;
flag_SOW_set = false;
}
}
}
else
{
@ -509,16 +464,50 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
// ******* SAMPLES TO SYMBOLS *******
if (corr_value > 0) //normal PLL lock
{
int32_t k = 0;
for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++)
{
d_subframe_symbols[i] = d_symbol_history.at(i);
d_subframe_symbols[i] = 0;
// integrate samples into symbols
for (uint32_t m = 0; m < d_samples_per_symbol; m++)
{
if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6)
{
// because last symbol of the preamble is just received now!
d_subframe_symbols[i] += d_symbol_history.at(i * d_samples_per_symbol + m);
}
else
{
// because last symbol of the preamble is just received now!
d_subframe_symbols[i] += static_cast<float>(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m);
k++;
k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH;
}
}
}
}
else // 180 deg. inverted carrier phase PLL lock
{
int32_t k = 0;
for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++)
{
d_subframe_symbols[i] = -d_symbol_history.at(i);
d_subframe_symbols[i] = 0;
// integrate samples into symbols
for (uint32_t m = 0; m < d_samples_per_symbol; m++)
{
if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6)
{
// because last symbol of the preamble is just received now!
d_subframe_symbols[i] -= d_symbol_history.at(i * d_samples_per_symbol + m);
}
else
{
// because last symbol of the preamble is just received now!
d_subframe_symbols[i] -= static_cast<float>(d_secondary_code_symbols[k]) * d_symbol_history.at(i * d_samples_per_symbol + m);
k++;
k = k % BEIDOU_B1I_SECONDARY_CODE_LENGTH;
}
}
}
}
@ -542,7 +531,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
{
DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite;
LOG(INFO) << "BeiDou DNAV frame sync lost for SAT " << this->d_satellite;
d_flag_frame_sync = false;
d_stat = 0;
flag_SOW_set = false;
@ -550,6 +539,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
}
}
}
// UPDATE GNSS SYNCHRO DATA
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true)
@ -557,67 +547,52 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
{
// Reporting sow as gps time of week
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.d_SOW + 14) * 1000.0);
//check TOW update consistency
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
//compute new TOW
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms;
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) * BEIDOU_B1I_CODE_PERIOD_MS);
flag_SOW_set = true;
d_nav.flag_new_SOW_available = false;
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol_ms += static_cast<uint32_t>(BEIDOU_B1I_CODE_PERIOD_MS);
}
if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms)
{
LOG(INFO) << "Warning: BEIDOU B1I TOW update in ch " << d_channel
<< " does not match the TLM TOW counter " << static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n";
d_TOW_at_current_symbol_ms = 0;
d_flag_valid_word = false;
}
else
{
d_last_valid_preamble = d_sample_counter;
d_flag_valid_word = true;
}
if (d_flag_frame_sync == true and flag_SOW_set == true)
{
current_symbol.Flag_valid_word = true;
}
else
{
if (d_flag_valid_word)
{
d_TOW_at_current_symbol_ms += d_symbol_duration_ms;
if (current_symbol.Flag_valid_symbol_output == false)
{
d_flag_valid_word = false;
}
}
current_symbol.Flag_valid_word = false;
}
if (d_flag_valid_word == true)
current_symbol.PRN = this->d_satellite.get_PRN();
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
if (d_dump == true)
{
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_symbol.Flag_valid_word = d_flag_valid_word;
if (d_dump == true)
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
uint64_t tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what();
}
double tmp_double;
uint64_t tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
tmp_double = d_nav.d_SOW;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = static_cast<uint64_t>(d_required_symbols);
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
}
return 0;
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
}

24
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h
View File

@ -39,7 +39,7 @@
#include <boost/circular_buffer.hpp>
#include <boost/shared_ptr.hpp> // for boost::shared_ptr
#include <gnuradio/block.h> // for block
#include <gnuradio/types.h> // for gr_vector_const_void_star
#include <gnuradio/types.h> // for gr_vector_const_void_star
#include <cstdint>
#include <fstream>
#include <string>
@ -62,8 +62,10 @@ public:
~beidou_b1i_telemetry_decoder_gs(); //!< Class destructor
void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN
void set_channel(int channel); //!< Set receiver's channel
void reset();
inline void reset()
{
return;
}
/*!
* \brief This is where all signal processing takes place
*/
@ -75,17 +77,19 @@ private:
beidou_b1i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump);
beidou_b1i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump);
void decode_subframe(float *symbols);
void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols);
void decode_subframe(double *symbols);
void decode_word(int32_t word_counter, const double *enc_word_symbols, int32_t *dec_word_symbols);
void decode_bch15_11_01(const int32_t *bits, int32_t *decbits);
// Preamble decoding
int32_t *d_preamble_samples;
int32_t *d_secondary_code_symbols;
uint32_t d_samples_per_symbol;
int32_t d_symbols_per_preamble;
int32_t d_samples_per_preamble;
int32_t d_preamble_period_samples;
float *d_subframe_symbols;
double *d_subframe_symbols;
uint32_t d_required_symbols;
// Storage for incoming data
@ -103,16 +107,12 @@ private:
//!< Navigation Message variable
Beidou_Dnav_Navigation_Message d_nav;
// Values to populate gnss synchronization structure
uint32_t d_symbol_duration_ms;
//!< Values to populate gnss synchronization structure
uint32_t d_TOW_at_Preamble_ms;
uint32_t d_TOW_at_current_symbol_ms;
uint64_t d_last_valid_preamble;
bool d_flag_valid_word;
bool d_sent_tlm_failed_msg;
bool Flag_valid_word;
// Satellite Information and logging capacity
//!< Satellite Information and logging capacity
Gnss_Satellite d_satellite;
int32_t d_channel;
bool d_dump;

374
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc
View File

@ -53,7 +53,8 @@ beidou_b3i_telemetry_decoder_gs_sptr
beidou_b3i_make_telemetry_decoder_gs(const Gnss_Satellite &satellite,
bool dump)
{
return beidou_b3i_telemetry_decoder_gs_sptr(new beidou_b3i_telemetry_decoder_gs(satellite, dump));
return beidou_b3i_telemetry_decoder_gs_sptr(
new beidou_b3i_telemetry_decoder_gs(satellite, dump));
}
@ -69,37 +70,74 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs(
this->message_port_register_out(pmt::mp("telemetry"));
// Control messages to tracking block
this->message_port_register_out(pmt::mp("telemetry_to_trk"));
// initialize internal vars
d_dump = dump;
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite " << this->d_satellite;
LOG(INFO) << "Initializing BeiDou B3I Telemetry Decoding for satellite "
<< this->d_satellite;
d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS;
d_samples_per_symbol =
(BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) /
BEIDOU_D1NAV_SYMBOL_RATE_SPS;
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS;
d_samples_per_preamble =
BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol;
d_secondary_code_symbols = static_cast<int32_t *>(
volk_gnsssdr_malloc(BEIDOU_B3I_SECONDARY_CODE_LENGTH * sizeof(int32_t),
volk_gnsssdr_get_alignment()));
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(
d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_preamble_period_samples =
BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol;
// Setting samples of preamble code
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
// Setting samples of secondary code
for (int32_t i = 0; i < BEIDOU_B3I_SECONDARY_CODE_LENGTH; i++)
{
if (BEIDOU_DNAV_PREAMBLE.at(i) == '1')
if (BEIDOU_B3I_SECONDARY_CODE.at(i) == '1')
{
d_preamble_samples[i] = 1;
d_secondary_code_symbols[i] = 1;
}
else
{
d_preamble_samples[i] = -1;
d_secondary_code_symbols[i] = -1;
}
}
d_subframe_symbols = static_cast<float *>(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols);
// Setting samples of preamble code
int32_t n = 0;
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
{
int32_t m = 0;
if (BEIDOU_DNAV_PREAMBLE.at(i) == '1')
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = d_secondary_code_symbols[m];
n++;
m++;
m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH;
}
}
else
{
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = -d_secondary_code_symbols[m];
n++;
m++;
m = m % BEIDOU_B3I_SECONDARY_CODE_LENGTH;
}
}
}
d_subframe_symbols = static_cast<double *>(
volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double),
volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol +
d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols + 1);
d_last_valid_preamble = 0;
d_sent_tlm_failed_msg = false;
d_flag_valid_word = false;
// Generic settings
d_sample_counter = 0;
d_stat = 0;
@ -118,6 +156,7 @@ beidou_b3i_telemetry_decoder_gs::beidou_b3i_telemetry_decoder_gs(
beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs()
{
volk_gnsssdr_free(d_preamble_samples);
volk_gnsssdr_free(d_secondary_code_symbols);
volk_gnsssdr_free(d_subframe_symbols);
if (d_dump_file.is_open() == true)
@ -128,7 +167,8 @@ beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs()
}
catch (const std::exception &ex)
{
LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what();
LOG(WARNING) << "Exception in destructor closing the dump file "
<< ex.what();
}
}
}
@ -165,8 +205,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits,
void beidou_b3i_telemetry_decoder_gs::decode_word(
int32_t word_counter,
const float *enc_word_symbols,
int32_t word_counter, const double *enc_word_symbols,
int32_t *dec_word_symbols)
{
int32_t bitsbch[30], first_branch[15], second_branch[15];
@ -175,7 +214,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_word(
{
for (uint32_t j = 0; j < 30; j++)
{
dec_word_symbols[j] = static_cast<int32_t>(enc_word_symbols[j] > 0) ? (1) : (-1);
dec_word_symbols[j] =
static_cast<int32_t>(enc_word_symbols[j] > 0) ? (1) : (-1);
}
}
else
@ -184,7 +224,8 @@ void beidou_b3i_telemetry_decoder_gs::decode_word(
{
for (uint32_t c = 0; c < 15; c++)
{
bitsbch[r * 15 + c] = static_cast<int32_t>(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1);
bitsbch[r * 15 + c] =
static_cast<int32_t>(enc_word_symbols[c * 2 + r] > 0) ? (1) : (-1);
}
}
@ -206,7 +247,7 @@ void beidou_b3i_telemetry_decoder_gs::decode_word(
}
void beidou_b3i_telemetry_decoder_gs::decode_subframe(float *frame_symbols)
void beidou_b3i_telemetry_decoder_gs::decode_subframe(double *frame_symbols)
{
// 1. Transform from symbols to bits
std::string data_bits;
@ -315,62 +356,50 @@ void beidou_b3i_telemetry_decoder_gs::set_satellite(
{
// Clear values from previous declaration
volk_gnsssdr_free(d_preamble_samples);
volk_gnsssdr_free(d_secondary_code_symbols);
volk_gnsssdr_free(d_subframe_symbols);
d_samples_per_symbol =
(BEIDOU_B3I_CODE_RATE_HZ / BEIDOU_B3I_CODE_LENGTH_CHIPS) /
BEIDOU_D2NAV_SYMBOL_RATE_SPS;
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t),
volk_gnsssdr_get_alignment()));
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS;
d_samples_per_preamble =
BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS * d_samples_per_symbol;
d_secondary_code_symbols = nullptr;
d_preamble_samples = static_cast<int32_t *>(
volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t),
volk_gnsssdr_get_alignment()));
d_preamble_period_samples =
BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * d_samples_per_symbol;
// Setting samples of preamble code
int32_t n = 0;
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
{
if (BEIDOU_DNAV_PREAMBLE.at(i) == '1')
{
d_preamble_samples[i] = 1;
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = 1;
n++;
}
}
else
{
d_preamble_samples[i] = -1;
for (uint32_t j = 0; j < d_samples_per_symbol; j++)
{
d_preamble_samples[n] = -1;
n++;
}
}
}
d_symbol_duration_ms = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS;
d_subframe_symbols = static_cast<float *>(volk_gnsssdr_malloc(
BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float),
d_subframe_symbols = static_cast<double *>(volk_gnsssdr_malloc(
BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(double),
volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols);
}
else
{
// Clear values from previous declaration
volk_gnsssdr_free(d_preamble_samples);
volk_gnsssdr_free(d_subframe_symbols);
//back to normal satellites
d_symbol_duration_ms = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT * BEIDOU_B3I_CODE_PERIOD_MS;
d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS;
d_preamble_samples = static_cast<int32_t *>(volk_gnsssdr_malloc(d_samples_per_preamble * sizeof(int32_t), volk_gnsssdr_get_alignment()));
d_preamble_period_samples = BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS;
// Setting samples of preamble code
for (int32_t i = 0; i < d_symbols_per_preamble; i++)
{
if (BEIDOU_DNAV_PREAMBLE.at(i) == '1')
{
d_preamble_samples[i] = 1;
}
else
{
d_preamble_samples[i] = -1;
}
}
d_subframe_symbols = static_cast<float *>(volk_gnsssdr_malloc(BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS * sizeof(float), volk_gnsssdr_get_alignment()));
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS + d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols);
d_required_symbols = BEIDOU_DNAV_SUBFRAME_SYMBOLS * d_samples_per_symbol +
d_samples_per_preamble;
d_symbol_history.set_capacity(d_required_symbols + 1);
}
}
@ -405,15 +434,6 @@ void beidou_b3i_telemetry_decoder_gs::set_channel(int32_t channel)
}
}
void beidou_b3i_telemetry_decoder_gs::reset()
{
d_last_valid_preamble = d_sample_counter;
d_TOW_at_current_symbol_ms = 0;
d_sent_tlm_failed_msg = false;
d_flag_valid_word = false;
DLOG(INFO) << "Beidou B3I Telemetry decoder reset for satellite " << d_satellite;
return;
}
int beidou_b3i_telemetry_decoder_gs::general_work(
int noutput_items __attribute__((unused)),
@ -434,9 +454,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
d_symbol_history.push_back(current_symbol.Prompt_I); // add new symbol to the symbol queue
d_sample_counter++; // count for the processed samples
consume_each(1);
d_flag_preamble = false;
if (d_symbol_history.size() >= d_required_symbols)
if (d_symbol_history.size() > d_required_symbols)
{
//******* preamble correlation ********
for (int32_t i = 0; i < d_samples_per_preamble; i++)
@ -451,6 +472,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
}
}
}
//******* frame sync ******************
if (d_stat == 0) // no preamble information
{
@ -458,7 +480,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
{
// Record the preamble sample stamp
d_preamble_index = d_sample_counter;
DLOG(INFO) << "Preamble detection for BEIDOU B3I SAT " << this->d_satellite;
LOG(INFO) << "Preamble detection for BEIDOU B3I SAT "
<< this->d_satellite;
// Enter into frame pre-detection status
d_stat = 1;
}
@ -472,55 +495,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
if (abs(preamble_diff - d_preamble_period_samples) == 0)
{
// try to decode frame
DLOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT "
<< this->d_satellite;
LOG(INFO) << "Starting BeiDou DNAV frame decoding for BeiDou B3I SAT "
<< this->d_satellite;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
d_stat = 2;
// ******* SAMPLES TO SYMBOLS *******
if (corr_value > 0) //normal PLL lock
{
for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++)
{
d_subframe_symbols[i] = d_symbol_history.at(i);
}
}
else // 180 deg. inverted carrier phase PLL lock
{
for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++)
{
d_subframe_symbols[i] = -d_symbol_history.at(i);
}
}
// call the decoder
decode_subframe(d_subframe_symbols);
if (d_nav.flag_crc_test == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
DLOG(INFO) << "BeiDou DNAV frame sync found for SAT "
<< this->d_satellite;
}
}
else
{
d_CRC_error_counter++;
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
{
DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT "
<< this->d_satellite;
d_flag_frame_sync = false;
d_stat = 0;
flag_SOW_set = false;
}
}
}
else
{
@ -535,21 +513,62 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
}
else if (d_stat == 2) // preamble acquired
{
if (d_sample_counter == d_preamble_index + static_cast<uint64_t>(d_preamble_period_samples))
if (d_sample_counter ==
d_preamble_index + static_cast<uint64_t>(d_preamble_period_samples))
{
// ******* SAMPLES TO SYMBOLS *******
if (corr_value > 0) //normal PLL lock
//******* SAMPLES TO SYMBOLS *******
if (corr_value > 0) // normal PLL lock
{
int32_t k = 0;
for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++)
{
d_subframe_symbols[i] = d_symbol_history.at(i);
d_subframe_symbols[i] = 0;
// integrate samples into symbols
for (uint32_t m = 0; m < d_samples_per_symbol; m++)
{
if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6)
{
// because last symbol of the preamble is just received now!
d_subframe_symbols[i] +=
d_symbol_history.at(i * d_samples_per_symbol + m);
}
else
{
// because last symbol of the preamble is just received now!
d_subframe_symbols[i] +=
static_cast<float>(d_secondary_code_symbols[k]) *
d_symbol_history.at(i * d_samples_per_symbol + m);
k++;
k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH;
}
}
}
}
else // 180 deg. inverted carrier phase PLL lock
{
int32_t k = 0;
for (uint32_t i = 0; i < BEIDOU_DNAV_PREAMBLE_PERIOD_SYMBOLS; i++)
{
d_subframe_symbols[i] = -d_symbol_history.at(i);
d_subframe_symbols[i] = 0;
// integrate samples into symbols
for (uint32_t m = 0; m < d_samples_per_symbol; m++)
{
if (d_satellite.get_PRN() > 0 and d_satellite.get_PRN() < 6)
{
// because last symbol of the preamble is just received now!
d_subframe_symbols[i] -=
d_symbol_history.at(i * d_samples_per_symbol + m);
}
else
{
// because last symbol of the preamble is just received now!
d_subframe_symbols[i] -=
static_cast<float>(d_secondary_code_symbols[k]) *
d_symbol_history.at(i * d_samples_per_symbol + m);
k++;
k = k % BEIDOU_B3I_SECONDARY_CODE_LENGTH;
}
}
}
}
@ -559,8 +578,10 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
if (d_nav.flag_crc_test == true)
{
d_CRC_error_counter = 0;
d_flag_preamble = true; // valid preamble indicator (initialized to false every work())
d_preamble_index = d_sample_counter; // record the preamble sample stamp (t_P)
d_flag_preamble = true; // valid preamble indicator (initialized to
// false every work())
d_preamble_index =
d_sample_counter; // record the preamble sample stamp (t_P)
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
@ -574,8 +595,8 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
d_preamble_index = d_sample_counter; // record the preamble sample stamp
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
{
DLOG(INFO) << "BeiDou DNAV frame sync lost for SAT "
<< this->d_satellite;
LOG(INFO) << "BeiDou DNAV frame sync lost for SAT "
<< this->d_satellite;
d_flag_frame_sync = false;
d_stat = 0;
flag_SOW_set = false;
@ -583,6 +604,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
}
}
}
// UPDATE GNSS SYNCHRO DATA
// 2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_nav.flag_new_SOW_available == true)
@ -590,67 +612,55 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
{
// Reporting sow as gps time of week
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.d_SOW + 14) * 1000.0);
//check TOW update consistency
uint32_t last_d_TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
//compute new TOW
d_TOW_at_current_symbol_ms = d_TOW_at_Preamble_ms + d_required_symbols * d_symbol_duration_ms;
d_TOW_at_current_symbol_ms =
d_TOW_at_Preamble_ms + static_cast<uint32_t>((d_required_symbols + 1) *
BEIDOU_B3I_CODE_PERIOD_MS);
flag_SOW_set = true;
d_nav.flag_new_SOW_available = false;
}
else // if there is not a new preamble, we define the TOW of the current
// symbol
{
d_TOW_at_current_symbol_ms +=
static_cast<uint32_t>(BEIDOU_B3I_CODE_PERIOD_MS);
}
if (last_d_TOW_at_current_symbol_ms != 0 and abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > d_symbol_duration_ms)
{
LOG(INFO) << "Warning: BEIDOU B3I TOW update in ch " << d_channel
<< " does not match the TLM TOW counter " << static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms) << " ms \n";
d_TOW_at_current_symbol_ms = 0;
d_flag_valid_word = false;
}
else
{
d_last_valid_preamble = d_sample_counter;
d_flag_valid_word = true;
}
if (d_flag_frame_sync == true and flag_SOW_set == true)
{
current_symbol.Flag_valid_word = true;
}
else
{
if (d_flag_valid_word)
{
d_TOW_at_current_symbol_ms += d_symbol_duration_ms;
if (current_symbol.Flag_valid_symbol_output == false)
{
d_flag_valid_word = false;
}
}
current_symbol.Flag_valid_word = false;
}
if (d_flag_valid_word == true)
current_symbol.PRN = this->d_satellite.get_PRN();
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
if (d_dump == true)
{
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
current_symbol.Flag_valid_word = d_flag_valid_word;
if (d_dump == true)
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
// MULTIPLEXED FILE RECORDING - Record results to file
try
{
double tmp_double;
uint64_t tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
tmp_double = static_cast<double>(d_TOW_at_Preamble_ms) / 1000.0;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing Telemetry GPS L5 dump file " << e.what();
}
double tmp_double;
uint64_t tmp_ulong_int;
tmp_double = static_cast<double>(d_TOW_at_current_symbol_ms);
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = current_symbol.Tracking_sample_counter;
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
tmp_double = d_nav.d_SOW;
d_dump_file.write(reinterpret_cast<char *>(&tmp_double), sizeof(double));
tmp_ulong_int = static_cast<uint64_t>(d_required_symbols);
d_dump_file.write(reinterpret_cast<char *>(&tmp_ulong_int), sizeof(uint64_t));
}
catch (const std::ifstream::failure &e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
}
return 0;
// 3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_symbol;
return 1;
}

21
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h
View File

@ -60,8 +60,10 @@ public:
~beidou_b3i_telemetry_decoder_gs(); //!< Class destructor
void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN
void set_channel(int channel); //!< Set receiver's channel
void reset();
inline void reset()
{
return;
}
/*!
* \brief This is where all signal processing takes place
*/
@ -75,24 +77,27 @@ private:
bool dump);
beidou_b3i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump);
void decode_subframe(float *symbols);
void decode_word(int32_t word_counter, const float *enc_word_symbols,
void decode_subframe(double *symbols);
void decode_word(int32_t word_counter, const double *enc_word_symbols,
int32_t *dec_word_symbols);
void decode_bch15_11_01(const int32_t *bits, int32_t *decbits);
// Preamble decoding
int32_t *d_preamble_samples;
int32_t *d_secondary_code_symbols;
uint32_t d_samples_per_symbol;
int32_t d_symbols_per_preamble;
int32_t d_samples_per_preamble;
int32_t d_preamble_period_samples;
float *d_subframe_symbols;
double *d_subframe_symbols;
uint32_t d_required_symbols;
// Storage for incoming data
boost::circular_buffer<float> d_symbol_history;
// Variables for internal functionality
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc) indicating number of samples processed
uint64_t d_sample_counter; // Sample counter as an index (1,2,3,..etc)
// indicating number of samples processed
uint64_t d_preamble_index; // Index of sample number where preamble was found
uint32_t d_stat; // Status of decoder
bool d_flag_frame_sync; // Indicate when a frame sync is achieved
@ -104,12 +109,8 @@ private:
Beidou_Dnav_Navigation_Message d_nav;
// Values to populate gnss synchronization structure
uint32_t d_symbol_duration_ms;
uint32_t d_TOW_at_Preamble_ms;
uint32_t d_TOW_at_current_symbol_ms;
uint64_t d_last_valid_preamble;
bool d_flag_valid_word;
bool d_sent_tlm_failed_msg;
bool Flag_valid_word;
// Satellite Information and logging capacity

67
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc
View File

@ -286,16 +286,17 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
d_code_period = BEIDOU_B1I_CODE_PERIOD;
d_code_chip_rate = BEIDOU_B1I_CODE_RATE_HZ;
d_code_length_chips = static_cast<uint32_t>(BEIDOU_B1I_CODE_LENGTH_CHIPS);
d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;
//d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_secondary = true;
d_secondary = false;
trk_parameters.track_pilot = false;
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
}
else if (signal_type == "B3")
{
@ -304,15 +305,16 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
d_code_period = BEIDOU_B3I_CODE_PERIOD;
d_code_chip_rate = BEIDOU_B3I_CODE_RATE_HZ;
d_code_length_chips = static_cast<uint32_t>(BEIDOU_B3I_CODE_LENGTH_CHIPS);
d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT;
//d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_secondary = true;
d_secondary = false;
trk_parameters.track_pilot = false;
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
}
else
{
@ -649,7 +651,8 @@ void dll_pll_veml_tracking::start_tracking()
// GEO Satellites use different secondary code
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT;
//d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_secondary = false;
@ -662,16 +665,17 @@ void dll_pll_veml_tracking::start_tracking()
}
else
{
d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;
//d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_secondary = true;
d_secondary = false;
trk_parameters.track_pilot = false;
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B1I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_SECONDARY_CODE_STR);
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
}
}
@ -682,7 +686,8 @@ void dll_pll_veml_tracking::start_tracking()
// Update secondary code settings for geo satellites
if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6)
{
d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT;
//d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT;//todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_secondary = false;
@ -695,16 +700,17 @@ void dll_pll_veml_tracking::start_tracking()
}
else
{
d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT;
//d_symbols_per_bit = BEIDOU_B3I_TELEMETRY_SYMBOLS_PER_BIT; //todo: enable after fixing beidou symbol synchronization
d_symbols_per_bit = 1;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_secondary = true;
d_secondary = false;
trk_parameters.track_pilot = false;
// synchronize and remove data secondary code
d_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
//d_data_secondary_code_length = static_cast<uint32_t>(BEIDOU_B3I_SECONDARY_CODE_LENGTH);
//d_data_secondary_code_string = const_cast<std::string *>(&BEIDOU_B3I_SECONDARY_CODE_STR);
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
}
}
@ -1002,21 +1008,24 @@ void dll_pll_veml_tracking::run_dll_pll()
d_code_freq_chips = (1.0 + (d_carrier_doppler_hz / d_signal_carrier_freq)) * d_code_chip_rate - d_code_error_filt_chips;
// Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler
if (d_pull_in_transitory == false and d_corrected_doppler == false)
if (trk_parameters.enable_doppler_correction == true)
{
d_dll_filt_history.push_back(static_cast<float>(d_code_error_filt_chips));
if (d_dll_filt_history.full())
if (d_pull_in_transitory == false and d_corrected_doppler == false)
{
float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast<float>(d_dll_filt_history.capacity());
if (fabs(avg_code_error_chips_s) > 0.10)
d_dll_filt_history.push_back(static_cast<float>(d_code_error_filt_chips));
if (d_dll_filt_history.full())
{
float carrier_doppler_error_hz = static_cast<float>(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast<float>(d_code_chip_rate);
LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN);
d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz);
d_corrected_doppler = true;
float avg_code_error_chips_s = std::accumulate(d_dll_filt_history.begin(), d_dll_filt_history.end(), 0.0) / static_cast<float>(d_dll_filt_history.capacity());
if (fabs(avg_code_error_chips_s) > 1.0)
{
float carrier_doppler_error_hz = static_cast<float>(d_signal_carrier_freq) * avg_code_error_chips_s / static_cast<float>(d_code_chip_rate);
LOG(INFO) << "Detected and corrected carrier doppler error: " << carrier_doppler_error_hz << " [Hz] on sat " << Gnss_Satellite(systemName, d_acquisition_gnss_synchro->PRN);
d_carrier_loop_filter.initialize(d_carrier_doppler_hz - carrier_doppler_error_hz);
d_corrected_doppler = true;
}
d_dll_filt_history.clear();
}
d_dll_filt_history.clear();
}
}
}

1
src/algorithms/tracking/libs/dll_pll_conf.cc
View File

@ -69,6 +69,7 @@ Dll_Pll_Conf::Dll_Pll_Conf()
max_carrier_lock_fail = FLAGS_max_carrier_lock_fail;
max_code_lock_fail = FLAGS_max_lock_fail;
carrier_lock_th = FLAGS_carrier_lock_th;
enable_doppler_correction = false;
track_pilot = false;
system = 'G';
char sig_[3] = "1C";

1
src/algorithms/tracking/libs/dll_pll_conf.h
View File

@ -75,6 +75,7 @@ public:
uint32_t smoother_length;
double carrier_lock_th;
bool track_pilot;
bool enable_doppler_correction;
char system;
char signal[3]{};