From 1574e277d79eae8544ca2701fba18acfb82ee0f9 Mon Sep 17 00:00:00 2001 From: Javier Arribas Date: Tue, 1 Aug 2017 17:03:31 +0200 Subject: [PATCH] Temporary bug fix related to the GPS L1 C/A symbol TOW assignation. Position accuracy improvement confirmed. In-deep review is being done --- .../gnuradio_blocks/hybrid_observables_cc.cc | 35 ++++++----- .../gps_l1_ca_telemetry_decoder_cc.cc | 2 +- .../libs/read_true_sim_observables_dump.m | 61 +++++++++++++++++++ 3 files changed, 81 insertions(+), 17 deletions(-) create mode 100644 src/utils/matlab/libs/read_true_sim_observables_dump.m diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index 606362471..f7440f868 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -230,22 +230,25 @@ int hybrid_observables_cc::general_work (int noutput_items, int distance = std::distance(d_gnss_synchro_history_queue[i].begin(), gnss_synchro_deque_iter); if (distance > 0) { - double T_rx_channel_prev = (double)d_gnss_synchro_history_queue[i].at(distance - 1).Tracking_sample_counter / (double)gnss_synchro_deque_iter->fs; - double delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; - if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) - { - realigned_gnss_synchro_map.insert(std::pair( - d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance-1))); - adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - } - else - { - realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); - adjacent_gnss_synchro_map.insert(std::pair( - d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID, - d_gnss_synchro_history_queue[i].at(distance-1))); - } + if (d_gnss_synchro_history_queue[i].at(distance-1).Flag_valid_word) + { + double T_rx_channel_prev = (double)d_gnss_synchro_history_queue[i].at(distance - 1).Tracking_sample_counter / (double)gnss_synchro_deque_iter->fs; + double delta_T_rx_s_prev = T_rx_channel_prev - T_rx_s; + if (fabs(delta_T_rx_s_prev) < fabs(delta_T_rx_s)) + { + realigned_gnss_synchro_map.insert(std::pair( + d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID, + d_gnss_synchro_history_queue[i].at(distance-1))); + adjacent_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + } + else + { + realigned_gnss_synchro_map.insert(std::pair(gnss_synchro_deque_iter->Channel_ID, *gnss_synchro_deque_iter)); + adjacent_gnss_synchro_map.insert(std::pair( + d_gnss_synchro_history_queue[i].at(distance-1).Channel_ID, + d_gnss_synchro_history_queue[i].at(distance-1))); + } + } } else { 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 2874e90a3..d92128f61 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,7 +350,7 @@ 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 + GPS_L1_CA_CODE_PERIOD + GPS_CA_PREAMBLE_DURATION_S; + 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; diff --git a/src/utils/matlab/libs/read_true_sim_observables_dump.m b/src/utils/matlab/libs/read_true_sim_observables_dump.m new file mode 100644 index 000000000..7c43389cc --- /dev/null +++ b/src/utils/matlab/libs/read_true_sim_observables_dump.m @@ -0,0 +1,61 @@ +% Javier Arribas 2011 +function [observables] = read_true_sim_observables_dump (filename, count) + + %% usage: read_true_sim_observables_dump (filename, [count]) + %% + %% open gnss-sdr-sim observables dump and read all chennels + %% + + m = nargchk (1,2,nargin); + channels=12; %Simulator always use 12 channels + num_double_vars=6; + double_size_bytes=8; + skip_bytes_each_read=double_size_bytes*num_double_vars*channels; + bytes_shift=0; + + if (m) + usage (m); + end + + if (nargin < 2) + count = Inf; + end + %loops_counter = fread (f, count, 'uint32',4*12); + f = fopen (filename, 'rb'); + if (f < 0) + else + for N=1:1:channels + observables.RX_time(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + observables.Carrier_Doppler_hz(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + observables.Carrier_phase_hz(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + observables.Pseudorange_m(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + observables.Carrier_phase_hz_v2(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + observables.PRN(N,:) = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + end + + fclose (f); + +% %%%%%%%% output vars %%%%%%%% +% for(int i=0;i<12;i++) +% { +% d_dump_file.read((char *) &gps_time_sec[i], sizeof(double)); +% d_dump_file.read((char *) &doppler_l1_hz, sizeof(double)); +% d_dump_file.read((char *) &acc_carrier_phase_l1_cycles[i], sizeof(double)); +% d_dump_file.read((char *) &dist_m[i], sizeof(double)); +% d_dump_file.read((char *) &carrier_phase_l1_cycles[i], sizeof(double)); +% d_dump_file.read((char *) &prn[i], sizeof(double)); +% } + end +