From 6967fc833ee15833a5689b73bd8cdbf5dbe33bde Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 19 Jul 2014 19:29:27 +0200 Subject: [PATCH] Fixes rounding errors in frequency to period conversion that were causing tracking losses. Bug spotted by Marc Sales. --- .../galileo_e1_dll_pll_veml_tracking_cc.cc | 28 ++++----- .../galileo_e1_dll_pll_veml_tracking_cc.h | 6 +- .../galileo_e1_tcp_connector_tracking_cc.cc | 54 ++++++++-------- .../galileo_e1_tcp_connector_tracking_cc.h | 6 +- .../gps_l1_ca_dll_fll_pll_tracking_cc.cc | 11 ++-- .../gps_l1_ca_dll_pll_optim_tracking_cc.cc | 61 +++++++++---------- .../gps_l1_ca_dll_pll_optim_tracking_cc.h | 6 +- .../gps_l1_ca_dll_pll_tracking_cc.cc | 58 +++++++++--------- .../gps_l1_ca_dll_pll_tracking_cc.h | 6 +- .../gps_l1_ca_tcp_connector_tracking_cc.cc | 53 ++++++++-------- .../gps_l1_ca_tcp_connector_tracking_cc.h | 18 +++--- 11 files changed, 152 insertions(+), 155 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc index a04dc59fc..d1e8957f8 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.cc @@ -7,11 +7,11 @@ * Code DLL + carrier PLL according to the algorithms described in: * [1] K.Borre, D.M.Akos, N.Bertelsen, P.Rinder, and S.H.Jensen, * A Software-Defined GPS and Galileo Receiver. A Single-Frequency - * Approach, Birkha user, 2007 + * Approach, Birkhauser, 2007 * * ------------------------------------------------------------------------- * - * Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors) + * Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors) * * GNSS-SDR is a software defined Global Navigation * Satellite Systems receiver @@ -21,7 +21,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -37,6 +37,7 @@ #include "galileo_e1_dll_pll_veml_tracking_cc.h" #include #include +#include #include #include #include @@ -148,7 +149,7 @@ galileo_e1_dll_pll_veml_tracking_cc::galileo_e1_dll_pll_veml_tracking_cc( //--- Initializations ------------------------------ // Initial code frequency basis of NCO - d_code_freq_chips = Galileo_E1_CODE_CHIP_RATE_HZ; + d_code_freq_chips = (double)Galileo_E1_CODE_CHIP_RATE_HZ; // Residual code phase (in chips) d_rem_code_phase_samples = 0.0; // Residual carrier phase @@ -369,7 +370,7 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect //carrier phase accumulator for (K) Doppler estimation d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; //remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI* d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * Galileo_E1_CODE_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); // ################## DLL ########################################################## @@ -385,15 +386,15 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer - float T_chip_seconds; - float T_prn_seconds; - float T_prn_samples; - float K_blk_samples; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; // Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / d_code_freq_chips; + T_chip_seconds = 1 / (double)d_code_freq_chips; T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * (float)d_fs_in; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs*(float)d_fs_in; + T_prn_samples = T_prn_seconds * (double)d_fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (double)d_fs_in; d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample @@ -427,12 +428,11 @@ int galileo_e1_dll_pll_veml_tracking_cc::general_work (int noutput_items,gr_vect { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - ControlMessageFactory* cmf = new ControlMessageFactory(); + std::unique_ptr cmf(new ControlMessageFactory()); if (d_queue != gr::msg_queue::sptr()) { d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); } - delete cmf; d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine } diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h index c65874a56..e93ae0a93 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_dll_pll_veml_tracking_cc.h @@ -16,7 +16,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -145,7 +145,7 @@ private: gr_complex *d_Very_Late; // remaining code phase and carrier phase between tracking loops - float d_rem_code_phase_samples; + double d_rem_code_phase_samples; float d_rem_carr_phase_rad; // PLL and DLL filter library @@ -160,7 +160,7 @@ private: Correlator d_correlator; // tracking vars - float d_code_freq_chips; + double d_code_freq_chips; float d_carrier_doppler_hz; double d_acc_carrier_phase_rad; double d_acc_code_phase_secs; diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc index cfa25a851..fb744618e 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.cc @@ -39,6 +39,7 @@ #include "galileo_e1_tcp_connector_tracking_cc.h" #include #include +#include #include #include #include @@ -391,20 +392,20 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve code_error_filt_chips = tcp_data.proc_pack_code_error; //Code phase accumulator float code_error_filt_secs; - code_error_filt_secs=(Galileo_E1_CODE_PERIOD*code_error_filt_chips)/Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] - d_acc_code_phase_secs=d_acc_code_phase_secs+code_error_filt_secs; + code_error_filt_secs = (Galileo_E1_CODE_PERIOD * code_error_filt_chips) / Galileo_E1_CODE_CHIP_RATE_HZ; //[seconds] + d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer - float T_chip_seconds; - float T_prn_seconds; - float T_prn_samples; - float K_blk_samples; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; // Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / d_code_freq_chips; + T_chip_seconds = 1 / (double)d_code_freq_chips; T_prn_seconds = T_chip_seconds * Galileo_E1_B_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * (float)d_fs_in; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs*(float)d_fs_in; + T_prn_samples = T_prn_seconds * (double)d_fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (double)d_fs_in; d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample @@ -438,12 +439,11 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - ControlMessageFactory* cmf = new ControlMessageFactory(); + std::unique_ptr cmf(new ControlMessageFactory()); if (d_queue != gr::msg_queue::sptr()) { d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); } - delete cmf; d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine } @@ -489,23 +489,23 @@ int Galileo_E1_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_ve } else { - // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) - /*! - * \todo The stop timer has to be moved to the signal source! - */ - // stream to collect cout calls to improve thread safety - std::stringstream tmp_str_stream; - if (floor(d_sample_counter / d_fs_in) != d_last_seg) - { - d_last_seg = floor(d_sample_counter / d_fs_in); + // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) + /*! + * \todo The stop timer has to be moved to the signal source! + */ + // stream to collect cout calls to improve thread safety + std::stringstream tmp_str_stream; + if (floor(d_sample_counter / d_fs_in) != d_last_seg) + { + d_last_seg = floor(d_sample_counter / d_fs_in); - if (d_channel == 0) - { - // debug: Second counter in channel 0 - tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; - std::cout << tmp_str_stream.rdbuf() << std::flush; - } - } + if (d_channel == 0) + { + // debug: Second counter in channel 0 + tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; + std::cout << tmp_str_stream.rdbuf() << std::flush; + } + } *d_Early = gr_complex(0,0); *d_Prompt = gr_complex(0,0); *d_Late = gr_complex(0,0); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h index 7427a49db..b51eb025f 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_tcp_connector_tracking_cc.h @@ -23,7 +23,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -149,7 +149,7 @@ private: gr_complex *d_Very_Late; // remaining code phase and carrier phase between tracking loops - float d_rem_code_phase_samples; + double d_rem_code_phase_samples; float d_next_rem_code_phase_samples; float d_rem_carr_phase_rad; @@ -161,7 +161,7 @@ private: Correlator d_correlator; // tracking vars - float d_code_freq_chips; + double d_code_freq_chips; float d_carrier_doppler_hz; float d_acc_carrier_phase_rad; float d_acc_code_phase_secs; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc index d5ff6d6e4..79a7272c1 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_fll_pll_tracking_cc.cc @@ -21,7 +21,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -474,12 +474,11 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - ControlMessageFactory* cmf = new ControlMessageFactory(); + std::unique_ptr cmf(new ControlMessageFactory()); if (d_queue != gr::msg_queue::sptr()) { d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); } - delete cmf; d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine } @@ -513,12 +512,12 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto double T_prn_seconds; double T_prn_samples; double K_blk_samples; - T_chip_seconds = 1/d_code_freq_hz; + T_chip_seconds = 1 / (double)d_code_freq_hz; T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; T_prn_samples = T_prn_seconds * d_fs_in; float code_error_filt_samples; - code_error_filt_samples = T_prn_seconds*code_error_filt_chips*T_chip_seconds*(float)d_fs_in; //[seconds] + code_error_filt_samples = T_prn_seconds * code_error_filt_chips * T_chip_seconds * (double)d_fs_in; //[seconds] d_acc_code_phase_samples = d_acc_code_phase_samples + code_error_filt_samples; K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_samples; @@ -529,7 +528,7 @@ int Gps_L1_Ca_Dll_Fll_Pll_Tracking_cc::general_work (int noutput_items, gr_vecto current_synchro_data.Prompt_I = (double)(*d_Prompt).real(); current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag(); // Tracking_timestamp_secs is aligned with the PRN start sample - current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + d_rem_code_phase_samples)/d_fs_in; + current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + (double)d_rem_code_phase_samples) / (double)d_fs_in; // This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, Code_phase_secs=0 current_synchro_data.Code_phase_secs = 0; current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc index e6a03e60b..18ccbb523 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.cc @@ -21,7 +21,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -36,6 +36,7 @@ #include "gps_l1_ca_dll_pll_optim_tracking_cc.h" #include +#include #include #include #include @@ -396,9 +397,9 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec // New code Doppler frequency estimation d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); //carrier phase accumulator for (K) doppler estimation - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD; + d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; //remnant carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD; + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); // ################## DLL ########################################################## @@ -408,20 +409,20 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator float code_error_filt_secs; - code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD*code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds] + code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds] d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer - float T_chip_seconds; - float T_prn_seconds; - float T_prn_samples; - float K_blk_samples; - // Compute the next buffer lenght based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / d_code_freq_chips; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; + // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation + T_chip_seconds = 1 / (double)d_code_freq_chips; T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * (float)d_fs_in; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs*(float)d_fs_in; + T_prn_samples = T_prn_seconds * (double)d_fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (double)d_fs_in; d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample @@ -452,12 +453,11 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - ControlMessageFactory* cmf = new ControlMessageFactory(); + std::unique_ptr cmf(new ControlMessageFactory()); if (d_queue != gr::msg_queue::sptr()) { d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); } - delete cmf; d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine } @@ -499,24 +499,23 @@ int Gps_L1_Ca_Dll_Pll_Optim_Tracking_cc::general_work (int noutput_items, gr_vec } else { + // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) + /*! + * \todo The stop timer has to be moved to the signal source! + */ + // stream to collect cout calls to improve thread safety + std::stringstream tmp_str_stream; + if (floor(d_sample_counter / d_fs_in) != d_last_seg) + { + d_last_seg = floor(d_sample_counter / d_fs_in); - // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) - /*! - * \todo The stop timer has to be moved to the signal source! - */ - // stream to collect cout calls to improve thread safety - std::stringstream tmp_str_stream; - if (floor(d_sample_counter / d_fs_in) != d_last_seg) - { - d_last_seg = floor(d_sample_counter / d_fs_in); - - if (d_channel == 0) - { - // debug: Second counter in channel 0 - tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; - std::cout << tmp_str_stream.rdbuf() << std::flush; - } - } + if (d_channel == 0) + { + // debug: Second counter in channel 0 + tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; + std::cout << tmp_str_stream.rdbuf() << std::flush; + } + } *d_Early = gr_complex(0,0); *d_Prompt = gr_complex(0,0); *d_Late = gr_complex(0,0); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h index 66891105a..d3cd215c6 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_optim_tracking_cc.h @@ -21,7 +21,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -134,7 +134,7 @@ private: gr_complex *d_Late; // remaining code phase and carrier phase between tracking loops - float d_rem_code_phase_samples; + double d_rem_code_phase_samples; float d_rem_carr_phase_rad; // PLL and DLL filter library @@ -148,7 +148,7 @@ private: Correlator d_correlator; // tracking vars - float d_code_freq_chips; + double d_code_freq_chips; float d_carrier_doppler_hz; float d_acc_carrier_phase_rad; float d_code_phase_samples; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc index 537f5bfc5..ad11a6354 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.cc @@ -21,7 +21,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -37,6 +37,7 @@ #include "gps_l1_ca_dll_pll_tracking_cc.h" #include #include +#include #include #include #include @@ -408,9 +409,9 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in // New code Doppler frequency estimation d_code_freq_chips = GPS_L1_CA_CODE_RATE_HZ + ((d_carrier_doppler_hz * GPS_L1_CA_CODE_RATE_HZ) / GPS_L1_FREQ_HZ); //carrier phase accumulator for (K) doppler estimation - d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI*d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD; + d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; //remanent carrier phase to prevent overflow in the code NCO - d_rem_carr_phase_rad = d_rem_carr_phase_rad+GPS_TWO_PI*d_carrier_doppler_hz*GPS_L1_CA_CODE_PERIOD; + d_rem_carr_phase_rad = d_rem_carr_phase_rad + GPS_TWO_PI * d_carrier_doppler_hz * GPS_L1_CA_CODE_PERIOD; d_rem_carr_phase_rad = fmod(d_rem_carr_phase_rad, GPS_TWO_PI); // ################## DLL ########################################################## @@ -420,20 +421,20 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips); //[chips/second] //Code phase accumulator float code_error_filt_secs; - code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD*code_error_filt_chips)/GPS_L1_CA_CODE_RATE_HZ; //[seconds] + code_error_filt_secs = (GPS_L1_CA_CODE_PERIOD * code_error_filt_chips) / GPS_L1_CA_CODE_RATE_HZ; //[seconds] d_acc_code_phase_secs = d_acc_code_phase_secs + code_error_filt_secs; // ################## CARRIER AND CODE NCO BUFFER ALIGNEMENT ####################### // keep alignment parameters for the next input buffer - float T_chip_seconds; - float T_prn_seconds; - float T_prn_samples; - float K_blk_samples; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; // Compute the next buffer length based in the new period of the PRN sequence and the code phase error estimation - T_chip_seconds = 1 / d_code_freq_chips; + T_chip_seconds = 1 / (double)d_code_freq_chips; T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * (float)d_fs_in; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs*(float)d_fs_in; + T_prn_samples = T_prn_seconds * (double)d_fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_secs * (double)d_fs_in; d_current_prn_length_samples = round(K_blk_samples); //round to a discrete samples d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error < 1 sample @@ -464,12 +465,11 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - ControlMessageFactory* cmf = new ControlMessageFactory(); + std::unique_ptr cmf(new ControlMessageFactory()); if (d_queue != gr::msg_queue::sptr()) { d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); } - delete cmf; d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine } @@ -515,23 +515,23 @@ int Gps_L1_Ca_Dll_Pll_Tracking_cc::general_work (int noutput_items, gr_vector_in } else { - // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) - /*! - * \todo The stop timer has to be moved to the signal source! - */ - // stream to collect cout calls to improve thread safety - std::stringstream tmp_str_stream; - if (floor(d_sample_counter / d_fs_in) != d_last_seg) - { - d_last_seg = floor(d_sample_counter / d_fs_in); + // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) + /*! + * \todo The stop timer has to be moved to the signal source! + */ + // stream to collect cout calls to improve thread safety + std::stringstream tmp_str_stream; + if (floor(d_sample_counter / d_fs_in) != d_last_seg) + { + d_last_seg = floor(d_sample_counter / d_fs_in); - if (d_channel == 0) - { - // debug: Second counter in channel 0 - tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; - std::cout << tmp_str_stream.rdbuf() << std::flush; - } - } + if (d_channel == 0) + { + // debug: Second counter in channel 0 + tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; + std::cout << tmp_str_stream.rdbuf() << std::flush; + } + } *d_Early = gr_complex(0,0); *d_Prompt = gr_complex(0,0); *d_Late = gr_complex(0,0); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h index 8e4ad732a..012b1c86d 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_dll_pll_tracking_cc.h @@ -21,7 +21,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -138,7 +138,7 @@ private: gr_complex *d_Late; // remaining code phase and carrier phase between tracking loops - float d_rem_code_phase_samples; + double d_rem_code_phase_samples; float d_rem_carr_phase_rad; // PLL and DLL filter library @@ -152,7 +152,7 @@ private: Correlator d_correlator; // tracking vars - float d_code_freq_chips; + double d_code_freq_chips; float d_carrier_doppler_hz; float d_acc_carrier_phase_rad; float d_code_phase_samples; diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc index 4b935d35e..b78397808 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.cc @@ -457,19 +457,19 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec // sampling frequency (fixed) d_code_phase_step_chips = d_code_freq_hz / (float)d_fs_in; //[chips] // variable code PRN sample block size - float T_chip_seconds; - float T_prn_seconds; - float T_prn_samples; - float K_blk_samples; - T_chip_seconds = 1 / d_code_freq_hz; + double T_chip_seconds; + double T_prn_seconds; + double T_prn_samples; + double K_blk_samples; + T_chip_seconds = 1 / (double)d_code_freq_hz; T_prn_seconds = T_chip_seconds * GPS_L1_CA_CODE_LENGTH_CHIPS; - T_prn_samples = T_prn_seconds * d_fs_in; + T_prn_samples = T_prn_seconds * (double)d_fs_in; d_rem_code_phase_samples = d_next_rem_code_phase_samples; - K_blk_samples = T_prn_samples + d_rem_code_phase_samples;//-code_error*(float)d_fs_in; + K_blk_samples = T_prn_samples + d_rem_code_phase_samples;//-code_error*(double)d_fs_in; // Update the current PRN delay (code phase in samples) - float T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; - float T_prn_true_samples = T_prn_true_seconds * (float)d_fs_in; + double T_prn_true_seconds = GPS_L1_CA_CODE_LENGTH_CHIPS / GPS_L1_CA_CODE_RATE_HZ; + double T_prn_true_samples = T_prn_true_seconds * (double)d_fs_in; d_code_phase_samples = d_code_phase_samples + T_prn_samples - T_prn_true_samples; if (d_code_phase_samples < 0) { @@ -509,11 +509,10 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec { std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl; LOG(INFO) << "Loss of lock in channel " << d_channel << "!"; - ControlMessageFactory* cmf = new ControlMessageFactory(); + std::unique_ptr cmf(new ControlMessageFactory()); if (d_queue != gr::msg_queue::sptr()) { d_queue->handle(cmf->GetQueueMessage(d_channel, 2)); } - delete cmf; d_carrier_lock_fail_counter = 0; d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine @@ -558,23 +557,23 @@ int Gps_L1_Ca_Tcp_Connector_Tracking_cc::general_work (int noutput_items, gr_vec } else { - // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) - /*! - * \todo The stop timer has to be moved to the signal source! - */ - // stream to collect cout calls to improve thread safety - std::stringstream tmp_str_stream; - if (floor(d_sample_counter / d_fs_in) != d_last_seg) - { - d_last_seg = floor(d_sample_counter / d_fs_in); + // ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled) + /*! + * \todo The stop timer has to be moved to the signal source! + */ + // stream to collect cout calls to improve thread safety + std::stringstream tmp_str_stream; + if (floor(d_sample_counter / d_fs_in) != d_last_seg) + { + d_last_seg = floor(d_sample_counter / d_fs_in); - if (d_channel == 0) - { - // debug: Second counter in channel 0 - tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; - std::cout << tmp_str_stream.rdbuf() << std::flush; - } - } + if (d_channel == 0) + { + // debug: Second counter in channel 0 + tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush; + std::cout << tmp_str_stream.rdbuf() << std::flush; + } + } *d_Early = gr_complex(0,0); *d_Prompt = gr_complex(0,0); *d_Late = gr_complex(0,0); diff --git a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h index 547f72ced..bc7e37914 100644 --- a/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/gps_l1_ca_tcp_connector_tracking_cc.h @@ -21,7 +21,7 @@ * GNSS-SDR is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or - * at your option) any later version. + * (at your option) any later version. * * GNSS-SDR is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of @@ -132,9 +132,9 @@ private: long d_if_freq; long d_fs_in; - float d_early_late_spc_chips; + double d_early_late_spc_chips; - float d_code_phase_step_chips; + double d_code_phase_step_chips; gr_complex* d_ca_code; @@ -148,8 +148,8 @@ private: gr_complex *d_Late; // remaining code phase and carrier phase between tracking loops - float d_rem_code_phase_samples; - float d_next_rem_code_phase_samples; + double d_rem_code_phase_samples; + double d_next_rem_code_phase_samples; float d_rem_carr_phase_rad; // PLL and DLL filter library @@ -163,10 +163,10 @@ private: Correlator d_correlator; // tracking vars - float d_code_freq_hz; - float d_carrier_doppler_hz; - float d_acc_carrier_phase_rad; - float d_code_phase_samples; + double d_code_freq_hz; + double d_carrier_doppler_hz; + double d_acc_carrier_phase_rad; + double d_code_phase_samples; size_t d_port_ch0; size_t d_port; int d_listen_connection;