mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

GNSS-SDR Major changes: 

New tracking libraries:
        -  tracking_discriminators: Library with a set of code tracking and carrier tracking discriminators that is used by the tracking algorithms. (fully documented, including math algorithms using doxygen!)
        -  tracking_2rd_DLL_filter: Class that implements 2 order DLL filter for code tracking loop.
        -  tracking_2rd_PLL_filter: Class that implements 2 order PLL filter for carrier tracking loop.
        -  tracking_FLL_PLL_filter:  Class that implements hybrid FLL and PLL filter for tracking carrier loop.
        -  CN_estimators: Library with a set of Carrier to Noise estimators and lock detectors. (fully documented, including math algorithms using doxygen!)

    Tracking:
        - gps_l1_ca_dll_pll_tracking: The existing DLL + PLL tracking module, which is the K.Borre and D.Akos one, is now completely re-factored. Now uses the above described libraries.
        - gps_l1_ca_dll_fll_pll_tracking: This is a brand new tracking module, which implements the FLL assisted PLL described in Kaplan (2nd edition). (also documentedwith references)

    Configuration options:
        - The following tracking parameters are added:
            ;######### TRACKING CONFIG ############
            ; Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking or GPS_L1_CA_DLL_FLL_PLL_Tracking 
            Tracking.implementation=GPS_L1_CA_DLL_FLL_PLL_Tracking
            ;PLL filter bandwidth in Hz.
            Tracking.pll_bw_hz=50.0;
            ;DLL filter bandwidth in Hz.
            Tracking.dll_bw_hz=2.0;
            ;FLL filter bandwidth in Hz.
            Tracking.fll_bw_hz=50;
            ;filter order: choice between 2 or 3 at this moment, only for FLL assisted PLL
            Tracking.order=2;
            ;Correlator space in chips units
            Tracking.early_late_space_chips=0.5;

Other files have also been modified with minor changes to adapt to new modules or minor bug fixes.


git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@80 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Javier Arribas 2011-11-22 17:21:54 +00:00
parent 004cd776d9
commit 34d1c5110b
31 changed files with 2192 additions and 474 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
conf/mercurio.conf
View File

@ -9,14 +9,14 @@ ControlThread.wait_for_flowgraph=false
SignalSource.implementation=File_Signal_Source
;SignalSource.filename=/media/DATALOGGER/signals/spirent scenario 2/data/sc2_d8.dat
;SignalSource.filename=/media/My Passport/KINGSTON (G)/Project Luis/GPSL1_Fs_8MHz_ID_1_CN0_60.dat
SignalSource.filename=/home/luis/Project/signals/cap2/agilent_cap2.dat
SignalSource.filename=/media/DATALOGGER/signals/Agilent GPS Generator/cap2/agilent_cap2.dat
;SignalSource.filename=/home/luis/Project/signals/GPS_L1_8sats_4_Msps_CN0_52.dat
SignalSource.item_type=gr_complex
SignalSource.sampling_frequency=4000000
SignalSource.samples=0
SignalSource.samples=292000000
SignalSource.repeat=false
SignalSource.dump=false
SignalSource.enable_throttle_control=true
SignalSource.enable_throttle_control=false
;######### SIGNAL_CONDITIONER CONFIG ############
SignalConditioner.implementation=Pass_Through
@ -26,7 +26,7 @@ SignalConditioner.sample_freq_out=4000000
SignalConditioner.dump=false
;######### CHANNELS CONFIGURATION CONFIG ############
Channels.count=4
Channels.count=1
;######### ACQUISITION CONFIG ############
@ -42,7 +42,7 @@ Acquisition0.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition0.threshold=440
Acquisition0.doppler_max=10000
Acquisition0.doppler_step=500
Acquisition0.satellite=2
Acquisition0.satellite=14
Acquisition0.repeat_satellite=true
;######### ACQUISITION 1 CONFIG ############
@ -58,7 +58,7 @@ Acquisition2.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition2.threshold=440
Acquisition2.doppler_max=10000
Acquisition2.doppler_step=500
Acquisition2.satellite=11
Acquisition2.satellite=1
Acquisition2.repeat_satellite=true
;######### ACQUISITION 3 CONFIG ############
@ -66,17 +66,22 @@ Acquisition3.implementation=GPS_L1_CA_PCPS_Acquisition
Acquisition3.threshold=440
Acquisition3.doppler_max=10000
Acquisition3.doppler_step=500
Acquisition3.satellite=32
Acquisition3.satellite=1
Acquisition3.repeat_satellite=true
;######### TRACKING CONFIG ############
Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
;Tracking.implementation=GPS_L1_CA_DLL_PLL_Tracking
Tracking.implementation=GPS_L1_CA_DLL_FLL_PLL_Tracking
Tracking.item_type=gr_complex
Tracking.vector_length=4000
Tracking.fs_in=4000000
Tracking.if=0
Tracking.dump=true
Tracking.dump_filename=./trk_dump.dat
Tracking.pll_bw_hz=50.0;
Tracking.dll_bw_hz=2.0;
Tracking.fll_bw_hz=50;
Tracking.order=2;
Tracking.early_late_space_chips=0.5;
;######### TELEMETRY DECODER CONFIG ############
TelemetryDecoder.implementation=GPS_L1_CA_Telemetry_Decoder

1
jamroot.jam
View File

@ -38,6 +38,7 @@ project : requirements
<include>src/algorithms/telemetry_decoder/libs
<include>src/algorithms/tracking/adapters
<include>src/algorithms/tracking/gnuradio_blocks
<include>src/algorithms/tracking/libs
<include>src/core/interfaces
<include>src/core/libs
<include>src/core/receiver

18
src/algorithms/signal_source/adapters/file_signal_source.cc
View File

@ -234,16 +234,16 @@ gr_basic_block_sptr FileSignalSource::get_left_block()
gr_basic_block_sptr FileSignalSource::get_right_block()
{
if (dump_==true)
{
return file_source_;
}else{
if (samples_ != 0)
{
return valve_;
}else
if (samples_ != 0)
{
return valve_;
}else
{
if (enable_throttle_control_ == true)
{
return throttle_;
}else{
return file_source_;
}
}
}
}

8
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc
View File

@ -125,7 +125,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
{
d_GPS_FSM.Event_gps_word_preamble();
d_preamble_index=d_sample_counter;//record the preamble sample stamp
std::cout<<"Pre-detection SAT "<<this->d_satellite+1<<std::endl;
std::cout<<"Pre-detection SAT "<<this->d_satellite<<std::endl;
d_symbol_accumulator=0; //sync the symbol to bits integrator
d_symbol_accumulator_counter=0;
d_frame_bit_index=8;
@ -141,12 +141,12 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
if (!d_flag_frame_sync){
d_flag_frame_sync=true;
std::cout<<" Frame sync SAT "<<this->d_satellite+1<<" with preamble start at "<<in[2][0]<<" [ms]"<<std::endl;
std::cout<<" Frame sync SAT "<<this->d_satellite<<" with preamble start at "<<in[2][0]<<" [ms]"<<std::endl;
}
}else
{
if (preamble_diff>7000){
std::cout<<"lost of frame sync SAT "<<this->d_satellite+1<<std::endl;
std::cout<<"lost of frame sync SAT "<<this->d_satellite<<std::endl;
d_stat=0; //lost of frame sync
d_flag_frame_sync=false;
}
@ -217,7 +217,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_i
//gps_synchro.preamble_delay_ms=(float)d_preamble_index;
gps_synchro.preamble_delay_ms=(float)d_preamble_index;
gps_synchro.prn_delay_ms=in[3][0];
gps_synchro.satellite_PRN=d_satellite+1;
gps_synchro.satellite_PRN=d_satellite;
gps_synchro.channel_ID=d_channel;
*out[0]=gps_synchro;
return 1;

2
src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.cc
View File

@ -147,7 +147,7 @@ void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
subframe_ID=d_nav.subframe_decoder(this->d_subframe); //decode the subframe
d_nav.d_satellite_PRN=d_satellite_PRN+1;
d_nav.d_satellite_PRN=d_satellite_PRN;
d_nav.d_channel_ID=d_channel_ID;
if (subframe_ID==1) {
d_nav.d_subframe1_timestamp_ms=this->d_preamble_time_ms-6002;

167
src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc Normal file
View File

@ -0,0 +1,167 @@
/*!
* \file gps_l1_ca_dll_fll_pll_tracking.cc
* \brief code DLL + carrier FLL/PLL tracking
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* This file implements the code Delay Locked Loop (DLL) + carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) stage
* according to the algorithms described in [1]
* [1] E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
* Applications, Second Edition, Artech House Publishers, 2005.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 "gps_l1_ca_dll_fll_pll_tracking.h"
#include "configuration_interface.h"
#include <gnuradio/gr_io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
using google::LogMessage;
GpsL1CaDllFllPllTracking::GpsL1CaDllFllPllTracking(
ConfigurationInterface* configuration, std::string role,
unsigned int in_streams, unsigned int out_streams,
gr_msg_queue_sptr queue) :
role_(role), in_streams_(in_streams), out_streams_(out_streams), queue_(
queue)
{
DLOG(INFO) << "role " << role;
//DLOG(INFO) << "vector length " << vector_length;
//################# 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_bw_hz;
float fll_bw_hz;
float dll_bw_hz;
float early_late_space_chips;
int order;
item_type = configuration->property(role + ".item_type",default_item_type);
vector_length = configuration->property(role + ".vector_length", 2048);
fs_in = configuration->property(role + ".fs_in", 2048000);
f_if = configuration->property(role + ".if", 0);
dump = configuration->property(role + ".dump", false);
order = configuration->property(role + ".order", 2);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
fll_bw_hz = configuration->property(role + ".fll_bw_hz", 100.0);
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./tracking.dat";
dump_filename = configuration->property(role + ".dump_filename",
default_dump_filename); //unused!
//################# MAKE TRACKING GNURadio object ###################
if (item_type.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
tracking_ = gps_l1_ca_dll_fll_pll_make_tracking_cc(satellite_, f_if,
fs_in, vector_length, queue_, dump, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,early_late_space_chips);
}
else
{
LOG_AT_LEVEL(WARNING) << item_type << " unknown tracking item type.";
}
DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
}
GpsL1CaDllFllPllTracking::~GpsL1CaDllFllPllTracking()
{
}
void GpsL1CaDllFllPllTracking::start_tracking()
{
tracking_->start_tracking();
}
void GpsL1CaDllFllPllTracking::set_satellite(unsigned int satellite)
{
satellite_ = satellite;
tracking_->set_satellite(satellite);
DLOG(INFO) << "satellite set to " << satellite_;
}
void GpsL1CaDllFllPllTracking::set_channel(unsigned int channel)
{
channel_ = channel;
tracking_->set_channel(channel);
}
void GpsL1CaDllFllPllTracking::set_channel_queue(
concurrent_queue<int> *channel_internal_queue)
{
channel_internal_queue_ = channel_internal_queue;
tracking_->set_channel_queue(channel_internal_queue_);
}
void GpsL1CaDllFllPllTracking::set_prn_code_phase(signed int phase_samples)
{
return tracking_->set_acq_code_phase((float)phase_samples);
}
void GpsL1CaDllFllPllTracking::set_doppler_freq_shift(float doppler_freq_hz)
{
return tracking_->set_acq_doppler(doppler_freq_hz);
}
void GpsL1CaDllFllPllTracking::set_acq_sample_stamp(
unsigned long int sample_stamp)
{
return tracking_->set_acq_sample_stamp(sample_stamp);
}
void GpsL1CaDllFllPllTracking::connect(gr_top_block_sptr top_block)
{
//nothing to connect, now the tracking uses gr_sync_decimator
}
void GpsL1CaDllFllPllTracking::disconnect(gr_top_block_sptr top_block)
{
//nothing to disconnect, now the tracking uses gr_sync_decimator
}
gr_basic_block_sptr GpsL1CaDllFllPllTracking::get_left_block()
{
return tracking_;
}
gr_basic_block_sptr GpsL1CaDllFllPllTracking::get_right_block()
{
return tracking_;
}

104
src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h Normal file
View File

@ -0,0 +1,104 @@
/*!
* \file gps_l1_ca_dll_fll_pll_tracking.h
* \brief code DLL + carrier FLL/PLL tracking
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* This file implements the code Delay Locked Loop (DLL) + carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) stage
* according to the algorithms described in [1]
* [1] E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
* Applications, Second Edition, Artech House Publishers, 2005.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_
#define GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_
#include "tracking_interface.h"
#include "gps_l1_ca_dll_fll_pll_tracking_cc.h"
#include <gnuradio/gr_msg_queue.h>
class ConfigurationInterface;
class GpsL1CaDllFllPllTracking : public TrackingInterface
{
public:
GpsL1CaDllFllPllTracking(ConfigurationInterface* configuration,
std::string role,
unsigned int in_streams,
unsigned int out_streams,
gr_msg_queue_sptr queue);
virtual ~GpsL1CaDllFllPllTracking();
std::string role()
{
return role_;
}
std::string implementation()
{
return "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();
void set_satellite(unsigned int satellite);
void set_channel(unsigned int channel);
void set_prn_code_phase(signed int phase_samples);
void set_doppler_freq_shift(float doppler_freq_hz);
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
void start_tracking();
void set_acq_sample_stamp(unsigned long int sample_stamp);
private:
gps_l1_ca_dll_fll_pll_tracking_cc_sptr tracking_;
size_t item_size_;
unsigned int satellite_;
unsigned int channel_;
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
gr_msg_queue_sptr queue_;
concurrent_queue<int> *channel_internal_queue_;
};
#endif // GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_

66
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc
View File

@ -52,31 +52,45 @@ GpsL1CaDllPllTracking::GpsL1CaDllPllTracking(
queue)
{
std::string default_item_type = "gr_complex";
std::string default_dump_filename = "./tracking.dat";
DLOG(INFO) << "role " << role;
DLOG(INFO) << "vector length " << vector_length_;
//DLOG(INFO) << "vector length " << vector_length;
item_type_ = configuration->property(role + ".item_type",
default_item_type);
vector_length_ = configuration->property(role + ".vector_length", 2048);
fs_in_ = configuration->property(role + ".fs_in", 2048000);
if_ = configuration->property(role + ".if", 0);
dump_ = configuration->property(role + ".dump", false);
dump_filename_ = configuration->property(role + ".dump_filename",
//################# 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_bw_hz;
float dll_bw_hz;
float early_late_space_chips;
item_type = configuration->property(role + ".item_type",default_item_type);
vector_length = configuration->property(role + ".vector_length", 2048);
fs_in = configuration->property(role + ".fs_in", 2048000);
f_if = configuration->property(role + ".if", 0);
dump = configuration->property(role + ".dump", false);
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
std::string default_dump_filename = "./tracking.dat";
dump_filename = configuration->property(role + ".dump_filename",
default_dump_filename); //unused!
if (item_type_.compare("gr_complex") == 0)
//################# MAKE TRACKING GNURadio object ###################
if (item_type.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
tracking_ = gps_l1_ca_dll_pll_make_tracking_cc(satellite_, if_,
fs_in_, vector_length_, queue_, dump_);
tracking_ = gps_l1_ca_dll_pll_make_tracking_cc(satellite_, f_if,
fs_in, vector_length, queue_, dump,pll_bw_hz,dll_bw_hz,early_late_space_chips);
}
else
{
LOG_AT_LEVEL(WARNING) << item_type_ << " unknown tracking item type.";
LOG_AT_LEVEL(WARNING) << item_type << " unknown tracking item type.";
}
DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
@ -91,6 +105,9 @@ void GpsL1CaDllPllTracking::start_tracking()
tracking_->start_tracking();
}
/*!
* Set satellite ID
*/
void GpsL1CaDllPllTracking::set_satellite(unsigned int satellite)
{
satellite_ = satellite;
@ -98,12 +115,18 @@ void GpsL1CaDllPllTracking::set_satellite(unsigned int satellite)
DLOG(INFO) << "satellite set to " << satellite_;
}
/*!
* Set tracking channel unique ID
*/
void GpsL1CaDllPllTracking::set_channel(unsigned int channel)
{
channel_ = channel;
tracking_->set_channel(channel);
}
/*!
* Set tracking channel internal queue
*/
void GpsL1CaDllPllTracking::set_channel_queue(
concurrent_queue<int> *channel_internal_queue)
{
@ -112,16 +135,23 @@ void GpsL1CaDllPllTracking::set_channel_queue(
tracking_->set_channel_queue(channel_internal_queue_);
}
/*!
* Set acquisition code phase in samples
*/
void GpsL1CaDllPllTracking::set_prn_code_phase(signed int phase_samples)
{
return tracking_->set_acq_code_phase((float)phase_samples);
}
/*!
* Set acquisition Doppler frequency in Hz.
*/
void GpsL1CaDllPllTracking::set_doppler_freq_shift(float doppler_freq_hz)
{
return tracking_->set_acq_doppler(doppler_freq_hz);
}
/*!
* Set acquisition sample stamp in samples, in order to detect the delay between acquisition and tracking
*/
void GpsL1CaDllPllTracking::set_acq_sample_stamp(
unsigned long int sample_stamp)
{

7
src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h
View File

@ -90,14 +90,9 @@ private:
gps_l1_ca_dll_pll_tracking_cc_sptr tracking_;
size_t item_size_;
std::string item_type_;
unsigned int vector_length_;
unsigned int satellite_;
unsigned int channel_;
long fs_in_;
long if_;
bool dump_;
std::string dump_filename_;
std::string role_;
unsigned int in_streams_;

3
src/algorithms/tracking/adapters/jamfile.jam
View File

@ -1,3 +1,4 @@
project : build-dir ../../../../build ;
obj gps_l1_ca_dll_pll_tracking : gps_l1_ca_dll_pll_tracking.cc ;
obj gps_l1_ca_dll_pll_tracking : gps_l1_ca_dll_pll_tracking.cc ;
obj gps_l1_ca_dll_fll_pll_tracking : gps_l1_ca_dll_fll_pll_tracking.cc ;

510
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc Normal file
View File

@ -0,0 +1,510 @@
/*!
* \file gps_l1_ca_dll_fll_pll_tracking_cc.cc
* \brief code DLL + carrier FLL/PLL tracking
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* This file implements the code Delay Locked Loop (DLL) + carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) stage
* according to the algorithms described in [1]
* [1] E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
* Applications, Second Edition, Artech House Publishers, 2005.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 "gps_l1_ca_dll_fll_pll_tracking_cc.h"
#include "gps_sdr_signal_processing.h"
#include "GPS_L1_CA.h"
#include "tracking_discriminators.h"
#include "CN_estimators.h"
#include "tracking_FLL_PLL_filter.h"
#include "control_message_factory.h"
#include <boost/lexical_cast.hpp>
#include <iostream>
#include <sstream>
#include <cmath>
#include "math.h"
#include <gnuradio/gr_io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
/*!
* \todo Include in definition header file
*/
#define CN0_ESTIMATION_SAMPLES 10
using google::LogMessage;
gps_l1_ca_dll_fll_pll_tracking_cc_sptr
gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
return gps_l1_ca_dll_fll_pll_tracking_cc_sptr(new gps_l1_ca_dll_fll_pll_tracking_cc(satellite, if_freq,
fs_in, vector_length, queue, dump, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,early_late_space_chips));
}
void gps_l1_ca_dll_fll_pll_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required){
ninput_items_required[0] =d_vector_length*2; //set the required available samples in each call
}
gps_l1_ca_dll_fll_pll_tracking_cc::gps_l1_ca_dll_fll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump, int order, float fll_bw_hz, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
gr_block ("gps_l1_ca_dll_fll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
gr_make_io_signature(5, 5, sizeof(float))) {
//gr_sync_decimator ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
// gr_make_io_signature(3, 3, sizeof(float)),vector_length) {
// initialize internal vars
d_queue = queue;
d_dump = dump;
d_satellite = satellite;
d_if_freq = if_freq;
d_fs_in = fs_in;
d_vector_length = vector_length;
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
// Initialize tracking variables ==========================================
d_carrier_loop_filter.set_params(fll_bw_hz,pll_bw_hz,order);
// Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code=new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+2];
// Get space for the resampled early / prompt / late local replicas
d_early_code= new gr_complex[d_vector_length*2];
d_prompt_code=new gr_complex[d_vector_length*2];
d_late_code=new gr_complex[d_vector_length*2];
// space for carrier wipeoff LO vector
d_carr_sign=new gr_complex[d_vector_length*2];
// sample synchronization
d_sample_counter=0;
d_acq_sample_stamp=0;
d_last_seg=0;// this is for debug output only
d_enable_tracking=false;
d_current_prn_length_samples=(int)d_vector_length;
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter=0;
d_Prompt_buffer=new gr_complex[CN0_ESTIMATION_SAMPLES];
d_carrier_lock_test=1;
d_CN0_SNV_dB_Hz=0;
d_carrier_lock_fail_counter=0;
d_carrier_lock_threshold=5;
}
void gps_l1_ca_dll_fll_pll_tracking_cc::start_tracking(){
/*!
* correct the code phase according to the delay between acq and trk
*/
unsigned long int acq_trk_diff_samples;
float acq_trk_diff_seconds;
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp-d_vector_length;
acq_trk_diff_seconds=acq_trk_diff_samples/(float)d_fs_in;
//doppler effect
// Fd=(C/(C+Vr))*F
float radial_velocity;
radial_velocity=(GPS_L1_FREQ_HZ+d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
float T_chip_mod_seconds;
float T_prn_mod_seconds;
float T_prn_mod_samples;
d_code_freq_hz=radial_velocity*GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds=1/d_code_freq_hz;
T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in;
d_next_prn_length_samples=round(T_prn_mod_samples);
//compute the code phase chips prediction
float delta_T_prn_samples;
float delay_correction_samples;
delta_T_prn_samples=fmod((float)acq_trk_diff_samples,T_prn_mod_samples);
delay_correction_samples=T_prn_mod_samples-delta_T_prn_samples;
d_acq_code_phase_samples=d_acq_code_phase_samples-delay_correction_samples;
if (d_acq_code_phase_samples<0){
d_acq_code_phase_samples=d_acq_code_phase_samples+T_prn_mod_samples;
}
d_carrier_doppler_hz=d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz);
d_FLL_wait=1;
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
code_gen_conplex(&d_ca_code[1],d_satellite,0);
d_ca_code[0]=d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+1]=d_ca_code[1];
d_carrier_lock_fail_counter=0;
d_Prompt_prev=0;
d_rem_code_phase_samples=0;
d_rem_carr_phase=0;
d_FLL_discriminator_hz=0;
d_rem_code_phase_samples=0;
d_next_rem_code_phase_samples=0;
d_acc_carrier_phase_rad=0;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
{
if (d_dump_file.is_open()==false)
{
try {
d_dump_filename="track_ch"; //base path and name for the tracking log file
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
}
}
}
// DEBUG OUTPUT
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl;
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
// enable tracking
d_pull_in=true;
d_enable_tracking=true;
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" PULL-IN Code Phase [chips]= "<<d_acq_code_phase_samples<<"\r\n";
}
void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_code()
{
float tcode_chips;
float rem_code_phase_chips;
float code_phase_step_chips;
int associated_chip_index;
int code_length_chips=(int)GPS_L1_CA_CODE_LENGTH_CHIPS;
code_phase_step_chips=d_code_freq_hz/((float)d_fs_in);
rem_code_phase_chips=d_rem_code_phase_samples*(d_code_freq_hz/d_fs_in);
// unified loop for E, P, L code vectors
tcode_chips=-rem_code_phase_chips;
for (int i=0;i<d_current_prn_length_samples;i++)
{
associated_chip_index=1+round(fmod(tcode_chips-d_early_late_spc_chips,code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index];
associated_chip_index = 1+round(fmod(tcode_chips, code_length_chips));
d_prompt_code[i] = d_ca_code[associated_chip_index];
associated_chip_index = 1+round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips));
d_late_code[i] = d_ca_code[associated_chip_index];
tcode_chips=tcode_chips+code_phase_step_chips;
}
}
void gps_l1_ca_dll_fll_pll_tracking_cc::update_local_carrier()
{
float phase, phase_step;
phase_step = (float)TWO_PI*d_carrier_doppler_hz/d_fs_in;
phase=d_rem_carr_phase;
for(int i = 0; i < d_current_prn_length_samples; i++) {
d_carr_sign[i] = gr_complex(cos(phase),sin(phase));
phase += phase_step;
}
d_rem_carr_phase=fmod(phase,TWO_PI);
d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+d_rem_carr_phase;
}
gps_l1_ca_dll_fll_pll_tracking_cc::~gps_l1_ca_dll_fll_pll_tracking_cc() {
d_dump_file.close();
delete d_ca_code;
delete d_early_code;
delete d_prompt_code;
delete d_late_code;
delete d_carr_sign;
delete d_Prompt_buffer;
}
/*! Tracking signal processing
* Notice that this is a class derived from gr_sync_decimator, so each of the ninput_items has vector_length samples
*/
int gps_l1_ca_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
if (d_enable_tracking==true){
/*!
* Receiver signal alignment
*/
if (d_pull_in==true)
{
int samples_offset=round(d_acq_code_phase_samples);
d_sample_counter+=samples_offset; //count for the processed samples
d_pull_in=false;
std::cout<<" samples_offset "<<samples_offset<<"\r\n";
consume_each(samples_offset); //shift input to perform alignement with local replica
return 1;
}
// get the sample in and out pointers
const gr_complex* in = (gr_complex*) input_items[0]; //block input samples pointer
float **out = (float **) &output_items[0]; //block output streams pointer
// Update the prn length based on code freq (variable) and
// sampling frequency (fixed)
// variable code PRN sample block size
d_current_prn_length_samples=d_next_prn_length_samples;
update_local_code();
update_local_carrier();
gr_complex bb_signal_sample(0,0);
d_Prompt_prev=d_Prompt; // for the FLL discriminator
d_Early=gr_complex(0,0);
d_Prompt=gr_complex(0,0);
d_Late=gr_complex(0,0);
// perform Early, Prompt and Late correlation
/*!
* \todo Use SIMD-enabled correlators
*/
for(int i=0;i<d_current_prn_length_samples;i++) {
//Perform the carrier wipe-off
bb_signal_sample = in[i] * d_carr_sign[i];
// Now get early, late, and prompt values for each
d_Early += bb_signal_sample*d_early_code[i];
d_Prompt += bb_signal_sample*d_prompt_code[i];
d_Late += bb_signal_sample*d_late_code[i];
}
/*!
* DLL, FLL, and PLL discriminators
*/
// Compute DLL error
float code_error_chips;
code_error_chips=dll_nc_e_minus_l_normalized(d_Early,d_Late);
//compute FLL error
float correlation_time_s;
correlation_time_s=((float)d_current_prn_length_samples)/(float)d_fs_in;
if (d_FLL_wait==1)
{
d_Prompt_prev=d_Prompt;
d_FLL_wait=0;
}else{
d_FLL_discriminator_hz=fll_four_quadrant_atan(d_Prompt_prev, d_Prompt, 0, correlation_time_s)/(float)TWO_PI;
d_Prompt_prev=d_Prompt;
d_FLL_wait=1;
}
// Compute PLL error
float PLL_discriminator_hz;
PLL_discriminator_hz=pll_cloop_two_quadrant_atan(d_Prompt)/(float)TWO_PI;
//PLL_discriminator_hz=pll_four_quadrant_atan(d_Prompt)/(float)TWO_PI;
/*!
* \todo Update FLL assistance algorithm!
*/
if (((float)d_sample_counter-(float)d_acq_sample_stamp)/(float)d_fs_in>3)
{
d_FLL_discriminator_hz=0; //disconnect the FLL after the initial lock
}
/*!
* DLL and FLL+PLL filter and get current carrier Doppler and code frequency
*/
float carr_nco_hz;
carr_nco_hz=d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz,PLL_discriminator_hz,correlation_time_s);
d_carrier_doppler_hz = (float)d_if_freq + carr_nco_hz;
d_code_freq_hz= GPS_L1_CA_CODE_RATE_HZ- (((d_carrier_doppler_hz - (float)d_if_freq)*GPS_L1_CA_CODE_RATE_HZ)/GPS_L1_FREQ_HZ)-code_error_chips;
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_Prompt_buffer[d_cn0_estimation_counter]=d_Prompt;
d_cn0_estimation_counter++;
}else{
d_cn0_estimation_counter=0;
d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in);
d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES);
// ###### TRACKING UNLOCK NOTIFICATION #####
int tracking_message;
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>30)
{
d_carrier_lock_fail_counter++;
}else{
if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter>300)
{
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
tracking_message=3; //loss of lock
d_channel_internal_queue->push(tracking_message);
d_carrier_lock_fail_counter=0;
d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine
}
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
}
// ########### Output the tracking data to navigation and PVT ##########
// Output channel 1: Prompt correlator output Q
*out[0]=d_Early.real();
// Output channel 2: Prompt correlator output I
*out[1]=d_Early.imag();
// Output channel 3: PRN absolute delay [ms]
*out[2]=(float)(((double)d_sample_counter/(double)d_fs_in)*1000.0);
// Output channel 4: PRN code error [ms]
*out[3]=d_acc_carrier_phase_rad;
if(d_dump) {
// MULTIPLEXED FILE RECORDING - Record results to file
float prompt_I;
float prompt_Q;
float tmp_E,tmp_P,tmp_L;
float tmp_float;
prompt_I=d_Prompt.imag();
prompt_Q=d_Prompt.real();
tmp_E=std::abs<float>(d_Early);
tmp_P=std::abs<float>(d_Prompt);
tmp_L=std::abs<float>(d_Late);
try {
// EPR
d_dump_file.write((char*)&tmp_E, sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float));
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_I, sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float));
// PRN start sample stamp
tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&tmp_float, sizeof(float));
// accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
// carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
//PLL commands
d_dump_file.write((char*)&PLL_discriminator_hz, sizeof(float));
d_dump_file.write((char*)&carr_nco_hz, sizeof(float));
//DLL commands
d_dump_file.write((char*)&code_error_chips, sizeof(float));
d_dump_file.write((char*)&code_error_chips, sizeof(float));
// CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes)
tmp_float=d_FLL_discriminator_hz;
d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=0.0;
d_dump_file.write((char*)&tmp_float, sizeof(float));
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
}
}
// ########## DEBUG OUTPUT
/*!
* \todo The stop timer has to be moved to the signal source!
*/
// debug: Second counter in channel 0
if (d_channel==0)
{
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"t="<<d_last_seg<<std::endl;
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}else
{
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
//predict the next loop PRN period length prediction
//float T_chip_seconds,T_prn_seconds,T_prn_samples;
//T_chip_seconds=1/d_code_freq_hz;
//T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS-d_rem_code_phase_chips*T_chip_seconds;
//T_prn_samples=T_prn_seconds*(float)d_fs_in;
//d_next_prn_length_samples=round(T_prn_samples);
float T_chip_seconds;
float T_prn_seconds;
float T_prn_samples;
float K_blk_samples;
T_chip_seconds=1/d_code_freq_hz;
T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples=T_prn_seconds*d_fs_in;
d_rem_code_phase_samples=d_next_rem_code_phase_samples;
K_blk_samples=T_prn_samples+d_rem_code_phase_samples;
d_next_prn_length_samples=round(K_blk_samples);
d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples;
}
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter+=d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}
void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_code_phase(float code_phase) {
d_acq_code_phase_samples = code_phase;
LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
}
void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_doppler(float doppler) {
d_acq_carrier_doppler_hz = doppler;
LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
}
void gps_l1_ca_dll_fll_pll_tracking_cc::set_satellite(unsigned int satellite) {
d_satellite = satellite;
LOG_AT_LEVEL(INFO) << "Tracking Satellite set to " << d_satellite;
}
void gps_l1_ca_dll_fll_pll_tracking_cc::set_channel(unsigned int channel) {
d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
}
void gps_l1_ca_dll_fll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)
{
d_acq_sample_stamp = sample_stamp;
}
void gps_l1_ca_dll_fll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{
d_channel_internal_queue = channel_internal_queue;
}

201
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h Normal file
View File

@ -0,0 +1,201 @@
/*!
* \file gps_l1_ca_dll_fll_pll_tracking_cc.h
* \brief code DLL + carrier FLL/PLL tracking
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* This file implements the code Delay Locked Loop (DLL) +
* carrier Phase Locked Loop (PLL) helped with a carrier Frequency Locked Loop (FLL) stage
* according to the algorithms described in [1]
* [1] E.D. Kaplan and C. Hegarty, Understanding GPS. Principles and
* Applications, Second Edition, Artech House Publishers, 2005.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H
#define GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H
#include <fstream>
#include <gnuradio/gr_block.h>
#include <gnuradio/gr_msg_queue.h>
//#include <gnuradio/gr_sync_decimator.h>
#include "gps_sdr_signal_processing.h"
#include "tracking_FLL_PLL_filter.h"
#include <queue>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include "concurrent_queue.h"
class gps_l1_ca_dll_fll_pll_tracking_cc;
typedef boost::shared_ptr<gps_l1_ca_dll_fll_pll_tracking_cc>
gps_l1_ca_dll_fll_pll_tracking_cc_sptr;
gps_l1_ca_dll_fll_pll_tracking_cc_sptr
gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite,
long if_freq,
long fs_in,
unsigned int vector_length,
gr_msg_queue_sptr queue,
bool dump,
int order,
float fll_bw_hz,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
//class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
class gps_l1_ca_dll_fll_pll_tracking_cc: public gr_block
{
private:
friend gps_l1_ca_dll_fll_pll_tracking_cc_sptr
gps_l1_ca_dll_fll_pll_make_tracking_cc(unsigned int satellite,
long if_freq,
long fs_in, unsigned
int vector_length,
gr_msg_queue_sptr queue,
bool dump,
int order,
float fll_bw_hz,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
gps_l1_ca_dll_fll_pll_tracking_cc(unsigned int satellite,
long if_freq,
long fs_in, unsigned
int vector_length,
gr_msg_queue_sptr queue,
bool dump,
int order,
float fll_bw_hz,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
void CN0_estimation_and_lock_detectors();
// class private vars
gr_msg_queue_sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue;
unsigned int d_vector_length;
bool d_dump;
unsigned int d_satellite;
unsigned int d_channel;
int d_last_seg;
long d_if_freq;
long d_fs_in;
gr_complex* d_ca_code;
gr_complex* d_early_code;
gr_complex* d_late_code;
gr_complex* d_prompt_code;
gr_complex* d_carr_sign;
gr_complex d_Early;
gr_complex d_Prompt;
gr_complex d_Prompt_prev;
gr_complex d_Late;
float d_early_late_spc_chips;
float d_carrier_doppler_hz;
float d_code_freq_hz;
int d_current_prn_length_samples;
int d_next_prn_length_samples;
int d_FLL_wait;
float d_rem_carr_phase;
float d_rem_code_phase_samples;
float d_next_rem_code_phase_samples;
bool d_pull_in;
// acquisition
float d_acq_code_phase_samples;
float d_acq_carrier_doppler_hz;
// FLL + PLL filter
float d_FLL_discriminator_hz; // This is a class variable because FLL needs to have memory
tracking_FLL_PLL_filter d_carrier_loop_filter;
float d_acc_carrier_phase_rad;
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;
bool d_enable_tracking;
std::string d_dump_filename;
std::ofstream d_dump_file;
public:
~gps_l1_ca_dll_fll_pll_tracking_cc();
void set_satellite(unsigned int satellite);
void set_channel(unsigned int channel);
void set_acq_code_phase(float code_phase);
void set_acq_doppler(float doppler);
void start_tracking();
void update_local_code();
void update_local_carrier();
void set_FLL_and_PLL_BW(float fll_bw_hz,float pll_bw_hz);
void set_acq_sample_stamp(unsigned long int sample_stamp);
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
/*!
* \brief just like gr_block::general_work, only this arranges to call consume_each for you
*
* The user must override work to define the signal processing code
*/
//virtual int work (int noutput_items,
// gr_vector_const_void_star &input_items,
// gr_vector_void_star &output_items) = 0;
//int work(int noutput_items, gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
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);
};
#endif //GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H

605
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc
View File

@ -32,15 +32,12 @@
*
* -------------------------------------------------------------------------
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include "gps_l1_ca_dll_pll_tracking_cc.h"
#include "gps_sdr_signal_processing.h"
#include "gps_sdr_simd.h"
#include "gps_sdr_x86.h"
#include "tracking_discriminators.h"
#include "CN_estimators.h"
#include "GPS_L1_CA.h"
#include "control_message_factory.h"
#include <boost/lexical_cast.hpp>
@ -51,8 +48,6 @@
#include <gnuradio/gr_io_signature.h>
#include <glog/log_severity.h>
#include <glog/logging.h>
@ -65,19 +60,19 @@ using google::LogMessage;
gps_l1_ca_dll_pll_tracking_cc_sptr
gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump) {
int vector_length, gr_msg_queue_sptr queue, bool dump, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) {
return gps_l1_ca_dll_pll_tracking_cc_sptr(new gps_l1_ca_dll_pll_tracking_cc(satellite, if_freq,
fs_in, vector_length, queue, dump));
fs_in, vector_length, queue, dump, pll_bw_hz, dll_bw_hz, early_late_space_chips));
}
void gps_l1_ca_dll_pll_tracking_cc::forecast (int noutput_items,
gr_vector_int &ninput_items_required){
ninput_items_required[0] =d_vector_length*2; //set the required available samples in each call
ninput_items_required[0] =(int)d_vector_length*2; //set the required available samples in each call
}
gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satellite, long if_freq, long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue, bool dump) :
int vector_length, gr_msg_queue_sptr queue, bool dump, float pll_bw_hz, float dll_bw_hz, float early_late_space_chips) :
gr_block ("gps_l1_ca_dll_pll_tracking_cc", gr_make_io_signature (1, 1, sizeof(gr_complex)),
gr_make_io_signature(5, 5, sizeof(float))) {
@ -91,31 +86,19 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
d_fs_in = fs_in;
d_vector_length = vector_length;
// Initialize tracking variables ==========================================
/*!
* \todo Include PLL and DLL filter setting in configuration file
*/
//std::cout<<"pll_bw_hz= "<<pll_bw_hz<<"dll_bw_hz="<<dll_bw_hz<<"\r\n";
// Initialize tracking ==========================================
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
d_carrier_loop_filter.set_PLL_BW(pll_bw_hz);
//--- DLL variables --------------------------------------------------------
d_early_late_spc = 0.5; // Define early-late offset (in chips)
d_pdi_code = 0.001;// Summation interval for code
d_dllnoisebandwidth=1; //Hz
d_dlldampingratio=0.7;
calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio,1.0);// Calculate filter coefficient values
//--- PLL variables --------------------------------------------------------
d_pdi_carr = 0.001;// Summation interval for carrier
d_plldampingratio=0.7;
d_pllnoisebandwidth=50;
//Calculate filter coefficient values
calculate_lopp_coef(&d_tau1_carr, &d_tau2_carr, d_pllnoisebandwidth, d_plldampingratio,0.25);// Calculate filter coefficient values
d_early_late_spc_chips = early_late_space_chips; // Define early-late offset (in chips)
// Initialization of local code replica
d_code_length=1023;
// Get space for a vector with the C/A code replica sampled 1x/chip
d_ca_code=new gr_complex[d_code_length+2];
d_ca_code=new gr_complex[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+2];
// Get space for the resampled early / prompt / late local replicas
d_early_code= new gr_complex[d_vector_length*2];
@ -124,29 +107,14 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
// space for carrier wipeoff and signal baseband vectors
d_carr_sign=new gr_complex[d_vector_length*2];
d_bb_sign=new gr_complex[d_vector_length*2];
//--- Perform initializations ------------------------------
// define initial code frequency basis of NCO
d_code_freq = 1023000; //Hz
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ;
// define residual code phase (in chips)
d_rem_code_phase = 0.0;
// define carrier frequency which is used over whole tracking period
// it must be set with set_acq_code_phase() and set_acq_doppler()
d_rem_code_phase_samples = 0.0;
// define residual carrier phase
d_rem_carr_phase = 0.0;
// code tracking loop parameters
d_old_code_nco = 0.0;
d_old_code_error = 0.0;
// carrier/Costas loop parameters
d_old_carr_nco = 0.0;
d_old_carr_error = 0.0;
d_absolute_code_phase_samples = 0;
d_rem_carr_phase_rad = 0.0;
// sample synchronization
d_sample_counter=0;
@ -156,142 +124,135 @@ gps_l1_ca_dll_pll_tracking_cc::gps_l1_ca_dll_pll_tracking_cc(unsigned int satell
d_pull_in=false;
d_last_seg=0;
d_blksize=d_vector_length;
d_loops_count=0;
d_current_prn_length_samples=(int)d_vector_length;
// CN0 estimation and lock detector buffers
d_cn0_estimation_counter=0;
d_P_I_buffer=new float[CN0_ESTIMATION_SAMPLES];
d_P_Q_buffer=new float[CN0_ESTIMATION_SAMPLES];
d_Prompt_buffer=new gr_complex[CN0_ESTIMATION_SAMPLES];
d_carrier_lock_test=1;
d_SNR_SNV=0;
d_SNR_SNV_dB_Hz=0;
d_SNR_MM=0;
d_CN0_SNV_dB_Hz=0;
d_carrier_lock_fail_counter=0;
d_carrier_lock_threshold=5;
}
void gps_l1_ca_dll_pll_tracking_cc::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
// Solve natural frequency
float Wn;
Wn = lbw*8*zeta / (4*zeta*zeta + 1);
// solve for t1 & t2
*tau1 = k / (Wn * Wn);
*tau2 = (2.0 * zeta) / Wn;
}
void gps_l1_ca_dll_pll_tracking_cc::start_tracking(){
/*!
* correct the code phase according to the delay between acq and trk
*/
unsigned long int acq_trk_diff_samples;
float acq_trk_diff_seconds;
acq_trk_diff_samples=d_sample_counter-d_acq_sample_stamp-d_vector_length;
acq_trk_diff_seconds=acq_trk_diff_samples/(float)d_fs_in;
//doppler effect
// Fd=(C/(C+Vr))*F
float radial_velocity;
radial_velocity=(GPS_L1_FREQ_HZ+d_acq_carrier_doppler_hz)/GPS_L1_FREQ_HZ;
// new chip and prn sequence periods based on acq Doppler
float T_chip_mod_seconds;
float T_prn_mod_seconds;
float T_prn_mod_samples;
d_code_freq_hz=radial_velocity*GPS_L1_CA_CODE_RATE_HZ;
T_chip_mod_seconds=1/d_code_freq_hz;
T_prn_mod_seconds=T_chip_mod_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_mod_samples=T_prn_mod_seconds*(float)d_fs_in;
unsigned long int acq_sample_difference;
int trk_corrected_code_phase;
d_code_phase_step_chips = d_code_freq_hz / (float)d_fs_in; //[chips]
d_next_prn_length_samples=round(T_prn_mod_samples);
//compute the code phase chips prediction
float delta_T_prn_samples;
float delay_correction_samples;
delta_T_prn_samples=fmod((float)acq_trk_diff_samples,T_prn_mod_samples);
delay_correction_samples=T_prn_mod_samples-delta_T_prn_samples;
d_acq_code_phase_samples=d_acq_code_phase_samples-delay_correction_samples;
if (d_acq_code_phase_samples<0){
d_acq_code_phase_samples=d_acq_code_phase_samples+T_prn_mod_samples;
}
d_carrier_doppler_hz=d_acq_carrier_doppler_hz;
// DLL/PLL filter initialization
d_carrier_loop_filter.initialize(d_carrier_doppler_hz); //initialize the carrier filter
d_code_loop_filter.initialize(d_acq_code_phase_samples); //initialize the code filter
acq_sample_difference=this->d_sample_counter-d_acq_sample_stamp-d_vector_length;
float velocity_ratio,code_freq_mod,T_prn,T_prn_mod,T_chip_mod;
const float carrier_freq=1575420000;
velocity_ratio=(carrier_freq+d_carrier_doppler)/carrier_freq;
code_freq_mod=velocity_ratio*d_code_freq;
T_prn=(1/d_code_freq)*(float)d_code_length;
T_chip_mod=1/code_freq_mod;
T_prn_mod=T_chip_mod*(float)d_code_length;
//compute the code phase chips prediction
trk_corrected_code_phase=round(fmod((d_code_phase+(float)acq_sample_difference+(T_prn-T_prn_mod)*((float)acq_sample_difference/(float)d_vector_length)*(float)d_fs_in),(float)d_vector_length));
if (trk_corrected_code_phase<0)
{
trk_corrected_code_phase=d_vector_length+trk_corrected_code_phase;
}
d_absolute_code_phase_samples=(float)trk_corrected_code_phase;
// generate local reference ALWAYS starting at chip 1, not corrected
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
code_gen_conplex(&d_ca_code[1],d_satellite,0);
d_ca_code[0]=d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS];
d_ca_code[(int)GPS_L1_CA_CODE_LENGTH_CHIPS+1]=d_ca_code[1];
// Then make it possible to do early and late versions
d_ca_code[0]=d_ca_code[1023];
d_ca_code[1024]=d_ca_code[1];
d_carrier_lock_fail_counter=0;
d_rem_code_phase_samples=0;
d_next_rem_code_phase_samples=0;
d_rem_carr_phase_rad=0;
d_acc_carrier_phase_rad=0;
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
if (d_dump==true)
{
//std::stringstream d_dump_filename_str;//create a stringstream to form the dump filename
//d_dump_filename_str<<"./data/trk_epl_CH_"<<this->d_channel<<"_SAT_"<<this->d_satellite<<".dat";
//d_dump_filename=d_dump_filename_str.str();
if (d_dump_file.is_open()==false)
{
try {
d_dump_filename="track_ch"; //base path and name for the tracking log file
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
}
}
}
d_carrier_lock_fail_counter=0;
d_pull_in=true;
d_enable_tracking=true;
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID "<< this->d_satellite+1 << std::endl;
// ############# ENABLE DATA FILE LOG #################
if (d_dump==true)
{
if (d_dump_file.is_open()==false)
{
try {
d_dump_filename="track_ch"; //base path and name for the tracking log file
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
d_dump_filename.append(".dat");
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
std::cout<<"Tracking dump enabled on channel "<<d_channel<<" Log file: "<<d_dump_filename.c_str()<<std::endl;
}
catch (std::ifstream::failure e) {
std::cout << "channel "<<d_channel <<" Exception opening trk dump file "<<e.what()<<"\r\n";
}
}
}
// DEBUG OUTPUT
std::cout<<"Tracking start on channel "<<d_channel<<" for satellite ID* "<< this->d_satellite<< std::endl;
DLOG(INFO) << "Start tracking for satellite "<<this->d_satellite<<" received ";
// enable tracking
d_pull_in=true;
d_enable_tracking=true;
std::cout<<"PULL-IN Doppler [Hz]= "<<d_carrier_doppler_hz<<" PULL-IN Code Phase [samples]= "<<d_acq_code_phase_samples<<"\r\n";
}
void gps_l1_ca_dll_pll_tracking_cc::update_local_code_refs()
void gps_l1_ca_dll_pll_tracking_cc::update_local_code()
{
float tcode;
float tcode_chips;
float rem_code_phase_chips;
int associated_chip_index;
int code_length_chips=(int)GPS_L1_CA_CODE_LENGTH_CHIPS;
// unified loop for E, P, L code vectors
for (unsigned int i=0;i<d_blksize;i++)
rem_code_phase_chips=d_rem_code_phase_samples*(d_code_freq_hz/d_fs_in);
tcode_chips=-rem_code_phase_chips;
for (int i=0;i<d_current_prn_length_samples;i++)
{
tcode=i*d_code_phase_step+d_rem_code_phase-d_early_late_spc;
associated_chip_index=ceil(fmod(tcode,d_code_length));
associated_chip_index=1+round(fmod(tcode_chips-d_early_late_spc_chips,code_length_chips));
d_early_code[i] = d_ca_code[associated_chip_index];
tcode += d_early_late_spc;
associated_chip_index = ceil(fmod(tcode, d_code_length));
associated_chip_index = 1+round(fmod(tcode_chips, code_length_chips));
d_prompt_code[i] = d_ca_code[associated_chip_index];
tcode += d_early_late_spc;
associated_chip_index = ceil(fmod(tcode, d_code_length));
associated_chip_index = 1+round(fmod(tcode_chips+d_early_late_spc_chips, code_length_chips));
d_late_code[i] = d_ca_code[associated_chip_index];
tcode_chips=tcode_chips+d_code_phase_step_chips;
}
//**** Option 1: Keep the number of samples per PRN period constant and equal to the nominal value
//**** and record the size mismatch in a var: d_rem_code_phase
//max_tcode=((float)d_vector_length-1.0)*d_code_phase_step+d_rem_code_phase;
//d_rem_code_phase = (max_tcode + d_code_phase_step) - 1023.0;
//d_rem_code_phase = d_rem_code_phase+((float)d_vector_length-1023.0*(1.0/d_code_freq)*(float)d_fs_in)*d_code_phase_step;
//**** Option 2: Each loop, compute the new PRN sequence code length according to the estimated Doppler
tcode=d_blksize*d_code_phase_step+d_rem_code_phase;
d_rem_code_phase = tcode - 1023.0; //prompt remaining code phase
}
void gps_l1_ca_dll_pll_tracking_cc::update_local_carrier()
{
float phase, phase_step;
float phase_rad, phase_step_rad;
phase_step = (float)TWO_PI*d_carrier_doppler/d_fs_in;
phase=d_rem_carr_phase;
for(unsigned int i = 0; i < d_blksize; i++) {
d_carr_sign[i] = std::complex<float>(cos(phase),sin(phase));
phase += phase_step;
phase_step_rad = (float)TWO_PI*d_carrier_doppler_hz/d_fs_in;
phase_rad=d_rem_carr_phase_rad;
for(int i = 0; i < d_current_prn_length_samples; i++) {
d_carr_sign[i] = gr_complex(cos(phase_rad),sin(phase_rad));
phase_rad += phase_step_rad;
}
d_rem_carr_phase=fmod(phase,TWO_PI);
d_rem_carr_phase_rad=fmod(phase_rad,TWO_PI);
d_acc_carrier_phase_rad=d_acc_carrier_phase_rad+d_rem_carr_phase_rad;
}
gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
/*!
* \todo free memory!!
*/
d_dump_file.close();
delete d_ca_code;
delete d_early_code;
delete d_prompt_code;
delete d_late_code;
delete d_carr_sign;
delete d_Prompt_buffer;
}
/*! Tracking signal processing
@ -300,273 +261,205 @@ gps_l1_ca_dll_pll_tracking_cc::~gps_l1_ca_dll_pll_tracking_cc() {
int gps_l1_ca_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) {
d_loops_count++;
if (d_enable_tracking==true){
if (d_pull_in==true)
{
int samples_offset=ceil(d_absolute_code_phase_samples);
d_code_phase_ms=(d_absolute_code_phase_samples*1000.0)/(float)d_fs_in;
consume_each(samples_offset); //shift input to perform alignement with local replica
int samples_offset=ceil(d_acq_code_phase_samples);
consume_each(d_acq_code_phase_samples); //shift input to perform alignement with local replica
d_sample_counter+=samples_offset; //count for the processed samples
d_pull_in=false;
return 1;
}
d_current_prn_length_samples=d_next_prn_length_samples;
float carr_error;
float carr_nco;
float code_error;
float code_nco;
float tmp_E,tmp_P,tmp_L;
const gr_complex* in = (gr_complex*) input_items[0]; //PRN start block alignement
float **out = (float **) &output_items[0];
update_local_code();
update_local_carrier();
gr_complex bb_signal_sample(0,0);
d_Early=gr_complex(0,0);
d_Prompt=gr_complex(0,0);
d_Late=gr_complex(0,0);
// perform Early, Prompt and Late correlation
/*!
* \todo Use SIMD-enabled correlators
*/
for(int i=0;i<d_current_prn_length_samples;i++) {
//Perform the carrier wipe-off
bb_signal_sample = in[i] * d_carr_sign[i];
// Now get early, late, and prompt values for each
d_Early += bb_signal_sample*d_early_code[i];
d_Prompt += bb_signal_sample*d_prompt_code[i];
d_Late += bb_signal_sample*d_late_code[i];
}
// Compute PLL error and update carrier NCO -
carr_error=pll_cloop_two_quadrant_atan(d_Prompt)/ (float)TWO_PI;
// Implement carrier loop filter and generate NCO command
carr_nco=d_carrier_loop_filter.get_carrier_nco(carr_error);
// Modify carrier freq based on NCO command
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_nco;
// Compute DLL error and update code NCO
code_error=dll_nc_e_minus_l_normalized(d_Early,d_Late);
// Implement code loop filter and generate NCO command
code_nco=d_code_loop_filter.get_code_nco(code_error);
// Modify code freq based on NCO command
d_code_freq_hz = GPS_L1_CA_CODE_RATE_HZ - code_nco;
// Update the phasestep based on code freq (variable) and
// sampling frequency (fixed)
// code_phase_step_per_sample = T_sample/T_chip
d_code_phase_step = d_code_freq / (float)d_fs_in; //[chips]
d_code_phase_step_chips = d_code_freq_hz / (float)d_fs_in; //[chips]
// variable code PRN sample block size
d_blksize=ceil((1023.0-d_rem_code_phase) / d_code_phase_step); //[samples]
//float rem_code_phase_samples=d_rem_code_phase/d_code_phase_step;
//d_absolute_code_phase_samples-=floor(rem_code_phase_samples);
//d_rem_code_phase=d_code_phase_step*(rem_code_phase_samples-floor(rem_code_phase_samples));
this->update_local_code_refs();
this->update_local_carrier();
d_E_I=0.0;
d_P_I=0.0;
d_L_I=0.0;
d_E_Q=0.0;
d_P_Q=0.0;
d_L_Q=0.0;
// perform Early, Prompt and Late correlation
for(unsigned int i=0;i<d_blksize;i++) {
//Perform the carrier wipe-off
d_bb_sign[i] = in[i] * d_carr_sign[i];
// Now get early, late, and prompt values for each
d_E_Q += d_early_code[i].real()*d_bb_sign[i].real();
d_E_I += d_early_code[i].real()*d_bb_sign[i].imag();
d_P_Q += d_prompt_code[i].real()*d_bb_sign[i].real();
d_P_I += d_prompt_code[i].real()*d_bb_sign[i].imag();
d_L_Q += d_late_code[i].real()*d_bb_sign[i].real();
d_L_I += d_late_code[i].real()*d_bb_sign[i].imag();
}
//float block_error_samples;
//block_error_samples=(float)d_vector_length-d_blksize;
//d_absolute_code_phase_samples=d_absolute_code_phase_samples+block_error_samples;
// Find PLL error and update carrier NCO -
// Implement carrier loop discriminator (phase detector)
//carr_error = atan(d_P.imag() / d_P.real()) / TWO_PI;
carr_error = atan(d_P_Q / d_P_I) / (float)TWO_PI;
// Implement carrier loop filter and generate NCO command
carr_nco = d_old_carr_nco+(d_tau2_carr/d_tau1_carr)*(carr_error - d_old_carr_error) + carr_error * (d_pdi_carr/d_tau1_carr);
d_old_carr_nco = carr_nco;
d_old_carr_error = carr_error;
// Modify carrier freq based on NCO command
d_carrier_doppler = d_carr_freq_basis + carr_nco;
// Find DLL error and update code NCO -
//code_error = (abs(d_E)-abs(d_L))/(abs(d_E)+abs(d_L));
code_error = (sqrt(d_E_I*d_E_I+d_E_Q*d_E_Q)-sqrt(d_L_I*d_L_I+d_L_Q*d_L_Q))/(sqrt(d_E_I*d_E_I+d_E_Q*d_E_Q)+sqrt(d_L_I*d_L_I+d_L_Q*d_L_Q));
// Implement code loop filter and generate NCO command
code_nco = d_old_code_nco + (d_tau2_code/d_tau1_code)*(code_error - d_old_code_error) + code_error * (d_pdi_code/d_tau1_code);
d_old_code_nco = code_nco;
d_old_code_error = code_error; //[chips]
// Modify code freq based on NCO command
d_code_freq = 1023000 - code_nco;
d_code_phase_ms+=(1023000/d_code_freq)-1.0;
float T_chip_seconds;
float T_prn_seconds;
float T_prn_samples;
float K_blk_samples;
T_chip_seconds=1/d_code_freq_hz;
T_prn_seconds=T_chip_seconds*GPS_L1_CA_CODE_LENGTH_CHIPS;
T_prn_samples=T_prn_seconds*d_fs_in;
d_rem_code_phase_samples=d_next_rem_code_phase_samples;
K_blk_samples=T_prn_samples+d_rem_code_phase_samples;
d_next_prn_length_samples=round(K_blk_samples);
d_next_rem_code_phase_samples=K_blk_samples-d_next_prn_length_samples;
/*!
* \todo Code lock detector
* \todo Improve the lock detection algorithm!
*/
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
if (d_cn0_estimation_counter<CN0_ESTIMATION_SAMPLES)
{
// fill buffer with prompt correlator output values
d_P_I_buffer[d_cn0_estimation_counter]=d_P_I;
d_P_Q_buffer[d_cn0_estimation_counter]=d_P_Q;
d_Prompt_buffer[d_cn0_estimation_counter]=d_Prompt;
d_cn0_estimation_counter++;
}else{
// estimate CN0 and lock status using buffered values
// MATLAB CODE
//Psig=((1/N)*sum(abs(imag(x((n-N+1):n)))))^2;
//Ptot=(1/N)*sum(abs(x((n-N+1):n)).^2);
//M2=Ptot;
//M4=(1/N)*sum(abs(x((n-N+1):n)).^4);
//SNR_SNV(count)=Psig/(Ptot-Psig);
//SNR_MM(count)=sqrt(2*M2^2-M4)/(M2-Psig);
// lock detector operation
//NBD=sum(abs(imag(x((n-N+1):n))))^2 + sum(abs(real(x((n-N+1):n))))^2;
//NBP=sum(imag(x((n-N+1):n)).^2) - sum(real(x((n-N+1):n)).^2);
//LOCK(count)=NBD/NBP;
//CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length);
float tmp_abs_I,tmp_abs_Q;
float tmp_sum_abs_I,tmp_sum_abs_Q;
float tmp_sum_sqr_I,tmp_sum_sqr_Q;
float Psig,Ptot,M2,M4;
float NBD,NBP;
Psig=0;
Ptot=0;
NBD=0;
NBP=0;
tmp_sum_abs_I=0;
tmp_sum_abs_Q=0;
tmp_sum_sqr_I=0;
tmp_sum_sqr_Q=0;
for (int i=0;i<CN0_ESTIMATION_SAMPLES;i++)
{
tmp_abs_I=std::abs(d_P_I_buffer[i]);
tmp_abs_Q=std::abs(d_P_Q_buffer[i]);
Psig+=tmp_abs_I;
Ptot+=d_P_I_buffer[i]*d_P_I_buffer[i]+d_P_Q_buffer[i]*d_P_Q_buffer[i];
tmp_sum_abs_I+=tmp_abs_I;
tmp_sum_abs_Q+=tmp_abs_Q;
tmp_sum_sqr_I+=(d_P_I_buffer[i]*d_P_I_buffer[i]);
tmp_sum_sqr_Q+=(d_P_Q_buffer[i]*d_P_Q_buffer[i]);
}
Psig=Psig/(float)CN0_ESTIMATION_SAMPLES;
Psig=Psig*Psig;
d_SNR_SNV=Psig/(Ptot/(float)CN0_ESTIMATION_SAMPLES-Psig);
d_SNR_SNV_dB_Hz=10*log10(d_SNR_SNV)+10*log10(d_fs_in/2)-10*log10(d_code_length);
NBD=tmp_sum_abs_I*tmp_sum_abs_I+tmp_sum_abs_Q*tmp_sum_abs_Q;
NBP=tmp_sum_sqr_I-tmp_sum_sqr_Q;
d_carrier_lock_test=NBD/NBP;
d_cn0_estimation_counter=0;
d_CN0_SNV_dB_Hz=gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES,d_fs_in);
d_carrier_lock_test=carrier_lock_detector(d_Prompt_buffer,CN0_ESTIMATION_SAMPLES);
// ###### TRACKING UNLOCK NOTIFICATION #####
int tracking_message;
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>30)
{
d_carrier_lock_fail_counter++;
}else{
if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter>200)
{
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
tracking_message=3; //loss of lock
d_channel_internal_queue->push(tracking_message);
d_carrier_lock_fail_counter=0;
d_current_prn_length_samples=(int)d_vector_length; //original dsp block length
d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine
}
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
}
// ###### TRACKING UNLOCK NOTIFICATION #####
int tracking_message;
if (d_carrier_lock_test<d_carrier_lock_threshold or d_carrier_lock_test>30)
{
d_carrier_lock_fail_counter++;
}else{
if (d_carrier_lock_fail_counter>0) d_carrier_lock_fail_counter--;
}
if (d_carrier_lock_fail_counter>200)
{
std::cout<<"Channel "<<d_channel << " loss of lock!\r\n";
tracking_message=3; //loss of lock
d_channel_internal_queue->push(tracking_message);
d_carrier_lock_fail_counter=0;
d_enable_tracking=false; // TODO: check if disabling tracking is consistent with the channel state machine
}
//std::cout<<"d_carrier_lock_fail_counter"<<d_carrier_lock_fail_counter<<"\r\n";
// Output the tracking data to navigation and PVT
// Output channel 1: Prompt correlator output Q
*out[0]=d_P_Q;
*out[0]=d_Prompt.real();
// Output channel 2: Prompt correlator output I
*out[1]=d_P_I;
// Output channel 3: PRN absolute delay [ms]
*out[1]=d_Prompt.imag();
// Output channel 3: Current tracking time [ms]
*out[2]=(float)(((double)d_sample_counter/(double)d_fs_in)*1000.0);
// Output channel 4: PRN code error [ms]
*out[3]=d_code_phase_ms;//(code_error*1000.0)/d_code_freq;
// Output channel 4: Carrier accumulated phase
*out[3]=d_acc_carrier_phase_rad;
if(d_dump) {
// MULTIPLEXED FILE RECORDING - Record results to file
tmp_E=sqrt(d_E_I*d_E_I+d_E_Q*d_E_Q);
tmp_P=sqrt(d_P_I*d_P_I+d_P_Q*d_P_Q);
tmp_L=sqrt(d_L_I*d_L_I+d_L_Q*d_L_Q);
float prompt_I;
float prompt_Q;
float tmp_E,tmp_P,tmp_L;
float tmp_float;
prompt_I=d_Prompt.imag();
prompt_Q=d_Prompt.real();
tmp_E=std::abs<float>(d_Early);
tmp_P=std::abs<float>(d_Prompt);
tmp_L=std::abs<float>(d_Late);
try {
// EPR
d_dump_file.write((char*)&tmp_E, sizeof(float));
d_dump_file.write((char*)&tmp_P, sizeof(float));
d_dump_file.write((char*)&tmp_L, sizeof(float));
// DLL
d_dump_file.write((char*)&code_error, sizeof(float));
d_dump_file.write((char*)&code_nco, sizeof(float));
//PLL
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&prompt_I, sizeof(float));
d_dump_file.write((char*)&prompt_Q, sizeof(float));
// PRN start sample stamp
tmp_float=(float)d_sample_counter;
d_dump_file.write((char*)&tmp_float, sizeof(float));
// accumulated carrier phase
d_dump_file.write((char*)&d_acc_carrier_phase_rad, sizeof(float));
// carrier and code frequency
d_dump_file.write((char*)&d_carrier_doppler_hz, sizeof(float));
d_dump_file.write((char*)&d_code_freq_hz, sizeof(float));
//PLL commands
d_dump_file.write((char*)&carr_error, sizeof(float));
d_dump_file.write((char*)&carr_nco, sizeof(float));
//FREQ AND PHASE
d_dump_file.write((char*)&d_code_freq, sizeof(float));
d_dump_file.write((char*)&d_carrier_doppler, sizeof(float));
//DLL commands
d_dump_file.write((char*)&code_error, sizeof(float));
d_dump_file.write((char*)&code_nco, sizeof(float));
// PROMPT I and Q (to analyze navigation symbols)
d_dump_file.write((char*)&d_P_I, sizeof(float));
d_dump_file.write((char*)&d_P_Q, sizeof(float));
// Absolute PRN start sample (MATLAB version)
//d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
//d_dump_file.write((char*)&d_loops_count, sizeof(unsigned long int));
d_dump_file.write((char*)&d_SNR_SNV_dB_Hz, sizeof(float));
// CN0 and carrier lock test
d_dump_file.write((char*)&d_CN0_SNV_dB_Hz, sizeof(float));
d_dump_file.write((char*)&d_carrier_lock_test, sizeof(float));
// AUX vars (for debug purposes)
tmp_float=0.0;
d_dump_file.write((char*)&tmp_float, sizeof(float));
tmp_float=0.0;
d_dump_file.write((char*)&tmp_float, sizeof(float));
}
catch (std::ifstream::failure e) {
std::cout << "Exception writing trk dump file "<<e.what()<<"\r\n";
}
}
// ########## DEBUG OUTPUT
// debug: Second counter in channel 0
if (d_channel==0)
{
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"t="<<d_last_seg<<std::endl;
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_SNR_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
if (d_sample_counter>round((float)this->d_fs_in*70)){ //stop after some seconds debug only!
d_enable_tracking=false;
std::cout<<"Stop tracking at sample "<<d_sample_counter<<" and acq at sample "<<d_acq_sample_stamp<<std::endl;
if(d_queue != gr_msg_queue_sptr()) {
ControlMessageFactory* cmf = new ControlMessageFactory();
d_queue->handle(cmf->GetQueueMessage(200,0)); //send stop to the control_thread
delete cmf;
std::cout<<"stop sent from tracking";
}
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"t="<<d_last_seg<<std::endl;
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}else
{
if (floor(d_sample_counter/d_fs_in)!=d_last_seg)
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_SNR_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
{
d_last_seg=floor(d_sample_counter/d_fs_in);
std::cout<<"TRK CH "<<d_channel<<" CN0="<<d_CN0_SNV_dB_Hz<< std::endl;
std::cout<<"TRK CH "<<d_channel<<" Carrier_lock_test="<<d_carrier_lock_test<< std::endl;
}
}
}
consume_each(d_blksize); // this is necesary in gr_block derivates
d_sample_counter+=d_blksize; //count for the processed samples
consume_each(d_current_prn_length_samples); // this is necesary in gr_block derivates
d_sample_counter+=d_current_prn_length_samples; //count for the processed samples
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
}
void gps_l1_ca_dll_pll_tracking_cc::set_acq_code_phase(float code_phase) {
d_code_phase = code_phase;
LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_code_phase;
d_acq_code_phase_samples = code_phase;
LOG_AT_LEVEL(INFO) << "Tracking code phase set to " << d_acq_code_phase_samples;
}
void gps_l1_ca_dll_pll_tracking_cc::set_acq_doppler(float doppler) {
d_carrier_doppler = doppler;
d_carr_freq_basis = doppler;
LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_carrier_doppler;
d_acq_carrier_doppler_hz = doppler;
LOG_AT_LEVEL(INFO) << "Tracking carrier doppler set to " << d_acq_carrier_doppler_hz;
}
void gps_l1_ca_dll_pll_tracking_cc::set_satellite(unsigned int satellite) {
@ -578,3 +471,13 @@ void gps_l1_ca_dll_pll_tracking_cc::set_channel(unsigned int channel) {
d_channel = channel;
LOG_AT_LEVEL(INFO) << "Tracking Channel set to " << d_channel;
}
void gps_l1_ca_dll_pll_tracking_cc::set_acq_sample_stamp(unsigned long int sample_stamp)
{
d_acq_sample_stamp = sample_stamp;
}
void gps_l1_ca_dll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{
d_channel_internal_queue = channel_internal_queue;
}

123
src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h
View File

@ -42,15 +42,14 @@
//#include <gnuradio/gr_sync_decimator.h>
#include "gps_sdr_signal_processing.h"
#include "tracking_2rd_DLL_filter.h"
#include "tracking_2rd_PLL_filter.h"
#include <queue>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include "concurrent_queue.h"
// #include "gps_sdr_structs.h"
// #include "gps_sdr_channel.h"
class gps_l1_ca_dll_pll_tracking_cc;
typedef boost::shared_ptr<gps_l1_ca_dll_pll_tracking_cc>
gps_l1_ca_dll_pll_tracking_cc_sptr;
@ -59,7 +58,11 @@ gps_l1_ca_dll_pll_tracking_cc_sptr
gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq,
long fs_in, unsigned
int vector_length,
gr_msg_queue_sptr queue, bool dump);
gr_msg_queue_sptr queue,
bool dump,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
//class gps_l1_ca_dll_pll_tracking_cc: public gr_sync_decimator
class gps_l1_ca_dll_pll_tracking_cc: public gr_block
@ -71,106 +74,88 @@ private:
gps_l1_ca_dll_pll_make_tracking_cc(unsigned int satellite, long if_freq,
long fs_in, unsigned
int vector_length,
gr_msg_queue_sptr queue, bool dump);
gr_msg_queue_sptr queue,
bool dump,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
gps_l1_ca_dll_pll_tracking_cc(unsigned int satellite, long if_freq,
long fs_in, unsigned
int vector_length, gr_msg_queue_sptr queue,
bool dump);
void calculate_lopp_coef(float* tau1, float* tau2, float lbw, float zeta,
float k);
void update_local_code_refs();
int vector_length,
gr_msg_queue_sptr queue,
bool dump,
float pll_bw_hz,
float dll_bw_hz,
float early_late_space_chips);
void update_local_code();
void update_local_carrier();
// class private vars
// tracking configuration vars
gr_msg_queue_sptr d_queue;
concurrent_queue<int> *d_channel_internal_queue;
unsigned int d_vector_length;
bool d_dump;
unsigned int d_satellite;
unsigned int d_channel; //?
unsigned int d_channel;
int d_last_seg;
long d_if_freq;
long d_fs_in;
float d_tau1_code;
float d_tau2_code;
float d_tau1_carr;
float d_tau2_carr;
float d_early_late_spc_chips;
float d_early_late_spc;
float d_pdi_code;
float d_pdi_carr;
float d_dllnoisebandwidth;
float d_dlldampingratio;
float d_pllnoisebandwidth;
float d_plldampingratio;
unsigned int d_code_length;
unsigned int d_blk_size;
float d_code_phase_step;
float d_code_phase_step_chips;
gr_complex* d_ca_code;
gr_complex* d_early_code;
gr_complex* d_late_code;
gr_complex* d_prompt_code;
gr_complex* d_carr_sign;
gr_complex* d_bb_sign;
float d_code_freq;
float d_rem_code_phase;
gr_complex d_Early;
gr_complex d_Prompt;
gr_complex d_Late;
float d_rem_carr_phase;
// remaining code phase and carrier phase between tracking loops
float d_rem_code_phase_samples;
float d_next_rem_code_phase_samples;
float d_rem_carr_phase_rad;
float d_old_code_nco;
float d_old_code_error;
// PLL and DLL filter library
tracking_2rd_DLL_filter d_code_loop_filter;
tracking_2rd_PLL_filter d_carrier_loop_filter;
float d_old_carr_nco;
float d_old_carr_error;
// acquisition
float d_acq_code_phase_samples;
float d_acq_carrier_doppler_hz;
float d_code_phase;
float d_carrier_doppler;
float d_carr_freq_basis;
// tracking vars
float d_code_freq_hz;
float d_carrier_doppler_hz;
float d_acc_carrier_phase_rad;
//std::complex<double> d_E;
//std::complex<double> d_P;
//std::complex<double> d_L;
float d_E_I;
float d_E_Q;
float d_P_I;
float d_P_Q;
float d_L_I;
float d_L_Q;
float d_absolute_code_phase_samples;
float d_code_phase_ms;
unsigned int d_blksize;
//PRN period in samples
int d_current_prn_length_samples;
int d_next_prn_length_samples;
//processing samples counters
unsigned long int d_sample_counter;
unsigned long int d_acq_sample_stamp;
unsigned long int d_loops_count;
// CN0 estimation and lock detector
int d_cn0_estimation_counter;
float* d_P_I_buffer;
float* d_P_Q_buffer;
gr_complex* d_Prompt_buffer;
float d_carrier_lock_test;
float d_SNR_SNV;
float d_SNR_MM;
float d_SNR_SNV_dB_Hz;
float d_CN0_SNV_dB_Hz;
float d_carrier_lock_threshold;
int d_carrier_lock_fail_counter;
// control vars
bool d_enable_tracking;
bool d_pull_in;
// file dump
std::string d_dump_filename;
std::ofstream d_dump_file;
@ -183,16 +168,8 @@ public:
void set_acq_code_phase(float code_phase);
void set_acq_doppler(float doppler);
void start_tracking();
void set_acq_sample_stamp(unsigned long int sample_stamp)
{
d_acq_sample_stamp = sample_stamp;
}
void set_channel_queue(concurrent_queue<int> *channel_internal_queue)
{
d_channel_internal_queue = channel_internal_queue;
}
void set_acq_sample_stamp(unsigned long int sample_stamp);
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
/*!
* \brief just like gr_block::general_work, only this arranges to call consume_each for you

3
src/algorithms/tracking/gnuradio_blocks/jamfile.jam
View File

@ -1,3 +1,4 @@
project : build-dir ../../../../build ;
obj gps_l1_ca_dll_pll_tracking_cc : gps_l1_ca_dll_pll_tracking_cc.cc ;
obj gps_l1_ca_dll_pll_tracking_cc : gps_l1_ca_dll_pll_tracking_cc.cc ;
obj gps_l1_ca_dll_fll_pll_tracking_cc : gps_l1_ca_dll_fll_pll_tracking_cc.cc ;

1
src/algorithms/tracking/jamfile.jam
View File

@ -1,2 +1,3 @@
build-project libs ;
build-project adapters ;
build-project gnuradio_blocks ;

141
src/algorithms/tracking/libs/CN_estimators.cc Normal file
View File

@ -0,0 +1,141 @@
/*!
* \file CN_estimators.cc
* \brief Library with a set of Carrier to Noise estimators and lock detectors.
* SNV_CN0 is a Carrier-to-Noise (CN0) estimator based on the Signal-to-Noise Variance (SNV) estimator [1].
*
* Carrier lock detector using normalised estimate of the cosine
* of twice the carrier phase error [2].
*
*
* [1] Marco Pini, Emanuela Falletti and Maurizio Fantino, "Performance
* Evaluation of C/N0 Estimators using a Real Time GNSS Software Receiver,"
* IEEE 10th International Symposium on Spread Spectrum Techniques and
* Applications, pp.28-30, August 2008.
*
*
*
* [2] Van Dierendonck, A.J. (1996), Global Positioning System: Theory and
* Applications,
* Volume I, Chapter 8: GPS Receivers, AJ Systems, Los Altos, CA 94024.
* Inc.: 329-407.
*
*
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 "CN_estimators.h"
#include "GPS_L1_CA.h"
#include <gnuradio/gr_complex.h>
#include <math.h>
/*!
* Signal-to-Noise (SNR) (\f$\rho\f$) estimator using the Signal-to-Noise Variance (SNV) estimator:
* \f{equation}
* \hat{\rho}=\frac{\hat{P}_s}{\hat{P}_n}=\frac{\hat{P}_s}{\hat{P}_{tot}-\hat{P}_s},
* \f}
* where \f$\hat{P}_s=\left(\frac{1}{N}\sum^{N-1}_{i=0}|Re(Pc(i))|\right)^2\f$ is the estimation of the signal power,
* \f$\hat{P}_{tot}=\frac{1}{N}\sum^{N-1}_{i=0}|Pc(i)|^2\f$ is the estimator of the total power, \f$|\cdot|\f$ is the absolute value,
* \f$Re(\cdot)\f$ stands for the real part of the value, and \f$Pc(i)\f$ is the prompt correlator output for the sample index i.
*
* The SNR value is converted to CN0 [dB-Hz], taking to account the receiver bandwidth and the PRN code gain, using the following formula:
* \f{equation}
* CN0_{dB}=10*log(\hat{\rho})+10*log(\frac{f_s}{2})-10*log(L_{PRN}),
* \f}
* where \f$f_s\f$ is the sampling frequency and \f$L_{PRN}\f$ is the PRN sequence length.
*/
float gps_l1_ca_CN0_SNV(gr_complex* Prompt_buffer, int length, long fs_in)
{
// estimate CN0 using buffered values
// MATLAB CODE
//Psig=((1/N)*sum(abs(imag(x((n-N+1):n)))))^2;
//Ptot=(1/N)*sum(abs(x((n-N+1):n)).^2);
//SNR_SNV(count)=Psig/(Ptot-Psig);
//CN0_SNV_dB=10*log10(SNR_SNV)+10*log10(BW)-10*log10(PRN_length);
float SNR, SNR_dB_Hz;
float tmp_abs_I,tmp_abs_Q;
float Psig,Ptot;
//float M2,M4;
Psig=0;
Ptot=0;
for (int i=0;i<length;i++)
{
tmp_abs_I=std::abs(Prompt_buffer[i].imag());
tmp_abs_Q=std::abs(Prompt_buffer[i].real());
Psig+=tmp_abs_I;
Ptot+=Prompt_buffer[i].imag()*Prompt_buffer[i].imag()+Prompt_buffer[i].real()*Prompt_buffer[i].real();
}
Psig=Psig/(float)length;
Psig=Psig*Psig;
SNR=Psig/(Ptot/(float)length-Psig);
SNR_dB_Hz=10*log10(SNR)+10*log10(fs_in/2)-10*log10(GPS_L1_CA_CODE_LENGTH_CHIPS);
return SNR_dB_Hz;
}
/*!
* The Carrier Phase Lock Detector block uses the normalised estimate of the cosine of twice the carrier phase error is given by
* \f{equation}
* C2\phi=\frac{NBD}{NBP},
* \f}
* where \f$NBD=(\sum^{N-1}_{i=0}|Im(Pc(i))|)^2+(\sum^{N-1}_{i=0}|Re(Pc(i))|)^2\f$,
* \f$NBP=\sum^{N-1}_{i=0}Im(Pc(i))^2-\sum^{N-1}_{i=0}Re(Pc(i))^2\f$, and
* \f$Pc(i)\f$ is the prompt correlator output for the sample index i.
*/
float carrier_lock_detector(gr_complex* Prompt_buffer, int length)
{
/*!
* \todo Code lock detector
*/
// estimate using buffered values
// MATLAB CODE
// lock detector operation
//NBD=sum(abs(imag(x((n-N+1):n))))^2 + sum(abs(real(x((n-N+1):n))))^2;
//NBP=sum(imag(x((n-N+1):n)).^2) - sum(real(x((n-N+1):n)).^2);
//LOCK(count)=NBD/NBP;
float tmp_abs_I,tmp_abs_Q;
float tmp_sum_abs_I,tmp_sum_abs_Q;
float tmp_sum_sqr_I,tmp_sum_sqr_Q;
tmp_sum_abs_I=0;
tmp_sum_abs_Q=0;
tmp_sum_sqr_I=0;
tmp_sum_sqr_Q=0;
float NBD,NBP;
for (int i=0;i<length;i++)
{
tmp_abs_I=std::abs(Prompt_buffer[i].imag());
tmp_abs_Q=std::abs(Prompt_buffer[i].real());
tmp_sum_abs_I+=tmp_abs_I;
tmp_sum_abs_Q+=tmp_abs_Q;
tmp_sum_sqr_I+=(Prompt_buffer[i].imag()*Prompt_buffer[i].imag());
tmp_sum_sqr_Q+=(Prompt_buffer[i].real()*Prompt_buffer[i].real());
}
NBD=tmp_sum_abs_I*tmp_sum_abs_I+tmp_sum_abs_Q*tmp_sum_abs_Q;
NBP=tmp_sum_sqr_I-tmp_sum_sqr_Q;
return NBD/NBP;
}

42
src/algorithms/tracking/libs/CN_estimators.h Normal file
View File

@ -0,0 +1,42 @@
/*!
* \file CN_estimators.h
* \brief Library with a set of Carrier to Noise estimators and lock detectors
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Library with a set of Carrier to Noise estimators and lock detectors
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 CN_ESTIMATORS_H_
#define CN_ESTIMATORS_H_
#include <gnuradio/gr_complex.h>
float gps_l1_ca_CN0_SNV(gr_complex* Prompt_buffer, int length, long fs_in);
float carrier_lock_detector(gr_complex* Prompt_buffer, int length);
#endif

7
src/algorithms/tracking/libs/jamfile.jam Normal file
View File

@ -0,0 +1,7 @@
project : build-dir ../../../../build ;
obj tracking_discriminators : tracking_discriminators.cc ;
obj CN_estimators : CN_estimators.cc ;
obj tracking_FLL_PLL_filter : tracking_FLL_PLL_filter.cc ;
obj tracking_2rd_PLL_filter : tracking_2rd_PLL_filter.cc ;
obj tracking_2rd_DLL_filter : tracking_2rd_DLL_filter.cc ;

81
src/algorithms/tracking/libs/tracking_2rd_DLL_filter.cc Normal file
View File

@ -0,0 +1,81 @@
/*!
* \file tracking_2rd_DLL_filter.cc
* \brief Class that implements 2 order DLL filter for code tracking loop.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Class that implements 2 order PLL filter for code tracking loop. The algorithm is described in [1]
*
* [1] 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, Applied and Numerical Harmonic Analysis.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 "tracking_2rd_DLL_filter.h"
void tracking_2rd_DLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
// Solve natural frequency
float Wn;
Wn = lbw*8*zeta / (4*zeta*zeta + 1);
// solve for t1 & t2
*tau1 = k / (Wn * Wn);
*tau2 = (2.0 * zeta) / Wn;
}
void tracking_2rd_DLL_filter::set_DLL_BW(float dll_bw_hz)
{
//Calculate filter coefficient values
d_dllnoisebandwidth=dll_bw_hz;
calculate_lopp_coef(&d_tau1_code, &d_tau2_code, d_dllnoisebandwidth, d_dlldampingratio,1.0);// Calculate filter coefficient values
}
void tracking_2rd_DLL_filter::initialize(float d_acq_code_phase_samples)
{
// code tracking loop parameters
d_old_code_nco = 0.0;
d_old_code_error = 0.0;
}
float tracking_2rd_DLL_filter::get_code_nco(float DLL_discriminator)
{
float code_nco;
code_nco = d_old_code_nco + (d_tau2_code/d_tau1_code)*(DLL_discriminator - d_old_code_error) + DLL_discriminator * (d_pdi_code/d_tau1_code);
d_old_code_nco = code_nco;
d_old_code_error = DLL_discriminator; //[chips]
return code_nco;
}
tracking_2rd_DLL_filter::tracking_2rd_DLL_filter ()
{
d_pdi_code = 0.001;// Summation interval for code
d_dlldampingratio=0.7;
}
tracking_2rd_DLL_filter::~tracking_2rd_DLL_filter ()
{
}

61
src/algorithms/tracking/libs/tracking_2rd_DLL_filter.h Normal file
View File

@ -0,0 +1,61 @@
/*!
* \file tracking_2rd_DLL_filter.h
* \brief Class that implements 2 order DLL filter for code tracking loop.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Class that implements 2 order PLL filter for code tracking loop. The algorithm is described in [1]
*
* [1] 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, Applied and Numerical Harmonic Analysis.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 TRACKING_2RD_DLL_FILTER_H_
#define TRACKING_2RD_DLL_FILTER_H_
class tracking_2rd_DLL_filter
{
private:
// PLL filter parameters
float d_tau1_code;
float d_tau2_code;
float d_pdi_code;
float d_dllnoisebandwidth;
float d_dlldampingratio;
float d_old_code_error;
float d_old_code_nco;
void calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k);
public:
void set_DLL_BW(float dll_bw_hz);
void initialize(float d_acq_code_phase_samples);
float get_code_nco(float DLL_discriminator);
tracking_2rd_DLL_filter();
~tracking_2rd_DLL_filter();
};
#endif

80
src/algorithms/tracking/libs/tracking_2rd_PLL_filter.cc Normal file
View File

@ -0,0 +1,80 @@
/*!
* \file tracking_2rd_PLL_filter.cc
* \brief Class that implements 2 order PLL filter for tracking carrier loop.
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Class that implements 2 order PLL filter for tracking carrier loop. The algorithm is described in [1]
*
* [1] 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, Applied and Numerical Harmonic Analysis.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 "tracking_2rd_PLL_filter.h"
void tracking_2rd_PLL_filter::calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k){
// Solve natural frequency
float Wn;
Wn = lbw*8*zeta / (4*zeta*zeta + 1);
// solve for t1 & t2
*tau1 = k / (Wn * Wn);
*tau2 = (2.0 * zeta) / Wn;
}
void tracking_2rd_PLL_filter::set_PLL_BW(float pll_bw_hz)
{
//Calculate filter coefficient values
d_pllnoisebandwidth=pll_bw_hz;
calculate_lopp_coef(&d_tau1_carr, &d_tau2_carr, d_pllnoisebandwidth, d_plldampingratio,0.25);// Calculate filter coefficient values
}
void tracking_2rd_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
{
// carrier/Costas loop parameters
d_old_carr_nco = 0.0;
d_old_carr_error = 0.0;
}
float tracking_2rd_PLL_filter::get_carrier_nco(float PLL_discriminator)
{
float carr_nco;
carr_nco = d_old_carr_nco+(d_tau2_carr/d_tau1_carr)*(PLL_discriminator - d_old_carr_error) + PLL_discriminator * (d_pdi_carr/d_tau1_carr);
d_old_carr_nco = carr_nco;
d_old_carr_error = PLL_discriminator;
return carr_nco;
}
tracking_2rd_PLL_filter::tracking_2rd_PLL_filter ()
{
//--- PLL variables --------------------------------------------------------
d_pdi_carr = 0.001;// Summation interval for carrier
d_plldampingratio=0.65;
}
tracking_2rd_PLL_filter::~tracking_2rd_PLL_filter ()
{
}

62
src/algorithms/tracking/libs/tracking_2rd_PLL_filter.h Normal file
View File

@ -0,0 +1,62 @@
/*!
* \file tracking_2rd_PLL_filter.h
* \brief Class that implements 2 order PLL filter for tracking carrier loop
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Class that implements 2 order PLL filter for tracking carrier loop. The algorithm is described in [1]
*
* [1] 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, Applied and Numerical Harmonic Analysis.
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 TRACKING_2RD_PLL_FILTER_H_
#define TRACKING_2RD_PLL_FILTER_H_
class tracking_2rd_PLL_filter
{
private:
// PLL filter parameters
float d_tau1_carr;
float d_tau2_carr;
float d_pdi_carr;
float d_pllnoisebandwidth;
float d_plldampingratio;
float d_old_carr_error;
float d_old_carr_nco;
void calculate_lopp_coef(float* tau1,float* tau2, float lbw, float zeta, float k);
public:
void set_PLL_BW(float pll_bw_hz);
void initialize(float d_acq_carrier_doppler_hz);
float get_carrier_nco(float PLL_discriminator);
tracking_2rd_PLL_filter();
~tracking_2rd_PLL_filter();
};
#endif

121
src/algorithms/tracking/libs/tracking_FLL_PLL_filter.cc Normal file
View File

@ -0,0 +1,121 @@
/*!
* \file tracking_FLL_PLL_filter.cc
* \brief Class that implements hybrid FLL and PLL filter for tracking carrier loop
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Class that implements hybrid FLL and PLL filter for tracking carrier loop
* Filter design (Kaplan 2nd ed., Pag. 181 Fig. 181)
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 "tracking_FLL_PLL_filter.h"
#include <iostream>
void tracking_FLL_PLL_filter::set_params(float fll_bw_hz,float pll_bw_hz, int order)
{
/*!
* Filter design (Kaplan 2nd ed., Pag. 181 Fig. 181)
*/
d_order=order;
if (d_order==3)
{
/*!
* 3rd order PLL with 2nd order FLL assist
*/
d_pll_b3 = 2.400;
d_pll_a3 = 1.100;
d_pll_a2 = 1.414;
d_pll_w0p = pll_bw_hz/0.7845;
d_pll_w0p2 = d_pll_w0p*d_pll_w0p;
d_pll_w0p3 = d_pll_w0p2*d_pll_w0p;
d_pll_w0f = fll_bw_hz/0.53;
d_pll_w0f2 = d_pll_w0f*d_pll_w0f;
}else
{
/*!
* 2rd order PLL with 1st order FLL assist
*/
d_pll_a2 = 1.414;
d_pll_w0p = pll_bw_hz/0.53;
d_pll_w0p2 = d_pll_w0p*d_pll_w0p;
d_pll_w0f = fll_bw_hz/0.25;
}
}
void tracking_FLL_PLL_filter::initialize(float d_acq_carrier_doppler_hz)
{
if (d_order==3)
{
d_pll_x = 2.0*d_acq_carrier_doppler_hz;
d_pll_w = 0;
}else{
d_pll_w = d_acq_carrier_doppler_hz;
d_pll_x = 0;
}
std::cout<<" d_pll_x init = "<<d_pll_x<<"\r\n";
}
float tracking_FLL_PLL_filter::get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s)
{
float carrier_error_hz;
if (d_order==3)
{
/*!
* 3rd order PLL with 2nd order FLL assist
*/
d_pll_w = d_pll_w + correlation_time_s * (d_pll_w0p3 * PLL_discriminator + d_pll_w0f2 * FLL_discriminator);
d_pll_x = d_pll_x + correlation_time_s * (0.5*d_pll_w + d_pll_a2 * d_pll_w0f * FLL_discriminator + d_pll_a3 * d_pll_w0p2 * PLL_discriminator);
carrier_error_hz = 0.5*d_pll_x + d_pll_b3 * d_pll_w0p * PLL_discriminator;
}else
{
/*!
* 2rd order PLL with 1st order FLL assist
*/
float pll_w_new;
pll_w_new = d_pll_w + PLL_discriminator*d_pll_w0p2*correlation_time_s + FLL_discriminator*d_pll_w0f*correlation_time_s ;
carrier_error_hz = 0.5*(pll_w_new + d_pll_w)+d_pll_a2 * d_pll_w0p*PLL_discriminator;
d_pll_w =pll_w_new;
/*std::cout<<" d_pll_w = "<<carrier_error_hz<<
", pll_w_new = "<<pll_w_new
<<", PLL_discriminator=" <<PLL_discriminator
<<" FLL_discriminator ="<<FLL_discriminator
<<" correlation_time_s = "<<correlation_time_s<<"\r\n";*/
}
return carrier_error_hz;
}
tracking_FLL_PLL_filter::tracking_FLL_PLL_filter ()
{
}
tracking_FLL_PLL_filter::~tracking_FLL_PLL_filter ()
{
}

59
src/algorithms/tracking/libs/tracking_FLL_PLL_filter.h Normal file
View File

@ -0,0 +1,59 @@
/*!
* \file tracking_FLL_PLL_filter.h
* \brief Class that implements hybrid FLL and PLL filter for tracking carrier loop
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Class that implements hybrid FLL and PLL filter for tracking carrier loop
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 TRACKING_FLL_PLL_FILTER_H_
#define TRACKING_FLL_PLL_FILTER_H_
class tracking_FLL_PLL_filter
{
private:
// FLL + PLL filter parameters
int d_order;
float d_pll_w;
float d_pll_w0p3;
float d_pll_w0f2;
float d_pll_x;
float d_pll_a2;
float d_pll_w0f;
float d_pll_a3;
float d_pll_w0p2;
float d_pll_b3;
float d_pll_w0p;
public:
void set_params(float fll_bw_hz,float pll_bw_hz, int order);
void initialize(float d_acq_carrier_doppler_hz);
float get_carrier_error(float FLL_discriminator, float PLL_discriminator, float correlation_time_s);
tracking_FLL_PLL_filter();
~tracking_FLL_PLL_filter();
};
#endif

97
src/algorithms/tracking/libs/tracking_discriminators.cc Normal file
View File

@ -0,0 +1,97 @@
/*!
* \file tracking_discriminators.cc
* \brief Library with a set of code tracking and carrier tracking discriminators that is used by the tracking algorithms
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 "tracking_discriminators.h"
#include <math.h>
// All the outputs are in RADIANS
/*!
* FLL four quadrant arctan discriminator:
* \f{equation}
* \frac{\phi_2-\phi_1}{t_2-t1}=\frac{ATAN2(cross,dot)}{t_1-t_2},
* \f}
* where \f$cross=I_{PS1}Q_{PS2}-I_{PS2}Q_{PS1}\f$ and \f$dot=I_{PS1}I_{PS2}+Q_{PS1}Q_{PS2}\f$,
* \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_1\f$, and
* \f$I_{PS2},Q_{PS2}\f$ are the inphase and quadrature prompt correlator outputs respectively at sample time \f$t_2\f$. The output is in [radians/second].
*/
float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2,float t1, float t2)
{
float cross,dot;
dot=prompt_s1.imag()*prompt_s2.imag()+prompt_s1.real()*prompt_s2.real();
cross=prompt_s1.imag()*prompt_s2.real()-prompt_s2.imag()*prompt_s1.real();
return atan2(cross,dot)/(t2-t1);
}
/*!
* PLL four quadrant arctan discriminator:
* \f{equation}
* \phi=ATAN2(Q_{PS},I_{PS}),
* \f}
* where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians].
*/
float pll_four_quadrant_atan(gr_complex prompt_s1)
{
return atan2(prompt_s1.real(),prompt_s1.imag());
}
/*!
* PLL Costas loop two quadrant arctan discriminator:
* \f{equation}
* \phi=ATAN\left(\frac{Q_{PS}}{I_{PS}}\right),
* \f}
* where \f$I_{PS1},Q_{PS1}\f$ are the inphase and quadrature prompt correlator outputs respectively. The output is in [radians].
*/
float pll_cloop_two_quadrant_atan(gr_complex prompt_s1)
{
if (prompt_s1.imag()!=0.0)
{
return atan(prompt_s1.real()/prompt_s1.imag());
}else{
return 0;
}
}
/*!
* DLL Noncoherent Early minus Late envelope normalized discriminator:
* \f{equation}
* error=\frac{E-L}{E+L},
* \f}
* where \f$E=\sqrt{I_{ES}^2,Q_{ES}^2}\f$ is the Early correlator output absolute value and
* \f$L=\sqrt{I_{LS}^2,Q_{LS}^2}\f$ is the Late correlator output absolute value. The output is in [chips].
*/
float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1)
{
float P_early, P_late;
P_early=std::abs(early_s1);
P_late=std::abs(late_s1);
return (P_early-P_late)/((P_early+P_late));
}

47
src/algorithms/tracking/libs/tracking_discriminators.h Normal file
View File

@ -0,0 +1,47 @@
/*!
* \file tracking_discriminators.h
* \brief Library with a set of code tracking and carrier tracking disctiminators
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* Library with a set of code tracking and carrier tracking disctiminators that is used by the tracking algorithms
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2011 (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 TRACKING_DISCRIMINATORS_H_
#define TRACKING_DISCRIMINATORS_H_
#include <gnuradio/gr_complex.h>
float fll_four_quadrant_atan(gr_complex prompt_s1, gr_complex prompt_s2,float t1, float t2);
float pll_four_quadrant_atan(gr_complex prompt_s1);
float pll_cloop_two_quadrant_atan(gr_complex prompt_s1);
float dll_nc_e_minus_l_normalized(gr_complex early_s1, gr_complex late_s1);
#endif

6
src/core/receiver/gnss_block_factory.cc
View File

@ -55,6 +55,7 @@
#include "gps_l1_ca_pcps_acquisition.h"
#include "gps_l1_ca_tong_pcps_acquisition.h"
#include "gps_l1_ca_dll_pll_tracking.h"
#include "gps_l1_ca_dll_fll_pll_tracking.h"
#include "gps_l1_ca_telemetry_decoder.h"
#include "gps_l1_ca_observables.h"
@ -262,6 +263,11 @@ GNSSBlockInterface* GNSSBlockFactory::GetBlock(
block = new GpsL1CaDllPllTracking(configuration, role, in_streams,
out_streams, queue);
}
else if (implementation.compare("GPS_L1_CA_DLL_FLL_PLL_Tracking") == 0)
{
block = new GpsL1CaDllFllPllTracking(configuration, role, in_streams,
out_streams, queue);
}
else if (implementation.compare("GPS_L1_CA_Telemetry_Decoder") == 0)
{
block = new GpsL1CaTelemetryDecoder(configuration, role, in_streams,

8
src/core/system_parameters/GPS_L1_CA.h
View File

@ -43,11 +43,19 @@
const float GPS_C_m_s= 299792458.0; // The speed of light, [m/ms]
const float GPS_STARTOFFSET_ms= 68.802; //[ms] Initial sign. travel time
const float GPS_PI = 3.1415926535898; // Pi used in the GPS coordinate system
// carrier and code frequencies
const float GPS_L1_FREQ_HZ = 1.57542e9;
const float GPS_L2_FREQ_HZ = 1.22760e9;
const float GPS_L1_CA_CODE_RATE_HZ = 1.023e6;
const float GPS_L1_CA_CODE_LENGTH_CHIPS = 1023.0;
//-- Constants for satellite position calculation -------------------------
const double OMEGA_EARTH_DOT = 7.2921151467e-5; // Earth rotation rate, [rad/s]
const double GM = 3.986005e14; // Universal gravitational constant times the mass of the Earth, [m^3/s^2]
const double F = -4.442807633e-10; // Constant, [sec/(meter)^(1/2)]
// NAVIGATION MESSAGE DEMODULATION AND DECODING
#define GPS_PREAMBLE {1, 0, 0, 0, 1, 0, 1, 1}

7
src/main/jamfile.jam
View File

@ -30,7 +30,14 @@ exe mercurio : main.cc
../algorithms/telemetry_decoder/gnuradio_blocks//gps_l1_ca_telemetry_decoder_cc
../algorithms/telemetry_decoder/libs//gps_l1_ca_subframe_fsm
../algorithms/tracking/adapters//gps_l1_ca_dll_pll_tracking
../algorithms/tracking/adapters//gps_l1_ca_dll_fll_pll_tracking
../algorithms/tracking/gnuradio_blocks//gps_l1_ca_dll_pll_tracking_cc
../algorithms/tracking/gnuradio_blocks//gps_l1_ca_dll_fll_pll_tracking_cc
../algorithms/tracking/libs//tracking_discriminators
../algorithms/tracking/libs//CN_estimators
../algorithms/tracking/libs//tracking_FLL_PLL_filter
../algorithms/tracking/libs//tracking_2rd_PLL_filter
../algorithms/tracking/libs//tracking_2rd_DLL_filter
../core/libs//INIReader
../core/libs//ini
../core/libs//string_converter

5
src/main/main.cc
View File

@ -43,7 +43,10 @@
using google::LogMessage;
// TODO: make this queue generic for all the GNSS systems (javi)
/*!
* \todo make this queue generic for all the GNSS systems (javi)
*/
/*!
* \brief Concurrent queue that communicates the Telemetry Decoder
* to the Observables modules