mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-15 22:34:58 +00:00
Code Issues Releases Wiki Activity

FORMAT: clang-format applied

Browse Source
This commit is contained in:
M.A.Gomez 2023-03-11 19:32:16 +00:00
parent fd69416f4e
commit b815ee4d9d
6 changed files with 489 additions and 471 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -353,7 +353,6 @@ bool Rtklib_Solver::save_vtl_matfile() const
matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73);
if (reinterpret_cast<int64_t *>(matfp) != nullptr) if (reinterpret_cast<int64_t *>(matfp) != nullptr)
{ {
std::array<size_t, 2> dims{1, static_cast<size_t>(num_epoch)}; std::array<size_t, 2> dims{1, static_cast<size_t>(num_epoch)};
matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
@ -474,7 +473,6 @@ bool Rtklib_Solver::save_vtl_matfile() const
matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0); matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0);
Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE
Mat_VarFree(matvar); Mat_VarFree(matvar);
} }
Mat_Close(matfp); Mat_Close(matfp);
@ -1893,7 +1891,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s] vtl_data.rx_dts(1) = pvt_sol.dtr[5] / 1e6; // [ppm] to [s]
vtl_engine.vtl_loop(vtl_data); vtl_engine.vtl_loop(vtl_data);
} }
else else
{ {
@ -2088,7 +2085,6 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
try try
{ {
double tmp_double; double tmp_double;
uint32_t tmp_uint32; uint32_t tmp_uint32;
// TOW // TOW

4
src/algorithms/PVT/libs/vtl_data.cc Executable file → Normal file
View File

@ -16,8 +16,8 @@
#include "vtl_data.h" #include "vtl_data.h"
#include "vector"
#include "armadillo" #include "armadillo"
#include "vector"
Vtl_Data::Vtl_Data() Vtl_Data::Vtl_Data()
{ {
@ -66,5 +66,3 @@ void Vtl_Data::debug_print()
//doppler_hz.print("satellite Carrier Dopplers [Hz]"); //doppler_hz.print("satellite Carrier Dopplers [Hz]");
// carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]");
} }

0
src/algorithms/PVT/libs/vtl_data.h Executable file → Normal file
View File

67
src/algorithms/PVT/libs/vtl_engine.cc Executable file → Normal file
View File

@ -47,7 +47,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
static double delta_t_cmd = 0; static double delta_t_cmd = 0;
bool flag_cmd = false; bool flag_cmd = false;
delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command delta_t_cmd = delta_t_cmd + delta_t_vtl; // update timer for vtl trk command
if(delta_t_cmd>=0.3){ if (delta_t_cmd >= 0.3)
{
flag_cmd = true; flag_cmd = true;
delta_t_cmd = 0; // reset timer for vtl trk command delta_t_cmd = 0; // reset timer for vtl trk command
} }
@ -69,7 +70,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
kf_yerr = arma::zeros(3 * new_data.sat_number, 1); kf_yerr = arma::zeros(3 * new_data.sat_number, 1);
kf_xerr = arma::zeros(n_of_states, 1); kf_xerr = arma::zeros(n_of_states, 1);
kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix kf_S = arma::zeros(3 * new_data.sat_number, 3 * new_data.sat_number); // kf_P_y innovation covariance matrix
kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number); ; kf_K = arma::zeros(n_of_states, 3 * new_data.sat_number);
;
// ################## Kalman Tracking ###################################### // ################## Kalman Tracking ######################################
counter++; //uint64_t counter++; //uint64_t
//new_data.kf_state.print("new_data kf initial"); //new_data.kf_state.print("new_data kf initial");
@ -222,16 +224,22 @@ bool Vtl_Engine::vtl_loop(Vtl_Data new_data)
trk_cmd.carrier_freq_rate_hz_s = -(a_x(channel) * kf_x(6) + a_y(channel) * kf_x(7) + a_z(channel) * kf_x(8)) / Lambda_GPS_L1; trk_cmd.carrier_freq_rate_hz_s = -(a_x(channel) * kf_x(6) + a_y(channel) * kf_x(7) + a_z(channel) * kf_x(8)) / Lambda_GPS_L1;
trk_cmd.code_phase_chips = 0; //kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3; trk_cmd.code_phase_chips = 0; //kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3;
if (flag_cmd){ if (flag_cmd)
{
trk_cmd.enable_carrier_nco_cmd = true; trk_cmd.enable_carrier_nco_cmd = true;
}else{ }
else
{
trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue trk_cmd.enable_carrier_nco_cmd = false; // do NOT apply corrections! loop convergence issue
} }
if(counter<1500){ if (counter < 1500)
{
// std::cout<<"yet to soon"<<std::endl; // std::cout<<"yet to soon"<<std::endl;
trk_cmd.enable_code_nco_cmd = false; // do NOT apply corrections! initial convergence issue trk_cmd.enable_code_nco_cmd = false; // do NOT apply corrections! initial convergence issue
}else{ }
else
{
trk_cmd.enable_code_nco_cmd = true; trk_cmd.enable_code_nco_cmd = true;
} }
trk_cmd.sample_counter = new_data.sample_counter; trk_cmd.sample_counter = new_data.sample_counter;
@ -331,9 +339,12 @@ bool Vtl_Engine::kf_H_fill(arma::mat &kf_H,int sat_number, arma::colvec ax, arma
bool Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt) bool Vtl_Engine::kf_F_fill(arma::mat &kf_F, double kf_dt)
{ {
kf_F(0, 3) = kf_dt; kf_F(0, 6) = kf_dt*kf_dt/2; kf_F(0, 3) = kf_dt;
kf_F(1, 4) = kf_dt; kf_F(1, 7) = kf_dt*kf_dt/2; kf_F(0, 6) = kf_dt * kf_dt / 2;
kf_F(2, 5) = kf_dt; kf_F(2, 8) = kf_dt*kf_dt/2; kf_F(1, 4) = kf_dt;
kf_F(1, 7) = kf_dt * kf_dt / 2;
kf_F(2, 5) = kf_dt;
kf_F(2, 8) = kf_dt * kf_dt / 2;
kf_F(3, 6) = kf_dt; kf_F(3, 6) = kf_dt;
kf_F(4, 7) = kf_dt; kf_F(4, 7) = kf_dt;
@ -395,16 +406,21 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
//modulo de la velocidad //modulo de la velocidad
double u = norm(u_vec, 2); double u = norm(u_vec, 2);
if(counter>1500){ if (counter > 1500)
if(u>6){ {
if (u > 6)
{
t_disparo = t_disparo + dt; t_disparo = t_disparo + dt;
std::cout << "u : " << u << endl; std::cout << "u : " << u << endl;
double diam_cohete = 120.0e-3; // 120 mm double diam_cohete = 120.0e-3; // 120 mm
double mass_rocket = 50.0; //50Kg double mass_rocket = 50.0; //50Kg
if(t_disparo<.2){ if (t_disparo < .2)
{
u_dir = {.90828, -.13984, -.388756}; u_dir = {.90828, -.13984, -.388756};
}else{ }
else
{
u_dir = u_vec / u; u_dir = u_vec / u;
} }
// u_dir.print("u_dir"); // u_dir.print("u_dir");
@ -418,8 +434,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
Empuje = EmpujeLkTable(t_disparo); Empuje = EmpujeLkTable(t_disparo);
// cout<<"Empuje: "<<Empuje<<endl; // cout<<"Empuje: "<<Empuje<<endl;
acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir acc_vec = -(GNSS_PI * densidad * diam_cohete * diam_cohete / 8) * ballistic_coef * u * u_dir + gravity_ECEF + Empuje * u_dir;
+gravity_ECEF+Empuje*u_dir;
// acc_vec.print("acc_vec"); // acc_vec.print("acc_vec");
// % return // % return
@ -427,15 +442,18 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
acc_x = acc_vec(0); acc_x = acc_vec(0);
acc_y = acc_vec(1); acc_y = acc_vec(1);
acc_z = acc_vec(2); acc_z = acc_vec(2);
}else{ }
else
{
t_disparo = 0; t_disparo = 0;
// % return // % return
acc_x = 0; acc_x = 0;
acc_y = 0; acc_y = 0;
acc_z = 0; acc_z = 0;
} }
}else{ }
else
{
// % return // % return
acc_x = 0; acc_x = 0;
acc_y = 0; acc_y = 0;
@ -622,21 +640,25 @@ double Vtl_Engine::EmpujeLkTable(double t_disparo)
{1.70982658959538, 529.326074922632}, {1.70982658959538, 529.326074922632},
{1.71988439306358, 532.152158438731}, {1.71988439306358, 532.152158438731},
{1.72994219653179, 534.995939192065}, {1.72994219653179, 534.995939192065},
{1.74000000000000, 537.866310625605},}; {1.74000000000000, 537.866310625605},
};
//encuentra el mas cercano justo anterior. //encuentra el mas cercano justo anterior.
// int index_E = LkTable.elem(find(LkTable<=t_disparo)).max(); // int index_E = LkTable.elem(find(LkTable<=t_disparo)).max();
arma::uvec index_E = find(LkTable <= t_disparo, 1, "last"); arma::uvec index_E = find(LkTable <= t_disparo, 1, "last");
// index_E.print("indice E: "); // index_E.print("indice E: ");
// uint kk = index_E(0); // uint kk = index_E(0);
if (index_E(0)<(LkTable.n_rows-1)){ if (index_E(0) < (LkTable.n_rows - 1))
{
double tdisparo1 = LkTable(index_E(0), 0); double tdisparo1 = LkTable(index_E(0), 0);
double tdisparo2 = LkTable(index_E(0) + 1, 0); double tdisparo2 = LkTable(index_E(0) + 1, 0);
double E1 = LkTable(index_E(0), 1); double E1 = LkTable(index_E(0), 1);
double E2 = LkTable(index_E(0) + 1, 1); double E2 = LkTable(index_E(0) + 1, 1);
E = (t_disparo - tdisparo1) * (E2 - E1) / (tdisparo2 - tdisparo1) + E1; E = (t_disparo - tdisparo1) * (E2 - E1) / (tdisparo2 - tdisparo1) + E1;
}else{ }
else
{
E = 0; E = 0;
} }
@ -704,14 +726,11 @@ std::vector<double> Vtl_Engine::get_accel_var_ecef_m_s2()
double Vtl_Engine::get_latitude() double Vtl_Engine::get_latitude()
{ {
return -1.0; return -1.0;
} }
double Vtl_Engine::get_longitude() double Vtl_Engine::get_longitude()
{ {
return -1.0; return -1.0;
} }

2
src/algorithms/PVT/libs/vtl_engine.h Executable file → Normal file
View File

@ -17,10 +17,10 @@
#ifndef GNSS_SDR_VTL_ENGINE_H #ifndef GNSS_SDR_VTL_ENGINE_H
#define GNSS_SDR_VTL_ENGINE_H #define GNSS_SDR_VTL_ENGINE_H
#include "MATH_CONSTANTS.h"
#include "trackingcmd.h" #include "trackingcmd.h"
#include "vtl_conf.h" #include "vtl_conf.h"
#include "vtl_data.h" #include "vtl_data.h"
#include "MATH_CONSTANTS.h"
#include <armadillo> #include <armadillo>
#include <cstdint> #include <cstdint>
#include <string> #include <string>

23
src/algorithms/tracking/gnuradio_blocks/kf_tracking.cc
View File

@ -49,16 +49,16 @@
#include <matio.h> // for Mat_VarCreate #include <matio.h> // for Mat_VarCreate
#include <pmt/pmt_sugar.h> // for mp #include <pmt/pmt_sugar.h> // for mp
#include <volk_gnsssdr/volk_gnsssdr.h> #include <volk_gnsssdr/volk_gnsssdr.h>
#include "iostream"
#include <algorithm> // for fill_n #include <algorithm> // for fill_n
#include <array> #include <array>
#include <cmath> // for fmod, round, floor #include <cmath> // for fmod, round, floor
#include <exception> // for exception #include <exception> // for exception
#include <fstream>
#include <iostream> // for cout, cerr #include <iostream> // for cout, cerr
#include <map> #include <map>
#include <numeric> #include <numeric>
#include <vector> #include <vector>
#include "iostream"
#include <fstream>
#if HAS_GENERIC_LAMBDA #if HAS_GENERIC_LAMBDA
#else #else
@ -637,7 +637,6 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
//std::cout<< "test cast CH "<<cmd->sample_counter <<"\n"; //std::cout<< "test cast CH "<<cmd->sample_counter <<"\n";
if (cmd->channel_id == this->d_channel) if (cmd->channel_id == this->d_channel)
{ {
arma::vec x_tmp; arma::vec x_tmp;
arma::mat F_tmp; arma::mat F_tmp;
@ -657,14 +656,19 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
double old_doppler_rate = d_x_old_old(3); double old_doppler_rate = d_x_old_old(3);
double old_code_phase_chips = d_x_old_old(0); double old_code_phase_chips = d_x_old_old(0);
if(cmd->enable_carrier_nco_cmd){ if (cmd->enable_carrier_nco_cmd)
if(cmd->enable_code_nco_cmd){ {
if(abs(d_x_old_old(2) - tmp_x(2))>50){ if (cmd->enable_code_nco_cmd)
{
if (abs(d_x_old_old(2) - tmp_x(2)) > 50)
{
std::cout << "channel: " << this->d_channel std::cout << "channel: " << this->d_channel
<< " tracking_cmd TOO FAR: " << " tracking_cmd TOO FAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz" << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
<< " \n"; << " \n";
}else{ }
else
{
std::cout << "channel: " << this->d_channel std::cout << "channel: " << this->d_channel
<< " tracking_cmd NEAR: " << " tracking_cmd NEAR: "
<< abs(d_x_old_old(2) - tmp_x(2)) << "Hz" << abs(d_x_old_old(2) - tmp_x(2)) << "Hz"
@ -672,8 +676,9 @@ void kf_tracking::msg_handler_pvt_to_trk(const pmt::pmt_t &msg)
} }
d_x_old_old(2) = tmp_x(2); //replace DOPPLER d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE
}
}else{ else
{
// std::cout<<"yet to soon"<<std::endl; // std::cout<<"yet to soon"<<std::endl;
//d_x_old_old(2) = tmp_x(2); //replace DOPPLER //d_x_old_old(2) = tmp_x(2); //replace DOPPLER
// d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE // d_x_old_old(3) = tmp_x(3); //replace DOPPLER RATE