From e2c2da67c4b7163bd539cb33f2570b8dc597f8cf Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Thu, 22 Feb 2018 16:15:07 +0100 Subject: [PATCH] debug3 --- src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc | 6 ++---- .../gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc | 10 +++++++++- .../gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc | 10 ++++------ src/core/system_parameters/MATH_CONSTANTS.h | 7 +++++++ 4 files changed, 22 insertions(+), 11 deletions(-) diff --git a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc index 1554884b0..4914bf020 100644 --- a/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc +++ b/src/algorithms/PVT/gnuradio_blocks/rtklib_pvt_cc.cc @@ -1713,11 +1713,9 @@ int rtklib_pvt_cc::work (int noutput_items, gr_vector_const_void_star &input_ite // DEBUG MESSAGE: Display position in console output if( (d_ls_pvt->is_valid_position() == true) && (flag_display_pvt == true) ) { - std::string text_green = "\033[32m"; - std::string text_reset = "\033[0m"; - std::cout << text_green << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) + std::cout << TEXT_GREEN << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using " << d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() - << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << text_reset << std::endl; + << " [deg], Height= " << d_ls_pvt->get_height() << " [m]" << TEXT_RESET << std::endl; LOG(INFO) << "Position at " << boost::posix_time::to_simple_string(d_ls_pvt->get_position_UTC_time()) << " UTC using "<< d_ls_pvt->get_num_valid_observations() << " observations is Lat = " << d_ls_pvt->get_latitude() << " [deg], Long = " << d_ls_pvt->get_longitude() diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc index 79810f000..f0766f528 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.cc @@ -350,11 +350,19 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ //double decoder_latency_ms=(double)(current_symbol.Tracking_sample_counter-d_symbol_history.at(0).Tracking_sample_counter) // /(double)current_symbol.fs; // update TOW at the preamble instant (account with decoder latency) - d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; + double tmp_tow = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD; + d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + 2 * GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; d_TOW_at_current_symbol = floor(d_TOW_at_Preamble * 1000.0) / 1000.0; flag_TOW_set = true; d_flag_new_tow_available = false; + double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); + if ((tmp_tow != 0) or (tmp_diff > 0.0)) + { + std::cout << TEXT_RED << + "GPS L1 C/A. TOW incoherence on channel: "<< d_channel << ". TOW difference = " << + tmp_diff * 1000.0 << " [ms]" << TEXT_RESET << std::endl; + } } else { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc index 20be2dcf9..be426b70a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_cc.cc @@ -156,17 +156,15 @@ int gps_l2c_telemetry_decoder_cc::general_work (int noutput_items __attribute__( //* delay by the formulae: //* \code //* symbolTime_ms = msg->tow * 6000 + *pdelay * 20 + (12 * 20); 12 symbols of the encoder's transitory - double tmp_tow= d_TOW_at_current_symbol += GPS_L2_M_PERIOD; + double tmp_tow = d_TOW_at_current_symbol + GPS_L2_M_PERIOD; d_TOW_at_current_symbol = static_cast(msg.tow) * 6.0 + static_cast(delay) * GPS_L2_M_PERIOD + 12 * GPS_L2_M_PERIOD; d_TOW_at_current_symbol = floor(d_TOW_at_current_symbol * 1000.0) / 1000.0; - double tmp_diff = std::fabs(tmp_tow-d_TOW_at_current_symbol); + double tmp_diff = std::fabs(tmp_tow - d_TOW_at_current_symbol); if ((tmp_tow != 0) or (tmp_diff > 0.0)) { - std::string text_red = "\033[31m"; - std::string text_reset = "\033[0m"; - std::cout << text_red << + std::cout << TEXT_RED << "GPS L2C. TOW incoherence on channel: "<< d_channel << ". TOW difference = " << - tmp_diff * 1000.0 << " [ms]" << text_reset << std::endl; + tmp_diff * 1000.0 << " [ms]" << TEXT_RESET << std::endl; } d_flag_valid_word = true; } diff --git a/src/core/system_parameters/MATH_CONSTANTS.h b/src/core/system_parameters/MATH_CONSTANTS.h index 8c8dfbbe5..2a5f376e6 100644 --- a/src/core/system_parameters/MATH_CONSTANTS.h +++ b/src/core/system_parameters/MATH_CONSTANTS.h @@ -31,6 +31,8 @@ #ifndef GNSS_SDR_MATH_CONSTANTS_H_ #define GNSS_SDR_MATH_CONSTANTS_H_ +#include + /* Constants for scaling the ephemeris found in the data message the format is the following: TWO_N5 -> 2^-5, TWO_P4 -> 2^4, PI_TWO_N43 -> Pi*2^-43, etc etc Additionally some of the PI*2^N terms are used in the tracking stuff @@ -41,6 +43,11 @@ ONE_PI_TWO_PX = (1/Pi)*2^X */ +const std::string TEXT_RED = "\033[31m"; +const std::string TEXT_GREEN = "\033[32m"; +const std::string TEXT_RESET = "\033[0m"; + + const double PI = 3.1415926535897932; //!< pi const double PI_2 = 2.0 * PI; //!< 2 * pi