mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-11-16 06:44:57 +00:00
Code Issues Releases Wiki Activity

MOD: debug in trk_cmd update

Browse Source
This commit is contained in:
M.A.Gomez 2022-12-17 12:46:22 +00:00
parent c0c1c20eb6
commit 54c3d6f964
1 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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";
}
}