mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Modify RTKLIB PVT band selection and Tel. Dec. E5a

This commit is contained in:
Antonio Ramos 2017-11-20 12:31:49 +01:00
parent bc78416f52
commit 140b388066
10 changed files with 288 additions and 353 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/algorithms/PVT/adapters/rtklib_pvt.cc
View File

@ -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) )
{

5
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -184,7 +184,10 @@ bool rtklib_solver::get_PVT(const std::map<int,Gnss_Synchro> & 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<unsigned char>(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,

18
src/algorithms/libs/rtklib/rtklib_conversions.cc
View File

@ -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<unsigned char>(CODE_L1C);
break;
case 1:
rtklib_obs.code[band] = static_cast<unsigned char>(CODE_L2S);
break;
case 2:
rtklib_obs.code[band] = static_cast<unsigned char>(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;

89
src/algorithms/libs/rtklib/rtklib_pntpos.cc
View File

@ -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;k<n;k++)
// {
// printf("OBS[%i]: sat %i, P:%f ,LLI:%s \r\n",k,obs[k].sat,obs[k].P[0], obs[k].LLI);
// }
//
// for (k = 0;k<nav->n;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];

2
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

@ -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)

9
src/algorithms/observables/gnuradio_blocks/hybrid_observables_cc.cc
View File

@ -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["<<i<<"] items "<<n_consume[i]
/// <<" latest T_rx: "<<(double)in[i][ninput_items[i]-1].Tracking_sample_counter/(double)in[i][ninput_items[i]-1].fs
// <<" [s] q size: "
// <<d_gnss_synchro_history_queue[i].size()
// <<std::endl;
}
bool channel_history_ok;
@ -448,10 +443,6 @@ int hybrid_observables_cc::general_work (int noutput_items __attribute__((unused
}
}
else
{
//std::cout<<"ch["<<i<<"] delta_T_rx:"<<delta_T_rx_s*1000.0<<std::endl;
}
}
}
}

457
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.cc
View File

@ -39,8 +39,8 @@
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <cmath>
#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<Gnss_Synchro *>(output_items[0]); // Get the output buffer pointer
const Gnss_Synchro *in = reinterpret_cast<const Gnss_Synchro *>(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<Gnss_Synchro*>(output_items[0]); // Get the output buffer pointer
const Gnss_Synchro* in = reinterpret_cast<const Gnss_Synchro*>(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<double>(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<int>::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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<double>(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<char*>(&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<char*>(&tmp_ulong_int), sizeof(unsigned long int));
tmp_double = d_TOW_at_Preamble;
tmp_double = 0.0;
d_dump_file.write(reinterpret_cast<char*>(&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;
}

47
src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_e5a_telemetry_decoder_cc.h
View File

@ -39,6 +39,7 @@
#include <fstream>
#include <string>
#include <deque>
#include <gnuradio/block.h>
#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<int> 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<Gnss_Synchro> d_symbol_history;
Gnss_Satellite d_satellite;
// navigation message vars
Galileo_Fnav_Message d_nav;
};
#endif /* GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_ */

1
src/core/system_parameters/Galileo_E1.h
View File

@ -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<std::pair<int,int>> type({{1,6}});

8
src/core/system_parameters/Galileo_E5a.h
View File

@ -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<std::pair<int,int>> FNAV_E5ahs_1_bit({{154,2}});
const std::vector<std::pair<int,int>> FNAV_WN_1_bit({{156,12}});
const std::vector<std::pair<int,int>> FNAV_TOW_1_bit({{168,20}});
const std::vector<std::pair<int,int>> FNAV_E5advs_1_bit({{189,1}});
const std::vector<std::pair<int,int>> FNAV_E5advs_1_bit({{188,1}});
// WORD 2 Ephemeris (1/3)
const std::vector<std::pair<int,int>> FNAV_IODnav_2_bit({{7,10}});
@ -145,7 +145,7 @@ const std::vector<std::pair<int,int>> FNAV_TOW_3_bit({{187,20}});
// WORD 4 Ephemeris (3/3)
const std::vector<std::pair<int,int>> FNAV_IODnav_4_bit({{7,10}});
const std::vector<std::pair<int,int>> FNAV_Cic_4_bit({{18,16}});
const std::vector<std::pair<int,int>> FNAV_Cic_4_bit({{17,16}});
const double FNAV_Cic_4_LSB = TWO_N29;
const std::vector<std::pair<int,int>> FNAV_Cis_4_bit({{33,16}});
const double FNAV_Cis_4_LSB = TWO_N29;