mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-26 12:04:55 +00:00
Code Issues Releases Wiki Activity

tracking working

Browse Source
This commit is contained in:
Sergi Segura
2018-08-06 13:52:08 +02:00
parent 51aa80aa46
commit e4a86173fa
28 changed files with 1743 additions and 436 deletions
Download Patch File Download Diff File Expand all files Collapse all files

179
src/algorithms/libs/NHbeidou.cc Normal file
View File

@@ -0,0 +1,179 @@
void decodeBCHBeidou(std::list<bool> *firstBranch_encoded, std::list<int> *output)
{
bool input[15];
std::copy(firstBranch_encoded.begin(),firstBranch_encoded.end(),input);
std::array<bool, 4> D_register = {0,0,0,0};
std::array<bool, 15> stage_buffer;
std::array<bool, 15> ROM_list_circuit;
for (i = 0; i < 15; i++)
{
D_register = {inputBit[i] ^ D_register[3], D_register[0] ^ D_register[3], D_register[1], D_register[2]};
stage_buffer[i] = inputBit[i];
}
if(D_register == {0,0,0,0}) {
ROM_list_circuit = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
} else if(D_register == {0,0,0,1}) {
ROM_list_circuit = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,1};
} else if(D_register == {0,0,1,0}) {
ROM_list_circuit = {0,0,0,0,0,0,0,0,0,0,0,0,0,1,0};
} else if(D_register == {0,0,1,1}) {
ROM_list_circuit = {0,0,0,0,0,0,0,0,0,0,1,0,0,0,0};
} else if(D_register == {0,1,0,0}) {
ROM_list_circuit = {0,0,0,0,0,0,0,0,0,0,0,0,1,0,0};
} else if(D_register == {0,1,0,1}) {
ROM_list_circuit = {0,0,0,0,0,0,1,0,0,0,0,0,0,0,0};
} else if(D_register == {0,1,1,0}) {
ROM_list_circuit = {0,0,0,0,0,0,0,0,0,1,0,0,0,0,0};
} else if(D_register == {0,1,1,1}) {
ROM_list_circuit = {0,0,0,0,1,0,0,0,0,0,0,0,0,0,0};
} else if(D_register == {1,0,0,0}) {
ROM_list_circuit = {0,0,0,0,0,0,0,0,0,0,0,1,0,0,0};
} else if(D_register == {1,0,0,1}) {
ROM_list_circuit = {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
} else if(D_register == {1,0,1,0}) {
ROM_list_circuit = {0,0,0,0,0,1,0,0,0,0,0,0,0,0,0};
} else if(D_register == {1,0,1,1}) {
ROM_list_circuit = {0,0,0,0,0,0,0,1,0,0,0,0,0,0,0};
} else if(D_register == {1,1,0,0}) {
ROM_list_circuit = {0,0,0,0,0,0,0,0,1,0,0,0,0,0,0};
} else if(D_register == {1,1,0,1}) {
ROM_list_circuit = {0,1,0,0,0,0,0,0,0,0,0,0,0,0,0};
} else if(D_register == {1,1,1,0}) {
ROM_list_circuit = {0,0,0,1,0,0,0,0,0,0,0,0,0,0,0};
} else if(D_register == {1,1,1,1}) {
ROM_list_circuit = {0,0,1,0,0,0,0,0,0,0,0,0,0,0,0};
}
for (i = 0; i < 15; i++)
{
if(stage_buffer[i] ^ ROM_list_circuit[i])
{
output.push_back(1);
}
else
{
output.push_back(-1);
}
}
}
void remove_NH_Beidou(bool *input, bool *output)
{
int *code_NH_Beidou = {-1,-1,-1,-1,-1,+1,-1,-1,+1,+1,-1,+1,-1,+1,-1,-1,+1,+1,+1,-1};
int corr_NH = 0;
int correlation = 0;
for (int i = 0; i < 20; i++)
{
if ((code_NH_Beidou[i] * input[i]) > 0.0)
{
corr_NH += 1;
}
else
{
corr_NH -= 1;
}
}
if (abs(corr_NH) == 20)
{
correlation = 1;
if (corr_NH > 0)
{
output = 1;
}
else
{
output = 0;
}
}
}
void process_TRK_output_Beidou(bool *input, int n_input_bits, int *output)
{
bool buffer_NH[15];
bool new_bit;
std::list<bool> firstBranch_encoded;
std::list<int> firstBranch_decoded;
std::list<bool> secondBranch_encoded;
std::list<int> secondBranch_decoded;
std::list<int> output_list;
for (int i = 0; i < n_input_bits/15; i++)
{
for (int j = 0; j < 15; i++)
{
buffer_NH[j] = input[i + j];
}
remove_NH_Beidou(buffer_NH, &new_bit)
if ( i % 2 == 0 )
{
firstBranch_encoded.push_back(new_bit);
}
else
{
secondBranch_encoded.push_back(new_bit);
}
if (firstBranch_encoded.size() == 15)
{
decodeBCHBeidou(&firstBranch_encoded, &firstBranch_decoded);
firstBranch_encoded.clear();
}
if (secondBranch_encoded.size() == 15)
{
decodeBCHBeidou(&secondBranch_encoded, &secondBranch_decoded);
secondBranch_encoded.clear();
}
if (firstBranch_decoded.size() > 10)
{
for (i = 0; i < 11; i++)
{
output_list.push_back(firstBranch_decoded.front());
firstBranch_decoded.pop_front();
}
}
if (secondBranch_decoded.size() > 10)
{
for (i = 0; i < 11; i++)
{
output_list.push_back(secondBranch_decoded.front());
secondBranch_decoded.pop_front();
}
}
}
std::copy(output_list.begin(),output_list.end(),output);
}

26
src/algorithms/telemetry_decoder/adapters/beidou_b1i_telemetry_decoder.cc
View File

@@ -30,19 +30,19 @@
*/
#include "gps_l1_ca_telemetry_decoder.h"
#include "beidou_b1i_telemetry_decoder.h"
#include "configuration_interface.h"
#include "gps_ephemeris.h"
#include "gps_almanac.h"
#include "gps_iono.h"
#include "gps_utc_model.h"
#include "beidou_ephemeris.h"
#include "beidou_almanac.h"
#include "beidou_iono.h"
#include "beidou_utc_model.h"
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
using google::LogMessage;
GpsL1CaTelemetryDecoder::GpsL1CaTelemetryDecoder(ConfigurationInterface* configuration,
BeidouB1iTelemetryDecoder::BeidouB1iTelemetryDecoder(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams) : role_(role),
@@ -54,7 +54,7 @@ GpsL1CaTelemetryDecoder::GpsL1CaTelemetryDecoder(ConfigurationInterface* configu
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
// make telemetry decoder object
telemetry_decoder_ = gps_l1_ca_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me
telemetry_decoder_ = beidou_b1i_make_telemetry_decoder_cc(satellite_, dump_); // TODO fix me
DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")";
channel_ = 0;
if (in_streams_ > 1)
@@ -68,12 +68,12 @@ GpsL1CaTelemetryDecoder::GpsL1CaTelemetryDecoder(ConfigurationInterface* configu
}
GpsL1CaTelemetryDecoder::~GpsL1CaTelemetryDecoder()
BeidouB1iTelemetryDecoder::~BeidouB1iTelemetryDecoder()
{
}
void GpsL1CaTelemetryDecoder::set_satellite(const Gnss_Satellite& satellite)
void BeidouB1iTelemetryDecoder::set_satellite(const Gnss_Satellite& satellite)
{
satellite_ = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
telemetry_decoder_->set_satellite(satellite_);
@@ -81,7 +81,7 @@ void GpsL1CaTelemetryDecoder::set_satellite(const Gnss_Satellite& satellite)
}
void GpsL1CaTelemetryDecoder::connect(gr::top_block_sptr top_block)
void BeidouB1iTelemetryDecoder::connect(gr::top_block_sptr top_block)
{
if (top_block)
{ /* top_block is not null */
@@ -91,7 +91,7 @@ void GpsL1CaTelemetryDecoder::connect(gr::top_block_sptr top_block)
}
void GpsL1CaTelemetryDecoder::disconnect(gr::top_block_sptr top_block)
void BeidouB1iTelemetryDecoder::disconnect(gr::top_block_sptr top_block)
{
if (top_block)
{ /* top_block is not null */
@@ -100,13 +100,13 @@ void GpsL1CaTelemetryDecoder::disconnect(gr::top_block_sptr top_block)
}
gr::basic_block_sptr GpsL1CaTelemetryDecoder::get_left_block()
gr::basic_block_sptr BeidouB1iTelemetryDecoder::get_left_block()
{
return telemetry_decoder_;
}
gr::basic_block_sptr GpsL1CaTelemetryDecoder::get_right_block()
gr::basic_block_sptr BeidouB1iTelemetryDecoder::get_right_block()
{
return telemetry_decoder_;
}

14
src/algorithms/telemetry_decoder/adapters/beidou_b1i_telemetry_decoder.h
View File

@@ -30,10 +30,10 @@
*/
#ifndef GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_H_
#define GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_H_
#ifndef GNSS_SDR_BEIDOU_B1I_TELEMETRY_DECODER_H_
#define GNSS_SDR_BEIDOU_B1I_TELEMETRY_DECODER_H_
#include "gps_l1_ca_telemetry_decoder_cc.h"
#include "beidou_b1i_telemetry_decoder_cc.h"
#include "telemetry_decoder_interface.h"
#include <string>
@@ -42,15 +42,15 @@ class ConfigurationInterface;
/*!
* \brief This class implements a NAV data decoder for GPS L1 C/A
*/
class GpsL1CaTelemetryDecoder : public TelemetryDecoderInterface
class BeidouB1iTelemetryDecoder : public TelemetryDecoderInterface
{
public:
GpsL1CaTelemetryDecoder(ConfigurationInterface* configuration,
BeidouB1iTelemetryDecoder(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams);
virtual ~GpsL1CaTelemetryDecoder();
virtual ~BeidouB1iTelemetryDecoder();
inline std::string role() override
{
@@ -82,7 +82,7 @@ public:
}
private:
gps_l1_ca_telemetry_decoder_cc_sptr telemetry_decoder_;
beidou_b1i_telemetry_decoder_cc_sptr telemetry_decoder_;
Gnss_Satellite satellite_;
int channel_;
bool dump_;

395
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.cc
View File

@@ -1,8 +1,8 @@
/*!
* \file gps_l1_ca_telemetry_decoder_cc.cc
* \file beidou_b1i_telemetry_decoder_cc.cc
* \brief Implementation of a NAV message demodulator block based on
* Kay Borre book MATLAB-based GPS receiver
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.es
*
* -------------------------------------------------------------------------
*
@@ -29,7 +29,7 @@
* -------------------------------------------------------------------------
*/
#include "gps_l1_ca_telemetry_decoder_cc.h"
#include "beidou_b1i_telemetry_decoder_cc.h"
#include "control_message_factory.h"
#include <boost/lexical_cast.hpp>
#include <glog/logging.h>
@@ -43,18 +43,19 @@
using google::LogMessage;
gps_l1_ca_telemetry_decoder_cc_sptr
gps_l1_ca_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump)
beidou_b1i_telemetry_decoder_cc_sptr
beidou_b1i_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump)
{
return gps_l1_ca_telemetry_decoder_cc_sptr(new gps_l1_ca_telemetry_decoder_cc(satellite, dump));
return beidou_b1i_telemetry_decoder_cc_sptr(new beidou_b1i_telemetry_decoder_cc(satellite, dump));
}
gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
beidou_b1i_telemetry_decoder_cc::beidou_b1i_telemetry_decoder_cc(
const Gnss_Satellite &satellite,
bool dump) : gr::block("gps_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
bool dump) : gr::block("beidou_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
{
// Ephemeris data port out
this->message_port_register_out(pmt::mp("telemetry"));
// initialize internal vars
@@ -62,15 +63,14 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
// set the preamble
unsigned short int preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE;
unsigned short int preambles_bits[BEIDOU_B1I_PREAMBLE_LENGTH_BITS] = BEIDOU_PREAMBLE;
// preamble bits to sampled symbols
d_preambles_symbols = static_cast<int *>(volk_gnsssdr_malloc(GPS_CA_PREAMBLE_LENGTH_SYMBOLS * sizeof(int), volk_gnsssdr_get_alignment()));
d_preambles_symbols = static_cast<int *>(volk_gnsssdr_malloc(BEIDOU_B1I_PREAMBLE_LENGTH_SYMBOLS * sizeof(int), volk_gnsssdr_get_alignment()));
int n = 0;
for (int i = 0; i < GPS_CA_PREAMBLE_LENGTH_BITS; i++)
for (int i = 0; i < BEIDOU_B1I_PREAMBLE_LENGTH_BITS; i++)
{
for (unsigned int j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++)
{
if (preambles_bits[i] == 1)
{
d_preambles_symbols[n] = 1;
@@ -80,33 +80,50 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
d_preambles_symbols[n] = -1;
}
n++;
}
}
d_stat = 0;
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_flag_frame_sync = false;
d_GPS_frame_4bytes = 0;
d_prev_GPS_frame_4bytes = 0;
d_BEIDOU_frame_4bytes = 0;
d_prev_BEIDOU_frame_4bytes = 0;
d_flag_parity = false;
d_TOW_at_Preamble_ms = 0;
flag_TOW_set = false;
d_flag_preamble = false;
d_flag_new_tow_available = false;
d_word_number = 0;
word_number = 0;
d_channel = 0;
flag_PLL_180_deg_phase_locked = false;
d_preamble_time_samples = 0;
d_TOW_at_current_symbol_ms = 0;
d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS + 1); // Change fixed buffer size
d_symbol_history.resize(BEIDOU_B1I_PREAMBLE_LENGTH_BITS); // Change fixed buffer size
d_symbol_nh_history.resize(BEIDOU_B1I_NH_CODE_LENGTH + 1); // Change fixed buffer size
d_bit_buffer.resize(30); // Change fixed buffer size
d_symbol_history.clear(); // Clear all the elements in the buffer
d_symbol_nh_history.clear();
d_bit_buffer.clear();
d_make_correlation = true;
d_symbol_counter_corr = 0;
for (int aux = 0; aux < BEIDOU_B1I_NH_CODE_LENGTH; aux++)
{
if (BEIDOU_B1I_NH_CODE[aux] == 0)
{
bits_NH[aux] = -1.0;
}
else
{
bits_NH[aux] = 1.0;
}
}
sync_NH = false;
new_sym = false;
}
gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc()
beidou_b1i_telemetry_decoder_cc::~beidou_b1i_telemetry_decoder_cc()
{
volk_gnsssdr_free(d_preambles_symbols);
if (d_dump_file.is_open() == true)
@@ -123,44 +140,45 @@ gps_l1_ca_telemetry_decoder_cc::~gps_l1_ca_telemetry_decoder_cc()
}
bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
bool beidou_b1i_telemetry_decoder_cc::beidou_word_parityCheck(unsigned int beidouword)
{
unsigned int d1, d2, d3, d4, d5, d6, d7, t, parity;
/* XOR as many bits in parallel as possible. The magic constants pick
up bits which are to be XOR'ed together to implement the GPS parity
check algorithm described in IS-GPS-200E. This avoids lengthy shift-
and-xor loops. */
d1 = gpsword & 0xFBFFBF00;
d2 = _rotl(gpsword, 1) & 0x07FFBF01;
d3 = _rotl(gpsword, 2) & 0xFC0F8100;
d4 = _rotl(gpsword, 3) & 0xF81FFE02;
d5 = _rotl(gpsword, 4) & 0xFC00000E;
d6 = _rotl(gpsword, 5) & 0x07F00001;
d7 = _rotl(gpsword, 6) & 0x00003000;
d1 = beidouword & 0xFBFFBF00;
d2 = _rotl(beidouword, 1) & 0x07FFBF01;
d3 = _rotl(beidouword, 2) & 0xFC0F8100;
d4 = _rotl(beidouword, 3) & 0xF81FFE02;
d5 = _rotl(beidouword, 4) & 0xFC00000E;
d6 = _rotl(beidouword, 5) & 0x07F00001;
d7 = _rotl(beidouword, 6) & 0x00003000;
t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;
// Now XOR the 5 6-bit fields together to produce the 6-bit final result.
parity = t ^ _rotl(t, 6) ^ _rotl(t, 12) ^ _rotl(t, 18) ^ _rotl(t, 24);
parity = parity & 0x3F;
if (parity == (gpsword & 0x3F))
if (parity == (beidouword & 0x3F))
return (true);
else
return (false);
}
void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite)
void beidou_b1i_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite)
{
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
d_GPS_FSM.i_satellite_PRN = d_satellite.get_PRN();
d_BEIDOU_FSM.i_satellite_PRN = d_satellite.get_PRN();
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
}
void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
void beidou_b1i_telemetry_decoder_cc::set_channel(int channel)
{
d_channel = channel;
d_GPS_FSM.i_channel_ID = channel;
d_BEIDOU_FSM.i_channel_ID = channel;
DLOG(INFO) << "Navigation channel set to " << channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
@@ -186,32 +204,156 @@ void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
}
int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
void beidou_b1i_telemetry_decoder_cc::decode_word(int word_counter, boost::circular_buffer<signed int> *d_bit_buffer, unsigned int& d_BEIDOU_frame_4bytes)
{
//std::cout << word_counter << std::endl;
signed int firstBranch[15];
signed int secondBranch[15];
d_BEIDOU_frame_4bytes = 0;
if (word_counter == 1)
{
for (unsigned int i = 0; i < 15 ; i++)
{
if (d_bit_buffer->at(i) == 1)
{
d_BEIDOU_frame_4bytes++;
}
d_BEIDOU_frame_4bytes <<= 1;
}
for (unsigned int i = 15; i < 30 ; i++)
{
if (d_bit_buffer->at(i) == 1)
{
d_BEIDOU_frame_4bytes++;
}
d_BEIDOU_frame_4bytes <<= 1;
}
d_BEIDOU_frame_4bytes >>= 1;
}
else
{
for (unsigned int i = 0; i < 30 ; i = i + 2)
{
firstBranch[i/2] = d_bit_buffer->at(i);
secondBranch[i/2] = d_bit_buffer->at(i + 1);
}
for (unsigned int i = 0; i < 11 ; i++)
{
if (firstBranch[i] == 1)
{
d_BEIDOU_frame_4bytes++;
}
d_BEIDOU_frame_4bytes <<= 1;;
}
for (unsigned int i = 0; i < 11 ; i++)
{
if (secondBranch[i] == 1)
{
d_BEIDOU_frame_4bytes++;
}
d_BEIDOU_frame_4bytes <<= 1;;
}
for (unsigned int i = 11; i < 15 ; i++)
{
if (firstBranch[i] == 1)
{
d_BEIDOU_frame_4bytes++;
}
d_BEIDOU_frame_4bytes <<= 1;;
}
for (unsigned int i = 11; i < 15 ; i++)
{
if (secondBranch[i] == 1)
{
d_BEIDOU_frame_4bytes++;
}
d_BEIDOU_frame_4bytes <<= 1;;
}
d_BEIDOU_frame_4bytes >>= 1;
}
for (unsigned int i = 0; i < d_bit_buffer->size() ; i++)
{
std::cout << d_bit_buffer->at(i);
}
std::cout << std::endl;
// std::cout << d_BEIDOU_frame_4bytes << std::endl;
}
int beidou_b1i_telemetry_decoder_cc::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)
{
int corr_value = 0;
int preamble_diff_ms = 0;
int corr_NH = 0;
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); // Get the output buffer pointer
const Gnss_Synchro **in = reinterpret_cast<const Gnss_Synchro **>(&input_items[0]); // Get the input buffer pointer
new_sym = false;
Gnss_Synchro current_symbol; //structure to save the synchronization information and send the output object to the next block
//1. Copy the current tracking output
current_symbol = in[0][0];
d_symbol_history.push_back(current_symbol); //add new symbol to the symbol queue
double current_time_samples = current_symbol.Tracking_sample_counter;
double current_samples_fs = current_symbol.fs;
int symbol_value = 0;
bool Flag_valid_symbol_output = false;
d_symbol_nh_history.push_back(current_symbol.Prompt_I); //add new symbol to the symbol queue
consume_each(1);
unsigned int required_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS;
d_flag_preamble = false;
if (d_symbol_nh_history.size() == BEIDOU_B1I_NH_CODE_LENGTH)
{
for (int i = 0; i < BEIDOU_B1I_NH_CODE_LENGTH; i++)
{
if ((bits_NH[i] * d_symbol_nh_history.at(i)) > 0.0)
{
corr_NH += 1;
}
else
{
corr_NH -= 1;
}
}
if (abs(corr_NH) == BEIDOU_B1I_NH_CODE_LENGTH)
{
sync_NH = true;
if (corr_NH > 0)
{
symbol_value = 1;
}
else
{
symbol_value = -1;
}
// std::cout << "SUCCESSFUL NH CORRELATION" << std::endl;
if ((d_symbol_history.size() > required_symbols) and (d_make_correlation or !d_flag_frame_sync))
d_symbol_history.push_back(symbol_value);
new_sym = true;
d_symbol_nh_history.clear();
}
else
{
d_symbol_nh_history.pop_front();
sync_NH = false;
new_sym = false;
}
}
if ((d_symbol_history.size() >= BEIDOU_B1I_PREAMBLE_LENGTH_BITS) and (d_make_correlation or !d_flag_frame_sync))
{
//******* preamble correlation ********
for (unsigned int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++)
for (unsigned int i = 0; i < BEIDOU_B1I_PREAMBLE_LENGTH_BITS; i++)
{
if (d_symbol_history.at(i).Flag_valid_symbol_output == true)
{
if (d_symbol_history.at(i).Prompt_I < 0) // symbols clipping
if (d_symbol_history.at(i) < 0) // symbols clipping
{
corr_value -= d_preambles_symbols[i];
}
@@ -219,41 +361,69 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
{
corr_value += d_preambles_symbols[i];
}
}
}
if (std::abs(corr_value) >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
//std::cout << corr_value << std::endl;
if (std::abs(corr_value) >= BEIDOU_B1I_PREAMBLE_LENGTH_BITS)
{
/* for (unsigned int i = 0; i < d_symbol_history.size() ; i++)
{
std::cout << d_symbol_history.at(i);
}
std::cout << std::endl;
*/
// std::cout << "SUCCESSFUL PREAMBLE CORRELATION" << std::endl;
d_symbol_history.clear();
d_symbol_counter_corr++;
}
}
//******* frame sync ******************
if (std::abs(corr_value) == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
/*if (new_sym and )
{
flag_new_cnav_frame = beidou_nav_msg_decoder_add_symbol(&d_cnav_decoder, symbol_clip, &msg, &delay);
new_sym = false;
}*/
unsigned int required_symbols = BEIDOU_B1I_PREAMBLE_LENGTH_SYMBOLS;
d_flag_preamble = false;
//******* frame sync ******************
if (std::abs(corr_value) == BEIDOU_B1I_PREAMBLE_LENGTH_BITS)
{
// std::cout << "FRAME SYNC" << std::endl;
//TODO: Rewrite with state machine
if (d_stat == 0)
{
d_GPS_FSM.Event_gps_word_preamble();
// std::cout << "STATE MACHINE" << std::endl;
d_BEIDOU_FSM.Event_beidou_word_preamble();
//record the preamble sample stamp
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the preamble sample stamp
DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history.at(0).Tracking_sample_counter=" << d_symbol_history.at(0).Tracking_sample_counter;
d_preamble_time_samples = current_time_samples; // record the preamble sample stamp
DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "current_time_samples=" << current_time_samples;
//sync the symbol to bits integrator
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_stat = 1; // enter into frame pre-detection status
}
else if (d_stat == 1) //check 6 seconds of preamble separation
{
preamble_diff_ms = std::round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1)
// std::cout << "6 SECONDS" << std::endl;
preamble_diff_ms = std::round(((static_cast<double>(current_time_samples) - d_preamble_time_samples) / static_cast<double>(current_samples_fs)) * 1000.0);
if (std::abs(preamble_diff_ms - BEIDOU_SUBFRAME_MS) < 1)
{
std::cout << "Preamble confirmation for SAT" << std::endl;
DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite;
d_GPS_FSM.Event_gps_word_preamble();
d_BEIDOU_FSM.Event_beidou_word_preamble();
d_flag_preamble = true;
d_make_correlation = false;
d_symbol_counter_corr = 0;
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the PRN start sample index associated to the preamble
d_preamble_time_samples = current_time_samples; // record the PRN start sample index associated to the preamble
if (!d_flag_frame_sync)
{
d_flag_frame_sync = true;
@@ -267,22 +437,29 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
flag_PLL_180_deg_phase_locked = false;
}
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
<< static_cast<double>(d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs) << " [s]";
<< static_cast<double>(d_preamble_time_samples) / static_cast<double>(current_samples_fs) << " [s]";
}
}
d_frame_bit_index = 11;
d_symbol_history.clear();
for (int i = 0; i < BEIDOU_B1I_PREAMBLE_LENGTH_BITS; i++)
{
d_bit_buffer.push_back(d_preambles_symbols[i]);
}
word_number = 0;
}
}
else
{
d_symbol_counter_corr++;
if (d_symbol_counter_corr > (GPS_SUBFRAME_MS - GPS_CA_TELEMETRY_SYMBOLS_PER_BIT))
if (d_symbol_counter_corr > (BEIDOU_SUBFRAME_MS - BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT))
{
d_make_correlation = true;
}
if (d_stat == 1)
{
preamble_diff_ms = round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
if (preamble_diff_ms > GPS_SUBFRAME_MS + 1)
preamble_diff_ms = round(((static_cast<double>(current_time_samples) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(current_samples_fs)) * 1000.0);
if (preamble_diff_ms > BEIDOU_SUBFRAME_MS + 1)
{
DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms;
d_stat = 0; //lost of frame sync
@@ -293,73 +470,52 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
}
}
}
//******* SYMBOL TO BIT *******
if (d_symbol_history.at(0).Flag_valid_symbol_output == true)
{
// extended correlation to bit period is enabled in tracking!
d_symbol_accumulator += d_symbol_history.at(0).Prompt_I; // accumulate the input value in d_symbol_accumulator
d_symbol_accumulator_counter += d_symbol_history.at(0).correlation_length_ms;
}
if (d_symbol_accumulator_counter >= 20)
{
if (d_symbol_accumulator > 0)
{ //symbol to bit
d_GPS_frame_4bytes += 1; //insert the telemetry bit in LSB
}
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
if (d_flag_frame_sync and new_sym)
{
// std::cout << symbol_value << std::endl;
if (flag_PLL_180_deg_phase_locked)
{
d_bit_buffer.push_back(-symbol_value);
}
else
{
d_bit_buffer.push_back(symbol_value);
}
//******* bits to words ******
d_frame_bit_index++;
if (d_frame_bit_index == 30)
{
word_number++;
beidou_b1i_telemetry_decoder_cc::decode_word(word_number, &d_bit_buffer, d_BEIDOU_frame_4bytes);
// std::cout << d_BEIDOU_frame_4bytes << std::endl;
d_bit_buffer.clear();
d_frame_bit_index = 0;
// parity check
// Each word in wordbuff is composed of:
// Bits 0 to 29 = the GPS data word
// Bits 30 to 31 = 2 LSBs of the GPS word ahead.
// prepare the extended frame [-2 -1 0 ... 30]
if (d_prev_GPS_frame_4bytes & 0x00000001)
{
d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x40000000;
}
if (d_prev_GPS_frame_4bytes & 0x00000002)
{
d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x80000000;
}
/* Check that the 2 most recently logged words pass parity. Have to first
invert the data bits according to bit 30 of the previous word. */
if (d_GPS_frame_4bytes & 0x40000000)
{
d_GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR)
}
if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes))
{
memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char) * 4);
//d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0;
d_GPS_FSM.Event_gps_word_valid();
memcpy(&d_BEIDOU_FSM.d_BEIDOU_frame_4bytes, &d_BEIDOU_frame_4bytes, sizeof(char) * 4);
//d_BEIDOU_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0;
d_BEIDOU_FSM.Event_beidou_word_valid();
// send TLM data to PVT using asynchronous message queues
if (d_GPS_FSM.d_flag_new_subframe == true)
if (d_BEIDOU_FSM.d_flag_new_subframe == true)
{
switch (d_GPS_FSM.d_subframe_ID)
switch (d_BEIDOU_FSM.d_subframe_ID)
{
case 3: //we have a new set of ephemeris data for the current SV
if (d_GPS_FSM.d_nav.satellite_validation() == true)
if (d_BEIDOU_FSM.d_nav.satellite_validation() == true)
{
// get ephemeris object for this SV (mandatory)
std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(d_GPS_FSM.d_nav.get_ephemeris());
std::shared_ptr<Beidou_Ephemeris> tmp_obj = std::make_shared<Beidou_Ephemeris>(d_BEIDOU_FSM.d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 4: // Possible IONOSPHERE and UTC model update (page 18)
if (d_GPS_FSM.d_nav.flag_iono_valid == true)
if (d_BEIDOU_FSM.d_nav.flag_iono_valid == true)
{
std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(d_GPS_FSM.d_nav.get_iono());
std::shared_ptr<Beidou_Iono> tmp_obj = std::make_shared<Beidou_Iono>(d_BEIDOU_FSM.d_nav.get_iono());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_GPS_FSM.d_nav.flag_utc_model_valid == true)
if (d_BEIDOU_FSM.d_nav.flag_utc_model_valid == true)
{
std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(d_GPS_FSM.d_nav.get_utc_model());
std::shared_ptr<Beidou_Utc_Model> tmp_obj = std::make_shared<Beidou_Utc_Model>(d_BEIDOU_FSM.d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
@@ -370,37 +526,22 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
default:
break;
}
d_GPS_FSM.clear_flag_new_subframe();
d_BEIDOU_FSM.clear_flag_new_subframe();
d_flag_new_tow_available = true;
}
d_flag_parity = true;
}
else
{
d_GPS_FSM.Event_gps_word_invalid();
d_flag_parity = false;
}
d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame
d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0;
}
else
{
d_GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word
}
}
}
//2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_flag_new_tow_available == true)
{
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_GPS_FSM.d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS;
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_BEIDOU_FSM.d_nav.d_SOW) * 1000 + BEIDOU_B1I_CODE_PERIOD_MS + BEIDOU_B1I_PREAMBLE_DURATION_MS;
d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms;
flag_TOW_set = true;
d_flag_new_tow_available = false;
}
else
{
d_TOW_at_current_symbol_ms += GPS_L1_CA_CODE_PERIOD_MS;
d_TOW_at_current_symbol_ms += BEIDOU_B1I_CODE_PERIOD_MS;
}
current_symbol.TOW_at_current_symbol_ms = d_TOW_at_current_symbol_ms;
@@ -409,7 +550,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
if (flag_PLL_180_deg_phase_locked == true)
{
//correct the accumulated phase for the Costas loop phase shift, if required
current_symbol.Carrier_phase_rads += GPS_PI;
current_symbol.Carrier_phase_rads += BEIDOU_PI;
}
if (d_dump == true)

54
src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_cc.h
View File

@@ -1,8 +1,8 @@
/*!
* \file gps_l1_ca_telemetry_decoder_cc.h
* \file beidou_b1i_telemetry_decoder_cc.h
* \brief Interface of a NAV message demodulator block based on
* Kay Borre book MATLAB-based GPS receiver
* \author Javier Arribas, 2011. jarribas(at)cttc.es
* \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
@@ -28,11 +28,11 @@
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_CC_H
#define GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_CC_H
#ifndef GNSS_SDR_BEIDOU_B1I_TELEMETRY_DECODER_CC_H
#define GNSS_SDR_BEIDOU_B1I_TELEMETRY_DECODER_CC_H
#include "GPS_L1_CA.h"
#include "gps_l1_ca_subframe_fsm.h"
#include "beidou_b1I.h"
#include "beidou_b1i_subframe_fsm.h"
#include "gnss_satellite.h"
#include "gnss_synchro.h"
#include <gnuradio/block.h>
@@ -40,23 +40,25 @@
#include <string>
#include <boost/circular_buffer.hpp>
class gps_l1_ca_telemetry_decoder_cc;
class beidou_b1i_telemetry_decoder_cc;
typedef boost::shared_ptr<gps_l1_ca_telemetry_decoder_cc> gps_l1_ca_telemetry_decoder_cc_sptr;
typedef boost::shared_ptr<beidou_b1i_telemetry_decoder_cc> beidou_b1i_telemetry_decoder_cc_sptr;
gps_l1_ca_telemetry_decoder_cc_sptr
gps_l1_ca_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
beidou_b1i_telemetry_decoder_cc_sptr
beidou_b1i_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
/*!
* \brief This class implements a block that decodes the NAV data defined in IS-GPS-200E
*
*/
class gps_l1_ca_telemetry_decoder_cc : public gr::block
class beidou_b1i_telemetry_decoder_cc : public gr::block
{
public:
~gps_l1_ca_telemetry_decoder_cc();
~beidou_b1i_telemetry_decoder_cc();
void set_satellite(const Gnss_Satellite &satellite); //!< Set satellite PRN
void set_channel(int channel); //!< Set receiver's channel
void set_channel(int channel);
void decode_word(int word_counter, boost::circular_buffer<signed int> *d_bit_buffer, unsigned int& d_BEIDOU_frame_4bytes);
/*!
* \brief This is where all signal processing takes place
@@ -65,12 +67,12 @@ public:
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
private:
friend gps_l1_ca_telemetry_decoder_cc_sptr
gps_l1_ca_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
friend beidou_b1i_telemetry_decoder_cc_sptr
beidou_b1i_make_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
gps_l1_ca_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
beidou_b1i_telemetry_decoder_cc(const Gnss_Satellite &satellite, bool dump);
bool gps_word_parityCheck(unsigned int gpsword);
bool beidou_word_parityCheck(unsigned int beidouword);
// class private vars
@@ -79,7 +81,9 @@ private:
bool d_flag_frame_sync;
// symbols
boost::circular_buffer<Gnss_Synchro> d_symbol_history;
boost::circular_buffer<signed int> d_symbol_history;
boost::circular_buffer<signed int> d_symbol_nh_history;
boost::circular_buffer<signed int> d_bit_buffer;
double d_symbol_accumulator;
short int d_symbol_accumulator_counter;
@@ -90,16 +94,17 @@ private:
//bits and frame
unsigned short int d_frame_bit_index;
unsigned int d_GPS_frame_4bytes;
unsigned int d_prev_GPS_frame_4bytes;
double bits_NH[BEIDOU_B1I_NH_CODE_LENGTH];
unsigned int d_BEIDOU_frame_4bytes;
unsigned int d_prev_BEIDOU_frame_4bytes;
bool d_flag_parity;
bool d_flag_preamble;
bool d_flag_new_tow_available;
int d_word_number;
// navigation message vars
Gps_Navigation_Message d_nav;
GpsL1CaSubframeFsm d_GPS_FSM;
Beidou_Navigation_Message_D1 d_nav;
BeidouB1iSubframeFsm d_BEIDOU_FSM;
bool d_dump;
Gnss_Satellite d_satellite;
@@ -109,12 +114,15 @@ private:
unsigned int d_TOW_at_Preamble_ms;
unsigned int d_TOW_at_current_symbol_ms;
unsigned int word_number;
bool flag_TOW_set;
bool flag_PLL_180_deg_phase_locked;
std::string d_dump_filename;
std::ofstream d_dump_file;
bool sync_NH;
bool new_sym;
};
#endif

3
src/algorithms/telemetry_decoder/libs/CMakeLists.txt
View File

@@ -19,7 +19,8 @@
add_subdirectory(libswiftcnav)
set(TELEMETRY_DECODER_LIB_SOURCES
gps_l1_ca_subframe_fsm.cc
gps_l1_ca_subframe_fsm.cc
beidou_b1i_subframe_fsm.cc
viterbi_decoder.cc
)

289
src/algorithms/telemetry_decoder/libs/beidou_b1i_subframe_fsm.cc Normal file
View File

@@ -0,0 +1,289 @@
/*!
* \file beidou_b1i_subframe_fsm.cc
* \brief Implementation of a BEIDOU NAV message word-to-subframe decoder state machine
* \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "beidou_b1i_subframe_fsm.h"
#include "gnss_satellite.h"
#include <boost/statechart/simple_state.hpp>
#include <boost/statechart/state.hpp>
#include <boost/statechart/transition.hpp>
#include <boost/statechart/custom_reaction.hpp>
#include <boost/mpl/list.hpp>
#include <cstring>
#include <iostream>
//************ GPS WORD TO SUBFRAME DECODER STATE MACHINE **********
struct Ev_beidou_word_valid : sc::event<Ev_beidou_word_valid>
{
};
struct Ev_beidou_word_invalid : sc::event<Ev_beidou_word_invalid>
{
};
struct Ev_beidou_word_preamble : sc::event<Ev_beidou_word_preamble>
{
};
struct beidou_subframe_fsm_S0 : public sc::state<beidou_subframe_fsm_S0, BeidouB1iSubframeFsm>
{
public:
// sc::transition(event,next_status)
typedef sc::transition<Ev_beidou_word_preamble, beidou_subframe_fsm_S1> reactions;
beidou_subframe_fsm_S0(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S0 "<<std::endl;
}
};
struct beidou_subframe_fsm_S1 : public sc::state<beidou_subframe_fsm_S1, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S2> >
reactions;
beidou_subframe_fsm_S1(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S1 "<<std::endl;
}
};
struct beidou_subframe_fsm_S2 : public sc::state<beidou_subframe_fsm_S2, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S3> >
reactions;
beidou_subframe_fsm_S2(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S2 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(0);
}
};
struct beidou_subframe_fsm_S3 : public sc::state<beidou_subframe_fsm_S3, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S4> >
reactions;
beidou_subframe_fsm_S3(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S3 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(1);
}
};
struct beidou_subframe_fsm_S4 : public sc::state<beidou_subframe_fsm_S4, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S5> >
reactions;
beidou_subframe_fsm_S4(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S4 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(2);
}
};
struct beidou_subframe_fsm_S5 : public sc::state<beidou_subframe_fsm_S5, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S6> >
reactions;
beidou_subframe_fsm_S5(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S5 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(3);
}
};
struct beidou_subframe_fsm_S6 : public sc::state<beidou_subframe_fsm_S6, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S7> >
reactions;
beidou_subframe_fsm_S6(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S6 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(4);
}
};
struct beidou_subframe_fsm_S7 : public sc::state<beidou_subframe_fsm_S7, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S8> >
reactions;
beidou_subframe_fsm_S7(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S7 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(5);
}
};
struct beidou_subframe_fsm_S8 : public sc::state<beidou_subframe_fsm_S8, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S9> >
reactions;
beidou_subframe_fsm_S8(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S8 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(6);
}
};
struct beidou_subframe_fsm_S9 : public sc::state<beidou_subframe_fsm_S9, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S10> >
reactions;
beidou_subframe_fsm_S9(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S9 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(7);
}
};
struct beidou_subframe_fsm_S10 : public sc::state<beidou_subframe_fsm_S10, BeidouB1iSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_beidou_word_invalid, beidou_subframe_fsm_S0>,
sc::transition<Ev_beidou_word_valid, beidou_subframe_fsm_S11> >
reactions;
beidou_subframe_fsm_S10(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S10 "<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(8);
}
};
struct beidou_subframe_fsm_S11 : public sc::state<beidou_subframe_fsm_S11, BeidouB1iSubframeFsm>
{
public:
typedef sc::transition<Ev_beidou_word_preamble, beidou_subframe_fsm_S1> reactions;
beidou_subframe_fsm_S11(my_context ctx) : my_base(ctx)
{
//std::cout<<"Completed GPS Subframe!"<<std::endl;
context<BeidouB1iSubframeFsm>().beidou_word_to_subframe(9);
context<BeidouB1iSubframeFsm>().beidou_subframe_to_nav_msg(); //decode the subframe
// DECODE SUBFRAME
//std::cout<<"Enter S11"<<std::endl;
}
};
BeidouB1iSubframeFsm::BeidouB1iSubframeFsm()
{
d_nav.reset();
i_channel_ID = 0;
i_satellite_PRN = 0;
d_subframe_ID = 0;
d_flag_new_subframe = false;
initiate(); //start the FSM
}
void BeidouB1iSubframeFsm::beidou_word_to_subframe(int position)
{
// insert the word in the correct position of the subframe
std::memcpy(&d_subframe[position * BEIDOU_WORD_LENGTH], &d_BEIDOU_frame_4bytes, sizeof(char) * BEIDOU_WORD_LENGTH);
}
void BeidouB1iSubframeFsm::clear_flag_new_subframe()
{
d_flag_new_subframe = false;
}
void BeidouB1iSubframeFsm::beidou_subframe_to_nav_msg()
{
//int subframe_ID;
// NEW GPS SUBFRAME HAS ARRIVED!
d_subframe_ID = d_nav.subframe_decoder(this->d_subframe); //decode the subframe
std::cout << "New BEIDOU NAV message received in channel " << i_channel_ID << ": "
<< "subframe "
<< d_subframe_ID << " from satellite "
<< Gnss_Satellite(std::string("Beidou"), i_satellite_PRN) << std::endl;
d_nav.i_satellite_PRN = i_satellite_PRN;
d_nav.i_channel_ID = i_channel_ID;
d_flag_new_subframe = true;
}
void BeidouB1iSubframeFsm::Event_beidou_word_valid()
{
this->process_event(Ev_beidou_word_valid());
}
void BeidouB1iSubframeFsm::Event_beidou_word_invalid()
{
this->process_event(Ev_beidou_word_invalid());
}
void BeidouB1iSubframeFsm::Event_beidou_word_preamble()
{
this->process_event(Ev_beidou_word_preamble());
}

100
src/algorithms/telemetry_decoder/libs/beidou_b1i_subframe_fsm.h Normal file
View File

@@ -0,0 +1,100 @@
/*!
* \file gps_l1_ca_subframe_fsm.h
* \brief Interface of a BEIDOU NAV message word-to-subframe decoder state machine
* \author Sergi Segura, 2018. sergi.segura.munoz(at)gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_BEIDOU_B1I_SUBFRAME_FSM_H_
#define GNSS_SDR_BEIDOU_B1I_SUBFRAME_FSM_H_
#include "beidou_b1I.h"
#include "beidou_navigation_message.h"
#include "beidou_ephemeris.h"
#include "beidou_iono.h"
#include "beidou_almanac.h"
#include "beidou_utc_model.h"
#include <boost/statechart/state_machine.hpp>
namespace sc = boost::statechart;
namespace mpl = boost::mpl;
struct beidou_subframe_fsm_S0;
struct beidou_subframe_fsm_S1;
struct beidou_subframe_fsm_S2;
struct beidou_subframe_fsm_S3;
struct beidou_subframe_fsm_S4;
struct beidou_subframe_fsm_S5;
struct beidou_subframe_fsm_S6;
struct beidou_subframe_fsm_S7;
struct beidou_subframe_fsm_S8;
struct beidou_subframe_fsm_S9;
struct beidou_subframe_fsm_S10;
struct beidou_subframe_fsm_S11;
/*!
* \brief This class implements a Finite State Machine that handles the decoding
* of the GPS L1 C/A NAV message
*/
class BeidouB1iSubframeFsm : public sc::state_machine<BeidouB1iSubframeFsm, beidou_subframe_fsm_S0>
{
public:
BeidouB1iSubframeFsm(); //!< The constructor starts the Finite State Machine
void clear_flag_new_subframe();
// channel and satellite info
int i_channel_ID; //!< Channel id
unsigned int i_satellite_PRN; //!< Satellite PRN number
Beidou_Navigation_Message_D1 d_nav; //!< GPS L1 C/A navigation message object
// GPS SV and System parameters
Beidou_Ephemeris ephemeris; //!< Object that handles GPS ephemeris parameters
Beidou_Almanac almanac; //!< Object that handles GPS almanac data
Beidou_Utc_Model utc_model; //!< Object that handles UTM model parameters
Beidou_Iono iono; //!< Object that handles ionospheric parameters
char d_subframe[BEIDOU_SUBFRAME_LENGTH];
int d_subframe_ID;
bool d_flag_new_subframe;
char d_BEIDOU_frame_4bytes[BEIDOU_WORD_LENGTH];
//double d_preamble_time_ms;
void beidou_word_to_subframe(int position); //!< inserts the word in the correct position of the subframe
/*!
* \brief This function decodes a NAv message subframe and pushes the information to the right queues
*/
void beidou_subframe_to_nav_msg();
//FSM EVENTS
void Event_beidou_word_valid(); //!< FSM event: the received word is valid
void Event_beidou_word_invalid(); //!< FSM event: the received word is not valid
void Event_beidou_word_preamble(); //!< FSM event: word preamble detected
};
#endif

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

@@ -254,9 +254,12 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(dllpllconf_t conf_) : gr::block("dl
d_symbols_per_bit = BEIDOU_B1I_TELEMETRY_SYMBOLS_PER_BIT;
d_correlation_length_ms = 1;
d_code_samples_per_chip = 1;
d_secondary = false;
d_secondary = true;
trk_parameters.track_pilot = false;
interchange_iq = false;
d_secondary_code_length = static_cast<unsigned int>(BEIDOU_B1I_NH_CODE_LENGTH);
d_secondary_code_string = const_cast<std::string *>(&BEIDOU_B1I_NH_CODE_STR);
}
else
{