mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-01-07 07:50:32 +00:00
Prefer initialization to assignment in constructors
This commit is contained in:
parent
33366ef6df
commit
71321581ed
@ -33,11 +33,11 @@ Gnss_Sdr_Timestamp::Gnss_Sdr_Timestamp(size_t sizeof_stream_item,
|
|||||||
gr::io_signature::make(1, 20, sizeof_stream_item),
|
gr::io_signature::make(1, 20, sizeof_stream_item),
|
||||||
gr::io_signature::make(1, 20, sizeof_stream_item)),
|
gr::io_signature::make(1, 20, sizeof_stream_item)),
|
||||||
d_timefile(std::move(timestamp_file)),
|
d_timefile(std::move(timestamp_file)),
|
||||||
d_clock_offset_ms(clock_offset_ms)
|
d_clock_offset_ms(clock_offset_ms),
|
||||||
|
d_fraction_ms_offset(modf(d_clock_offset_ms, &d_integer_ms_offset)), // optional clockoffset parameter to convert UTC timestamps to GPS time in some receiver's configuration
|
||||||
|
d_next_timetag_samplecount(0),
|
||||||
|
d_get_next_timetag(true)
|
||||||
{
|
{
|
||||||
d_fraction_ms_offset = modf(d_clock_offset_ms, &d_integer_ms_offset); // optional clockoffset parameter to convert UTC timestamps to GPS time in some receiver's configuration
|
|
||||||
get_next_timetag = true;
|
|
||||||
next_timetag_samplecount = 0;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -50,7 +50,7 @@ gnss_shared_ptr<Gnss_Sdr_Timestamp> gnss_sdr_make_Timestamp(size_t sizeof_stream
|
|||||||
|
|
||||||
bool Gnss_Sdr_Timestamp::read_next_timetag()
|
bool Gnss_Sdr_Timestamp::read_next_timetag()
|
||||||
{
|
{
|
||||||
d_timefilestream.read(reinterpret_cast<char*>(&next_timetag_samplecount), sizeof(uint64_t));
|
d_timefilestream.read(reinterpret_cast<char*>(&d_next_timetag_samplecount), sizeof(uint64_t));
|
||||||
if (!d_timefilestream)
|
if (!d_timefilestream)
|
||||||
{
|
{
|
||||||
return false;
|
return false;
|
||||||
@ -98,20 +98,20 @@ int Gnss_Sdr_Timestamp::work(int noutput_items,
|
|||||||
gr_vector_void_star& output_items)
|
gr_vector_void_star& output_items)
|
||||||
{
|
{
|
||||||
// multichannel support
|
// multichannel support
|
||||||
if (get_next_timetag == true)
|
if (d_get_next_timetag == true)
|
||||||
{
|
{
|
||||||
if (read_next_timetag() == false)
|
if (read_next_timetag() == false)
|
||||||
{
|
{
|
||||||
// std::cout << "End of TimeTag file reached!\n";
|
// std::cout << "End of TimeTag file reached!\n";
|
||||||
// return 0; // todo: find why return -1 does not stop gnss-sdr!
|
// return 0; // todo: find why return -1 does not stop gnss-sdr!
|
||||||
}
|
}
|
||||||
get_next_timetag = false;
|
d_get_next_timetag = false;
|
||||||
}
|
}
|
||||||
for (size_t ch = 0; ch < output_items.size(); ch++)
|
for (size_t ch = 0; ch < output_items.size(); ch++)
|
||||||
{
|
{
|
||||||
std::memcpy(output_items[ch], input_items[ch], noutput_items * input_signature()->sizeof_stream_item(ch));
|
std::memcpy(output_items[ch], input_items[ch], noutput_items * input_signature()->sizeof_stream_item(ch));
|
||||||
uint64_t bytes_to_samples = 2; // todo: improve this.. hardcoded 2 bytes -> 1 complex sample!
|
uint64_t bytes_to_samples = 2; // todo: improve this.. hardcoded 2 bytes -> 1 complex sample!
|
||||||
int64_t diff_samplecount = uint64diff(this->nitems_written(ch), next_timetag_samplecount * bytes_to_samples);
|
int64_t diff_samplecount = uint64diff(this->nitems_written(ch), d_next_timetag_samplecount * bytes_to_samples);
|
||||||
// std::cout << "diff_samplecount: " << diff_samplecount << ", noutput_items: " << noutput_items << "\n";
|
// std::cout << "diff_samplecount: " << diff_samplecount << ", noutput_items: " << noutput_items << "\n";
|
||||||
if (diff_samplecount <= noutput_items and std::labs(diff_samplecount) <= noutput_items)
|
if (diff_samplecount <= noutput_items and std::labs(diff_samplecount) <= noutput_items)
|
||||||
{
|
{
|
||||||
@ -121,8 +121,8 @@ int Gnss_Sdr_Timestamp::work(int noutput_items,
|
|||||||
tmp_obj->tow_ms_fraction = d_fraction_ms_offset;
|
tmp_obj->tow_ms_fraction = d_fraction_ms_offset;
|
||||||
tmp_obj->rx_time = 0;
|
tmp_obj->rx_time = 0;
|
||||||
add_item_tag(ch, this->nitems_written(ch) - diff_samplecount, pmt::mp("timetag"), pmt::make_any(tmp_obj));
|
add_item_tag(ch, this->nitems_written(ch) - diff_samplecount, pmt::mp("timetag"), pmt::make_any(tmp_obj));
|
||||||
// std::cout << "[" << this->nitems_written(ch) - diff_samplecount << "] Sent TimeTag SC: " << next_timetag_samplecount * bytes_to_samples << ", Week: " << next_timetag.week << ", TOW: " << next_timetag.tow_ms << " [ms] \n";
|
// std::cout << "[" << this->nitems_written(ch) - diff_samplecount << "] Sent TimeTag SC: " << d_next_timetag_samplecount * bytes_to_samples << ", Week: " << next_timetag.week << ", TOW: " << next_timetag.tow_ms << " [ms] \n";
|
||||||
get_next_timetag = true;
|
d_get_next_timetag = true;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -66,10 +66,10 @@ private:
|
|||||||
std::fstream d_timefilestream;
|
std::fstream d_timefilestream;
|
||||||
GnssTime next_timetag{};
|
GnssTime next_timetag{};
|
||||||
double d_clock_offset_ms;
|
double d_clock_offset_ms;
|
||||||
double d_integer_ms_offset;
|
|
||||||
double d_fraction_ms_offset;
|
double d_fraction_ms_offset;
|
||||||
uint64_t next_timetag_samplecount;
|
double d_integer_ms_offset;
|
||||||
bool get_next_timetag;
|
uint64_t d_next_timetag_samplecount;
|
||||||
|
bool d_get_next_timetag;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
@ -33,9 +33,9 @@ Gnss_Sdr_Valve::Gnss_Sdr_Valve(size_t sizeof_stream_item,
|
|||||||
d_nitems(nitems),
|
d_nitems(nitems),
|
||||||
d_ncopied_items(0),
|
d_ncopied_items(0),
|
||||||
d_queue(queue),
|
d_queue(queue),
|
||||||
d_stop_flowgraph(stop_flowgraph)
|
d_stop_flowgraph(stop_flowgraph),
|
||||||
|
d_open_valve(false)
|
||||||
{
|
{
|
||||||
d_open_valve = false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -35,6 +35,7 @@
|
|||||||
#include "galileo_e6_signal_replica.h"
|
#include "galileo_e6_signal_replica.h"
|
||||||
#include "gnss_satellite.h"
|
#include "gnss_satellite.h"
|
||||||
#include "gnss_sdr_create_directory.h"
|
#include "gnss_sdr_create_directory.h"
|
||||||
|
#include "gnss_sdr_filesystem.h"
|
||||||
#include "gnss_synchro.h"
|
#include "gnss_synchro.h"
|
||||||
#include "gps_l2c_signal_replica.h"
|
#include "gps_l2c_signal_replica.h"
|
||||||
#include "gps_l5_signal_replica.h"
|
#include "gps_l5_signal_replica.h"
|
||||||
@ -62,19 +63,13 @@
|
|||||||
#include <boost/bind/bind.hpp>
|
#include <boost/bind/bind.hpp>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if HAS_STD_FILESYSTEM
|
#if PMT_USES_BOOST_ANY
|
||||||
#if HAS_STD_FILESYSTEM_EXPERIMENTAL
|
#include <boost/any.hpp>
|
||||||
#include <experimental/filesystem>
|
namespace wht = boost;
|
||||||
namespace fs = std::experimental::filesystem;
|
|
||||||
#else
|
#else
|
||||||
#include <filesystem>
|
#include <any>
|
||||||
namespace fs = std::filesystem;
|
namespace wht = std;
|
||||||
#endif
|
#endif
|
||||||
#else
|
|
||||||
#include <boost/filesystem/path.hpp>
|
|
||||||
namespace fs = boost::filesystem;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
kf_vtl_tracking_sptr kf_vtl_make_tracking(const Kf_Conf &conf_)
|
kf_vtl_tracking_sptr kf_vtl_make_tracking(const Kf_Conf &conf_)
|
||||||
{
|
{
|
||||||
@ -83,12 +78,60 @@ kf_vtl_tracking_sptr kf_vtl_make_tracking(const Kf_Conf &conf_)
|
|||||||
|
|
||||||
|
|
||||||
kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
|
kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
|
||||||
: gr::block("kf_vtl_tracking", gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
: gr::block("kf_vtl_tracking",
|
||||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||||
|
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))),
|
||||||
|
d_trk_parameters(conf_),
|
||||||
|
d_acquisition_gnss_synchro(nullptr),
|
||||||
|
d_signal_type(d_trk_parameters.signal),
|
||||||
|
d_acq_code_phase_samples(0.0),
|
||||||
|
d_acq_carrier_doppler_hz(0.0),
|
||||||
|
d_current_correlation_time_s(0.0),
|
||||||
|
d_carr_phase_error_disc_hz(0.0),
|
||||||
|
d_code_error_disc_chips(0.0),
|
||||||
|
d_code_error_kf_chips(0.0),
|
||||||
|
d_code_freq_kf_chips_s(0.0),
|
||||||
|
d_carrier_phase_kf_rad(0.0),
|
||||||
|
d_carrier_doppler_kf_hz(0.0),
|
||||||
|
d_carrier_doppler_rate_kf_hz_s(0.0),
|
||||||
|
d_acc_carrier_phase_rad(0.0),
|
||||||
|
d_T_chip_seconds(0.0),
|
||||||
|
d_T_prn_seconds(0.0),
|
||||||
|
d_T_prn_samples(0.0),
|
||||||
|
d_K_blk_samples(0.0),
|
||||||
|
d_carrier_lock_test(1.0),
|
||||||
|
d_CN0_SNV_dB_Hz(0.0),
|
||||||
|
d_carrier_lock_threshold(d_trk_parameters.carrier_lock_th),
|
||||||
|
d_carrier_phase_step_rad(0.0),
|
||||||
|
d_carrier_phase_rate_step_rad(0.0),
|
||||||
|
d_code_phase_step_chips(0.0),
|
||||||
|
d_code_phase_rate_step_chips(0.0),
|
||||||
|
d_rem_code_phase_chips(0.0),
|
||||||
|
d_rem_code_phase_samples(0.0),
|
||||||
|
d_sample_counter(0ULL),
|
||||||
|
d_acq_sample_stamp(0ULL),
|
||||||
|
d_rem_carr_phase_rad(0.0),
|
||||||
|
d_channel(0U),
|
||||||
|
d_secondary_code_length(0U),
|
||||||
|
d_data_secondary_code_length(0U),
|
||||||
|
d_state(0),
|
||||||
|
d_current_prn_length_samples(static_cast<int32_t>(d_trk_parameters.vector_length)),
|
||||||
|
d_extend_correlation_symbols_count(0),
|
||||||
|
d_cn0_estimation_counter(0),
|
||||||
|
d_carrier_lock_fail_counter(0),
|
||||||
|
d_code_lock_fail_counter(0),
|
||||||
|
d_pull_in_transitory(true),
|
||||||
|
d_corrected_doppler(false),
|
||||||
|
d_interchange_iq(false),
|
||||||
|
d_veml(false),
|
||||||
|
d_cloop(true),
|
||||||
|
d_dump(d_trk_parameters.dump),
|
||||||
|
d_dump_mat(d_trk_parameters.dump_mat && d_dump),
|
||||||
|
d_acc_carrier_phase_initialized(false)
|
||||||
{
|
{
|
||||||
// prevent telemetry symbols accumulation in output buffers
|
// prevent telemetry symbols accumulation in output buffers
|
||||||
this->set_max_noutput_items(1);
|
this->set_max_noutput_items(1);
|
||||||
d_trk_parameters = conf_;
|
|
||||||
// Telemetry bit synchronization message port input
|
// Telemetry bit synchronization message port input
|
||||||
this->message_port_register_out(pmt::mp("events"));
|
this->message_port_register_out(pmt::mp("events"));
|
||||||
this->set_relative_rate(1.0 / static_cast<double>(d_trk_parameters.vector_length));
|
this->set_relative_rate(1.0 / static_cast<double>(d_trk_parameters.vector_length));
|
||||||
@ -122,16 +165,6 @@ kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
// initialize internal vars
|
// initialize internal vars
|
||||||
d_veml = false;
|
|
||||||
d_cloop = true;
|
|
||||||
d_pull_in_transitory = true;
|
|
||||||
d_code_chip_rate = 0.0;
|
|
||||||
d_secondary_code_length = 0U;
|
|
||||||
d_data_secondary_code_length = 0U;
|
|
||||||
d_preamble_length_symbols = 0;
|
|
||||||
d_interchange_iq = false;
|
|
||||||
d_signal_type = std::string(d_trk_parameters.signal);
|
|
||||||
|
|
||||||
std::map<std::string, std::string> map_signal_pretty_name;
|
std::map<std::string, std::string> map_signal_pretty_name;
|
||||||
map_signal_pretty_name["1C"] = "L1 C/A";
|
map_signal_pretty_name["1C"] = "L1 C/A";
|
||||||
map_signal_pretty_name["1B"] = "E1";
|
map_signal_pretty_name["1B"] = "E1";
|
||||||
@ -409,12 +442,6 @@ kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
|
|||||||
d_code_samples_per_chip = 0U;
|
d_code_samples_per_chip = 0U;
|
||||||
d_symbols_per_bit = 0;
|
d_symbols_per_bit = 0;
|
||||||
}
|
}
|
||||||
d_T_chip_seconds = 0.0;
|
|
||||||
d_T_prn_seconds = 0.0;
|
|
||||||
d_T_prn_samples = 0.0;
|
|
||||||
d_K_blk_samples = 0.0;
|
|
||||||
|
|
||||||
// Initialize tracking ==========================================
|
|
||||||
|
|
||||||
// Initialization of local code replica
|
// Initialization of local code replica
|
||||||
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
|
// Get space for a vector with the sinboc(1,1) replica sampled 2x/chip
|
||||||
@ -485,33 +512,13 @@ kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
|
|||||||
// --- Initializations ---
|
// --- Initializations ---
|
||||||
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
|
d_Prompt_circular_buffer.set_capacity(d_secondary_code_length);
|
||||||
d_multicorrelator_cpu.set_high_dynamics_resampler(d_trk_parameters.high_dyn);
|
d_multicorrelator_cpu.set_high_dynamics_resampler(d_trk_parameters.high_dyn);
|
||||||
|
|
||||||
// Initial code frequency basis of NCO
|
// Initial code frequency basis of NCO
|
||||||
d_code_freq_kf_chips_s = d_code_chip_rate;
|
d_code_freq_kf_chips_s = d_code_chip_rate;
|
||||||
// Residual code phase (in chips)
|
|
||||||
d_rem_code_phase_samples = 0.0;
|
|
||||||
// Residual carrier phase
|
|
||||||
d_rem_carr_phase_rad = 0.0;
|
|
||||||
|
|
||||||
// sample synchronization
|
|
||||||
d_sample_counter = 0ULL;
|
|
||||||
d_acq_sample_stamp = 0ULL;
|
|
||||||
|
|
||||||
d_current_prn_length_samples = static_cast<int32_t>(d_trk_parameters.vector_length);
|
|
||||||
d_current_correlation_time_s = 0.0;
|
|
||||||
|
|
||||||
d_carr_phase_error_disc_hz = 0.0;
|
|
||||||
d_code_error_disc_chips = 0.0;
|
|
||||||
d_code_error_kf_chips = 0.0;
|
|
||||||
d_code_freq_kf_chips_s = 0.0;
|
|
||||||
|
|
||||||
// CN0 estimation and lock detector buffers
|
// CN0 estimation and lock detector buffers
|
||||||
d_cn0_estimation_counter = 0;
|
|
||||||
d_Prompt_buffer.reserve(d_trk_parameters.cn0_samples);
|
d_Prompt_buffer.reserve(d_trk_parameters.cn0_samples);
|
||||||
d_carrier_lock_test = 1.0;
|
|
||||||
d_CN0_SNV_dB_Hz = 0.0;
|
|
||||||
d_carrier_lock_fail_counter = 0;
|
|
||||||
d_code_lock_fail_counter = 0;
|
|
||||||
d_carrier_lock_threshold = d_trk_parameters.carrier_lock_th;
|
|
||||||
d_Prompt_Data.reserve(1);
|
d_Prompt_Data.reserve(1);
|
||||||
d_cn0_smoother = Exponential_Smoother();
|
d_cn0_smoother = Exponential_Smoother();
|
||||||
d_cn0_smoother.set_alpha(d_trk_parameters.cn0_smoother_alpha);
|
d_cn0_smoother.set_alpha(d_trk_parameters.cn0_smoother_alpha);
|
||||||
@ -527,26 +534,8 @@ kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
|
|||||||
d_carrier_lock_test_smoother.set_offset(0.0);
|
d_carrier_lock_test_smoother.set_offset(0.0);
|
||||||
d_carrier_lock_test_smoother.set_samples_for_initialization(d_trk_parameters.carrier_lock_test_smoother_samples);
|
d_carrier_lock_test_smoother.set_samples_for_initialization(d_trk_parameters.carrier_lock_test_smoother_samples);
|
||||||
|
|
||||||
d_acquisition_gnss_synchro = nullptr;
|
|
||||||
d_channel = 0;
|
|
||||||
d_acq_code_phase_samples = 0.0;
|
|
||||||
d_acq_carrier_doppler_hz = 0.0;
|
|
||||||
d_carrier_phase_kf_rad = 0;
|
|
||||||
d_carrier_doppler_kf_hz = 0.0;
|
|
||||||
d_carrier_doppler_rate_kf_hz_s = 0.0;
|
|
||||||
d_acc_carrier_phase_rad = 0.0;
|
|
||||||
|
|
||||||
d_extend_correlation_symbols_count = 0;
|
|
||||||
d_code_phase_step_chips = 0.0;
|
|
||||||
d_code_phase_rate_step_chips = 0.0;
|
|
||||||
d_carrier_phase_step_rad = 0.0;
|
|
||||||
d_carrier_phase_rate_step_rad = 0.0;
|
|
||||||
d_rem_code_phase_chips = 0.0;
|
|
||||||
d_state = 0; // initial state: standby
|
|
||||||
clear_tracking_vars();
|
clear_tracking_vars();
|
||||||
|
|
||||||
d_dump = d_trk_parameters.dump;
|
|
||||||
d_dump_mat = d_trk_parameters.dump_mat and d_dump;
|
|
||||||
if (d_dump)
|
if (d_dump)
|
||||||
{
|
{
|
||||||
d_dump_filename = d_trk_parameters.dump_filename;
|
d_dump_filename = d_trk_parameters.dump_filename;
|
||||||
@ -580,8 +569,6 @@ kf_vtl_tracking::kf_vtl_tracking(const Kf_Conf &conf_)
|
|||||||
d_dump = false;
|
d_dump = false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
d_corrected_doppler = false;
|
|
||||||
d_acc_carrier_phase_initialized = false;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -601,7 +588,7 @@ void kf_vtl_tracking::msg_handler_telemetry_to_trk(const pmt::pmt_t &msg)
|
|||||||
{
|
{
|
||||||
if (pmt::any_ref(msg).type().hash_code() == d_int_type_hash_code)
|
if (pmt::any_ref(msg).type().hash_code() == d_int_type_hash_code)
|
||||||
{
|
{
|
||||||
const int tlm_event = boost::any_cast<int>(pmt::any_ref(msg));
|
const int tlm_event = wht::any_cast<int>(pmt::any_ref(msg));
|
||||||
if (tlm_event == 1)
|
if (tlm_event == 1)
|
||||||
{
|
{
|
||||||
DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel;
|
DLOG(INFO) << "Telemetry fault received in ch " << this->d_channel;
|
||||||
@ -623,7 +610,7 @@ void kf_vtl_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
|
|||||||
{
|
{
|
||||||
if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr<TrackingCmd>).hash_code())
|
if (pmt::any_ref(msg).type().hash_code() == typeid(const std::shared_ptr<TrackingCmd>).hash_code())
|
||||||
{
|
{
|
||||||
const auto cmd = boost::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg));
|
const auto cmd = wht::any_cast<const std::shared_ptr<TrackingCmd>>(pmt::any_ref(msg));
|
||||||
// std::cout << "RX pvt-to-trk cmd with delay: "
|
// std::cout << "RX pvt-to-trk cmd with delay: "
|
||||||
// << static_cast<double>(nitems_read(0) - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n";
|
// << static_cast<double>(nitems_read(0) - cmd->sample_counter) / d_trk_parameters.fs_in << " [s]\n";
|
||||||
}
|
}
|
||||||
@ -1173,24 +1160,21 @@ void kf_vtl_tracking::run_Kf()
|
|||||||
// Measurement update
|
// Measurement update
|
||||||
arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain
|
arma::mat K = d_P_new_old * d_H.t() * arma::inv(d_H * d_P_new_old * d_H.t() + d_R); // Kalman gain
|
||||||
|
|
||||||
x_new_new = d_x_new_old + K * z;
|
d_x_new_new = d_x_new_old + K * z;
|
||||||
|
|
||||||
d_P_new_new = (arma::eye(5, 5) - K * d_H) * d_P_new_old;
|
d_P_new_new = (arma::eye(5, 5) - K * d_H) * d_P_new_old;
|
||||||
|
|
||||||
// new code phase estimation
|
// new code phase estimation
|
||||||
d_code_error_kf_chips = x_new_new(0);
|
d_code_error_kf_chips = d_x_new_new(0);
|
||||||
x_new_new(0) = 0; // reset error estimation because the NCO corrects the code phase
|
d_x_new_new(0) = 0; // reset error estimation because the NCO corrects the code phase
|
||||||
|
|
||||||
// new carrier phase estimation
|
// new carrier phase estimation
|
||||||
d_carrier_phase_kf_rad = x_new_new(1);
|
d_carrier_phase_kf_rad = d_x_new_new(1);
|
||||||
|
|
||||||
// New carrier Doppler frequency estimation
|
// New carrier Doppler frequency estimation
|
||||||
d_carrier_doppler_kf_hz = x_new_new(2); // d_carrier_loop_filter.get_carrier_error(0, static_cast<float>(d_carr_phase_error_hz), static_cast<float>(d_current_correlation_time_s));
|
d_carrier_doppler_kf_hz = d_x_new_new(2); // d_carrier_loop_filter.get_carrier_error(0, static_cast<float>(d_carr_phase_error_hz), static_cast<float>(d_current_correlation_time_s));
|
||||||
|
|
||||||
d_carrier_doppler_rate_kf_hz_s = x_new_new(3);
|
d_carrier_doppler_rate_kf_hz_s = d_x_new_new(3);
|
||||||
|
|
||||||
// std::cout << "d_carrier_doppler_hz: " << d_carrier_doppler_hz << '\n';
|
|
||||||
// std::cout << "d_CN0_SNV_dB_Hz: " << this->d_CN0_SNV_dB_Hz << '\n';
|
|
||||||
|
|
||||||
// New code Doppler frequency estimation
|
// New code Doppler frequency estimation
|
||||||
if (d_trk_parameters.carrier_aiding)
|
if (d_trk_parameters.carrier_aiding)
|
||||||
@ -1201,9 +1185,9 @@ void kf_vtl_tracking::run_Kf()
|
|||||||
else
|
else
|
||||||
{
|
{
|
||||||
// use its own KF code rate estimation
|
// use its own KF code rate estimation
|
||||||
d_code_freq_kf_chips_s -= x_new_new(4);
|
d_code_freq_kf_chips_s -= d_x_new_new(4);
|
||||||
}
|
}
|
||||||
x_new_new(4) = 0;
|
d_x_new_new(4) = 0;
|
||||||
// Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler
|
// Experimental: detect Carrier Doppler vs. Code Doppler incoherence and correct the Carrier Doppler
|
||||||
// if (d_trk_parameters.enable_doppler_correction == true)
|
// if (d_trk_parameters.enable_doppler_correction == true)
|
||||||
// {
|
// {
|
||||||
@ -1218,7 +1202,7 @@ void kf_vtl_tracking::run_Kf()
|
|||||||
d_rem_carr_phase_rad = d_carrier_phase_kf_rad;
|
d_rem_carr_phase_rad = d_carrier_phase_kf_rad;
|
||||||
|
|
||||||
// prepare data for next KF epoch
|
// prepare data for next KF epoch
|
||||||
d_x_old_old = x_new_new;
|
d_x_old_old = d_x_new_new;
|
||||||
d_P_old_old = d_P_new_new;
|
d_P_old_old = d_P_new_new;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -1271,7 +1255,8 @@ void kf_vtl_tracking::update_tracking_vars()
|
|||||||
// d_rem_carr_phase_rad = d_carrier_phase_kf_rad;
|
// d_rem_carr_phase_rad = d_carrier_phase_kf_rad;
|
||||||
|
|
||||||
// remnant carrier phase to prevent overflow in the code NCO
|
// remnant carrier phase to prevent overflow in the code NCO
|
||||||
d_rem_carr_phase_rad += static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
|
d_rem_carr_phase_rad += static_cast<float>(d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) +
|
||||||
|
0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
|
||||||
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
|
d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, TWO_PI);
|
||||||
|
|
||||||
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
|
// carrier phase rate step (NCO phase increment rate per sample) [rads/sample^2]
|
||||||
@ -1288,7 +1273,8 @@ void kf_vtl_tracking::update_tracking_vars()
|
|||||||
// double a = d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
|
// double a = d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples);
|
||||||
// double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples);
|
// double b = 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples);
|
||||||
// std::cout << fmod(b, TWO_PI) / fmod(a, TWO_PI) << '\n';
|
// std::cout << fmod(b, TWO_PI) / fmod(a, TWO_PI) << '\n';
|
||||||
d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) + 0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
|
d_acc_carrier_phase_rad -= (d_carrier_phase_step_rad * static_cast<double>(d_current_prn_length_samples) +
|
||||||
|
0.5 * d_carrier_phase_rate_step_rad * static_cast<double>(d_current_prn_length_samples) * static_cast<double>(d_current_prn_length_samples));
|
||||||
|
|
||||||
// ################### DLL COMMANDS #################################################
|
// ################### DLL COMMANDS #################################################
|
||||||
// code phase step (Code resampler phase increment per sample) [chips/sample]
|
// code phase step (Code resampler phase increment per sample) [chips/sample]
|
||||||
@ -1488,7 +1474,7 @@ void kf_vtl_tracking::log_data()
|
|||||||
// Carrier estimation
|
// Carrier estimation
|
||||||
tmp_float = static_cast<float>(d_carr_phase_error_disc_hz);
|
tmp_float = static_cast<float>(d_carr_phase_error_disc_hz);
|
||||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||||
tmp_float = static_cast<float>(x_new_new(2));
|
tmp_float = static_cast<float>(d_x_new_new(2));
|
||||||
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
d_dump_file.write(reinterpret_cast<char *>(&tmp_float), sizeof(float));
|
||||||
// code estimation
|
// code estimation
|
||||||
tmp_float = static_cast<float>(d_code_error_disc_chips);
|
tmp_float = static_cast<float>(d_code_error_disc_chips);
|
||||||
|
@ -126,7 +126,30 @@ private:
|
|||||||
arma::mat d_P_new_new;
|
arma::mat d_P_new_new;
|
||||||
arma::vec d_x_old_old;
|
arma::vec d_x_old_old;
|
||||||
arma::vec d_x_new_old;
|
arma::vec d_x_new_old;
|
||||||
arma::vec x_new_new;
|
arma::vec d_x_new_new;
|
||||||
|
|
||||||
|
std::string d_secondary_code_string;
|
||||||
|
std::string d_data_secondary_code_string;
|
||||||
|
std::string d_systemName;
|
||||||
|
std::string d_signal_type;
|
||||||
|
std::string d_signal_pretty_name;
|
||||||
|
std::string d_dump_filename;
|
||||||
|
|
||||||
|
std::ofstream d_dump_file;
|
||||||
|
|
||||||
|
gr_complex *d_Very_Early;
|
||||||
|
gr_complex *d_Early;
|
||||||
|
gr_complex *d_Prompt;
|
||||||
|
gr_complex *d_Late;
|
||||||
|
gr_complex *d_Very_Late;
|
||||||
|
|
||||||
|
gr_complex d_VE_accu;
|
||||||
|
gr_complex d_E_accu;
|
||||||
|
gr_complex d_P_accu;
|
||||||
|
gr_complex d_P_accu_old;
|
||||||
|
gr_complex d_L_accu;
|
||||||
|
gr_complex d_VL_accu;
|
||||||
|
gr_complex d_P_data_accu;
|
||||||
|
|
||||||
// nominal signal parameters
|
// nominal signal parameters
|
||||||
double d_signal_carrier_freq;
|
double d_signal_carrier_freq;
|
||||||
@ -171,37 +194,17 @@ private:
|
|||||||
double d_rem_code_phase_chips;
|
double d_rem_code_phase_chips;
|
||||||
double d_rem_code_phase_samples;
|
double d_rem_code_phase_samples;
|
||||||
|
|
||||||
gr_complex *d_Very_Early;
|
|
||||||
gr_complex *d_Early;
|
|
||||||
gr_complex *d_Prompt;
|
|
||||||
gr_complex *d_Late;
|
|
||||||
gr_complex *d_Very_Late;
|
|
||||||
|
|
||||||
gr_complex d_VE_accu;
|
|
||||||
gr_complex d_E_accu;
|
|
||||||
gr_complex d_P_accu;
|
|
||||||
gr_complex d_P_accu_old;
|
|
||||||
gr_complex d_L_accu;
|
|
||||||
gr_complex d_VL_accu;
|
|
||||||
gr_complex d_P_data_accu;
|
|
||||||
|
|
||||||
std::string d_secondary_code_string;
|
|
||||||
std::string d_data_secondary_code_string;
|
|
||||||
std::string d_systemName;
|
|
||||||
std::string d_signal_type;
|
|
||||||
std::string d_signal_pretty_name;
|
|
||||||
std::string d_dump_filename;
|
|
||||||
|
|
||||||
std::ofstream d_dump_file;
|
|
||||||
|
|
||||||
uint64_t d_sample_counter;
|
uint64_t d_sample_counter;
|
||||||
uint64_t d_acq_sample_stamp;
|
uint64_t d_acq_sample_stamp;
|
||||||
|
|
||||||
float *d_prompt_data_shift;
|
float *d_prompt_data_shift;
|
||||||
float d_rem_carr_phase_rad;
|
float d_rem_carr_phase_rad;
|
||||||
|
|
||||||
|
uint32_t d_channel;
|
||||||
|
uint32_t d_secondary_code_length;
|
||||||
|
uint32_t d_data_secondary_code_length;
|
||||||
|
|
||||||
int32_t d_symbols_per_bit;
|
int32_t d_symbols_per_bit;
|
||||||
int32_t d_preamble_length_symbols;
|
|
||||||
int32_t d_state;
|
int32_t d_state;
|
||||||
int32_t d_correlation_length_ms;
|
int32_t d_correlation_length_ms;
|
||||||
int32_t d_n_correlator_taps;
|
int32_t d_n_correlator_taps;
|
||||||
@ -215,10 +218,6 @@ private:
|
|||||||
int32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled)
|
int32_t d_code_samples_per_chip; // All signals have 1 sample per chip code except Gal. E1 which has 2 (CBOC disabled) or 12 (CBOC enabled)
|
||||||
int32_t d_code_length_chips;
|
int32_t d_code_length_chips;
|
||||||
|
|
||||||
uint32_t d_channel;
|
|
||||||
uint32_t d_secondary_code_length;
|
|
||||||
uint32_t d_data_secondary_code_length;
|
|
||||||
|
|
||||||
bool d_pull_in_transitory;
|
bool d_pull_in_transitory;
|
||||||
bool d_corrected_doppler;
|
bool d_corrected_doppler;
|
||||||
bool d_interchange_iq;
|
bool d_interchange_iq;
|
||||||
|
Loading…
Reference in New Issue
Block a user