mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-11-18 08:05:17 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of https://github.com/gnss-sdr/gnss-sdr into glamountain-kf

Browse Source
This commit is contained in:
Carles Fernandez
2018-07-20 12:59:46 +02:00
parent 486ac195db 23a036f58f
commit 5a908fcf7d
63 changed files with 1948 additions and 1122 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
src/algorithms/acquisition/adapters/galileo_e1_pcps_8ms_ambiguous_acquisition.h
View File

@@ -32,12 +32,11 @@
#ifndef GNSS_SDR_GALILEO_E1_PCPS_8MS_AMBIGUOUS_ACQUISITION_H_
#define GNSS_SDR_GALILEO_E1_PCPS_8MS_AMBIGUOUS_ACQUISITION_H_
#include <string>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "galileo_pcps_8ms_acquisition_cc.h"
#include <gnuradio/blocks/stream_to_vector.h>
#include <string>
class ConfigurationInterface;
@@ -123,6 +122,7 @@ public:
* \brief Restart acquisition algorithm
*/
void reset() override;
void set_state(int state __attribute__((unused))) override{};
private:
ConfigurationInterface* configuration_;

27
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.cc
View File

@@ -58,31 +58,38 @@ GalileoE1PcpsAmbiguousAcquisition::GalileoE1PcpsAmbiguousAcquisition(
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 4000000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_parameters.fs_in = fs_in_;
dump_ = configuration_->property(role + ".dump", false);
acq_parameters.dump = dump_;
acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0);
blocking_ = configuration_->property(role + ".blocking", true);
acq_parameters.blocking = blocking_;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil((1.0 / Galileo_E1_CODE_CHIP_RATE_HZ) * static_cast<float>(acq_parameters.fs_in)));
doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
acq_parameters.doppler_max = doppler_max_;
sampled_ms_ = 4;
acq_parameters.ms_per_code = 4;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", acq_parameters.ms_per_code);
acq_parameters.sampled_ms = sampled_ms_;
if ((acq_parameters.sampled_ms % acq_parameters.ms_per_code) != 0)
{
LOG(WARNING) << "Parameter coherent_integration_time_ms should be a multiple of 4. Setting it to 4";
acq_parameters.sampled_ms = acq_parameters.ms_per_code;
}
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
acq_parameters.bit_transition_flag = bit_transition_flag_;
use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
acq_parameters.use_CFAR_algorithm_flag = use_CFAR_algorithm_flag_;
acquire_pilot_ = configuration_->property(role + ".acquire_pilot", false); //will be true in future versions
max_dwells_ = configuration_->property(role + ".max_dwells", 1);
acq_parameters.max_dwells = max_dwells_;
dump_ = configuration_->property(role + ".dump", false);
acq_parameters.dump = dump_;
acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0);
blocking_ = configuration_->property(role + ".blocking", true);
acq_parameters.blocking = blocking_;
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
acq_parameters.dump_filename = dump_filename_;
//--- Find number of samples per spreading code (4 ms) -----------------
code_length_ = static_cast<unsigned int>(std::round(static_cast<double>(fs_in_) / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)));
acq_parameters.samples_per_code = code_length_;
int samples_per_ms = static_cast<int>(std::round(static_cast<double>(fs_in_) * 0.001));
code_length_ = static_cast<unsigned int>(std::floor(static_cast<double>(fs_in_) / (Galileo_E1_CODE_CHIP_RATE_HZ / Galileo_E1_B_CODE_LENGTH_CHIPS)));
float samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.samples_per_ms = samples_per_ms;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(Galileo_E1_CODE_PERIOD_MS);
vector_length_ = sampled_ms_ * samples_per_ms;
if (bit_transition_flag_)

2
src/algorithms/acquisition/adapters/galileo_e1_pcps_ambiguous_acquisition.h
View File

@@ -130,7 +130,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

7
src/algorithms/acquisition/adapters/galileo_e1_pcps_cccwsr_ambiguous_acquisition.h
View File

@@ -32,12 +32,11 @@
#ifndef GNSS_SDR_GALILEO_E1_PCPS_CCCWSR_AMBIGUOUS_ACQUISITION_H_
#define GNSS_SDR_GALILEO_E1_PCPS_CCCWSR_AMBIGUOUS_ACQUISITION_H_
#include <string>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_cccwsr_acquisition_cc.h"
#include <gnuradio/blocks/stream_to_vector.h>
#include <string>
class ConfigurationInterface;
@@ -124,7 +123,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

6
src/algorithms/acquisition/adapters/galileo_e1_pcps_quicksync_ambiguous_acquisition.h
View File

@@ -32,11 +32,11 @@
#ifndef GNSS_SDR_GALILEO_E1_PCPS_QUICKSYNC_AMBIGUOUS_ACQUISITION_H_
#define GNSS_SDR_GALILEO_E1_PCPS_QUICKSYNC_AMBIGUOUS_ACQUISITION_H_
#include <string>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_quicksync_acquisition_cc.h"
#include <gnuradio/blocks/stream_to_vector.h>
#include <string>
class ConfigurationInterface;
@@ -127,7 +127,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

7
src/algorithms/acquisition/adapters/galileo_e1_pcps_tong_ambiguous_acquisition.h
View File

@@ -32,12 +32,11 @@
#ifndef GNSS_SDR_GALILEO_E1_PCPS_TONG_AMBIGUOUS_ACQUISITION_H_
#define GNSS_SDR_GALILEO_E1_PCPS_TONG_AMBIGUOUS_ACQUISITION_H_
#include <string>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_tong_acquisition_cc.h"
#include <gnuradio/blocks/stream_to_vector.h>
#include <string>
class ConfigurationInterface;
@@ -127,7 +126,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

4
src/algorithms/acquisition/adapters/galileo_e5a_noncoherent_iq_acquisition_caf.h
View File

@@ -38,10 +38,10 @@
#ifndef GALILEO_E5A_NONCOHERENT_IQ_ACQUISITION_CAF_H_
#define GALILEO_E5A_NONCOHERENT_IQ_ACQUISITION_CAF_H_
#include <string>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "galileo_e5a_noncoherent_iq_acquisition_caf_cc.h"
#include <string>
class ConfigurationInterface;
@@ -129,7 +129,7 @@ public:
* first available sample.
* \param state - int=1 forces start of acquisition
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

6
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.cc
View File

@@ -57,6 +57,7 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 32000000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_parameters.fs_in = fs_in_;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil((1.0 / Galileo_E5a_CODE_CHIP_RATE_HZ) * static_cast<float>(acq_parameters.fs_in)));
acq_pilot_ = configuration_->property(role + ".acquire_pilot", false);
acq_iq_ = configuration_->property(role + ".acquire_iq", false);
if (acq_iq_)
@@ -100,9 +101,10 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(ConfigurationInterface* con
LOG(WARNING) << item_type_ << " unknown acquisition item type";
}
acq_parameters.it_size = item_size_;
acq_parameters.samples_per_code = code_length_;
acq_parameters.samples_per_ms = code_length_;
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.sampled_ms = sampled_ms_;
acq_parameters.ms_per_code = 1;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GALILEO_E5a_CODE_PERIOD_MS);
acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4);
acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0);
acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false);

2
src/algorithms/acquisition/adapters/galileo_e5a_pcps_acquisition.h
View File

@@ -121,7 +121,7 @@ public:
* first available sample.
* \param state - int=1 forces start of acquisition
*/
void set_state(int state);
void set_state(int state) override;
private:
float calculate_threshold(float pfa);

6
src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.cc
View File

@@ -59,6 +59,7 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition(
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_parameters.fs_in = fs_in_;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil(GLONASS_L1_CA_CHIP_PERIOD * static_cast<float>(acq_parameters.fs_in)));
dump_ = configuration_->property(role + ".dump", false);
acq_parameters.dump = dump_;
acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0);
@@ -99,8 +100,9 @@ GlonassL1CaPcpsAcquisition::GlonassL1CaPcpsAcquisition(
}
acq_parameters.it_size = item_size_;
acq_parameters.sampled_ms = sampled_ms_;
acq_parameters.samples_per_ms = code_length_;
acq_parameters.samples_per_code = code_length_;
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.ms_per_code = 1;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GLONASS_L1_CA_CODE_PERIOD * 1000.0);
acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4);
acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0);
acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false);

2
src/algorithms/acquisition/adapters/glonass_l1_ca_pcps_acquisition.h
View File

@@ -130,7 +130,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

6
src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.cc
View File

@@ -58,6 +58,7 @@ GlonassL2CaPcpsAcquisition::GlonassL2CaPcpsAcquisition(
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_parameters.fs_in = fs_in_;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil(GLONASS_L2_CA_CHIP_PERIOD * static_cast<float>(acq_parameters.fs_in)));
dump_ = configuration_->property(role + ".dump", false);
acq_parameters.dump = dump_;
acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0);
@@ -98,8 +99,9 @@ GlonassL2CaPcpsAcquisition::GlonassL2CaPcpsAcquisition(
}
acq_parameters.it_size = item_size_;
acq_parameters.sampled_ms = sampled_ms_;
acq_parameters.samples_per_ms = code_length_;
acq_parameters.samples_per_code = code_length_;
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.ms_per_code = 1;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GLONASS_L2_CA_CODE_PERIOD * 1000.0);
acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4);
acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0);
acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false);

2
src/algorithms/acquisition/adapters/glonass_l2_ca_pcps_acquisition.h
View File

@@ -129,7 +129,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

17
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.cc
View File

@@ -60,6 +60,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_parameters.fs_in = fs_in_;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil(GPS_L1_CA_CHIP_PERIOD * static_cast<float>(acq_parameters.fs_in)));
dump_ = configuration_->property(role + ".dump", false);
acq_parameters.dump = dump_;
acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0);
@@ -70,6 +71,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
acq_parameters.doppler_max = doppler_max_;
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
acq_parameters.sampled_ms = sampled_ms_;
acq_parameters.ms_per_code = 1;
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
acq_parameters.bit_transition_flag = bit_transition_flag_;
use_CFAR_algorithm_flag_ = configuration_->property(role + ".use_CFAR_algorithm", true); //will be false in future versions
@@ -82,15 +84,11 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0);
acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false);
//--- Find number of samples per spreading code -------------------------
code_length_ = static_cast<unsigned int>(std::round(static_cast<double>(fs_in_) / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)));
vector_length_ = code_length_ * sampled_ms_;
if (bit_transition_flag_)
{
vector_length_ *= 2;
}
code_length_ = static_cast<unsigned int>(std::floor(static_cast<double>(fs_in_) / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS)));
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GPS_L1_CA_CODE_PERIOD * 1000.0);
vector_length_ = std::floor(acq_parameters.sampled_ms * acq_parameters.samples_per_ms) * (acq_parameters.bit_transition_flag ? 2 : 1);
code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0)
@@ -101,8 +99,7 @@ GpsL1CaPcpsAcquisition::GpsL1CaPcpsAcquisition(
{
item_size_ = sizeof(gr_complex);
}
acq_parameters.samples_per_ms = code_length_;
acq_parameters.samples_per_code = code_length_;
acq_parameters.it_size = item_size_;
acq_parameters.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false);
acquisition_ = pcps_make_acquisition(acq_parameters);

2
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition.h
View File

@@ -134,7 +134,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

26
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.cc
View File

@@ -33,11 +33,12 @@
*/
#include "gps_l1_ca_pcps_acquisition_fine_doppler.h"
#include <glog/logging.h>
#include "gps_sdr_signal_processing.h"
#include "GPS_L1_CA.h"
#include "configuration_interface.h"
#include "gnss_sdr_flags.h"
#include "acq_conf.h"
#include <glog/logging.h>
using google::LogMessage;
@@ -49,29 +50,36 @@ GpsL1CaPcpsAcquisitionFineDoppler::GpsL1CaPcpsAcquisitionFineDoppler(
std::string default_dump_filename = "./data/acquisition.dat";
DLOG(INFO) << "role " << role;
Acq_Conf acq_parameters = Acq_Conf();
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);
acq_parameters.fs_in = fs_in_;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil(GPS_L1_CA_CHIP_PERIOD * static_cast<float>(acq_parameters.fs_in)));
dump_ = configuration->property(role + ".dump", false);
acq_parameters.dump = dump_;
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
acq_parameters.dump_filename = dump_filename_;
doppler_max_ = configuration->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
doppler_min_ = configuration->property(role + ".doppler_min", -doppler_max_);
acq_parameters.doppler_max = doppler_max_;
sampled_ms_ = configuration->property(role + ".coherent_integration_time_ms", 1);
acq_parameters.sampled_ms = sampled_ms_;
max_dwells_ = configuration->property(role + ".max_dwells", 1);
acq_parameters.max_dwells = max_dwells_;
acq_parameters.blocking_on_standby = configuration->property(role + ".blocking_on_standby", false);
//--- Find number of samples per spreading code -------------------------
vector_length_ = round(fs_in_ / (GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS));
acq_parameters.samples_per_ms = vector_length_;
code_ = new gr_complex[vector_length_];
if (item_type_.compare("gr_complex") == 0)
{
item_size_ = sizeof(gr_complex);
acquisition_cc_ = pcps_make_acquisition_fine_doppler_cc(max_dwells_, sampled_ms_,
doppler_max_, doppler_min_, fs_in_, vector_length_,
dump_, dump_filename_);
acquisition_cc_ = pcps_make_acquisition_fine_doppler_cc(acq_parameters);
}
else
{
@@ -161,6 +169,12 @@ void GpsL1CaPcpsAcquisitionFineDoppler::reset()
}
void GpsL1CaPcpsAcquisitionFineDoppler::set_state(int state)
{
acquisition_cc_->set_state(state);
}
void GpsL1CaPcpsAcquisitionFineDoppler::connect(boost::shared_ptr<gr::top_block> top_block)
{
if (top_block)

9
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fine_doppler.h
View File

@@ -34,11 +34,10 @@
#ifndef GNSS_SDR_GPS_L1_CA_PCPS_ACQUISITION_FINE_DOPPLER_H_
#define GNSS_SDR_GPS_L1_CA_PCPS_ACQUISITION_FINE_DOPPLER_H_
#include <string>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_acquisition_fine_doppler_cc.h"
#include <string>
class ConfigurationInterface;
@@ -122,6 +121,11 @@ public:
*/
void reset() override;
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state) override;
private:
pcps_acquisition_fine_doppler_cc_sptr acquisition_cc_;
size_t item_size_;
@@ -131,7 +135,6 @@ private:
float threshold_;
int doppler_max_;
unsigned int doppler_step_;
int doppler_min_;
unsigned int sampled_ms_;
int max_dwells_;
long fs_in_;

1
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.cc
View File

@@ -60,6 +60,7 @@ GpsL1CaPcpsAcquisitionFpga::GpsL1CaPcpsAcquisitionFpga(
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
long fs_in = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_parameters.fs_in = fs_in;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil(GPS_L1_CA_CHIP_PERIOD * static_cast<float>(acq_parameters.fs_in)));
doppler_max_ = configuration_->property(role + ".doppler_max", 5000);
if (FLAGS_doppler_max != 0) doppler_max_ = FLAGS_doppler_max;
acq_parameters.doppler_max = doppler_max_;

2
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_acquisition_fpga.h
View File

@@ -132,7 +132,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

4
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_assisted_acquisition.h
View File

@@ -34,11 +34,10 @@
#ifndef GNSS_SDR_GPS_L1_CA_PCPS_ASSISTED_ACQUISITION_H_
#define GNSS_SDR_GPS_L1_CA_PCPS_ASSISTED_ACQUISITION_H_
#include <string>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_assisted_acquisition_cc.h"
#include <string>
class ConfigurationInterface;
@@ -121,6 +120,7 @@ public:
* \brief Restart acquisition algorithm
*/
void reset() override;
void set_state(int state __attribute__((unused))) override{};
private:
pcps_assisted_acquisition_cc_sptr acquisition_cc_;

6
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_opencl_acquisition.h
View File

@@ -32,12 +32,11 @@
#ifndef GNSS_SDR_GPS_L1_CA_PCPS_OPENCL_ACQUISITION_H_
#define GNSS_SDR_GPS_L1_CA_PCPS_OPENCL_ACQUISITION_H_
#include <string>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_opencl_acquisition_cc.h"
#include <gnuradio/blocks/stream_to_vector.h>
#include <string>
class ConfigurationInterface;
@@ -123,6 +122,7 @@ public:
* \brief Restart acquisition algorithm
*/
void reset() override;
void set_state(int state __attribute__((unused))) override{};
private:
ConfigurationInterface* configuration_;

7
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_quicksync_acquisition.h
View File

@@ -33,13 +33,12 @@
#ifndef GNSS_SDR_GPS_L1_CA_PCPS_QUICKSYNC_ACQUISITION_H_
#define GNSS_SDR_GPS_L1_CA_PCPS_QUICKSYNC_ACQUISITION_H_
#include <string>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_quicksync_acquisition_cc.h"
#include "configuration_interface.h"
#include <gnuradio/blocks/stream_to_vector.h>
#include <string>
class ConfigurationInterface;
@@ -129,7 +128,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

6
src/algorithms/acquisition/adapters/gps_l1_ca_pcps_tong_acquisition.h
View File

@@ -32,12 +32,12 @@
#ifndef GNSS_SDR_GPS_L1_CA_TONG_ACQUISITION_H_
#define GNSS_SDR_GPS_L1_CA_TONG_ACQUISITION_H_
#include <string>
#include <gnuradio/blocks/stream_to_vector.h>
#include "gnss_synchro.h"
#include "acquisition_interface.h"
#include "pcps_tong_acquisition_cc.h"
#include "configuration_interface.h"
#include <gnuradio/blocks/stream_to_vector.h>
#include <string>
class ConfigurationInterface;
@@ -127,7 +127,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;

37
src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.cc
View File

@@ -60,6 +60,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_parameters.fs_in = fs_in_;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil((1.0 / GPS_L2_M_CODE_RATE_HZ) * static_cast<float>(acq_parameters.fs_in)));
dump_ = configuration_->property(role + ".dump", false);
acq_parameters.dump = dump_;
acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0);
@@ -77,15 +78,19 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
dump_filename_ = configuration_->property(role + ".dump_filename", default_dump_filename);
acq_parameters.dump_filename = dump_filename_;
//--- Find number of samples per spreading code -------------------------
code_length_ = std::round(static_cast<double>(fs_in_) / (GPS_L2_M_CODE_RATE_HZ / static_cast<double>(GPS_L2_M_CODE_LENGTH_CHIPS)));
vector_length_ = code_length_;
if (bit_transition_flag_)
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.ms_per_code = 20;
acq_parameters.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", acq_parameters.ms_per_code);
if ((acq_parameters.sampled_ms % acq_parameters.ms_per_code) != 0)
{
vector_length_ *= 2;
LOG(WARNING) << "Parameter coherent_integration_time_ms should be a multiple of 20. Setting it to 20";
acq_parameters.sampled_ms = acq_parameters.ms_per_code;
}
code_length_ = acq_parameters.ms_per_code * acq_parameters.samples_per_ms;
vector_length_ = acq_parameters.sampled_ms * acq_parameters.samples_per_ms * (acq_parameters.bit_transition_flag ? 2 : 1);
code_ = new gr_complex[vector_length_];
if (item_type_.compare("cshort") == 0)
@@ -96,13 +101,13 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
{
item_size_ = sizeof(gr_complex);
}
acq_parameters.samples_per_ms = static_cast<int>(std::round(static_cast<double>(fs_in_) * 0.001));
acq_parameters.samples_per_code = code_length_;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GPS_L2_M_PERIOD * 1000.0);
acq_parameters.it_size = item_size_;
acq_parameters.sampled_ms = 20;
acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4);
acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0);
acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", true);
acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false);
acq_parameters.blocking_on_standby = configuration_->property(role + ".blocking_on_standby", false);
acquisition_ = pcps_make_acquisition(acq_parameters);
DLOG(INFO) << "acquisition(" << acquisition_->unique_id() << ")";
@@ -120,6 +125,7 @@ GpsL2MPcpsAcquisition::GpsL2MPcpsAcquisition(
threshold_ = 0.0;
doppler_step_ = 0;
gnss_synchro_ = 0;
num_codes_ = acq_parameters.sampled_ms / acq_parameters.ms_per_code;
if (in_streams_ > 1)
{
LOG(ERROR) << "This implementation only supports one input stream";
@@ -208,9 +214,18 @@ void GpsL2MPcpsAcquisition::init()
void GpsL2MPcpsAcquisition::set_local_code()
{
gps_l2c_m_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_);
std::complex<float>* code = new std::complex<float>[code_length_];
gps_l2c_m_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_);
for (unsigned int i = 0; i < num_codes_; i++)
{
memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex) * code_length_);
}
acquisition_->set_local_code(code_);
delete[] code;
}

3
src/algorithms/acquisition/adapters/gps_l2_m_pcps_acquisition.h
View File

@@ -132,7 +132,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;
@@ -160,6 +160,7 @@ private:
std::string role_;
unsigned int in_streams_;
unsigned int out_streams_;
unsigned int num_codes_;
float calculate_threshold(float pfa);
};

22
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.cc
View File

@@ -1,5 +1,5 @@
/*!
* \file gps_l5i pcps_acquisition.cc
* \file gps_l5i_pcps_acquisition.cc
* \brief Adapts a PCPS acquisition block to an Acquisition Interface for
* GPS L5i signals
* \authors <ul>
@@ -59,6 +59,7 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
long fs_in_deprecated = configuration_->property("GNSS-SDR.internal_fs_hz", 2048000);
fs_in_ = configuration_->property("GNSS-SDR.internal_fs_sps", fs_in_deprecated);
acq_parameters.fs_in = fs_in_;
acq_parameters.samples_per_chip = static_cast<unsigned int>(ceil((1.0 / GPS_L5i_CODE_RATE_HZ) * static_cast<float>(acq_parameters.fs_in)));
dump_ = configuration_->property(role + ".dump", false);
acq_parameters.dump = dump_;
acq_parameters.dump_channel = configuration_->property(role + ".dump_channel", 0);
@@ -95,10 +96,12 @@ GpsL5iPcpsAcquisition::GpsL5iPcpsAcquisition(
{
item_size_ = sizeof(gr_complex);
}
acq_parameters.samples_per_code = code_length_;
acq_parameters.samples_per_ms = code_length_;
acq_parameters.samples_per_ms = static_cast<float>(fs_in_) * 0.001;
acq_parameters.samples_per_code = acq_parameters.samples_per_ms * static_cast<float>(GPS_L5i_PERIOD * 1000.0);
acq_parameters.ms_per_code = 1;
acq_parameters.it_size = item_size_;
acq_parameters.sampled_ms = 1;
acq_parameters.sampled_ms = configuration_->property(role + ".coherent_integration_time_ms", 1);
num_codes_ = acq_parameters.sampled_ms;
acq_parameters.num_doppler_bins_step2 = configuration_->property(role + ".second_nbins", 4);
acq_parameters.doppler_step2 = configuration_->property(role + ".second_doppler_step", 125.0);
acq_parameters.make_2_steps = configuration_->property(role + ".make_two_steps", false);
@@ -205,9 +208,18 @@ void GpsL5iPcpsAcquisition::init()
void GpsL5iPcpsAcquisition::set_local_code()
{
gps_l5i_code_gen_complex_sampled(code_, gnss_synchro_->PRN, fs_in_);
std::complex<float>* code = new std::complex<float>[code_length_];
gps_l5i_code_gen_complex_sampled(code, gnss_synchro_->PRN, fs_in_);
for (unsigned int i = 0; i < num_codes_; i++)
{
memcpy(&(code_[i * code_length_]), code,
sizeof(gr_complex) * code_length_);
}
acquisition_->set_local_code(code_);
delete[] code;
}

5
src/algorithms/acquisition/adapters/gps_l5i_pcps_acquisition.h
View File

@@ -1,5 +1,5 @@
/*!
* \file GPS_L5i_PCPS_Acquisition.h
* \file gps_l5i_pcps_acquisition.h
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
* GPS L5i signals
* \authors <ul>
@@ -132,7 +132,7 @@ public:
/*!
* \brief If state = 1, it forces the block to start acquiring from the first sample
*/
void set_state(int state);
void set_state(int state) override;
private:
ConfigurationInterface* configuration_;
@@ -158,6 +158,7 @@ private:
std::complex<float>* code_;
Gnss_Synchro* gnss_synchro_;
std::string role_;
unsigned int num_codes_;
unsigned int in_streams_;
unsigned int out_streams_;

363
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.cc
View File

@@ -52,8 +52,8 @@ pcps_acquisition_sptr pcps_make_acquisition(const Acq_Conf& conf_)
pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acquisition",
gr::io_signature::make(1, 1, conf_.it_size * conf_.sampled_ms * conf_.samples_per_ms * (conf_.bit_transition_flag ? 2 : 1)),
gr::io_signature::make(0, 0, conf_.it_size * conf_.sampled_ms * conf_.samples_per_ms * (conf_.bit_transition_flag ? 2 : 1)))
gr::io_signature::make(1, 1, conf_.it_size * std::floor(conf_.sampled_ms * conf_.samples_per_ms) * (conf_.bit_transition_flag ? 2 : 1)),
gr::io_signature::make(0, 0, conf_.it_size))
{
this->message_port_register_out(pmt::mp("events"));
@@ -63,8 +63,17 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu
d_positive_acq = 0;
d_state = 0;
d_old_freq = 0;
d_well_count = 0;
d_fft_size = acq_parameters.sampled_ms * acq_parameters.samples_per_ms;
d_num_noncoherent_integrations_counter = 0;
d_consumed_samples = acq_parameters.sampled_ms * acq_parameters.samples_per_ms * (acq_parameters.bit_transition_flag ? 2 : 1);
if (acq_parameters.sampled_ms == acq_parameters.ms_per_code)
{
d_fft_size = d_consumed_samples;
}
else
{
d_fft_size = d_consumed_samples * 2;
}
//d_fft_size = next power of two? ////
d_mag = 0;
d_input_power = 0.0;
d_num_doppler_bins = 0;
@@ -94,12 +103,14 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu
// size of the input buffer and padding the code with zeros.
if (acq_parameters.bit_transition_flag)
{
d_fft_size *= 2;
acq_parameters.max_dwells = 1; //Activation of acq_parameters.bit_transition_flag invalidates the value of acq_parameters.max_dwells
d_fft_size = d_consumed_samples * 2;
acq_parameters.max_dwells = 1; // Activation of acq_parameters.bit_transition_flag invalidates the value of acq_parameters.max_dwells
}
d_tmp_buffer = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_fft_codes = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_input_signal = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
// Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -110,11 +121,12 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu
d_gnss_synchro = 0;
d_grid_doppler_wipeoffs = nullptr;
d_grid_doppler_wipeoffs_step_two = nullptr;
d_magnitude_grid = nullptr;
d_worker_active = false;
d_data_buffer = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_data_buffer = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_consumed_samples * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
if (d_cshort)
{
d_data_buffer_sc = static_cast<lv_16sc_t*>(volk_gnsssdr_malloc(d_fft_size * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment()));
d_data_buffer_sc = static_cast<lv_16sc_t*>(volk_gnsssdr_malloc(d_consumed_samples * sizeof(lv_16sc_t), volk_gnsssdr_get_alignment()));
}
else
{
@@ -124,6 +136,16 @@ pcps_acquisition::pcps_acquisition(const Acq_Conf& conf_) : gr::block("pcps_acqu
d_step_two = false;
d_dump_number = 0;
d_dump_channel = acq_parameters.dump_channel;
d_samplesPerChip = acq_parameters.samples_per_chip;
// todo: CFAR statistic not available for non-coherent integration
if (acq_parameters.max_dwells == 1)
{
d_use_CFAR_algorithm_flag = acq_parameters.use_CFAR_algorithm_flag;
}
else
{
d_use_CFAR_algorithm_flag = false;
}
}
@@ -134,8 +156,10 @@ pcps_acquisition::~pcps_acquisition()
for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{
volk_gnsssdr_free(d_grid_doppler_wipeoffs[i]);
volk_gnsssdr_free(d_magnitude_grid[i]);
}
delete[] d_grid_doppler_wipeoffs;
delete[] d_magnitude_grid;
}
if (acq_parameters.make_2_steps)
{
@@ -147,6 +171,8 @@ pcps_acquisition::~pcps_acquisition()
}
volk_gnsssdr_free(d_fft_codes);
volk_gnsssdr_free(d_magnitude);
volk_gnsssdr_free(d_tmp_buffer);
volk_gnsssdr_free(d_input_signal);
delete d_ifft;
delete d_fft_if;
volk_gnsssdr_free(d_data_buffer);
@@ -179,7 +205,15 @@ void pcps_acquisition::set_local_code(std::complex<float>* code)
}
else
{
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_fft_size);
if (acq_parameters.sampled_ms == acq_parameters.ms_per_code)
{
memcpy(d_fft_if->get_inbuf(), code, sizeof(gr_complex) * d_consumed_samples);
}
else
{
std::fill_n(d_fft_if->get_inbuf(), d_fft_size - d_consumed_samples, gr_complex(0.0, 0.0));
memcpy(d_fft_if->get_inbuf() + d_consumed_samples, code, sizeof(gr_complex) * d_consumed_samples);
}
}
d_fft_if->execute(); // We need the FFT of local code
@@ -244,12 +278,20 @@ void pcps_acquisition::init()
d_grid_doppler_wipeoffs_step_two[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
}
}
d_magnitude_grid = new float*[d_num_doppler_bins];
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
d_grid_doppler_wipeoffs[doppler_index] = static_cast<gr_complex*>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude_grid[doppler_index] = static_cast<float*>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
for (unsigned k = 0; k < d_fft_size; k++)
{
d_magnitude_grid[doppler_index][k] = 0.0;
}
int doppler = -static_cast<int>(acq_parameters.doppler_max) + d_doppler_step * doppler_index;
update_local_carrier(d_grid_doppler_wipeoffs[doppler_index], d_fft_size, d_old_freq + doppler);
}
d_worker_active = false;
if (acq_parameters.dump)
@@ -269,6 +311,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs()
}
}
void pcps_acquisition::update_grid_doppler_wipeoffs_step2()
{
for (unsigned int doppler_index = 0; doppler_index < acq_parameters.num_doppler_bins_step2; doppler_index++)
@@ -278,6 +321,7 @@ void pcps_acquisition::update_grid_doppler_wipeoffs_step2()
}
}
void pcps_acquisition::set_state(int state)
{
gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
@@ -288,7 +332,6 @@ void pcps_acquisition::set_state(int state)
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_doppler_step = 0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;
@@ -419,117 +462,202 @@ void pcps_acquisition::dump_results(int effective_fft_size)
}
float pcps_acquisition::max_to_input_power_statistic(uint32_t& indext, int& doppler, float input_power, unsigned int num_doppler_bins, int doppler_max, int doppler_step)
{
float grid_maximum = 0.0;
unsigned int index_doppler = 0;
uint32_t tmp_intex_t = 0;
uint32_t index_time = 0;
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
// Find the correlation peak and the carrier frequency
for (unsigned int i = 0; i < num_doppler_bins; i++)
{
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i], d_fft_size);
if (d_magnitude_grid[i][tmp_intex_t] > grid_maximum)
{
grid_maximum = d_magnitude_grid[i][tmp_intex_t];
index_doppler = i;
index_time = tmp_intex_t;
}
}
indext = index_time;
if (!d_step_two)
{
doppler = -static_cast<int>(doppler_max) + doppler_step * static_cast<int>(index_doppler);
}
else
{
doppler = static_cast<int>(d_doppler_center_step_two + (index_doppler - (acq_parameters.num_doppler_bins_step2 / 2.0) * acq_parameters.doppler_step2));
}
float magt = grid_maximum / (fft_normalization_factor * fft_normalization_factor);
return magt / input_power;
}
float pcps_acquisition::first_vs_second_peak_statistic(uint32_t& indext, int& doppler, unsigned int num_doppler_bins, int doppler_max, int doppler_step)
{
// Look for correlation peaks in the results
// Find the highest peak and compare it to the second highest peak
// The second peak is chosen not closer than 1 chip to the highest peak
float firstPeak = 0.0;
unsigned int index_doppler = 0;
uint32_t tmp_intex_t = 0;
uint32_t index_time = 0;
// Find the correlation peak and the carrier frequency
for (unsigned int i = 0; i < num_doppler_bins; i++)
{
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_magnitude_grid[i], d_fft_size);
if (d_magnitude_grid[i][tmp_intex_t] > firstPeak)
{
firstPeak = d_magnitude_grid[i][tmp_intex_t];
index_doppler = i;
index_time = tmp_intex_t;
}
}
indext = index_time;
if (!d_step_two)
{
doppler = -static_cast<int>(doppler_max) + doppler_step * static_cast<int>(index_doppler);
}
else
{
doppler = static_cast<int>(d_doppler_center_step_two + (index_doppler - (acq_parameters.num_doppler_bins_step2 / 2.0) * acq_parameters.doppler_step2));
}
// Find 1 chip wide code phase exclude range around the peak
int32_t excludeRangeIndex1 = index_time - d_samplesPerChip;
int32_t excludeRangeIndex2 = index_time + d_samplesPerChip;
// Correct code phase exclude range if the range includes array boundaries
if (excludeRangeIndex1 < 0)
{
excludeRangeIndex1 = d_fft_size + excludeRangeIndex1;
}
else if (excludeRangeIndex2 >= static_cast<int>(d_fft_size))
{
excludeRangeIndex2 = excludeRangeIndex2 - d_fft_size;
}
int32_t idx = excludeRangeIndex1;
memcpy(d_tmp_buffer, d_magnitude_grid[index_doppler], d_fft_size);
do
{
d_tmp_buffer[idx] = 0.0;
idx++;
if (idx == static_cast<int>(d_fft_size)) idx = 0;
}
while (idx != excludeRangeIndex2);
// Find the second highest correlation peak in the same freq. bin ---
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_tmp_buffer, d_fft_size);
float secondPeak = d_tmp_buffer[tmp_intex_t];
// Compute the test statistics and compare to the threshold
return firstPeak / secondPeak;
}
void pcps_acquisition::acquisition_core(unsigned long int samp_count)
{
gr::thread::scoped_lock lk(d_setlock);
// initialize acquisition algorithm
int doppler = 0;
uint32_t indext = 0;
float magt = 0.0;
const gr_complex* in = d_data_buffer; // Get the input samples pointer
int effective_fft_size = (acq_parameters.bit_transition_flag ? d_fft_size / 2 : d_fft_size);
if (d_cshort)
{
volk_gnsssdr_16ic_convert_32fc(d_data_buffer, d_data_buffer_sc, d_fft_size);
volk_gnsssdr_16ic_convert_32fc(d_data_buffer, d_data_buffer_sc, d_consumed_samples);
}
float fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
memcpy(d_input_signal, d_data_buffer, d_consumed_samples * sizeof(gr_complex));
if (d_fft_size > d_consumed_samples)
{
for (unsigned int i = d_consumed_samples; i < d_fft_size; i++)
{
d_input_signal[i] = gr_complex(0.0, 0.0);
}
}
const gr_complex* in = d_input_signal; // Get the input samples pointer
d_input_power = 0.0;
d_mag = 0.0;
d_well_count++;
d_num_noncoherent_integrations_counter++;
DLOG(INFO) << "Channel: " << d_channel
<< " , doing acquisition of satellite: " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN
<< " ,sample stamp: " << samp_count << ", threshold: "
<< d_threshold << ", doppler_max: " << acq_parameters.doppler_max
<< ", doppler_step: " << d_doppler_step
<< ", use_CFAR_algorithm_flag: " << (acq_parameters.use_CFAR_algorithm_flag ? "true" : "false");
<< ", use_CFAR_algorithm_flag: " << (d_use_CFAR_algorithm_flag ? "true" : "false");
lk.unlock();
if (acq_parameters.use_CFAR_algorithm_flag)
if (d_use_CFAR_algorithm_flag or acq_parameters.bit_transition_flag)
{
// 1- (optional) Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_magnitude, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, d_fft_size);
// Compute the input signal power estimation
volk_32fc_magnitude_squared_32f(d_tmp_buffer, in, d_fft_size);
volk_32f_accumulator_s32f(&d_input_power, d_tmp_buffer, d_fft_size);
d_input_power /= static_cast<float>(d_fft_size);
}
// 2- Doppler frequency search loop
// Doppler frequency grid loop
if (!d_step_two)
{
for (unsigned int doppler_index = 0; doppler_index < d_num_doppler_bins; doppler_index++)
{
// doppler search steps
int doppler = -static_cast<int>(acq_parameters.doppler_max) + d_doppler_step * doppler_index;
// Remove Doppler
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute();
// Multiply carrier wiped--off, Fourier transformed incoming signal
// with the local FFT'd code reference using SIMD operations with VOLK library
// Multiply carrier wiped--off, Fourier transformed incoming signal with the local FFT'd code reference
volk_32fc_x2_multiply_32fc(d_ifft->get_inbuf(), d_fft_if->get_outbuf(), d_fft_codes, d_fft_size);
// compute the inverse FFT
// Compute the inverse FFT
d_ifft->execute();
// Search maximum
// Compute squared magnitude (and accumulate in case of non-coherent integration)
size_t offset = (acq_parameters.bit_transition_flag ? effective_fft_size : 0);
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size);
magt = d_magnitude[indext];
if (acq_parameters.use_CFAR_algorithm_flag)
if (d_num_noncoherent_integrations_counter == 1)
{
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index], d_ifft->get_outbuf() + offset, effective_fft_size);
}
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
else
{
d_mag = magt;
if (!acq_parameters.use_CFAR_algorithm_flag)
{
// Search grid noise floor approximation for this doppler line
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size);
d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1);
}
// In case that acq_parameters.bit_transition_flag = true, we compare the potentially
// new maximum test statistics (d_mag/d_input_power) with the value in
// d_test_statistics. When the second dwell is being processed, the value
// of d_mag/d_input_power could be lower than d_test_statistics (i.e,
// the maximum test statistics in the previous dwell is greater than
// current d_mag/d_input_power). Note that d_test_statistics is not
// restarted between consecutive dwells in multidwell operation.
if (d_test_statistics < (d_mag / d_input_power) or !acq_parameters.bit_transition_flag)
{
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % acq_parameters.samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samp_count;
// 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
d_test_statistics = d_mag / d_input_power;
}
volk_32fc_magnitude_squared_32f(d_tmp_buffer, d_ifft->get_outbuf() + offset, effective_fft_size);
volk_32f_x2_add_32f(d_magnitude_grid[doppler_index], d_magnitude_grid[doppler_index], d_tmp_buffer, effective_fft_size);
}
// Record results to file if required
if (acq_parameters.dump and d_channel == d_dump_channel)
{
memcpy(grid_.colptr(doppler_index), d_magnitude, sizeof(float) * effective_fft_size);
memcpy(grid_.colptr(doppler_index), d_magnitude_grid[doppler_index], sizeof(float) * effective_fft_size);
}
}
// Compute the test statistic
if (d_use_CFAR_algorithm_flag)
{
d_test_statistics = max_to_input_power_statistic(indext, doppler, d_input_power, d_num_doppler_bins, acq_parameters.doppler_max, d_doppler_step);
}
else
{
d_test_statistics = first_vs_second_peak_statistic(indext, doppler, d_num_doppler_bins, acq_parameters.doppler_max, d_doppler_step);
}
d_gnss_synchro->Acq_delay_samples = static_cast<double>(std::fmod(static_cast<float>(indext), acq_parameters.samples_per_code));
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samp_count;
}
else
{
for (unsigned int doppler_index = 0; doppler_index < acq_parameters.num_doppler_bins_step2; doppler_index++)
{
// doppler search steps
float doppler = d_doppler_center_step_two + (static_cast<float>(doppler_index) - static_cast<float>(acq_parameters.num_doppler_bins_step2) / 2.0) * acq_parameters.doppler_step2;
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs_step_two[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
@@ -543,55 +671,31 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count)
// compute the inverse FFT
d_ifft->execute();
// Search maximum
size_t offset = (acq_parameters.bit_transition_flag ? effective_fft_size : 0);
volk_32fc_magnitude_squared_32f(d_magnitude, d_ifft->get_outbuf() + offset, effective_fft_size);
volk_gnsssdr_32f_index_max_32u(&indext, d_magnitude, effective_fft_size);
magt = d_magnitude[indext];
if (acq_parameters.use_CFAR_algorithm_flag)
if (d_num_noncoherent_integrations_counter == 1)
{
// Normalize the maximum value to correct the scale factor introduced by FFTW
magt = d_magnitude[indext] / (fft_normalization_factor * fft_normalization_factor);
volk_32fc_magnitude_squared_32f(d_magnitude_grid[doppler_index], d_ifft->get_outbuf() + offset, effective_fft_size);
}
// 4- record the maximum peak and the associated synchronization parameters
if (d_mag < magt)
else
{
d_mag = magt;
if (!acq_parameters.use_CFAR_algorithm_flag)
{
// Search grid noise floor approximation for this doppler line
volk_32f_accumulator_s32f(&d_input_power, d_magnitude, effective_fft_size);
d_input_power = (d_input_power - d_mag) / (effective_fft_size - 1);
}
// In case that acq_parameters.bit_transition_flag = true, we compare the potentially
// new maximum test statistics (d_mag/d_input_power) with the value in
// d_test_statistics. When the second dwell is being processed, the value
// of d_mag/d_input_power could be lower than d_test_statistics (i.e,
// the maximum test statistics in the previous dwell is greater than
// current d_mag/d_input_power). Note that d_test_statistics is not
// restarted between consecutive dwells in multidwell operation.
if (d_test_statistics < (d_mag / d_input_power) or !acq_parameters.bit_transition_flag)
{
d_gnss_synchro->Acq_delay_samples = static_cast<double>(indext % acq_parameters.samples_per_code);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samp_count;
// 5- Compute the test statistics and compare to the threshold
//d_test_statistics = 2 * d_fft_size * d_mag / d_input_power;
d_test_statistics = d_mag / d_input_power;
}
}
// Record results to file if required
if (acq_parameters.dump and d_channel == d_dump_channel)
{
memcpy(grid_.colptr(doppler_index), d_magnitude, sizeof(float) * effective_fft_size);
volk_32fc_magnitude_squared_32f(d_tmp_buffer, d_ifft->get_outbuf() + offset, effective_fft_size);
volk_32f_x2_add_32f(d_magnitude_grid[doppler_index], d_magnitude_grid[doppler_index], d_tmp_buffer, effective_fft_size);
}
}
// Compute the test statistic
if (d_use_CFAR_algorithm_flag)
{
d_test_statistics = max_to_input_power_statistic(indext, doppler, d_input_power, acq_parameters.num_doppler_bins_step2, static_cast<int>(d_doppler_center_step_two - (static_cast<float>(acq_parameters.num_doppler_bins_step2) / 2.0) * acq_parameters.doppler_step2), acq_parameters.doppler_step2);
}
else
{
d_test_statistics = first_vs_second_peak_statistic(indext, doppler, acq_parameters.num_doppler_bins_step2, static_cast<int>(d_doppler_center_step_two - (static_cast<float>(acq_parameters.num_doppler_bins_step2) / 2.0) * acq_parameters.doppler_step2), acq_parameters.doppler_step2);
}
d_gnss_synchro->Acq_delay_samples = static_cast<double>(std::fmod(static_cast<float>(indext), acq_parameters.samples_per_code));
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(doppler);
d_gnss_synchro->Acq_samplestamp_samples = samp_count;
}
lk.lock();
if (!acq_parameters.bit_transition_flag)
{
@@ -618,12 +722,13 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count)
d_state = 0; // Positive acquisition
}
}
else if (d_well_count == acq_parameters.max_dwells)
if (d_num_noncoherent_integrations_counter == acq_parameters.max_dwells)
{
if (d_state != 0) send_negative_acquisition();
d_state = 0;
d_active = false;
d_step_two = false;
send_negative_acquisition();
}
}
else
@@ -659,10 +764,24 @@ void pcps_acquisition::acquisition_core(unsigned long int samp_count)
}
}
d_worker_active = false;
// Record results to file if required
if (acq_parameters.dump and d_channel == d_dump_channel)
if ((d_num_noncoherent_integrations_counter == acq_parameters.max_dwells) or (d_positive_acq == 1))
{
pcps_acquisition::dump_results(effective_fft_size);
// Record results to file if required
if (acq_parameters.dump and d_channel == d_dump_channel)
{
pcps_acquisition::dump_results(effective_fft_size);
}
d_num_noncoherent_integrations_counter = 0;
d_positive_acq = 0;
// Reset grid
for (unsigned int i = 0; i < d_num_doppler_bins; i++)
{
for (unsigned k = 0; k < d_fft_size; k++)
{
d_magnitude_grid[i][k] = 0.0;
}
}
}
}
@@ -681,13 +800,12 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)),
* 5. Compute the test statistics and compare to the threshold
* 6. Declare positive or negative acquisition using a message port
*/
gr::thread::scoped_lock lk(d_setlock);
if (!d_active or d_worker_active)
{
if (!acq_parameters.blocking_on_standby)
{
d_sample_counter += d_fft_size * ninput_items[0];
d_sample_counter += d_consumed_samples * ninput_items[0];
consume_each(ninput_items[0]);
}
if (d_step_two)
@@ -708,14 +826,13 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)),
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_well_count = 0;
d_mag = 0.0;
d_input_power = 0.0;
d_test_statistics = 0.0;
d_state = 1;
if (!acq_parameters.blocking_on_standby)
{
d_sample_counter += d_fft_size * ninput_items[0]; // sample counter
d_sample_counter += d_consumed_samples * ninput_items[0]; // sample counter
consume_each(ninput_items[0]);
}
break;
@@ -726,11 +843,11 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)),
// Copy the data to the core and let it know that new data is available
if (d_cshort)
{
memcpy(d_data_buffer_sc, input_items[0], d_fft_size * sizeof(lv_16sc_t));
memcpy(d_data_buffer_sc, input_items[0], d_consumed_samples * sizeof(lv_16sc_t));
}
else
{
memcpy(d_data_buffer, input_items[0], d_fft_size * sizeof(gr_complex));
memcpy(d_data_buffer, input_items[0], d_consumed_samples * sizeof(gr_complex));
}
if (acq_parameters.blocking)
{
@@ -742,7 +859,7 @@ int pcps_acquisition::general_work(int noutput_items __attribute__((unused)),
gr::thread::thread d_worker(&pcps_acquisition::acquisition_core, this, d_sample_counter);
d_worker_active = true;
}
d_sample_counter += d_fft_size;
d_sample_counter += d_consumed_samples;
consume_each(1);
break;
}

13
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition.h
View File

@@ -95,24 +95,33 @@ private:
void dump_results(int effective_fft_size);
float first_vs_second_peak_statistic(uint32_t& indext, int& doppler, unsigned int num_doppler_bins, int doppler_max, int doppler_step);
float max_to_input_power_statistic(uint32_t& indext, int& doppler, float input_power, unsigned int num_doppler_bins, int doppler_max, int doppler_step);
Acq_Conf acq_parameters;
bool d_active;
bool d_worker_active;
bool d_cshort;
bool d_step_two;
bool d_use_CFAR_algorithm_flag;
int d_positive_acq;
float d_threshold;
float d_mag;
float d_input_power;
float d_test_statistics;
float* d_magnitude;
float** d_magnitude_grid;
float* d_tmp_buffer;
gr_complex* d_input_signal;
uint32_t d_samplesPerChip;
long d_old_freq;
int d_state;
unsigned int d_channel;
unsigned int d_doppler_step;
float d_doppler_center_step_two;
unsigned int d_well_count;
unsigned int d_num_noncoherent_integrations_counter;
unsigned int d_fft_size;
unsigned int d_consumed_samples;
unsigned int d_num_doppler_bins;
unsigned long int d_sample_counter;
gr_complex** d_grid_doppler_wipeoffs;
@@ -150,7 +159,7 @@ public:
}
/*!
* \brief Initializes acquisition algorithm.
* \brief Initializes acquisition algorithm and reserves memory.
*/
void init();

408
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.cc
View File

@@ -40,46 +40,41 @@
#include <volk_gnsssdr/volk_gnsssdr.h>
#include <algorithm> // std::rotate, std::fill_n
#include <sstream>
#include <matio.h>
using google::LogMessage;
pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min,
long fs_in, int samples_per_ms, bool dump,
std::string dump_filename)
pcps_acquisition_fine_doppler_cc_sptr pcps_make_acquisition_fine_doppler_cc(const Acq_Conf &conf_)
{
return pcps_acquisition_fine_doppler_cc_sptr(
new pcps_acquisition_fine_doppler_cc(max_dwells, sampled_ms, doppler_max, doppler_min,
fs_in, samples_per_ms, dump, dump_filename));
new pcps_acquisition_fine_doppler_cc(conf_));
}
pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
int max_dwells, unsigned int sampled_ms, int doppler_max, int doppler_min,
long fs_in, int samples_per_ms, bool dump,
std::string dump_filename) : gr::block("pcps_acquisition_fine_doppler_cc",
gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(0, 0, sizeof(gr_complex)))
pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(const Acq_Conf &conf_)
: gr::block("pcps_acquisition_fine_doppler_cc",
gr::io_signature::make(1, 1, sizeof(gr_complex)),
gr::io_signature::make(0, 0, sizeof(gr_complex)))
{
this->message_port_register_out(pmt::mp("events"));
acq_parameters = conf_;
d_sample_counter = 0; // SAMPLE COUNTER
d_active = false;
d_fs_in = fs_in;
d_samples_per_ms = samples_per_ms;
d_sampled_ms = sampled_ms;
d_config_doppler_max = doppler_max;
d_config_doppler_min = doppler_min;
d_fft_size = d_sampled_ms * d_samples_per_ms;
d_fs_in = conf_.fs_in;
d_samples_per_ms = conf_.samples_per_ms;
d_config_doppler_max = conf_.doppler_max;
d_fft_size = d_samples_per_ms;
// HS Acquisition
d_max_dwells = max_dwells;
d_max_dwells = conf_.max_dwells;
d_gnuradio_forecast_samples = d_fft_size;
d_input_power = 0.0;
d_state = 0;
d_carrier = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_fft_codes = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
d_magnitude = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
d_10_ms_buffer = static_cast<gr_complex *>(volk_gnsssdr_malloc(50 * d_samples_per_ms * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
// Direct FFT
d_fft_if = new gr::fft::fft_complex(d_fft_size, true);
@@ -87,10 +82,10 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
d_ifft = new gr::fft::fft_complex(d_fft_size, false);
// For dumping samples into a file
d_dump = dump;
d_dump_filename = dump_filename;
d_dump = conf_.dump;
d_dump_filename = conf_.dump_filename;
d_doppler_resolution = 0;
d_n_samples_in_buffer = 0;
d_threshold = 0;
d_num_doppler_points = 0;
d_doppler_step = 0;
@@ -102,21 +97,46 @@ pcps_acquisition_fine_doppler_cc::pcps_acquisition_fine_doppler_cc(
d_test_statistics = 0;
d_well_count = 0;
d_channel = 0;
d_positive_acq = 0;
d_dump_number = 0;
d_dump_channel = 0; // this implementation can only produce dumps in channel 0
//todo: migrate config parameters to the unified acquisition config class
}
// Finds next power of two
// for n. If n itself is a
// power of two then returns n
unsigned int pcps_acquisition_fine_doppler_cc::nextPowerOf2(unsigned int n)
{
n--;
n |= n >> 1;
n |= n >> 2;
n |= n >> 4;
n |= n >> 8;
n |= n >> 16;
n++;
return n;
}
void pcps_acquisition_fine_doppler_cc::set_doppler_step(unsigned int doppler_step)
{
d_doppler_step = doppler_step;
// Create the search grid array
d_num_doppler_points = floor(std::abs(d_config_doppler_max - d_config_doppler_min) / d_doppler_step);
d_num_doppler_points = floor(std::abs(2 * d_config_doppler_max) / d_doppler_step);
d_grid_data = new float *[d_num_doppler_points];
for (int i = 0; i < d_num_doppler_points; i++)
{
d_grid_data[i] = static_cast<float *>(volk_gnsssdr_malloc(d_fft_size * sizeof(float), volk_gnsssdr_get_alignment()));
}
if (d_dump)
{
grid_ = arma::fmat(d_fft_size, d_num_doppler_points, arma::fill::zeros);
}
update_carrier_wipeoff();
}
@@ -138,12 +158,9 @@ pcps_acquisition_fine_doppler_cc::~pcps_acquisition_fine_doppler_cc()
volk_gnsssdr_free(d_carrier);
volk_gnsssdr_free(d_fft_codes);
volk_gnsssdr_free(d_magnitude);
volk_gnsssdr_free(d_10_ms_buffer);
delete d_ifft;
delete d_fft_if;
if (d_dump)
{
d_dump_file.close();
}
free_grid_memory();
}
@@ -167,7 +184,6 @@ void pcps_acquisition_fine_doppler_cc::init()
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_input_power = 0.0;
d_state = 0;
}
@@ -187,6 +203,7 @@ void pcps_acquisition_fine_doppler_cc::reset_grid()
d_well_count = 0;
for (int i = 0; i < d_num_doppler_points; i++)
{
//todo: use memset here
for (unsigned int j = 0; j < d_fft_size; j++)
{
d_grid_data[i][j] = 0.0;
@@ -203,7 +220,7 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
d_grid_doppler_wipeoffs = new gr_complex *[d_num_doppler_points];
for (int doppler_index = 0; doppler_index < d_num_doppler_points; doppler_index++)
{
doppler_hz = d_config_doppler_min + d_doppler_step * doppler_index;
doppler_hz = d_doppler_step * doppler_index - d_config_doppler_max;
// doppler search steps
// compute the carrier doppler wipe-off signal and store it
phase_step_rad = static_cast<float>(GPS_TWO_PI) * doppler_hz / static_cast<float>(d_fs_in);
@@ -215,52 +232,70 @@ void pcps_acquisition_fine_doppler_cc::update_carrier_wipeoff()
}
double pcps_acquisition_fine_doppler_cc::search_maximum()
double pcps_acquisition_fine_doppler_cc::compute_CAF()
{
float magt = 0.0;
float fft_normalization_factor;
float firstPeak = 0.0;
int index_doppler = 0;
uint32_t tmp_intex_t = 0;
uint32_t index_time = 0;
// Look for correlation peaks in the results ==============================
// Find the highest peak and compare it to the second highest peak
// The second peak is chosen not closer than 1 chip to the highest peak
//--- Find the correlation peak and the carrier frequency --------------
for (int i = 0; i < d_num_doppler_points; i++)
{
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[i], d_fft_size);
if (d_grid_data[i][tmp_intex_t] > magt)
if (d_grid_data[i][tmp_intex_t] > firstPeak)
{
magt = d_grid_data[i][tmp_intex_t];
//std::cout<<magt<<std::endl;
firstPeak = d_grid_data[i][tmp_intex_t];
index_doppler = i;
index_time = tmp_intex_t;
}
// Record results to file if required
if (d_dump and d_channel == d_dump_channel)
{
memcpy(grid_.colptr(i), d_grid_data[i], sizeof(float) * d_fft_size);
}
}
// Normalize the maximum value to correct the scale factor introduced by FFTW
fft_normalization_factor = static_cast<float>(d_fft_size) * static_cast<float>(d_fft_size);
magt = magt / (fft_normalization_factor * fft_normalization_factor);
// -- - Find 1 chip wide code phase exclude range around the peak
uint32_t samplesPerChip = ceil(GPS_L1_CA_CHIP_PERIOD * static_cast<float>(this->d_fs_in));
int32_t excludeRangeIndex1 = index_time - samplesPerChip;
int32_t excludeRangeIndex2 = index_time + samplesPerChip;
// -- - Correct code phase exclude range if the range includes array boundaries
if (excludeRangeIndex1 < 0)
{
excludeRangeIndex1 = d_fft_size + excludeRangeIndex1;
}
else if (excludeRangeIndex2 >= static_cast<int>(d_fft_size))
{
excludeRangeIndex2 = excludeRangeIndex2 - d_fft_size;
}
int32_t idx = excludeRangeIndex1;
do
{
d_grid_data[index_doppler][idx] = 0.0;
idx++;
if (idx == static_cast<int>(d_fft_size)) idx = 0;
}
while (idx != excludeRangeIndex2);
//--- Find the second highest correlation peak in the same freq. bin ---
volk_gnsssdr_32f_index_max_32u(&tmp_intex_t, d_grid_data[index_doppler], d_fft_size);
float secondPeak = d_grid_data[index_doppler][tmp_intex_t];
// 5- Compute the test statistics and compare to the threshold
d_test_statistics = magt / (d_input_power * std::sqrt(d_well_count));
d_test_statistics = firstPeak / secondPeak;
// 4- record the maximum peak and the associated synchronization parameters
d_gnss_synchro->Acq_delay_samples = static_cast<double>(index_time);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step + d_config_doppler_min);
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(index_doppler * d_doppler_step - d_config_doppler_max);
d_gnss_synchro->Acq_samplestamp_samples = d_sample_counter;
// Record results to file if required
if (d_dump)
{
std::stringstream filename;
std::streamsize n = 2 * sizeof(float) * (d_fft_size); // complex file write
filename.str("");
filename << "../data/test_statistics_" << d_gnss_synchro->System
<< "_" << d_gnss_synchro->Signal << "_sat_"
<< d_gnss_synchro->PRN << "_doppler_" << d_gnss_synchro->Acq_doppler_hz << ".dat";
d_dump_file.open(filename.str().c_str(), std::ios::out | std::ios::binary);
d_dump_file.write(reinterpret_cast<char *>(d_grid_data[index_doppler]), n); //write directly |abs(x)|^2 in this Doppler bin?
d_dump_file.close();
}
return d_test_statistics;
}
@@ -296,6 +331,7 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons
// doppler search steps
// Perform the carrier wipe-off
volk_32fc_x2_multiply_32fc(d_fft_if->get_inbuf(), in, d_grid_doppler_wipeoffs[doppler_index], d_fft_size);
// 3- Perform the FFT-based convolution (parallel time search)
// Compute the FFT of the carrier wiped--off incoming signal
d_fft_if->execute();
@@ -308,29 +344,38 @@ int pcps_acquisition_fine_doppler_cc::compute_and_accumulate_grid(gr_vector_cons
d_ifft->execute();
// save the grid matrix delay file
volk_32fc_magnitude_squared_32f(p_tmp_vector, d_ifft->get_outbuf(), d_fft_size);
const float *old_vector = d_grid_data[doppler_index];
volk_32f_x2_add_32f(d_grid_data[doppler_index], old_vector, p_tmp_vector, d_fft_size);
//accumulate grid values
volk_32f_x2_add_32f(d_grid_data[doppler_index], d_grid_data[doppler_index], p_tmp_vector, d_fft_size);
}
volk_gnsssdr_free(p_tmp_vector);
return d_fft_size;
//debug
// std::cout << "iff=[";
// for (int n = 0; n < d_fft_size; n++)
// {
// std::cout << std::real(d_ifft->get_outbuf()[n]) << "+" << std::imag(d_ifft->get_outbuf()[n]) << "i,";
// }
// std::cout << "]\n";
// getchar();
}
int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star &input_items)
int pcps_acquisition_fine_doppler_cc::estimate_Doppler()
{
// Direct FFT
int zero_padding_factor = 2;
int fft_size_extended = d_fft_size * zero_padding_factor;
int zero_padding_factor = 8;
int prn_replicas = 10;
int signal_samples = prn_replicas * d_fft_size;
//int fft_size_extended = nextPowerOf2(signal_samples * zero_padding_factor);
int fft_size_extended = signal_samples * zero_padding_factor;
gr::fft::fft_complex *fft_operator = new gr::fft::fft_complex(fft_size_extended, true);
//zero padding the entire vector
std::fill_n(fft_operator->get_inbuf(), fft_size_extended, gr_complex(0.0, 0.0));
//1. generate local code aligned with the acquisition code phase estimation
gr_complex *code_replica = static_cast<gr_complex *>(volk_gnsssdr_malloc(d_fft_size * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
gr_complex *code_replica = static_cast<gr_complex *>(volk_gnsssdr_malloc(signal_samples * sizeof(gr_complex), volk_gnsssdr_get_alignment()));
gps_l1_ca_code_gen_complex_sampled(code_replica, d_gnss_synchro->PRN, d_fs_in, 0);
@@ -342,10 +387,12 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
std::rotate(code_replica, code_replica + (d_fft_size - shift_index), code_replica + d_fft_size - 1);
}
for (int n = 0; n < prn_replicas - 1; n++)
{
memcpy(&code_replica[(n + 1) * d_fft_size], code_replica, d_fft_size * sizeof(gr_complex));
}
//2. Perform code wipe-off
const gr_complex *in = reinterpret_cast<const gr_complex *>(input_items[0]); //Get the input samples pointer
volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), in, code_replica, d_fft_size);
volk_32fc_x2_multiply_32fc(fft_operator->get_inbuf(), d_10_ms_buffer, code_replica, signal_samples);
// 3. Perform the FFT (zero padded!)
fft_operator->execute();
@@ -360,8 +407,8 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
//case even
int counter = 0;
float *fftFreqBins = new float[fft_size_extended];
float fftFreqBins[fft_size_extended];
std::fill_n(fftFreqBins, fft_size_extended, 0.0);
for (int k = 0; k < (fft_size_extended / 2); k++)
@@ -372,7 +419,7 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
for (int k = fft_size_extended / 2; k > 0; k--)
{
fftFreqBins[counter] = ((-static_cast<float>(d_fs_in) / 2) * static_cast<float>(k)) / (static_cast<float>(fft_size_extended) / 2.0);
fftFreqBins[counter] = ((-static_cast<float>(d_fs_in) / 2.0) * static_cast<float>(k)) / (static_cast<float>(fft_size_extended) / 2.0);
counter++;
}
@@ -380,50 +427,56 @@ int pcps_acquisition_fine_doppler_cc::estimate_Doppler(gr_vector_const_void_star
if (std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz) < 1000)
{
d_gnss_synchro->Acq_doppler_hz = static_cast<double>(fftFreqBins[tmp_index_freq]);
//std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
//std::cout << "FFT maximum present at " << fftFreqBins[tmp_index_freq] << " [Hz]" << std::endl;
}
else
{
DLOG(INFO) << "Abs(Grid Doppler - FFT Doppler)=" << std::abs(fftFreqBins[tmp_index_freq] - d_gnss_synchro->Acq_doppler_hz);
DLOG(INFO) << "Error estimating fine frequency Doppler";
//debug log
//
// std::cout<<"FFT maximum present at "<<fftFreqBins[tmp_index_freq]<<" [Hz]"<<std::endl;
// std::stringstream filename;
// std::streamsize n = sizeof(gr_complex) * (d_fft_size);
//
// filename.str("");
// filename << "../data/code_prn_" << d_gnss_synchro->PRN << ".dat";
// d_dump_file.open(filename.str().c_str(), std::ios::out
// | std::ios::binary);
// d_dump_file.write(reinterpret_cast<char*>(code_replica), n); //write directly |abs(x)|^2 in this Doppler bin?
// d_dump_file.close();
//
// filename.str("");
// filename << "../data/signal_prn_" << d_gnss_synchro->PRN << ".dat";
// d_dump_file.open(filename.str().c_str(), std::ios::out
// | std::ios::binary);
// d_dump_file.write(reinterpret_cast<char*>(in), n); //write directly |abs(x)|^2 in this Doppler bin?
// d_dump_file.close();
//
//
// n = sizeof(float) * (fft_size_extended);
// filename.str("");
// filename << "../data/fft_prn_" << d_gnss_synchro->PRN << ".dat";
// d_dump_file.open(filename.str().c_str(), std::ios::out
// | std::ios::binary);
// d_dump_file.write(reinterpret_cast<char*>(p_tmp_vector), n); //write directly |abs(x)|^2 in this Doppler bin?
// d_dump_file.close();
}
// free memory!!
delete fft_operator;
volk_gnsssdr_free(code_replica);
volk_gnsssdr_free(p_tmp_vector);
delete[] fftFreqBins;
return d_fft_size;
}
// Called by gnuradio to enable drivers, etc for i/o devices.
bool pcps_acquisition_fine_doppler_cc::start()
{
d_sample_counter = 0;
return true;
}
void pcps_acquisition_fine_doppler_cc::set_state(int state)
{
//gr::thread::scoped_lock lock(d_setlock); // require mutex with work function called by the scheduler
d_state = state;
if (d_state == 1)
{
d_gnss_synchro->Acq_delay_samples = 0.0;
d_gnss_synchro->Acq_doppler_hz = 0.0;
d_gnss_synchro->Acq_samplestamp_samples = 0;
d_well_count = 0;
d_test_statistics = 0.0;
d_active = true;
reset_grid();
}
else if (d_state == 0)
{
}
else
{
LOG(ERROR) << "State can only be set to 0 or 1";
}
}
int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items,
gr_vector_void_star &output_items __attribute__((unused)))
@@ -442,29 +495,36 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
* S5. Negative_Acq: Send message and stop acq -> S0
*/
int samples_remaining;
switch (d_state)
{
case 0: // S0. StandBy
//DLOG(INFO) <<"S0"<<std::endl;
if (d_active == true)
{
reset_grid();
d_n_samples_in_buffer = 0;
d_state = 1;
}
if (!acq_parameters.blocking_on_standby)
{
d_sample_counter += d_fft_size; // sample counter
consume_each(d_fft_size);
}
break;
case 1: // S1. ComputeGrid
//DLOG(INFO) <<"S1"<<std::endl;
compute_and_accumulate_grid(input_items);
memcpy(&d_10_ms_buffer[d_n_samples_in_buffer], reinterpret_cast<const gr_complex *>(input_items[0]), d_fft_size * sizeof(gr_complex));
d_n_samples_in_buffer += d_fft_size;
d_well_count++;
if (d_well_count >= d_max_dwells)
{
d_state = 2;
}
d_sample_counter += d_fft_size; // sample counter
consume_each(d_fft_size);
break;
case 2: // Compute test statistics and decide
//DLOG(INFO) <<"S2"<<std::endl;
d_input_power = estimate_input_power(input_items);
d_test_statistics = search_maximum();
d_test_statistics = compute_CAF();
if (d_test_statistics > d_threshold)
{
d_state = 3; //perform fine doppler estimation
@@ -472,16 +532,34 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
else
{
d_state = 5; //negative acquisition
d_n_samples_in_buffer = 0;
}
break;
case 3: // Fine doppler estimation
//DLOG(INFO) <<"S3"<<std::endl;
DLOG(INFO) << "Performing fine Doppler estimation";
estimate_Doppler(input_items); //disabled in repo
d_state = 4;
samples_remaining = 10 * d_samples_per_ms - d_n_samples_in_buffer;
if (samples_remaining > noutput_items)
{
memcpy(&d_10_ms_buffer[d_n_samples_in_buffer], reinterpret_cast<const gr_complex *>(input_items[0]), noutput_items * sizeof(gr_complex));
d_n_samples_in_buffer += noutput_items;
d_sample_counter += noutput_items; // sample counter
consume_each(noutput_items);
}
else
{
if (samples_remaining > 0)
{
memcpy(&d_10_ms_buffer[d_n_samples_in_buffer], reinterpret_cast<const gr_complex *>(input_items[0]), samples_remaining * sizeof(gr_complex));
d_sample_counter += samples_remaining; // sample counter
consume_each(samples_remaining);
}
estimate_Doppler(); //disabled in repo
d_n_samples_in_buffer = 0;
d_state = 4;
}
break;
case 4: // Positive_Acq
//DLOG(INFO) <<"S4"<<std::endl;
DLOG(INFO) << "positive acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
@@ -489,15 +567,23 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power;
d_positive_acq = 1;
d_active = false;
// Record results to file if required
if (d_dump and d_channel == d_dump_channel)
{
dump_results(d_fft_size);
}
// Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
this->message_port_pub(pmt::mp("events"), pmt::from_long(1));
d_state = 0;
if (!acq_parameters.blocking_on_standby)
{
d_sample_counter += noutput_items; // sample counter
consume_each(noutput_items);
}
break;
case 5: // Negative_Acq
//DLOG(INFO) <<"S5"<<std::endl;
DLOG(INFO) << "negative acquisition";
DLOG(INFO) << "satellite " << d_gnss_synchro->System << " " << d_gnss_synchro->PRN;
DLOG(INFO) << "sample_stamp " << d_sample_counter;
@@ -505,20 +591,108 @@ int pcps_acquisition_fine_doppler_cc::general_work(int noutput_items,
DLOG(INFO) << "test statistics threshold " << d_threshold;
DLOG(INFO) << "code phase " << d_gnss_synchro->Acq_delay_samples;
DLOG(INFO) << "doppler " << d_gnss_synchro->Acq_doppler_hz;
DLOG(INFO) << "input signal power " << d_input_power;
d_positive_acq = 0;
d_active = false;
// Record results to file if required
if (d_dump and d_channel == d_dump_channel)
{
dump_results(d_fft_size);
}
// Send message to channel port //0=STOP_CHANNEL 1=ACQ_SUCCEES 2=ACQ_FAIL
this->message_port_pub(pmt::mp("events"), pmt::from_long(2));
d_state = 0;
if (!acq_parameters.blocking_on_standby)
{
d_sample_counter += noutput_items; // sample counter
consume_each(noutput_items);
}
break;
default:
d_state = 0;
if (!acq_parameters.blocking_on_standby)
{
d_sample_counter += noutput_items; // sample counter
consume_each(noutput_items);
}
break;
}
//DLOG(INFO)<<"d_sample_counter="<<d_sample_counter<<std::endl;
d_sample_counter += d_fft_size; // sample counter
consume_each(d_fft_size);
return noutput_items;
return 0;
}
void pcps_acquisition_fine_doppler_cc::dump_results(int effective_fft_size)
{
d_dump_number++;
std::string filename = d_dump_filename;
filename.append("_");
filename.append(1, d_gnss_synchro->System);
filename.append("_");
filename.append(1, d_gnss_synchro->Signal[0]);
filename.append(1, d_gnss_synchro->Signal[1]);
filename.append("_ch_");
filename.append(std::to_string(d_channel));
filename.append("_");
filename.append(std::to_string(d_dump_number));
filename.append("_sat_");
filename.append(std::to_string(d_gnss_synchro->PRN));
filename.append(".mat");
mat_t *matfp = Mat_CreateVer(filename.c_str(), NULL, MAT_FT_MAT73);
if (matfp == NULL)
{
std::cout << "Unable to create or open Acquisition dump file" << std::endl;
d_dump = false;
}
else
{
size_t dims[2] = {static_cast<size_t>(effective_fft_size), static_cast<size_t>(d_num_doppler_points)};
matvar_t *matvar = Mat_VarCreate("acq_grid", MAT_C_SINGLE, MAT_T_SINGLE, 2, dims, grid_.memptr(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
dims[0] = static_cast<size_t>(1);
dims[1] = static_cast<size_t>(1);
matvar = Mat_VarCreate("doppler_max", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_config_doppler_max, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("doppler_step", MAT_C_UINT32, MAT_T_UINT32, 1, dims, &d_doppler_step, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("d_positive_acq", MAT_C_INT32, MAT_T_INT32, 1, dims, &d_positive_acq, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
float aux = static_cast<float>(d_gnss_synchro->Acq_doppler_hz);
matvar = Mat_VarCreate("acq_doppler_hz", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &aux, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
aux = static_cast<float>(d_gnss_synchro->Acq_delay_samples);
matvar = Mat_VarCreate("acq_delay_samples", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &aux, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("test_statistic", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &d_test_statistics, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("threshold", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &d_threshold, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
aux = 0.0;
matvar = Mat_VarCreate("input_power", MAT_C_SINGLE, MAT_T_SINGLE, 1, dims, &aux, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
matvar = Mat_VarCreate("sample_counter", MAT_C_UINT64, MAT_T_UINT64, 1, dims, &d_sample_counter, 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, 1, dims, &d_gnss_synchro->PRN, 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar);
Mat_Close(matfp);
}
}

61
src/algorithms/acquisition/gnuradio_blocks/pcps_acquisition_fine_doppler_cc.h
View File

@@ -1,8 +1,9 @@
/*!
* \file pcps_acquisition_fine_doppler_acquisition_cc.h
* \brief This class implements a Parallel Code Phase Search Acquisition with multi-dwells and fine Doppler estimation
* for GPS L1 C/A signal
*
* Acquisition strategy (Kay Borre book + CFAR threshold).
* Acquisition strategy (Kay Borre book).
* <ol>
* <li> Compute the input signal power estimation
* <li> Doppler serial search loop
@@ -49,6 +50,8 @@
#define GNSS_SDR_PCPS_ACQUISITION_FINE_DOPPLER_CC_H_
#include "gnss_synchro.h"
#include "acq_conf.h"
#include <armadillo>
#include <gnuradio/block.h>
#include <gnuradio/gr_complex.h>
#include <gnuradio/fft/fft.h>
@@ -61,59 +64,44 @@ typedef boost::shared_ptr<pcps_acquisition_fine_doppler_cc>
pcps_acquisition_fine_doppler_cc_sptr;
pcps_acquisition_fine_doppler_cc_sptr
pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long fs_in, int samples_per_ms,
bool dump, std::string dump_filename);
pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_);
/*!
* \brief This class implements a Parallel Code Phase Search Acquisition.
*
* Check \ref Navitec2012 "An Open Source Galileo E1 Software Receiver",
* Algorithm 1, for a pseudocode description of this implementation.
*/
class pcps_acquisition_fine_doppler_cc : public gr::block
{
private:
friend pcps_acquisition_fine_doppler_cc_sptr
pcps_make_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long fs_in,
int samples_per_ms, bool dump,
std::string dump_filename);
pcps_acquisition_fine_doppler_cc(int max_dwells, unsigned int sampled_ms,
int doppler_max, int doppler_min, long fs_in,
int samples_per_ms, bool dump,
std::string dump_filename);
void calculate_magnitudes(gr_complex* fft_begin, int doppler_shift,
int doppler_offset);
pcps_make_acquisition_fine_doppler_cc(const Acq_Conf& conf_);
pcps_acquisition_fine_doppler_cc(const Acq_Conf& conf_);
int compute_and_accumulate_grid(gr_vector_const_void_star& input_items);
int estimate_Doppler(gr_vector_const_void_star& input_items);
int estimate_Doppler();
float estimate_input_power(gr_vector_const_void_star& input_items);
double search_maximum();
double compute_CAF();
void reset_grid();
void update_carrier_wipeoff();
void free_grid_memory();
bool start();
Acq_Conf acq_parameters;
long d_fs_in;
int d_samples_per_ms;
int d_max_dwells;
unsigned int d_doppler_resolution;
int d_gnuradio_forecast_samples;
float d_threshold;
std::string d_satellite_str;
int d_config_doppler_max;
int d_config_doppler_min;
int d_num_doppler_points;
int d_doppler_step;
unsigned int d_sampled_ms;
unsigned int d_fft_size;
unsigned long int d_sample_counter;
gr_complex* d_carrier;
gr_complex* d_fft_codes;
gr_complex* d_10_ms_buffer;
float* d_magnitude;
float** d_grid_data;
@@ -124,17 +112,22 @@ private:
Gnss_Synchro* d_gnss_synchro;
unsigned int d_code_phase;
float d_doppler_freq;
float d_input_power;
float d_test_statistics;
std::ofstream d_dump_file;
int d_positive_acq;
int d_state;
bool d_active;
int d_well_count;
int d_n_samples_in_buffer;
bool d_dump;
unsigned int d_channel;
std::string d_dump_filename;
arma::fmat grid_;
long int d_dump_number;
unsigned int d_dump_channel;
public:
/*!
* \brief Default destructor.
@@ -187,6 +180,7 @@ public:
inline void set_channel(unsigned int channel)
{
d_channel = channel;
d_dump_channel = d_channel;
}
/*!
@@ -214,6 +208,13 @@ public:
*/
void set_doppler_step(unsigned int doppler_step);
/*!
* \brief If set to 1, ensures that acquisition starts at the
* first available sample.
* \param state - int=1 forces start of acquisition
*/
void set_state(int state);
/*!
* \brief Parallel Code Phase Search Acquisition signal processing.
*/
@@ -222,6 +223,14 @@ public:
gr_vector_void_star& output_items);
void forecast(int noutput_items, gr_vector_int& ninput_items_required);
/*!
* \brief Obtains the next power of 2 greater or equal to the input parameter
* \param n - Integer value to obtain the next power of 2.
*/
unsigned int nextPowerOf2(unsigned int n);
void dump_results(int effective_fft_size);
};
#endif /* pcps_acquisition_fine_doppler_cc*/

6
src/algorithms/acquisition/libs/acq_conf.cc
View File

@@ -35,13 +35,15 @@ Acq_Conf::Acq_Conf()
{
/* PCPS acquisition configuration */
sampled_ms = 0;
ms_per_code = 0;
max_dwells = 0;
samples_per_chip = 0;
doppler_max = 0;
num_doppler_bins_step2 = 0;
doppler_step2 = 0.0;
fs_in = 0;
samples_per_ms = 0;
samples_per_code = 0;
samples_per_ms = 0.0;
samples_per_code = 0.0;
bit_transition_flag = false;
use_CFAR_algorithm_flag = false;
dump = false;

6
src/algorithms/acquisition/libs/acq_conf.h
View File

@@ -40,13 +40,15 @@ class Acq_Conf
public:
/* PCPS Acquisition configuration */
unsigned int sampled_ms;
unsigned int ms_per_code;
unsigned int samples_per_chip;
unsigned int max_dwells;
unsigned int doppler_max;
unsigned int num_doppler_bins_step2;
float doppler_step2;
long fs_in;
int samples_per_ms;
int samples_per_code;
float samples_per_ms;
float samples_per_code;
bool bit_transition_flag;
bool use_CFAR_algorithm_flag;
bool dump;

1
src/algorithms/libs/gps_sdr_signal_processing.cc
View File

@@ -144,6 +144,7 @@ void gps_l1_ca_code_gen_complex(std::complex<float>* _dest, signed int _prn, uns
/*
* Generates complex GPS L1 C/A code for the desired SV ID and sampled to specific sampling frequency
* NOTICE: the number of samples is rounded towards zero (integer truncation)
*/
void gps_l1_ca_code_gen_complex_sampled(std::complex<float>* _dest, unsigned int _prn, signed int _fs, unsigned int _chip_shift)
{

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

@@ -83,26 +83,20 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc(
}
}
d_stat = 0;
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_flag_frame_sync = false;
d_GPS_frame_4bytes = 0;
d_prev_GPS_frame_4bytes = 0;
d_flag_parity = false;
d_TOW_at_Preamble_ms = 0;
flag_TOW_set = false;
d_flag_preamble = false;
d_flag_new_tow_available = false;
d_word_number = 0;
d_channel = 0;
flag_PLL_180_deg_phase_locked = false;
d_preamble_time_samples = 0;
d_TOW_at_current_symbol_ms = 0;
d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS + 1); // Change fixed buffer size
d_symbol_history.clear(); // Clear all the elements in the buffer
d_make_correlation = true;
d_symbol_counter_corr = 0;
d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size
d_symbol_history.clear(); // Clear all the elements in the buffer
d_crc_error_synchronization_counter = 0;
d_current_subframe_symbol = 0;
}
@@ -150,9 +144,10 @@ bool gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satellite)
{
d_nav.reset();
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
d_GPS_FSM.i_satellite_PRN = d_satellite.get_PRN();
d_nav.i_satellite_PRN = d_satellite.get_PRN();
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
}
@@ -160,7 +155,7 @@ void gps_l1_ca_telemetry_decoder_cc::set_satellite(const Gnss_Satellite &satelli
void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
{
d_channel = channel;
d_GPS_FSM.i_channel_ID = channel;
d_nav.i_channel_ID = channel;
DLOG(INFO) << "Navigation channel set to " << channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
@@ -186,10 +181,127 @@ void gps_l1_ca_telemetry_decoder_cc::set_channel(int channel)
}
bool gps_l1_ca_telemetry_decoder_cc::decode_subframe()
{
char subframe[GPS_SUBFRAME_LENGTH];
int symbol_accumulator_counter = 0;
int frame_bit_index = 0;
int word_index = 0;
uint32_t GPS_frame_4bytes = 0;
float symbol_accumulator = 0;
bool subframe_synchro_confirmation = false;
bool CRC_ok = true;
for (int n = 0; n < GPS_SUBFRAME_MS; n++)
{
//******* SYMBOL TO BIT *******
// extended correlation to bit period is enabled in tracking!
symbol_accumulator += d_subframe_symbols[n]; // accumulate the input value in d_symbol_accumulator
symbol_accumulator_counter++;
if (symbol_accumulator_counter == 20)
{
//symbol to bit
if (symbol_accumulator > 0) GPS_frame_4bytes += 1; //insert the telemetry bit in LSB
symbol_accumulator = 0;
symbol_accumulator_counter = 0;
//******* bits to words ******
frame_bit_index++;
if (frame_bit_index == 30)
{
frame_bit_index = 0;
// parity check
// Each word in wordbuff is composed of:
// Bits 0 to 29 = the GPS data word
// Bits 30 to 31 = 2 LSBs of the GPS word ahead.
// prepare the extended frame [-2 -1 0 ... 30]
if (d_prev_GPS_frame_4bytes & 0x00000001)
{
GPS_frame_4bytes = GPS_frame_4bytes | 0x40000000;
}
if (d_prev_GPS_frame_4bytes & 0x00000002)
{
GPS_frame_4bytes = GPS_frame_4bytes | 0x80000000;
}
/* Check that the 2 most recently logged words pass parity. Have to first
invert the data bits according to bit 30 of the previous word. */
if (GPS_frame_4bytes & 0x40000000)
{
GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR)
}
if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(GPS_frame_4bytes))
{
subframe_synchro_confirmation = true;
}
else
{
//std::cout << "word invalid sat " << this->d_satellite << std::endl;
CRC_ok = false;
}
//add word to subframe
// insert the word in the correct position of the subframe
std::memcpy(&subframe[word_index * GPS_WORD_LENGTH], &GPS_frame_4bytes, sizeof(uint32_t));
word_index++;
d_prev_GPS_frame_4bytes = GPS_frame_4bytes; // save the actual frame
GPS_frame_4bytes = 0;
}
else
{
GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word
}
}
}
//decode subframe
// NEW GPS SUBFRAME HAS ARRIVED!
if (CRC_ok)
{
int subframe_ID = d_nav.subframe_decoder(subframe); //decode the subframe
std::cout << "New GPS NAV message received in channel " << this->d_channel << ": "
<< "subframe "
<< subframe_ID << " from satellite "
<< Gnss_Satellite(std::string("GPS"), d_nav.i_satellite_PRN) << std::endl;
switch (subframe_ID)
{
case 3: //we have a new set of ephemeris data for the current SV
if (d_nav.satellite_validation() == true)
{
// get ephemeris object for this SV (mandatory)
std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 4: // Possible IONOSPHERE and UTC model update (page 18)
if (d_nav.flag_iono_valid == true)
{
std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(d_nav.get_iono());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_nav.flag_utc_model_valid == true)
{
std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 5:
// get almanac (if available)
//TODO: implement almanac reader in navigation_message
break;
default:
break;
}
d_flag_new_tow_available = true;
}
return subframe_synchro_confirmation;
}
int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)),
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
{
int corr_value = 0;
int preamble_diff_ms = 0;
Gnss_Synchro **out = reinterpret_cast<Gnss_Synchro **>(&output_items[0]); // Get the output buffer pointer
@@ -198,15 +310,25 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
Gnss_Synchro current_symbol; //structure to save the synchronization information and send the output object to the next block
//1. Copy the current tracking output
current_symbol = in[0][0];
//record the oldest subframe symbol before inserting a new symbol into the circular buffer
if (d_current_subframe_symbol < GPS_SUBFRAME_MS and d_symbol_history.size() > 0)
{
d_subframe_symbols[d_current_subframe_symbol] = d_symbol_history.at(0).Prompt_I;
d_current_subframe_symbol++;
}
d_symbol_history.push_back(current_symbol); //add new symbol to the symbol queue
consume_each(1);
unsigned int required_symbols = GPS_CA_PREAMBLE_LENGTH_SYMBOLS;
d_flag_preamble = false;
if ((d_symbol_history.size() > required_symbols) and (d_make_correlation or !d_flag_frame_sync))
//******* preamble correlation ********
int corr_value = 0;
if ((d_symbol_history.size() == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)) // and (d_make_correlation or !d_flag_frame_sync))
{
//******* preamble correlation ********
// std::cout << "-------\n";
for (unsigned int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++)
{
if (d_symbol_history.at(i).Flag_valid_symbol_output == true)
@@ -221,38 +343,27 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
}
}
}
if (std::abs(corr_value) >= GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
{
d_symbol_counter_corr++;
}
}
//******* frame sync ******************
if (std::abs(corr_value) == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
{
//TODO: Rewrite with state machine
if (d_stat == 0)
{
d_GPS_FSM.Event_gps_word_preamble();
//record the preamble sample stamp
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the preamble sample stamp
DLOG(INFO) << "Preamble detection for SAT " << this->d_satellite << "d_symbol_history.at(0).Tracking_sample_counter=" << d_symbol_history.at(0).Tracking_sample_counter;
//sync the symbol to bits integrator
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_stat = 1; // enter into frame pre-detection status
}
else if (d_stat == 1) //check 6 seconds of preamble separation
{
preamble_diff_ms = std::round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) < 1)
if (std::abs(preamble_diff_ms - GPS_SUBFRAME_MS) % GPS_SUBFRAME_MS == 0)
{
DLOG(INFO) << "Preamble confirmation for SAT " << this->d_satellite;
d_GPS_FSM.Event_gps_word_preamble();
d_flag_preamble = true;
d_make_correlation = false;
d_symbol_counter_corr = 0;
d_preamble_time_samples = d_symbol_history.at(0).Tracking_sample_counter; // record the PRN start sample index associated to the preamble
if (!d_flag_frame_sync)
{
@@ -269,131 +380,46 @@ int gps_l1_ca_telemetry_decoder_cc::general_work(int noutput_items __attribute__
DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at "
<< static_cast<double>(d_preamble_time_samples) / static_cast<double>(d_symbol_history.at(0).fs) << " [s]";
}
//try to decode the subframe:
if (decode_subframe() == false)
{
d_crc_error_synchronization_counter++;
if (d_crc_error_synchronization_counter > 3)
{
DLOG(INFO) << "TOO MANY CRC ERRORS: Lost of frame sync SAT " << this->d_satellite << std::endl;
d_stat = 0; //lost of frame sync
d_flag_frame_sync = false;
flag_TOW_set = false;
d_crc_error_synchronization_counter = 0;
}
}
d_current_subframe_symbol = 0;
}
}
}
else
{
d_symbol_counter_corr++;
if (d_symbol_counter_corr > (GPS_SUBFRAME_MS - GPS_CA_TELEMETRY_SYMBOLS_PER_BIT))
{
d_make_correlation = true;
}
if (d_stat == 1)
{
preamble_diff_ms = round(((static_cast<double>(d_symbol_history.at(0).Tracking_sample_counter) - static_cast<double>(d_preamble_time_samples)) / static_cast<double>(d_symbol_history.at(0).fs)) * 1000.0);
if (preamble_diff_ms > GPS_SUBFRAME_MS + 1)
if (preamble_diff_ms > GPS_SUBFRAME_MS)
{
DLOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms;
// std::cout << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff_ms << std::endl;
d_stat = 0; //lost of frame sync
d_flag_frame_sync = false;
flag_TOW_set = false;
d_make_correlation = true;
d_symbol_counter_corr = 0;
d_current_subframe_symbol = 0;
d_crc_error_synchronization_counter = 0;
}
}
}
//******* SYMBOL TO BIT *******
if (d_symbol_history.at(0).Flag_valid_symbol_output == true)
{
// extended correlation to bit period is enabled in tracking!
d_symbol_accumulator += d_symbol_history.at(0).Prompt_I; // accumulate the input value in d_symbol_accumulator
d_symbol_accumulator_counter += d_symbol_history.at(0).correlation_length_ms;
}
if (d_symbol_accumulator_counter >= 20)
{
if (d_symbol_accumulator > 0)
{ //symbol to bit
d_GPS_frame_4bytes += 1; //insert the telemetry bit in LSB
}
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
//******* bits to words ******
d_frame_bit_index++;
if (d_frame_bit_index == 30)
{
d_frame_bit_index = 0;
// parity check
// Each word in wordbuff is composed of:
// Bits 0 to 29 = the GPS data word
// Bits 30 to 31 = 2 LSBs of the GPS word ahead.
// prepare the extended frame [-2 -1 0 ... 30]
if (d_prev_GPS_frame_4bytes & 0x00000001)
{
d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x40000000;
}
if (d_prev_GPS_frame_4bytes & 0x00000002)
{
d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x80000000;
}
/* Check that the 2 most recently logged words pass parity. Have to first
invert the data bits according to bit 30 of the previous word. */
if (d_GPS_frame_4bytes & 0x40000000)
{
d_GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR)
}
if (gps_l1_ca_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes))
{
memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char) * 4);
//d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds * 1000.0;
d_GPS_FSM.Event_gps_word_valid();
// send TLM data to PVT using asynchronous message queues
if (d_GPS_FSM.d_flag_new_subframe == true)
{
switch (d_GPS_FSM.d_subframe_ID)
{
case 3: //we have a new set of ephemeris data for the current SV
if (d_GPS_FSM.d_nav.satellite_validation() == true)
{
// get ephemeris object for this SV (mandatory)
std::shared_ptr<Gps_Ephemeris> tmp_obj = std::make_shared<Gps_Ephemeris>(d_GPS_FSM.d_nav.get_ephemeris());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 4: // Possible IONOSPHERE and UTC model update (page 18)
if (d_GPS_FSM.d_nav.flag_iono_valid == true)
{
std::shared_ptr<Gps_Iono> tmp_obj = std::make_shared<Gps_Iono>(d_GPS_FSM.d_nav.get_iono());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
if (d_GPS_FSM.d_nav.flag_utc_model_valid == true)
{
std::shared_ptr<Gps_Utc_Model> tmp_obj = std::make_shared<Gps_Utc_Model>(d_GPS_FSM.d_nav.get_utc_model());
this->message_port_pub(pmt::mp("telemetry"), pmt::make_any(tmp_obj));
}
break;
case 5:
// get almanac (if available)
//TODO: implement almanac reader in navigation_message
break;
default:
break;
}
d_GPS_FSM.clear_flag_new_subframe();
d_flag_new_tow_available = true;
}
d_flag_parity = true;
}
else
{
d_GPS_FSM.Event_gps_word_invalid();
d_flag_parity = false;
}
d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame
d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0;
}
else
{
d_GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word
}
}
//2. Add the telemetry decoder information
if (this->d_flag_preamble == true and d_flag_new_tow_available == true)
{
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_GPS_FSM.d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS;
d_TOW_at_current_symbol_ms = static_cast<unsigned int>(d_nav.d_TOW) * 1000 + GPS_L1_CA_CODE_PERIOD_MS + GPS_CA_PREAMBLE_DURATION_MS;
d_TOW_at_Preamble_ms = d_TOW_at_current_symbol_ms;
flag_TOW_set = true;
d_flag_new_tow_available = false;

20
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h
View File

@@ -32,7 +32,7 @@
#define GNSS_SDR_GPS_L1_CA_TELEMETRY_DECODER_CC_H
#include "GPS_L1_CA.h"
#include "gps_l1_ca_subframe_fsm.h"
#include "gps_navigation_message.h"
#include "gnss_satellite.h"
#include "gnss_synchro.h"
#include <gnuradio/block.h>
@@ -72,7 +72,9 @@ private:
bool gps_word_parityCheck(unsigned int gpsword);
// class private vars
bool decode_subframe();
bool new_decoder();
int d_crc_error_synchronization_counter;
int *d_preambles_symbols;
unsigned int d_stat;
@@ -80,26 +82,16 @@ private:
// symbols
boost::circular_buffer<Gnss_Synchro> d_symbol_history;
double d_symbol_accumulator;
short int d_symbol_accumulator_counter;
// symbol counting
bool d_make_correlation;
unsigned int d_symbol_counter_corr;
float d_subframe_symbols[GPS_SUBFRAME_MS]; //symbols per subframe
int d_current_subframe_symbol;
//bits and frame
unsigned short int d_frame_bit_index;
unsigned int d_GPS_frame_4bytes;
unsigned int d_prev_GPS_frame_4bytes;
bool d_flag_parity;
bool d_flag_preamble;
bool d_flag_new_tow_available;
int d_word_number;
// navigation message vars
Gps_Navigation_Message d_nav;
GpsL1CaSubframeFsm d_GPS_FSM;
bool d_dump;
Gnss_Satellite d_satellite;

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

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

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

@@ -1,289 +0,0 @@
/*!
* \file gps_l1_ca_subframe_fsm.cc
* \brief Implementation of a GPS NAV message word-to-subframe decoder state machine
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#include "gps_l1_ca_subframe_fsm.h"
#include "gnss_satellite.h"
#include <boost/statechart/simple_state.hpp>
#include <boost/statechart/state.hpp>
#include <boost/statechart/transition.hpp>
#include <boost/statechart/custom_reaction.hpp>
#include <boost/mpl/list.hpp>
#include <string>
//************ GPS WORD TO SUBFRAME DECODER STATE MACHINE **********
struct Ev_gps_word_valid : sc::event<Ev_gps_word_valid>
{
};
struct Ev_gps_word_invalid : sc::event<Ev_gps_word_invalid>
{
};
struct Ev_gps_word_preamble : sc::event<Ev_gps_word_preamble>
{
};
struct gps_subframe_fsm_S0 : public sc::state<gps_subframe_fsm_S0, GpsL1CaSubframeFsm>
{
public:
// sc::transition(event,next_status)
typedef sc::transition<Ev_gps_word_preamble, gps_subframe_fsm_S1> reactions;
gps_subframe_fsm_S0(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S0 "<<std::endl;
}
};
struct gps_subframe_fsm_S1 : public sc::state<gps_subframe_fsm_S1, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S2> >
reactions;
gps_subframe_fsm_S1(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S1 "<<std::endl;
}
};
struct gps_subframe_fsm_S2 : public sc::state<gps_subframe_fsm_S2, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S3> >
reactions;
gps_subframe_fsm_S2(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S2 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(0);
}
};
struct gps_subframe_fsm_S3 : public sc::state<gps_subframe_fsm_S3, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S4> >
reactions;
gps_subframe_fsm_S3(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S3 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(1);
}
};
struct gps_subframe_fsm_S4 : public sc::state<gps_subframe_fsm_S4, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S5> >
reactions;
gps_subframe_fsm_S4(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S4 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(2);
}
};
struct gps_subframe_fsm_S5 : public sc::state<gps_subframe_fsm_S5, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S6> >
reactions;
gps_subframe_fsm_S5(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S5 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(3);
}
};
struct gps_subframe_fsm_S6 : public sc::state<gps_subframe_fsm_S6, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S7> >
reactions;
gps_subframe_fsm_S6(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S6 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(4);
}
};
struct gps_subframe_fsm_S7 : public sc::state<gps_subframe_fsm_S7, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S8> >
reactions;
gps_subframe_fsm_S7(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S7 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(5);
}
};
struct gps_subframe_fsm_S8 : public sc::state<gps_subframe_fsm_S8, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S9> >
reactions;
gps_subframe_fsm_S8(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S8 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(6);
}
};
struct gps_subframe_fsm_S9 : public sc::state<gps_subframe_fsm_S9, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S10> >
reactions;
gps_subframe_fsm_S9(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S9 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(7);
}
};
struct gps_subframe_fsm_S10 : public sc::state<gps_subframe_fsm_S10, GpsL1CaSubframeFsm>
{
public:
typedef mpl::list<sc::transition<Ev_gps_word_invalid, gps_subframe_fsm_S0>,
sc::transition<Ev_gps_word_valid, gps_subframe_fsm_S11> >
reactions;
gps_subframe_fsm_S10(my_context ctx) : my_base(ctx)
{
//std::cout<<"Enter S10 "<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(8);
}
};
struct gps_subframe_fsm_S11 : public sc::state<gps_subframe_fsm_S11, GpsL1CaSubframeFsm>
{
public:
typedef sc::transition<Ev_gps_word_preamble, gps_subframe_fsm_S1> reactions;
gps_subframe_fsm_S11(my_context ctx) : my_base(ctx)
{
//std::cout<<"Completed GPS Subframe!"<<std::endl;
context<GpsL1CaSubframeFsm>().gps_word_to_subframe(9);
context<GpsL1CaSubframeFsm>().gps_subframe_to_nav_msg(); //decode the subframe
// DECODE SUBFRAME
//std::cout<<"Enter S11"<<std::endl;
}
};
GpsL1CaSubframeFsm::GpsL1CaSubframeFsm()
{
d_nav.reset();
i_channel_ID = 0;
i_satellite_PRN = 0;
d_subframe_ID = 0;
d_flag_new_subframe = false;
initiate(); //start the FSM
}
void GpsL1CaSubframeFsm::gps_word_to_subframe(int position)
{
// insert the word in the correct position of the subframe
std::memcpy(&d_subframe[position * GPS_WORD_LENGTH], &d_GPS_frame_4bytes, sizeof(char) * GPS_WORD_LENGTH);
}
void GpsL1CaSubframeFsm::clear_flag_new_subframe()
{
d_flag_new_subframe = false;
}
void GpsL1CaSubframeFsm::gps_subframe_to_nav_msg()
{
//int subframe_ID;
// NEW GPS SUBFRAME HAS ARRIVED!
d_subframe_ID = d_nav.subframe_decoder(this->d_subframe); //decode the subframe
std::cout << "New GPS NAV message received in channel " << i_channel_ID << ": "
<< "subframe "
<< d_subframe_ID << " from satellite "
<< Gnss_Satellite(std::string("GPS"), i_satellite_PRN) << std::endl;
d_nav.i_satellite_PRN = i_satellite_PRN;
d_nav.i_channel_ID = i_channel_ID;
d_flag_new_subframe = true;
}
void GpsL1CaSubframeFsm::Event_gps_word_valid()
{
this->process_event(Ev_gps_word_valid());
}
void GpsL1CaSubframeFsm::Event_gps_word_invalid()
{
this->process_event(Ev_gps_word_invalid());
}
void GpsL1CaSubframeFsm::Event_gps_word_preamble()
{
this->process_event(Ev_gps_word_preamble());
}

100
src/algorithms/telemetry_decoder/libs/gps_l1_ca_subframe_fsm.h
View File

@@ -1,100 +0,0 @@
/*!
* \file gps_l1_ca_subframe_fsm.h
* \brief Interface of a GPS NAV message word-to-subframe decoder state machine
* \author Javier Arribas, 2011. jarribas(at)cttc.es
*
* -------------------------------------------------------------------------
*
* Copyright (C) 2010-2018 (see AUTHORS file for a list of contributors)
*
* GNSS-SDR is a software defined Global Navigation
* Satellite Systems receiver
*
* This file is part of GNSS-SDR.
*
* GNSS-SDR is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* GNSS-SDR is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with GNSS-SDR. If not, see <https://www.gnu.org/licenses/>.
*
* -------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_GPS_L1_CA_SUBFRAME_FSM_H_
#define GNSS_SDR_GPS_L1_CA_SUBFRAME_FSM_H_
#include "GPS_L1_CA.h"
#include "gps_navigation_message.h"
#include "gps_ephemeris.h"
#include "gps_iono.h"
#include "gps_almanac.h"
#include "gps_utc_model.h"
#include <boost/statechart/state_machine.hpp>
namespace sc = boost::statechart;
namespace mpl = boost::mpl;
struct gps_subframe_fsm_S0;
struct gps_subframe_fsm_S1;
struct gps_subframe_fsm_S2;
struct gps_subframe_fsm_S3;
struct gps_subframe_fsm_S4;
struct gps_subframe_fsm_S5;
struct gps_subframe_fsm_S6;
struct gps_subframe_fsm_S7;
struct gps_subframe_fsm_S8;
struct gps_subframe_fsm_S9;
struct gps_subframe_fsm_S10;
struct gps_subframe_fsm_S11;
/*!
* \brief This class implements a Finite State Machine that handles the decoding
* of the GPS L1 C/A NAV message
*/
class GpsL1CaSubframeFsm : public sc::state_machine<GpsL1CaSubframeFsm, gps_subframe_fsm_S0>
{
public:
GpsL1CaSubframeFsm(); //!< The constructor starts the Finite State Machine
void clear_flag_new_subframe();
// channel and satellite info
int i_channel_ID; //!< Channel id
unsigned int i_satellite_PRN; //!< Satellite PRN number
Gps_Navigation_Message d_nav; //!< GPS L1 C/A navigation message object
// GPS SV and System parameters
Gps_Ephemeris ephemeris; //!< Object that handles GPS ephemeris parameters
Gps_Almanac almanac; //!< Object that handles GPS almanac data
Gps_Utc_Model utc_model; //!< Object that handles UTM model parameters
Gps_Iono iono; //!< Object that handles ionospheric parameters
char d_subframe[GPS_SUBFRAME_LENGTH];
int d_subframe_ID;
bool d_flag_new_subframe;
char d_GPS_frame_4bytes[GPS_WORD_LENGTH];
//double d_preamble_time_ms;
void gps_word_to_subframe(int position); //!< inserts the word in the correct position of the subframe
/*!
* \brief This function decodes a NAv message subframe and pushes the information to the right queues
*/
void gps_subframe_to_nav_msg();
//FSM EVENTS
void Event_gps_word_valid(); //!< FSM event: the received word is valid
void Event_gps_word_invalid(); //!< FSM event: the received word is not valid
void Event_gps_word_preamble(); //!< FSM event: word preamble detected
};
#endif

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

@@ -2,6 +2,7 @@
* \file dll_pll_veml_tracking.cc
* \brief Implementation of a code DLL + carrier PLL tracking block.
* \author Antonio Ramos, 2018 antonio.ramosdet(at)gmail.com
* Javier Arribas, 2018. 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,
@@ -84,10 +85,12 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
this->message_port_register_out(pmt::mp("events"));
this->set_relative_rate(1.0 / static_cast<double>(trk_parameters.vector_length));
// Telemetry bit synchronization message port input (mainly for GPS L1 CA)
this->message_port_register_in(pmt::mp("preamble_samplestamp"));
// initialize internal vars
d_veml = false;
d_cloop = true;
d_synchonizing = false;
d_code_chip_rate = 0.0;
d_secondary_code_length = 0;
d_secondary_code_string = nullptr;
@@ -120,6 +123,30 @@ dll_pll_veml_tracking::dll_pll_veml_tracking(const Dll_Pll_Conf &conf_) : gr::bl
d_secondary = false;
trk_parameters.track_pilot = false;
interchange_iq = false;
// set the preamble
unsigned short int preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE;
// preamble bits to sampled symbols
d_gps_l1ca_preambles_symbols = static_cast<int *>(volk_gnsssdr_malloc(GPS_CA_PREAMBLE_LENGTH_SYMBOLS * sizeof(int), volk_gnsssdr_get_alignment()));
int n = 0;
for (int i = 0; i < GPS_CA_PREAMBLE_LENGTH_BITS; i++)
{
for (unsigned int j = 0; j < GPS_CA_TELEMETRY_SYMBOLS_PER_BIT; j++)
{
if (preambles_bits[i] == 1)
{
d_gps_l1ca_preambles_symbols[n] = 1;
}
else
{
d_gps_l1ca_preambles_symbols[n] = -1;
}
n++;
}
}
d_symbol_history.resize(GPS_CA_PREAMBLE_LENGTH_SYMBOLS); // Change fixed buffer size
d_symbol_history.clear(); // Clear all the elements in the buffer
}
else if (signal_type.compare("2S") == 0)
{
@@ -399,7 +426,8 @@ void dll_pll_veml_tracking::start_tracking()
double T_prn_mod_seconds = T_chip_mod_seconds * static_cast<double>(d_code_length_chips);
double T_prn_mod_samples = T_prn_mod_seconds * trk_parameters.fs_in;
d_current_prn_length_samples = std::round(T_prn_mod_samples);
//d_current_prn_length_samples = std::round(T_prn_mod_samples);
d_current_prn_length_samples = std::floor(T_prn_mod_samples);
double T_prn_true_seconds = static_cast<double>(d_code_length_chips) / d_code_chip_rate;
double T_prn_true_samples = T_prn_true_seconds * trk_parameters.fs_in;
@@ -520,7 +548,6 @@ void dll_pll_veml_tracking::start_tracking()
// enable tracking pull-in
d_state = 1;
d_synchonizing = false;
d_cloop = true;
d_Prompt_buffer_deque.clear();
d_last_prompt = gr_complex(0.0, 0.0);
@@ -532,6 +559,11 @@ void dll_pll_veml_tracking::start_tracking()
dll_pll_veml_tracking::~dll_pll_veml_tracking()
{
if (signal_type.compare("1C") == 0)
{
volk_gnsssdr_free(d_gps_l1ca_preambles_symbols);
}
if (d_dump_file.is_open())
{
try
@@ -753,7 +785,8 @@ void dll_pll_veml_tracking::update_tracking_vars()
// Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation
T_prn_samples = T_prn_seconds * trk_parameters.fs_in;
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * trk_parameters.fs_in;
d_current_prn_length_samples = static_cast<int>(round(K_blk_samples)); // round to a discrete number of samples
//d_current_prn_length_samples = static_cast<int>(round(K_blk_samples)); // round to a discrete number of samples
d_current_prn_length_samples = static_cast<int>(std::floor(K_blk_samples)); // round to a discrete number of samples
//################### PLL COMMANDS #################################################
// carrier phase step (NCO phase increment per sample) [rads/sample]
@@ -834,6 +867,7 @@ void dll_pll_veml_tracking::log_data(bool integrating)
float tmp_VE, tmp_E, tmp_P, tmp_L, tmp_VL;
float tmp_float;
double tmp_double;
unsigned long int tmp_long_int;
if (trk_parameters.track_pilot)
{
if (interchange_iq)
@@ -900,7 +934,8 @@ void dll_pll_veml_tracking::log_data(bool integrating)
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
d_dump_file.write(reinterpret_cast<char *>(&d_sample_counter), sizeof(unsigned long int));
tmp_long_int = d_sample_counter + d_current_prn_length_samples;
d_dump_file.write(reinterpret_cast<char *>(&tmp_long_int), sizeof(unsigned long int));
// accumulated carrier phase
tmp_float = d_acc_carrier_phase_rad;
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
@@ -1277,39 +1312,82 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
}
else if (d_symbols_per_bit > 1) //Signal does not have secondary code. Search a bit transition by sign change
{
if (d_synchonizing)
float current_tracking_time_s = static_cast<float>(d_sample_counter - d_acq_sample_stamp) / trk_parameters.fs_in;
if (current_tracking_time_s > 10)
{
if (d_Prompt->real() * d_last_prompt.real() > 0.0)
d_symbol_history.push_back(d_Prompt->real());
//******* preamble correlation ********
int corr_value = 0;
if ((d_symbol_history.size() == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)) // and (d_make_correlation or !d_flag_frame_sync))
{
d_current_symbol++;
for (unsigned int i = 0; i < GPS_CA_PREAMBLE_LENGTH_SYMBOLS; i++)
{
if (d_symbol_history.at(i) < 0) // symbols clipping
{
corr_value -= d_gps_l1ca_preambles_symbols[i];
}
else
{
corr_value += d_gps_l1ca_preambles_symbols[i];
}
}
}
else if (d_current_symbol > d_symbols_per_bit)
if (corr_value == GPS_CA_PREAMBLE_LENGTH_SYMBOLS)
{
d_synchonizing = false;
d_current_symbol = 1;
//std::cout << "Preamble detected at tracking!" << std::endl;
next_state = true;
}
else
{
d_current_symbol = 1;
d_last_prompt = *d_Prompt;
next_state = false;
}
}
else if (d_last_prompt.real() != 0.0)
{
d_current_symbol++;
if (d_current_symbol == d_symbols_per_bit) next_state = true;
}
else
{
d_last_prompt = *d_Prompt;
d_synchonizing = true;
d_current_symbol = 1;
next_state = false;
}
}
else
{
next_state = true;
}
// ########### Output the tracking results to Telemetry block ##########
if (interchange_iq)
{
if (trk_parameters.track_pilot)
{
// Note that data and pilot components are in quadrature. I and Q are interchanged
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt_Data).imag());
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt_Data).real());
}
else
{
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).imag());
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).real());
}
}
else
{
if (trk_parameters.track_pilot)
{
// Note that data and pilot components are in quadrature. I and Q are interchanged
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt_Data).real());
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt_Data).imag());
}
else
{
current_synchro_data.Prompt_I = static_cast<double>((*d_Prompt).real());
current_synchro_data.Prompt_Q = static_cast<double>((*d_Prompt).imag());
}
}
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 = d_correlation_length_ms;
if (next_state)
{ // reset extended correlator
d_VE_accu = gr_complex(0.0, 0.0);
@@ -1320,7 +1398,6 @@ int dll_pll_veml_tracking::general_work(int noutput_items __attribute__((unused)
d_last_prompt = gr_complex(0.0, 0.0);
d_Prompt_buffer_deque.clear();
d_current_symbol = 0;
d_synchonizing = false;
if (d_enable_extended_integration)
{

7
src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.h
View File

@@ -41,6 +41,7 @@
#include <string>
#include <map>
#include <queue>
#include <boost/circular_buffer.hpp>
class dll_pll_veml_tracking;
@@ -69,6 +70,7 @@ private:
friend dll_pll_veml_tracking_sptr dll_pll_veml_make_tracking(const Dll_Pll_Conf &conf_);
dll_pll_veml_tracking(const Dll_Pll_Conf &conf_);
void msg_handler_preamble_index(pmt::pmt_t msg);
bool cn0_and_tracking_lock_status(double coh_integration_time_s);
bool acquire_secondary();
@@ -102,9 +104,12 @@ private:
std::string *d_secondary_code_string;
std::string signal_pretty_name;
uint64_t d_preamble_sample_counter;
int *d_gps_l1ca_preambles_symbols;
boost::circular_buffer<float> d_symbol_history;
//tracking state machine
int d_state;
bool d_synchonizing;
//Integration period in samples
int d_correlation_length_ms;
int d_n_correlator_taps;