From 6eaad8f8a3d8a3a2e0bd4dc25f990954adf4e64d Mon Sep 17 00:00:00 2001 From: Cillian O'Driscoll Date: Sun, 22 Nov 2015 08:40:39 +0000 Subject: [PATCH] Split prs code phase into int and fract parts Applied to the VEML tracking. Previous commit was DE tracking only. --- .../galileo_e1_prs_veml_tracking_cc.cc | 88 +++++++++++++------ .../galileo_e1_prs_veml_tracking_cc.h | 4 +- 2 files changed, 63 insertions(+), 29 deletions(-) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_veml_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_veml_tracking_cc.cc index ed6f1daf4..ec825f212 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_veml_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_veml_tracking_cc.cc @@ -258,7 +258,8 @@ galileo_e1_prs_veml_tracking_cc::galileo_e1_prs_veml_tracking_cc( d_code_phase_chips = 0.0; d_rem_code_phase_samples_prs = 0.0; - d_code_phase_chips_prs = 0.0; + d_integer_code_phase_chips_prs = 0; + d_fractional_code_phase_chips_prs = 0.0; // Residual carrier phase d_rem_carr_phase_rad = 0.0; @@ -519,15 +520,15 @@ void galileo_e1_prs_veml_tracking_cc::update_local_code_prs() // Update the local PRS code if necessary: uint64_t last_code_phase_store = d_start_index_prs_code + d_size_prs_code; - uint64_t last_code_phase_required = static_cast< uint64_t >( - d_code_phase_chips_prs + code_phase_step_chips*d_current_prn_length_samples ); + uint64_t last_code_phase_required = d_integer_code_phase_chips_prs + static_cast< uint64_t >( + d_fractional_code_phase_chips_prs + code_phase_step_chips*d_current_prn_length_samples ); if( !d_prs_code_initialized || ( last_code_phase_required > last_code_phase_store - 2 ) ) { // Make sure we go back at least 2 chips: - d_start_index_prs_code = ( d_code_phase_chips_prs < 2.0 ? - static_cast( std::floor( d_code_phase_chips_prs ) ) + d_prs_code_gen->get_code_length() - 2 : - static_cast( std::floor( d_code_phase_chips_prs) ) - 2 ); + d_start_index_prs_code = ( d_integer_code_phase_chips_prs < 2 ? + d_integer_code_phase_chips_prs + d_prs_code_gen->get_code_length() - 2 : + d_integer_code_phase_chips_prs - 2 ); d_prs_code_gen->get_chips(d_start_index_prs_code, d_size_prs_code, d_prs_code_shorts ); @@ -540,13 +541,12 @@ void galileo_e1_prs_veml_tracking_cc::update_local_code_prs() } - uint64_t int_code_phase = static_cast< uint64_t >( std::floor( d_code_phase_chips_prs ) ); - double frac_code_phase = std::fmod( d_code_phase_chips_prs, 1.0 ); - tcode_chips = frac_code_phase + static_cast< double >( int_code_phase - d_start_index_prs_code ); + tcode_chips = d_fractional_code_phase_chips_prs + + static_cast< double >( d_integer_code_phase_chips_prs - d_start_index_prs_code ); //tcode_chips = d_code_phase_chips_prs - static_cast< double >( d_start_index_prs_code ); // Add 1/4 of a cyle here to account for cosine phasing: - tsubcarrier_phase_halfcyles = d_subcarrier_phase_cycles_prs*2.0 + 0.5; + tsubcarrier_phase_halfcyles = d_fractional_code_phase_chips_prs*chips_to_halfcycles + 0.5; early_late_spc_halfcycles = d_early_late_code_spc_cycles * 2.0; very_early_late_spc_halfcycles = d_very_early_late_code_spc_chips_prs * chips_to_halfcycles; @@ -815,13 +815,17 @@ int galileo_e1_prs_veml_tracking_cc::general_work (int noutput_items,gr_vector_i d_Very_Late_prs ); // Now update the code and carrier phase estimates: - d_code_phase_chips_prs += T*d_code_freq_chips_prs; - //d_code_phase_chips_prs = std::fmod( d_code_phase_chips_prs, - //static_cast( d_prs_code_gen->get_code_length() ) ); - if( d_code_phase_chips_prs >= static_cast< double >( d_prs_code_gen->get_code_length() ) ) - { - d_code_phase_chips_prs -= static_cast< double >( d_prs_code_gen->get_code_length() ); - } + double chips_to_halfcycles_prs = Galileo_E1_A_SUB_CARRIER_RATE_HZ / + Galileo_E1_A_CODE_CHIP_RATE_HZ * 2.0; + double delta_code_phase_prs = T*d_code_freq_chips_prs; + + d_fractional_code_phase_chips_prs += delta_code_phase_prs; + + d_integer_code_phase_chips_prs += static_cast< int64_t >( + std::floor( d_fractional_code_phase_chips_prs ) ); + + d_fractional_code_phase_chips_prs = std::fmod( + d_fractional_code_phase_chips_prs, 1.0 ); //DLOG(INFO) << "Propagated PRS code phase: " << std::fixed << std::setprecision( 12 ) << T*d_code_freq_chips_prs; if( d_use_sa ) @@ -838,14 +842,24 @@ int galileo_e1_prs_veml_tracking_cc::general_work (int noutput_items,gr_vector_i d_carrier_phase_rad_prs += T*2.0*M_PI*d_carrier_doppler_hz_prs; - double e1b_code_length_prs_chips = Galileo_E1_B_CODE_LENGTH_CHIPS / - Galileo_E1_CODE_CHIP_RATE_HZ * Galileo_E1_A_CODE_CHIP_RATE_HZ; + int64_t e1b_code_length_prs_chips = static_cast< int64_t>( + Galileo_E1_B_CODE_LENGTH_CHIPS / + Galileo_E1_CODE_CHIP_RATE_HZ * Galileo_E1_A_CODE_CHIP_RATE_HZ + ); - double rem_code_phase_chips_prs = e1b_code_length_prs_chips - - std::fmod( d_code_phase_chips_prs, e1b_code_length_prs_chips ); - if( rem_code_phase_chips_prs > e1b_code_length_prs_chips / 2.0 ) + + int64_t chips_into_e1b_code_period = + d_integer_code_phase_chips_prs % e1b_code_length_prs_chips; + + double rem_code_phase_chips_prs = static_cast< double >( + e1b_code_length_prs_chips - chips_into_e1b_code_period + ) - d_fractional_code_phase_chips_prs; + + if( rem_code_phase_chips_prs > static_cast< double >( + e1b_code_length_prs_chips / 2 ) ) { - rem_code_phase_chips_prs = ( rem_code_phase_chips_prs - e1b_code_length_prs_chips ); + rem_code_phase_chips_prs = ( rem_code_phase_chips_prs - + static_cast(e1b_code_length_prs_chips) ); } d_rem_code_phase_samples_prs = rem_code_phase_chips_prs * d_fs_in/Galileo_E1_A_CODE_CHIP_RATE_HZ; @@ -1114,7 +1128,19 @@ int galileo_e1_prs_veml_tracking_cc::general_work (int noutput_items,gr_vector_i { double half_cycle_in_chips = 0.5/d_chips_to_cycles_prs; - d_code_phase_chips_prs += half_cycle_in_chips*jump_dir; + d_fractional_code_phase_chips_prs += half_cycle_in_chips*jump_dir; + + if( d_fractional_code_phase_chips_prs >= 1.0 ) + { + d_fractional_code_phase_chips_prs -= 1.0; + d_integer_code_phase_chips_prs += 1; + } + + if( d_fractional_code_phase_chips_prs < 0.0 ) + { + d_fractional_code_phase_chips_prs += 1.0; + d_integer_code_phase_chips_prs -= 1; + } std::stringstream ss(""); @@ -1546,7 +1572,9 @@ int galileo_e1_prs_veml_tracking_cc::general_work (int noutput_items,gr_vector_i d_dump_file.write(reinterpret_cast(&subcarrier_error_filt_cycles_prs), sizeof(float)); // SLL commands - d_dump_file.write(reinterpret_cast(&d_code_phase_chips_prs), sizeof(double)); + tmp_double = static_cast< double >( d_integer_code_phase_chips_prs ) + + d_fractional_code_phase_chips_prs; + d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); d_dump_file.write(reinterpret_cast(&code_error_chips_veml_prs), sizeof(float)); d_dump_file.write(reinterpret_cast(&code_error_filt_chips_veml_prs), sizeof(float)); } @@ -1654,12 +1682,16 @@ void galileo_e1_prs_veml_tracking_cc::start_tracking_prs() curr_tow -= 604800.0; } - d_code_phase_chips_prs = std::fmod( curr_tow * Galileo_E1_A_CODE_CHIP_RATE_HZ, + double code_phase_chips_prs = std::fmod( curr_tow * Galileo_E1_A_CODE_CHIP_RATE_HZ, d_prs_code_gen->get_code_length() ); d_subcarrier_phase_cycles_prs = std::fmod( curr_tow, 1.0/Galileo_E1_A_SUB_CARRIER_RATE_HZ ) * Galileo_E1_A_SUB_CARRIER_RATE_HZ; + d_integer_code_phase_chips_prs = static_cast< int64_t >( std::floor( code_phase_chips_prs ) ); + + d_fractional_code_phase_chips_prs = std::fmod( code_phase_chips_prs, 1.0 ); + d_rem_carr_phase_rad_prs = d_rem_carr_phase_rad - M_PI/2.0; d_code_freq_chips_prs = d_code_freq_chips * Galileo_E1_A_CODE_CHIP_RATE_HZ / @@ -1694,7 +1726,7 @@ void galileo_e1_prs_veml_tracking_cc::start_tracking_prs() LOG(INFO) << "Starting tracking of PRS for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel; DLOG(INFO) << "Starting params: current TOW estimate: " << curr_tow - << " Code phase " << d_code_phase_chips_prs << " chips." + << " Code phase " << code_phase_chips_prs << " chips." << " Last TOW: " << d_last_tow << " @ " << d_timestamp_last_tow << " Correction: " << (curr_tow - d_last_tow); // enable tracking @@ -1712,7 +1744,7 @@ void galileo_e1_prs_veml_tracking_cc::start_tracking_prs() d_mean_subcarrier_error_prs = 0.0; LOG(INFO) << "PULL-IN Doppler [Hz]=" << d_carrier_doppler_hz_prs - << " PULL-IN Code Phase [samples]=" << d_code_phase_chips_prs; + << " PULL-IN Code Phase [samples]=" << code_phase_chips_prs; } void galileo_e1_prs_veml_tracking_cc::handle_gnss_message( pmt::pmt_t msg ) diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_veml_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_veml_tracking_cc.h index 08d7795ae..4a411d155 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_veml_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_veml_tracking_cc.h @@ -247,7 +247,9 @@ private: double d_acc_code_phase_secs; double d_code_freq_chips_prs; - double d_code_phase_chips_prs; + int64_t d_integer_code_phase_chips_prs; + double d_fractional_code_phase_chips_prs; + float d_carrier_doppler_hz_prs; double d_carrier_phase_rad_prs; double d_acc_carrier_phase_rad_prs;