From 54c3d6f964cb099c540277e6558a0a19e1845ed0 Mon Sep 17 00:00:00 2001 From: "M.A.Gomez" Date: Sat, 17 Dec 2022 12:46:22 +0000 Subject: [PATCH] MOD: debug in trk_cmd update --- src/algorithms/PVT/libs/vtl_engine.cc | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index a0ac10e00..d9055f7f6 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -232,20 +232,20 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_yerr = kf_H * kf_xerr; // Filtered pseudorange error measurement (in m) AND Filtered Doppler shift measurements (in Hz): - for (int32_t i = 0; i < new_data.sat_number; i++) // Measurement vector + for (int32_t channel = 0; channel < new_data.sat_number; channel++) // Measurement vector { - rho_pri_filt(i) = new_data.pr_m(i) + kf_yerr(i); // now filtered - rhoDot_pri_filt(i) = (new_data.doppler_hz(i) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(i + new_data.sat_number); // now filtered + rho_pri_filt(channel) = new_data.pr_m(channel) + kf_yerr(channel); // now filtered + rhoDot_pri_filt(channel) = (new_data.doppler_hz(channel) * Lambda_GPS_L1 + kf_x(7)) - kf_yerr(channel + new_data.sat_number); // now filtered // TO DO: convert rhoDot_pri to doppler shift! // Doppler shift defined as pseudorange rate measurement divided by the negative of carrier wavelength. - doppler_hz_filt(i) = (rhoDot_pri_filt(i) - kf_x(7)) / Lambda_GPS_L1; + doppler_hz_filt(channel) = (rhoDot_pri_filt(channel) - kf_x(7)) / Lambda_GPS_L1; //TODO: Fill the tracking commands outputs // Notice: keep the same satellite order as in the Vtl_Data matrices // sample code TrackingCmd trk_cmd; trk_cmd.carrier_phase_rads = 0; // difficult of calculation - trk_cmd.carrier_freq_hz = doppler_hz_filt(i) + kf_x(7)/Lambda_GPS_L1; // this is el doppler WITHOUTH sintony correction + 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 = 0; trk_cmd.code_phase_chips = 0; trk_cmd.enable_carrier_nco_cmd = true; @@ -253,6 +253,12 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) trk_cmd.sample_counter = new_data.sample_counter; trk_cmd.channel_id = 0; trk_cmd_outs.push_back(trk_cmd); + if(channel==0) + { + std::cout << "CH " << channel + << " Doppler vtl commanded: " << doppler_hz_filt(channel) << " [Hz]" + << " \n"; + } }