mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-09-05 20:37:59 +00:00
Code Issues Releases Wiki Activity

feat: rebasing vector tracking branch with next branch updates

Browse Source
This commit is contained in:
pedromiguelcp
2025-07-02 15:23:41 +01:00
committed by Carles Fernandez
parent d86e813a57
commit bf63c3e808
7 changed files with 212 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@@ -90,6 +90,32 @@ Rtklib_Pvt::Rtklib_Pvt(const ConfigurationInterface* configuration,
SensorDataSourceConfiguration sensor_data_configuration{configuration};
pvt_output_parameters.kf_use_imu_vel = configuration->property(role + ".kf_use_imu_vel", false);
// PVT VTL settings
const int gps_1C_count = configuration->property("Channels_1C.count", 0);
const int gps_2S_count = configuration->property("Channels_2S.count", 0);
const int gps_L5_count = configuration->property("Channels_L5.count", 0);
const int gal_1B_count = configuration->property("Channels_1B.count", 0);
const int gal_E5a_count = configuration->property("Channels_5X.count", 0);
const int gal_E5b_count = configuration->property("Channels_7X.count", 0);
const int gal_E6_count = configuration->property("Channels_E6.count", 0);
pvt_output_parameters.enable_pvt_vtl = configuration->property(role + ".enable_pvt_vtl", false);
pvt_output_parameters.enable_pvt_output_vtl = configuration->property(role + ".enable_pvt_output_vtl", false);
pvt_output_parameters.enable_pvt_closure_vtl = configuration->property(role + ".enable_pvt_closure_vtl", false);
pvt_output_parameters.vtl_kinematic = configuration->property(role + ".vtl_kinematic", false);
pvt_output_parameters.vtl_dump = configuration->property(role + ".vtl_dump", pvt_output_parameters.vtl_dump);
pvt_output_parameters.vtl_dump_filename = configuration->property(role + ".vtl_dump_filename", pvt_output_parameters.vtl_dump_filename);
pvt_output_parameters.vtl_gps_channels = gps_1C_count + gps_2S_count + gps_L5_count;
pvt_output_parameters.vtl_gal_channels = gal_1B_count + gal_E5a_count + gal_E5b_count + gal_E6_count;
pvt_output_parameters.vtl_init_pos_ecef_sd_m = configuration->property(role + ".vtl_init_pos_ecef_sd_m", 10.0);
pvt_output_parameters.vtl_init_vel_ecef_sd_ms = configuration->property(role + ".vtl_init_vel_ecef_sd_ms", 5.0);
pvt_output_parameters.vtl_init_clk_b_sd_m = configuration->property(role + ".vtl_init_clk_b_sd_m", 100.0);
pvt_output_parameters.vtl_init_clk_d_sd_ms = configuration->property(role + ".vtl_init_clk_d_sd_ms", 100.0);
pvt_output_parameters.vtl_sys_acc_noise_sd_ms2 = configuration->property(role + ".vtl_sys_acc_noise_sd_ms2", 0.1);
pvt_output_parameters.vtl_sys_clk_b_noise_sd_m = configuration->property(role + ".vtl_sys_clk_b_noise_sd_m", 0);
pvt_output_parameters.vtl_sys_clk_d_noise_sd_ms = configuration->property(role + ".vtl_sys_clk_d_noise_sd_ms", 0);
pvt_output_parameters.vtl_meas_prange_sd_m = configuration->property(role + ".vtl_meas_prange_sd_m", 7.75);
pvt_output_parameters.vtl_meas_prange_rate_sd_ms = configuration->property(role + ".vtl_meas_prange_rate_sd_ms", 0.45);
// NMEA Printer settings
pvt_output_parameters.flag_nmea_tty_port = configuration->property(role + ".flag_nmea_tty_port", false);
pvt_output_parameters.nmea_dump_filename = configuration->property(role + ".nmea_dump_filename", default_nmea_dump_filename);

2
src/algorithms/PVT/libs/CMakeLists.txt Normal file → Executable file
View File

@@ -25,6 +25,8 @@ set(PVT_LIB_SOURCES
geohash.cc
pvt_kf.cc
signal_enabled_flags.cc
receiver_type.cc
vtl_data.cc
)
set(PVT_LIB_HEADERS

19
src/algorithms/PVT/libs/pvt_conf.h
View File

@@ -106,6 +106,25 @@ public:
// Sensor Data parameters
bool kf_use_imu_vel = false;
// PVT VTL parameters
bool enable_pvt_vtl = false;
bool enable_pvt_output_vtl = false;
bool enable_pvt_closure_vtl = false;
bool vtl_kinematic = false;
bool vtl_dump = false;
std::string vtl_dump_filename;
int vtl_gps_channels = 5;
int vtl_gal_channels = 5;
double vtl_init_pos_ecef_sd_m = 10.0;
double vtl_init_vel_ecef_sd_ms = 5.0;
double vtl_init_clk_b_sd_m = 100.0;
double vtl_init_clk_d_sd_ms = 100.0;
double vtl_sys_acc_noise_sd_ms2 = 0.1;
double vtl_sys_clk_b_noise_sd_m = 0.0;
double vtl_sys_clk_d_noise_sd_ms = 0.0;
double vtl_meas_prange_sd_m = 7.75;
double vtl_meas_prange_rate_sd_ms = 0.45;
};

9
src/algorithms/PVT/libs/rtklib_solver.cc Normal file → Executable file
View File

@@ -57,7 +57,8 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
d_conf(conf),
d_signal_enabled_flags(signal_enabled_flags),
d_flag_dump_enabled(flag_dump_to_file),
d_flag_dump_mat_enabled(flag_dump_to_mat)
d_flag_dump_mat_enabled(flag_dump_to_mat),
vtl_data(nullptr)
{
// see freq index at src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
// function: satwavelen
@@ -149,6 +150,12 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk,
}
}
}
if (d_conf.enable_pvt_vtl == true)
{
vtl_data = std::make_unique<Vtl_Data>();
vtl_data->init_storage(d_conf.vtl_gps_channels + d_conf.vtl_gal_channels);
}
}

4
src/algorithms/PVT/libs/rtklib_solver.h Normal file → Executable file
View File

@@ -62,6 +62,7 @@
#include "rtklib.h"
#include "rtklib_conversions.h"
#include "sensor_data/sensor_data_aggregator.h"
#include "vtl_data.h"
#include <array>
#include <cstdint>
#include <fstream>
@@ -158,6 +159,9 @@ private:
uint32_t d_signal_enabled_flags;
bool d_flag_dump_enabled;
bool d_flag_dump_mat_enabled;
// vector tracking
std::unique_ptr<Vtl_Data> vtl_data;
};

67
src/algorithms/PVT/libs/vtl_data.cc Executable file
View File

@@ -0,0 +1,67 @@
/*!
* \file vtl_data.c
* \brief Class that exchange information to and from the Vector Tracking Loop (VTL)
* \author Pedro Pereira, 2025. pereirapedrocp@gmail.com
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#include "vtl_data.h"
#include "armadillo"
#include "vector"
Vtl_Data::Vtl_Data() = default;
void Vtl_Data::init_storage(int N_sv)
{
rx_ch = arma::vec(N_sv);
rx_ch2 = arma::vec(N_sv);
rx_p = arma::mat(1, 3);
rx_v = arma::mat(1, 3);
rx_clk = arma::mat(1, 3);
sv_id = arma::vec(N_sv);
sv_p = arma::mat(N_sv, 3);
sv_v = arma::mat(N_sv, 3);
sv_clk = arma::mat(N_sv, 2);
sv_elev = arma::vec(N_sv);
tropo_bias = arma::vec(N_sv);
iono_bias = arma::vec(N_sv);
code_bias = arma::vec(N_sv);
band = arma::vec(N_sv);
code_freq = arma::vec(N_sv);
CN0_dB_hz = arma::vec(N_sv);
ch_sample_counter = arma::vec(N_sv);
ch2_sample_counter = arma::vec(N_sv);
ionoopt = 0;
obs_pr = arma::vec(N_sv);
obs_prr = arma::vec(N_sv);
active_ch = arma::vec(N_sv);
past_active_ch = arma::vec(N_sv);
new_ch = arma::vec(N_sv);
old_ch = arma::vec(N_sv);
loop_closure = arma::vec(N_sv);
rx_time = 0.0;
dt_s = 0.0;
}
void Vtl_Data::clear_storage()
{
active_ch.zeros();
new_ch.zeros();
old_ch.zeros();
loop_closure.zeros();
}

86
src/algorithms/PVT/libs/vtl_data.h Normal file
View File

@@ -0,0 +1,86 @@
/*!
* \file vtl_data.h
* \brief Class that exchange information to and from the Vector Tracking Loop (VTL)
* \author Pedro Pereira, 2025. pereirapedrocp@gmail.com
*
* -----------------------------------------------------------------------------
*
* GNSS-SDR is a Global Navigation Satellite System software-defined receiver.
* This file is part of GNSS-SDR.
*
* Copyright (C) 2010-2022 (see AUTHORS file for a list of contributors)
* SPDX-License-Identifier: GPL-3.0-or-later
*
* -----------------------------------------------------------------------------
*/
#ifndef GNSS_SDR_VTL_DATA_H
#define GNSS_SDR_VTL_DATA_H
// constants definition
constexpr double Lambda_GPS_L1 = 0.1902936728;
constexpr double Lambda_GPS_L5 = 0.2548280488;
constexpr double L1E1_CODE_FREQ = 1023000;
constexpr double L5E5_CODE_FREQ = 10230000;
constexpr double RANGE_TO_FREQ_L1E1_FACTOR = 0.0034123607; // L1E1_CODE_FREQ / SPEED_OF_LIGHT_M_S
constexpr double RANGE_TO_FREQ_L5E5_FACTOR = 0.0341236069; // L5E5_CODE_FREQ / SPEED_OF_LIGHT_M_S
#include <armadillo>
#include <cstdint>
#include <string>
#include <vector>
/** \addtogroup PVT
* \{ */
/** \addtogroup PVT_libs
* \{ */
class Vtl_Data
{
public:
Vtl_Data();
void init_storage(int N_sv);
void clear_storage();
arma::mat rx_ch; // receiver channel
arma::mat rx_ch2; // receiver channel - second frequency
arma::mat rx_p; // receiver position [m]
arma::mat rx_v; // receiver velocity [m/s]
arma::mat rx_clk; // receiver clock bias and drift [s,m/s]
arma::mat sv_id; // satellite ID
arma::mat sv_p; // satellite position [m]
arma::mat sv_v; // satellite velocity [m/s]
arma::mat sv_clk; // satellite clock bias and drift [s,m/s]
arma::colvec sv_elev; // satellite elevation [rad]
arma::colvec tropo_bias; // troposphere bias [m]
arma::colvec iono_bias; // ionosphere bias [m]
arma::colvec code_bias; // code bias [m]
arma::colvec band; // frequency band
arma::colvec code_freq;
arma::colvec CN0_dB_hz;
arma::colvec ch_sample_counter;
arma::colvec ch2_sample_counter;
int ionoopt; // ionosphere option
arma::colvec obs_pr; // observed pseudorange [m]
arma::colvec obs_prr; // observed pseudorange rate [m/s]
arma::colvec active_ch; // active channels
arma::colvec past_active_ch; // past active channels
arma::colvec new_ch; // new channels
arma::colvec old_ch; // old channels
u_int8_t N_sv; // number of satellites for pvt
u_int8_t active_N_gps_ch; // active gps channels
u_int8_t active_N_gal_ch; // active gps channels
arma::colvec loop_closure; // vtl loop closure
double rx_time;
double dt_s;
};
/** \} */
/** \} */
#endif // GNSS_SDR_VTL_DATA_H