mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2026-08-26 03:58:54 +00:00
Code Issues Releases Wiki Activity

Improve handling of TOW rollover for Glonass and BeiDou signals

Browse Source
This commit is contained in:
Carles Fernandez
2026-06-10 11:35:25 +02:00
parent 51c84f328a
commit d5a7e2ec99
10 changed files with 152 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc
+34 -5
View File
@@ -128,6 +128,33 @@ beidou_b1i_telemetry_decoder_gs::~beidou_b1i_telemetry_decoder_gs()
}
uint32_t beidou_b1i_telemetry_decoder_gs::gps_week_ms()
{
return static_cast<uint32_t>(BEIDOU_DNAV_SECONDS_PER_WEEK * 1000.0);
}
uint64_t beidou_b1i_telemetry_decoder_gs::beidou_sow_to_gps_tow_ms(double sow_s)
{
return static_cast<uint64_t>((sow_s + static_cast<double>(BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET)) * 1000.0);
}
uint32_t beidou_b1i_telemetry_decoder_gs::wrap_gps_tow_ms(uint64_t tow_ms)
{
return static_cast<uint32_t>(tow_ms % gps_week_ms());
}
uint32_t beidou_b1i_telemetry_decoder_gs::circular_gps_tow_error_ms(uint32_t lhs_ms, uint32_t rhs_ms)
{
const uint32_t week_ms = gps_week_ms();
const uint32_t forward_error_ms = lhs_ms >= rhs_ms ? lhs_ms - rhs_ms : lhs_ms + week_ms - rhs_ms;
const uint32_t reverse_error_ms = rhs_ms >= lhs_ms ? rhs_ms - lhs_ms : rhs_ms + week_ms - lhs_ms;
return forward_error_ms < reverse_error_ms ? forward_error_ms : reverse_error_ms;
}
void beidou_b1i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits)
{
int32_t bit;
@@ -572,18 +599,20 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
// update TOW at the preamble instant
{
// Reporting sow as gps time of week
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.get_SOW() + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0);
const uint64_t decoded_tow_ms = beidou_sow_to_gps_tow_ms(d_nav.get_SOW());
d_TOW_at_Preamble_ms = wrap_gps_tow_ms(decoded_tow_ms);
// check TOW update consistency
const 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 = wrap_gps_tow_ms(decoded_tow_ms + static_cast<uint64_t>(d_required_symbols) * d_symbol_duration_ms);
d_flag_SOW_set = true;
d_nav.set_flag_new_SOW_available(false);
if (last_d_TOW_at_current_symbol_ms != 0 && abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > static_cast<int64_t>(d_symbol_duration_ms))
const uint32_t tow_update_error_ms = circular_gps_tow_error_ms(d_TOW_at_current_symbol_ms, last_d_TOW_at_current_symbol_ms);
if (last_d_TOW_at_current_symbol_ms != 0 && tow_update_error_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";
<< " does not match the TLM TOW counter " << tow_update_error_ms << " ms \n";
d_TOW_at_current_symbol_ms = 0;
d_flag_valid_word = false;
@@ -607,7 +636,7 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_
{
if (d_flag_valid_word)
{
d_TOW_at_current_symbol_ms += d_symbol_duration_ms;
d_TOW_at_current_symbol_ms = wrap_gps_tow_ms(static_cast<uint64_t>(d_TOW_at_current_symbol_ms) + d_symbol_duration_ms);
if (current_symbol.Flag_valid_symbol_output == false)
{
d_flag_valid_word = false;
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h
+5
View File
@@ -24,6 +24,7 @@
#include "nav_message_packet.h"
#include "telemetry_impl_interface.h"
#include <boost/circular_buffer.hpp>
#include <cstdint>
/** \addtogroup Telemetry_Decoder
@@ -66,6 +67,10 @@ private:
void decode_subframe(float *symbols, double cn0);
void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols);
void decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits);
static uint32_t gps_week_ms();
static uint64_t beidou_sow_to_gps_tow_ms(double sow_s);
static uint32_t wrap_gps_tow_ms(uint64_t tow_ms);
static uint32_t circular_gps_tow_error_ms(uint32_t lhs_ms, uint32_t rhs_ms);
// Preamble decoding
std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{};
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc
+34 -5
View File
@@ -124,6 +124,33 @@ beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs()
}
uint32_t beidou_b3i_telemetry_decoder_gs::gps_week_ms()
{
return static_cast<uint32_t>(BEIDOU_DNAV_SECONDS_PER_WEEK * 1000.0);
}
uint64_t beidou_b3i_telemetry_decoder_gs::beidou_sow_to_gps_tow_ms(double sow_s)
{
return static_cast<uint64_t>((sow_s + static_cast<double>(BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET)) * 1000.0);
}
uint32_t beidou_b3i_telemetry_decoder_gs::wrap_gps_tow_ms(uint64_t tow_ms)
{
return static_cast<uint32_t>(tow_ms % gps_week_ms());
}
uint32_t beidou_b3i_telemetry_decoder_gs::circular_gps_tow_error_ms(uint32_t lhs_ms, uint32_t rhs_ms)
{
const uint32_t week_ms = gps_week_ms();
const uint32_t forward_error_ms = lhs_ms >= rhs_ms ? lhs_ms - rhs_ms : lhs_ms + week_ms - rhs_ms;
const uint32_t reverse_error_ms = rhs_ms >= lhs_ms ? rhs_ms - lhs_ms : rhs_ms + week_ms - lhs_ms;
return forward_error_ms < reverse_error_ms ? forward_error_ms : reverse_error_ms;
}
void beidou_b3i_telemetry_decoder_gs::decode_bch15_11_01(const int32_t *bits,
std::array<int32_t, 15> &decbits)
{
@@ -588,18 +615,20 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
// update TOW at the preamble instant
{
// Reporting sow as gps time of week
d_TOW_at_Preamble_ms = static_cast<uint32_t>((d_nav.get_SOW() + BEIDOU_DNAV_BDT2GPST_LEAP_SEC_OFFSET) * 1000.0);
const uint64_t decoded_tow_ms = beidou_sow_to_gps_tow_ms(d_nav.get_SOW());
d_TOW_at_Preamble_ms = wrap_gps_tow_ms(decoded_tow_ms);
// check TOW update consistency
const 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 = wrap_gps_tow_ms(decoded_tow_ms + static_cast<uint64_t>(d_required_symbols) * d_symbol_duration_ms);
d_flag_SOW_set = true;
d_nav.set_flag_new_SOW_available(false);
if (last_d_TOW_at_current_symbol_ms != 0 && abs(static_cast<int64_t>(d_TOW_at_current_symbol_ms) - int64_t(last_d_TOW_at_current_symbol_ms)) > static_cast<int64_t>(d_symbol_duration_ms))
const uint32_t tow_update_error_ms = circular_gps_tow_error_ms(d_TOW_at_current_symbol_ms, last_d_TOW_at_current_symbol_ms);
if (last_d_TOW_at_current_symbol_ms != 0 && tow_update_error_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";
<< " does not match the TLM TOW counter " << tow_update_error_ms << " ms \n";
d_TOW_at_current_symbol_ms = 0;
d_flag_valid_word = false;
@@ -623,7 +652,7 @@ int beidou_b3i_telemetry_decoder_gs::general_work(
{
if (d_flag_valid_word)
{
d_TOW_at_current_symbol_ms += d_symbol_duration_ms;
d_TOW_at_current_symbol_ms = wrap_gps_tow_ms(static_cast<uint64_t>(d_TOW_at_current_symbol_ms) + d_symbol_duration_ms);
if (current_symbol.Flag_valid_symbol_output == false)
{
d_flag_valid_word = false;
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h
+5
View File
@@ -21,6 +21,7 @@
#include "nav_message_packet.h"
#include "telemetry_impl_interface.h"
#include <boost/circular_buffer.hpp>
#include <cstdint>
/** \addtogroup Telemetry_Decoder
@@ -62,6 +63,10 @@ private:
void decode_word(int32_t word_counter, const float *enc_word_symbols,
int32_t *dec_word_symbols);
void decode_bch15_11_01(const int32_t *bits, std::array<int32_t, 15> &decbits);
static uint32_t gps_week_ms();
static uint64_t beidou_sow_to_gps_tow_ms(double sow_s);
static uint32_t wrap_gps_tow_ms(uint64_t tow_ms);
static uint32_t circular_gps_tow_error_ms(uint32_t lhs_ms, uint32_t rhs_ms);
// Preamble decoding
std::array<int32_t, BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS> d_preamble_samples{};
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc
+26 -3
View File
@@ -106,6 +106,29 @@ glonass_l1_ca_telemetry_decoder_gs::~glonass_l1_ca_telemetry_decoder_gs()
}
double glonass_l1_ca_telemetry_decoder_gs::gps_week_s()
{
return GLONASS_GNAV_SECONDS_PER_WEEK;
}
double glonass_l1_ca_telemetry_decoder_gs::wrap_gps_tow_s(double tow_s)
{
double wrapped_tow_s = std::fmod(tow_s, gps_week_s());
if (wrapped_tow_s < 0.0)
{
wrapped_tow_s += gps_week_s();
}
return wrapped_tow_s;
}
uint32_t glonass_l1_ca_telemetry_decoder_gs::wrap_gps_tow_ms(double tow_s)
{
return static_cast<uint32_t>(std::round(wrap_gps_tow_s(tow_s) * 1000.0)) % GLONASS_GNAV_MILLISECONDS_PER_WEEK;
}
void glonass_l1_ca_telemetry_decoder_gs::decode_string(const double *frame_symbols, double cn0)
{
// 1. Transform from symbols to bits
@@ -357,12 +380,12 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
if (this->d_flag_preamble == true && d_nav.get_flag_TOW_new() == true)
// update TOW at the preamble instant
{
d_TOW_at_current_symbol = floor((d_nav.get_ephemeris().d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000;
d_TOW_at_current_symbol = wrap_gps_tow_s(floor((d_nav.get_ephemeris().d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000);
d_nav.set_flag_TOW_new(false);
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L1_CA_SYMBOL_PERIOD_S;
d_TOW_at_current_symbol = wrap_gps_tow_s(d_TOW_at_current_symbol + GLONASS_L1_CA_SYMBOL_PERIOD_S);
}
// if (d_flag_frame_sync == true && d_nav.flag_TOW_set==true && d_nav.get_flag_CRC_test() == true)
@@ -373,7 +396,7 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
// }
current_symbol.PRN = this->d_satellite.get_PRN();
current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_symbol.TOW_at_current_symbol_ms = wrap_gps_tow_ms(d_TOW_at_current_symbol);
if (d_flag_frame_sync == true && d_nav.is_flag_TOW_set() == true)
{
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h
+4
View File
@@ -25,6 +25,7 @@
#include "nav_message_packet.h"
#include "telemetry_impl_interface.h"
#include <boost/circular_buffer.hpp>
#include <cstdint>
/** \addtogroup Telemetry_Decoder
* \{ */
@@ -65,6 +66,9 @@ private:
const std::array<int16_t, GLONASS_GNAV_PREAMBLE_LENGTH_BITS> d_preambles_bits{GLONASS_GNAV_PREAMBLE_SAMPLES};
void decode_string(const double *symbols, double cn0);
static double gps_week_s();
static double wrap_gps_tow_s(double tow_s);
static uint32_t wrap_gps_tow_ms(double tow_s);
// Storage for incoming data
boost::circular_buffer<Gnss_Synchro> d_symbol_history;
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc
+26 -3
View File
@@ -106,6 +106,29 @@ glonass_l2_ca_telemetry_decoder_gs::~glonass_l2_ca_telemetry_decoder_gs()
}
double glonass_l2_ca_telemetry_decoder_gs::gps_week_s()
{
return GLONASS_GNAV_SECONDS_PER_WEEK;
}
double glonass_l2_ca_telemetry_decoder_gs::wrap_gps_tow_s(double tow_s)
{
double wrapped_tow_s = std::fmod(tow_s, gps_week_s());
if (wrapped_tow_s < 0.0)
{
wrapped_tow_s += gps_week_s();
}
return wrapped_tow_s;
}
uint32_t glonass_l2_ca_telemetry_decoder_gs::wrap_gps_tow_ms(double tow_s)
{
return static_cast<uint32_t>(std::round(wrap_gps_tow_s(tow_s) * 1000.0)) % GLONASS_GNAV_MILLISECONDS_PER_WEEK;
}
void glonass_l2_ca_telemetry_decoder_gs::decode_string(const double *frame_symbols, double cn0)
{
// 1. Transform from symbols to bits
@@ -355,12 +378,12 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
if (this->d_flag_preamble == true && d_nav.get_flag_TOW_new() == true)
// update TOW at the preamble instant
{
d_TOW_at_current_symbol = floor((d_nav.get_ephemeris().d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000;
d_TOW_at_current_symbol = wrap_gps_tow_s(floor((d_nav.get_ephemeris().d_TOW - GLONASS_GNAV_PREAMBLE_DURATION_S) * 1000) / 1000);
d_nav.set_flag_TOW_new(false);
}
else // if there is not a new preamble, we define the TOW of the current symbol
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GLONASS_L2_CA_SYMBOL_PERIOD_S;
d_TOW_at_current_symbol = wrap_gps_tow_s(d_TOW_at_current_symbol + GLONASS_L2_CA_SYMBOL_PERIOD_S);
}
// if (d_flag_frame_sync == true && d_nav.flag_TOW_set==true && d_nav.get_flag_CRC_test() == true)
@@ -371,7 +394,7 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu
// }
current_symbol.PRN = this->d_satellite.get_PRN();
current_symbol.TOW_at_current_symbol_ms = round(d_TOW_at_current_symbol * 1000.0);
current_symbol.TOW_at_current_symbol_ms = wrap_gps_tow_ms(d_TOW_at_current_symbol);
if (d_flag_frame_sync == true && d_nav.is_flag_TOW_set() == true)
{
src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h
+4
View File
@@ -24,6 +24,7 @@
#include "nav_message_packet.h"
#include "telemetry_impl_interface.h"
#include <boost/circular_buffer.hpp>
#include <cstdint>
/** \addtogroup Telemetry_Decoder
* \{ */
@@ -63,6 +64,9 @@ private:
const std::array<int16_t, GLONASS_GNAV_PREAMBLE_LENGTH_BITS> d_preambles_bits{GLONASS_GNAV_PREAMBLE_SAMPLES};
void decode_string(const double *symbols, double cn0);
static double gps_week_s();
static double wrap_gps_tow_s(double tow_s);
static uint32_t wrap_gps_tow_ms(double tow_s);
// Storage for incoming data
boost::circular_buffer<Gnss_Synchro> d_symbol_history;
src/core/system_parameters/GLONASS_L1_L2_CA.h
+2
View File
@@ -101,6 +101,8 @@ constexpr int32_t GLONASS_L1_CA_HISTORY_DEEP = 100;
constexpr char GLONASS_GNAV_PREAMBLE_STR[301] = "111111111111111111111111111111111111111111111111110000000000000000000000000000001111111111111111111100000000001111111111111111111111111111110000000000111111111100000000001111111111000000000000000000000000000000000000000011111111110000000000000000000011111111110000000000111111111111111111110000000000";
constexpr double GLONASS_GNAV_PREAMBLE_DURATION_S = 0.300;
constexpr double GLONASS_GNAV_SECONDS_PER_WEEK = 604800.0;
constexpr uint32_t GLONASS_GNAV_MILLISECONDS_PER_WEEK = 604800000U;
constexpr int32_t GLONASS_GNAV_PREAMBLE_LENGTH_BITS = 30;
constexpr int32_t GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS = 300;
constexpr int32_t GLONASS_GNAV_PREAMBLE_PERIOD_SYMBOLS = 2000;
src/core/system_parameters/glonass_gnav_ephemeris.cc
+12 -2
View File
@@ -144,12 +144,22 @@ void Glonass_Gnav_Ephemeris::glot_to_gpst(double tod_offset, double glot2utc_cor
total_sec = days * 86400 + sec_of_day;
// Compute Week number
*wn = floor(total_sec / 604800);
*wn = floor(total_sec / GLONASS_GNAV_SECONDS_PER_WEEK);
// Compute the arithmetic modules to wrap around range
*tow = total_sec - 604800 * floor(total_sec / 604800);
*tow = total_sec - GLONASS_GNAV_SECONDS_PER_WEEK * floor(total_sec / GLONASS_GNAV_SECONDS_PER_WEEK);
// Perform corrections from fractional seconds
*tow += glot2utc_corr + glot2gpst_corr;
while (*tow >= GLONASS_GNAV_SECONDS_PER_WEEK)
{
*tow -= GLONASS_GNAV_SECONDS_PER_WEEK;
++(*wn);
}
while (*tow < 0.0)
{
*tow += GLONASS_GNAV_SECONDS_PER_WEEK;
--(*wn);
}
}