From b45854b4a8674440491d46699b4a42cdb73e6a6b Mon Sep 17 00:00:00 2001 From: Cillian O'Driscoll Date: Mon, 27 Jun 2016 08:47:45 +0100 Subject: [PATCH] Fixes for handling TOW_at_start also added some extra logging info --- .../galileo_e1_prs_de_tracking_cc.cc | 17 ++++++++++------- .../galileo_e1_prs_veml_tracking_cc.cc | 7 ++++--- src/core/receiver/gnss_flowgraph.cc | 10 ++++++++-- 3 files changed, 22 insertions(+), 12 deletions(-) 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 4f86cca34..30f27da74 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 @@ -1564,9 +1564,10 @@ void galileo_e1_prs_de_tracking_cc::start_tracking_prs() double last_tow_round = std::ceil( last_tow/Galileo_E1_CODE_PERIOD ) * Galileo_E1_CODE_PERIOD; - double curr_tow = last_tow_round + code_periods_since_tow*Galileo_E1_CODE_PERIOD + - //std::fmod( d_code_phase_chips, Galileo_E1_B_CODE_LENGTH_CHIPS ) / Galileo_E1_CODE_CHIP_RATE_HZ; - d_rem_code_phase_samples / static_cast( d_fs_in ); + double curr_tow = std::floor( + (d_last_tow + time_since_tow)/Galileo_E1_CODE_PERIOD + + 0.5 ) * Galileo_E1_CODE_PERIOD + + d_rem_code_phase_samples/static_cast(d_fs_in); // Handle week rollover: if( curr_tow > 604800.0 ){ @@ -1620,12 +1621,14 @@ void galileo_e1_prs_de_tracking_cc::start_tracking_prs() std::cout << "PRS tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl; 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 + DLOG(INFO) << "Starting params: current TOW estimate: " << std::fixed << std::setprecision(12) <(d_fs_in); // enable tracking d_prs_tracking_enabled = true; d_prs_code_gen->set_prn( d_acquisition_gnss_synchro->PRN ); @@ -1652,7 +1655,7 @@ void galileo_e1_prs_de_tracking_cc::handle_gnss_message( pmt::pmt_t msg ) if( gnss_message::get_message( msg ) == "TOW_ACQUIRED" ){ d_tow_received = true; d_last_tow = pmt::to_double( pmt::dict_ref( msg, pmt::mp( "TOW" ), not_found ) ) ; - log_str << ". TOW: " << d_last_tow; + log_str << ". TOW: " << std::fixed << std::setprecision(12) << d_last_tow; d_timestamp_last_tow = gnss_message::get_timestamp( msg ); if( d_code_locked and !d_prs_tracking_enabled ) 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 216724ff0..479581adc 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 @@ -1849,9 +1849,10 @@ void galileo_e1_prs_veml_tracking_cc::start_tracking_prs() double last_tow_round = std::ceil( last_tow/Galileo_E1_CODE_PERIOD ) * Galileo_E1_CODE_PERIOD; - double curr_tow = last_tow_round + code_periods_since_tow*Galileo_E1_CODE_PERIOD + - //std::fmod( d_code_phase_chips, Galileo_E1_B_CODE_LENGTH_CHIPS ) / Galileo_E1_CODE_CHIP_RATE_HZ; - d_rem_code_phase_samples / static_cast( d_fs_in ); + double curr_tow = std::floor( + (d_last_tow + time_since_tow)/Galileo_E1_CODE_PERIOD + + 0.5 ) * Galileo_E1_CODE_PERIOD + + d_rem_code_phase_samples/static_cast(d_fs_in); // Handle week rollover: if( curr_tow > 604800.0 ){ diff --git a/src/core/receiver/gnss_flowgraph.cc b/src/core/receiver/gnss_flowgraph.cc index 891194ba6..fe01f41b0 100644 --- a/src/core/receiver/gnss_flowgraph.cc +++ b/src/core/receiver/gnss_flowgraph.cc @@ -38,6 +38,7 @@ #include #include #include +#include // setprecision #include #include #include @@ -351,10 +352,13 @@ void GNSSFlowgraph::connect() "Prn" + boost::lexical_cast( available_GNSS_signals_.front().get_satellite().get_PRN() ) + - ".TOW_at_start", NAN ); + ".TOW_at_start", static_cast< double >( NAN ) ); if( !std::isnan( TOW_at_start ) ) { + DLOG(INFO) << "Read TOW_at_start for PRN: " << + available_GNSS_signals_.front().get_satellite().get_PRN() + << ". " << std::fixed << std::setprecision(12) << TOW_at_start; bool has_gnss_message_port = channels_.at(i)->get_right_block()->has_msg_port( GNSS_MESSAGE_PORT_ID ); if( has_gnss_message_port ) @@ -525,10 +529,12 @@ void GNSSFlowgraph::apply_action(unsigned int who, unsigned int what) "Prn" + boost::lexical_cast( the_sat.get_PRN() ) + - ".TOW_at_start", NAN ); + ".TOW_at_start", static_cast< double>( NAN ) ); if( !std::isnan( TOW_at_start ) ) { + DLOG(INFO) << "Read TOW_at_start for PRN: " << the_sat.get_PRN() + << ". " << std::fixed << std::setprecision(12) << TOW_at_start; has_gnss_message_port = channels_.at(who)->get_right_block()->has_msg_port( GNSS_MESSAGE_PORT_ID ); if( has_gnss_message_port )