From 140b38806623155fa907f6e426781efa774a31d5 Mon Sep 17 00:00:00 2001 From: Antonio Ramos Date: Mon, 20 Nov 2017 12:31:49 +0100 Subject: [PATCH] Modify RTKLIB PVT band selection and Tel. Dec. E5a --- src/algorithms/PVT/adapters/rtklib_pvt.cc | 5 +- src/algorithms/PVT/libs/rtklib_solver.cc | 5 +- .../libs/rtklib/rtklib_conversions.cc | 18 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 89 ++-- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 2 +- .../gnuradio_blocks/hybrid_observables_cc.cc | 9 - .../galileo_e5a_telemetry_decoder_cc.cc | 457 ++++++++---------- .../galileo_e5a_telemetry_decoder_cc.h | 47 +- src/core/system_parameters/Galileo_E1.h | 1 - src/core/system_parameters/Galileo_E5a.h | 8 +- 10 files changed, 288 insertions(+), 353 deletions(-) diff --git a/src/algorithms/PVT/adapters/rtklib_pvt.cc b/src/algorithms/PVT/adapters/rtklib_pvt.cc index 8e1e9fbe0..be1c9bd8d 100644 --- a/src/algorithms/PVT/adapters/rtklib_pvt.cc +++ b/src/algorithms/PVT/adapters/rtklib_pvt.cc @@ -193,9 +193,8 @@ RtklibPvt::RtklibPvt(ConfigurationInterface* configuration, int num_bands = 0; if ((gps_1C_count > 0) || (gal_1B_count > 0)) num_bands = 1; - if (((gps_1C_count > 0) || (gal_1B_count > 0)) && (gps_2S_count > 0) ) num_bands = 2; - if (((gps_1C_count > 0) || (gal_1B_count > 0)) && ((gal_E5a_count > 0) || (gal_E5b_count > 0)) ) num_bands = 2; - if (((gps_1C_count > 0) || (gal_1B_count > 0)) && (gps_2S_count > 0) && ((gal_E5a_count > 0) || (gal_E5b_count > 0))) num_bands = 3; + if (gps_2S_count > 0) num_bands = 2; + if ((gal_E5a_count > 0) || (gal_E5b_count > 0)) num_bands = 3; int number_of_frequencies = configuration->property(role + ".num_bands", num_bands); /* (1:L1, 2:L1+L2, 3:L1+L2+L5) */ if( (number_of_frequencies < 1) || (number_of_frequencies > 3) ) { diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 344888f67..1d29e5a5a 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -184,7 +184,10 @@ bool rtklib_solver::get_PVT(const std::map & gnss_observables_ //convert ephemeris from GNSS-SDR class to RTKLIB structure eph_data[valid_obs] = eph_to_rtklib(galileo_ephemeris_iter->second); //convert observation from GNSS-SDR class to RTKLIB structure - obsd_t newobs = {{0,0}, '0', '0', {}, {}, {}, {}, {}, {}}; + unsigned char default_code_ = static_cast(CODE_NONE); + obsd_t newobs = {{0,0}, '0', '0', {}, {}, + {default_code_, default_code_, default_code_}, + {}, {0.0, 0.0, 0.0}, {}}; obs_data[valid_obs] = insert_obs_to_rtklib(newobs, gnss_observables_iter->second, galileo_ephemeris_iter->second.WN_5, diff --git a/src/algorithms/libs/rtklib/rtklib_conversions.cc b/src/algorithms/libs/rtklib/rtklib_conversions.cc index c5e673b9c..ad8810fb3 100644 --- a/src/algorithms/libs/rtklib/rtklib_conversions.cc +++ b/src/algorithms/libs/rtklib/rtklib_conversions.cc @@ -36,7 +36,18 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch rtklib_obs.D[band] = gnss_synchro.Carrier_Doppler_hz; rtklib_obs.P[band] = gnss_synchro.Pseudorange_m; rtklib_obs.L[band] = gnss_synchro.Carrier_phase_rads / (2.0 * PI); - + switch(band) + { + case 0: + rtklib_obs.code[band] = static_cast(CODE_L1C); + break; + case 1: + rtklib_obs.code[band] = static_cast(CODE_L2S); + break; + case 2: + rtklib_obs.code[band] = static_cast(CODE_L5X); + break; + } double CN0_dB_Hz_est = gnss_synchro.CN0_dB_hz; if (CN0_dB_Hz_est > 63.75) CN0_dB_Hz_est = 63.75; if (CN0_dB_Hz_est < 0.0) CN0_dB_Hz_est = 0.0; @@ -56,7 +67,6 @@ obsd_t insert_obs_to_rtklib(obsd_t & rtklib_obs, const Gnss_Synchro & gnss_synch } rtklib_obs.time = gpst2time(adjgpsweek(week), gnss_synchro.RX_time); rtklib_obs.rcv = 1; - //printf("OBS RX TIME [%i]: %s,%f\n\r",rtklib_obs.sat,time_str(rtklib_obs.time,3),rtklib_obs.time.sec); return rtklib_obs; } @@ -88,8 +98,8 @@ eph_t eph_to_rtklib(const Galileo_Ephemeris & gal_eph) rtklib_sat.f0 = gal_eph.af0_4; rtklib_sat.f1 = gal_eph.af1_4; rtklib_sat.f2 = gal_eph.af2_4; - rtklib_sat.tgd[0] = 0; - rtklib_sat.tgd[1] = 0; + rtklib_sat.tgd[0] = gal_eph.BGD_E1E5a_5; + rtklib_sat.tgd[1] = gal_eph.BGD_E1E5b_5; rtklib_sat.tgd[2] = 0; rtklib_sat.tgd[3] = 0; rtklib_sat.toes = gal_eph.t0e_1; diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 6bf14c19e..056dc2901 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -84,7 +84,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, int iter, const prcopt_t *opt, double *var) { const double *lam = nav->lam[obs->sat - 1]; - double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma; + double PC, P1, P2, P1_P2, P1_C1, P2_C2, gamma_; int i = 0, j = 1, sys; *var = 0.0; @@ -124,7 +124,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, } } } - gamma = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */ + gamma_ = std::pow(lam[j], 2.0) / std::pow(lam[i], 2.0); /* f1^2/f2^2 */ P1 = obs->P[i]; P2 = obs->P[j]; P1_P2 = nav->cbias[obs->sat-1][0]; @@ -134,7 +134,7 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, /* if no P1-P2 DCB, use TGD instead */ if (P1_P2 == 0.0 && (sys & (SYS_GPS | SYS_GAL | SYS_QZS))) { - P1_P2 = (1.0 - gamma) * gettgd(obs->sat, nav); + P1_P2 = (1.0 - gamma_) * gettgd(obs->sat, nav); } if (opt->ionoopt == IONOOPT_IFLC) { /* dual-frequency */ @@ -144,15 +144,30 @@ double prange(const obsd_t *obs, const nav_t *nav, const double *azel, if (obs->code[j] == CODE_L2C) P2 += P2_C2; /* C2->P2 */ /* iono-free combination */ - PC = (gamma * P1 - P2) / (gamma - 1.0); + PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); } else { /* single-frequency */ + if((obs->code[i] == CODE_NONE) && (obs->code[j] == CODE_NONE)){return 0.0;} - if (P1 == 0.0) return 0.0; - if (obs->code[i] == CODE_L1C) P1 += P1_C1; /* C1->P1 */ - PC = P1 - P1_P2 / (1.0 - gamma); - } + else if((obs->code[i] != CODE_NONE) && (obs->code[j] == CODE_NONE)) + { + P1 += P1_C1; /* C1->P1 */ + PC = P1 - P1_P2 / (1.0 - gamma_); + } + else if((obs->code[i] == CODE_NONE) && (obs->code[j] != CODE_NONE)) + { + P2 += P2_C2; /* C2->P2 */ + PC = P2 - gamma_ * P1_P2 / (1.0 - gamma_); + } + /* dual-frequency */ + else + { + P1 += P1_C1; + P2 += P2_C2; + PC = (gamma_ * P1 - P2) / (gamma_ - 1.0); + } + } if (opt->sateph == EPHOPT_SBAS) PC -= P1_C1; /* sbas clock based C1 */ *var = std::pow(ERR_CBIAS, 2.0); @@ -285,12 +300,17 @@ int rescode(int iter, const obsd_t *obs, int n, const double *rs, continue; } /* geometric distance/azimuth/elevation angle */ - if ((r = geodist(rs + i * 6, rr, e)) <= 0.0 || satazel(pos, e, azel + i * 2) < opt->elmin) + if ((r = geodist(rs + i * 6, rr, e)) <= 0.0) { - trace(4, "geodist / satazel error\n"); + trace(4, "geodist error\n"); + continue; + } + double elaux = satazel(pos, e, azel + i * 2); + if(elaux < opt->elmin) + { + trace(4, "satazel error. el = %lf , elmin = %lf\n", elaux, opt->elmin); continue; } - /* psudorange with code bias correction */ if ((P = prange(obs+i, nav, azel+i*2, iter, opt, &vmeas)) == 0.0) { @@ -671,53 +691,6 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat, char *msg) { - // int k = 0; - // for (k = 0;kn;k++) - // { - // printf("NAV[%i]: sat %i, %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f , %f \r\n", - // k, - // nav->eph[k].sat, - // nav->eph[k].A, - // nav->eph[k].Adot, - // nav->eph[k].M0, - // nav->eph[k].OMG0, - // nav->eph[k].OMGd, - // nav->eph[k].cic, - // nav->eph[k].cis, - // nav->eph[k].code, - // nav->eph[k].crc, - // nav->eph[k].crs, - // nav->eph[k].cuc, - // nav->eph[k].cus, - // nav->eph[k].deln, - // nav->eph[k].e, - // nav->eph[k].f0, - // nav->eph[k].f1, - // nav->eph[k].f2, - // nav->eph[k].fit, - // nav->eph[k].flag, - // nav->eph[k].i0, - // nav->eph[k].idot, - // nav->eph[k].iodc, - // nav->eph[k].iode, - // nav->eph[k].ndot, - // nav->eph[k].omg, - // nav->eph[k].sat, - // nav->eph[k].sva, - // nav->eph[k].svh, - // nav->eph[k].tgd[0], - // nav->eph[k].toc.sec, - // nav->eph[k].toe.sec, - // nav->eph[k].toes, - // nav->eph[k].ttr.sec, - // nav->eph[k].week); - // } - prcopt_t opt_ = *opt; double *rs, *dts, *var, *azel_, *resp; int i, stat, vsat[MAXOBS] = {0}, svh[MAXOBS]; diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 1d569997e..a9c74ab55 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -3461,7 +3461,7 @@ double geodist(const double *rs, const double *rr, double *e) /* satellite azimuth/elevation angle ------------------------------------------- * compute satellite azimuth/elevation angle * args : double *pos I geodetic position {lat,lon,h} (rad,m) - * double *e I receiver-to-satellilte unit vevtor (ecef) + * double *e I receiver-to-satellilte unit vector (ecef) * double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output) * (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2) * return : elevation angle (rad) diff --git a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc index fb30b58ac..d63570996 100644 --- a/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc +++ b/src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc @@ -358,11 +358,6 @@ int hybrid_observables_cc::general_work (int noutput_items __attribute__((unused { d_gnss_synchro_history_queue[i].push_back(in[i][j]); } - //std::cout<<"push["< #include #include +#include #include "control_message_factory.h" -#include "gnss_synchro.h" #include "convolutional.h" @@ -114,7 +114,7 @@ void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols, int fra double page_symbols_deint[frame_length]; // 1. De-interleave - galileo_e5a_telemetry_decoder_cc::deinterleaver(GALILEO_FNAV_INTERLEAVER_ROWS, GALILEO_FNAV_INTERLEAVER_COLS, page_symbols, page_symbols_deint); + deinterleaver(GALILEO_FNAV_INTERLEAVER_ROWS, GALILEO_FNAV_INTERLEAVER_COLS, page_symbols, page_symbols_deint); // 2. Viterbi decoder // 2.1 Take into account the NOT gate in G2 polynomial (Galileo ICD Figure 13, FEC encoder) @@ -127,7 +127,7 @@ void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols, int fra } } int page_bits[frame_length/2]; - galileo_e5a_telemetry_decoder_cc::viterbi_decoder(page_symbols_deint, page_bits); + viterbi_decoder(page_symbols_deint, page_bits); // 3. Call the Galileo page decoder std::string page_String; @@ -197,32 +197,37 @@ galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc( { if (GALILEO_FNAV_PREAMBLE.at(i) == '0') { - d_preamble_bits[i] = 1; + d_preambles_bits[i] = 1; } else { - d_preamble_bits[i] = -1; + d_preambles_bits[i] = -1; + } + } + for (int i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++) + { + for (int k = 0; k < GALILEO_FNAV_CODES_PER_SYMBOL; k++) + { + d_preamble_samples[(i * GALILEO_FNAV_CODES_PER_SYMBOL) + k] = d_preambles_bits[i]; } } d_sample_counter = 0; - d_state = 0; - d_preamble_lock = false; + d_stat = 0; + corr_value = 0; + d_flag_preamble = false; d_preamble_index = 0; d_flag_frame_sync = false; - d_current_symbol = 0; - d_prompt_counter = 0; - d_symbol_counter = 0; - - d_TOW_at_Preamble = 0; - d_TOW_at_current_symbol = 0; - - d_CRC_error_counter = 0; - d_sign_init = 0; - - d_flag_preamble = false; - d_channel = 0; + d_TOW_at_current_symbol = 0.0; flag_TOW_set = false; + d_CRC_error_counter = 0; + d_channel = 0; + delta_t = 0.0; + d_symbol_counter = 0; + d_prompt_acum = 0.0; + flag_bit_start = false; + new_symbol = false; + required_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE + GALILEO_FNAV_PREAMBLE_LENGTH_BITS; } @@ -245,265 +250,222 @@ galileo_e5a_telemetry_decoder_cc::~galileo_e5a_telemetry_decoder_cc() int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribute__((unused)), gr_vector_int &ninput_items __attribute__((unused)), gr_vector_const_void_star &input_items, gr_vector_void_star &output_items) { - Gnss_Synchro *out = reinterpret_cast(output_items[0]); // Get the output buffer pointer - const Gnss_Synchro *in = reinterpret_cast(input_items[0]); // Get the input buffer pointer + int preamble_diff = 0; - /* Terminology: Prompt: output from tracking Prompt correlator (Prompt samples) - * Symbol: encoded navigation bits. 1 symbol = 20 samples in E5a - * Bit: decoded navigation bits forming words as described in Galileo ICD - * States: 0 Receiving dummy samples. - * 1 Preamble not locked - * 3 Preamble lock - */ - switch (d_state) + Gnss_Synchro* out = reinterpret_cast(output_items[0]); // Get the output buffer pointer + const Gnss_Synchro* in = reinterpret_cast(input_items[0]); // Get the input buffer pointer + + Gnss_Synchro current_sample; //structure to save the synchronization information and send the output object to the next block + //1. Copy the current tracking output + current_sample = in[0]; + d_symbol_counter++; + if(flag_bit_start) { - case 0: - { - if (in[0].Prompt_I != 0) - { - d_current_symbol += in[0].Prompt_I; - if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1) - { - if (d_current_symbol > 0) - { - d_page_symbols[d_symbol_counter] = 1; - } - else - { - d_page_symbols[d_symbol_counter] = -1; - } - d_current_symbol = 0; - d_symbol_counter++; - d_prompt_counter = 0; - if (d_symbol_counter == GALILEO_FNAV_PREAMBLE_LENGTH_BITS - 1) - { - d_state = 1; - } - } - else - { - d_prompt_counter++; - } - } - break; - } - case 1: - { - d_current_symbol += in[0].Prompt_I; - if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1) - { - if (d_current_symbol > 0) - { - d_page_symbols[d_symbol_counter] = 1; - } - else - { - d_page_symbols[d_symbol_counter] = -1; - } - // d_page_symbols[d_symbol_counter] = d_current_symbol_float/(float)GALILEO_FNAV_CODES_PER_SYMBOL; - d_current_symbol = 0; - d_symbol_counter++; - d_prompt_counter = 0; - // **** Attempt Preamble correlation **** - bool corr_flag = true; - int corr_sign = 0; // sequence can be found inverted - // check if the preamble starts positive correlated or negative correlated - if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS] < 0) // symbols clipping - { - corr_sign = -d_preamble_bits[0]; - } - else - { - corr_sign = d_preamble_bits[0]; - } - // the preamble is fully correlated only if maintains corr_sign along the whole sequence - for (int i = 1; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++) - { - if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS + i] < 0 && d_preamble_bits[i]+corr_sign != 0) - { - //exit for - corr_flag = false; - break; - } - if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS + i] > 0 && d_preamble_bits[i]+corr_sign == 0) - { - //exit for - corr_flag = false; - break; - } - } - // - if (corr_flag == true) // preamble fully correlates - { - d_preamble_index = d_sample_counter - GALILEO_FNAV_CODES_PER_PREAMBLE;//record the preamble sample stamp. Remember correlation appears at the end of the preamble in this design - LOG(INFO) << "Preamble detection in E5a for Galileo satellite " << this->d_satellite << std::endl; - d_symbol_counter = 0; // d_page_symbols start right after preamble and finish at the end of next preamble. - d_state = 2; // preamble lock - } - if (d_symbol_counter >= GALILEO_FNAV_SYMBOLS_PER_PAGE + GALILEO_FNAV_PREAMBLE_LENGTH_BITS) - { - d_symbol_counter = GALILEO_FNAV_PREAMBLE_LENGTH_BITS; // prevents overflow - } - } - else - { - d_prompt_counter++; - } - break; - } - case 2: - { - d_current_symbol += in[0].Prompt_I; - if (d_prompt_counter == GALILEO_FNAV_CODES_PER_SYMBOL - 1) - { - if (d_current_symbol > 0) - { - d_page_symbols[d_symbol_counter] = 1; - } - else - { - d_page_symbols[d_symbol_counter] = -1; - } - // d_page_symbols[d_symbol_counter] = d_current_symbol_float/(float)GALILEO_FNAV_CODES_PER_SYMBOL; - d_current_symbol = 0; - d_symbol_counter++; - d_prompt_counter = 0; - // At the right sample stamp, check preamble synchro - if (d_sample_counter == d_preamble_index + GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) - { - // **** Attempt Preamble correlation **** - bool corr_flag = true; - int corr_sign = 0; // sequence can be found inverted - // check if the preamble starts positive correlated or negative correlated - if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS] < 0) // symbols clipping - { - corr_sign = -d_preamble_bits[0]; - } - else - { - corr_sign = d_preamble_bits[0]; - } - // the preamble is fully correlated only if maintains corr_sign along the whole sequence - for (int i = 1; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++) - { - if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS + i] < 0 && d_preamble_bits[i]+corr_sign != 0) - { - //exit for - corr_flag = false; - break; - } - if (d_page_symbols[d_symbol_counter - GALILEO_FNAV_PREAMBLE_LENGTH_BITS + i] > 0 && d_preamble_bits[i]+corr_sign == 0) - { - //exit for - corr_flag = false; - break; - } - } + d_prompt_acum += current_sample.Prompt_I; + if(d_symbol_counter == GALILEO_FNAV_CODES_PER_SYMBOL) + { + current_sample.Prompt_I = d_prompt_acum / static_cast(GALILEO_FNAV_CODES_PER_SYMBOL); + d_symbol_history.push_back(current_sample); //add new symbol to the symbol queue + d_prompt_acum = 0.0; + d_symbol_counter = 0; + new_symbol = true; + } + } + else + { + if(current_sample.Prompt_I < 0.0) + { + d_preamble_init.push_back(1); + } + else + { + d_preamble_init.push_back(-1); + } - if (corr_flag == true) // NEW PREAMBLE RECEIVED. DECODE PAGE - { - d_preamble_index = d_sample_counter - GALILEO_FNAV_CODES_PER_PREAMBLE;//record the preamble sample stamp - // DECODE WORD - decode_word(d_page_symbols, GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS); - // CHECK CRC - if (d_nav.flag_CRC_test == true) - { - d_CRC_error_counter = 0; - d_flag_preamble = true; //valid preamble indicator (initialized to false every work()) - if (!d_flag_frame_sync) - { - d_flag_frame_sync = true; - DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " - << in[0].Tracking_sample_counter << " [samples]"; - } - d_symbol_counter = 0; // d_page_symbols start right after preamble and finish at the end of next preamble. - } - else - { - d_CRC_error_counter++; - if (d_CRC_error_counter > GALILEO_E5a_CRC_ERROR_LIMIT) - { - LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; - d_state = 1; - d_symbol_counter = GALILEO_FNAV_PREAMBLE_LENGTH_BITS; // prevents overflow - d_flag_frame_sync = false; - } - else - { - d_symbol_counter = 0; // d_page_symbols start right after preamble and finish at the end of next preamble. - } - } - } - } - } - else - { - d_prompt_counter++; - } - break; - } + if (d_preamble_init.size() == GALILEO_FNAV_CODES_PER_PREAMBLE) + { + std::deque::iterator iter; + int k = 0; + corr_value = 0; + for (iter = d_preamble_init.begin(); iter != d_preamble_init.end(); iter++) + { + corr_value += *iter * d_preamble_samples[k]; + k++; + } + if(abs(corr_value) == GALILEO_FNAV_CODES_PER_PREAMBLE) + { + d_symbol_counter = 0; + flag_bit_start = true; + corr_value = 0; + while(d_preamble_init.size() > 0) + { //Clear preamble correlating queue + d_preamble_init.pop_front(); + } + while(d_symbol_history.size() > 0) + { //Clear symbol queue in order to prevent possible symbol discontinuities + d_symbol_history.pop_front(); + } + LOG(INFO) << "Bit start sync for Galileo E5a satellite " << d_satellite; + } + else + { + d_preamble_init.pop_front(); + } + } + } + d_sample_counter++; //count for the processed samples + consume_each(1); + + d_flag_preamble = false; + + if ((d_symbol_history.size() > required_symbols) && new_symbol) + { + //******* preamble correlation ******** + corr_value = 0; + for (int i = 0; i < GALILEO_FNAV_PREAMBLE_LENGTH_BITS; i++) + { + if (d_symbol_history.at(i).Prompt_I < 0.0) // symbols clipping + { + corr_value -= d_preambles_bits[i]; + } + else + { + corr_value += d_preambles_bits[i]; + } + } } + //******* frame sync ****************** + if ((d_stat == 0) && new_symbol) //no preamble information + { + if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS) + { + d_preamble_index = d_sample_counter;//record the preamble sample stamp + LOG(INFO) << "Preamble detection for Galileo E5a satellite " << d_satellite; + d_stat = 1; // enter into frame pre-detection status + } + } + else if ((d_stat == 1) && new_symbol) // posible preamble lock + { + if (abs(corr_value) >= GALILEO_FNAV_PREAMBLE_LENGTH_BITS) + { + //check preamble separation + preamble_diff = d_sample_counter - d_preamble_index; + if (preamble_diff == GALILEO_FNAV_CODES_PER_PAGE) + { + //try to decode frame + LOG(INFO) << "Starting page decoder for Galileo E5a satellite " << d_satellite; + d_preamble_index = d_sample_counter; //record the preamble sample stamp + d_stat = 2; + } + else if (preamble_diff > GALILEO_FNAV_CODES_PER_PAGE) + { + d_stat = 0; // start again + flag_bit_start = false; + LOG(INFO) << "Preamble diff = " << preamble_diff; + } + } + } + else if ((d_stat == 2) && new_symbol) + { + if (d_sample_counter == (d_preamble_index + GALILEO_FNAV_CODES_PER_PAGE)) + { + // NEW Galileo page part is received + // 0. fetch the symbols into an array + int frame_length = GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS; + double corr_sign = 0.0; + if(corr_value > 0) + { + corr_sign = -1.0; + } + else + { + corr_sign = 1.0; + } + for (int i = 0; i < frame_length; i++) + { + page_symbols[i] = corr_sign * d_symbol_history.at(i + GALILEO_FNAV_PREAMBLE_LENGTH_BITS).Prompt_I; // because last symbol of the preamble is just received now! + } + + //call the decoder + decode_word(page_symbols, frame_length); + if (d_nav.flag_CRC_test == true) + { + d_CRC_error_counter = 0; + d_flag_preamble = true; //valid preamble indicator (initialized to false every work()) + d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P) + if (!d_flag_frame_sync) + { + d_flag_frame_sync = true; + DLOG(INFO) << " Frame sync SAT " << this->d_satellite << " with preamble start at " + << d_symbol_history.at(0).Tracking_sample_counter << " [samples]"; + } + } + else + { + d_CRC_error_counter++; + d_preamble_index = d_sample_counter; //record the preamble sample stamp + if (d_CRC_error_counter > GALILEO_E5A_CRC_ERROR_LIMIT) + { + LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite; + d_flag_frame_sync = false; + d_stat = 0; + flag_bit_start = false; + } + } + } + } + new_symbol = false; // UPDATE GNSS SYNCHRO DATA - Gnss_Synchro current_synchro_data; //structure to save the synchronization information and send the output object to the next block - //1. Copy the current tracking output - current_synchro_data = in[0]; - //2. Add the telemetry decoder information - if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true) + //Add the telemetry decoder information + if (d_flag_preamble and d_nav.flag_TOW_set) //update TOW at the preamble instant //We expect a preamble each 10 seconds (FNAV page period) { if (d_nav.flag_TOW_1 == true) { - d_TOW_at_Preamble = d_nav.FNAV_TOW_1; - d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); + d_TOW_at_current_symbol = d_nav.FNAV_TOW_1 + (static_cast(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_nav.flag_TOW_1 = false; } - if (d_nav.flag_TOW_2 == true) + else if (d_nav.flag_TOW_2 == true) { - d_TOW_at_Preamble = d_nav.FNAV_TOW_2; - d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); + d_TOW_at_current_symbol = d_nav.FNAV_TOW_2 + (static_cast(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_nav.flag_TOW_2 = false; } - if (d_nav.flag_TOW_3 == true) + else if (d_nav.flag_TOW_3 == true) { - d_TOW_at_Preamble = d_nav.FNAV_TOW_3; - d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); + d_TOW_at_current_symbol = d_nav.FNAV_TOW_3 + (static_cast(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_nav.flag_TOW_3 = false; } - if (d_nav.flag_TOW_4 == true) + else if (d_nav.flag_TOW_4 == true) { - d_TOW_at_Preamble = d_nav.FNAV_TOW_4; - d_TOW_at_current_symbol = d_TOW_at_Preamble + (static_cast(GALILEO_FNAV_CODES_PER_PAGE+GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); + d_TOW_at_current_symbol = d_nav.FNAV_TOW_4 + (static_cast(GALILEO_FNAV_CODES_PER_PAGE + GALILEO_FNAV_CODES_PER_PREAMBLE) * GALILEO_E5a_CODE_PERIOD); d_nav.flag_TOW_4 = false; } else { - //this page has no timming information - d_TOW_at_Preamble = d_TOW_at_Preamble + GALILEO_FNAV_SECONDS_PER_PAGE; - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E5a_CODE_PERIOD; + d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD; } - } else //if there is not a new preamble, we define the TOW of the current symbol { - d_TOW_at_current_symbol = d_TOW_at_current_symbol + GALILEO_E5a_CODE_PERIOD; + d_TOW_at_current_symbol += GALILEO_E5a_CODE_PERIOD; } //if (d_flag_frame_sync == true and d_nav.flag_TOW_set==true and d_nav.flag_CRC_test == true) - if (d_flag_frame_sync == true and d_nav.flag_TOW_set == true) + if (d_flag_frame_sync and d_nav.flag_TOW_set) { - current_synchro_data.Flag_valid_word = true; + current_sample.Flag_valid_word = true; } else { - current_synchro_data.Flag_valid_word = false; + current_sample.Flag_valid_word = false; } - current_synchro_data.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol*1000.0)/1000.0; + current_sample.TOW_at_current_symbol_s = floor(d_TOW_at_current_symbol*1000.0)/1000.0; - if(d_dump == true) + if(d_dump) { // MULTIPLEXED FILE RECORDING - Record results to file try @@ -512,20 +474,23 @@ int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items __attribut unsigned long int tmp_ulong_int; tmp_double = d_TOW_at_current_symbol; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_ulong_int = current_synchro_data.Tracking_sample_counter; + tmp_ulong_int = current_sample.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(unsigned long int)); - tmp_double = d_TOW_at_Preamble; + tmp_double = 0.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); } catch (const std::ifstream::failure & e) { - LOG(WARNING) << "Exception writing observables dump file " << e.what(); + LOG(WARNING) << "Exception writing Galileo E5a Telemetry Decoder dump file " << e.what(); } } - d_sample_counter++; //count for the processed samples - //3. Make the output (copy the object contents to the GNURadio reserved memory) - out[0] = current_synchro_data; - consume_each(1); + // remove used symbols from history + while (d_symbol_history.size() > required_symbols) + { + d_symbol_history.pop_front(); + } + //3. Make the output + out[0] = current_sample; return 1; } diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h index 46620a86e..4390b3181 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h @@ -39,6 +39,7 @@ #include #include +#include #include #include "Galileo_E5a.h" #include "concurrent_queue.h" @@ -48,8 +49,7 @@ #include "galileo_almanac.h" #include "galileo_iono.h" #include "galileo_utc_model.h" - -//#include "convolutional.h" +#include "gnss_synchro.h" class galileo_e5a_telemetry_decoder_cc; @@ -85,38 +85,33 @@ private: void decode_word(double *page_symbols, int frame_length); - int d_preamble_bits[GALILEO_FNAV_PREAMBLE_LENGTH_BITS]; - double d_page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE + GALILEO_FNAV_PREAMBLE_LENGTH_BITS]; - - double d_current_symbol; - long unsigned int d_symbol_counter; - int d_prompt_counter; - int d_sign_init; - + int d_preambles_bits[GALILEO_FNAV_PREAMBLE_LENGTH_BITS]; + int d_preamble_samples[GALILEO_FNAV_CODES_PER_PREAMBLE]; + std::deque d_preamble_init; + int d_stat; + int d_CRC_error_counter; + int d_channel; + int d_symbol_counter; + int corr_value; + unsigned int required_symbols; long unsigned int d_sample_counter; long unsigned int d_preamble_index; - - bool d_preamble_lock; bool d_flag_frame_sync; - int d_state; - bool d_flag_preamble; - int d_CRC_error_counter; - - // navigation message vars - Galileo_Fnav_Message d_nav; - bool d_dump; - Gnss_Satellite d_satellite; - int d_channel; - - double d_TOW_at_Preamble; - double d_TOW_at_current_symbol; - bool flag_TOW_set; - + bool flag_bit_start; + bool new_symbol; + double d_prompt_acum; + double page_symbols[GALILEO_FNAV_SYMBOLS_PER_PAGE - GALILEO_FNAV_PREAMBLE_LENGTH_BITS]; + double d_TOW_at_current_symbol; + double delta_t; //GPS-GALILEO time offset std::string d_dump_filename; std::ofstream d_dump_file; + std::deque d_symbol_history; + Gnss_Satellite d_satellite; + // navigation message vars + Galileo_Fnav_Message d_nav; }; #endif /* GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_ */ diff --git a/src/core/system_parameters/Galileo_E1.h b/src/core/system_parameters/Galileo_E1.h index fcd1e5129..a95afd1ca 100644 --- a/src/core/system_parameters/Galileo_E1.h +++ b/src/core/system_parameters/Galileo_E1.h @@ -84,7 +84,6 @@ const int GALILEO_PAGE_TYPE_BITS = 6; const int GALILEO_DATA_JK_BITS = 128; const int GALILEO_DATA_FRAME_BITS = 196; const int GALILEO_DATA_FRAME_BYTES = 25; -//const double GALIELO_E1_CODE_PERIOD = 0.004; const double GALILEO_E1_CODE_PERIOD = 0.004; const std::vector> type({{1,6}}); diff --git a/src/core/system_parameters/Galileo_E5a.h b/src/core/system_parameters/Galileo_E5a.h index d38a2ed14..5663d8dc2 100644 --- a/src/core/system_parameters/Galileo_E5a.h +++ b/src/core/system_parameters/Galileo_E5a.h @@ -52,9 +52,9 @@ const int Galileo_E5a_SYMBOL_RATE_BPS = 50; //!< Galileo E5a symbo const int Galileo_E5a_NUMBER_OF_CODES = 50; -// OBSERVABLE HISTORY DEEP FOR INTERPOLATION +// OBSERVABLE HISTORY DEEP FOR INTERPOLATION AND CRC ERROR LIMIT const int GALILEO_E5A_HISTORY_DEEP = 20; - +const int GALILEO_E5A_CRC_ERROR_LIMIT = 6; // F/NAV message structure @@ -103,7 +103,7 @@ const double FNAV_BGD_1_LSB = TWO_N32; const std::vector> FNAV_E5ahs_1_bit({{154,2}}); const std::vector> FNAV_WN_1_bit({{156,12}}); const std::vector> FNAV_TOW_1_bit({{168,20}}); -const std::vector> FNAV_E5advs_1_bit({{189,1}}); +const std::vector> FNAV_E5advs_1_bit({{188,1}}); // WORD 2 Ephemeris (1/3) const std::vector> FNAV_IODnav_2_bit({{7,10}}); @@ -145,7 +145,7 @@ const std::vector> FNAV_TOW_3_bit({{187,20}}); // WORD 4 Ephemeris (3/3) const std::vector> FNAV_IODnav_4_bit({{7,10}}); -const std::vector> FNAV_Cic_4_bit({{18,16}}); +const std::vector> FNAV_Cic_4_bit({{17,16}}); const double FNAV_Cic_4_LSB = TWO_N29; const std::vector> FNAV_Cis_4_bit({{33,16}}); const double FNAV_Cis_4_LSB = TWO_N29;