From 34d1c5110b9ee28c2d770718df122f8e2c5f7c51 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 22 Nov 2011 17:21:54 +0000 Subject: [PATCH] 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 --- conf/mercurio.conf | 23 +- jamroot.jam | 1 + .../adapters/file_signal_source.cc | 18 +- .../gps_l1_ca_telemetry_decoder_cc.cc | 8 +- .../libs/gps_l1_ca_subframe_fsm.cc | 2 +- .../gps_l1_ca_dll_fll_pll_tracking.cc | 167 +++++ .../adapters/gps_l1_ca_dll_fll_pll_tracking.h | 104 +++ .../adapters/gps_l1_ca_dll_pll_tracking.cc | 66 +- .../adapters/gps_l1_ca_dll_pll_tracking.h | 7 +- src/algorithms/tracking/adapters/jamfile.jam | 3 +- .../gps_l1_ca_dll_fll_pll_tracking_cc.cc | 510 +++++++++++++++ .../gps_l1_ca_dll_fll_pll_tracking_cc.h | 201 ++++++ .../gps_l1_ca_dll_pll_tracking_cc.cc | 605 ++++++++---------- .../gps_l1_ca_dll_pll_tracking_cc.h | 123 ++-- .../tracking/gnuradio_blocks/jamfile.jam | 3 +- src/algorithms/tracking/jamfile.jam | 1 + src/algorithms/tracking/libs/CN_estimators.cc | 141 ++++ src/algorithms/tracking/libs/CN_estimators.h | 42 ++ src/algorithms/tracking/libs/jamfile.jam | 7 + .../tracking/libs/tracking_2rd_DLL_filter.cc | 81 +++ .../tracking/libs/tracking_2rd_DLL_filter.h | 61 ++ .../tracking/libs/tracking_2rd_PLL_filter.cc | 80 +++ .../tracking/libs/tracking_2rd_PLL_filter.h | 62 ++ .../tracking/libs/tracking_FLL_PLL_filter.cc | 121 ++++ .../tracking/libs/tracking_FLL_PLL_filter.h | 59 ++ .../tracking/libs/tracking_discriminators.cc | 97 +++ .../tracking/libs/tracking_discriminators.h | 47 ++ src/core/receiver/gnss_block_factory.cc | 6 + src/core/system_parameters/GPS_L1_CA.h | 8 + src/main/jamfile.jam | 7 + src/main/main.cc | 5 +- 31 files changed, 2192 insertions(+), 474 deletions(-) create mode 100644 src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc create mode 100644 src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h create mode 100644 src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc create mode 100644 src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h create mode 100644 src/algorithms/tracking/libs/CN_estimators.cc create mode 100644 src/algorithms/tracking/libs/CN_estimators.h create mode 100644 src/algorithms/tracking/libs/jamfile.jam create mode 100644 src/algorithms/tracking/libs/tracking_2rd_DLL_filter.cc create mode 100644 src/algorithms/tracking/libs/tracking_2rd_DLL_filter.h create mode 100644 src/algorithms/tracking/libs/tracking_2rd_PLL_filter.cc create mode 100644 src/algorithms/tracking/libs/tracking_2rd_PLL_filter.h create mode 100644 src/algorithms/tracking/libs/tracking_FLL_PLL_filter.cc create mode 100644 src/algorithms/tracking/libs/tracking_FLL_PLL_filter.h create mode 100644 src/algorithms/tracking/libs/tracking_discriminators.cc create mode 100644 src/algorithms/tracking/libs/tracking_discriminators.h diff --git a/conf/mercurio.conf b/conf/mercurio.conf index e3e97f410..b3d671852 100644 --- a/conf/mercurio.conf +++ b/conf/mercurio.conf @@ -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 diff --git a/jamroot.jam b/jamroot.jam index a603700ba..18f527339 100644 --- a/jamroot.jam +++ b/jamroot.jam @@ -38,6 +38,7 @@ project : requirements src/algorithms/telemetry_decoder/libs src/algorithms/tracking/adapters src/algorithms/tracking/gnuradio_blocks +src/algorithms/tracking/libs src/core/interfaces src/core/libs src/core/receiver diff --git a/src/algorithms/signal_source/adapters/file_signal_source.cc b/src/algorithms/signal_source/adapters/file_signal_source.cc index acc7a0dc1..e3340ef5d 100644 --- a/src/algorithms/signal_source/adapters/file_signal_source.cc +++ b/src/algorithms/signal_source/adapters/file_signal_source.cc @@ -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_; } - } + } } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 7619cb70f..58931fdc4 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -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 "<d_satellite+1<d_satellite<d_satellite+1<<" with preamble start at "<d_satellite<<" with preamble start at "<7000){ - std::cout<<"lost of frame sync SAT "<d_satellite+1<d_satellite<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; diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc new file mode 100644 index 000000000..29d6f0f7c --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include "gps_l1_ca_dll_fll_pll_tracking.h" + +#include "configuration_interface.h" + +#include + +#include +#include + +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 *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_; +} + diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h new file mode 100644 index 000000000..b7c5bee95 --- /dev/null +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_fll_pll_tracking.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 + +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 *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 *channel_internal_queue_; +}; + +#endif // GPS_L1_CA_DLL_FLL_PLL_TRACKING_H_ diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc index 573cd02ab..88f543a22 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.cc @@ -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 *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) { diff --git a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h index 77fbbed18..146aad798 100644 --- a/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h +++ b/src/algorithms/tracking/adapters/gps_l1_ca_dll_pll_tracking.h @@ -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_; diff --git a/src/algorithms/tracking/adapters/jamfile.jam b/src/algorithms/tracking/adapters/jamfile.jam index c4cdeb803..9e9101fd7 100644 --- a/src/algorithms/tracking/adapters/jamfile.jam +++ b/src/algorithms/tracking/adapters/jamfile.jam @@ -1,3 +1,4 @@ project : build-dir ../../../../build ; -obj gps_l1_ca_dll_pll_tracking : gps_l1_ca_dll_pll_tracking.cc ; \ No newline at end of file +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 ; \ No newline at end of file diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc new file mode 100644 index 000000000..b815f576f --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 +#include +#include +#include +#include "math.h" + +#include + +#include +#include + +/*! + * \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(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_satellite<< std::endl; + DLOG(INFO) << "Start tracking for satellite "<d_satellite<<" received "; + + // enable tracking + d_pull_in=true; + d_enable_tracking=true; + + std::cout<<"PULL-IN Doppler [Hz]= "<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_counter30) + { + 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 "<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_Early); + tmp_P=std::abs(d_Prompt); + tmp_L=std::abs(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 "< *channel_internal_queue) +{ + d_channel_internal_queue = channel_internal_queue; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h new file mode 100644 index 000000000..2d576cbd9 --- /dev/null +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H +#define GPS_L1_CA_DLL_FLL_PLL_TRACKING_CC_H + +#include + +#include +#include +//#include + +#include "gps_sdr_signal_processing.h" +#include "tracking_FLL_PLL_filter.h" + +#include +#include +#include +#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_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 *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 *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 diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index 5416757e7..7522a832a 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -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 @@ -51,8 +48,6 @@ #include - - #include #include @@ -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= "<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 "<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_"<d_channel<<"_SAT_"<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(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_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(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_satellite<< std::endl; + DLOG(INFO) << "Start tracking for satellite "<d_satellite<<" received "; + // enable tracking + d_pull_in=true; + d_enable_tracking=true; + std::cout<<"PULL-IN Doppler [Hz]= "<(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;iupdate_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;i30) + { + 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 "<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"<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 "<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_Early); + tmp_P=std::abs(d_Prompt); + tmp_L=std::abs(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 "<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="< *channel_internal_queue) +{ + d_channel_internal_queue = channel_internal_queue; +} diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h index 7eec65799..45862c555 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h @@ -42,15 +42,14 @@ //#include #include "gps_sdr_signal_processing.h" +#include "tracking_2rd_DLL_filter.h" +#include "tracking_2rd_PLL_filter.h" #include #include #include #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_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 *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 d_E; - //std::complex d_P; - //std::complex 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 *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 *channel_internal_queue); /*! * \brief just like gr_block::general_work, only this arranges to call consume_each for you diff --git a/src/algorithms/tracking/gnuradio_blocks/jamfile.jam b/src/algorithms/tracking/gnuradio_blocks/jamfile.jam index 56b5be0ef..a301a90c1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/jamfile.jam +++ b/src/algorithms/tracking/gnuradio_blocks/jamfile.jam @@ -1,3 +1,4 @@ project : build-dir ../../../../build ; -obj gps_l1_ca_dll_pll_tracking_cc : gps_l1_ca_dll_pll_tracking_cc.cc ; \ No newline at end of file +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 ; \ No newline at end of file diff --git a/src/algorithms/tracking/jamfile.jam b/src/algorithms/tracking/jamfile.jam index 86bceb14f..2f461561c 100644 --- a/src/algorithms/tracking/jamfile.jam +++ b/src/algorithms/tracking/jamfile.jam @@ -1,2 +1,3 @@ +build-project libs ; build-project adapters ; build-project gnuradio_blocks ; diff --git a/src/algorithms/tracking/libs/CN_estimators.cc b/src/algorithms/tracking/libs/CN_estimators.cc new file mode 100644 index 000000000..c0d88a142 --- /dev/null +++ b/src/algorithms/tracking/libs/CN_estimators.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ +#include "CN_estimators.h" +#include "GPS_L1_CA.h" +#include +#include + +/*! + * 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. + * + * ------------------------------------------------------------------------- + */ + +#ifndef CN_ESTIMATORS_H_ +#define CN_ESTIMATORS_H_ + +#include + +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 diff --git a/src/algorithms/tracking/libs/jamfile.jam b/src/algorithms/tracking/libs/jamfile.jam new file mode 100644 index 000000000..895e3c6b9 --- /dev/null +++ b/src/algorithms/tracking/libs/jamfile.jam @@ -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 ; \ No newline at end of file diff --git a/src/algorithms/tracking/libs/tracking_2rd_DLL_filter.cc b/src/algorithms/tracking/libs/tracking_2rd_DLL_filter.cc new file mode 100644 index 000000000..43f4a04be --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_2rd_DLL_filter.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + + +#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 () +{ + +} diff --git a/src/algorithms/tracking/libs/tracking_2rd_DLL_filter.h b/src/algorithms/tracking/libs/tracking_2rd_DLL_filter.h new file mode 100644 index 000000000..f403c5709 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_2rd_DLL_filter.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 diff --git a/src/algorithms/tracking/libs/tracking_2rd_PLL_filter.cc b/src/algorithms/tracking/libs/tracking_2rd_PLL_filter.cc new file mode 100644 index 000000000..b9a839676 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_2rd_PLL_filter.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 () +{ + +} diff --git a/src/algorithms/tracking/libs/tracking_2rd_PLL_filter.h b/src/algorithms/tracking/libs/tracking_2rd_PLL_filter.h new file mode 100644 index 000000000..2fa780697 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_2rd_PLL_filter.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#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 diff --git a/src/algorithms/tracking/libs/tracking_FLL_PLL_filter.cc b/src/algorithms/tracking/libs/tracking_FLL_PLL_filter.cc new file mode 100644 index 000000000..67ec76147 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_FLL_PLL_filter.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include "tracking_FLL_PLL_filter.h" +#include + +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 = "<. + * + * ------------------------------------------------------------------------- + */ + +#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 diff --git a/src/algorithms/tracking/libs/tracking_discriminators.cc b/src/algorithms/tracking/libs/tracking_discriminators.cc new file mode 100644 index 000000000..ab04f20ee --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_discriminators.cc @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#include "tracking_discriminators.h" +#include + +// 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)); +} diff --git a/src/algorithms/tracking/libs/tracking_discriminators.h b/src/algorithms/tracking/libs/tracking_discriminators.h new file mode 100644 index 000000000..8420f6d28 --- /dev/null +++ b/src/algorithms/tracking/libs/tracking_discriminators.h @@ -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 . + * + * ------------------------------------------------------------------------- + */ + +#ifndef TRACKING_DISCRIMINATORS_H_ +#define TRACKING_DISCRIMINATORS_H_ + +#include + +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 diff --git a/src/core/receiver/gnss_block_factory.cc b/src/core/receiver/gnss_block_factory.cc index 9bae2c3cc..cdc49367b 100644 --- a/src/core/receiver/gnss_block_factory.cc +++ b/src/core/receiver/gnss_block_factory.cc @@ -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, diff --git a/src/core/system_parameters/GPS_L1_CA.h b/src/core/system_parameters/GPS_L1_CA.h index 4aec10b51..783b5d87d 100644 --- a/src/core/system_parameters/GPS_L1_CA.h +++ b/src/core/system_parameters/GPS_L1_CA.h @@ -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} diff --git a/src/main/jamfile.jam b/src/main/jamfile.jam index a83d2510f..687625e5a 100644 --- a/src/main/jamfile.jam +++ b/src/main/jamfile.jam @@ -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 diff --git a/src/main/main.cc b/src/main/main.cc index 837ed0465..aeb66fac0 100644 --- a/src/main/main.cc +++ b/src/main/main.cc @@ -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