From c96bbaba01bef034461aeeaccfdb83c45e02ba44 Mon Sep 17 00:00:00 2001 From: Cillian O'Driscoll Date: Fri, 20 Nov 2015 16:57:37 +0000 Subject: [PATCH] First draft of DPE for PRS Note this operates as a DPE but actually generates a full BOC subcarrier. The next step will make it possible to actually generate the local replica subcarrier as a cosine wave. --- .../adapters/galileo_e1_prs_de_tracking.cc | 5 +- .../galileo_e1_prs_de_tracking_cc.cc | 117 ++++++++++++++---- .../galileo_e1_prs_de_tracking_cc.h | 9 ++ src/algorithms/tracking/libs/correlator.cc | 32 +++++ src/algorithms/tracking/libs/correlator.h | 14 +++ 5 files changed, 149 insertions(+), 28 deletions(-) diff --git a/src/algorithms/tracking/adapters/galileo_e1_prs_de_tracking.cc b/src/algorithms/tracking/adapters/galileo_e1_prs_de_tracking.cc index 737a1aedc..075d204fe 100644 --- a/src/algorithms/tracking/adapters/galileo_e1_prs_de_tracking.cc +++ b/src/algorithms/tracking/adapters/galileo_e1_prs_de_tracking.cc @@ -69,6 +69,7 @@ GalileoE1PrsDeTracking::GalileoE1PrsDeTracking( float early_late_subcarrier_space_chips; bool aid_subcarrier_with_carrier; bool aid_code_with_subcarrier; + bool model_prs_subcarrier_as_sinusoid; item_type = configuration->property(role + ".item_type", default_item_type); fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000); @@ -85,6 +86,7 @@ GalileoE1PrsDeTracking::GalileoE1PrsDeTracking( early_late_subcarrier_space_chips = configuration->property(role + ".early_late_subcarrier_space_cycles", 0.125); aid_subcarrier_with_carrier = configuration->property(role + ".aid_subcarrier_with_carrier", false ); aid_code_with_subcarrier = configuration->property(role + ".aid_code_with_subcarrier", false ); + model_prs_subcarrier_as_sinusoid = configuration->property(role + ".model_prs_subcarrier_as_sinusoid", false ); pll_loop_order = configuration->property(role + ".pll_loop_order", 3); sll_loop_order = configuration->property(role + ".sll_loop_order", 3); @@ -131,7 +133,8 @@ GalileoE1PrsDeTracking::GalileoE1PrsDeTracking( sll_loop_order, sll_initial_bw_hz, sll_final_bw_hz, initial_early_late_code_space_chips, final_early_late_code_space_chips, early_late_subcarrier_space_chips, - aid_subcarrier_with_carrier, aid_code_with_subcarrier, code_gen); + aid_subcarrier_with_carrier, aid_code_with_subcarrier, + model_prs_subcarrier_as_sinusoid, code_gen); } else { diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.cc b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.cc index 4e410a82d..8158a7cda 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.cc +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.cc @@ -87,6 +87,7 @@ galileo_e1_prs_de_make_tracking_cc( float early_late_subcarrier_space_cycles, bool aid_subcarrier_with_carrier, bool aid_code_with_subcarrier, + bool prs_sinusoidal_subcarrier, LongCodeInterface_sptr prs_code_gen) { return galileo_e1_prs_de_tracking_cc_sptr(new galileo_e1_prs_de_tracking_cc(if_freq, @@ -96,7 +97,8 @@ galileo_e1_prs_de_make_tracking_cc( sll_loop_order, sll_initial_bw_hz, sll_final_bw_hz, initial_early_late_code_space_chips, final_early_late_code_space_chips, early_late_subcarrier_space_cycles, - aid_subcarrier_with_carrier, aid_code_with_subcarrier, prs_code_gen)); + aid_subcarrier_with_carrier, aid_code_with_subcarrier, + prs_sinusoidal_subcarrier, prs_code_gen)); } @@ -128,6 +130,7 @@ galileo_e1_prs_de_tracking_cc::galileo_e1_prs_de_tracking_cc( float early_late_subcarrier_space_cycles, bool aid_subcarrier_with_carrier, bool aid_code_with_subcarrier, + bool prs_sinusoidal_subcarrier, LongCodeInterface_sptr prs_code_gen): gr::block("galileo_e1_prs_de_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)), gr::io_signature::make(1, 1, sizeof(Gnss_Synchro))) @@ -208,6 +211,9 @@ galileo_e1_prs_de_tracking_cc::galileo_e1_prs_de_tracking_cc( d_late_subcarrier_prs = static_cast(volk_malloc(2 * d_vector_length * sizeof(gr_complex), volk_get_alignment())); d_carr_sign = static_cast(volk_malloc(2*d_vector_length * sizeof(gr_complex), volk_get_alignment())); + // Re-use the late subcarrier for the DPE + d_prompt_quadrature_subcarrier_prs = d_late_subcarrier_prs; + // correlator outputs (scalar) d_Prompt_Subcarrier_Early_Code = static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); d_Prompt_Subcarrier_Prompt_Code = static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); @@ -222,6 +228,9 @@ galileo_e1_prs_de_tracking_cc::galileo_e1_prs_de_tracking_cc( d_Prompt_Code_Early_Subcarrier_prs = static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); d_Prompt_Code_Late_Subcarrier_prs= static_cast(volk_malloc(sizeof(gr_complex), volk_get_alignment())); + // Re-use the prompt code late subcarrier for the DPE + d_Prompt_Code_Quadrature_Subcarrier_prs = d_Prompt_Code_Late_Subcarrier_prs; + //--- Initializations ------------------------------ // Initial code frequency basis of NCO d_code_freq_chips = static_cast(Galileo_E1_CODE_CHIP_RATE_HZ); @@ -292,6 +301,8 @@ galileo_e1_prs_de_tracking_cc::galileo_e1_prs_de_tracking_cc( d_tow_received = false; d_rx_time_set = false; d_preamble_start_detected = false; + + d_prs_sinusoidal_subcarrier = prs_sinusoidal_subcarrier; } void galileo_e1_prs_de_tracking_cc::start_tracking() @@ -476,7 +487,14 @@ void galileo_e1_prs_de_tracking_cc::update_local_code_prs() // Add 1/4 of a cyle here to account for cosine phasing: tsubcarrier_phase_halfcyles = static_cast(d_subcarrier_phase_halfcycles_prs) + 0.5; - early_late_subcarrier_spc_halfcycles = d_early_late_subcarrier_spc_cycles * 2.0; + if( d_prs_sinusoidal_subcarrier ) + { + early_late_subcarrier_spc_halfcycles = 0.5; // DPE requires quarter cycle spacing + } + else + { + early_late_subcarrier_spc_halfcycles = d_early_late_subcarrier_spc_cycles * 2.0; + } int64_t early_code_phase_fxp = double_to_fxpt64( tcode_chips + d_early_late_code_spc_chips ); int64_t prompt_code_phase_fxp = double_to_fxpt64( tcode_chips ); @@ -714,22 +732,47 @@ int galileo_e1_prs_de_tracking_cc::general_work (int noutput_items,gr_vector_int -std::sin( carrier_doppler_inc_rad ) ); - // perform carrier wipe-off and compute Very Early, Early, Prompt, Late and Very Late correlation - d_correlator.Carrier_rotate_and_DE_volk(d_current_prn_length_samples, - in, - &phase_as_complex, - phase_inc_as_complex, - d_early_code_prs, - d_prompt_code_prs, - d_late_code_prs, - d_early_subcarrier_prs, - d_prompt_subcarrier_prs, - d_late_subcarrier_prs, - d_Prompt_Subcarrier_Early_Code_prs, - d_Prompt_Subcarrier_Prompt_Code_prs, - d_Prompt_Subcarrier_Late_Code_prs, - d_Prompt_Code_Early_Subcarrier_prs, - d_Prompt_Code_Late_Subcarrier_prs ); + // Two options: Double estimator or Double phase estimator + // In the double estimator the subcarrier is assumed to be a square wave + // In the double phase estiamtor the subcarrier is approximated by its fundamental + // Fourier component + // + if( d_prs_sinusoidal_subcarrier ){ + + d_correlator.Carrier_rotate_and_DPE_volk(d_current_prn_length_samples, + in, + &phase_as_complex, + phase_inc_as_complex, + d_early_code_prs, + d_prompt_code_prs, + d_late_code_prs, + d_prompt_quadrature_subcarrier_prs, + d_prompt_subcarrier_prs, + d_Prompt_Subcarrier_Early_Code_prs, + d_Prompt_Subcarrier_Prompt_Code_prs, + d_Prompt_Subcarrier_Late_Code_prs, + d_Prompt_Code_Quadrature_Subcarrier_prs ); + } + else + { + + // perform carrier wipe-off and compute Very Early, Early, Prompt, Late and Very Late correlation + d_correlator.Carrier_rotate_and_DE_volk(d_current_prn_length_samples, + in, + &phase_as_complex, + phase_inc_as_complex, + d_early_code_prs, + d_prompt_code_prs, + d_late_code_prs, + d_early_subcarrier_prs, + d_prompt_subcarrier_prs, + d_late_subcarrier_prs, + d_Prompt_Subcarrier_Early_Code_prs, + d_Prompt_Subcarrier_Prompt_Code_prs, + d_Prompt_Subcarrier_Late_Code_prs, + d_Prompt_Code_Early_Subcarrier_prs, + d_Prompt_Code_Late_Subcarrier_prs ); + } // Now update the code and carrier phase estimates: double chips_to_halfcycles_prs = Galileo_E1_A_SUB_CARRIER_RATE_HZ / @@ -892,13 +935,25 @@ int galileo_e1_prs_de_tracking_cc::general_work (int noutput_items,gr_vector_int // ################## SLL ########################################################## // SLL discriminator - subcarrier_error_cycles_prs = dll_nc_e_minus_l_normalized( - *d_Prompt_Code_Early_Subcarrier_prs, - *d_Prompt_Code_Late_Subcarrier_prs); //[chips/Ti] - // normalise the SLL discriminator by the slope of the - // BOC(1,1) at the origin: - corr_slope = 2.0; - subcarrier_error_cycles_prs *= ( 1 - corr_slope*d_early_late_subcarrier_spc_cycles) / corr_slope; + if( d_prs_sinusoidal_subcarrier ) + { + gr_complex Z = ( + *d_Prompt_Code_Quadrature_Subcarrier_prs / + *d_Prompt_Subcarrier_Prompt_Code_prs ); + + subcarrier_error_cycles_prs = std::atan( Z.real() ) / static_cast(GPS_TWO_PI); + } + else + { + subcarrier_error_cycles_prs = dll_nc_e_minus_l_normalized( + *d_Prompt_Code_Early_Subcarrier_prs, + *d_Prompt_Code_Late_Subcarrier_prs); //[chips/Ti] + + // normalise the SLL discriminator by the slope of the + // BOC(1,1) at the origin: + corr_slope = 2.0; + subcarrier_error_cycles_prs *= ( 1 - corr_slope*d_early_late_subcarrier_spc_cycles) / corr_slope; + } // Subcarrier discriminator filter subcarrier_error_filt_cycles_prs = d_subcarrier_loop_filter_prs.apply(subcarrier_error_cycles_prs); //[chips/second] @@ -1206,11 +1261,19 @@ int galileo_e1_prs_de_tracking_cc::general_work (int noutput_items,gr_vector_int // PRS Variables: prompt_I = (*d_Prompt_Subcarrier_Prompt_Code_prs).real(); prompt_Q = (*d_Prompt_Subcarrier_Prompt_Code_prs).imag(); - tmp_VE = std::abs(*d_Prompt_Code_Early_Subcarrier_prs); + if( d_prs_sinusoidal_subcarrier ) + { + tmp_VE = d_Prompt_Code_Quadrature_Subcarrier_prs->real(); + tmp_VL = d_Prompt_Code_Quadrature_Subcarrier_prs->imag(); + } + else + { + tmp_VE = std::abs(*d_Prompt_Code_Early_Subcarrier_prs); + tmp_VL = std::abs(*d_Prompt_Code_Late_Subcarrier_prs); + } tmp_E = std::abs(*d_Prompt_Subcarrier_Early_Code_prs); tmp_P = std::abs(*d_Prompt_Subcarrier_Prompt_Code_prs); tmp_L = std::abs(*d_Prompt_Subcarrier_Late_Code_prs); - tmp_VL = std::abs(*d_Prompt_Code_Late_Subcarrier_prs); // Dump correlators output d_dump_file.write(reinterpret_cast(&tmp_VE), sizeof(float)); d_dump_file.write(reinterpret_cast(&tmp_E), sizeof(float)); diff --git a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.h b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.h index dc2225173..40a95a531 100644 --- a/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.h +++ b/src/algorithms/tracking/gnuradio_blocks/galileo_e1_prs_de_tracking_cc.h @@ -71,6 +71,7 @@ galileo_e1_prs_de_make_tracking_cc(long if_freq, float early_late_subcarrier_space_cycles, bool aid_subcarrier_with_carrier, bool aid_code_with_subcarrier, + bool prs_sinusoidal_subcarrier, LongCodeInterface_sptr prs_code_gen); /*! @@ -120,6 +121,7 @@ private: float early_late_subcarrier_space_cycles, bool aid_subcarrier_with_carrier, bool aid_code_with_subcarrier, + bool prs_sinusoidal_subcarrier, LongCodeInterface_sptr prs_code_gen); galileo_e1_prs_de_tracking_cc(long if_freq, @@ -142,6 +144,7 @@ private: float early_late_subcarrier_space_cycles, bool aid_subcarrier_with_carrier, bool aid_code_with_subcarrier, + bool prs_sinusoidal_subcarrier, LongCodeInterface_sptr prs_code_gen); void update_local_code(); @@ -192,6 +195,8 @@ private: gr_complex* d_prompt_subcarrier_prs; gr_complex* d_late_subcarrier_prs; + gr_complex* d_prompt_quadrature_subcarrier_prs; + gr_complex *d_Prompt_Subcarrier_Early_Code; gr_complex *d_Prompt_Subcarrier_Prompt_Code; gr_complex *d_Prompt_Subcarrier_Late_Code; @@ -204,6 +209,8 @@ private: gr_complex *d_Prompt_Code_Early_Subcarrier_prs; gr_complex *d_Prompt_Code_Late_Subcarrier_prs; + gr_complex *d_Prompt_Code_Quadrature_Subcarrier_prs; + // remaining code phase and carrier phase between tracking loops double d_rem_code_phase_samples; double d_rem_subcarrier_phase_samples; @@ -301,6 +308,8 @@ private: bool d_prs_tracking_enabled; + // Should we approximate the PRS subcarrier as a sinusoid? + bool d_prs_sinusoidal_subcarrier; // file dump std::string d_dump_filename; diff --git a/src/algorithms/tracking/libs/correlator.cc b/src/algorithms/tracking/libs/correlator.cc index d3ac8ed79..4c3a19e49 100644 --- a/src/algorithms/tracking/libs/correlator.cc +++ b/src/algorithms/tracking/libs/correlator.cc @@ -290,3 +290,35 @@ void Correlator::Carrier_rotate_and_DE_volk(int signal_length_samples, volk_free(subcarrier_wipeoff); volk_free(code_wipeoff); } + +void Correlator::Carrier_rotate_and_DPE_volk(int signal_length_samples, + const gr_complex* input, + gr_complex *phase_as_complex, + gr_complex phase_inc_as_complex, + const gr_complex* E_code, + const gr_complex* P_code, + const gr_complex* L_code, + const gr_complex* P_subcarrier, + const gr_complex* PQ_subcarrier, + gr_complex* P_subcarrier_E_code_out, + gr_complex* P_subcarrier_P_code_out, + gr_complex* P_subcarrier_L_code_out, + gr_complex* P_code_PQ_subcarrier_out ) +{ + gr_complex* bb_signal = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); + gr_complex* subcarrier_wipeoff = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); + gr_complex* code_wipeoff = static_cast(volk_malloc(signal_length_samples * sizeof(gr_complex), volk_get_alignment())); + + volk_32fc_s32fc_x2_rotator_32fc(bb_signal, input, phase_inc_as_complex, phase_as_complex, signal_length_samples); + volk_32fc_x2_multiply_32fc(subcarrier_wipeoff, bb_signal, P_subcarrier, signal_length_samples ); + volk_32fc_x2_multiply_32fc(code_wipeoff, bb_signal, P_code, signal_length_samples ); + + volk_32fc_x2_dot_prod_32fc(P_subcarrier_E_code_out, subcarrier_wipeoff, E_code, signal_length_samples); + volk_32fc_x2_dot_prod_32fc(P_subcarrier_P_code_out, subcarrier_wipeoff, P_code, signal_length_samples); + volk_32fc_x2_dot_prod_32fc(P_subcarrier_L_code_out, subcarrier_wipeoff, L_code, signal_length_samples); + volk_32fc_x2_dot_prod_32fc(P_code_PQ_subcarrier_out, code_wipeoff, PQ_subcarrier, signal_length_samples); + + volk_free(bb_signal); + volk_free(subcarrier_wipeoff); + volk_free(code_wipeoff); +} diff --git a/src/algorithms/tracking/libs/correlator.h b/src/algorithms/tracking/libs/correlator.h index 562a6d2ab..93a345154 100644 --- a/src/algorithms/tracking/libs/correlator.h +++ b/src/algorithms/tracking/libs/correlator.h @@ -146,6 +146,20 @@ public: gr_complex* P_code_E_subcarrier_out, gr_complex* P_code_L_subcarrier_out ); + void Carrier_rotate_and_DPE_volk(int signal_length_samples, + const gr_complex* input, + gr_complex *phase_as_complex, + gr_complex phase_inc_as_complex, + const gr_complex* E_code, + const gr_complex* P_code, + const gr_complex* L_code, + const gr_complex* P_subcarrier, + const gr_complex* PQ_subcarrier, + gr_complex* P_subcarrier_E_code_out, + gr_complex* P_subcarrier_P_code_out, + gr_complex* P_subcarrier_L_code_out, + gr_complex* P_code_PQ_subcarrier_out ); + private: unsigned long next_power_2(unsigned long v); };