mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2026-09-24 01:48:53 +00:00
Merge branch 'glonass' of https://github.com/gnss-sdr/gnss-sdr
Grab latest code addition to ease up pull request
This commit is contained in:
Damian Miralles
@@ -211,10 +211,12 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration,
|
||||
}
|
||||
|
||||
int num_bands = 0;
|
||||
|
||||
if ((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) num_bands = 1;
|
||||
if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && (gps_2S_count > 0) ) num_bands = 2;
|
||||
if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0)) ) num_bands = 2;
|
||||
if (((gps_1C_count > 0) || (gal_1B_count > 0) || (glo_1G_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0))) num_bands = 3;
|
||||
|
||||
int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */
|
||||
if( (number_of_frequencies < 1) || (number_of_frequencies > 3) )
|
||||
{
|
||||
|
||||
@@ -138,75 +138,72 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
|
||||
{
|
||||
case 'E':
|
||||
{
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.end())
|
||||
{
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
// Galileo E1
|
||||
if(sig_.compare("1B") == 0)
|
||||
{
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
|
||||
{
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
0);
|
||||
valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
// Galileo E1
|
||||
if(sig_.compare("1B") == 0)
|
||||
{
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
|
||||
{
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
0);
|
||||
valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
// Galileo E5
|
||||
if(sig_.compare("5X") == 0)
|
||||
{
|
||||
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
|
||||
{
|
||||
bool found_E1_obs=false;
|
||||
for (int i = 0; i < valid_obs; i++)
|
||||
{
|
||||
if (eph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO)))
|
||||
{
|
||||
obs_data[i+glo_valid_obs] = insert_obs_to_rtklib(obs_data[i+glo_valid_obs],
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
2);//Band 3 (L5/E5)
|
||||
found_E1_obs=true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found_E1_obs)
|
||||
{
|
||||
//insert Galileo E5 obs as new obs and also insert its ephemeris
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
2); //Band 3 (L5/E5)
|
||||
valid_obs++;
|
||||
}
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
}
|
||||
// 1 Gal - find the ephemeris for the current GALILEO SV observation. The SV PRN ID is the map key
|
||||
galileo_ephemeris_iter = galileo_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (galileo_ephemeris_iter != galileo_ephemeris_map.cend())
|
||||
{
|
||||
bool found_E1_obs = false;
|
||||
for (int i = 0; i < valid_obs; i++)
|
||||
{
|
||||
if (eph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN + NSATGPS + NSATGLO)))
|
||||
{
|
||||
obs_data[i + glo_valid_obs] = insert_obs_to_rtklib(obs_data[i + glo_valid_obs],
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
2);//Band 3 (L5/E5)
|
||||
found_E1_obs = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found_E1_obs)
|
||||
{
|
||||
//insert Galileo E5 obs as new obs and also insert its ephemeris
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
unsigned char default_code_ = static_cast<unsigned char>(CODE_NONE);
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {},
|
||||
{default_code_, default_code_, default_code_},
|
||||
{}, {0.0, 0.0, 0.0}, {}};
|
||||
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
galileo_ephemeris_iter->second.WN_5,
|
||||
2); //Band 3 (L5/E5)
|
||||
valid_obs++;
|
||||
}
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 'G':
|
||||
@@ -280,75 +277,75 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & gnss_observables_
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 'R': //TODO This should be using rtk lib nomenclature
|
||||
{
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
// GLONASS GNAV L1
|
||||
if(sig_.compare("1G") == 0)
|
||||
{
|
||||
// 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key
|
||||
glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend())
|
||||
{
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
glonass_gnav_ephemeris_iter->second.d_WN,
|
||||
0);//Band 0 (L1)
|
||||
glo_valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
case 'R': //TODO This should be using rtk lib nomenclature
|
||||
{
|
||||
std::string sig_(gnss_observables_iter->second.Signal);
|
||||
// GLONASS GNAV L1
|
||||
if(sig_.compare("1G") == 0)
|
||||
{
|
||||
// 1 Glo - find the ephemeris for the current GLONASS SV observation. The SV Slot Number (PRN ID) is the map key
|
||||
glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend())
|
||||
{
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
glonass_gnav_ephemeris_iter->second.d_WN,
|
||||
0);//Band 0 (L1)
|
||||
glo_valid_obs++;
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
|
||||
}
|
||||
// GLONASS GNAV L2
|
||||
if(sig_.compare("2G") == 0)
|
||||
{
|
||||
// 1 GLONASS - find the ephemeris for the current GLONASS SV observation. The SV PRN ID is the map key
|
||||
glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend())
|
||||
{
|
||||
bool found_L1_obs=false;
|
||||
for (int i = 0; i < glo_valid_obs; i++)
|
||||
{
|
||||
if (geph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN+NSATGPS)))
|
||||
{
|
||||
obs_data[i+valid_obs] = insert_obs_to_rtklib(obs_data[i+valid_obs],
|
||||
gnss_observables_iter->second,
|
||||
glonass_gnav_ephemeris_iter->second.d_WN,
|
||||
1);//Band 1 (L2)
|
||||
found_L1_obs=true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found_L1_obs)
|
||||
{
|
||||
//insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
glonass_gnav_ephemeris_iter->second.d_WN,
|
||||
1); //Band 1 (L2)
|
||||
glo_valid_obs++;
|
||||
}
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
}
|
||||
// GLONASS GNAV L2
|
||||
if(sig_.compare("2G") == 0)
|
||||
{
|
||||
// 1 GLONASS - find the ephemeris for the current GLONASS SV observation. The SV PRN ID is the map key
|
||||
glonass_gnav_ephemeris_iter = glonass_gnav_ephemeris_map.find(gnss_observables_iter->second.PRN);
|
||||
if (glonass_gnav_ephemeris_iter != glonass_gnav_ephemeris_map.cend())
|
||||
{
|
||||
bool found_L1_obs = false;
|
||||
for (int i = 0; i < glo_valid_obs; i++)
|
||||
{
|
||||
if (geph_data[i].sat == (static_cast<int>(gnss_observables_iter->second.PRN+NSATGPS)))
|
||||
{
|
||||
obs_data[i+valid_obs] = insert_obs_to_rtklib(obs_data[i+valid_obs],
|
||||
gnss_observables_iter->second,
|
||||
glonass_gnav_ephemeris_iter->second.d_WN,
|
||||
1);//Band 1 (L2)
|
||||
found_L1_obs = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found_L1_obs)
|
||||
{
|
||||
//insert GLONASS GNAV L2 obs as new obs and also insert its ephemeris
|
||||
//convert ephemeris from GNSS-SDR class to RTKLIB structure
|
||||
geph_data[glo_valid_obs] = eph_to_rtklib(glonass_gnav_ephemeris_iter->second, gnav_utc);
|
||||
//convert observation from GNSS-SDR class to RTKLIB structure
|
||||
obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}};
|
||||
obs_data[valid_obs+glo_valid_obs] = insert_obs_to_rtklib(newobs,
|
||||
gnss_observables_iter->second,
|
||||
glonass_gnav_ephemeris_iter->second.d_WN,
|
||||
1); //Band 1 (L2)
|
||||
glo_valid_obs++;
|
||||
}
|
||||
}
|
||||
else // the ephemeris are not available for this SV
|
||||
{
|
||||
DLOG(INFO) << "No ephemeris data for SV " << gnss_observables_iter->second.PRN;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
default :
|
||||
DLOG(INFO) << "Hybrid observables: Unknown GNSS";
|
||||
break;
|
||||
|
||||
@@ -24,6 +24,7 @@ set(ACQ_ADAPTER_SOURCES
|
||||
gps_l1_ca_pcps_tong_acquisition.cc
|
||||
gps_l1_ca_pcps_quicksync_acquisition.cc
|
||||
gps_l2_m_pcps_acquisition.cc
|
||||
gps_l5i_pcps_acquisition.cc
|
||||
galileo_e1_pcps_ambiguous_acquisition.cc
|
||||
galileo_e1_pcps_cccwsr_ambiguous_acquisition.cc
|
||||
galileo_e1_pcps_quicksync_ambiguous_acquisition.cc
|
||||
|
||||
@@ -0,0 +1,387 @@
|
||||
/*!
|
||||
* \file gps_l5i pcps_acquisition.cc
|
||||
* \brief Adapts a PCPS acquisition block to an Acquisition Interface for
|
||||
* GPS L5i signals
|
||||
* \authors <ul>
|
||||
* <li> Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
* </ul>
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2017 (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_l5i_pcps_acquisition.h"
|
||||
#include <boost/math/distributions/exponential.hpp>
|
||||
#include <glog/logging.h>
|
||||
#include "gps_l5_signal.h"
|
||||
#include "GPS_L5.h"
|
||||
#include "configuration_interface.h"
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
|
||||
ConfigurationInterface* configuration, std::string role,
|
||||
unsigned int in_streams, unsigned int out_streams) :
|
||||
role_(role), in_streams_(in_streams), out_streams_(out_streams)
|
||||
{
|
||||
configuration_ = configuration;
|
||||
std::string default_item_type = "gr_complex";
|
||||
std::string default_dump_filename = "./data/acquisition.dat";
|
||||
|
||||
LOG(INFO) << "role " << role;
|
||||
|
||||
item_type_ = configuration_->property(role + ".item_type", default_item_type);
|
||||
|
||||
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
|
||||
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
|
||||
if_ = configuration_->property(role + ".if", 0);
|
||||
dump_ = configuration_->property(role + ".dump", false);
|
||||
blocking_ = configuration_->property(role + ".blocking", true);
|
||||
doppler_max_ = configuration->property(role + ".doppler_max", 5000);
|
||||
|
||||
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
|
||||
use_CFAR_algorithm_flag_=configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
|
||||
|
||||
max_dwells_ = configuration_->property(role + ".max_dwells", 1);
|
||||
|
||||
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
|
||||
|
||||
//--- Find number of samples per spreading code -------------------------
|
||||
code_length_ = round(static_cast<double>(fs_in_)
|
||||
/ (GPS_L5i_CODE_RATE_HZ / static_cast<double>(GPS_L5i_CODE_LENGTH_CHIPS)));
|
||||
|
||||
vector_length_ = code_length_;
|
||||
|
||||
if( bit_transition_flag_ )
|
||||
{
|
||||
vector_length_ *= 2;
|
||||
}
|
||||
|
||||
code_ = new gr_complex[vector_length_];
|
||||
|
||||
if (item_type_.compare("cshort") == 0 )
|
||||
{
|
||||
item_size_ = sizeof(lv_16sc_t);
|
||||
acquisition_sc_ = pcps_make_acquisition_sc(1, max_dwells_,
|
||||
doppler_max_, if_, fs_in_, code_length_, code_length_,
|
||||
bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_, dump_filename_);
|
||||
DLOG(INFO) << "acquisition(" << acquisition_sc_->unique_id() << ")";
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
item_size_ = sizeof(gr_complex);
|
||||
acquisition_cc_ = pcps_make_acquisition_cc(1, max_dwells_,
|
||||
doppler_max_, if_, fs_in_, code_length_, code_length_,
|
||||
bit_transition_flag_, use_CFAR_algorithm_flag_, dump_, blocking_,
|
||||
dump_filename_);
|
||||
DLOG(INFO) << "acquisition(" << acquisition_cc_->unique_id() << ")";
|
||||
}
|
||||
|
||||
stream_to_vector_ = gr::blocks::stream_to_vector::make(item_size_, vector_length_);
|
||||
DLOG(INFO) << "stream_to_vector(" << stream_to_vector_->unique_id() << ")";
|
||||
|
||||
if (item_type_.compare("cbyte") == 0)
|
||||
{
|
||||
cbyte_to_float_x2_ = make_complex_byte_to_float_x2();
|
||||
float_to_complex_ = gr::blocks::float_to_complex::make();
|
||||
}
|
||||
|
||||
channel_ = 0;
|
||||
threshold_ = 0.0;
|
||||
doppler_step_ = 0;
|
||||
gnss_synchro_ = 0;
|
||||
}
|
||||
|
||||
|
||||
GpsL5iPcpsAcquisition::~GpsL5iPcpsAcquisition()
|
||||
{
|
||||
delete[] code_;
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iPcpsAcquisition::set_channel(unsigned int channel)
|
||||
{
|
||||
channel_ = channel;
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->set_channel(channel_);
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->set_channel(channel_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iPcpsAcquisition::set_threshold(float threshold)
|
||||
{
|
||||
float pfa = configuration_->property(role_ + boost::lexical_cast<std::string>(channel_) + ".pfa", 0.0);
|
||||
|
||||
if(pfa == 0.0)
|
||||
{
|
||||
pfa = configuration_->property(role_ + ".pfa", 0.0);
|
||||
}
|
||||
if(pfa == 0.0)
|
||||
{
|
||||
threshold_ = threshold;
|
||||
}
|
||||
else
|
||||
{
|
||||
threshold_ = calculate_threshold(pfa);
|
||||
}
|
||||
|
||||
DLOG(INFO) << "Channel " << channel_ <<" Threshold = " << threshold_;
|
||||
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->set_threshold(threshold_);
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->set_threshold(threshold_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iPcpsAcquisition::set_doppler_max(unsigned int doppler_max)
|
||||
{
|
||||
doppler_max_ = doppler_max;
|
||||
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->set_doppler_max(doppler_max_);
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->set_doppler_max(doppler_max_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Be aware that Doppler step should be set to 2/(3T) Hz, where T is the coherent integration time (GPS L2 period is 0.02s)
|
||||
// Doppler bin minimum size= 33 Hz
|
||||
void GpsL5iPcpsAcquisition::set_doppler_step(unsigned int doppler_step)
|
||||
{
|
||||
doppler_step_ = doppler_step;
|
||||
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->set_doppler_step(doppler_step_);
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->set_doppler_step(doppler_step_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iPcpsAcquisition::set_gnss_synchro(Gnss_Synchro* gnss_synchro)
|
||||
{
|
||||
gnss_synchro_ = gnss_synchro;
|
||||
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->set_gnss_synchro(gnss_synchro_);
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->set_gnss_synchro(gnss_synchro_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
signed int GpsL5iPcpsAcquisition::mag()
|
||||
{
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
return acquisition_sc_->mag();
|
||||
}
|
||||
else
|
||||
{
|
||||
return acquisition_cc_->mag();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iPcpsAcquisition::init()
|
||||
{
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->init();
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->init();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void GpsL5iPcpsAcquisition::set_local_code()
|
||||
{
|
||||
|
||||
gps_l5i_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_);
|
||||
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->set_local_code(code_);
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->set_local_code(code_);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iPcpsAcquisition::reset()
|
||||
{
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->set_active(true);
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->set_active(true);
|
||||
}
|
||||
}
|
||||
|
||||
void GpsL5iPcpsAcquisition::set_state(int state)
|
||||
{
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
acquisition_sc_->set_state(state);
|
||||
}
|
||||
else
|
||||
{
|
||||
acquisition_cc_->set_state(state);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
float GpsL5iPcpsAcquisition::calculate_threshold(float pfa)
|
||||
{
|
||||
//Calculate the threshold
|
||||
unsigned int frequency_bins = 0;
|
||||
for (int doppler = static_cast<int>(-doppler_max_); doppler <= static_cast<int>(doppler_max_); doppler += doppler_step_)
|
||||
{
|
||||
frequency_bins++;
|
||||
}
|
||||
DLOG(INFO) << "Channel " << channel_<< " Pfa = " << pfa;
|
||||
unsigned int ncells = vector_length_ * frequency_bins;
|
||||
double exponent = 1.0 / static_cast<double>(ncells);
|
||||
double val = pow(1.0 - pfa, exponent);
|
||||
double lambda = double(vector_length_);
|
||||
boost::math::exponential_distribution<double> mydist (lambda);
|
||||
float threshold = static_cast<float>(quantile(mydist,val));
|
||||
|
||||
return threshold;
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iPcpsAcquisition::connect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if (item_type_.compare("gr_complex") == 0)
|
||||
{
|
||||
top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0);
|
||||
}
|
||||
else if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
top_block->connect(stream_to_vector_, 0, acquisition_sc_, 0);
|
||||
}
|
||||
else if (item_type_.compare("cbyte") == 0)
|
||||
{
|
||||
top_block->connect(cbyte_to_float_x2_, 0, float_to_complex_, 0);
|
||||
top_block->connect(cbyte_to_float_x2_, 1, float_to_complex_, 1);
|
||||
top_block->connect(float_to_complex_, 0, stream_to_vector_, 0);
|
||||
top_block->connect(stream_to_vector_, 0, acquisition_cc_, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << item_type_ << " unknown acquisition item type";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iPcpsAcquisition::disconnect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if (item_type_.compare("gr_complex") == 0)
|
||||
{
|
||||
top_block->disconnect(stream_to_vector_, 0, acquisition_cc_, 0);
|
||||
}
|
||||
else if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
top_block->disconnect(stream_to_vector_, 0, acquisition_sc_, 0);
|
||||
}
|
||||
else if (item_type_.compare("cbyte") == 0)
|
||||
{
|
||||
// Since a byte-based acq implementation is not available,
|
||||
// we just convert cshorts to gr_complex
|
||||
top_block->disconnect(cbyte_to_float_x2_, 0, float_to_complex_, 0);
|
||||
top_block->disconnect(cbyte_to_float_x2_, 1, float_to_complex_, 1);
|
||||
top_block->disconnect(float_to_complex_, 0, stream_to_vector_, 0);
|
||||
top_block->disconnect(stream_to_vector_, 0, acquisition_cc_, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << item_type_ << " unknown acquisition item type";
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr GpsL5iPcpsAcquisition::get_left_block()
|
||||
{
|
||||
if (item_type_.compare("gr_complex") == 0)
|
||||
{
|
||||
return stream_to_vector_;
|
||||
}
|
||||
else if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
return stream_to_vector_;
|
||||
}
|
||||
else if (item_type_.compare("cbyte") == 0)
|
||||
{
|
||||
return cbyte_to_float_x2_;
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << item_type_ << " unknown acquisition item type";
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr GpsL5iPcpsAcquisition::get_right_block()
|
||||
{
|
||||
if (item_type_.compare("cshort") == 0)
|
||||
{
|
||||
return acquisition_sc_;
|
||||
}
|
||||
else
|
||||
{
|
||||
return acquisition_cc_;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,171 @@
|
||||
/*!
|
||||
* \file GPS_L5i_PCPS_Acquisition.h
|
||||
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
|
||||
* GPS L5i signals
|
||||
* \authors <ul>
|
||||
* <li> Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
* </ul>
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_
|
||||
#define GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_
|
||||
|
||||
#include <string>
|
||||
#include <gnuradio/blocks/stream_to_vector.h>
|
||||
#include <gnuradio/blocks/float_to_complex.h>
|
||||
#include "gnss_synchro.h"
|
||||
#include "acquisition_interface.h"
|
||||
#include "pcps_acquisition_cc.h"
|
||||
#include "pcps_acquisition_sc.h"
|
||||
#include "complex_byte_to_float_x2.h"
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
|
||||
|
||||
|
||||
class ConfigurationInterface;
|
||||
|
||||
/*!
|
||||
* \brief This class adapts a PCPS acquisition block to an AcquisitionInterface
|
||||
* for GPS L5i signals
|
||||
*/
|
||||
class GpsL5iPcpsAcquisition: public AcquisitionInterface
|
||||
{
|
||||
public:
|
||||
GpsL5iPcpsAcquisition(ConfigurationInterface* configuration,
|
||||
std::string role, unsigned int in_streams,
|
||||
unsigned int out_streams);
|
||||
|
||||
virtual ~GpsL5iPcpsAcquisition();
|
||||
|
||||
inline std::string role() override
|
||||
{
|
||||
return role_;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Returns "GPS_L5i_PCPS_Acquisition"
|
||||
*/
|
||||
inline std::string implementation() override
|
||||
{
|
||||
return "GPS_L5i_PCPS_Acquisition";
|
||||
}
|
||||
|
||||
inline size_t item_size() override
|
||||
{
|
||||
return item_size_;
|
||||
}
|
||||
|
||||
void connect(gr::top_block_sptr top_block) override;
|
||||
void disconnect(gr::top_block_sptr top_block) override;
|
||||
gr::basic_block_sptr get_left_block() override;
|
||||
gr::basic_block_sptr get_right_block() override;
|
||||
|
||||
/*!
|
||||
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
|
||||
* to efficiently exchange synchronization data between acquisition and
|
||||
* tracking blocks
|
||||
*/
|
||||
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override;
|
||||
|
||||
/*!
|
||||
* \brief Set acquisition channel unique ID
|
||||
*/
|
||||
void set_channel(unsigned int channel) override;
|
||||
|
||||
/*!
|
||||
* \brief Set statistics threshold of PCPS algorithm
|
||||
*/
|
||||
void set_threshold(float threshold) override;
|
||||
|
||||
/*!
|
||||
* \brief Set maximum Doppler off grid search
|
||||
*/
|
||||
void set_doppler_max(unsigned int doppler_max) override;
|
||||
|
||||
/*!
|
||||
* \brief Set Doppler steps for the grid search
|
||||
*/
|
||||
void set_doppler_step(unsigned int doppler_step) override;
|
||||
|
||||
/*!
|
||||
* \brief Initializes acquisition algorithm.
|
||||
*/
|
||||
void init() override;
|
||||
|
||||
/*!
|
||||
* \brief Sets local code for GPS L2/M PCPS acquisition algorithm.
|
||||
*/
|
||||
void set_local_code() override;
|
||||
|
||||
/*!
|
||||
* \brief Returns the maximum peak of grid search
|
||||
*/
|
||||
signed int mag() override;
|
||||
|
||||
/*!
|
||||
* \brief Restart acquisition algorithm
|
||||
*/
|
||||
void reset() override;
|
||||
|
||||
/*!
|
||||
* \brief If state = 1, it forces the block to start acquiring from the first sample
|
||||
*/
|
||||
void set_state(int state);
|
||||
|
||||
private:
|
||||
ConfigurationInterface* configuration_;
|
||||
pcps_acquisition_cc_sptr acquisition_cc_;
|
||||
pcps_acquisition_sc_sptr acquisition_sc_;
|
||||
gr::blocks::stream_to_vector::sptr stream_to_vector_;
|
||||
gr::blocks::float_to_complex::sptr float_to_complex_;
|
||||
complex_byte_to_float_x2_sptr cbyte_to_float_x2_;
|
||||
size_t item_size_;
|
||||
std::string item_type_;
|
||||
unsigned int vector_length_;
|
||||
unsigned int code_length_;
|
||||
bool bit_transition_flag_;
|
||||
bool use_CFAR_algorithm_flag_;
|
||||
unsigned int channel_;
|
||||
float threshold_;
|
||||
unsigned int doppler_max_;
|
||||
unsigned int doppler_step_;
|
||||
unsigned int max_dwells_;
|
||||
long fs_in_;
|
||||
long if_;
|
||||
bool dump_;
|
||||
bool blocking_;
|
||||
std::string dump_filename_;
|
||||
std::complex<float> * code_;
|
||||
Gnss_Synchro * gnss_synchro_;
|
||||
std::string role_;
|
||||
unsigned int in_streams_;
|
||||
unsigned int out_streams_;
|
||||
|
||||
float calculate_threshold(float pfa);
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_GPS_L5i_PCPS_ACQUISITION_H_ */
|
||||
@@ -20,6 +20,7 @@ add_subdirectory(rtklib)
|
||||
|
||||
set(GNSS_SPLIBS_SOURCES
|
||||
gps_l2c_signal.cc
|
||||
gps_l5_signal.cc
|
||||
galileo_e1_signal_processing.cc
|
||||
gnss_sdr_valve.cc
|
||||
gnss_sdr_sample_counter.cc
|
||||
|
||||
@@ -30,8 +30,8 @@
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_GPS_L2C_SIGNAL_H_
|
||||
#define GNSS_GPS_L2C_SIGNAL_H_
|
||||
#ifndef GNSS_SDR_GPS_L2C_SIGNAL_H_
|
||||
#define GNSS_SDR_GPS_L2C_SIGNAL_H_
|
||||
|
||||
#include <complex>
|
||||
|
||||
|
||||
@@ -0,0 +1,316 @@
|
||||
/*!
|
||||
* \file gps_l5_signal.cc
|
||||
* \brief This class implements signal generators for the GPS L5 signals
|
||||
* \author Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
*
|
||||
* Detailed description of the file here if needed.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2017 (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 <iostream>
|
||||
#include <deque>
|
||||
#include <cmath>
|
||||
#include <cinttypes>
|
||||
#include <complex>
|
||||
#include "GPS_L5.h"
|
||||
|
||||
|
||||
std::deque<bool> l5i_xa_shift(std::deque<bool> xa)
|
||||
{
|
||||
if (xa == std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,0,1})
|
||||
{
|
||||
return std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,1,1};
|
||||
}
|
||||
else
|
||||
{
|
||||
std::deque<bool> out(xa.begin(), xa.end() - 1);
|
||||
out.push_front(xa[12] xor xa[11] xor xa[9] xor xa[8]);
|
||||
return out;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
std::deque<bool> l5q_xa_shift(std::deque<bool> xa)
|
||||
{
|
||||
if (xa == std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,0,1})
|
||||
{
|
||||
return std::deque<bool>{1,1,1,1,1,1,1,1,1,1,1,1,1};
|
||||
}
|
||||
else
|
||||
{
|
||||
std::deque<bool> out(xa.begin(), xa.end() - 1);
|
||||
out.push_front(xa[12] xor xa[11] xor xa[9] xor xa[8]);
|
||||
return out;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
std::deque<bool> l5i_xb_shift(std::deque<bool> xb)
|
||||
{
|
||||
std::deque<bool> out(xb.begin(), xb.end() - 1);
|
||||
out.push_front(xb[12] xor xb[11] xor xb[7] xor xb[6] xor xb[5] xor xb[3] xor xb[2] xor xb[0]);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
std::deque<bool> l5q_xb_shift(std::deque<bool> xb)
|
||||
{
|
||||
std::deque<bool> out(xb.begin(), xb.end()-1);
|
||||
out.push_front(xb[12] xor xb[11] xor xb[7] xor xb[6] xor xb[5] xor xb[3] xor xb[2] xor xb[0]);
|
||||
return out;
|
||||
}
|
||||
|
||||
|
||||
std::deque<bool> make_l5i_xa()
|
||||
{
|
||||
std::deque<bool> xa = {1,1,1,1,1,1,1,1,1,1,1,1,1};
|
||||
std::deque<bool> y(GPS_L5i_CODE_LENGTH_CHIPS, 0);
|
||||
|
||||
for (int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++)
|
||||
{
|
||||
y[i] = xa[12];
|
||||
xa = l5i_xa_shift(xa);
|
||||
}
|
||||
return y;
|
||||
}
|
||||
|
||||
|
||||
std::deque<bool> make_l5i_xb()
|
||||
{
|
||||
std::deque<bool> xb = {1,1,1,1,1,1,1,1,1,1,1,1,1};
|
||||
std::deque<bool> y(GPS_L5i_CODE_LENGTH_CHIPS,0);
|
||||
|
||||
for (int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++)
|
||||
{
|
||||
y[i] = xb[12];
|
||||
xb = l5i_xb_shift(xb);
|
||||
}
|
||||
return y;
|
||||
}
|
||||
|
||||
|
||||
std::deque<bool> make_l5q_xa()
|
||||
{
|
||||
std::deque<bool> xa = {1,1,1,1,1,1,1,1,1,1,1,1,1};
|
||||
std::deque<bool> y(GPS_L5q_CODE_LENGTH_CHIPS, 0);
|
||||
|
||||
for (int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++)
|
||||
{
|
||||
y[i] = xa[12];
|
||||
xa = l5q_xa_shift(xa);
|
||||
}
|
||||
return y;
|
||||
}
|
||||
|
||||
|
||||
std::deque<bool> make_l5q_xb()
|
||||
{
|
||||
std::deque<bool> xb = {1,1,1,1,1,1,1,1,1,1,1,1,1};
|
||||
std::deque<bool> y(GPS_L5q_CODE_LENGTH_CHIPS, 0);
|
||||
|
||||
for (int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++)
|
||||
{
|
||||
y[i] = xb[12];
|
||||
xb = l5q_xb_shift(xb);
|
||||
}
|
||||
return y;
|
||||
}
|
||||
|
||||
|
||||
void make_l5i(int32_t * _dest, int prn)
|
||||
{
|
||||
int xb_offset = GPS_L5i_INIT_REG[prn];
|
||||
|
||||
std::deque<bool> xb = make_l5i_xb();
|
||||
std::deque<bool> xa = make_l5i_xa();
|
||||
std::deque<bool> xb_shift(GPS_L5i_CODE_LENGTH_CHIPS, 0);
|
||||
|
||||
for (int n = 0; n < GPS_L5i_CODE_LENGTH_CHIPS; n++)
|
||||
{
|
||||
xb_shift[n] = xb[(xb_offset + n) % GPS_L5i_CODE_LENGTH_CHIPS];
|
||||
}
|
||||
std::deque<bool> out_code(GPS_L5i_CODE_LENGTH_CHIPS, 0);
|
||||
for (int n = 0; n < GPS_L5i_CODE_LENGTH_CHIPS; n++)
|
||||
{
|
||||
_dest[n] = xa[n] xor xb_shift[n];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void make_l5q(int32_t * _dest, int prn)
|
||||
{
|
||||
int xb_offset = GPS_L5q_INIT_REG[prn];
|
||||
|
||||
std::deque<bool> xb = make_l5q_xb();
|
||||
std::deque<bool> xa = make_l5q_xa();
|
||||
std::deque<bool> xb_shift(GPS_L5q_CODE_LENGTH_CHIPS, 0);
|
||||
|
||||
for (int n = 0; n < GPS_L5q_CODE_LENGTH_CHIPS; n++)
|
||||
{
|
||||
xb_shift[n] = xb[(xb_offset + n) % GPS_L5q_CODE_LENGTH_CHIPS];
|
||||
}
|
||||
std::deque<bool> out_code(GPS_L5q_CODE_LENGTH_CHIPS, 0);
|
||||
for (int n = 0; n < GPS_L5q_CODE_LENGTH_CHIPS; n++)
|
||||
{
|
||||
_dest[n] = xa[n] xor xb_shift[n];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void gps_l5i_code_gen_complex(std::complex<float>* _dest, unsigned int _prn)
|
||||
{
|
||||
int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS];
|
||||
|
||||
if (_prn > 0 and _prn < 51)
|
||||
{
|
||||
make_l5i(_code, _prn);
|
||||
}
|
||||
|
||||
for (signed int i = 0; i < GPS_L5i_CODE_LENGTH_CHIPS; i++)
|
||||
{
|
||||
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[i], 0.0);
|
||||
}
|
||||
|
||||
delete[] _code;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency
|
||||
*/
|
||||
void gps_l5i_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs)
|
||||
{
|
||||
int32_t* _code = new int32_t[GPS_L5i_CODE_LENGTH_CHIPS];
|
||||
if (_prn > 0 and _prn < 51)
|
||||
{
|
||||
make_l5i(_code, _prn);
|
||||
}
|
||||
|
||||
signed int _samplesPerCode, _codeValueIndex;
|
||||
float _ts;
|
||||
float _tc;
|
||||
const signed int _codeLength = GPS_L5i_CODE_LENGTH_CHIPS;
|
||||
|
||||
//--- Find number of samples per spreading code ----------------------------
|
||||
_samplesPerCode = static_cast<int>(static_cast<double>(_fs) / (static_cast<double>(GPS_L5i_CODE_RATE_HZ) / static_cast<double>(_codeLength)));
|
||||
|
||||
//--- Find time constants --------------------------------------------------
|
||||
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
|
||||
_tc = 1.0 / static_cast<float>(GPS_L5i_CODE_RATE_HZ); // C/A chip period in sec
|
||||
|
||||
//float aux;
|
||||
for (signed int i = 0; i < _samplesPerCode; i++)
|
||||
{
|
||||
//=== Digitizing =======================================================
|
||||
|
||||
//--- Make index array to read L5 code values -------------------------
|
||||
//TODO: Check this formula! Seems to start with an extra sample
|
||||
_codeValueIndex = ceil((_ts * (static_cast<float>(i) + 1)) / _tc) - 1;
|
||||
//aux = (_ts * (i + 1)) / _tc;
|
||||
//_codeValueIndex = static_cast<int>(static_cast<long>(aux)) - 1;
|
||||
|
||||
//--- Make the digitized version of the L2C code -----------------------
|
||||
if (i == _samplesPerCode - 1)
|
||||
{
|
||||
//--- Correct the last index (due to number rounding issues) -----------
|
||||
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeLength - 1], 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
|
||||
}
|
||||
}
|
||||
delete[] _code;
|
||||
}
|
||||
|
||||
|
||||
void gps_l5q_code_gen_complex(std::complex<float>* _dest, unsigned int _prn)
|
||||
{
|
||||
int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS];
|
||||
|
||||
if (_prn > 0 and _prn < 51)
|
||||
{
|
||||
make_l5q(_code, _prn);
|
||||
}
|
||||
|
||||
for (signed int i = 0; i < GPS_L5q_CODE_LENGTH_CHIPS; i++)
|
||||
{
|
||||
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[i], 0.0);
|
||||
}
|
||||
|
||||
delete[] _code;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Generates complex GPS L5i code for the desired SV ID and sampled to specific sampling frequency
|
||||
*/
|
||||
void gps_l5q_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs)
|
||||
{
|
||||
int32_t* _code = new int32_t[GPS_L5q_CODE_LENGTH_CHIPS];
|
||||
if (_prn > 0 and _prn < 51)
|
||||
{
|
||||
make_l5q(_code, _prn);
|
||||
}
|
||||
|
||||
signed int _samplesPerCode, _codeValueIndex;
|
||||
float _ts;
|
||||
float _tc;
|
||||
const signed int _codeLength = GPS_L5q_CODE_LENGTH_CHIPS;
|
||||
|
||||
//--- Find number of samples per spreading code ----------------------------
|
||||
_samplesPerCode = static_cast<int>(static_cast<double>(_fs) / (static_cast<double>(GPS_L5q_CODE_RATE_HZ) / static_cast<double>(_codeLength)));
|
||||
|
||||
//--- Find time constants --------------------------------------------------
|
||||
_ts = 1.0 / static_cast<float>(_fs); // Sampling period in sec
|
||||
_tc = 1.0 / static_cast<float>(GPS_L5q_CODE_RATE_HZ); // C/A chip period in sec
|
||||
|
||||
//float aux;
|
||||
for (signed int i = 0; i < _samplesPerCode; i++)
|
||||
{
|
||||
//=== Digitizing =======================================================
|
||||
|
||||
//--- Make index array to read L5 code values -------------------------
|
||||
//TODO: Check this formula! Seems to start with an extra sample
|
||||
_codeValueIndex = ceil((_ts * (static_cast<float>(i) + 1)) / _tc) - 1;
|
||||
//aux = (_ts * (i + 1)) / _tc;
|
||||
//_codeValueIndex = static_cast<int>(static_cast<long>(aux)) - 1;
|
||||
|
||||
//--- Make the digitized version of the L2C code -----------------------
|
||||
if (i == _samplesPerCode - 1)
|
||||
{
|
||||
//--- Correct the last index (due to number rounding issues) -----------
|
||||
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeLength - 1], 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
_dest[i] = std::complex<float>(1.0 - 2.0 * _code[_codeValueIndex], 0); //repeat the chip -> upsample
|
||||
}
|
||||
}
|
||||
delete[] _code;
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,52 @@
|
||||
/*!
|
||||
* \file gps_l5_signal.h
|
||||
* \brief This class implements signal generators for the GPS L5 signals
|
||||
* \author Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
*
|
||||
* Detailed description of the file here if needed.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2017 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GPS_L5_SIGNAL_H_
|
||||
#define GNSS_SDR_GPS_L5_SIGNAL_H_
|
||||
|
||||
#include <complex>
|
||||
|
||||
|
||||
//!Generates complex GPS L5i M code for the desired SV ID
|
||||
void gps_l5i_code_gen_complex(std::complex<float>* _dest, unsigned int _prn);
|
||||
|
||||
//!Generates complex GPS L5q M code for the desired SV ID
|
||||
void gps_l5q_code_gen_complex(std::complex<float>* _dest, unsigned int _prn);
|
||||
|
||||
//! Generates complex GPS L5i M code for the desired SV ID, and sampled to specific sampling frequency
|
||||
void gps_l5i_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs);
|
||||
|
||||
//! Generates complex GPS L5q M code for the desired SV ID, and sampled to specific sampling frequency
|
||||
void gps_l5q_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs);
|
||||
|
||||
|
||||
#endif /* GNSS_SDR_GPS_L5_SIGNAL_H_ */
|
||||
@@ -36,7 +36,18 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch
|
||||
rtklib_obs.D[band] = gnss_synchro.Carrier_Doppler_hz;
|
||||
rtklib_obs.P[band] = gnss_synchro.Pseudorange_m;
|
||||
rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / (2.0 * PI);
|
||||
|
||||
switch(band)
|
||||
{
|
||||
case 0:
|
||||
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L1C);
|
||||
break;
|
||||
case 1:
|
||||
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L2S);
|
||||
break;
|
||||
case 2:
|
||||
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L5X);
|
||||
break;
|
||||
}
|
||||
double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz;
|
||||
if (CN0_dB_Hz_est > 63.75) CN0_dB_Hz_est = 63.75;
|
||||
if (CN0_dB_Hz_est < 0.0) CN0_dB_Hz_est = 0.0;
|
||||
@@ -60,7 +71,6 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch
|
||||
}
|
||||
rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time);
|
||||
rtklib_obs.rcv = 1;
|
||||
//printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec);
|
||||
return rtklib_obs;
|
||||
}
|
||||
|
||||
@@ -132,8 +142,8 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph)
|
||||
rtklib_sat.f0 = gal_eph.af0_4;
|
||||
rtklib_sat.f1 = gal_eph.af1_4;
|
||||
rtklib_sat.f2 = gal_eph.af2_4;
|
||||
rtklib_sat.tgd[0] = 0;
|
||||
rtklib_sat.tgd[1] = 0;
|
||||
rtklib_sat.tgd[0] = gal_eph.BGD_E1E5a_5;
|
||||
rtklib_sat.tgd[1] = gal_eph.BGD_E1E5b_5;
|
||||
rtklib_sat.tgd[2] = 0;
|
||||
rtklib_sat.tgd[3] = 0;
|
||||
rtklib_sat.toes = gal_eph.t0e_1;
|
||||
|
||||
@@ -84,7 +84,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
|
||||
int iter, const prcopt_t *opt, double *var)
|
||||
{
|
||||
const double *lam = nav->lam[obs->sat - 1];
|
||||
double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma;
|
||||
double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma_;
|
||||
int i = 0, j = 1, sys;
|
||||
|
||||
*var = 0.0;
|
||||
@@ -124,7 +124,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
|
||||
}
|
||||
}
|
||||
}
|
||||
gamma = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */
|
||||
gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */
|
||||
P1 = obs->P[i];
|
||||
P2 = obs->P[j];
|
||||
P1_P2 = nav->cbias[obs->sat-1][0];
|
||||
@@ -134,7 +134,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
|
||||
/* if no P1-P2 DCB, use TGD instead */
|
||||
if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS)))
|
||||
{
|
||||
P1_P2 = (1.0 - gamma) * gettgd(obs->sat, nav);
|
||||
P1_P2 = (1.0 - gamma_) * gettgd(obs->sat, nav);
|
||||
}
|
||||
if (opt->ionoopt == IONOOPT_IFLC)
|
||||
{ /* dual-frequency */
|
||||
@@ -144,15 +144,30 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
|
||||
if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */
|
||||
|
||||
/* iono-free combination */
|
||||
PC = (gamma * P1 - P2) / (gamma - 1.0);
|
||||
PC = (gamma_ * P1 - P2) / (gamma_ - 1.0);
|
||||
}
|
||||
else
|
||||
{ /* single-frequency */
|
||||
if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)){return 0.0;}
|
||||
|
||||
if (P1 == 0.0) return 0.0;
|
||||
if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */
|
||||
PC = P1 - P1_P2 / (1.0 - gamma);
|
||||
}
|
||||
else if((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE))
|
||||
{
|
||||
P1 += P1_C1; /* C1->P1 */
|
||||
PC = P1 - P1_P2 / (1.0 - gamma_);
|
||||
}
|
||||
else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE))
|
||||
{
|
||||
P2 += P2_C2; /* C2->P2 */
|
||||
PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_);
|
||||
}
|
||||
/* dual-frequency */
|
||||
else
|
||||
{
|
||||
P1 += P1_C1;
|
||||
P2 += P2_C2;
|
||||
PC = (gamma_ * P1 - P2) / (gamma_ - 1.0);
|
||||
}
|
||||
}
|
||||
if (opt->sateph == EPHOPT_SBAS) PC -= P1_C1; /* sbas clock based C1 */
|
||||
|
||||
*var = std::pow(ERR_CBIAS, 2.0);
|
||||
@@ -285,12 +300,17 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs,
|
||||
continue;
|
||||
}
|
||||
/* geometric distance/azimuth/elevation angle */
|
||||
if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 || satazel(pos, e, azel + i * 2) < opt->elmin)
|
||||
if ((r = geodist(rs + i * 6, rr, e)) <= 0.0)
|
||||
{
|
||||
trace(4, "geodist / satazel error\n");
|
||||
trace(4, "geodist error\n");
|
||||
continue;
|
||||
}
|
||||
double elaux = satazel(pos, e, azel + i * 2);
|
||||
if(elaux < opt->elmin)
|
||||
{
|
||||
trace(4, "satazel error. el = %lf , elmin = %lf\n", elaux, opt->elmin);
|
||||
continue;
|
||||
}
|
||||
|
||||
/* psudorange with code bias correction */
|
||||
if ((P = prange(obs+i, nav, azel+i*2, iter, opt, &vmeas)) == 0.0)
|
||||
{
|
||||
@@ -671,53 +691,6 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav,
|
||||
const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
|
||||
char *msg)
|
||||
{
|
||||
// int k = 0;
|
||||
// for (k = 0;k<n;k++)
|
||||
// {
|
||||
// printf("OBS[%i]: sat %i, P:%f ,LLI:%s \r\n",k,obs[k].sat,obs[k].P[0], obs[k].LLI);
|
||||
// }
|
||||
//
|
||||
// for (k = 0;k<nav->n;k++)
|
||||
// {
|
||||
// printf("NAV[%i]: sat %i, %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \r\n",
|
||||
// k,
|
||||
// nav->eph[k].sat,
|
||||
// nav->eph[k].A,
|
||||
// nav->eph[k].Adot,
|
||||
// nav->eph[k].M0,
|
||||
// nav->eph[k].OMG0,
|
||||
// nav->eph[k].OMGd,
|
||||
// nav->eph[k].cic,
|
||||
// nav->eph[k].cis,
|
||||
// nav->eph[k].code,
|
||||
// nav->eph[k].crc,
|
||||
// nav->eph[k].crs,
|
||||
// nav->eph[k].cuc,
|
||||
// nav->eph[k].cus,
|
||||
// nav->eph[k].deln,
|
||||
// nav->eph[k].e,
|
||||
// nav->eph[k].f0,
|
||||
// nav->eph[k].f1,
|
||||
// nav->eph[k].f2,
|
||||
// nav->eph[k].fit,
|
||||
// nav->eph[k].flag,
|
||||
// nav->eph[k].i0,
|
||||
// nav->eph[k].idot,
|
||||
// nav->eph[k].iodc,
|
||||
// nav->eph[k].iode,
|
||||
// nav->eph[k].ndot,
|
||||
// nav->eph[k].omg,
|
||||
// nav->eph[k].sat,
|
||||
// nav->eph[k].sva,
|
||||
// nav->eph[k].svh,
|
||||
// nav->eph[k].tgd[0],
|
||||
// nav->eph[k].toc.sec,
|
||||
// nav->eph[k].toe.sec,
|
||||
// nav->eph[k].toes,
|
||||
// nav->eph[k].ttr.sec,
|
||||
// nav->eph[k].week);
|
||||
// }
|
||||
|
||||
prcopt_t opt_ = *opt;
|
||||
double *rs, *dts, *var, *azel_, *resp;
|
||||
int i, stat, vsat[MAXOBS] = {0}, svh[MAXOBS];
|
||||
|
||||
@@ -3461,7 +3461,7 @@ double geodist(const double *rs, const double *rr, double *e)
|
||||
/* satellite azimuth/elevation angle -------------------------------------------
|
||||
* compute satellite azimuth/elevation angle
|
||||
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
|
||||
* double *e I receiver-to-satellilte unit vevtor (ecef)
|
||||
* double *e I receiver-to-satellilte unit vector (ecef)
|
||||
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
|
||||
* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
|
||||
* return : elevation angle (rad)
|
||||
|
||||
@@ -38,5 +38,9 @@ file(GLOB OBS_GR_BLOCKS_HEADERS "*.h")
|
||||
list(SORT OBS_GR_BLOCKS_HEADERS)
|
||||
add_library(obs_gr_blocks ${OBS_GR_BLOCKS_SOURCES} ${OBS_GR_BLOCKS_HEADERS})
|
||||
source_group(Headers FILES ${OBS_GR_BLOCKS_HEADERS})
|
||||
add_dependencies(obs_gr_blocks glog-${glog_RELEASE} armadillo-${armadillo_RELEASE})
|
||||
if(MATIO_FOUND)
|
||||
add_dependencies(obs_gr_blocks glog-${glog_RELEASE} armadillo-${armadillo_RELEASE})
|
||||
else(MATIO_FOUND)
|
||||
add_dependencies(obs_gr_blocks glog-${glog_RELEASE} armadillo-${armadillo_RELEASE} matio-${GNSSSDR_MATIO_LOCAL_VERSION})
|
||||
endif(MATIO_FOUND)
|
||||
target_link_libraries(obs_gr_blocks ${GNURADIO_RUNTIME_LIBRARIES} ${ARMADILLO_LIBRARIES} ${MATIO_LIBRARIES})
|
||||
|
||||
@@ -358,11 +358,6 @@ int hybrid_observables_cc::general_work (int noutput_items __attribute__((unused
|
||||
{
|
||||
d_gnss_synchro_history_queue[i].push_back(in[i][j]);
|
||||
}
|
||||
//std::cout<<"push["<<i<<"] items "<<n_consume[i]
|
||||
/// <<" latest T_rx: "<<(double)in[i][ninput_items[i]-1].Tracking_sample_counter/(double)in[i][ninput_items[i]-1].fs
|
||||
// <<" [s] q size: "
|
||||
// <<d_gnss_synchro_history_queue[i].size()
|
||||
// <<std::endl;
|
||||
}
|
||||
|
||||
bool channel_history_ok;
|
||||
@@ -448,10 +443,6 @@ int hybrid_observables_cc::general_work (int noutput_items __attribute__((unused
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
//std::cout<<"ch["<<i<<"] delta_T_rx:"<<delta_T_rx_s*1000.0<<std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
+211
-246
@@ -39,8 +39,8 @@
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <glog/logging.h>
|
||||
#include <cmath>
|
||||
#include "control_message_factory.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "convolutional.h"
|
||||
|
||||
|
||||
@@ -114,7 +114,7 @@ void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols, int fra
|
||||
double page_symbols_deint[frame_length];
|
||||
// 1. De-interleave
|
||||
|
||||
galileo_e5a_telemetry_decoder_cc::deinterleaver(GALILEO_FNAV_INTERLEAVER_ROWS, GALILEO_FNAV_INTERLEAVER_COLS, page_symbols, page_symbols_deint);
|
||||
deinterleaver(GALILEO_FNAV_INTERLEAVER_ROWS, GALILEO_FNAV_INTERLEAVER_COLS, page_symbols, page_symbols_deint);
|
||||
|
||||
// 2. Viterbi decoder
|
||||
// 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder)
|
||||
@@ -127,7 +127,7 @@ void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols, int fra
|
||||
}
|
||||
}
|
||||
int page_bits[frame_length/2];
|
||||
galileo_e5a_telemetry_decoder_cc::viterbi_decoder(page_symbols_deint, page_bits);
|
||||
viterbi_decoder(page_symbols_deint, page_bits);
|
||||
|
||||
// 3. Call the Galileo page decoder
|
||||
std::string page_String;
|
||||
@@ -197,32 +197,37 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
|
||||
{
|
||||
if (GALILEO_FNAV_PREAMBLE.at(i) == '0')
|
||||
{
|
||||
d_preamble_bits[i] = 1;
|
||||
d_preambles_bits[i] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_preamble_bits[i] = -1;
|
||||
d_preambles_bits[i] = -1;
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
|
||||
{
|
||||
for (int k = 0; k < GALILEO_FNAV_CODES_PER_SYMBOL; k++)
|
||||
{
|
||||
d_preamble_samples[(i * GALILEO_FNAV_CODES_PER_SYMBOL) + k] = d_preambles_bits[i];
|
||||
}
|
||||
}
|
||||
|
||||
d_sample_counter = 0;
|
||||
d_state = 0;
|
||||
d_preamble_lock = false;
|
||||
d_stat = 0;
|
||||
corr_value = 0;
|
||||
d_flag_preamble = false;
|
||||
d_preamble_index = 0;
|
||||
d_flag_frame_sync = false;
|
||||
d_current_symbol = 0;
|
||||
d_prompt_counter = 0;
|
||||
d_symbol_counter = 0;
|
||||
|
||||
d_TOW_at_Preamble = 0;
|
||||
d_TOW_at_current_symbol = 0;
|
||||
|
||||
d_CRC_error_counter = 0;
|
||||
d_sign_init = 0;
|
||||
|
||||
d_flag_preamble = false;
|
||||
d_channel = 0;
|
||||
d_TOW_at_current_symbol = 0.0;
|
||||
flag_TOW_set = false;
|
||||
d_CRC_error_counter = 0;
|
||||
d_channel = 0;
|
||||
delta_t = 0.0;
|
||||
d_symbol_counter = 0;
|
||||
d_prompt_acum = 0.0;
|
||||
flag_bit_start = false;
|
||||
new_symbol = false;
|
||||
required_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE + GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
|
||||
}
|
||||
|
||||
|
||||
@@ -245,265 +250,222 @@ galileo_e5a_telemetry_decoder_cc::~galileo_e5a_telemetry_decoder_cc()
|
||||
int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||
{
|
||||
Gnss_Synchro *out = reinterpret_cast<Gnss_Synchro *>(output_items[0]); // Get the output buffer pointer
|
||||
const Gnss_Synchro *in = reinterpret_cast<const Gnss_Synchro *>(input_items[0]); // Get the input buffer pointer
|
||||
int preamble_diff = 0;
|
||||
|
||||
/* Terminology: Prompt: output from tracking Prompt correlator (Prompt samples)
|
||||
* Symbol: encoded navigation bits. 1 symbol = 20 samples in E5a
|
||||
* Bit: decoded navigation bits forming words as described in Galileo ICD
|
||||
* States: 0 Receiving dummy samples.
|
||||
* 1 Preamble not locked
|
||||
* 3 Preamble lock
|
||||
*/
|
||||
switch (d_state)
|
||||
Gnss_Synchro* out = reinterpret_cast<Gnss_Synchro*>(output_items[0]); // Get the output buffer pointer
|
||||
const Gnss_Synchro* in = reinterpret_cast<const Gnss_Synchro*>(input_items[0]); // Get the input buffer pointer
|
||||
|
||||
Gnss_Synchro current_sample; //structure to save the synchronization information and send the output object to the next block
|
||||
//1. Copy the current tracking output
|
||||
current_sample = in[0];
|
||||
d_symbol_counter++;
|
||||
if(flag_bit_start)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
if (in[0].Prompt_I != 0)
|
||||
{
|
||||
d_current_symbol += in[0].Prompt_I;
|
||||
if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1)
|
||||
{
|
||||
if (d_current_symbol > 0)
|
||||
{
|
||||
d_page_symbols[d_symbol_counter] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_page_symbols[d_symbol_counter] = -1;
|
||||
}
|
||||
d_current_symbol = 0;
|
||||
d_symbol_counter++;
|
||||
d_prompt_counter = 0;
|
||||
if (d_symbol_counter == GALILEO_FNAV_PREAMBLE_LENGTH_BITS - 1)
|
||||
{
|
||||
d_state = 1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
d_prompt_counter++;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
d_current_symbol += in[0].Prompt_I;
|
||||
if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1)
|
||||
{
|
||||
if (d_current_symbol > 0)
|
||||
{
|
||||
d_page_symbols[d_symbol_counter] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_page_symbols[d_symbol_counter] = -1;
|
||||
}
|
||||
// d_page_symbols[d_symbol_counter] = d_current_symbol_float/(float)GALILEO_FNAV_CODES_PER_SYMBOL;
|
||||
d_current_symbol = 0;
|
||||
d_symbol_counter++;
|
||||
d_prompt_counter = 0;
|
||||
// **** Attempt Preamble correlation ****
|
||||
bool corr_flag = true;
|
||||
int corr_sign = 0; // sequence can be found inverted
|
||||
// check if the preamble starts positive correlated or negative correlated
|
||||
if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS] < 0) // symbols clipping
|
||||
{
|
||||
corr_sign = -d_preamble_bits[0];
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_sign = d_preamble_bits[0];
|
||||
}
|
||||
// the preamble is fully correlated only if maintains corr_sign along the whole sequence
|
||||
for (int i = 1; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
|
||||
{
|
||||
if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS + i] < 0 && d_preamble_bits[i]+corr_sign != 0)
|
||||
{
|
||||
//exit for
|
||||
corr_flag = false;
|
||||
break;
|
||||
}
|
||||
if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS + i] > 0 && d_preamble_bits[i]+corr_sign == 0)
|
||||
{
|
||||
//exit for
|
||||
corr_flag = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
//
|
||||
if (corr_flag == true) // preamble fully correlates
|
||||
{
|
||||
d_preamble_index = d_sample_counter - GALILEO_FNAV_CODES_PER_PREAMBLE;//record the preamble sample stamp. Remember correlation appears at the end of the preamble in this design
|
||||
LOG(INFO) << "Preamble detection in E5a for Galileo satellite " << this->d_satellite << std::endl;
|
||||
d_symbol_counter = 0; // d_page_symbols start right after preamble and finish at the end of next preamble.
|
||||
d_state = 2; // preamble lock
|
||||
}
|
||||
if (d_symbol_counter >= GALILEO_FNAV_SYMBOLS_PER_PAGE + GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
|
||||
{
|
||||
d_symbol_counter = GALILEO_FNAV_PREAMBLE_LENGTH_BITS; // prevents overflow
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
d_prompt_counter++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
d_current_symbol += in[0].Prompt_I;
|
||||
if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1)
|
||||
{
|
||||
if (d_current_symbol > 0)
|
||||
{
|
||||
d_page_symbols[d_symbol_counter] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_page_symbols[d_symbol_counter] = -1;
|
||||
}
|
||||
// d_page_symbols[d_symbol_counter] = d_current_symbol_float/(float)GALILEO_FNAV_CODES_PER_SYMBOL;
|
||||
d_current_symbol = 0;
|
||||
d_symbol_counter++;
|
||||
d_prompt_counter = 0;
|
||||
// At the right sample stamp, check preamble synchro
|
||||
if (d_sample_counter == d_preamble_index + GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE)
|
||||
{
|
||||
// **** Attempt Preamble correlation ****
|
||||
bool corr_flag = true;
|
||||
int corr_sign = 0; // sequence can be found inverted
|
||||
// check if the preamble starts positive correlated or negative correlated
|
||||
if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS] < 0) // symbols clipping
|
||||
{
|
||||
corr_sign = -d_preamble_bits[0];
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_sign = d_preamble_bits[0];
|
||||
}
|
||||
// the preamble is fully correlated only if maintains corr_sign along the whole sequence
|
||||
for (int i = 1; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
|
||||
{
|
||||
if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS + i] < 0 && d_preamble_bits[i]+corr_sign != 0)
|
||||
{
|
||||
//exit for
|
||||
corr_flag = false;
|
||||
break;
|
||||
}
|
||||
if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS + i] > 0 && d_preamble_bits[i]+corr_sign == 0)
|
||||
{
|
||||
//exit for
|
||||
corr_flag = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
d_prompt_acum += current_sample.Prompt_I;
|
||||
if(d_symbol_counter == GALILEO_FNAV_CODES_PER_SYMBOL)
|
||||
{
|
||||
current_sample.Prompt_I = d_prompt_acum / static_cast<double>(GALILEO_FNAV_CODES_PER_SYMBOL);
|
||||
d_symbol_history.push_back(current_sample); //add new symbol to the symbol queue
|
||||
d_prompt_acum = 0.0;
|
||||
d_symbol_counter = 0;
|
||||
new_symbol = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if(current_sample.Prompt_I < 0.0)
|
||||
{
|
||||
d_preamble_init.push_back(1);
|
||||
}
|
||||
else
|
||||
{
|
||||
d_preamble_init.push_back(-1);
|
||||
}
|
||||
|
||||
if (corr_flag == true) // NEW PREAMBLE RECEIVED. DECODE PAGE
|
||||
{
|
||||
d_preamble_index = d_sample_counter - GALILEO_FNAV_CODES_PER_PREAMBLE;//record the preamble sample stamp
|
||||
// DECODE WORD
|
||||
decode_word(d_page_symbols, GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS);
|
||||
// CHECK CRC
|
||||
if (d_nav.flag_CRC_test == true)
|
||||
{
|
||||
d_CRC_error_counter = 0;
|
||||
d_flag_preamble = true; //valid preamble indicator (initialized to false every work())
|
||||
if (!d_flag_frame_sync)
|
||||
{
|
||||
d_flag_frame_sync = true;
|
||||
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
|
||||
<< in[0].Tracking_sample_counter << " [samples]";
|
||||
}
|
||||
d_symbol_counter = 0; // d_page_symbols start right after preamble and finish at the end of next preamble.
|
||||
}
|
||||
else
|
||||
{
|
||||
d_CRC_error_counter++;
|
||||
if (d_CRC_error_counter > GALILEO_E5a_CRC_ERROR_LIMIT)
|
||||
{
|
||||
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
|
||||
d_state = 1;
|
||||
d_symbol_counter = GALILEO_FNAV_PREAMBLE_LENGTH_BITS; // prevents overflow
|
||||
d_flag_frame_sync = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_symbol_counter = 0; // d_page_symbols start right after preamble and finish at the end of next preamble.
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
d_prompt_counter++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (d_preamble_init.size() == GALILEO_FNAV_CODES_PER_PREAMBLE)
|
||||
{
|
||||
std::deque<int>::iterator iter;
|
||||
int k = 0;
|
||||
corr_value = 0;
|
||||
for (iter = d_preamble_init.begin(); iter != d_preamble_init.end(); iter++)
|
||||
{
|
||||
corr_value += *iter * d_preamble_samples[k];
|
||||
k++;
|
||||
}
|
||||
if(abs(corr_value) == GALILEO_FNAV_CODES_PER_PREAMBLE)
|
||||
{
|
||||
d_symbol_counter = 0;
|
||||
flag_bit_start = true;
|
||||
corr_value = 0;
|
||||
while(d_preamble_init.size() > 0)
|
||||
{ //Clear preamble correlating queue
|
||||
d_preamble_init.pop_front();
|
||||
}
|
||||
while(d_symbol_history.size() > 0)
|
||||
{ //Clear symbol queue in order to prevent possible symbol discontinuities
|
||||
d_symbol_history.pop_front();
|
||||
}
|
||||
LOG(INFO) << "Bit start sync for Galileo E5a satellite " << d_satellite;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_preamble_init.pop_front();
|
||||
}
|
||||
}
|
||||
}
|
||||
d_sample_counter++; //count for the processed samples
|
||||
consume_each(1);
|
||||
|
||||
d_flag_preamble = false;
|
||||
|
||||
if ((d_symbol_history.size() > required_symbols) && new_symbol)
|
||||
{
|
||||
//******* preamble correlation ********
|
||||
corr_value = 0;
|
||||
for (int i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++)
|
||||
{
|
||||
if (d_symbol_history.at(i).Prompt_I < 0.0) // symbols clipping
|
||||
{
|
||||
corr_value -= d_preambles_bits[i];
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_value += d_preambles_bits[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//******* frame sync ******************
|
||||
if ((d_stat == 0) && new_symbol) //no preamble information
|
||||
{
|
||||
if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
|
||||
{
|
||||
d_preamble_index = d_sample_counter;//record the preamble sample stamp
|
||||
LOG(INFO) << "Preamble detection for Galileo E5a satellite " << d_satellite;
|
||||
d_stat = 1; // enter into frame pre-detection status
|
||||
}
|
||||
}
|
||||
else if ((d_stat == 1) && new_symbol) // posible preamble lock
|
||||
{
|
||||
if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS)
|
||||
{
|
||||
//check preamble separation
|
||||
preamble_diff = d_sample_counter - d_preamble_index;
|
||||
if (preamble_diff == GALILEO_FNAV_CODES_PER_PAGE)
|
||||
{
|
||||
//try to decode frame
|
||||
LOG(INFO) << "Starting page decoder for Galileo E5a satellite " << d_satellite;
|
||||
d_preamble_index = d_sample_counter; //record the preamble sample stamp
|
||||
d_stat = 2;
|
||||
}
|
||||
else if (preamble_diff > GALILEO_FNAV_CODES_PER_PAGE)
|
||||
{
|
||||
d_stat = 0; // start again
|
||||
flag_bit_start = false;
|
||||
LOG(INFO) << "Preamble diff = " << preamble_diff;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if ((d_stat == 2) && new_symbol)
|
||||
{
|
||||
if (d_sample_counter == (d_preamble_index + GALILEO_FNAV_CODES_PER_PAGE))
|
||||
{
|
||||
// NEW Galileo page part is received
|
||||
// 0. fetch the symbols into an array
|
||||
int frame_length = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
|
||||
double corr_sign = 0.0;
|
||||
if(corr_value > 0)
|
||||
{
|
||||
corr_sign = -1.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_sign = 1.0;
|
||||
}
|
||||
for (int i = 0; i < frame_length; i++)
|
||||
{
|
||||
page_symbols[i] = corr_sign * d_symbol_history.at(i + GALILEO_FNAV_PREAMBLE_LENGTH_BITS).Prompt_I; // because last symbol of the preamble is just received now!
|
||||
}
|
||||
|
||||
//call the decoder
|
||||
decode_word(page_symbols, frame_length);
|
||||
if (d_nav.flag_CRC_test == true)
|
||||
{
|
||||
d_CRC_error_counter = 0;
|
||||
d_flag_preamble = true; //valid preamble indicator (initialized to false every work())
|
||||
d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P)
|
||||
if (!d_flag_frame_sync)
|
||||
{
|
||||
d_flag_frame_sync = true;
|
||||
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
|
||||
<< d_symbol_history.at(0).Tracking_sample_counter << " [samples]";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
d_CRC_error_counter++;
|
||||
d_preamble_index = d_sample_counter; //record the preamble sample stamp
|
||||
if (d_CRC_error_counter > GALILEO_E5A_CRC_ERROR_LIMIT)
|
||||
{
|
||||
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
|
||||
d_flag_frame_sync = false;
|
||||
d_stat = 0;
|
||||
flag_bit_start = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
new_symbol = false;
|
||||
|
||||
// UPDATE GNSS SYNCHRO DATA
|
||||
Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block
|
||||
//1. Copy the current tracking output
|
||||
current_synchro_data = in[0];
|
||||
//2. Add the telemetry decoder information
|
||||
if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true)
|
||||
//Add the telemetry decoder information
|
||||
if (d_flag_preamble and d_nav.flag_TOW_set)
|
||||
//update TOW at the preamble instant
|
||||
//We expect a preamble each 10 seconds (FNAV page period)
|
||||
{
|
||||
if (d_nav.flag_TOW_1 == true)
|
||||
{
|
||||
d_TOW_at_Preamble = d_nav.FNAV_TOW_1;
|
||||
d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
|
||||
d_TOW_at_current_symbol = d_nav.FNAV_TOW_1 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
|
||||
d_nav.flag_TOW_1 = false;
|
||||
}
|
||||
if (d_nav.flag_TOW_2 == true)
|
||||
else if (d_nav.flag_TOW_2 == true)
|
||||
{
|
||||
d_TOW_at_Preamble = d_nav.FNAV_TOW_2;
|
||||
d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
|
||||
d_TOW_at_current_symbol = d_nav.FNAV_TOW_2 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
|
||||
d_nav.flag_TOW_2 = false;
|
||||
}
|
||||
if (d_nav.flag_TOW_3 == true)
|
||||
else if (d_nav.flag_TOW_3 == true)
|
||||
{
|
||||
d_TOW_at_Preamble = d_nav.FNAV_TOW_3;
|
||||
d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
|
||||
d_TOW_at_current_symbol = d_nav.FNAV_TOW_3 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
|
||||
d_nav.flag_TOW_3 = false;
|
||||
}
|
||||
if (d_nav.flag_TOW_4 == true)
|
||||
else if (d_nav.flag_TOW_4 == true)
|
||||
{
|
||||
d_TOW_at_Preamble = d_nav.FNAV_TOW_4;
|
||||
d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
|
||||
d_TOW_at_current_symbol = d_nav.FNAV_TOW_4 + (static_cast<double>(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD);
|
||||
d_nav.flag_TOW_4 = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
//this page has no timming information
|
||||
d_TOW_at_Preamble = d_TOW_at_Preamble + GALILEO_FNAV_SECONDS_PER_PAGE;
|
||||
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E5a_CODE_PERIOD;
|
||||
d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD;
|
||||
}
|
||||
|
||||
}
|
||||
else //if there is not a new preamble, we define the TOW of the current symbol
|
||||
{
|
||||
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E5a_CODE_PERIOD;
|
||||
d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD;
|
||||
}
|
||||
|
||||
//if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true)
|
||||
if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true)
|
||||
if (d_flag_frame_sync and d_nav.flag_TOW_set)
|
||||
{
|
||||
current_synchro_data.Flag_valid_word = true;
|
||||
current_sample.Flag_valid_word = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Flag_valid_word = false;
|
||||
current_sample.Flag_valid_word = false;
|
||||
}
|
||||
|
||||
current_synchro_data.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol*1000.0)/1000.0;
|
||||
current_sample.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol*1000.0)/1000.0;
|
||||
|
||||
if(d_dump == true)
|
||||
if(d_dump)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
@@ -512,20 +474,23 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut
|
||||
unsigned long int tmp_ulong_int;
|
||||
tmp_double = d_TOW_at_current_symbol;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
tmp_ulong_int = current_synchro_data.Tracking_sample_counter;
|
||||
tmp_ulong_int = current_sample.Tracking_sample_counter;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_ulong_int), sizeof(unsigned long int));
|
||||
tmp_double = d_TOW_at_Preamble;
|
||||
tmp_double = 0.0;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure & e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing observables dump file " << e.what();
|
||||
LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what();
|
||||
}
|
||||
}
|
||||
d_sample_counter++; //count for the processed samples
|
||||
//3. Make the output (copy the object contents to the GNURadio reserved memory)
|
||||
out[0] = current_synchro_data;
|
||||
consume_each(1);
|
||||
// remove used symbols from history
|
||||
while (d_symbol_history.size() > required_symbols)
|
||||
{
|
||||
d_symbol_history.pop_front();
|
||||
}
|
||||
//3. Make the output
|
||||
out[0] = current_sample;
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
+21
-26
@@ -39,6 +39,7 @@
|
||||
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <deque>
|
||||
#include <gnuradio/block.h>
|
||||
#include "Galileo_E5a.h"
|
||||
#include "concurrent_queue.h"
|
||||
@@ -48,8 +49,7 @@
|
||||
#include "galileo_almanac.h"
|
||||
#include "galileo_iono.h"
|
||||
#include "galileo_utc_model.h"
|
||||
|
||||
//#include "convolutional.h"
|
||||
#include "gnss_synchro.h"
|
||||
|
||||
class galileo_e5a_telemetry_decoder_cc;
|
||||
|
||||
@@ -85,38 +85,33 @@ private:
|
||||
|
||||
void decode_word(double *page_symbols, int frame_length);
|
||||
|
||||
int d_preamble_bits[GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
|
||||
double d_page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE + GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
|
||||
|
||||
double d_current_symbol;
|
||||
long unsigned int d_symbol_counter;
|
||||
int d_prompt_counter;
|
||||
int d_sign_init;
|
||||
|
||||
int d_preambles_bits[GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
|
||||
int d_preamble_samples[GALILEO_FNAV_CODES_PER_PREAMBLE];
|
||||
std::deque<int> d_preamble_init;
|
||||
int d_stat;
|
||||
int d_CRC_error_counter;
|
||||
int d_channel;
|
||||
int d_symbol_counter;
|
||||
int corr_value;
|
||||
unsigned int required_symbols;
|
||||
long unsigned int d_sample_counter;
|
||||
long unsigned int d_preamble_index;
|
||||
|
||||
bool d_preamble_lock;
|
||||
bool d_flag_frame_sync;
|
||||
int d_state;
|
||||
|
||||
bool d_flag_preamble;
|
||||
int d_CRC_error_counter;
|
||||
|
||||
// navigation message vars
|
||||
Galileo_Fnav_Message d_nav;
|
||||
|
||||
bool d_dump;
|
||||
Gnss_Satellite d_satellite;
|
||||
int d_channel;
|
||||
|
||||
double d_TOW_at_Preamble;
|
||||
double d_TOW_at_current_symbol;
|
||||
|
||||
bool flag_TOW_set;
|
||||
|
||||
bool flag_bit_start;
|
||||
bool new_symbol;
|
||||
double d_prompt_acum;
|
||||
double page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
|
||||
double d_TOW_at_current_symbol;
|
||||
double delta_t; //GPS-GALILEO time offset
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
std::deque<Gnss_Synchro> d_symbol_history;
|
||||
Gnss_Satellite d_satellite;
|
||||
// navigation message vars
|
||||
Galileo_Fnav_Message d_nav;
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_ */
|
||||
|
||||
@@ -35,6 +35,7 @@ set(TRACKING_ADAPTER_SOURCES
|
||||
gps_l2_m_dll_pll_tracking.cc
|
||||
glonass_l1_ca_dll_pll_tracking.cc
|
||||
glonass_l1_ca_dll_pll_c_aid_tracking.cc
|
||||
gps_l5i_dll_pll_tracking.cc
|
||||
${OPT_TRACKING_ADAPTERS}
|
||||
)
|
||||
|
||||
|
||||
@@ -0,0 +1,145 @@
|
||||
/*!
|
||||
* \file gps_l5idll_pll_tracking.cc
|
||||
* \brief Interface of an adapter of a DLL+PLL tracking loop block
|
||||
* for GPS L5i to a TrackingInterface
|
||||
* \author Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
*
|
||||
* Code DLL + carrier PLL according to the algorithms described in:
|
||||
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
|
||||
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency
|
||||
* Approach, Birkhauser, 2007
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
|
||||
#include "gps_l5i_dll_pll_tracking.h"
|
||||
#include <glog/logging.h>
|
||||
#include "GPS_L5.h"
|
||||
#include "configuration_interface.h"
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
GpsL5iDllPllTracking::GpsL5iDllPllTracking(
|
||||
ConfigurationInterface* configuration, std::string role,
|
||||
unsigned int in_streams, unsigned int out_streams) :
|
||||
role_(role), in_streams_(in_streams), out_streams_(out_streams)
|
||||
{
|
||||
DLOG(INFO) << "role " << role;
|
||||
//################# CONFIGURATION PARAMETERS ########################
|
||||
int fs_in;
|
||||
int vector_length;
|
||||
int f_if;
|
||||
bool dump;
|
||||
std::string dump_filename;
|
||||
std::string item_type;
|
||||
std::string default_item_type = "gr_complex";
|
||||
float pll_bw_hz;
|
||||
float dll_bw_hz;
|
||||
float early_late_space_chips;
|
||||
item_type = configuration->property(role + ".item_type", default_item_type);
|
||||
int fs_in_deprecated = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
|
||||
fs_in = configuration->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
|
||||
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 = "./track_ch";
|
||||
dump_filename = configuration->property(role + ".dump_filename",
|
||||
default_dump_filename); //unused!
|
||||
vector_length = std::round(static_cast<double>(fs_in) / (static_cast<double>(GPS_L5i_CODE_RATE_HZ) / static_cast<double>(GPS_L5i_CODE_LENGTH_CHIPS)));
|
||||
|
||||
//################# MAKE TRACKING GNURadio object ###################
|
||||
if (item_type.compare("gr_complex") == 0)
|
||||
{
|
||||
item_size_ = sizeof(gr_complex);
|
||||
tracking_ = gps_l5i_dll_pll_make_tracking_cc(
|
||||
f_if,
|
||||
fs_in,
|
||||
vector_length,
|
||||
dump,
|
||||
dump_filename,
|
||||
pll_bw_hz,
|
||||
dll_bw_hz,
|
||||
early_late_space_chips);
|
||||
}
|
||||
else
|
||||
{
|
||||
item_size_ = sizeof(gr_complex);
|
||||
LOG(WARNING) << item_type << " unknown tracking item type.";
|
||||
}
|
||||
channel_ = 0;
|
||||
DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
|
||||
}
|
||||
|
||||
|
||||
GpsL5iDllPllTracking::~GpsL5iDllPllTracking()
|
||||
{}
|
||||
|
||||
|
||||
void GpsL5iDllPllTracking::start_tracking()
|
||||
{
|
||||
tracking_->start_tracking();
|
||||
}
|
||||
|
||||
/*
|
||||
* Set tracking channel unique ID
|
||||
*/
|
||||
void GpsL5iDllPllTracking::set_channel(unsigned int channel)
|
||||
{
|
||||
channel_ = channel;
|
||||
tracking_->set_channel(channel);
|
||||
}
|
||||
|
||||
|
||||
void GpsL5iDllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
|
||||
{
|
||||
tracking_->set_gnss_synchro(p_gnss_synchro);
|
||||
}
|
||||
|
||||
void GpsL5iDllPllTracking::connect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if(top_block) { /* top_block is not null */};
|
||||
//nothing to connect, now the tracking uses gr_sync_decimator
|
||||
}
|
||||
|
||||
void GpsL5iDllPllTracking::disconnect(gr::top_block_sptr top_block)
|
||||
{
|
||||
if(top_block) { /* top_block is not null */};
|
||||
//nothing to disconnect, now the tracking uses gr_sync_decimator
|
||||
}
|
||||
|
||||
gr::basic_block_sptr GpsL5iDllPllTracking::get_left_block()
|
||||
{
|
||||
return tracking_;
|
||||
}
|
||||
|
||||
gr::basic_block_sptr GpsL5iDllPllTracking::get_right_block()
|
||||
{
|
||||
return tracking_;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,103 @@
|
||||
/*!
|
||||
* \file gps_l5idll_pll_tracking.h
|
||||
* \brief Interface of an adapter of a DLL+PLL tracking loop block
|
||||
* for GPS L5i to a TrackingInterface
|
||||
* \author Javier Arribas, 2017. jarribas(at)cttc.es
|
||||
*
|
||||
* Code DLL + carrier PLL according to the algorithms described in:
|
||||
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
|
||||
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency
|
||||
* Approach, Birkhauser, 2007
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_gps_l5i_dll_pll_tracking_H_
|
||||
#define GNSS_SDR_gps_l5i_dll_pll_tracking_H_
|
||||
|
||||
#include <string>
|
||||
#include "tracking_interface.h"
|
||||
#include "gps_l5i_dll_pll_tracking_cc.h"
|
||||
|
||||
|
||||
class ConfigurationInterface;
|
||||
|
||||
/*!
|
||||
* \brief This class implements a code DLL + carrier PLL tracking loop
|
||||
*/
|
||||
class GpsL5iDllPllTracking : public TrackingInterface
|
||||
{
|
||||
public:
|
||||
GpsL5iDllPllTracking(ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams);
|
||||
|
||||
virtual ~GpsL5iDllPllTracking();
|
||||
|
||||
inline std::string role() override
|
||||
{
|
||||
return role_;
|
||||
}
|
||||
|
||||
//! Returns "GPS_L2_M_DLL_PLL_Tracking"
|
||||
inline std::string implementation() override
|
||||
{
|
||||
return "GPS_L5i_DLL_PLL_Tracking";
|
||||
}
|
||||
|
||||
inline size_t item_size() override
|
||||
{
|
||||
return item_size_;
|
||||
}
|
||||
|
||||
void connect(gr::top_block_sptr top_block) override;
|
||||
void disconnect(gr::top_block_sptr top_block) override;
|
||||
gr::basic_block_sptr get_left_block() override;
|
||||
gr::basic_block_sptr get_right_block() override;
|
||||
|
||||
/*!
|
||||
* \brief Set tracking channel unique ID
|
||||
*/
|
||||
void set_channel(unsigned int channel) override;
|
||||
|
||||
/*!
|
||||
* \brief Set acquisition/tracking common Gnss_Synchro object pointer
|
||||
* to efficiently exchange synchronization data between acquisition and tracking blocks
|
||||
*/
|
||||
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro) override;
|
||||
|
||||
void start_tracking() override;
|
||||
|
||||
private:
|
||||
gps_l5i_dll_pll_tracking_cc_sptr tracking_;
|
||||
size_t item_size_;
|
||||
unsigned int channel_;
|
||||
std::string role_;
|
||||
unsigned int in_streams_;
|
||||
unsigned int out_streams_;
|
||||
};
|
||||
|
||||
#endif // GNSS_SDR_gps_l5i_dll_pll_tracking_H_
|
||||
@@ -33,6 +33,7 @@ set(TRACKING_GR_BLOCKS_SOURCES
|
||||
gps_l1_ca_tcp_connector_tracking_cc.cc
|
||||
galileo_e5a_dll_pll_tracking_cc.cc
|
||||
gps_l2_m_dll_pll_tracking_cc.cc
|
||||
gps_l5i_dll_pll_tracking_cc.cc
|
||||
gps_l1_ca_dll_pll_c_aid_tracking_cc.cc
|
||||
gps_l1_ca_dll_pll_c_aid_tracking_sc.cc
|
||||
glonass_l1_ca_dll_pll_tracking_cc.cc
|
||||
@@ -69,5 +70,13 @@ source_group(Headers FILES ${TRACKING_GR_BLOCKS_HEADERS})
|
||||
target_link_libraries(tracking_gr_blocks tracking_lib ${GNURADIO_RUNTIME_LIBRARIES} gnss_sp_libs ${Boost_LIBRARIES} ${VOLK_GNSSSDR_LIBRARIES} ${MATIO_LIBRARIES} ${OPT_TRACKING_LIBRARIES})
|
||||
|
||||
if(NOT VOLK_GNSSSDR_FOUND)
|
||||
add_dependencies(tracking_gr_blocks volk_gnsssdr_module)
|
||||
if(MATIO_FOUND)
|
||||
add_dependencies(tracking_gr_blocks volk_gnsssdr_module)
|
||||
else(MATIO_FOUND)
|
||||
add_dependencies(tracking_gr_blocks volk_gnsssdr_module matio-${GNSSSDR_MATIO_LOCAL_VERSION})
|
||||
endif(MATIO_FOUND)
|
||||
else(NOT VOLK_GNSSSDR_FOUND)
|
||||
if(NOT MATIO_FOUND)
|
||||
add_dependencies(tracking_gr_blocks matio-${GNSSSDR_MATIO_LOCAL_VERSION})
|
||||
endif(NOT MATIO_FOUND)
|
||||
endif(NOT VOLK_GNSSSDR_FOUND)
|
||||
|
||||
@@ -0,0 +1,765 @@
|
||||
/*!
|
||||
* \file gps_l5i_dll_pll_tracking_cc.cc
|
||||
* \brief Implementation of a code DLL + carrier PLL tracking block for GPS L2C
|
||||
* \author Carlos Aviles, 2010. carlos.avilesr(at)googlemail.com
|
||||
* Javier Arribas, 2011. jarribas(at)cttc.es
|
||||
*
|
||||
* Code DLL + carrier PLL according to the algorithms described in:
|
||||
* [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
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "gps_l5i_dll_pll_tracking_cc.h"
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <sstream>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <glog/logging.h>
|
||||
#include <matio.h>
|
||||
#include <volk_gnsssdr/volk_gnsssdr.h>
|
||||
#include "gps_l5_signal.h"
|
||||
#include "tracking_discriminators.h"
|
||||
#include "lock_detectors.h"
|
||||
#include "GPS_L5.h"
|
||||
#include "control_message_factory.h"
|
||||
|
||||
|
||||
/*!
|
||||
* \todo Include in definition header file
|
||||
*/
|
||||
#define GPS_L2M_CN0_ESTIMATION_SAMPLES 10
|
||||
#define GPS_L2M_MINIMUM_VALID_CN0 25
|
||||
#define GPS_L2M_MAXIMUM_LOCK_FAIL_COUNTER 50
|
||||
#define GPS_L2M_CARRIER_LOCK_THRESHOLD 0.75
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
gps_l5i_dll_pll_tracking_cc_sptr
|
||||
gps_l5i_dll_pll_make_tracking_cc(
|
||||
long if_freq,
|
||||
long fs_in,
|
||||
unsigned int vector_length,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips)
|
||||
{
|
||||
return gps_l5i_dll_pll_tracking_cc_sptr(new gps_l5i_dll_pll_tracking_cc(if_freq,
|
||||
fs_in, vector_length, dump, dump_filename, pll_bw_hz, dll_bw_hz, early_late_space_chips));
|
||||
}
|
||||
|
||||
|
||||
void gps_l5i_dll_pll_tracking_cc::forecast (int noutput_items,
|
||||
gr_vector_int &ninput_items_required)
|
||||
{
|
||||
if (noutput_items != 0)
|
||||
{
|
||||
ninput_items_required[0] = static_cast<int>(d_vector_length) * 2; //set the required available samples in each call
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
gps_l5i_dll_pll_tracking_cc::gps_l5i_dll_pll_tracking_cc(
|
||||
long if_freq,
|
||||
long fs_in,
|
||||
unsigned int vector_length,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips) :
|
||||
gr::block("gps_l5i_dll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
// Telemetry bit synchronization message port input
|
||||
this->message_port_register_in(pmt::mp("preamble_timestamp_s"));
|
||||
this->message_port_register_out(pmt::mp("events"));
|
||||
// initialize internal vars
|
||||
d_dump = dump;
|
||||
d_if_freq = if_freq;
|
||||
d_fs_in = fs_in;
|
||||
d_vector_length = vector_length;
|
||||
d_dump_filename = dump_filename;
|
||||
|
||||
d_current_prn_length_samples = static_cast<int>(d_vector_length);
|
||||
|
||||
// DLL/PLL filter initialization
|
||||
d_carrier_loop_filter=Tracking_2nd_PLL_filter(GPS_L5i_PERIOD);
|
||||
d_code_loop_filter=Tracking_2nd_DLL_filter(GPS_L5i_PERIOD);
|
||||
|
||||
// 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_chips = early_late_space_chips; // Define early-late offset (in chips)
|
||||
|
||||
// Initialization of local code replica
|
||||
// Get space for a vector with the C/A code replica sampled 1x/chip
|
||||
d_ca_code = static_cast<gr_complex*>(volk_gnsssdr_malloc(static_cast<int>(GPS_L5i_CODE_LENGTH_CHIPS) * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
||||
|
||||
// correlator outputs (scalar)
|
||||
d_n_correlator_taps = 3; // Early, Prompt, and Late
|
||||
d_correlator_outs = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(gr_complex), volk_gnsssdr_get_alignment()));
|
||||
for (int n = 0; n < d_n_correlator_taps; n++)
|
||||
{
|
||||
d_correlator_outs[n] = gr_complex(0,0);
|
||||
}
|
||||
d_local_code_shift_chips = static_cast<float*>(volk_gnsssdr_malloc(d_n_correlator_taps*sizeof(float), volk_gnsssdr_get_alignment()));
|
||||
// Set TAPs delay values [chips]
|
||||
d_local_code_shift_chips[0] = - d_early_late_spc_chips;
|
||||
d_local_code_shift_chips[1] = 0.0;
|
||||
d_local_code_shift_chips[2] = d_early_late_spc_chips;
|
||||
|
||||
multicorrelator_cpu.init(2 * d_current_prn_length_samples, d_n_correlator_taps);
|
||||
|
||||
//--- Perform initializations ------------------------------
|
||||
// define initial code frequency basis of NCO
|
||||
d_code_freq_chips = GPS_L5i_CODE_RATE_HZ;
|
||||
// define residual code phase (in chips)
|
||||
d_rem_code_phase_samples = 0.0;
|
||||
// define residual carrier phase
|
||||
d_rem_carr_phase_rad = 0.0;
|
||||
|
||||
// sample synchronization
|
||||
d_sample_counter = 0;
|
||||
//d_sample_counter_seconds = 0;
|
||||
d_acq_sample_stamp = 0;
|
||||
|
||||
d_enable_tracking = false;
|
||||
d_pull_in = false;
|
||||
|
||||
// CN0 estimation and lock detector buffers
|
||||
d_cn0_estimation_counter = 0;
|
||||
d_Prompt_buffer = new gr_complex[GPS_L2M_CN0_ESTIMATION_SAMPLES];
|
||||
d_carrier_lock_test = 1;
|
||||
d_CN0_SNV_dB_Hz = 0;
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_carrier_lock_threshold = GPS_L2M_CARRIER_LOCK_THRESHOLD;
|
||||
|
||||
systemName["G"] = std::string("GPS");
|
||||
|
||||
//set_min_output_buffer((long int)300);
|
||||
|
||||
d_acquisition_gnss_synchro = 0;
|
||||
d_channel = 0;
|
||||
d_acq_code_phase_samples = 0.0;
|
||||
d_acq_carrier_doppler_hz = 0.0;
|
||||
d_carrier_doppler_hz = 0.0;
|
||||
d_acc_carrier_phase_rad = 0.0;
|
||||
d_code_phase_samples = 0.0;
|
||||
|
||||
d_rem_code_phase_chips = 0.0;
|
||||
d_code_phase_step_chips = 0.0;
|
||||
d_carrier_phase_step_rad = 0.0;
|
||||
|
||||
set_relative_rate(1.0 / static_cast<double>(d_vector_length));
|
||||
}
|
||||
|
||||
|
||||
void gps_l5i_dll_pll_tracking_cc::start_tracking()
|
||||
{
|
||||
/*
|
||||
* correct the code phase according to the delay between acq and trk
|
||||
*/
|
||||
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
|
||||
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
|
||||
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
|
||||
|
||||
long int acq_trk_diff_samples;
|
||||
double acq_trk_diff_seconds;
|
||||
acq_trk_diff_samples = static_cast<long int>(d_sample_counter) - static_cast<long int>(d_acq_sample_stamp); //-d_vector_length;
|
||||
DLOG(INFO) << "Number of samples between Acquisition and Tracking =" << acq_trk_diff_samples;
|
||||
acq_trk_diff_seconds = static_cast<float>(acq_trk_diff_samples) / static_cast<float>(d_fs_in);
|
||||
// Doppler effect
|
||||
// Fd=(C/(C+Vr))*F
|
||||
double radial_velocity = (GPS_L5_FREQ_HZ + d_acq_carrier_doppler_hz) / GPS_L5_FREQ_HZ;
|
||||
// new chip and prn sequence periods based on acq Doppler
|
||||
double T_chip_mod_seconds;
|
||||
double T_prn_mod_seconds;
|
||||
double T_prn_mod_samples;
|
||||
d_code_freq_chips = radial_velocity * GPS_L5i_CODE_RATE_HZ;
|
||||
d_code_phase_step_chips = static_cast<double>(d_code_freq_chips) / static_cast<double>(d_fs_in);
|
||||
T_chip_mod_seconds = 1/d_code_freq_chips;
|
||||
T_prn_mod_seconds = T_chip_mod_seconds * GPS_L5i_CODE_LENGTH_CHIPS;
|
||||
T_prn_mod_samples = T_prn_mod_seconds * static_cast<double>(d_fs_in);
|
||||
|
||||
d_current_prn_length_samples = round(T_prn_mod_samples);
|
||||
|
||||
double T_prn_true_seconds = GPS_L5i_CODE_LENGTH_CHIPS / GPS_L5i_CODE_RATE_HZ;
|
||||
double T_prn_true_samples = T_prn_true_seconds * static_cast<double>(d_fs_in);
|
||||
double T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
|
||||
double N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
|
||||
double corrected_acq_phase_samples, delay_correction_samples;
|
||||
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * static_cast<double>(d_fs_in)), T_prn_true_samples);
|
||||
if (corrected_acq_phase_samples < 0)
|
||||
{
|
||||
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
|
||||
}
|
||||
delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
|
||||
|
||||
d_acq_code_phase_samples = corrected_acq_phase_samples;
|
||||
|
||||
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
|
||||
d_carrier_phase_step_rad = GPS_L5_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
|
||||
|
||||
// DLL/PLL filter initialization
|
||||
d_carrier_loop_filter.initialize(); // initialize the carrier filter
|
||||
d_code_loop_filter.initialize(); // initialize the code filter
|
||||
|
||||
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
|
||||
gps_l5i_code_gen_complex(d_ca_code, d_acquisition_gnss_synchro->PRN);
|
||||
|
||||
multicorrelator_cpu.set_local_code_and_taps(static_cast<int>(GPS_L5i_CODE_LENGTH_CHIPS), d_ca_code, d_local_code_shift_chips);
|
||||
for (int n = 0; n < d_n_correlator_taps; n++)
|
||||
{
|
||||
d_correlator_outs[n] = gr_complex(0,0);
|
||||
}
|
||||
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_rem_code_phase_samples = 0;
|
||||
d_rem_carr_phase_rad = 0.0;
|
||||
d_rem_code_phase_chips = 0.0;
|
||||
d_acc_carrier_phase_rad = 0.0;
|
||||
|
||||
d_code_phase_samples = d_acq_code_phase_samples;
|
||||
|
||||
std::string sys_ = &d_acquisition_gnss_synchro->System;
|
||||
sys = sys_.substr(0,1);
|
||||
|
||||
// DEBUG OUTPUT
|
||||
std::cout << "Tracking of GPS L2CM signal started on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
LOG(INFO) << "Starting GPS L2CM tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
|
||||
|
||||
// enable tracking
|
||||
d_pull_in = true;
|
||||
d_enable_tracking = true;
|
||||
|
||||
LOG(INFO) << "GPS L2CM PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz
|
||||
<< " Code Phase correction [samples]=" << delay_correction_samples
|
||||
<< " PULL-IN Code Phase [samples]=" << d_acq_code_phase_samples;
|
||||
}
|
||||
|
||||
|
||||
int gps_l5i_dll_pll_tracking_cc::save_matfile()
|
||||
{
|
||||
// READ DUMP FILE
|
||||
std::ifstream::pos_type size;
|
||||
int number_of_double_vars = 11;
|
||||
int number_of_float_vars = 5;
|
||||
int epoch_size_bytes = sizeof(unsigned long int) + sizeof(double) * number_of_double_vars +
|
||||
sizeof(float) * number_of_float_vars + sizeof(unsigned int);
|
||||
std::ifstream dump_file;
|
||||
dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit);
|
||||
try
|
||||
{
|
||||
dump_file.open(d_dump_filename.c_str(), std::ios::binary | std::ios::ate);
|
||||
}
|
||||
catch(const std::ifstream::failure &e)
|
||||
{
|
||||
std::cerr << "Problem opening dump file:" << e.what() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
// count number of epochs and rewind
|
||||
long int num_epoch = 0;
|
||||
if (dump_file.is_open())
|
||||
{
|
||||
size = dump_file.tellg();
|
||||
num_epoch = static_cast<long int>(size) / static_cast<long int>(epoch_size_bytes);
|
||||
dump_file.seekg(0, std::ios::beg);
|
||||
}
|
||||
else
|
||||
{
|
||||
return 1;
|
||||
}
|
||||
float * abs_E = new float [num_epoch];
|
||||
float * abs_P = new float [num_epoch];
|
||||
float * abs_L = new float [num_epoch];
|
||||
float * Prompt_I = new float [num_epoch];
|
||||
float * Prompt_Q = new float [num_epoch];
|
||||
unsigned long int * PRN_start_sample_count = new unsigned long int [num_epoch];
|
||||
double * acc_carrier_phase_rad = new double [num_epoch];
|
||||
double * carrier_doppler_hz = new double [num_epoch];
|
||||
double * code_freq_chips = new double [num_epoch];
|
||||
double * carr_error_hz = new double [num_epoch];
|
||||
double * carr_error_filt_hz = new double [num_epoch];
|
||||
double * code_error_chips = new double [num_epoch];
|
||||
double * code_error_filt_chips = new double [num_epoch];
|
||||
double * CN0_SNV_dB_Hz = new double [num_epoch];
|
||||
double * carrier_lock_test = new double [num_epoch];
|
||||
double * aux1 = new double [num_epoch];
|
||||
double * aux2 = new double [num_epoch];
|
||||
unsigned int * PRN = new unsigned int [num_epoch];
|
||||
|
||||
try
|
||||
{
|
||||
if (dump_file.is_open())
|
||||
{
|
||||
for(long int i = 0; i < num_epoch; i++)
|
||||
{
|
||||
dump_file.read(reinterpret_cast<char *>(&abs_E[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&abs_P[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&abs_L[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&Prompt_I[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&Prompt_Q[i]), sizeof(float));
|
||||
dump_file.read(reinterpret_cast<char *>(&PRN_start_sample_count[i]), sizeof(unsigned long int));
|
||||
dump_file.read(reinterpret_cast<char *>(&acc_carrier_phase_rad[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&carrier_doppler_hz[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_freq_chips[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&carr_error_hz[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&carr_error_filt_hz[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_error_chips[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&code_error_filt_chips[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&CN0_SNV_dB_Hz[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&carrier_lock_test[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&aux1[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&aux2[i]), sizeof(double));
|
||||
dump_file.read(reinterpret_cast<char *>(&PRN[i]), sizeof(unsigned int));
|
||||
}
|
||||
}
|
||||
dump_file.close();
|
||||
}
|
||||
catch (const std::ifstream::failure &e)
|
||||
{
|
||||
std::cerr << "Problem reading dump file:" << e.what() << std::endl;
|
||||
delete[] abs_E;
|
||||
delete[] abs_P;
|
||||
delete[] abs_L;
|
||||
delete[] Prompt_I;
|
||||
delete[] Prompt_Q;
|
||||
delete[] PRN_start_sample_count;
|
||||
delete[] acc_carrier_phase_rad;
|
||||
delete[] carrier_doppler_hz;
|
||||
delete[] code_freq_chips;
|
||||
delete[] carr_error_hz;
|
||||
delete[] carr_error_filt_hz;
|
||||
delete[] code_error_chips;
|
||||
delete[] code_error_filt_chips;
|
||||
delete[] CN0_SNV_dB_Hz;
|
||||
delete[] carrier_lock_test;
|
||||
delete[] aux1;
|
||||
delete[] aux2;
|
||||
delete[] PRN;
|
||||
return 1;
|
||||
}
|
||||
|
||||
// WRITE MAT FILE
|
||||
mat_t *matfp;
|
||||
matvar_t *matvar;
|
||||
std::string filename = d_dump_filename;
|
||||
filename.erase(filename.length() - 4, 4);
|
||||
filename.append(".mat");
|
||||
matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
|
||||
if(reinterpret_cast<long*>(matfp) != NULL)
|
||||
{
|
||||
size_t dims[2] = {1, static_cast<size_t>(num_epoch)};
|
||||
matvar = Mat_VarCreate("abs_E", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_E, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("abs_P", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_P, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("abs_L", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, abs_L, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("Prompt_I", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_I, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("Prompt_Q", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, Prompt_Q, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("PRN_start_sample_count", MAT_C_UINT64, MAT_T_UINT64, 2, dims, PRN_start_sample_count, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("acc_carrier_phase_rad", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, acc_carrier_phase_rad, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("carrier_doppler_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_doppler_hz, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("code_freq_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_freq_chips, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("carr_error_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_hz, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("carr_error_filt_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carr_error_filt_hz, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("code_error_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_chips, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("code_error_filt_chips", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, code_error_filt_chips, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("CN0_SNV_dB_Hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, CN0_SNV_dB_Hz, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("carrier_lock_test", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, carrier_lock_test, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("aux1", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux1, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("aux2", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims, aux2, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
|
||||
matvar = Mat_VarCreate("PRN", MAT_C_UINT32, MAT_T_UINT32, 2, dims, PRN, 0);
|
||||
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
|
||||
Mat_VarFree(matvar);
|
||||
}
|
||||
Mat_Close(matfp);
|
||||
delete[] abs_E;
|
||||
delete[] abs_P;
|
||||
delete[] abs_L;
|
||||
delete[] Prompt_I;
|
||||
delete[] Prompt_Q;
|
||||
delete[] PRN_start_sample_count;
|
||||
delete[] acc_carrier_phase_rad;
|
||||
delete[] carrier_doppler_hz;
|
||||
delete[] code_freq_chips;
|
||||
delete[] carr_error_hz;
|
||||
delete[] carr_error_filt_hz;
|
||||
delete[] code_error_chips;
|
||||
delete[] code_error_filt_chips;
|
||||
delete[] CN0_SNV_dB_Hz;
|
||||
delete[] carrier_lock_test;
|
||||
delete[] aux1;
|
||||
delete[] aux2;
|
||||
delete[] PRN;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
gps_l5i_dll_pll_tracking_cc::~gps_l5i_dll_pll_tracking_cc()
|
||||
{
|
||||
if (d_dump_file.is_open())
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
catch(const std::exception & ex)
|
||||
{
|
||||
LOG(WARNING) << "Exception in destructor " << ex.what();
|
||||
}
|
||||
}
|
||||
if(d_dump)
|
||||
{
|
||||
if(d_channel == 0)
|
||||
{
|
||||
std::cout << "Writing .mat files ...";
|
||||
}
|
||||
gps_l5i_dll_pll_tracking_cc::save_matfile();
|
||||
if(d_channel == 0)
|
||||
{
|
||||
std::cout << " done." << std::endl;
|
||||
}
|
||||
}
|
||||
try
|
||||
{
|
||||
volk_gnsssdr_free(d_local_code_shift_chips);
|
||||
volk_gnsssdr_free(d_correlator_outs);
|
||||
volk_gnsssdr_free(d_ca_code);
|
||||
delete[] d_Prompt_buffer;
|
||||
multicorrelator_cpu.free();
|
||||
}
|
||||
catch(const std::exception & ex)
|
||||
{
|
||||
LOG(WARNING) << "Exception in destructor " << ex.what();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
int gps_l5i_dll_pll_tracking_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||
{
|
||||
// process vars
|
||||
double carr_error_hz = 0;
|
||||
double carr_error_filt_hz = 0;
|
||||
double code_error_chips = 0;
|
||||
double code_error_filt_chips = 0;
|
||||
|
||||
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
|
||||
Gnss_Synchro current_synchro_data = Gnss_Synchro();
|
||||
|
||||
// Block input data and block output stream pointers
|
||||
const gr_complex* in = reinterpret_cast<const gr_complex *>(input_items[0]);
|
||||
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]);
|
||||
|
||||
if (d_enable_tracking == true)
|
||||
{
|
||||
// Fill the acquisition data
|
||||
current_synchro_data = *d_acquisition_gnss_synchro;
|
||||
// Receiver signal alignment
|
||||
if (d_pull_in == true)
|
||||
{
|
||||
int samples_offset;
|
||||
double acq_trk_shif_correction_samples;
|
||||
int acq_to_trk_delay_samples;
|
||||
acq_to_trk_delay_samples = d_sample_counter - d_acq_sample_stamp;
|
||||
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod(static_cast<float>(acq_to_trk_delay_samples), static_cast<float>(d_current_prn_length_samples));
|
||||
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
|
||||
current_synchro_data.Tracking_sample_counter = d_sample_counter + samples_offset;
|
||||
d_sample_counter = d_sample_counter + samples_offset; // count for the processed samples
|
||||
d_pull_in = false;
|
||||
// take into account the carrier cycles accumulated in the pull in signal alignment
|
||||
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * samples_offset;
|
||||
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
|
||||
current_synchro_data.fs = d_fs_in;
|
||||
current_synchro_data.correlation_length_ms = 20;
|
||||
*out[0] = current_synchro_data;
|
||||
consume_each(samples_offset); // shift input to perform alignment with local replica
|
||||
return 1;
|
||||
}
|
||||
|
||||
// ################# CARRIER WIPEOFF AND CORRELATORS ##############################
|
||||
// perform carrier wipe-off and compute Early, Prompt and Late correlation
|
||||
multicorrelator_cpu.set_input_output_vectors(d_correlator_outs, in);
|
||||
multicorrelator_cpu.Carrier_wipeoff_multicorrelator_resampler(d_rem_carr_phase_rad,
|
||||
d_carrier_phase_step_rad,
|
||||
d_rem_code_phase_chips,
|
||||
d_code_phase_step_chips,
|
||||
d_current_prn_length_samples);
|
||||
|
||||
// ################## PLL ##########################################################
|
||||
// PLL discriminator
|
||||
// Update PLL discriminator [rads/Ti -> Secs/Ti]
|
||||
carr_error_hz = pll_cloop_two_quadrant_atan(d_correlator_outs[1]) / GPS_L5_TWO_PI;
|
||||
// Carrier discriminator filter
|
||||
carr_error_filt_hz = d_carrier_loop_filter.get_carrier_nco(carr_error_hz);
|
||||
// New carrier Doppler frequency estimation
|
||||
d_carrier_doppler_hz = d_acq_carrier_doppler_hz + carr_error_filt_hz;
|
||||
// New code Doppler frequency estimation
|
||||
d_code_freq_chips = GPS_L5i_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L5i_CODE_RATE_HZ) / GPS_L5_FREQ_HZ);
|
||||
|
||||
// ################## DLL ##########################################################
|
||||
// DLL discriminator
|
||||
code_error_chips = dll_nc_e_minus_l_normalized(d_correlator_outs[0], d_correlator_outs[2]); // [chips/Ti]
|
||||
// Code discriminator filter
|
||||
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second]
|
||||
double T_chip_seconds = 1.0 / static_cast<double>(d_code_freq_chips);
|
||||
double T_prn_seconds = T_chip_seconds * GPS_L5i_CODE_LENGTH_CHIPS;
|
||||
double code_error_filt_secs = (T_prn_seconds * code_error_filt_chips*T_chip_seconds); //[seconds]
|
||||
//double code_error_filt_secs = (GPS_L5i_PERIOD * code_error_filt_chips) / GPS_L5i_CODE_RATE_HZ; //[seconds]
|
||||
|
||||
// ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT #######################
|
||||
// keep alignment parameters for the next input buffer
|
||||
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
|
||||
double T_prn_samples = T_prn_seconds * static_cast<double>(d_fs_in);
|
||||
double K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * static_cast<double>(d_fs_in);
|
||||
d_current_prn_length_samples = round(K_blk_samples); // round to a discrete number of samples
|
||||
|
||||
//################### PLL COMMANDS #################################################
|
||||
// carrier phase step (NCO phase increment per sample) [rads/sample]
|
||||
d_carrier_phase_step_rad = GPS_L5_TWO_PI * d_carrier_doppler_hz / static_cast<double>(d_fs_in);
|
||||
// remnant carrier phase to prevent overflow in the code NCO
|
||||
d_rem_carr_phase_rad = d_rem_carr_phase_rad + d_carrier_phase_step_rad * d_current_prn_length_samples;
|
||||
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_L5_TWO_PI);
|
||||
// carrier phase accumulator
|
||||
d_acc_carrier_phase_rad -= d_carrier_phase_step_rad * d_current_prn_length_samples;
|
||||
|
||||
//################### DLL COMMANDS #################################################
|
||||
// code phase step (Code resampler phase increment per sample) [chips/sample]
|
||||
d_code_phase_step_chips = d_code_freq_chips / static_cast<double>(d_fs_in);
|
||||
// remnant code phase [chips]
|
||||
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; // rounding error < 1 sample
|
||||
d_rem_code_phase_chips = d_code_freq_chips * (d_rem_code_phase_samples / static_cast<double>(d_fs_in));
|
||||
|
||||
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
|
||||
if (d_cn0_estimation_counter < GPS_L2M_CN0_ESTIMATION_SAMPLES)
|
||||
{
|
||||
// fill buffer with prompt correlator output values
|
||||
d_Prompt_buffer[d_cn0_estimation_counter] = d_correlator_outs[1];
|
||||
d_cn0_estimation_counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_cn0_estimation_counter = 0;
|
||||
// Code lock indicator
|
||||
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, GPS_L2M_CN0_ESTIMATION_SAMPLES, d_fs_in, GPS_L5i_CODE_LENGTH_CHIPS);
|
||||
// Carrier lock indicator
|
||||
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, GPS_L2M_CN0_ESTIMATION_SAMPLES);
|
||||
// Loss of lock detection
|
||||
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < GPS_L2M_MINIMUM_VALID_CN0)
|
||||
{
|
||||
d_carrier_lock_fail_counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
|
||||
}
|
||||
if (d_carrier_lock_fail_counter > GPS_L2M_MAXIMUM_LOCK_FAIL_COUNTER)
|
||||
{
|
||||
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
|
||||
LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
|
||||
this->message_port_pub(pmt::mp("events"), pmt::from_long(3));//3 -> loss of lock
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
|
||||
}
|
||||
}
|
||||
// ########### Output the tracking data to navigation and PVT ##########
|
||||
current_synchro_data.Prompt_I = static_cast<double>(d_correlator_outs[1].real());
|
||||
current_synchro_data.Prompt_Q = static_cast<double>(d_correlator_outs[1].imag());
|
||||
current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples;
|
||||
current_synchro_data.Code_phase_samples = d_rem_code_phase_samples;
|
||||
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
|
||||
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
|
||||
current_synchro_data.Flag_valid_symbol_output = true;
|
||||
current_synchro_data.correlation_length_ms = 20;
|
||||
}
|
||||
else
|
||||
{
|
||||
for (int n = 0; n < d_n_correlator_taps; n++)
|
||||
{
|
||||
d_correlator_outs[n] = gr_complex(0,0);
|
||||
}
|
||||
current_synchro_data.Tracking_sample_counter = d_sample_counter + d_current_prn_length_samples;
|
||||
current_synchro_data.correlation_length_ms = 20;
|
||||
}
|
||||
//assign the GNURadio block output data
|
||||
current_synchro_data.fs = d_fs_in;
|
||||
*out[0] = current_synchro_data;
|
||||
|
||||
if(d_dump)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
float prompt_I;
|
||||
float prompt_Q;
|
||||
float tmp_E, tmp_P, tmp_L;
|
||||
double tmp_double;
|
||||
prompt_I = d_correlator_outs[1].real();
|
||||
prompt_Q = d_correlator_outs[1].imag();
|
||||
tmp_E = std::abs<float>(d_correlator_outs[0]);
|
||||
tmp_P = std::abs<float>(d_correlator_outs[1]);
|
||||
tmp_L = std::abs<float>(d_correlator_outs[2]);
|
||||
try
|
||||
{
|
||||
// EPR
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_E), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_P), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_L), sizeof(float));
|
||||
// PROMPT I and Q (to analyze navigation symbols)
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prompt_I), sizeof(float));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prompt_Q), sizeof(float));
|
||||
// PRN start sample stamp
|
||||
//tmp_float=(float)d_sample_counter;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_sample_counter), sizeof(unsigned long int));
|
||||
// accumulated carrier phase
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_acc_carrier_phase_rad), sizeof(double));
|
||||
|
||||
// carrier and code frequency
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_code_freq_chips), sizeof(double));
|
||||
|
||||
//PLL commands
|
||||
d_dump_file.write(reinterpret_cast<char*>(&carr_error_hz), sizeof(double));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_doppler_hz), sizeof(double));
|
||||
|
||||
//DLL commands
|
||||
d_dump_file.write(reinterpret_cast<char*>(&code_error_chips), sizeof(double));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&code_error_filt_chips), sizeof(double));
|
||||
|
||||
// CN0 and carrier lock test
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_CN0_SNV_dB_Hz), sizeof(double));
|
||||
d_dump_file.write(reinterpret_cast<char*>(&d_carrier_lock_test), sizeof(double));
|
||||
|
||||
// AUX vars (for debug purposes)
|
||||
tmp_double = d_rem_code_phase_samples;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
|
||||
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
|
||||
|
||||
// PRN
|
||||
unsigned int prn_ = d_acquisition_gnss_synchro->PRN;
|
||||
d_dump_file.write(reinterpret_cast<char*>(&prn_), sizeof(unsigned int));
|
||||
}
|
||||
catch (std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing trk dump file " << e.what();
|
||||
}
|
||||
}
|
||||
consume_each(d_current_prn_length_samples); // this is necessary 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_l5i_dll_pll_tracking_cc::set_channel(unsigned int channel)
|
||||
{
|
||||
d_channel = channel;
|
||||
LOG(INFO) << "Tracking Channel set to " << d_channel;
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_dump == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
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);
|
||||
LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void gps_l5i_dll_pll_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
|
||||
{
|
||||
d_acquisition_gnss_synchro = p_gnss_synchro;
|
||||
}
|
||||
@@ -0,0 +1,169 @@
|
||||
/*!
|
||||
* \file gps_l5i_dll_pll_tracking_cc.h
|
||||
* \brief Interface of a code DLL + carrier PLL tracking block for GPS L2C
|
||||
* \author Javier Arribas, 2015. jarribas(at)cttc.es
|
||||
*
|
||||
* Code DLL + carrier PLL according to the algorithms described in:
|
||||
* K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen,
|
||||
* A Software-Defined GPS and Galileo Receiver. A Single-Frequency Approach,
|
||||
* Birkhauser, 2007
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2015 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_CC_H
|
||||
#define GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_CC_H
|
||||
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <gnuradio/block.h>
|
||||
#include "gnss_synchro.h"
|
||||
#include "tracking_2nd_DLL_filter.h"
|
||||
#include "tracking_2nd_PLL_filter.h"
|
||||
#include "cpu_multicorrelator.h"
|
||||
|
||||
class gps_l5i_dll_pll_tracking_cc;
|
||||
|
||||
typedef boost::shared_ptr<gps_l5i_dll_pll_tracking_cc>
|
||||
gps_l5i_dll_pll_tracking_cc_sptr;
|
||||
|
||||
gps_l5i_dll_pll_tracking_cc_sptr
|
||||
gps_l5i_dll_pll_make_tracking_cc(long if_freq,
|
||||
long fs_in, unsigned
|
||||
int vector_length,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This class implements a DLL + PLL tracking loop block
|
||||
*/
|
||||
class gps_l5i_dll_pll_tracking_cc: public gr::block
|
||||
{
|
||||
public:
|
||||
~gps_l5i_dll_pll_tracking_cc();
|
||||
|
||||
void set_channel(unsigned int channel);
|
||||
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
|
||||
void start_tracking();
|
||||
|
||||
int general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
|
||||
|
||||
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
|
||||
|
||||
private:
|
||||
friend gps_l5i_dll_pll_tracking_cc_sptr
|
||||
gps_l5i_dll_pll_make_tracking_cc(long if_freq,
|
||||
long fs_in, unsigned
|
||||
int vector_length,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
gps_l5i_dll_pll_tracking_cc(long if_freq,
|
||||
long fs_in, unsigned
|
||||
int vector_length,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
// tracking configuration vars
|
||||
unsigned int d_vector_length;
|
||||
bool d_dump;
|
||||
|
||||
Gnss_Synchro* d_acquisition_gnss_synchro;
|
||||
unsigned int d_channel;
|
||||
long d_if_freq;
|
||||
long d_fs_in;
|
||||
|
||||
double d_early_late_spc_chips;
|
||||
|
||||
// remaining code phase and carrier phase between tracking loops
|
||||
double d_rem_code_phase_samples;
|
||||
double d_rem_code_phase_chips;
|
||||
double d_rem_carr_phase_rad;
|
||||
|
||||
// PLL and DLL filter library
|
||||
Tracking_2nd_DLL_filter d_code_loop_filter;
|
||||
Tracking_2nd_PLL_filter d_carrier_loop_filter;
|
||||
|
||||
// acquisition
|
||||
double d_acq_code_phase_samples;
|
||||
double d_acq_carrier_doppler_hz;
|
||||
// correlator
|
||||
int d_n_correlator_taps;
|
||||
gr_complex* d_ca_code;
|
||||
float* d_local_code_shift_chips;
|
||||
gr_complex* d_correlator_outs;
|
||||
cpu_multicorrelator multicorrelator_cpu;
|
||||
|
||||
// tracking vars
|
||||
double d_code_freq_chips;
|
||||
double d_code_phase_step_chips;
|
||||
double d_carrier_doppler_hz;
|
||||
double d_carrier_phase_step_rad;
|
||||
double d_acc_carrier_phase_rad;
|
||||
double d_code_phase_samples;
|
||||
|
||||
// PRN period in samples
|
||||
int d_current_prn_length_samples;
|
||||
|
||||
// processing samples counters
|
||||
unsigned long int d_sample_counter;
|
||||
unsigned long int d_acq_sample_stamp;
|
||||
|
||||
// CN0 estimation and lock detector
|
||||
int d_cn0_estimation_counter;
|
||||
gr_complex* d_Prompt_buffer;
|
||||
double d_carrier_lock_test;
|
||||
double d_CN0_SNV_dB_Hz;
|
||||
double 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;
|
||||
|
||||
std::map<std::string, std::string> systemName;
|
||||
std::string sys;
|
||||
|
||||
int save_matfile();
|
||||
};
|
||||
|
||||
#endif //GNSS_SDR_GPS_L5i_DLL_PLL_TRACKING_CC_H
|
||||
Reference in New Issue
Block a user