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

FIX: bug in vtlEngine

Browse Source
This commit is contained in:
M.A. Gomez 2023-02-16 00:26:14 +01:00
parent 2b6f7d3c08
commit 028e71dc2f
1 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/algorithms/PVT/libs/vtl_engine.cc
View File

@ -91,7 +91,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
double acc_x = 0;
double acc_y = 0;
double acc_z = 0;
test = model3DoF(acc_x,acc_x,acc_x,kf_x,kf_dt);
test = model3DoF(acc_x,acc_y,acc_z,kf_x,kf_dt);
kf_x(6) = acc_x;
kf_x(7) = acc_y;
kf_x(8) = acc_z;
@ -208,7 +208,7 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data)
// sample code
trk_cmd.carrier_phase_rads = 0; // difficult of calculation
trk_cmd.carrier_freq_hz = doppler_hz_filt(channel); //+ kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction
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 = kf_yerr(channel)/SPEED_OF_LIGHT_M_S*1023e3;
trk_cmd.enable_carrier_nco_cmd = true;
trk_cmd.enable_code_nco_cmd = true;
@ -369,8 +369,6 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
//vector velocidad
u_vec = kf_x.rows(3, 5);
u_vec.print("u_vec");
// double u=sqrt(pow(kf_x(4),2)+pow(kf_x(5),2)+pow(kf_x(6),2));
//modulo de la velocidad
double u = norm(u_vec, 2);
@ -382,10 +380,11 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
double mass_rocket=50.0; //50Kg
if(t_disparo<.2){
u_dir={.90828, -.13984, .388756};
u_dir={.90828, -.13984, -.388756};
}else{
u_dir = u_vec / u;
}
// u_dir.print("u_dir");
// lla= ecef2lla([kf_State(1) kf_State(2) kf_State(3)]);
// [T, sound_v, P, densidad] = atmosisa(lla(3));
// sound_v=320;% @ 5km and -17.5C
@ -399,7 +398,7 @@ bool Vtl_Engine::model3DoF(double &acc_x,double &acc_y,double &acc_z,arma::mat k
acc_vec = -(GNSS_PI*densidad*diam_cohete*diam_cohete/8)*ballistic_coef*u*u_dir
+gravity_ECEF+Empuje*u_dir;
//acc_vec.print("acc_vec");
// acc_vec.print("acc_vec");
// % return
acc_x = acc_vec(0);
acc_y = acc_vec(1);