diff --git a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc index a0d197dc8..9fec55e7a 100644 --- a/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc +++ b/src/algorithms/PVT/libs/gps_l1_ca_ls_pvt.cc @@ -156,7 +156,7 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, // ****** SOLVE LEAST SQUARES****************************************************** // ******************************************************************************** d_valid_observations = valid_obs; - LOG(INFO) << "(new)PVT: valid observations=" << valid_obs; + DLOG(INFO) << "(new)PVT: valid observations=" << valid_obs; if (valid_obs >= 4) { @@ -247,6 +247,9 @@ bool gps_l1_ca_ls_pvt::get_PVT(std::map gnss_pseudoranges_map, { d_rx_dt_s = 0; //reset rx time estimation LOG(WARNING) << "Problem with the solver, invalid solution!" << e.what(); + LOG(WARNING) << "satpos=" << satpos; + LOG(WARNING) << "obs=" << obs; + LOG(WARNING) << "W=" << W; b_valid_position = false; } } diff --git a/src/algorithms/PVT/libs/pvt_solution.cc b/src/algorithms/PVT/libs/pvt_solution.cc index 69ebd6e6d..9901ab878 100644 --- a/src/algorithms/PVT/libs/pvt_solution.cc +++ b/src/algorithms/PVT/libs/pvt_solution.cc @@ -132,7 +132,7 @@ int Pvt_Solution::cart2geo(double X, double Y, double Z, int elipsoid_selection) iterations = iterations + 1; if (iterations > 100) { - LOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh; + DLOG(WARNING) << "Failed to approximate h with desired precision. h-oldh= " << h - oldh; break; } } diff --git a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc index 2c7a64170..91629e23b 100644 --- a/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/gps_l1_ca_observables_cc.cc @@ -208,7 +208,7 @@ int gps_l1_ca_observables_cc::general_work (int noutput_items, gr_vector_int &ni current_gnss_synchro[gnss_synchro_iter->second.Channel_ID] = gnss_synchro_iter->second; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Pseudorange_m = pseudorange_m; current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].Flag_valid_pseudorange = true; - current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = round(d_TOW_reference * 1000.0) / 1000.0 + GPS_STARTOFFSET_ms / 1000.0; + current_gnss_synchro[gnss_synchro_iter->second.Channel_ID].d_TOW_at_current_symbol = d_TOW_reference + GPS_STARTOFFSET_ms / 1000.0; if (d_symbol_TOW_queue_s[gnss_synchro_iter->second.Channel_ID].size() >= history_deep) { 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 c90930ce3..482e4b6af 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 @@ -105,6 +105,7 @@ gps_l1_ca_telemetry_decoder_cc::gps_l1_ca_telemetry_decoder_cc( flag_TOW_set = false; d_average_count = 0; d_flag_preamble = false; + d_flag_new_tow_available=false; d_word_number = 0; d_decimation_output_factor = 1; d_channel = 0; @@ -197,7 +198,6 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ d_GPS_FSM.Event_gps_word_preamble(); d_flag_preamble = true; d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // record the PRN start sample index associated to the preamble - if (!d_flag_frame_sync) { // send asynchronous message to tracking to inform of frame sync and extend correlation time @@ -310,6 +310,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ break; } d_GPS_FSM.clear_flag_new_subframe(); + d_flag_new_tow_available=true; } d_flag_parity = true; @@ -336,16 +337,14 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ current_synchro_data = in[0][0]; //2. Add the telemetry decoder information - if (this->d_flag_preamble == true and d_GPS_FSM.d_nav.d_TOW > 0) + if (this->d_flag_preamble == true and d_flag_new_tow_available==true) { // update TOW at the preamble instant d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + GPS_L1_CA_CODE_PERIOD; Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0; d_TOW_at_current_symbol = d_TOW_at_Preamble; - if (flag_TOW_set == false) - { - flag_TOW_set = true; - } + flag_TOW_set = true; + d_flag_new_tow_available=false; } else { @@ -355,7 +354,7 @@ int gps_l1_ca_telemetry_decoder_cc::general_work (int noutput_items __attribute_ current_synchro_data.d_TOW = d_TOW_at_Preamble; current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol; current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol; // to be used in the hybrid configuration - current_synchro_data.Flag_valid_word = (d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set == true); + current_synchro_data.Flag_valid_word = flag_TOW_set;//(d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set == true); current_synchro_data.Flag_preamble = d_flag_preamble; current_synchro_data.Prn_timestamp_ms = in[0][0].Tracking_timestamp_secs * 1000.0; current_synchro_data.Prn_timestamp_at_preamble_ms = Prn_timestamp_at_preamble_ms; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h index 495b5b252..9251dd0b6 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_cc.h @@ -104,6 +104,7 @@ private: unsigned int d_prev_GPS_frame_4bytes; bool d_flag_parity; bool d_flag_preamble; + bool d_flag_new_tow_available; int d_word_number; // output averaging and decimation