1
0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-12 15:56:02 +00:00

Added PRS VEML tracking

For now this ignores the very early and very late correlators. Plan on
implementing:
1) Bump jumping
2) sliding subcarrier

approaches.
This commit is contained in:
Cillian O'Driscoll
2015-11-19 11:14:40 +00:00
parent 926029de54
commit 4460a440e0
7 changed files with 2006 additions and 0 deletions

View File

@@ -34,6 +34,7 @@ set(TRACKING_ADAPTER_SOURCES
gps_l2_m_dll_pll_tracking.cc
galileo_e1_de_tracking.cc
galileo_e1_prs_de_tracking.cc
galileo_e1_prs_veml_tracking.cc
${OPT_TRACKING_ADAPTERS}
)

View File

@@ -0,0 +1,197 @@
/*!
* \file galileo_e1_prs_veml_tracking.h
* \brief Adapts a VEML tracking loop block
* to a TrackingInterface for Galileo E1 signals including PRS
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "galileo_e1_prs_veml_tracking.h"
#include <glog/logging.h>
#include "GPS_L1_CA.h"
#include "Galileo_E1.h"
#include "configuration_interface.h"
#include "spirent_prs_code_generator.h"
using google::LogMessage;
GalileoE1PrsVemlTracking::GalileoE1PrsVemlTracking(
ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams,
boost::shared_ptr<gr::msg_queue> queue) :
role_(role), in_streams_(in_streams), out_streams_(out_streams),
queue_(queue)
{
DLOG(INFO) << "role " << role;
//################# CONFIGURATION PARAMETERS ########################
int fs_in;
int vector_length;
int f_if;
bool dump;
std::string dump_filename;
std::string item_type;
std::string default_item_type = "gr_complex";
float pll_initial_bw_hz;
float pll_final_bw_hz;
float dll_initial_bw_hz;
float dll_final_bw_hz;
int pll_loop_order;
int dll_loop_order;
float initial_early_late_code_space_cycles;
float final_early_late_code_space_cycles;
float initial_very_early_late_code_space_chips;
float final_very_early_late_code_space_chips;
bool aid_code_with_carrier;
item_type = configuration->property(role + ".item_type", default_item_type);
fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
f_if = configuration->property(role + ".if", 0);
dump = configuration->property(role + ".dump", false);
pll_initial_bw_hz = configuration->property(role + ".pll_initial_bw_hz", 15.0);
pll_final_bw_hz = configuration->property(role + ".pll_final_bw_hz", 15.0);
dll_initial_bw_hz = configuration->property(role + ".dll_initial_bw_hz", 0.5);
dll_final_bw_hz = configuration->property(role + ".dll_final_bw_hz", 0.5);
initial_early_late_code_space_cycles = configuration->property(role + ".initial_early_late_code_space_cycles", 0.125);
final_early_late_code_space_cycles = configuration->property(role + ".final_early_late_code_space_cycles", 0.125);
initial_very_early_late_code_space_chips = configuration->property(role + ".initial_very_early_late_code_space_chips", 0.5);
final_very_early_late_code_space_chips = configuration->property(role + ".final_very_early_late_code_space_chips", 0.5);
aid_code_with_carrier = configuration->property(role + ".aid_code_with_carrier", false );
pll_loop_order = configuration->property(role + ".pll_loop_order", 3);
dll_loop_order = configuration->property(role + ".dll_loop_order", 1);
std::string default_code_type = "Spirent";
std::string code_type = configuration->property(role + ".prs_code_type", default_code_type );
std::string default_dump_filename = "./track_de_";
dump_filename = configuration->property(role + ".dump_filename",
default_dump_filename); //unused!
vector_length = std::round(fs_in / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS));
boost::shared_ptr< LongCodeInterface > code_gen;
if( not code_type.compare("Spirent") )// anything other than zero means not a match
{
code_gen = boost::shared_ptr< LongCodeInterface >(
new SpirentPrsCodeGenerator( 1, true ) );
if( code_gen == 0 )
{
LOG(ERROR) << "Unable to create a SpirentPrsCodeGenerator";
}
}
else
{
LOG(ERROR) << code_type << " unknown PRS code type";
}
//################# MAKE TRACKING GNURadio object ###################
if (item_type.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
tracking_ = galileo_e1_prs_veml_make_tracking_cc(
f_if,
fs_in,
vector_length,
queue_,
dump,
dump_filename,
pll_loop_order, pll_initial_bw_hz, pll_final_bw_hz,
dll_loop_order, dll_initial_bw_hz, dll_final_bw_hz,
initial_early_late_code_space_cycles, final_early_late_code_space_cycles,
initial_very_early_late_code_space_chips, final_very_early_late_code_space_chips,
aid_code_with_carrier, code_gen);
}
else
{
item_size_ = sizeof(gr_complex);
LOG(WARNING) << item_type << " unknown tracking item type.";
}
channel_ = 0;
channel_internal_queue_ = 0;
DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
}
GalileoE1PrsVemlTracking::~GalileoE1PrsVemlTracking()
{}
void GalileoE1PrsVemlTracking::start_tracking()
{
tracking_->start_tracking();
}
/*
* Set tracking channel unique ID
*/
void GalileoE1PrsVemlTracking::set_channel(unsigned int channel)
{
channel_ = channel;
tracking_->set_channel(channel);
}
/*
* Set tracking channel internal queue
*/
void GalileoE1PrsVemlTracking::set_channel_queue(
concurrent_queue<int> *channel_internal_queue)
{
channel_internal_queue_ = channel_internal_queue;
tracking_->set_channel_queue(channel_internal_queue_);
}
void GalileoE1PrsVemlTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
{
tracking_->set_gnss_synchro(p_gnss_synchro);
}
void GalileoE1PrsVemlTracking::connect(gr::top_block_sptr top_block)
{
if(top_block) { /* top_block is not null */};
//nothing to connect, now the tracking uses gr_sync_decimator
}
void GalileoE1PrsVemlTracking::disconnect(gr::top_block_sptr top_block)
{
if(top_block) { /* top_block is not null */};
//nothing to disconnect, now the tracking uses gr_sync_decimator
}
gr::basic_block_sptr GalileoE1PrsVemlTracking::get_left_block()
{
return tracking_;
}
gr::basic_block_sptr GalileoE1PrsVemlTracking::get_right_block()
{
return tracking_;
}

View File

@@ -0,0 +1,116 @@
/*!
* \file galileo_e1_prs_veml_tracking.h
* \brief Adapts a VEML tracking loop block
* to a TrackingInterface for Galileo E1 signals inclding PRS
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GALILEO_e1_prs_veml_TRACKING_H_
#define GNSS_SDR_GALILEO_e1_prs_veml_TRACKING_H_
#include <string>
#include <gnuradio/msg_queue.h>
#include "tracking_interface.h"
#include "galileo_e1_prs_veml_tracking_cc.h"
class ConfigurationInterface;
/*!
* \brief This class Adapts a DLL+PLL VEML (Very Early Minus Late) tracking
* loop block to a TrackingInterface for Galileo E1 signals
*/
class GalileoE1PrsVemlTracking : public TrackingInterface
{
public:
GalileoE1PrsVemlTracking(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams,
boost::shared_ptr<gr::msg_queue> queue);
virtual ~GalileoE1PrsVemlTracking();
std::string role()
{
return role_;
}
//! Returns "Galileo_E1_DE_Tracking"
std::string implementation()
{
return "Galileo_e1_prs_veml_Tracking";
}
size_t item_size()
{
return item_size_;
}
void connect(gr::top_block_sptr top_block);
void disconnect(gr::top_block_sptr top_block);
gr::basic_block_sptr get_left_block();
gr::basic_block_sptr get_right_block();
/*!
* \brief Set tracking channel unique ID
*/
void set_channel(unsigned int channel);
/*!
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
* to efficiently exchange synchronization data between acquisition and
* tracking blocks
*/
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
/*!
* \brief Set tracking channel internal queue
*/
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
void start_tracking();
private:
galileo_e1_prs_veml_tracking_cc_sptr tracking_;
size_t item_size_;
unsigned int channel_;
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
boost::shared_ptr<gr::msg_queue> queue_;
concurrent_queue<int> *channel_internal_queue_;
};
#endif // GNSS_SDR_GALILEO_e1_prs_veml_TRACKING_H_

View File

@@ -36,6 +36,7 @@ set(TRACKING_GR_BLOCKS_SOURCES
gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
galileo_e1_de_tracking_cc.cc
galileo_e1_prs_de_tracking_cc.cc
galileo_e1_prs_veml_tracking_cc.cc
${OPT_TRACKING_BLOCKS}
)

File diff suppressed because it is too large Load Diff

View File

@@ -0,0 +1,297 @@
/*!
* \file galileo_e1_prs_veml_tracking_cc.h
* \brief Implementation of VEML tracking for Galileo E1 PRS
* \author Cillian O'Driscoll, 2015. cillian.odriscoll(at)gmail.com
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GALILEO_E1_PRS_VEML_TRACKING_CC_H
#define GNSS_SDR_GALILEO_E1_PRS_VEML_TRACKING_CC_H
#include <fstream>
#include <queue>
#include <string>
#include <map>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
#include "concurrent_queue.h"
#include "gnss_synchro.h"
#include "tracking_loop_filter.h"
#include "correlator.h"
#include "gnss_message.h"
#include "long_code_interface.h" // for prs code gen
class galileo_e1_prs_veml_tracking_cc;
typedef boost::shared_ptr<galileo_e1_prs_veml_tracking_cc> galileo_e1_prs_veml_tracking_cc_sptr;
galileo_e1_prs_veml_tracking_cc_sptr
galileo_e1_prs_veml_make_tracking_cc(long if_freq,
long fs_in, unsigned
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
bool dump,
std::string dump_filename,
int pll_loop_order,
float pll_initial_bw_hz,
float pll_final_bw_hz,
int dll_loop_order,
float dll_initial_bw_hz,
float dll_final_bw_hz,
float initial_early_late_code_space_cycles,
float final_early_late_code_space_cycles,
float initial_very_early_late_code_space_chips,
float final_very_early_late_code_space_chips,
bool aid_code_with_carrier,
LongCodeInterface_sptr prs_code_gen);
/*!
* \brief This class implements a double estimator tracking block for Galileo E1 signals
*/
class galileo_e1_prs_veml_tracking_cc: public gr::block
{
public:
~galileo_e1_prs_veml_tracking_cc();
void set_channel(unsigned int channel);
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
void start_tracking();
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
/*!
* \brief Code DLL + carrier PLL according to the algorithms described in:
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach,
* Birkhauser, 2007
*/
int general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
private:
void start_tracking_prs();
friend galileo_e1_prs_veml_tracking_cc_sptr
galileo_e1_prs_veml_make_tracking_cc(long if_freq,
long fs_in, unsigned
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
bool dump,
std::string dump_filename,
int pll_loop_order,
float pll_initial_bw_hz,
float pll_final_bw_hz,
int dll_loop_order,
float dll_initial_bw_hz,
float dll_final_bw_hz,
float initial_early_late_code_space_cycles,
float final_early_late_code_space_cycles,
float initial_very_early_late_code_space_chips,
float final_very_early_late_code_space_chips,
bool aid_code_with_carrier,
LongCodeInterface_sptr prs_code_gen);
galileo_e1_prs_veml_tracking_cc(long if_freq,
long fs_in, unsigned
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
bool dump,
std::string dump_filename,
int pll_loop_order,
float pll_initial_bw_hz,
float pll_final_bw_hz,
int dll_loop_order,
float dll_initial_bw_hz,
float dll_final_bw_hz,
float initial_early_late_code_space_cycles,
float final_early_late_code_space_cycles,
float initial_very_early_late_code_space_chips,
float final_very_early_late_code_space_chips,
bool aid_code_with_carrier,
LongCodeInterface_sptr prs_code_gen);
void update_local_code();
void update_local_code_prs();
void update_local_carrier();
// tracking configuration vars
boost::shared_ptr<gr::msg_queue> d_queue;
concurrent_queue<int> *d_channel_internal_queue;
unsigned int d_vector_length;
bool d_dump;
Gnss_Synchro* d_acquisition_gnss_synchro;
unsigned int d_channel;
int d_last_seg;
long d_if_freq;
long d_fs_in;
bool d_aid_code_with_carrier;
float d_early_late_code_spc_cycles;
float d_very_early_late_code_spc_chips;
float d_very_early_late_code_spc_chips_prs;
LongCodeInterface_sptr d_prs_code_gen;
gr_complex* d_e1b_code;
gr_complex* d_prs_code;
std::vector< short > d_prs_code_shorts;
uint64_t d_start_index_prs_code;
unsigned int d_size_prs_code;
bool d_prs_code_initialized;
gr_complex* d_early_code;
gr_complex* d_prompt_code;
gr_complex* d_late_code;
gr_complex* d_very_early_code;
gr_complex* d_very_late_code;
gr_complex* d_carr_sign;
gr_complex* d_early_code_prs;
gr_complex* d_prompt_code_prs;
gr_complex* d_late_code_prs;
gr_complex* d_very_early_code_prs;
gr_complex* d_very_late_code_prs;
gr_complex *d_Very_Early;
gr_complex *d_Early;
gr_complex *d_Prompt;
gr_complex *d_Late;
gr_complex *d_Very_Late;
gr_complex *d_Very_Early_prs;
gr_complex *d_Early_prs;
gr_complex *d_Prompt_prs;
gr_complex *d_Late_prs;
gr_complex *d_Very_Late_prs;
// remaining code phase and carrier phase between tracking loops
double d_rem_code_phase_samples;
float d_rem_carr_phase_rad;
double d_rem_code_phase_samples_prs;
float d_rem_carr_phase_rad_prs;
// PLL and DLL filter library
Tracking_loop_filter d_code_loop_filter;
Tracking_loop_filter d_carrier_loop_filter;
int d_pll_loop_order;
float d_initial_pll_bw_hz;
float d_final_pll_bw_hz;
int d_dll_loop_order;
float d_initial_dll_bw_hz;
float d_final_dll_bw_hz;
float d_initial_early_late_code_space_cycles;
float d_final_early_late_code_space_cycles;
float d_initial_very_early_late_code_space_chips;
float d_final_very_early_late_code_space_chips;
Tracking_loop_filter d_code_loop_filter_prs;
Tracking_loop_filter d_carrier_loop_filter_prs;
// acquisition
float d_acq_code_phase_samples;
float d_acq_carrier_doppler_hz;
// correlator
Correlator d_correlator;
// tracking vars
double d_code_freq_chips;
double d_code_phase_chips;
float d_carrier_doppler_hz;
double d_carrier_phase_rad;
double d_acc_carrier_phase_rad;
double d_acc_code_phase_secs;
double d_code_freq_chips_prs;
double d_code_phase_chips_prs;
float d_carrier_doppler_hz_prs;
double d_carrier_phase_rad_prs;
double d_acc_carrier_phase_rad_prs;
double d_acc_code_phase_secs_prs;
//PRN period in samples
int d_current_prn_length_samples;
//processing samples counters
unsigned long int d_sample_counter;
unsigned long int d_acq_sample_stamp;
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
gr_complex* d_Prompt_buffer;
float d_carrier_lock_test;
float d_CN0_SNV_dB_Hz;
float d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
int d_carrier_lock_success_counter;
// control vars
bool d_enable_tracking;
bool d_pull_in;
bool d_carrier_locked;
bool d_code_locked;
bool d_code_locked_prs;
bool d_tow_received;
double d_last_tow;
double d_timestamp_last_tow;
bool d_rx_time_set;
double d_tow_rx_time;
double d_timestamp_rx_time;
bool d_preamble_start_detected;
double d_preamble_timestamp;
bool d_prs_tracking_enabled;
// file dump
std::string d_dump_filename;
std::ofstream d_dump_file;
std::map<std::string, std::string> systemName;
std::string sys;
// Handler for gnss_messages:
void handle_gnss_message( pmt::pmt_t msg );
};
#endif //GNSS_SDR_GALILEO_E1_PRS_VEML_TRACKING_CC_H

View File

@@ -89,6 +89,7 @@
#include "gps_l2_m_dll_pll_tracking.h"
#include "galileo_e1_de_tracking.h"
#include "galileo_e1_prs_de_tracking.h"
#include "galileo_e1_prs_veml_tracking.h"
#include "gps_l1_ca_telemetry_decoder.h"
#include "gps_l2_m_telemetry_decoder.h"
#include "galileo_e1b_telemetry_decoder.h"
@@ -1657,6 +1658,12 @@ std::unique_ptr<TrackingInterface> GNSSBlockFactory::GetTrkBlock(
out_streams, queue));
block = std::move(block_);
}
else if (implementation.compare("Galileo_E1_PRS_VEML_Tracking") == 0)
{
std::unique_ptr<TrackingInterface> block_(new GalileoE1PrsVemlTracking(configuration.get(), role, in_streams,
out_streams, queue));
block = std::move(block_);
}
#if CUDA_GPU_ACCEL
else if (implementation.compare("GPS_L1_CA_DLL_PLL_Tracking_GPU") == 0)
{