mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-06-25 22:43:14 +00:00
Tracking adapted blocks + Telemetry decoder
This commit is contained in:
marc-sales
parent
19f6d9aaa8
commit
9b5c055bfd
|
|
@ -1,5 +1,5 @@
|
|||
/*!
|
||||
* \file galileo_e1_pcps_ambiguous_acquisition.cc
|
||||
* \file galileo_e5a_pcps_acquisition.cc
|
||||
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
|
||||
* Galileo E5a data and pilot Signals
|
||||
* \author Marc Sales, 2014. marcsales92(at)gmail.com
|
||||
|
|
@ -63,7 +63,17 @@ GalileoE5aPcpsAcquisition::GalileoE5aPcpsAcquisition(
|
|||
shift_resolution_ = configuration_->property(role + ".doppler_max", 15);
|
||||
sampled_ms_ = configuration_->property(role + ".coherent_integration_time_ms", 1);
|
||||
// sampled_ms is an integer, always multiple of primary code period
|
||||
max_dwells_= configuration->property(role + ".max_dwells", 1);
|
||||
bit_transition_flag_ = configuration_->property(role + ".bit_transition_flag", false);
|
||||
|
||||
if (!bit_transition_flag_)
|
||||
{
|
||||
max_dwells_ = configuration_->property(role + ".max_dwells", 1);
|
||||
}
|
||||
else
|
||||
{
|
||||
max_dwells_ = 2;
|
||||
}
|
||||
|
||||
dump_filename_ = configuration_->property(role + ".dump_filename",
|
||||
default_dump_filename);
|
||||
|
||||
|
|
@ -206,8 +216,8 @@ void GalileoE5aPcpsAcquisition::set_local_code()
|
|||
|
||||
std::complex<float>* code = new std::complex<float>[code_length_];
|
||||
|
||||
galileo_e5_a_code_gen_complex_sampled(code,
|
||||
pilot, gnss_synchro_->PRN, fs_in_, 0, false);
|
||||
galileo_e5_a_code_gen_complex_sampled(code, gnss_synchro_->Signal,
|
||||
gnss_synchro_->PRN, fs_in_, 0, false);
|
||||
|
||||
for (unsigned int i = 0; i < sampled_ms_/4; i++)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
/*!
|
||||
* \file galileo_e1_pcps_ambiguous_acquisition.cc
|
||||
* \file galileo_e5a_pcps_acquisition.cc
|
||||
* \brief Adapts a PCPS acquisition block to an AcquisitionInterface for
|
||||
* Galileo E5a data and pilot Signals
|
||||
* \author Marc Sales, 2014. marcsales92(at)gmail.com
|
||||
|
|
@ -29,8 +29,8 @@
|
|||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GALILEO_E5A_PCPS_ACQUISITION_H_
|
||||
#define GALILEO_E5A_PCPS_ACQUISITION_H_
|
||||
#ifndef GNSS_SDR_GALILEO_E5A_PCPS_ACQUISITION_H_
|
||||
#define GNSS_SDR_GALILEO_E5A_PCPS_ACQUISITION_H_
|
||||
|
||||
#include <string>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
|
|
@ -154,4 +154,4 @@ private:
|
|||
float calculate_threshold(float pfa);
|
||||
};
|
||||
|
||||
#endif /* GALILEO_E5A_PCPS_ACQUISITION_H_ */
|
||||
#endif /* GNSS_SDR_GALILEO_E5A_PCPS_ACQUISITION_H_ */
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
/*!
|
||||
* \file galileo_e1_signal_processing.cc
|
||||
* \file galileo_e5_signal_processing.cc
|
||||
* \brief This library implements various functions for Galileo E5 signals such
|
||||
* as replica code generation
|
||||
* \author Marc Sales, 2014. marcsales92(at)gmail.com
|
||||
|
|
@ -33,8 +33,9 @@
|
|||
|
||||
#include "galileo_e5_signal_processing.h"
|
||||
|
||||
void galileo_e5_a_code_gen_complex(std::complex<float>* _dest, signed int _prn, bool _pilot)
|
||||
void galileo_e5_a_code_gen_complex(std::complex<float>* _dest, signed int _prn, char _Signal[3])
|
||||
{
|
||||
std::string _galileo_signal = _Signal;
|
||||
unsigned int prn=_prn-1;
|
||||
unsigned int index=0;
|
||||
//int _code_int[(int)Galileo_E5a_CODE_LENGTH_CHIPS];
|
||||
|
|
@ -44,7 +45,7 @@ void galileo_e5_a_code_gen_complex(std::complex<float>* _dest, signed int _prn,
|
|||
{
|
||||
return;
|
||||
}
|
||||
if (_pilot)
|
||||
if (_galileo_signal.rfind("5Q") != std::string::npos && _galileo_signal.length() >= 2)
|
||||
{
|
||||
for (size_t i = 0; i < Galileo_E5a_Q_PRIMARY_CODE[prn].length(); i++)
|
||||
{
|
||||
|
|
@ -61,7 +62,7 @@ void galileo_e5_a_code_gen_complex(std::complex<float>* _dest, signed int _prn,
|
|||
index = index + 4;
|
||||
}
|
||||
}
|
||||
else
|
||||
else if (_galileo_signal.rfind("5I") != std::string::npos && _galileo_signal.length() >= 2)
|
||||
{
|
||||
for (size_t i = 0; i < Galileo_E5a_I_PRIMARY_CODE[prn].length(); i++)
|
||||
{
|
||||
|
|
@ -78,13 +79,14 @@ void galileo_e5_a_code_gen_complex(std::complex<float>* _dest, signed int _prn,
|
|||
}
|
||||
}
|
||||
|
||||
void galileo_e5_a_code_gen_complex_sampled(std::complex<float>* _dest, bool _pilot,
|
||||
void galileo_e5_a_code_gen_complex_sampled(std::complex<float>* _dest, char _Signal[3],
|
||||
unsigned int _prn, signed int _fs, unsigned int _chip_shift,
|
||||
bool _secondary_flag)
|
||||
{
|
||||
// This function is based on the GNU software GPS for MATLAB in the Kay Borre book
|
||||
|
||||
std::complex<float> _code[Galileo_E5a_CODE_LENGTH_CHIPS];
|
||||
std::string _galileo_signal = _Signal;
|
||||
signed int _samplesPerCode, _codeValueIndex;
|
||||
float _ts;
|
||||
float _tc;
|
||||
|
|
@ -96,7 +98,7 @@ void galileo_e5_a_code_gen_complex_sampled(std::complex<float>* _dest, bool _pil
|
|||
% (int)Galileo_E5a_CODE_LENGTH_CHIPS)
|
||||
* _samplesPerCode / Galileo_E5a_CODE_LENGTH_CHIPS;
|
||||
|
||||
galileo_e5_a_code_gen_complex(_code , _prn , _pilot);
|
||||
galileo_e5_a_code_gen_complex(_code , _prn , _Signal);
|
||||
|
||||
if (_fs != _codeFreqBasis)
|
||||
{
|
||||
|
|
@ -106,8 +108,7 @@ void galileo_e5_a_code_gen_complex_sampled(std::complex<float>* _dest, bool _pil
|
|||
delete[] _code;
|
||||
_code = _resampled_signal;
|
||||
}
|
||||
// TODO generar codigo secundario cuando sepamos si se hace aqui o se replica en el tracking
|
||||
// o en una funcion a parte en esta misma clase
|
||||
// TODO secundary code generated here??
|
||||
for (unsigned int i = 0; i < _samplesPerCode; i++)
|
||||
{
|
||||
_dest[(i+delay)%_samplesPerCode] = _code[i];
|
||||
|
|
|
|||
|
|
@ -1,5 +1,5 @@
|
|||
/*!
|
||||
* \file galileo_e1_signal_processing.cc
|
||||
* \file galileo_e5_signal_processing.cc
|
||||
* \brief This library implements various functions for Galileo E5 signals such
|
||||
* as replica code generation
|
||||
* \author Marc Sales, 2014. marcsales92(at)gmail.com
|
||||
|
|
@ -31,8 +31,8 @@
|
|||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GALILEO_E5_SIGNAL_PROCESSING_H_
|
||||
#define GALILEO_E5_SIGNAL_PROCESSING_H_
|
||||
#ifndef GNSS_SDR_GALILEO_E5_SIGNAL_PROCESSING_H_
|
||||
#define GNSS_SDR_GALILEO_E5_SIGNAL_PROCESSING_H_
|
||||
|
||||
#include <complex>
|
||||
#include <iostream>
|
||||
|
|
@ -44,15 +44,15 @@
|
|||
* \brief Generates Galileo E5a code at 1 sample/chip
|
||||
* bool _pilot generates E5aQ code if true and E5aI (data signal) if false.
|
||||
*/
|
||||
void galileo_e5_a_code_gen_complex(std::complex<float>* _dest, signed int _prn, bool _pilot);
|
||||
void galileo_e5_a_code_gen_complex(std::complex<float>* _dest, signed int _prn, char _Signal[3]);
|
||||
|
||||
/*!
|
||||
* \brief Generates Galileo E5a complex code, shifted to the desired chip and sampled at a frequency fs
|
||||
* bool _pilot generates E5aQ code if true and E5aI (data signal) if false.
|
||||
*/
|
||||
void galileo_e5_a_code_gen_complex_sampled(std::complex<float>* _dest,
|
||||
bool _pilot, unsigned int _prn, signed int _fs, unsigned int _chip_shift,
|
||||
char _Signal[3], unsigned int _prn, signed int _fs, unsigned int _chip_shift,
|
||||
bool _secondary_flag);
|
||||
|
||||
|
||||
#endif /* GALILEO_E5_SIGNAL_PROCESSING_H_ */
|
||||
#endif /* GNSS_SDR_GALILEO_E5_SIGNAL_PROCESSING_H_ */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,116 @@
|
|||
/*!
|
||||
* \file galileo_e5a_telemetry_decoder.h
|
||||
* \brief Interface of an adapter of a GALILEO E5a FNAV data decoder block
|
||||
* to a TelemetryDecoderInterface
|
||||
* \author Marc Sales 2014 marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "galileo_e5a_telemetry_decoder.h"
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <glog/logging.h>
|
||||
#include "galileo_ephemeris.h"
|
||||
#include "galileo_almanac.h"
|
||||
#include "galileo_iono.h"
|
||||
#include "galileo_utc_model.h"
|
||||
#include "configuration_interface.h"
|
||||
#include "galileo_e5a_telemetry_decoder_cc.h"
|
||||
|
||||
extern concurrent_queue<Galileo_Ephemeris> global_galileo_ephemeris_queue;
|
||||
extern concurrent_queue<Galileo_Iono> global_galileo_iono_queue;
|
||||
extern concurrent_queue<Galileo_Utc_Model> global_galileo_utc_model_queue;
|
||||
extern concurrent_queue<Galileo_Almanac> global_galileo_almanac_queue;
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
GalileoE5aTelemetryDecoder::GalileoE5aTelemetryDecoder(ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams,
|
||||
boost::shared_ptr<gr::msg_queue> queue) :
|
||||
role_(role),
|
||||
in_streams_(in_streams),
|
||||
out_streams_(out_streams),
|
||||
queue_(queue)
|
||||
{
|
||||
std::string default_item_type = "gr_complex";
|
||||
std::string default_dump_filename = "./navigation.dat";
|
||||
DLOG(INFO) << "role " << role;
|
||||
DLOG(INFO) << "vector length " << vector_length_;
|
||||
vector_length_ = configuration->property(role + ".vector_length", 2048);
|
||||
dump_ = configuration->property(role + ".dump", false);
|
||||
dump_filename_ = configuration->property(role + ".dump_filename", default_dump_filename);
|
||||
int fs_in;
|
||||
fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
|
||||
// make telemetry decoder object
|
||||
telemetry_decoder_ = galileo_e5a_make_telemetry_decoder_cc(satellite_, 0, (long)fs_in, vector_length_, queue_, dump_); // TODO fix me
|
||||
DLOG(INFO) << "telemetry_decoder(" << telemetry_decoder_->unique_id() << ")";
|
||||
// set the navigation msg queue;
|
||||
telemetry_decoder_->set_ephemeris_queue(&global_galileo_ephemeris_queue);
|
||||
telemetry_decoder_->set_iono_queue(&global_galileo_iono_queue);
|
||||
telemetry_decoder_->set_almanac_queue(&global_galileo_almanac_queue);
|
||||
telemetry_decoder_->set_utc_model_queue(&global_galileo_utc_model_queue);
|
||||
|
||||
DLOG(INFO) << "global navigation message queue assigned to telemetry_decoder ("<< telemetry_decoder_->unique_id() << ")";
|
||||
}
|
||||
|
||||
|
||||
GalileoE5aTelemetryDecoder::~GalileoE5aTelemetryDecoder()
|
||||
{}
|
||||
|
||||
|
||||
void GalileoE5aTelemetryDecoder::set_satellite(Gnss_Satellite satellite)
|
||||
{
|
||||
satellite_ = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
telemetry_decoder_->set_satellite(satellite_);
|
||||
DLOG(INFO) << "TELEMETRY DECODER: satellite set to " << satellite_;
|
||||
}
|
||||
|
||||
|
||||
void GalileoE5aTelemetryDecoder::connect(gr::top_block_sptr top_block)
|
||||
{
|
||||
// Nothing to connect internally
|
||||
DLOG(INFO) << "nothing to connect internally";
|
||||
}
|
||||
|
||||
|
||||
void GalileoE5aTelemetryDecoder::disconnect(gr::top_block_sptr top_block)
|
||||
{
|
||||
// Nothing to disconnect
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr GalileoE5aTelemetryDecoder::get_left_block()
|
||||
{
|
||||
return telemetry_decoder_;
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr GalileoE5aTelemetryDecoder::get_right_block()
|
||||
{
|
||||
return telemetry_decoder_;
|
||||
}
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
/*!
|
||||
* \file galileo_e5a_telemetry_decoder.h
|
||||
* \brief Interface of an adapter of a GALILEO E5a FNAV data decoder block
|
||||
* to a TelemetryDecoderInterface
|
||||
* \author Marc Sales 2014 marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_H_
|
||||
#define GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_H_
|
||||
|
||||
#include <string>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
#include "telemetry_decoder_interface.h"
|
||||
#include "galileo_e5a_telemetry_decoder_cc.h"
|
||||
|
||||
|
||||
class ConfigurationInterface;
|
||||
|
||||
/*!
|
||||
* \brief This class implements a NAV data decoder for Galileo INAV frames in E1B radio link
|
||||
*/
|
||||
class GalileoE5aTelemetryDecoder: public TelemetryDecoderInterface
|
||||
{
|
||||
public:
|
||||
GalileoE5aTelemetryDecoder(ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams,
|
||||
boost::shared_ptr<gr::msg_queue> queue);
|
||||
|
||||
virtual ~GalileoE5aTelemetryDecoder();
|
||||
std::string role()
|
||||
{
|
||||
return role_;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Returns "Galileo_E5a_Telemetry_Decoder"
|
||||
*/
|
||||
std::string implementation()
|
||||
{
|
||||
return "Galileo_E5A_Telemetry_Decoder";
|
||||
}
|
||||
void connect(gr::top_block_sptr top_block);
|
||||
void disconnect(gr::top_block_sptr top_block);
|
||||
gr::basic_block_sptr get_left_block();
|
||||
gr::basic_block_sptr get_right_block();
|
||||
void set_satellite(Gnss_Satellite satellite);
|
||||
void set_channel(int channel){telemetry_decoder_->set_channel(channel);}
|
||||
void reset()
|
||||
{
|
||||
return;
|
||||
}
|
||||
size_t item_size()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
private:
|
||||
galileo_e5a_telemetry_decoder_cc_sptr telemetry_decoder_;
|
||||
Gnss_Satellite satellite_;
|
||||
int channel_;
|
||||
unsigned int vector_length_;
|
||||
std::string item_type_;
|
||||
bool dump_;
|
||||
std::string dump_filename_;
|
||||
std::string role_;
|
||||
unsigned int in_streams_;
|
||||
unsigned int out_streams_;
|
||||
boost::shared_ptr<gr::msg_queue> queue_;
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_H_ */
|
||||
|
|
@ -0,0 +1,540 @@
|
|||
/*!
|
||||
* \file galileo_e5a_telemetry_decoder_cc.cc
|
||||
* \brief Implementation of a Galileo FNAV message demodulator block
|
||||
* \author Marc Sales 2014. marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "galileo_e5a_telemetry_decoder_cc.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <glog/logging.h>
|
||||
#include "control_message_factory.h"
|
||||
#include "galileo_navigation_message.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "convolutional.h"
|
||||
|
||||
#define CRC_ERROR_LIMIT 6
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
|
||||
galileo_e5a_telemetry_decoder_cc_sptr
|
||||
galileo_e5a_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in, unsigned
|
||||
int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump)
|
||||
{
|
||||
return galileo_e5a_telemetry_decoder_cc_sptr(new galileo_e5a_telemetry_decoder_cc(satellite, if_freq,
|
||||
fs_in, vector_length, queue, dump));
|
||||
}
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
|
||||
{
|
||||
ninput_items_required[0] = GALILEO_FNAV_SYMBOLS_PER_PAGE; // set the required sample history
|
||||
}
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::viterbi_decoder(double *page_part_symbols, int *page_part_bits)
|
||||
{
|
||||
int CodeLength = 240;
|
||||
int DataLength;
|
||||
int nn, KK, mm, max_states;
|
||||
int g_encoder[2];
|
||||
|
||||
nn = 2; // Coding rate 1/n
|
||||
KK = 7; // Constraint Length
|
||||
g_encoder[0] = 121; // Polynomial G1
|
||||
g_encoder[1] = 91; // Polynomial G2
|
||||
|
||||
mm = KK - 1;
|
||||
max_states = 1 << mm; /* 2^mm */
|
||||
DataLength = (CodeLength/nn) - mm;
|
||||
|
||||
/* create appropriate transition matrices */
|
||||
int *out0, *out1, *state0, *state1;
|
||||
out0 = (int*)calloc( max_states, sizeof(int) );
|
||||
out1 = (int*)calloc( max_states, sizeof(int) );
|
||||
state0 = (int*)calloc( max_states, sizeof(int) );
|
||||
state1 = (int*)calloc( max_states, sizeof(int) );
|
||||
|
||||
nsc_transit( out0, state0, 0, g_encoder, KK, nn );
|
||||
nsc_transit( out1, state1, 1, g_encoder, KK, nn );
|
||||
|
||||
Viterbi(page_part_bits, out0, state0, out1, state1,
|
||||
page_part_symbols, KK, nn, DataLength );
|
||||
|
||||
/* Clean up memory */
|
||||
free( out0 );
|
||||
free( out1 );
|
||||
free( state0 );
|
||||
free( state1 );
|
||||
}
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::deinterleaver(int rows, int cols, double *in, double *out)
|
||||
{
|
||||
for (int r = 0; r < rows; r++)
|
||||
{
|
||||
for(int c = 0; c < cols; c++)
|
||||
{
|
||||
out[c*rows + r] = in[r*cols + c];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::decode_word(double *page_symbols,int frame_length)
|
||||
{
|
||||
double page_symbols_deint[frame_length];
|
||||
// 1. De-interleave
|
||||
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)
|
||||
// 2.2 Take into account the possible inversion of the polarity due to PLL lock at 180<38>
|
||||
for (int i = 0; i < frame_length; i++)
|
||||
{
|
||||
if ((i + 1) % 2 == 0)
|
||||
{
|
||||
page_symbols_deint[i] = -page_symbols_deint[i];
|
||||
}
|
||||
}
|
||||
int page_part_bits[frame_length];
|
||||
viterbi_decoder(page_symbols_deint, page_part_bits);
|
||||
|
||||
// 3. Call the Galileo page decoder
|
||||
std::string page_String;
|
||||
for(int i = 0; i < frame_length; i++)
|
||||
{
|
||||
if (page_part_bits[i] > 0)
|
||||
{
|
||||
page_String.push_back('1');
|
||||
}
|
||||
else
|
||||
{
|
||||
page_String.push_back('0');
|
||||
}
|
||||
}
|
||||
|
||||
// DECODE COMPLETE WORD (even + odd) and TEST CRC
|
||||
d_nav.split_page(page_String);
|
||||
if(d_nav.flag_CRC_test == true)
|
||||
{
|
||||
LOG(INFO) << "Galileo CRC correct on channel " << d_channel;
|
||||
std::cout << "Galileo CRC correct on channel " << d_channel << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "Galileo CRC error on channel " << d_channel << std::endl;
|
||||
LOG(INFO)<< "Galileo CRC error on channel " << d_channel;
|
||||
}
|
||||
|
||||
// 4. Push the new navigation data to the queues
|
||||
if (d_nav.have_new_ephemeris() == true)
|
||||
{
|
||||
// get ephemeris object for this SV
|
||||
Galileo_Ephemeris ephemeris = d_nav.get_ephemeris();//notice that the read operation will clear the valid flag
|
||||
//std::cout<<"New Galileo Ephemeris received for SV "<<d_satellite.get_PRN()<<std::endl;
|
||||
d_ephemeris_queue->push(ephemeris);
|
||||
}
|
||||
if (d_nav.have_new_iono_and_GST() == true)
|
||||
{
|
||||
Galileo_Iono iono = d_nav.get_iono(); //notice that the read operation will clear the valid flag
|
||||
//std::cout<<"New Galileo IONO model (and UTC) received for SV "<<d_satellite.get_PRN()<<std::endl;
|
||||
d_iono_queue->push(iono);
|
||||
}
|
||||
if (d_nav.have_new_utc_model() == true)
|
||||
{
|
||||
Galileo_Utc_Model utc_model = d_nav.get_utc_model(); //notice that the read operation will clear the valid flag
|
||||
//std::cout<<"New Galileo UTC model received for SV "<<d_satellite.get_PRN()<<std::endl;
|
||||
d_utc_model_queue->push(utc_model);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
galileo_e5a_telemetry_decoder_cc::galileo_e5a_telemetry_decoder_cc(
|
||||
Gnss_Satellite satellite,
|
||||
long if_freq,
|
||||
long fs_in,
|
||||
unsigned
|
||||
int vector_length,
|
||||
boost::shared_ptr<gr::msg_queue> queue,
|
||||
bool dump) :
|
||||
gr::block("galileo_e5a_telemetry_decoder_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
// initialize internal vars
|
||||
d_queue = queue;
|
||||
d_dump = dump;
|
||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
LOG(INFO) << "GALILEO E5A TELEMETRY PROCESSING: satellite " << d_satellite;
|
||||
d_vector_length = vector_length;
|
||||
//d_samples_per_symbol = ( Galileo_E5a_CODE_CHIP_RATE_HZ / Galileo_E5a_CODE_LENGTH_CHIPS ) / Galileo_E1_B_SYMBOL_RATE_BPS;
|
||||
d_fs_in = fs_in;
|
||||
|
||||
// set the preamble
|
||||
unsigned short int preambles_bits[GALILEO_INAV_PREAMBLE_LENGTH_BITS] = GALILEO_INAV_PREAMBLE;
|
||||
|
||||
//d_symbols_per_preamble = GALILEO_INAV_PREAMBLE_LENGTH_BITS * d_samples_per_symbol;
|
||||
|
||||
memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, GALILEO_INAV_PREAMBLE_LENGTH_BITS*sizeof(unsigned short int));
|
||||
|
||||
// preamble bits to sampled symbols
|
||||
d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * GALILEO_FNAV_SAMPLES_PER_PREAMBLE);
|
||||
int n = 0;
|
||||
for (int i = 0; i < GALILEO_INAV_PREAMBLE_LENGTH_BITS; i++)
|
||||
{
|
||||
for (unsigned int j = 0; j < GALILEO_FNAV_SAMPLES_PER_SYMBOL; j++)
|
||||
{
|
||||
if (d_preambles_bits[i] == 1)
|
||||
{
|
||||
d_preambles_symbols[n] = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_preambles_symbols[n] = -1;
|
||||
}
|
||||
n++;
|
||||
}
|
||||
}
|
||||
d_sample_counter = 0;
|
||||
//d_stat = 0;
|
||||
d_preamble_lock=false;
|
||||
d_preamble_index = 0;
|
||||
d_preamble_time_seconds = 0;
|
||||
d_flag_frame_sync = false;
|
||||
|
||||
d_TOW_at_Preamble = 0;
|
||||
d_TOW_at_current_symbol = 0;
|
||||
|
||||
d_CRC_error_counter = 0;
|
||||
}
|
||||
|
||||
galileo_e5a_telemetry_decoder_cc::~galileo_e5a_telemetry_decoder_cc()
|
||||
{
|
||||
delete d_preambles_symbols;
|
||||
d_dump_file.close();
|
||||
}
|
||||
|
||||
int galileo_e5a_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||
{
|
||||
int corr_value = 0;
|
||||
int preamble_diff = 0;
|
||||
int corr_sign=0;
|
||||
bool corr_flag=true;
|
||||
|
||||
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
|
||||
d_sample_counter++; //count for the processed samples
|
||||
|
||||
// ########### Output the tracking data to navigation and PVT ##########
|
||||
const Gnss_Synchro **in = (const Gnss_Synchro **) &input_items[0]; //Get the input samples pointer
|
||||
|
||||
d_flag_preamble = false;
|
||||
//******* frame sync ******************
|
||||
if (d_preamble_lock == false)
|
||||
{
|
||||
// d_preamble_lock tells if we have received a valid preamble and we are waiting
|
||||
// for the next one. Doesn't ensure frame sync yet.
|
||||
//******* preamble correlation ********
|
||||
// check if the preamble starts positive correlated or negative correlated
|
||||
if (in[0][0].Prompt_I < 0) // symbols clipping
|
||||
{
|
||||
corr_sign=d_preambles_symbols[0];
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_sign=-d_preambles_symbols[0];
|
||||
}
|
||||
// the preamble is fully correlated only if maintains corr_sign along the whole sequence
|
||||
for (int i = 1; i < GALILEO_FNAV_SAMPLES_PER_PREAMBLE; i++)
|
||||
{
|
||||
if (in[0][i].Prompt_I < 0 && d_preambles_symbols[i]+corr_sign != 0)
|
||||
{
|
||||
//exit for
|
||||
corr_flag=false;
|
||||
break;
|
||||
}
|
||||
if (in[0][i].Prompt_I > 0 && d_preambles_symbols[i]+corr_sign == 0)
|
||||
{
|
||||
//exit for
|
||||
corr_flag=false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (corr_flag==true)
|
||||
{
|
||||
d_preamble_index = d_sample_counter;//record the preamble sample stamp
|
||||
LOG(INFO) << "Preamble detection for Galileo SAT " << this->d_satellite << std::endl;
|
||||
d_preamble_lock=true;
|
||||
}
|
||||
}
|
||||
// else, preamble_lock == true , we are waiting for the next preamble at a specific time
|
||||
else if (d_sample_counter == d_preamble_index + GALILEO_FNAV_SAMPLES_PER_PAGE)
|
||||
{
|
||||
// only correlate preamble at the right time
|
||||
//******* preamble correlation ********
|
||||
// check if the preamble starts positive correlated or negative correlated
|
||||
if (in[0][0].Prompt_I < 0) // symbols clipping
|
||||
{
|
||||
corr_sign = d_preambles_symbols[0];
|
||||
}
|
||||
else
|
||||
{
|
||||
corr_sign = -d_preambles_symbols[0];
|
||||
}
|
||||
// the preamble is fully correlated only if maintains corr_sign along the whole sequence
|
||||
for (int i = 1; i < GALILEO_FNAV_SAMPLES_PER_PREAMBLE; i++)
|
||||
{
|
||||
if (in[0][i].Prompt_I < 0 && d_preambles_symbols[i] + corr_sign != 0)
|
||||
{
|
||||
//exit for
|
||||
corr_flag = false;
|
||||
break;
|
||||
}
|
||||
if (in[0][i].Prompt_I > 0
|
||||
&& d_preambles_symbols[i] + corr_sign == 0)
|
||||
{
|
||||
//exit for
|
||||
corr_flag = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (corr_flag == false) // lost preamble sync
|
||||
{
|
||||
d_preamble_lock = false;
|
||||
LOG(INFO)<< "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff;
|
||||
d_flag_frame_sync = false;
|
||||
flag_TOW_set = false;
|
||||
}
|
||||
else // NEW PAGE RECEIVED
|
||||
{
|
||||
// 1 - Obtain message symbols averaging samples (20 samples / symbol)
|
||||
int frame_length_symbols = GALILEO_FNAV_SYMBOLS_PER_PAGE
|
||||
- GALILEO_FNAV_PREAMBLE_LENGTH_BITS;
|
||||
//int page_symbols[frame_length_symbols];
|
||||
double samples_to_bit_accumulator[frame_length_symbols];
|
||||
//int samples_to_bit_accumulator = 0;
|
||||
for (int i = 0; i < frame_length_symbols; i++)
|
||||
{
|
||||
samples_to_bit_accumulator[i]=0.0;
|
||||
for (int k = 0; k < GALILEO_FNAV_SAMPLES_PER_SYMBOL; k++)
|
||||
{
|
||||
samples_to_bit_accumulator[i] += in[0][k + i*GALILEO_FNAV_SAMPLES_PER_SYMBOL + GALILEO_FNAV_SAMPLES_PER_PREAMBLE].Prompt_I;
|
||||
// Reminder: corr_sign is negative if phase lock is at 180º
|
||||
// if (in[0][k + i*GALILEO_FNAV_SAMPLES_PER_SYMBOL + GALILEO_FNAV_SAMPLES_PER_PREAMBLE].Prompt_I
|
||||
// > 0)
|
||||
// {
|
||||
// samples_to_bit_accumulator += corr_sign;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// samples_to_bit_accumulator -= corr_sign;
|
||||
// }
|
||||
|
||||
}
|
||||
samples_to_bit_accumulator[i] /= corr_sign*GALILEO_FNAV_SAMPLES_PER_SYMBOL;
|
||||
// if (samples_to_bit_accumulator > 0)
|
||||
// {
|
||||
// page_symbols[i] = 1; // because last symbol of the preamble is just received now!
|
||||
//
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// page_symbols[i] = -1; // because last symbol of the preamble is just received now!
|
||||
// }
|
||||
}
|
||||
// DECODE WORD
|
||||
decode_word(samples_to_bit_accumulator, frame_length_symbols);
|
||||
// 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())
|
||||
d_preamble_index = d_sample_counter; //record the preamble sample stamp (t_P)
|
||||
d_preamble_time_seconds = in[0][0].Tracking_timestamp_secs; // - d_preamble_duration_seconds; //record the PRN start sample index associated to the preamble
|
||||
if (!d_flag_frame_sync)
|
||||
{
|
||||
d_flag_frame_sync = true;
|
||||
LOG(INFO) <<" Frame sync SAT " << this->d_satellite << " with preamble start at " << d_preamble_time_seconds << " [s]";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
d_CRC_error_counter++;
|
||||
d_preamble_index = d_sample_counter; //record the preamble sample stamp
|
||||
if (d_CRC_error_counter > CRC_ERROR_LIMIT)
|
||||
{
|
||||
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite;
|
||||
d_flag_frame_sync = false;
|
||||
d_preamble_lock = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
consume_each(1); //one by one
|
||||
// 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][0];
|
||||
//2. Add the telemetry decoder information
|
||||
if (this->d_flag_preamble == true and d_nav.flag_TOW_set == true)
|
||||
//update TOW at the preamble instant
|
||||
//We expect a preamble each 10 seconds (FNAV page period)
|
||||
{
|
||||
Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
|
||||
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;
|
||||
d_nav.flag_TOW_1 = false;
|
||||
}
|
||||
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;
|
||||
d_nav.flag_TOW_2 = false;
|
||||
}
|
||||
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;
|
||||
d_nav.flag_TOW_3 = false;
|
||||
}
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
//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)
|
||||
{
|
||||
current_synchro_data.Flag_valid_word = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
current_synchro_data.Flag_valid_word = false;
|
||||
}
|
||||
|
||||
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.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;
|
||||
|
||||
if(d_dump == true)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
try
|
||||
{
|
||||
double tmp_double;
|
||||
tmp_double = d_TOW_at_current_symbol;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
tmp_double = current_synchro_data.Prn_timestamp_ms;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
tmp_double = d_TOW_at_Preamble;
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "Exception writing observables dump file " << e.what();
|
||||
}
|
||||
}
|
||||
//3. Make the output (copy the object contents to the GNURadio reserved memory)
|
||||
*out[0] = current_synchro_data;
|
||||
return 1;
|
||||
}
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::set_satellite(Gnss_Satellite satellite)
|
||||
{
|
||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
DLOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
|
||||
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
|
||||
}
|
||||
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::set_channel(int channel)
|
||||
{
|
||||
d_channel = channel;
|
||||
LOG(INFO) << "Navigation channel set to " << channel;
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_dump == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_filename = "telemetry";
|
||||
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
|
||||
d_dump_filename.append(".dat");
|
||||
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "Telemetry decoder dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (const std::ifstream::failure& e)
|
||||
{
|
||||
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::set_ephemeris_queue(concurrent_queue<Galileo_Ephemeris> *ephemeris_queue)
|
||||
{
|
||||
d_ephemeris_queue = ephemeris_queue;
|
||||
}
|
||||
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::set_iono_queue(concurrent_queue<Galileo_Iono> *iono_queue)
|
||||
{
|
||||
d_iono_queue = iono_queue;
|
||||
}
|
||||
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::set_almanac_queue(concurrent_queue<Galileo_Almanac> *almanac_queue)
|
||||
{
|
||||
d_almanac_queue = almanac_queue;
|
||||
}
|
||||
|
||||
|
||||
void galileo_e5a_telemetry_decoder_cc::set_utc_model_queue(concurrent_queue<Galileo_Utc_Model> *utc_model_queue)
|
||||
{
|
||||
d_utc_model_queue = utc_model_queue;
|
||||
}
|
||||
|
|
@ -0,0 +1,141 @@
|
|||
/*!
|
||||
* \file galileo_e5a_telemetry_decoder_cc.cc
|
||||
* \brief Implementation of a Galileo FNAV message demodulator block
|
||||
* \author Marc Sales 2014. marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_
|
||||
#define GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_
|
||||
|
||||
#include <fstream>
|
||||
#include <string>
|
||||
#include <gnuradio/block.h>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
#include <gnuradio/trellis/interleaver.h>
|
||||
#include <gnuradio/trellis/permutation.h>
|
||||
#include <gnuradio/fec/viterbi.h>
|
||||
#include "Galileo_E5a.h"
|
||||
#include "concurrent_queue.h"
|
||||
#include "gnss_satellite.h"
|
||||
#include "galileo_fnav_message.h"
|
||||
#include "galileo_ephemeris.h"
|
||||
#include "galileo_almanac.h"
|
||||
#include "galileo_iono.h"
|
||||
#include "galileo_utc_model.h"
|
||||
|
||||
class galileo_e5a_telemetry_decoder_cc;
|
||||
|
||||
typedef boost::shared_ptr<galileo_e5a_telemetry_decoder_cc> galileo_e5a_telemetry_decoder_cc_sptr;
|
||||
|
||||
galileo_e5a_telemetry_decoder_cc_sptr galileo_e5a_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in, unsigned
|
||||
int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump);
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This class implements a block that decodes the FNAV data defined in Galileo ICD
|
||||
*
|
||||
*/
|
||||
class galileo_e5a_telemetry_decoder_cc : public gr::block
|
||||
{
|
||||
public:
|
||||
~galileo_e5a_telemetry_decoder_cc();
|
||||
void set_satellite(Gnss_Satellite satellite); //!< Set satellite PRN
|
||||
void set_channel(int channel); //!< Set receiver's channel
|
||||
void set_ephemeris_queue(concurrent_queue<Galileo_Ephemeris> *ephemeris_queue); //!< Set the satellite data queue
|
||||
void set_iono_queue(concurrent_queue<Galileo_Iono> *iono_queue); //!< Set the iono data queue
|
||||
void set_almanac_queue(concurrent_queue<Galileo_Almanac> *almanac_queue); //!< Set the almanac data queue
|
||||
void set_utc_model_queue(concurrent_queue<Galileo_Utc_Model> *utc_model_queue); //!< Set the UTC model queue
|
||||
/*!
|
||||
* \brief This is where all signal processing takes place
|
||||
*/
|
||||
int general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
|
||||
|
||||
/*!
|
||||
* \brief Function which tells the scheduler how many input items
|
||||
* are required to produce noutput_items output items.
|
||||
*/
|
||||
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
|
||||
|
||||
private:
|
||||
friend galileo_e5a_telemetry_decoder_cc_sptr
|
||||
galileo_e5a_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in,unsigned
|
||||
int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump);
|
||||
galileo_e5a_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long fs_in, unsigned
|
||||
int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump);
|
||||
|
||||
void viterbi_decoder(double *page_part_symbols, int *page_part_bits);
|
||||
|
||||
void deinterleaver(int rows, int cols, double *in, double *out);
|
||||
|
||||
void decode_word(double *page_symbols,int frame_length);
|
||||
|
||||
unsigned short int d_preambles_bits[GALILEO_FNAV_PREAMBLE_LENGTH_BITS];
|
||||
|
||||
signed int *d_preambles_symbols;
|
||||
|
||||
long unsigned int d_sample_counter;
|
||||
long unsigned int d_preamble_index;
|
||||
|
||||
bool d_preamble_lock;
|
||||
bool d_flag_frame_sync;
|
||||
|
||||
bool d_flag_preamble;
|
||||
int d_CRC_error_counter;
|
||||
|
||||
long d_fs_in;
|
||||
|
||||
// navigation message vars
|
||||
Galileo_Fnav_Message d_nav;
|
||||
|
||||
// Galileo ephemeris queue
|
||||
concurrent_queue<Galileo_Ephemeris> *d_ephemeris_queue;
|
||||
// ionospheric parameters queue
|
||||
concurrent_queue<Galileo_Iono> *d_iono_queue;
|
||||
// UTC model parameters queue
|
||||
concurrent_queue<Galileo_Utc_Model> *d_utc_model_queue;
|
||||
// Almanac queue
|
||||
concurrent_queue<Galileo_Almanac> *d_almanac_queue;
|
||||
|
||||
boost::shared_ptr<gr::msg_queue> d_queue;
|
||||
unsigned int d_vector_length;
|
||||
bool d_dump;
|
||||
Gnss_Satellite d_satellite;
|
||||
int d_channel;
|
||||
|
||||
double d_preamble_time_seconds;
|
||||
|
||||
double d_TOW_at_Preamble;
|
||||
double d_TOW_at_current_symbol;
|
||||
double Prn_timestamp_at_preamble_ms;
|
||||
bool flag_TOW_set;
|
||||
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_GALILEO_E5A_TELEMETRY_DECODER_CC_H_ */
|
||||
|
|
@ -0,0 +1,149 @@
|
|||
/*!
|
||||
* \file galileo_e5a_dll_fll_pll_tracking_cc.h
|
||||
* \brief Adapts code DLL + carrier PLL aided with FLL
|
||||
* tracking block to TrackingInterface for Galileo E5a signals
|
||||
* \author Marc Sales, 2014. marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
#include "galileo_e5a_dll_fll_pll_tracking.h"
|
||||
#include <glog/logging.h>
|
||||
#include "Galileo_E5a.h"
|
||||
#include "configuration_interface.h"
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
GalileoE5aDllFllPllTracking::GalileoE5aDllFllPllTracking(
|
||||
ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams, unsigned int
|
||||
out_streams,
|
||||
boost::shared_ptr<gr::msg_queue> queue) :
|
||||
role_(role),
|
||||
in_streams_(in_streams),
|
||||
out_streams_(out_streams),
|
||||
queue_(queue)
|
||||
{
|
||||
DLOG(INFO) << "role " << role;
|
||||
//################# CONFIGURATION PARAMETERS ########################
|
||||
int fs_in;
|
||||
int vector_length;
|
||||
int f_if;
|
||||
bool dump;
|
||||
std::string dump_filename;
|
||||
std::string item_type;
|
||||
std::string default_item_type = "gr_complex";
|
||||
float pll_bw_hz;
|
||||
float fll_bw_hz;
|
||||
float dll_bw_hz;
|
||||
float early_late_space_chips;
|
||||
int order;
|
||||
item_type = configuration->property(role + ".item_type",default_item_type);
|
||||
//vector_length = configuration->property(role + ".vector_length", 2048);
|
||||
fs_in = configuration->property("GNSS-SDR.internal_fs_hz", 2048000);
|
||||
f_if = configuration->property(role + ".if", 0);
|
||||
dump = configuration->property(role + ".dump", false);
|
||||
order = configuration->property(role + ".order", 2);
|
||||
pll_bw_hz = configuration->property(role + ".pll_bw_hz", 50.0);
|
||||
fll_bw_hz = configuration->property(role + ".fll_bw_hz", 100.0);
|
||||
dll_bw_hz = configuration->property(role + ".dll_bw_hz", 2.0);
|
||||
early_late_space_chips = configuration->property(role + ".early_late_space_chips", 0.5);
|
||||
std::string default_dump_filename = "./track_ch";
|
||||
dump_filename = configuration->property(role + ".dump_filename",
|
||||
default_dump_filename); //unused!
|
||||
vector_length = round(fs_in / (Galileo_E5a_CODE_CHIP_RATE_HZ / Galileo_E5a_CODE_LENGTH_CHIPS));
|
||||
|
||||
//################# MAKE TRACKING GNURadio object ###################
|
||||
if (item_type.compare("gr_complex") == 0)
|
||||
{
|
||||
item_size_ = sizeof(gr_complex);
|
||||
tracking_ = galileo_e5a_dll_fll_pll_make_tracking_cc(
|
||||
f_if,
|
||||
fs_in,
|
||||
vector_length,
|
||||
queue_,
|
||||
dump,
|
||||
dump_filename,
|
||||
order,
|
||||
fll_bw_hz,
|
||||
pll_bw_hz,
|
||||
dll_bw_hz,
|
||||
early_late_space_chips);
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(WARNING) << item_type << " unknown tracking item type.";
|
||||
}
|
||||
|
||||
DLOG(INFO) << "tracking(" << tracking_->unique_id() << ")";
|
||||
}
|
||||
|
||||
|
||||
GalileoE5aDllFllPllTracking::~GalileoE5aDllFllPllTracking()
|
||||
{}
|
||||
|
||||
|
||||
void GalileoE5aDllFllPllTracking::start_tracking()
|
||||
{
|
||||
tracking_->start_tracking();
|
||||
}
|
||||
|
||||
void GalileoE5aDllFllPllTracking::set_channel(unsigned int channel)
|
||||
{
|
||||
channel_ = channel;
|
||||
tracking_->set_channel(channel);
|
||||
}
|
||||
|
||||
void GalileoE5aDllFllPllTracking::set_channel_queue(
|
||||
concurrent_queue<int> *channel_internal_queue)
|
||||
{
|
||||
channel_internal_queue_ = channel_internal_queue;
|
||||
tracking_->set_channel_queue(channel_internal_queue_);
|
||||
}
|
||||
|
||||
void GalileoE5aDllFllPllTracking::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
|
||||
{
|
||||
return tracking_->set_gnss_synchro(p_gnss_synchro);
|
||||
}
|
||||
|
||||
void GalileoE5aDllFllPllTracking::connect(gr::top_block_sptr top_block)
|
||||
{
|
||||
//nothing to connect, now the tracking uses gr_sync_decimator
|
||||
}
|
||||
|
||||
void GalileoE5aDllFllPllTracking::disconnect(gr::top_block_sptr top_block)
|
||||
{
|
||||
//nothing to disconnect, now the tracking uses gr_sync_decimator
|
||||
}
|
||||
|
||||
gr::basic_block_sptr GalileoE5aDllFllPllTracking::get_left_block()
|
||||
{
|
||||
return tracking_;
|
||||
}
|
||||
|
||||
gr::basic_block_sptr GalileoE5aDllFllPllTracking::get_right_block()
|
||||
{
|
||||
return tracking_;
|
||||
}
|
||||
|
|
@ -0,0 +1,87 @@
|
|||
/*!
|
||||
* \file galileo_e5a_dll_fll_pll_tracking_cc.h
|
||||
* \brief Adapts code DLL + carrier PLL aided with FLL
|
||||
* tracking block to TrackingInterface for Galileo E5a signals
|
||||
* \author Marc Sales, 2014. marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GALILEO_E5A_DLL_FLL_PLL_TRACKING_H_
|
||||
#define GNSS_SDR_GALILEO_E5A_DLL_FLL_PLL_TRACKING_H_
|
||||
|
||||
#include <string>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
#include "tracking_interface.h"
|
||||
#include "galileo_e5a_dll_fll_pll_tracking_cc.h"
|
||||
|
||||
class GalileoE5aDllFllPllTracking: public TrackingInterface
|
||||
{
|
||||
public:
|
||||
GalileoE5aDllFllPllTracking(ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams,
|
||||
boost::shared_ptr<gr::msg_queue> queue);
|
||||
|
||||
virtual ~GalileoE5aDllFllPllTracking();
|
||||
|
||||
std::string role()
|
||||
{
|
||||
return role_;
|
||||
}
|
||||
|
||||
//! Returns "Galileo_E5a_DLL_FLL_PLL_Tracking"
|
||||
std::string implementation()
|
||||
{
|
||||
return "Galileo_E5a_DLL_FLL_PLL_Tracking";
|
||||
}
|
||||
size_t item_size()
|
||||
{
|
||||
return item_size_;
|
||||
}
|
||||
|
||||
void connect(gr::top_block_sptr top_block);
|
||||
void disconnect(gr::top_block_sptr top_block);
|
||||
gr::basic_block_sptr get_left_block();
|
||||
gr::basic_block_sptr get_right_block();
|
||||
|
||||
void set_channel(unsigned int channel);
|
||||
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
|
||||
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
|
||||
void start_tracking();
|
||||
|
||||
private:
|
||||
galileo_e5a_dll_fll_pll_tracking_cc_sptr tracking_;
|
||||
size_t item_size_;
|
||||
unsigned int channel_;
|
||||
std::string role_;
|
||||
unsigned int in_streams_;
|
||||
unsigned int out_streams_;
|
||||
boost::shared_ptr<gr::msg_queue> queue_;
|
||||
concurrent_queue<int> *channel_internal_queue_;
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_GALILEO_E5A_DLL_FLL_PLL_TRACKING_H_ */
|
||||
|
|
@ -0,0 +1,667 @@
|
|||
/*!
|
||||
* \file galileo_e5a_dll_fll_pll_tracking_cc.h
|
||||
* \brief Implementation of a code DLL + carrier PLL aided with FLL
|
||||
* tracking block for Galileo E5a signals
|
||||
* \author Marc Sales, 2014. marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "galileo_e5a_dll_fll_pll_tracking_cc.h"
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <glog/logging.h>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include "gnss_synchro.h"
|
||||
#include "galileo_e5_signal_processing.h"
|
||||
#include "Galileo_E5a.h"
|
||||
#include "tracking_discriminators.h"
|
||||
#include "lock_detectors.h"
|
||||
#include "tracking_FLL_PLL_filter.h"
|
||||
#include "control_message_factory.h"
|
||||
#include "gnss_flowgraph.h"
|
||||
|
||||
/*!
|
||||
* \todo Include in definition header file
|
||||
*/
|
||||
#define CN0_ESTIMATION_SAMPLES 20
|
||||
#define MINIMUM_VALID_CN0 25
|
||||
#define MAXIMUM_LOCK_FAIL_COUNTER 50
|
||||
#define CARRIER_LOCK_THRESHOLD 0.85
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
galileo_e5a_dll_fll_pll_tracking_cc_sptr galileo_e5a_dll_fll_pll_make_tracking_cc(
|
||||
long if_freq,
|
||||
long fs_in,
|
||||
unsigned
|
||||
int vector_length,
|
||||
boost::shared_ptr<gr::msg_queue> queue,
|
||||
bool dump, std::string dump_filename,
|
||||
int order,
|
||||
float fll_bw_hz,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips)
|
||||
{
|
||||
|
||||
return galileo_e5a_dll_fll_pll_tracking_cc_sptr(new galileo_e5a_dll_fll_pll_tracking_cc(if_freq,
|
||||
fs_in, vector_length, queue, dump, dump_filename, order, fll_bw_hz, pll_bw_hz,dll_bw_hz,
|
||||
early_late_space_chips));
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void galileo_e5a_dll_fll_pll_tracking_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
|
||||
{
|
||||
ninput_items_required[0] = d_vector_length*2; //set the required available samples in each call
|
||||
}
|
||||
|
||||
|
||||
galileo_e5a_dll_fll_pll_tracking_cc::galileo_e5a_dll_fll_pll_tracking_cc(
|
||||
long if_freq,
|
||||
long fs_in,
|
||||
unsigned int vector_length,
|
||||
boost::shared_ptr<gr::msg_queue> queue,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
int order,
|
||||
float fll_bw_hz,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips) :
|
||||
gr::block("galileo_e5a_dll_fll_pll_tracking_cc", gr::io_signature::make(1, 1, sizeof(gr_complex)),
|
||||
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
|
||||
{
|
||||
// initialize internal vars
|
||||
d_queue = queue;
|
||||
d_dump = dump;
|
||||
|
||||
d_acquisition_gnss_synchro = NULL;
|
||||
|
||||
d_if_freq = (double)if_freq;
|
||||
d_fs_in = (double)fs_in;
|
||||
d_vector_length = vector_length;
|
||||
d_early_late_spc_chips = (double)early_late_space_chips; // Define early-late offset (in chips)
|
||||
d_dump_filename = dump_filename;
|
||||
|
||||
// Initialize tracking variables ==========================================
|
||||
d_carrier_loop_filter.set_params(fll_bw_hz, pll_bw_hz,order);
|
||||
|
||||
d_code_loop_filter = Tracking_2nd_DLL_filter(Galileo_E5a_CODE_LENGTH_CHIPS/Galileo_E5a_CODE_CHIP_RATE_HZ);
|
||||
d_code_loop_filter.set_DLL_BW(dll_bw_hz);
|
||||
|
||||
// Get space for a vector with the C/A code replica sampled 1x/chip
|
||||
d_ca_code = new gr_complex[(int)Galileo_E5a_CODE_LENGTH_CHIPS + 2];
|
||||
|
||||
/* If an array is partitioned for more than one thread to operate on,
|
||||
* having the sub-array boundaries unaligned to cache lines could lead
|
||||
* to performance degradation. Here we allocate memory
|
||||
* (gr_complex array of size 2*d_vector_length) aligned to cache of 16 bytes
|
||||
*/
|
||||
// todo: do something if posix_memalign fails
|
||||
// Get space for the resampled early / prompt / late local replicas
|
||||
if (posix_memalign((void**)&d_early_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
|
||||
if (posix_memalign((void**)&d_late_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
|
||||
if (posix_memalign((void**)&d_prompt_code, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
|
||||
// space for carrier wipeoff and signal baseband vectors
|
||||
if (posix_memalign((void**)&d_carr_sign, 16, d_vector_length * sizeof(gr_complex) * 2) == 0){};
|
||||
if (posix_memalign((void**)&d_Early, 16, sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_Prompt, 16, sizeof(gr_complex)) == 0){};
|
||||
if (posix_memalign((void**)&d_Late, 16, sizeof(gr_complex)) == 0){};
|
||||
|
||||
// sample synchronization
|
||||
d_sample_counter = 0;
|
||||
d_acq_sample_stamp = 0;
|
||||
d_last_seg = 0;// this is for debug output only
|
||||
d_code_phase_samples = 0;
|
||||
d_enable_tracking = false;
|
||||
d_current_prn_length_samples = (int)d_vector_length;
|
||||
|
||||
// CN0 estimation and lock detector buffers
|
||||
d_cn0_estimation_counter = 0;
|
||||
d_Prompt_buffer = new gr_complex[CN0_ESTIMATION_SAMPLES];
|
||||
d_carrier_lock_test = 1;
|
||||
d_CN0_SNV_dB_Hz = 0;
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_carrier_lock_threshold = CARRIER_LOCK_THRESHOLD;
|
||||
|
||||
systemName["G"] = std::string("GPS");
|
||||
systemName["R"] = std::string("GLONASS");
|
||||
systemName["S"] = std::string("SBAS");
|
||||
systemName["E"] = std::string("Galileo");
|
||||
systemName["C"] = std::string("Compass");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void galileo_e5a_dll_fll_pll_tracking_cc::start_tracking()
|
||||
{
|
||||
/*
|
||||
* correct the code phase according to the delay between acq and trk
|
||||
*/
|
||||
d_acq_code_phase_samples = d_acquisition_gnss_synchro->Acq_delay_samples;
|
||||
d_acq_carrier_doppler_hz = d_acquisition_gnss_synchro->Acq_doppler_hz;
|
||||
d_acq_sample_stamp = d_acquisition_gnss_synchro->Acq_samplestamp_samples;
|
||||
|
||||
long int acq_trk_diff_samples;
|
||||
float acq_trk_diff_seconds;
|
||||
acq_trk_diff_samples = (long int)d_sample_counter - (long int)d_acq_sample_stamp;
|
||||
acq_trk_diff_seconds = (double)acq_trk_diff_samples / d_fs_in;
|
||||
//doppler effect
|
||||
// Fd=(C/(C+Vr))*F
|
||||
double radial_velocity;
|
||||
radial_velocity = (Galileo_E5a_FREQ_HZ + d_acq_carrier_doppler_hz) / Galileo_E5a_FREQ_HZ;
|
||||
// new chip and prn sequence periods based on acq Doppler
|
||||
double T_chip_mod_seconds;
|
||||
double T_prn_mod_seconds;
|
||||
double T_prn_mod_samples;
|
||||
d_code_freq_hz = radial_velocity * Galileo_E5a_CODE_CHIP_RATE_HZ;
|
||||
T_chip_mod_seconds = 1 / d_code_freq_hz;
|
||||
T_prn_mod_seconds = T_chip_mod_seconds * Galileo_E5a_CODE_LENGTH_CHIPS;
|
||||
T_prn_mod_samples = T_prn_mod_seconds * d_fs_in;
|
||||
d_current_prn_length_samples = round(T_prn_mod_samples);
|
||||
|
||||
double T_prn_true_seconds = Galileo_E5a_CODE_LENGTH_CHIPS / Galileo_E5a_CODE_CHIP_RATE_HZ;
|
||||
double T_prn_true_samples = T_prn_true_seconds * d_fs_in;
|
||||
double T_prn_diff_seconds;
|
||||
T_prn_diff_seconds = T_prn_true_seconds - T_prn_mod_seconds;
|
||||
double N_prn_diff;
|
||||
N_prn_diff = acq_trk_diff_seconds / T_prn_true_seconds;
|
||||
double corrected_acq_phase_samples, delay_correction_samples;
|
||||
corrected_acq_phase_samples = fmod((d_acq_code_phase_samples + T_prn_diff_seconds * N_prn_diff * d_fs_in), T_prn_true_samples);
|
||||
|
||||
if (corrected_acq_phase_samples < 0)
|
||||
{
|
||||
corrected_acq_phase_samples = T_prn_mod_samples + corrected_acq_phase_samples;
|
||||
}
|
||||
delay_correction_samples = d_acq_code_phase_samples - corrected_acq_phase_samples;
|
||||
d_acq_code_phase_samples = corrected_acq_phase_samples;
|
||||
|
||||
d_carrier_doppler_hz = d_acq_carrier_doppler_hz;
|
||||
// DLL/PLL filter initialization
|
||||
d_carrier_loop_filter.initialize(d_acq_carrier_doppler_hz);
|
||||
d_FLL_wait = 1;
|
||||
|
||||
// generate local reference ALWAYS starting at chip 1 (1 sample per chip)
|
||||
galileo_e5_a_code_gen_complex(&d_ca_code[1], d_acquisition_gnss_synchro->PRN, d_acquisition_gnss_synchro->Signal);
|
||||
|
||||
d_ca_code[0] = d_ca_code[(int)Galileo_E5a_CODE_LENGTH_CHIPS];
|
||||
d_ca_code[(int)Galileo_E5a_CODE_LENGTH_CHIPS + 1] = d_ca_code[1];
|
||||
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_Prompt_prev = 0;
|
||||
d_rem_code_phase_samples = 0;
|
||||
d_rem_carr_phase = 0;
|
||||
d_FLL_discriminator_hz = 0;
|
||||
d_rem_code_phase_samples = 0;
|
||||
d_acc_carrier_phase_rad = 0;
|
||||
|
||||
std::string sys_ = &d_acquisition_gnss_synchro->System;
|
||||
sys = sys_.substr(0,1);
|
||||
|
||||
// DEBUG OUTPUT
|
||||
std::cout << "Tracking start on channel " << d_channel << " for satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << std::endl;
|
||||
LOG(INFO) << "Starting tracking of satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << " on channel " << d_channel;
|
||||
|
||||
// enable tracking Gnss_Satellite(systemName[&d_acquisition_gnss_synchro->System], d_acquisition_gnss_synchro->PRN)
|
||||
d_pull_in = true;
|
||||
d_enable_tracking = true;
|
||||
|
||||
LOG(INFO) << "PULL-IN Doppler [Hz]= " << d_carrier_doppler_hz
|
||||
<< " Code Phase correction [samples]=" << delay_correction_samples
|
||||
<< " PULL-IN Code Phase [samples]= " << d_acq_code_phase_samples << std::endl;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void galileo_e5a_dll_fll_pll_tracking_cc::update_local_code()
|
||||
{
|
||||
double tcode_chips;
|
||||
double rem_code_phase_chips;
|
||||
double code_phase_step_chips;
|
||||
int early_late_spc_samples;
|
||||
int epl_loop_length_samples;
|
||||
|
||||
int associated_chip_index;
|
||||
int code_length_chips = (int)Galileo_E5a_CODE_LENGTH_CHIPS;
|
||||
code_phase_step_chips = d_code_freq_hz / d_fs_in;
|
||||
rem_code_phase_chips = d_rem_code_phase_samples * (d_code_freq_hz / d_fs_in);
|
||||
// unified loop for E, P, L code vectors
|
||||
tcode_chips = -rem_code_phase_chips;
|
||||
// Alternative EPL code generation (40% of speed improvement!)
|
||||
early_late_spc_samples = round(d_early_late_spc_chips/code_phase_step_chips);
|
||||
epl_loop_length_samples = d_current_prn_length_samples + early_late_spc_samples*2;
|
||||
for (int i = 0; i < epl_loop_length_samples; i++)
|
||||
{
|
||||
associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
|
||||
d_early_code[i] = d_ca_code[associated_chip_index];
|
||||
tcode_chips = tcode_chips + code_phase_step_chips;
|
||||
}
|
||||
|
||||
memcpy(d_prompt_code,&d_early_code[early_late_spc_samples],d_current_prn_length_samples* sizeof(gr_complex));
|
||||
memcpy(d_late_code,&d_early_code[early_late_spc_samples*2],d_current_prn_length_samples* sizeof(gr_complex));
|
||||
|
||||
// for (int i=0; i<d_current_prn_length_samples; i++)
|
||||
// {
|
||||
// associated_chip_index = 1 + round(fmod(tcode_chips - d_early_late_spc_chips, code_length_chips));
|
||||
// d_early_code[i] = d_ca_code[associated_chip_index];
|
||||
// associated_chip_index = 1 + round(fmod(tcode_chips, code_length_chips));
|
||||
// d_prompt_code[i] = d_ca_code[associated_chip_index];
|
||||
// associated_chip_index = 1 + round(fmod(tcode_chips + d_early_late_spc_chips, code_length_chips));
|
||||
// d_late_code[i] = d_ca_code[associated_chip_index];
|
||||
// tcode_chips = tcode_chips + code_phase_step_chips;
|
||||
// }
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void galileo_e5a_dll_fll_pll_tracking_cc::update_local_carrier()
|
||||
{
|
||||
double phase, phase_step;
|
||||
phase_step = 2*GALILEO_PI * d_carrier_doppler_hz / d_fs_in;
|
||||
phase = d_rem_carr_phase;
|
||||
for(int i = 0; i < d_current_prn_length_samples; i++)
|
||||
{
|
||||
d_carr_sign[i] = gr_complex(cos(phase), -sin(phase));
|
||||
phase += phase_step;
|
||||
}
|
||||
d_rem_carr_phase = fmod(phase, 2*GALILEO_PI);
|
||||
d_acc_carrier_phase_rad = d_acc_carrier_phase_rad + phase;
|
||||
}
|
||||
|
||||
|
||||
|
||||
galileo_e5a_dll_fll_pll_tracking_cc::~galileo_e5a_dll_fll_pll_tracking_cc()
|
||||
{
|
||||
d_dump_file.close();
|
||||
delete[] d_ca_code;
|
||||
|
||||
free(d_prompt_code);
|
||||
free(d_late_code);
|
||||
free(d_early_code);
|
||||
free(d_carr_sign);
|
||||
free(d_Early);
|
||||
free(d_Prompt);
|
||||
free(d_Late);
|
||||
delete[] d_Prompt_buffer;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int galileo_e5a_dll_fll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items)
|
||||
{
|
||||
double code_error_chips = 0;
|
||||
double code_error_filt_chips = 0;
|
||||
double correlation_time_s = 0;
|
||||
double PLL_discriminator_hz = 0;
|
||||
double carr_nco_hz = 0;
|
||||
// get the sample in and out pointers
|
||||
const gr_complex* in = (gr_complex*) input_items[0]; // block input samples pointer
|
||||
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; // block output streams pointer
|
||||
|
||||
d_Prompt_prev = *d_Prompt; // for the FLL discriminator
|
||||
|
||||
if (d_enable_tracking == true)
|
||||
{
|
||||
// GNSS_SYNCHRO OBJECT to interchange data between tracking->telemetry_decoder
|
||||
Gnss_Synchro current_synchro_data;
|
||||
// Fill the acquisition data
|
||||
current_synchro_data = *d_acquisition_gnss_synchro;
|
||||
/*
|
||||
* Receiver signal alignment
|
||||
*/
|
||||
if (d_pull_in == true)
|
||||
{
|
||||
int samples_offset;
|
||||
double acq_trk_shif_correction_samples;
|
||||
int acq_to_trk_delay_samples;
|
||||
acq_to_trk_delay_samples = d_sample_counter-d_acq_sample_stamp;
|
||||
acq_trk_shif_correction_samples = d_current_prn_length_samples - fmod((double)acq_to_trk_delay_samples, (double)d_current_prn_length_samples);
|
||||
samples_offset = round(d_acq_code_phase_samples + acq_trk_shif_correction_samples);
|
||||
// /todo: Check if the sample counter sent to the next block as a time reference should be incremented AFTER sended or BEFORE
|
||||
d_sample_counter = d_sample_counter + samples_offset; //count for the processed samples
|
||||
d_pull_in = false;
|
||||
consume_each(samples_offset); //shift input to perform alignment with local replica
|
||||
|
||||
// make an output to not stop the rest of the processing blocks
|
||||
current_synchro_data.Prompt_I = 0.0;
|
||||
current_synchro_data.Prompt_Q = 0.0;
|
||||
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in;
|
||||
current_synchro_data.Carrier_phase_rads = 0.0;
|
||||
current_synchro_data.Code_phase_secs = 0.0;
|
||||
current_synchro_data.CN0_dB_hz = 0.0;
|
||||
current_synchro_data.Flag_valid_tracking = false;
|
||||
|
||||
*out[0] = current_synchro_data;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
update_local_code();
|
||||
update_local_carrier();
|
||||
|
||||
// perform Early, Prompt and Late correlation
|
||||
d_correlator.Carrier_wipeoff_and_EPL_volk(d_current_prn_length_samples,
|
||||
in,
|
||||
d_carr_sign,
|
||||
d_early_code,
|
||||
d_prompt_code,
|
||||
d_late_code,
|
||||
d_Early,
|
||||
d_Prompt,
|
||||
d_Late,
|
||||
is_unaligned());
|
||||
// check for samples consistency (this should be done before in the receiver / here only if the source is a file)
|
||||
if (isnan((*d_Prompt).real()) == true or isnan((*d_Prompt).imag()) == true )// or std::isinf(in[i].real())==true or std::isinf(in[i].imag())==true)
|
||||
{
|
||||
const int samples_available = ninput_items[0];
|
||||
d_sample_counter = d_sample_counter + samples_available;
|
||||
LOG(WARNING) << "Detected NaN samples at sample number " << d_sample_counter;
|
||||
consume_each(samples_available);
|
||||
|
||||
// make an output to not stop the rest of the processing blocks
|
||||
current_synchro_data.Prompt_I = 0.0;
|
||||
current_synchro_data.Prompt_Q = 0.0;
|
||||
current_synchro_data.Tracking_timestamp_secs = (double)d_sample_counter/d_fs_in;
|
||||
current_synchro_data.Carrier_phase_rads = 0.0;
|
||||
current_synchro_data.Code_phase_secs = 0.0;
|
||||
current_synchro_data.CN0_dB_hz = 0.0;
|
||||
current_synchro_data.Flag_valid_tracking = false;
|
||||
|
||||
*out[0] =current_synchro_data;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
/*
|
||||
* DLL, FLL, and PLL discriminators
|
||||
*/
|
||||
// Compute DLL error
|
||||
code_error_chips = dll_nc_e_minus_l_normalized(*d_Early, *d_Late);
|
||||
// Compute DLL filtered error
|
||||
code_error_filt_chips = d_code_loop_filter.get_code_nco(code_error_chips);
|
||||
|
||||
//compute FLL error
|
||||
correlation_time_s = ((double)d_current_prn_length_samples) / d_fs_in;
|
||||
if (d_FLL_wait == 1)
|
||||
{
|
||||
d_Prompt_prev = *d_Prompt;
|
||||
d_FLL_wait = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_FLL_discriminator_hz = fll_four_quadrant_atan(d_Prompt_prev, *d_Prompt, 0, correlation_time_s) / GPS_TWO_PI;
|
||||
d_Prompt_prev = *d_Prompt;
|
||||
d_FLL_wait = 1;
|
||||
}
|
||||
|
||||
// Compute PLL error
|
||||
PLL_discriminator_hz = pll_cloop_two_quadrant_atan(*d_Prompt) / GPS_TWO_PI;
|
||||
|
||||
/*
|
||||
* DLL and FLL+PLL filter and get current carrier Doppler and code frequency
|
||||
*/
|
||||
carr_nco_hz = d_carrier_loop_filter.get_carrier_error(d_FLL_discriminator_hz, PLL_discriminator_hz, correlation_time_s);
|
||||
d_carrier_doppler_hz = d_if_freq + carr_nco_hz;
|
||||
|
||||
d_code_freq_hz = Galileo_E5a_CODE_CHIP_RATE_HZ + (((d_carrier_doppler_hz + d_if_freq) * Galileo_E5a_CODE_CHIP_RATE_HZ) / Galileo_E5a_FREQ_HZ);
|
||||
|
||||
/*!
|
||||
* \todo Improve the lock detection algorithm!
|
||||
*/
|
||||
// ####### CN0 ESTIMATION AND LOCK DETECTORS ######
|
||||
if (d_cn0_estimation_counter < CN0_ESTIMATION_SAMPLES)
|
||||
{
|
||||
// fill buffer with prompt correlator output values
|
||||
d_Prompt_buffer[d_cn0_estimation_counter] = *d_Prompt;
|
||||
d_cn0_estimation_counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
d_cn0_estimation_counter = 0;
|
||||
//d_CN0_SNV_dB_Hz = gps_l1_ca_CN0_SNV(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in);
|
||||
d_CN0_SNV_dB_Hz = cn0_svn_estimator(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES, d_fs_in, Galileo_E5a_CODE_LENGTH_CHIPS);
|
||||
|
||||
d_carrier_lock_test = carrier_lock_detector(d_Prompt_buffer, CN0_ESTIMATION_SAMPLES);
|
||||
// ###### TRACKING UNLOCK NOTIFICATION #####
|
||||
if (d_carrier_lock_test < d_carrier_lock_threshold or d_CN0_SNV_dB_Hz < MINIMUM_VALID_CN0)
|
||||
{
|
||||
d_carrier_lock_fail_counter++;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (d_carrier_lock_fail_counter > 0) d_carrier_lock_fail_counter--;
|
||||
}
|
||||
if (d_carrier_lock_fail_counter > MAXIMUM_LOCK_FAIL_COUNTER)
|
||||
{
|
||||
std::cout << "Loss of lock in channel " << d_channel << "!" << std::endl;
|
||||
LOG(INFO) << "Loss of lock in channel " << d_channel << "!";
|
||||
ControlMessageFactory* cmf = new ControlMessageFactory();
|
||||
if (d_queue != gr::msg_queue::sptr())
|
||||
{
|
||||
d_queue->handle(cmf->GetQueueMessage(d_channel, 2));
|
||||
}
|
||||
delete cmf;
|
||||
d_carrier_lock_fail_counter = 0;
|
||||
d_enable_tracking = false; // TODO: check if disabling tracking is consistent with the channel state machine
|
||||
}
|
||||
}
|
||||
|
||||
// ########## DEBUG OUTPUT
|
||||
/*!
|
||||
* \todo The stop timer has to be moved to the signal source!
|
||||
*/
|
||||
// debug: Second counter in channel 0
|
||||
if (d_channel == 0)
|
||||
{
|
||||
if (floor(d_sample_counter/d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter/d_fs_in);
|
||||
std::cout << "Current input signal time = " << d_last_seg << " [s]" << std::endl;
|
||||
LOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]";
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (floor(d_sample_counter/d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter/d_fs_in);
|
||||
LOG(INFO) << "Tracking CH " << d_channel << ": Satellite " << Gnss_Satellite(systemName[sys], d_acquisition_gnss_synchro->PRN) << ", CN0 = " << d_CN0_SNV_dB_Hz << " [dB-Hz]";
|
||||
}
|
||||
}
|
||||
|
||||
//predict the next loop PRN period length prediction
|
||||
double T_chip_seconds;
|
||||
double T_prn_seconds;
|
||||
double T_prn_samples;
|
||||
double K_blk_samples;
|
||||
T_chip_seconds = 1/d_code_freq_hz;
|
||||
T_prn_seconds = T_chip_seconds * Galileo_E5a_CODE_LENGTH_CHIPS;
|
||||
T_prn_samples = T_prn_seconds * d_fs_in;
|
||||
|
||||
float code_error_filt_samples;
|
||||
code_error_filt_samples = T_prn_seconds*code_error_filt_chips*T_chip_seconds*(float)d_fs_in; //[seconds]
|
||||
d_acc_code_phase_samples = d_acc_code_phase_samples + code_error_filt_samples;
|
||||
|
||||
K_blk_samples = T_prn_samples + d_rem_code_phase_samples + code_error_filt_samples;
|
||||
d_current_prn_length_samples = round(K_blk_samples); //round to a discrete sample
|
||||
d_rem_code_phase_samples = K_blk_samples - d_current_prn_length_samples; //rounding error
|
||||
|
||||
// ########### Output the tracking data to navigation and PVT ##########
|
||||
current_synchro_data.Prompt_I = (double)(*d_Prompt).real();
|
||||
current_synchro_data.Prompt_Q = (double)(*d_Prompt).imag();
|
||||
// Tracking_timestamp_secs is aligned with the PRN start sample
|
||||
current_synchro_data.Tracking_timestamp_secs = ((double)d_sample_counter + (double)d_current_prn_length_samples + d_rem_code_phase_samples)/d_fs_in;
|
||||
// This tracking block aligns the Tracking_timestamp_secs with the start sample of the PRN, Code_phase_secs=0
|
||||
current_synchro_data.Code_phase_secs = 0;
|
||||
current_synchro_data.Carrier_phase_rads = d_acc_carrier_phase_rad;
|
||||
current_synchro_data.Carrier_Doppler_hz = d_carrier_doppler_hz;
|
||||
current_synchro_data.CN0_dB_hz = d_CN0_SNV_dB_Hz;
|
||||
current_synchro_data.Flag_valid_tracking = true;
|
||||
*out[0] = current_synchro_data;
|
||||
}
|
||||
else
|
||||
{
|
||||
// ########## DEBUG OUTPUT (TIME ONLY for channel 0 when tracking is disabled)
|
||||
/*!
|
||||
* \todo The stop timer has to be moved to the signal source!
|
||||
*/
|
||||
// stream to collect cout calls to improve thread safety
|
||||
std::stringstream tmp_str_stream;
|
||||
if (floor(d_sample_counter / d_fs_in) != d_last_seg)
|
||||
{
|
||||
d_last_seg = floor(d_sample_counter / d_fs_in);
|
||||
|
||||
if (d_channel == 0)
|
||||
{
|
||||
// debug: Second counter in channel 0
|
||||
tmp_str_stream << "Current input signal time = " << d_last_seg << " [s]" << std::endl << std::flush;
|
||||
std::cout << tmp_str_stream.rdbuf() << std::flush;
|
||||
}
|
||||
}
|
||||
*d_Early = gr_complex(0,0);
|
||||
*d_Prompt = gr_complex(0,0);
|
||||
*d_Late = gr_complex(0,0);
|
||||
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0]; //block output streams pointer
|
||||
*out[0] = *d_acquisition_gnss_synchro;
|
||||
}
|
||||
|
||||
|
||||
if(d_dump)
|
||||
{
|
||||
// MULTIPLEXED FILE RECORDING - Record results to file
|
||||
float prompt_I;
|
||||
float prompt_Q;
|
||||
float tmp_E, tmp_P, tmp_L;
|
||||
float tmp_float;
|
||||
double tmp_double;
|
||||
prompt_I = (*d_Prompt).real();
|
||||
prompt_Q = (*d_Prompt).imag();
|
||||
tmp_E = std::abs<float>(*d_Early);
|
||||
tmp_P = std::abs<float>(*d_Prompt);
|
||||
tmp_L = std::abs<float>(*d_Late);
|
||||
try
|
||||
{
|
||||
// EPR
|
||||
d_dump_file.write((char*)&tmp_E, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_P, sizeof(float));
|
||||
d_dump_file.write((char*)&tmp_L, sizeof(float));
|
||||
// PROMPT I and Q (to analyze navigation symbols)
|
||||
d_dump_file.write((char*)&prompt_I, sizeof(float));
|
||||
d_dump_file.write((char*)&prompt_Q, sizeof(float));
|
||||
// PRN start sample stamp
|
||||
d_dump_file.write((char*)&d_sample_counter, sizeof(unsigned long int));
|
||||
// accumulated carrier phase
|
||||
tmp_float = (float)d_acc_carrier_phase_rad;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
|
||||
// carrier and code frequency
|
||||
tmp_float = (float)d_carrier_doppler_hz;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
tmp_float = (float)d_code_freq_hz;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
|
||||
//PLL commands
|
||||
tmp_float = (float)PLL_discriminator_hz;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
tmp_float = (float)carr_nco_hz;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
|
||||
//DLL commands
|
||||
tmp_float = (float)code_error_chips;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
tmp_float = (float)code_error_filt_chips;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
|
||||
// CN0 and carrier lock test
|
||||
tmp_float = (float)d_CN0_SNV_dB_Hz;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
tmp_float = (float)d_carrier_lock_test;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
|
||||
// AUX vars (for debug purposes)
|
||||
tmp_float = (float)d_rem_code_phase_samples;
|
||||
d_dump_file.write((char*)&tmp_float, sizeof(float));
|
||||
tmp_double = (double)(d_sample_counter + d_current_prn_length_samples);
|
||||
d_dump_file.write((char*)&tmp_double, sizeof(double));
|
||||
}
|
||||
catch (std::ifstream::failure e)
|
||||
{
|
||||
LOG(INFO) << "Exception writing trk dump file "<< e.what() << std::endl;
|
||||
}
|
||||
}
|
||||
consume_each(d_current_prn_length_samples); // this is necessary in gr::block derivates
|
||||
d_sample_counter += d_current_prn_length_samples; //count for the processed samples
|
||||
return 1; //output tracking result ALWAYS even in the case of d_enable_tracking==false
|
||||
}
|
||||
|
||||
|
||||
void galileo_e5a_dll_fll_pll_tracking_cc::set_channel(unsigned int channel)
|
||||
{
|
||||
d_channel = channel;
|
||||
LOG(INFO) << "Tracking Channel set to " << d_channel;
|
||||
// ############# ENABLE DATA FILE LOG #################
|
||||
if (d_dump == true)
|
||||
{
|
||||
if (d_dump_file.is_open() == false)
|
||||
{
|
||||
try
|
||||
{
|
||||
d_dump_filename.append(boost::lexical_cast<std::string>(d_channel));
|
||||
d_dump_filename.append(".dat");
|
||||
d_dump_file.exceptions ( std::ifstream::failbit | std::ifstream::badbit );
|
||||
d_dump_file.open(d_dump_filename.c_str(), std::ios::out | std::ios::binary);
|
||||
LOG(INFO) << "Tracking dump enabled on channel " << d_channel << " Log file: " << d_dump_filename.c_str();
|
||||
}
|
||||
catch (std::ifstream::failure e)
|
||||
{
|
||||
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void galileo_e5a_dll_fll_pll_tracking_cc::set_channel_queue(concurrent_queue<int> *channel_internal_queue)
|
||||
{
|
||||
d_channel_internal_queue = channel_internal_queue;
|
||||
}
|
||||
|
||||
void galileo_e5a_dll_fll_pll_tracking_cc::set_gnss_synchro(Gnss_Synchro* p_gnss_synchro)
|
||||
{
|
||||
d_acquisition_gnss_synchro=p_gnss_synchro;
|
||||
}
|
||||
|
|
@ -0,0 +1,205 @@
|
|||
/*!
|
||||
* \file galileo_e5a_dll_fll_pll_tracking_cc.h
|
||||
* \brief Implementation of a code DLL + carrier PLL aided with FLL
|
||||
* tracking block for Galileo E5a signals
|
||||
* \author Marc Sales, 2014. marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GALILEO_E5A_DLL_FLL_PLL_TRACKING_CC_H_
|
||||
#define GNSS_SDR_GALILEO_E5A_DLL_FLL_PLL_TRACKING_CC_H_
|
||||
|
||||
#include <fstream>
|
||||
#include <queue>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <boost/thread/thread.hpp>
|
||||
#include <gnuradio/block.h>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
#include "concurrent_queue.h"
|
||||
#include "galileo_e5_signal_processing.h"
|
||||
#include "tracking_FLL_PLL_filter.h"
|
||||
#include "tracking_2nd_DLL_filter.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "correlator.h"
|
||||
|
||||
class galileo_e5a_dll_fll_pll_tracking_cc;
|
||||
|
||||
typedef boost::shared_ptr<galileo_e5a_dll_fll_pll_tracking_cc>
|
||||
galileo_e5a_dll_fll_pll_tracking_cc_sptr;
|
||||
|
||||
galileo_e5a_dll_fll_pll_tracking_cc_sptr
|
||||
galileo_e5a_dll_fll_pll_make_tracking_cc(
|
||||
long if_freq,
|
||||
long fs_in,
|
||||
unsigned int vector_length,
|
||||
boost::shared_ptr<gr::msg_queue> queue,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
int order,
|
||||
float fll_bw_hz,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
|
||||
/*!
|
||||
* \brief This class implements a DLL and a FLL assisted PLL tracking loop block
|
||||
*/
|
||||
class galileo_e5a_dll_fll_pll_tracking_cc: public gr::block
|
||||
{
|
||||
public:
|
||||
~galileo_e5a_dll_fll_pll_tracking_cc();
|
||||
|
||||
void set_channel(unsigned int channel);
|
||||
void start_tracking();
|
||||
void update_local_code();
|
||||
void update_local_carrier();
|
||||
void set_FLL_and_PLL_BW(float fll_bw_hz,float pll_bw_hz);
|
||||
/*
|
||||
* \brief Satellite signal synchronization parameters uses shared memory between acquisition and tracking
|
||||
*/
|
||||
void set_gnss_synchro(Gnss_Synchro* p_gnss_synchro);
|
||||
void set_channel_queue(concurrent_queue<int> *channel_internal_queue);
|
||||
|
||||
/*
|
||||
* \brief just like gr_block::general_work, only this arranges to call consume_each for you
|
||||
*
|
||||
* The user must override work to define the signal processing code
|
||||
*/
|
||||
|
||||
int general_work (int noutput_items, gr_vector_int &ninput_items,
|
||||
gr_vector_const_void_star &input_items, gr_vector_void_star &output_items);
|
||||
|
||||
void forecast (int noutput_items, gr_vector_int &ninput_items_required);
|
||||
private:
|
||||
friend galileo_e5a_dll_fll_pll_tracking_cc_sptr
|
||||
galileo_e5a_dll_fll_pll_make_tracking_cc(
|
||||
long if_freq,
|
||||
long fs_in, unsigned
|
||||
int vector_length,
|
||||
boost::shared_ptr<gr::msg_queue> queue,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
int order,
|
||||
float fll_bw_hz,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
galileo_e5a_dll_fll_pll_tracking_cc(
|
||||
long if_freq,
|
||||
long fs_in, unsigned
|
||||
int vector_length,
|
||||
boost::shared_ptr<gr::msg_queue> queue,
|
||||
bool dump,
|
||||
std::string dump_filename,
|
||||
int order,
|
||||
float fll_bw_hz,
|
||||
float pll_bw_hz,
|
||||
float dll_bw_hz,
|
||||
float early_late_space_chips);
|
||||
|
||||
void CN0_estimation_and_lock_detectors();
|
||||
|
||||
// class private vars
|
||||
Gnss_Synchro *d_acquisition_gnss_synchro;
|
||||
boost::shared_ptr<gr::msg_queue> d_queue;
|
||||
concurrent_queue<int> *d_channel_internal_queue;
|
||||
unsigned int d_vector_length;
|
||||
bool d_dump;
|
||||
unsigned int d_channel;
|
||||
int d_last_seg;
|
||||
double d_if_freq;
|
||||
double d_fs_in;
|
||||
|
||||
gr_complex* d_ca_code;
|
||||
|
||||
gr_complex* d_early_code;
|
||||
gr_complex* d_late_code;
|
||||
gr_complex* d_prompt_code;
|
||||
|
||||
gr_complex* d_carr_sign;
|
||||
|
||||
gr_complex* d_Early;
|
||||
gr_complex* d_Prompt;
|
||||
gr_complex* d_Late;
|
||||
|
||||
gr_complex d_Prompt_prev;
|
||||
|
||||
double d_early_late_spc_chips;
|
||||
|
||||
double d_carrier_doppler_hz;
|
||||
double d_code_freq_hz;
|
||||
double d_code_phase_samples;
|
||||
int d_current_prn_length_samples;
|
||||
//int d_next_prn_length_samples;
|
||||
int d_FLL_wait;
|
||||
double d_rem_carr_phase;
|
||||
double d_rem_code_phase_samples;
|
||||
//double d_next_rem_code_phase_samples;
|
||||
bool d_pull_in;
|
||||
|
||||
// acquisition
|
||||
double d_acq_code_phase_samples;
|
||||
double d_acq_carrier_doppler_hz;
|
||||
|
||||
// correlator
|
||||
Correlator d_correlator;
|
||||
|
||||
// FLL + PLL filter
|
||||
double d_FLL_discriminator_hz; // This is a class variable because FLL needs to have memory
|
||||
Tracking_FLL_PLL_filter d_carrier_loop_filter;
|
||||
double d_acc_carrier_phase_rad;
|
||||
double d_acc_code_phase_samples;
|
||||
|
||||
Tracking_2nd_DLL_filter d_code_loop_filter;
|
||||
|
||||
unsigned long int d_sample_counter;
|
||||
|
||||
unsigned long int d_acq_sample_stamp;
|
||||
|
||||
// CN0 estimation and lock detector
|
||||
int d_cn0_estimation_counter;
|
||||
gr_complex* d_Prompt_buffer;
|
||||
double d_carrier_lock_test;
|
||||
double d_CN0_SNV_dB_Hz;
|
||||
|
||||
double d_carrier_lock_threshold;
|
||||
|
||||
int d_carrier_lock_fail_counter;
|
||||
|
||||
bool d_enable_tracking;
|
||||
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
|
||||
std::map<std::string, std::string> systemName;
|
||||
std::string sys;
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_GALILEO_E5A_DLL_FLL_PLL_TRACKING_CC_H_ */
|
||||
|
|
@ -5,7 +5,7 @@
|
|||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2012 (see AUTHORS file for a list of contributors)
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
|
|
@ -28,15 +28,15 @@
|
|||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GALILEO_E5A_H_
|
||||
#define GALILEO_E5A_H_
|
||||
#ifndef GNSS_SDR_GALILEO_E5A_H_
|
||||
#define GNSS_SDR_GALILEO_E5A_H_
|
||||
|
||||
#include <complex>
|
||||
#include <gnss_satellite.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility> // std::pair
|
||||
//#include "MATH_CONSTANTS.h"
|
||||
#include "MATH_CONSTANTS.h"
|
||||
|
||||
// Physical constants
|
||||
const double GALILEO_PI = 3.1415926535898; //!< Pi as defined in GALILEO ICD
|
||||
|
|
@ -54,11 +54,177 @@ const double Galileo_E5a_Q_TIERED_CODE_PERIOD = 0.100; //!< Galileo
|
|||
const double Galileo_E5a_CODE_LENGTH_CHIPS = 10230.0; //!< Galileo E5a primary code length [chips]
|
||||
const double Galileo_E5a_I_SECONDARY_CODE_LENGTH = 20.0; //!< Galileo E5a-I secondary code length [chips]
|
||||
const double Galileo_E5a_Q_SECONDARY_CODE_LENGTH = 100.0; //!< Galileo E5a-Q secondary code length [chips]
|
||||
const double GALILEO_E5a_CODE_PERIOD = 0.001;
|
||||
const double Galileo_E5a_SYMBOL_RATE_BPS = 50.0; //!< Galileo E5a symbol rate [bits/second]
|
||||
const int Galileo_E5a_NUMBER_OF_CODES = 50;
|
||||
|
||||
// F/NAV message structure
|
||||
|
||||
#define GALILEO_FNAV_PREAMBLE {1, 0, 1, 1, 0, 1, 1, 1, 0, 0, 0, 0, 0}
|
||||
|
||||
const int GALILEO_FNAV_PREAMBLE_LENGTH_BITS = 12;
|
||||
const int GALILEO_FNAV_SAMPLES_PER_SYMBOL = 20; // (chip rate/ code length)/telemetry bps
|
||||
//const int GALILEO_FNAV_SYMBOLS_PER_PREAMBLE=240;
|
||||
const int GALILEO_FNAV_SAMPLES_PER_PREAMBLE = 240; // bits preamble * samples/symbol
|
||||
const int GALILEO_FNAV_SYMBOLS_PER_PAGE = 500; //Total symbols per page including preamble. See Galileo ICD 4.2.2
|
||||
const int GALILEO_FNAV_SECONDS_PER_PAGE = 10;
|
||||
const int GALILEO_FNAV_SAMPLES_PER_PAGE = 10000; // symbols * samples/symbol, where each 'sample' is a primary code
|
||||
|
||||
const int GALILEO_FNAV_INTERLEAVER_ROWS = 8;
|
||||
const int GALILEO_FNAV_INTERLEAVER_COLS = 61;
|
||||
const int GALILEO_FNAV_PAGE_TYPE_BITS = 6;
|
||||
|
||||
const int GALILEO_FNAV_DATA_FRAME_BITS = 214;
|
||||
const int GALILEO_FNAV_DATA_FRAME_BYTES = 27;
|
||||
|
||||
const std::vector<std::pair<int,int>> FNAV_PAGE_TYPE_bit({{1,6}});
|
||||
|
||||
/* WORD 1 iono corrections. FNAV (Galileo E5a message)*/
|
||||
const std::vector<std::pair<int,int>> FNAV_SV_ID_PRN_1_bit({{6,6}});
|
||||
const std::vector<std::pair<int,int>> FNAV_IODnav_1_bit({{12,10}});
|
||||
const std::vector<std::pair<int,int>> FNAV_t0c_1_bit({{22,14}});
|
||||
const double FNAV_t0c_1_LSB = 60;
|
||||
const std::vector<std::pair<int,int>> FNAV_af0_1_bit({{36,31}});
|
||||
const double FNAV_af0_1_LSB = TWO_N34;
|
||||
const std::vector<std::pair<int,int>> FNAV_af1_1_bit({{67,21}});
|
||||
const double FNAV_af1_1_LSB = TWO_N46;
|
||||
const std::vector<std::pair<int,int>> FNAV_af2_1_bit({{88,6}});
|
||||
const double FNAV_af2_1_LSB = TWO_N59;
|
||||
const std::vector<std::pair<int,int>> FNAV_SISA_1_bit({{94,8}});
|
||||
const std::vector<std::pair<int,int>> FNAV_ai0_1_bit({{102,11}});
|
||||
const double FNAV_ai0_1_LSB = TWO_N2;
|
||||
const std::vector<std::pair<int,int>> FNAV_ai1_1_bit({{113,11}});
|
||||
const double FNAV_ai1_1_LSB = TWO_N8;
|
||||
const std::vector<std::pair<int,int>> FNAV_ai2_1_bit({{124,14}});
|
||||
const double FNAV_ai2_1_LSB = TWO_N15;
|
||||
const std::vector<std::pair<int,int>> FNAV_region1_1_bit({{138,1}});
|
||||
const std::vector<std::pair<int,int>> FNAV_region2_1_bit({{139,1}});
|
||||
const std::vector<std::pair<int,int>> FNAV_region3_1_bit({{140,1}});
|
||||
const std::vector<std::pair<int,int>> FNAV_region4_1_bit({{141,1}});
|
||||
const std::vector<std::pair<int,int>> FNAV_region5_1_bit({{142,1}});
|
||||
const std::vector<std::pair<int,int>> FNAV_BGD_1_bit({{143,10}});
|
||||
const double FNAV_BGD_1_LSB = TWO_N32;
|
||||
const std::vector<std::pair<int,int>> FNAV_E5ahs_1_bit({{153,2}});
|
||||
const std::vector<std::pair<int,int>> FNAV_WN_1_bit({{155,12}});
|
||||
const std::vector<std::pair<int,int>> FNAV_TOW_1_bit({{167,20}});
|
||||
const std::vector<std::pair<int,int>> FNAV_E5advs_1_bit({{187,1}});
|
||||
|
||||
// WORD 2 Ephemeris (1/3)
|
||||
const std::vector<std::pair<int,int>> FNAV_IODnav_2_bit({{6,10}});
|
||||
const std::vector<std::pair<int,int>> FNAV_M0_2_bit({{16,32}});
|
||||
const double FNAV_M0_2_LSB = PI_TWO_N31;
|
||||
const std::vector<std::pair<int,int>> FNAV_omegadot_2_bit({{48,24}});
|
||||
const double FNAV_omegadot_2_LSB = PI_TWO_N43;
|
||||
const std::vector<std::pair<int,int>> FNAV_e_2_bit({{72,32}});
|
||||
const double FNAV_e_2_LSB = TWO_N33;
|
||||
const std::vector<std::pair<int,int>> FNAV_a12_2_bit({{104,32}});
|
||||
const double FNAV_a12_2_LSB = TWO_N19;
|
||||
const std::vector<std::pair<int,int>> FNAV_omega0_2_bit({{136,32}});
|
||||
const double FNAV_omega0_2_LSB = PI_TWO_N31;
|
||||
const std::vector<std::pair<int,int>> FNAV_idot_2_bit({{168,14}});
|
||||
const double FNAV_idot_2_LSB = PI_TWO_N43;
|
||||
const std::vector<std::pair<int,int>> FNAV_WN_2_bit({{182,12}});
|
||||
const std::vector<std::pair<int,int>> FNAV_TOW_2_bit({{194,20}});
|
||||
|
||||
// WORD 3 Ephemeris (2/3)
|
||||
const std::vector<std::pair<int,int>> FNAV_IODnav_3_bit({{6,10}});
|
||||
const std::vector<std::pair<int,int>> FNAV_i0_3_bit({{16,32}});
|
||||
const double FNAV_i0_3_LSB = PI_TWO_N31;
|
||||
const std::vector<std::pair<int,int>> FNAV_w_3_bit({{48,32}});
|
||||
const double FNAV_w_3_LSB = PI_TWO_N31;
|
||||
const std::vector<std::pair<int,int>> FNAV_deltan_3_bit({{80,16}});
|
||||
const double FNAV_deltan_3_LSB = PI_TWO_N43;
|
||||
const std::vector<std::pair<int,int>> FNAV_Cuc_3_bit({{96,16}});
|
||||
const double FNAV_Cuc_3_LSB = TWO_N29;
|
||||
const std::vector<std::pair<int,int>> FNAV_Cus_3_bit({{112,16}});
|
||||
const double FNAV_Cus_3_LSB = TWO_N29;
|
||||
const std::vector<std::pair<int,int>> FNAV_Crc_3_bit({{128,16}});
|
||||
const double FNAV_Crc_3_LSB = TWO_N5;
|
||||
const std::vector<std::pair<int,int>> FNAV_Crs_3_bit({{144,16}});
|
||||
const double FNAV_Crs_3_LSB = TWO_N5;
|
||||
const std::vector<std::pair<int,int>> FNAV_t0e_3_bit({{160,14}});
|
||||
const double FNAV_t0e_3_LSB = 60;
|
||||
const std::vector<std::pair<int,int>> FNAV_WN_3_bit({{174,12}});
|
||||
const std::vector<std::pair<int,int>> FNAV_TOW_3_bit({{186,20}});
|
||||
|
||||
// WORD 4 Ephemeris (3/3)
|
||||
const std::vector<std::pair<int,int>> FNAV_IODnav_4_bit({{6,10}});
|
||||
const std::vector<std::pair<int,int>> FNAV_Cic_4_bit({{16,16}});
|
||||
const double FNAV_Cic_4_LSB = TWO_N29;
|
||||
const std::vector<std::pair<int,int>> FNAV_Cis_4_bit({{32,16}});
|
||||
const double FNAV_Cis_4_LSB = TWO_N29;
|
||||
const std::vector<std::pair<int,int>> FNAV_A0_4_bit({{48,32}});
|
||||
const double FNAV_A0_4_LSB = TWO_N30;
|
||||
const std::vector<std::pair<int,int>> FNAV_A1_4_bit({{80,24}});
|
||||
const double FNAV_A1_4_LSB = TWO_N50;
|
||||
const std::vector<std::pair<int,int>> FNAV_deltatls_4_bit({{104,8}});
|
||||
const std::vector<std::pair<int,int>> FNAV_t0t_4_bit({{112,8}});
|
||||
const double FNAV_t0t_4_LSB = 3600;
|
||||
const std::vector<std::pair<int,int>> FNAV_WNot_4_bit({{120,8}});
|
||||
const std::vector<std::pair<int,int>> FNAV_WNlsf_4_bit({{128,8}});
|
||||
const std::vector<std::pair<int,int>> FNAV_DN_4_bit({{136,3}});
|
||||
const std::vector<std::pair<int,int>> FNAV_deltatlsf_4_bit({{139,8}});
|
||||
const std::vector<std::pair<int,int>> FNAV_t0g_4_bit({{147,8}});
|
||||
const double FNAV_t0g_4_LSB = 3600;
|
||||
const std::vector<std::pair<int,int>> FNAV_A0g_4_bit({{155,16}});
|
||||
const double FNAV_A0g_4_LSB = TWO_N35;
|
||||
const std::vector<std::pair<int,int>> FNAV_A1g_4_bit({{171,12}});
|
||||
const double FNAV_A1g_4_LSB = TWO_N51;
|
||||
const std::vector<std::pair<int,int>> FNAV_WN0g_4_bit({{183,6}});
|
||||
const std::vector<std::pair<int,int>> FNAV_TOW_4_bit({{189,20}});
|
||||
|
||||
// WORD 5 Almanac SVID1 SVID2(1/2)
|
||||
const std::vector<std::pair<int,int>> FNAV_IODa_5_bit({{6,4}});
|
||||
const std::vector<std::pair<int,int>> FNAV_WNa_5_bit({{10,2}});
|
||||
const std::vector<std::pair<int,int>> FNAV_t0a_5_bit({{12,10}});
|
||||
const double FNAV_t0a_5_LSB = 600;
|
||||
const std::vector<std::pair<int,int>> FNAV_SVID1_5_bit({{22,6}});
|
||||
const std::vector<std::pair<int,int>> FNAV_Deltaa12_1_5_bit({{28,13}});
|
||||
const double FNAV_Deltaa12_5_LSB = TWO_N9;
|
||||
const std::vector<std::pair<int,int>> FNAV_e_1_5_bit({{41,11}});
|
||||
const double FNAV_e_5_LSB = TWO_N16;
|
||||
const std::vector<std::pair<int,int>> FNAV_w_1_5_bit({{52,16}});
|
||||
const double FNAV_w_5_LSB = TWO_N15;
|
||||
const std::vector<std::pair<int,int>> FNAV_deltai_1_5_bit({{68,11}});
|
||||
const double FNAV_deltai_5_LSB = TWO_N14;
|
||||
const std::vector<std::pair<int,int>> FNAV_Omega0_1_5_bit({{79,16}});
|
||||
const double FNAV_Omega0_5_LSB = TWO_N15;
|
||||
const std::vector<std::pair<int,int>> FNAV_Omegadot_1_5_bit({{95,11}});
|
||||
const double FNAV_Omegadot_5_LSB = TWO_N33;
|
||||
const std::vector<std::pair<int,int>> FNAV_M0_1_5_bit({{106,16}});
|
||||
const double FNAV_M0_5_LSB = TWO_N15;
|
||||
const std::vector<std::pair<int,int>> FNAV_af0_1_5_bit({{122,16}});
|
||||
const double FNAV_af0_5_LSB = TWO_N19;
|
||||
const std::vector<std::pair<int,int>> FNAV_af1_1_5_bit({{138,13}});
|
||||
const double FNAV_af1_5_LSB = TWO_N38;
|
||||
const std::vector<std::pair<int,int>> FNAV_E5ahs_1_5_bit({{151,2}});
|
||||
const std::vector<std::pair<int,int>> FNAV_SVID2_5_bit({{153,6}});
|
||||
const std::vector<std::pair<int,int>> FNAV_Deltaa12_2_5_bit({{159,13}});
|
||||
const std::vector<std::pair<int,int>> FNAV_e_2_5_bit({{172,11}});
|
||||
const std::vector<std::pair<int,int>> FNAV_w_2_5_bit({{183,16}});
|
||||
const std::vector<std::pair<int,int>> FNAV_deltai_2_5_bit({{199,11}});
|
||||
//const std::vector<std::pair<int,int>> FNAV_Omega012_2_5_bit({{210,4}});
|
||||
|
||||
// WORD 6 Almanac SVID2(1/2) SVID3
|
||||
const std::vector<std::pair<int,int>> FNAV_IODa_6_bit({{6,4}});
|
||||
//const std::vector<std::pair<int,int>> FNAV_Omega022_2_6_bit({{10,12}});
|
||||
const std::vector<std::pair<int,int>> FNAV_Omegadot_2_6_bit({{22,11}});
|
||||
const std::vector<std::pair<int,int>> FNAV_M0_2_6_bit({{33,16}});
|
||||
const std::vector<std::pair<int,int>> FNAV_af0_2_6_bit({{49,16}});
|
||||
const std::vector<std::pair<int,int>> FNAV_af1_2_6_bit({{65,13}});
|
||||
const std::vector<std::pair<int,int>> FNAV_E5ahs_2_6_bit({{78,2}});
|
||||
const std::vector<std::pair<int,int>> FNAV_SVID3_6_bit({{80,6}});
|
||||
const std::vector<std::pair<int,int>> FNAV_Deltaa12_3_6_bit({{86,13}});
|
||||
const std::vector<std::pair<int,int>> FNAV_e_3_6_bit({{99,11}});
|
||||
const std::vector<std::pair<int,int>> FNAV_w_3_6_bit({{110,16}});
|
||||
const std::vector<std::pair<int,int>> FNAV_deltai_3_6_bit({{126,11}});
|
||||
const std::vector<std::pair<int,int>> FNAV_Omega0_3_6_bit({{137,16}});
|
||||
const std::vector<std::pair<int,int>> FNAV_Omegadot_3_6_bit({{153,11}});
|
||||
const std::vector<std::pair<int,int>> FNAV_M0_3_6_bit({{164,16}});
|
||||
const std::vector<std::pair<int,int>> FNAV_af0_3_6_bit({{180,16}});
|
||||
const std::vector<std::pair<int,int>> FNAV_af1_3_6_bit({{196,13}});
|
||||
const std::vector<std::pair<int,int>> FNAV_E5ahs_3_6_bit({{209,2}});
|
||||
|
||||
// Galileo E5a-I primary codes
|
||||
const std::string Galileo_E5a_I_PRIMARY_CODE[Galileo_E5a_NUMBER_OF_CODES] = {
|
||||
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
|
||||
|
|
@ -219,4 +385,4 @@ const std::string Galileo_E5a_Q_SECONDARY_CODE[Galileo_E5a_NUMBER_OF_CODES] = {
|
|||
};
|
||||
|
||||
|
||||
#endif /* GALILEO_E5A_H_ */
|
||||
#endif /* GNSS_SDR_GALILEO_E5A_H_ */
|
||||
|
|
|
|||
|
|
@ -59,6 +59,7 @@ public:
|
|||
double af1_8;
|
||||
double E5b_HS_8;
|
||||
double E1B_HS_8;
|
||||
double E5a_HS_8;
|
||||
int SVID2_8;
|
||||
double DELTA_A_8;
|
||||
double e_8;
|
||||
|
|
@ -76,6 +77,7 @@ public:
|
|||
double af1_9;
|
||||
double E5b_HS_9;
|
||||
double E1B_HS_9;
|
||||
double E5a_HS_9;
|
||||
int SVID3_9;
|
||||
double DELTA_A_9;
|
||||
double e_9;
|
||||
|
|
@ -91,6 +93,7 @@ public:
|
|||
double af1_10;
|
||||
double E5b_HS_10;
|
||||
double E1B_HS_10;
|
||||
double E5a_HS_10;
|
||||
|
||||
Galileo_Almanac(); //!< Default constructor
|
||||
};
|
||||
|
|
|
|||
707
src/core/system_parameters/galileo_fnav_message.cc
Normal file
707
src/core/system_parameters/galileo_fnav_message.cc
Normal file
|
|
@ -0,0 +1,707 @@
|
|||
/*!
|
||||
* \file galileo_fnav_message.h
|
||||
* \brief Implementation of a Galileo F/NAV Data message
|
||||
* as described in Galileo OS SIS ICD Issue 1.1 (Sept. 2010)
|
||||
* \author Marc Sales 2014. marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#include "galileo_fnav_message.h"
|
||||
|
||||
typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> CRC_Galileo_FNAV_type;
|
||||
|
||||
void Galileo_Fnav_Message::reset()
|
||||
{
|
||||
flag_CRC_test = false;
|
||||
flag_all_ephemeris = false; //!< Flag indicating that all words containing ephemeris have been received
|
||||
flag_ephemeris_1 = false; //!< Flag indicating that ephemeris 1/3 (word 2) have been received
|
||||
flag_ephemeris_2 = false; //!< Flag indicating that ephemeris 2/3 (word 3) have been received
|
||||
flag_ephemeris_3 = false; //!< Flag indicating that ephemeris 3/3 (word 4) have been received
|
||||
|
||||
flag_iono_and_GST = false; //!< Flag indicating that ionospheric and GST parameters (word 1) have been received
|
||||
flag_TOW_1 = false;
|
||||
flag_TOW_2 = false;
|
||||
flag_TOW_3 = false;
|
||||
flag_TOW_4 = false;
|
||||
flag_TOW_set = false; //!< it is true when page 1,2,3 or 4 arrives
|
||||
flag_utc_model = false; //!< Flag indicating that utc model parameters (word 4) have been received
|
||||
|
||||
flag_all_almanac = false; //!< Flag indicating that all almanac have been received
|
||||
flag_almanac_1 = false; //!< Flag indicating that almanac 1/2 (word 5) have been received
|
||||
flag_almanac_2 = false; //!< Flag indicating that almanac 2/2 (word 6) have been received
|
||||
|
||||
IOD_ephemeris = 0;
|
||||
|
||||
page_type = 0;
|
||||
/* WORD 1 SVID, Clock correction, SISA, Ionospheric correction, BGD, GST, Signal
|
||||
* health and Data validity status*/
|
||||
FNAV_SV_ID_PRN_1 = 0;
|
||||
FNAV_IODnav_1 = 0;
|
||||
FNAV_t0c_1 = 0;
|
||||
FNAV_af0_1 = 0;
|
||||
FNAV_af1_1 = 0;
|
||||
FNAV_af2_1 = 0;
|
||||
FNAV_SISA_1 = 0;
|
||||
FNAV_ai0_1 = 0;
|
||||
FNAV_ai1_1 = 0;
|
||||
FNAV_ai2_1 = 0;
|
||||
FNAV_region1_1 = 0;
|
||||
FNAV_region2_1 = 0;
|
||||
FNAV_region3_1 = 0;
|
||||
FNAV_region4_1 = 0;
|
||||
FNAV_region5_1 = 0;
|
||||
FNAV_BGD_1 = 0;
|
||||
FNAV_E5ahs_1 = 0;
|
||||
FNAV_WN_1 = 0;
|
||||
FNAV_TOW_1 = 0;
|
||||
FNAV_E5advs_1 = 0;
|
||||
|
||||
// WORD 2 Ephemeris (1/3) and GST
|
||||
FNAV_IODnav_2 = 0;
|
||||
FNAV_M0_2 = 0;
|
||||
FNAV_omegadot_2 = 0;
|
||||
FNAV_e_2 = 0;
|
||||
FNAV_a12_2 = 0;
|
||||
FNAV_omega0_2 = 0;
|
||||
FNAV_idot_2 = 0;
|
||||
FNAV_WN_2 = 0;
|
||||
FNAV_TOW_2 = 0;
|
||||
|
||||
// WORD 3 Ephemeris (2/3) and GST
|
||||
FNAV_IODnav_3 = 0;
|
||||
FNAV_i0_3 = 0;
|
||||
FNAV_w_3 = 0;
|
||||
FNAV_deltan_3 = 0;
|
||||
FNAV_Cuc_3 = 0;
|
||||
FNAV_Cus_3 = 0;
|
||||
FNAV_Crc_3 = 0;
|
||||
FNAV_Crs_3 = 0;
|
||||
FNAV_t0e_3 = 0;
|
||||
FNAV_WN_3 = 0;
|
||||
FNAV_TOW_3 = 0;
|
||||
|
||||
/* WORD 4 Ephemeris (3/3), GST-UTC conversion, GST-GPS conversion and TOW.
|
||||
Note that the clock is repeated in this page type*/
|
||||
FNAV_IODnav_4 = 0;
|
||||
FNAV_Cic_4 = 0;
|
||||
FNAV_Cis_4 = 0;
|
||||
FNAV_A0_4 = 0;
|
||||
FNAV_A1_4 = 0;
|
||||
FNAV_deltatls_4 = 0;
|
||||
FNAV_t0t_4 = 0;
|
||||
FNAV_WNot_4 = 0;
|
||||
FNAV_WNlsf_4 = 0;
|
||||
FNAV_DN_4 = 0;
|
||||
FNAV_deltatlsf_4 = 0;
|
||||
FNAV_t0g_4 = 0;
|
||||
FNAV_A0g_4 = 0;
|
||||
FNAV_A1g_4 = 0;
|
||||
FNAV_WN0g_4 = 0;
|
||||
FNAV_TOW_4 = 0;
|
||||
|
||||
// WORD 5 Almanac (SVID1 and SVID2(1/2)), Week Number and almanac reference time
|
||||
FNAV_IODa_5 = 0;
|
||||
FNAV_WNa_5 = 0;
|
||||
FNAV_t0a_5 = 0;
|
||||
FNAV_SVID1_5 = 0;
|
||||
FNAV_Deltaa12_1_5 = 0;
|
||||
FNAV_e_1_5 = 0;
|
||||
FNAV_w_1_5 = 0;
|
||||
FNAV_deltai_1_5 = 0;
|
||||
FNAV_Omega0_1_5 = 0;
|
||||
FNAV_Omegadot_1_5 = 0;
|
||||
FNAV_M0_1_5 = 0;
|
||||
FNAV_af0_1_5 = 0;
|
||||
FNAV_af1_1_5 = 0;
|
||||
FNAV_E5ahs_1_5 = 0;
|
||||
FNAV_SVID2_5 = 0;
|
||||
FNAV_Deltaa12_2_5 = 0;
|
||||
FNAV_e_2_5 = 0;
|
||||
FNAV_w_2_5 = 0;
|
||||
FNAV_deltai_2_5 = 0;
|
||||
|
||||
// WORD 6 Almanac (SVID2(2/2) and SVID3)
|
||||
FNAV_IODa_6 = 0;
|
||||
FNAV_Omega0_2_6 = 0;
|
||||
FNAV_Omegadot_2_6 = 0;
|
||||
FNAV_M0_2_6 = 0;
|
||||
FNAV_af0_2_6 = 0;
|
||||
FNAV_af1_2_6 = 0;
|
||||
FNAV_E5ahs_2_6 = 0;
|
||||
FNAV_SVID3_6 = 0;
|
||||
FNAV_Deltaa12_3_6 = 0;
|
||||
FNAV_e_3_6 = 0;
|
||||
FNAV_w_3_6 = 0;
|
||||
FNAV_deltai_3_6 = 0;
|
||||
FNAV_Omega0_3_6 = 0;
|
||||
FNAV_Omegadot_3_6 = 0;
|
||||
FNAV_M0_3_6 = 0;
|
||||
FNAV_af0_3_6 = 0;
|
||||
FNAV_af1_3_6 = 0;
|
||||
FNAV_E5ahs_3_6 = 0;
|
||||
}
|
||||
|
||||
Galileo_Fnav_Message::Galileo_Fnav_Message()
|
||||
{
|
||||
reset();
|
||||
}
|
||||
//int Galileo_Fnav_Message::toInt(std::string bitString)
|
||||
//{
|
||||
// int tempInt;
|
||||
// int num=0;
|
||||
// int sLength = bitString.length();
|
||||
// for(int i=0; i<sLength; i++)
|
||||
// {
|
||||
// num |= (1 << (sLength-1-i))*tempInt;
|
||||
// }
|
||||
//
|
||||
// return num;
|
||||
//}
|
||||
void Galileo_Fnav_Message::split_page(std::string page_string)
|
||||
{
|
||||
std::string message_word = page_string.substr(0,214);
|
||||
std::string CRC_data = page_string.substr(214,24);
|
||||
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> Word_for_CRC_bits(message_word);
|
||||
std::bitset<24> checksum(CRC_data);
|
||||
if (CRC_test(Word_for_CRC_bits, checksum.to_ulong()) == true)
|
||||
{
|
||||
flag_CRC_test = true;
|
||||
// CRC correct: Decode word
|
||||
decode_page(message_word);
|
||||
}
|
||||
else
|
||||
{
|
||||
flag_CRC_test = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool Galileo_Fnav_Message::CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits,boost::uint32_t checksum)
|
||||
{
|
||||
CRC_Galileo_FNAV_type CRC_Galileo;
|
||||
|
||||
boost::uint32_t crc_computed;
|
||||
// Galileo FNAV frame for CRC is not an integer multiple of bytes
|
||||
// it needs to be filled with zeroes at the start of the frame.
|
||||
// This operation is done in the transformation from bits to bytes
|
||||
// using boost::dynamic_bitset.
|
||||
// ToDo: Use boost::dynamic_bitset for all the bitset operations in this class
|
||||
|
||||
boost::dynamic_bitset<unsigned char> frame_bits(std::string(bits.to_string()));
|
||||
|
||||
std::vector<unsigned char> bytes;
|
||||
boost::to_block_range(frame_bits, std::back_inserter(bytes));
|
||||
std::reverse(bytes.begin(),bytes.end());
|
||||
|
||||
CRC_Galileo.process_bytes( bytes.data(), GALILEO_FNAV_DATA_FRAME_BYTES );
|
||||
|
||||
crc_computed = CRC_Galileo.checksum();
|
||||
if (checksum == crc_computed)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
void Galileo_Fnav_Message::decode_page(std::string data)
|
||||
{
|
||||
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> data_bits(data);
|
||||
page_type = read_navigation_unsigned(data_bits,FNAV_PAGE_TYPE_bit);
|
||||
switch(page_type)
|
||||
{
|
||||
case 1: // SVID, Clock correction, SISA, Ionospheric correction, BGD, GST, Signal health and Data validity status
|
||||
FNAV_SV_ID_PRN_1=(int)read_navigation_unsigned(data_bits,FNAV_SV_ID_PRN_1_bit);
|
||||
FNAV_IODnav_1=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_1_bit);
|
||||
FNAV_t0c_1=(double)read_navigation_unsigned(data_bits,FNAV_t0c_1_bit);
|
||||
FNAV_t0c_1 *= FNAV_t0c_1_LSB;
|
||||
FNAV_af0_1=(double)read_navigation_signed(data_bits,FNAV_af0_1_bit);
|
||||
FNAV_af0_1 *= FNAV_af0_1_LSB;
|
||||
FNAV_af1_1=(double)read_navigation_signed(data_bits,FNAV_af1_1_bit);
|
||||
FNAV_af1_1 *= FNAV_af1_1_LSB;
|
||||
FNAV_af2_1=(double)read_navigation_signed(data_bits,FNAV_af2_1_bit);
|
||||
FNAV_af2_1 *= FNAV_af2_1_LSB;
|
||||
FNAV_SISA_1=(double)read_navigation_unsigned(data_bits,FNAV_SISA_1_bit);
|
||||
FNAV_ai0_1=(double)read_navigation_unsigned(data_bits,FNAV_ai0_1_bit);
|
||||
FNAV_ai0_1 *= FNAV_ai0_1_LSB;
|
||||
FNAV_ai1_1=(double)read_navigation_signed(data_bits,FNAV_ai1_1_bit);
|
||||
FNAV_ai1_1 *= FNAV_ai1_1_LSB;
|
||||
FNAV_ai2_1=(double)read_navigation_signed(data_bits,FNAV_ai2_1_bit);
|
||||
FNAV_ai2_1 *= FNAV_ai2_1_LSB;
|
||||
FNAV_region1_1=(bool)read_navigation_unsigned(data_bits,FNAV_region1_1_bit);
|
||||
FNAV_region2_1=(bool)read_navigation_unsigned(data_bits,FNAV_region2_1_bit);
|
||||
FNAV_region3_1=(bool)read_navigation_unsigned(data_bits,FNAV_region3_1_bit);
|
||||
FNAV_region4_1=(bool)read_navigation_unsigned(data_bits,FNAV_region4_1_bit);
|
||||
FNAV_region5_1=(bool)read_navigation_unsigned(data_bits,FNAV_region5_1_bit);
|
||||
FNAV_BGD_1=(double)read_navigation_signed(data_bits,FNAV_BGD_1_bit);
|
||||
FNAV_BGD_1 *= FNAV_BGD_1_LSB;
|
||||
FNAV_E5ahs_1=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_1_bit);
|
||||
FNAV_WN_1=(double)read_navigation_unsigned(data_bits,FNAV_WN_1_bit);
|
||||
FNAV_TOW_1=(double)read_navigation_unsigned(data_bits,FNAV_TOW_1_bit);
|
||||
FNAV_E5advs_1=(double)read_navigation_unsigned(data_bits,FNAV_E5advs_1_bit);
|
||||
|
||||
flag_TOW_1=true;
|
||||
flag_TOW_set=true;
|
||||
flag_iono_and_GST = true; //set to false externally
|
||||
break;
|
||||
case 2: // Ephemeris (1/3) and GST
|
||||
FNAV_IODnav_2=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_2_bit);
|
||||
FNAV_M0_2=(double)read_navigation_unsigned(data_bits,FNAV_M0_2_bit);
|
||||
FNAV_M0_2 *= FNAV_M0_2_LSB;
|
||||
FNAV_omegadot_2=(double)read_navigation_signed(data_bits,FNAV_omegadot_2_bit);
|
||||
FNAV_omegadot_2 *= FNAV_omegadot_2_LSB;
|
||||
FNAV_e_2=(double)read_navigation_unsigned(data_bits,FNAV_e_2_bit);
|
||||
FNAV_e_2 *= FNAV_e_2_LSB;
|
||||
FNAV_a12_2=(double)read_navigation_unsigned(data_bits,FNAV_a12_2_bit);
|
||||
FNAV_a12_2 *= FNAV_a12_2_LSB;
|
||||
FNAV_omega0_2=(double)read_navigation_signed(data_bits,FNAV_omega0_2_bit);
|
||||
FNAV_omega0_2 *= FNAV_omega0_2_LSB;
|
||||
FNAV_idot_2=(double)read_navigation_signed(data_bits,FNAV_idot_2_bit);
|
||||
FNAV_idot_2 *= FNAV_idot_2_LSB;
|
||||
FNAV_WN_2=(double)read_navigation_unsigned(data_bits,FNAV_WN_2_bit);
|
||||
FNAV_TOW_2=(double)read_navigation_unsigned(data_bits,FNAV_TOW_2_bit);
|
||||
|
||||
flag_TOW_2=true;
|
||||
flag_TOW_set=true;
|
||||
flag_ephemeris_1=true;
|
||||
break;
|
||||
case 3: // Ephemeris (2/3) and GST
|
||||
FNAV_IODnav_3=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_3_bit);
|
||||
FNAV_i0_3=(double)read_navigation_signed(data_bits,FNAV_i0_3_bit);
|
||||
FNAV_i0_3 *= FNAV_i0_3_LSB;
|
||||
FNAV_w_3=(double)read_navigation_signed(data_bits,FNAV_w_3_bit);
|
||||
FNAV_w_3 *= FNAV_w_3_LSB;
|
||||
FNAV_deltan_3=(double)read_navigation_unsigned(data_bits,FNAV_deltan_3_bit);
|
||||
FNAV_deltan_3 *= FNAV_deltan_3_LSB;
|
||||
FNAV_Cuc_3=(double)read_navigation_signed(data_bits,FNAV_Cuc_3_bit);
|
||||
FNAV_Cuc_3 *= FNAV_Cuc_3_LSB;
|
||||
FNAV_Cus_3=(double)read_navigation_signed(data_bits,FNAV_Cus_3_bit);
|
||||
FNAV_Cus_3 *= FNAV_Cus_3_LSB;
|
||||
FNAV_Crc_3=(double)read_navigation_signed(data_bits,FNAV_Crc_3_bit);
|
||||
FNAV_Crc_3 *= FNAV_Crc_3_LSB;
|
||||
FNAV_Crs_3=(double)read_navigation_signed(data_bits,FNAV_Crs_3_bit);
|
||||
FNAV_Crs_3 *= FNAV_Crs_3_LSB;
|
||||
FNAV_t0e_3=(double)read_navigation_unsigned(data_bits,FNAV_t0e_3_bit);
|
||||
FNAV_t0e_3 *= FNAV_t0e_3_LSB;
|
||||
FNAV_WN_3=(double)read_navigation_unsigned(data_bits,FNAV_WN_3_bit);
|
||||
FNAV_TOW_3=(double)read_navigation_unsigned(data_bits,FNAV_TOW_3_bit);
|
||||
|
||||
flag_TOW_3=true;
|
||||
flag_TOW_set=true;
|
||||
flag_ephemeris_2=true;
|
||||
break;
|
||||
case 4: // Ephemeris (3/3), GST-UTC conversion, GST-GPS conversion and TOW
|
||||
FNAV_IODnav_4=(int)read_navigation_unsigned(data_bits,FNAV_IODnav_4_bit);
|
||||
FNAV_Cic_4=(double)read_navigation_unsigned(data_bits,FNAV_Cic_4_bit);
|
||||
FNAV_Cic_4 *= FNAV_Cic_4_LSB;
|
||||
FNAV_Cis_4=(double)read_navigation_unsigned(data_bits,FNAV_Cis_4_bit);
|
||||
FNAV_Cis_4 *= FNAV_Cis_4_LSB;
|
||||
FNAV_A0_4=(double)read_navigation_unsigned(data_bits,FNAV_A0_4_bit);
|
||||
FNAV_A0_4 *= FNAV_A0_4_LSB;
|
||||
FNAV_A1_4=(double)read_navigation_unsigned(data_bits,FNAV_A1_4_bit);
|
||||
FNAV_A1_4 *= FNAV_A1_4_LSB;
|
||||
FNAV_deltatls_4=(double)read_navigation_signed(data_bits,FNAV_deltatls_4_bit);
|
||||
FNAV_t0t_4=(double)read_navigation_unsigned(data_bits,FNAV_t0t_4_bit);
|
||||
FNAV_t0t_4 *= FNAV_t0t_4_LSB;
|
||||
FNAV_WNot_4=(double)read_navigation_unsigned(data_bits,FNAV_WNot_4_bit);
|
||||
FNAV_WNlsf_4=(double)read_navigation_unsigned(data_bits,FNAV_WNlsf_4_bit);
|
||||
FNAV_DN_4=(double)read_navigation_unsigned(data_bits,FNAV_DN_4_bit);
|
||||
FNAV_deltatlsf_4=(double)read_navigation_signed(data_bits,FNAV_deltatlsf_4_bit);
|
||||
FNAV_t0g_4=(double)read_navigation_unsigned(data_bits,FNAV_t0g_4_bit);
|
||||
FNAV_t0g_4 *= FNAV_t0g_4_LSB;
|
||||
FNAV_A0g_4=(double)read_navigation_signed(data_bits,FNAV_A0g_4_bit);
|
||||
FNAV_A0g_4 *= FNAV_A0g_4_LSB;
|
||||
FNAV_A1g_4=(double)read_navigation_signed(data_bits,FNAV_A1g_4_bit);
|
||||
FNAV_A1g_4 *= FNAV_A1g_4_LSB;
|
||||
FNAV_WN0g_4=(double)read_navigation_unsigned(data_bits,FNAV_WN0g_4_bit);
|
||||
FNAV_TOW_4=(double)read_navigation_unsigned(data_bits,FNAV_TOW_4_bit);
|
||||
|
||||
flag_TOW_4=true;
|
||||
flag_TOW_set=true;
|
||||
flag_ephemeris_3=true;
|
||||
flag_utc_model = true; //set to false externally
|
||||
break;
|
||||
case 5: // Almanac (SVID1 and SVID2(1/2)), Week Number and almanac reference time
|
||||
FNAV_IODa_5=(int)read_navigation_unsigned(data_bits,FNAV_IODa_5_bit);
|
||||
FNAV_WNa_5=(double)read_navigation_unsigned(data_bits,FNAV_WNa_5_bit);
|
||||
FNAV_t0a_5=(double)read_navigation_unsigned(data_bits,FNAV_t0a_5_bit);
|
||||
FNAV_t0a_5 *= FNAV_t0a_5_LSB;
|
||||
FNAV_SVID1_5=(int)read_navigation_unsigned(data_bits,FNAV_SVID1_5_bit);
|
||||
FNAV_Deltaa12_1_5=(double)read_navigation_signed(data_bits,FNAV_Deltaa12_1_5_bit);
|
||||
FNAV_Deltaa12_1_5 *= FNAV_Deltaa12_5_LSB;
|
||||
FNAV_e_1_5=(double)read_navigation_unsigned(data_bits,FNAV_e_1_5_bit);
|
||||
FNAV_e_1_5 *= FNAV_e_5_LSB;
|
||||
FNAV_w_1_5=(double)read_navigation_signed(data_bits,FNAV_w_1_5_bit);
|
||||
FNAV_w_1_5 *= FNAV_w_5_LSB;
|
||||
FNAV_deltai_1_5=(double)read_navigation_signed(data_bits,FNAV_deltai_1_5_bit);
|
||||
FNAV_deltai_1_5 *= FNAV_deltai_5_LSB;
|
||||
FNAV_Omega0_1_5=(double)read_navigation_signed(data_bits,FNAV_Omega0_1_5_bit);
|
||||
FNAV_Omega0_1_5 *= FNAV_Omega0_5_LSB;
|
||||
FNAV_Omegadot_1_5=(double)read_navigation_signed(data_bits,FNAV_Omegadot_1_5_bit);
|
||||
FNAV_Omegadot_1_5 *= FNAV_Omegadot_5_LSB;
|
||||
FNAV_M0_1_5=(double)read_navigation_signed(data_bits,FNAV_M0_1_5_bit);
|
||||
FNAV_M0_1_5 *= FNAV_M0_5_LSB;
|
||||
FNAV_af0_1_5=(double)read_navigation_signed(data_bits,FNAV_af0_1_5_bit);
|
||||
FNAV_af0_1_5 *= FNAV_af0_5_LSB;
|
||||
FNAV_af1_1_5=(double)read_navigation_signed(data_bits,FNAV_af1_1_5_bit);
|
||||
FNAV_af1_1_5 *= FNAV_af1_5_LSB;
|
||||
FNAV_E5ahs_1_5=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_1_5_bit);
|
||||
FNAV_SVID2_5=(int)read_navigation_unsigned(data_bits,FNAV_SVID2_5_bit);
|
||||
FNAV_Deltaa12_2_5=(double)read_navigation_signed(data_bits,FNAV_Deltaa12_2_5_bit);
|
||||
FNAV_Deltaa12_2_5 *= FNAV_Deltaa12_5_LSB;
|
||||
FNAV_e_2_5=(double)read_navigation_unsigned(data_bits,FNAV_e_2_5_bit);
|
||||
FNAV_e_2_5 *= FNAV_e_5_LSB;
|
||||
FNAV_w_2_5=(double)read_navigation_signed(data_bits,FNAV_w_2_5_bit);
|
||||
FNAV_w_2_5 *= FNAV_w_5_LSB;
|
||||
FNAV_deltai_2_5=(double)read_navigation_signed(data_bits,FNAV_deltai_2_5_bit);
|
||||
FNAV_deltai_2_5 *= FNAV_deltai_5_LSB;
|
||||
//TODO check this
|
||||
// Omega0_2 must be decoded when the two pieces are joined
|
||||
omega0_1=data.substr(210,4);
|
||||
//omega_flag=true;
|
||||
//
|
||||
//FNAV_Omega012_2_5=(double)read_navigation_signed(data_bits,FNAV_Omega012_2_5_bit);
|
||||
|
||||
flag_almanac_1=true;
|
||||
break;
|
||||
case 6: // Almanac (SVID2(2/2) and SVID3)
|
||||
FNAV_IODa_6=(int)read_navigation_unsigned(data_bits,FNAV_IODa_6_bit);
|
||||
|
||||
/* Don't worry about omega pieces. If page 5 has not been received, all_ephemeris
|
||||
* flag will be set to false and the data won't be recorded.*/
|
||||
std::string omega0_2 = data.substr(10,12);
|
||||
std::string Omega0 = omega0_1 + omega0_2;
|
||||
std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> omega_bits(Omega0);
|
||||
const std::vector<std::pair<int,int>> om_bit({{0,12}});
|
||||
FNAV_Omega0_2_6=(double)read_navigation_signed(omega_bits,om_bit);
|
||||
FNAV_Omega0_2_6 *= FNAV_Omega0_5_LSB;
|
||||
//
|
||||
FNAV_Omegadot_2_6=(double)read_navigation_signed(data_bits,FNAV_Omegadot_2_6_bit);
|
||||
FNAV_Omegadot_2_6 *= FNAV_Omegadot_5_LSB;
|
||||
FNAV_M0_2_6=(double)read_navigation_signed(data_bits,FNAV_M0_2_6_bit);
|
||||
FNAV_M0_2_6 *= FNAV_M0_5_LSB;
|
||||
FNAV_af0_2_6=(double)read_navigation_signed(data_bits,FNAV_af0_2_6_bit);
|
||||
FNAV_af0_2_6 *= FNAV_af0_5_LSB;
|
||||
FNAV_af1_2_6=(double)read_navigation_signed(data_bits,FNAV_af1_2_6_bit);
|
||||
FNAV_af1_2_6 *= FNAV_af1_5_LSB;
|
||||
FNAV_E5ahs_2_6=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_2_6_bit);
|
||||
FNAV_SVID3_6=(int)read_navigation_unsigned(data_bits,FNAV_SVID3_6_bit);
|
||||
FNAV_Deltaa12_3_6=(double)read_navigation_signed(data_bits,FNAV_Deltaa12_3_6_bit);
|
||||
FNAV_Deltaa12_3_6 *= FNAV_Deltaa12_5_LSB;
|
||||
FNAV_e_3_6=(double)read_navigation_unsigned(data_bits,FNAV_e_3_6_bit);
|
||||
FNAV_e_3_6 *= FNAV_e_5_LSB;
|
||||
FNAV_w_3_6=(double)read_navigation_signed(data_bits,FNAV_w_3_6_bit);
|
||||
FNAV_w_3_6 *= FNAV_w_5_LSB;
|
||||
FNAV_deltai_3_6=(double)read_navigation_signed(data_bits,FNAV_deltai_3_6_bit);
|
||||
FNAV_deltai_3_6 *= FNAV_deltai_5_LSB;
|
||||
FNAV_Omega0_3_6=(double)read_navigation_signed(data_bits,FNAV_Omega0_3_6_bit);
|
||||
FNAV_Omega0_3_6 *= FNAV_Omega0_5_LSB;
|
||||
FNAV_Omegadot_3_6=(double)read_navigation_signed(data_bits,FNAV_Omegadot_3_6_bit);
|
||||
FNAV_Omegadot_3_6 *= FNAV_Omegadot_5_LSB;
|
||||
FNAV_M0_3_6=(double)read_navigation_signed(data_bits,FNAV_M0_3_6_bit);
|
||||
FNAV_M0_3_6 *= FNAV_M0_5_LSB;
|
||||
FNAV_af0_3_6=(double)read_navigation_signed(data_bits,FNAV_af0_3_6_bit);
|
||||
FNAV_af0_3_6 *= FNAV_af0_5_LSB;
|
||||
FNAV_af1_3_6=(double)read_navigation_signed(data_bits,FNAV_af1_3_6_bit);
|
||||
FNAV_af1_3_6 *= FNAV_af1_5_LSB;
|
||||
FNAV_E5ahs_3_6=(double)read_navigation_unsigned(data_bits,FNAV_E5ahs_3_6_bit);
|
||||
|
||||
flag_almanac_2=true;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
unsigned long int Galileo_Fnav_Message::read_navigation_unsigned(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int,int>> parameter)
|
||||
{
|
||||
unsigned long int value = 0;
|
||||
int num_of_slices = parameter.size();
|
||||
for (int i=0; i<num_of_slices; i++)
|
||||
{
|
||||
for (int j=0; j<parameter[i].second; j++)
|
||||
{
|
||||
value <<= 1; //shift left
|
||||
if (bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[i].first - j] == 1)
|
||||
{
|
||||
value += 1; // insert the bit
|
||||
}
|
||||
}
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
signed long int Galileo_Fnav_Message::read_navigation_signed(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int,int>> parameter)
|
||||
{
|
||||
signed long int value = 0;
|
||||
int num_of_slices = parameter.size();
|
||||
// Discriminate between 64 bits and 32 bits compiler
|
||||
int long_int_size_bytes = sizeof(signed long int);
|
||||
if (long_int_size_bytes == 8) // if a long int takes 8 bytes, we are in a 64 bits system
|
||||
{
|
||||
// read the MSB and perform the sign extension
|
||||
if (bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[0].first] == 1)
|
||||
{
|
||||
value ^= 0xFFFFFFFFFFFFFFFF; //64 bits variable
|
||||
}
|
||||
else
|
||||
{
|
||||
value &= 0;
|
||||
}
|
||||
|
||||
for (int i=0; i<num_of_slices; i++)
|
||||
{
|
||||
for (int j=0; j<parameter[i].second; j++)
|
||||
{
|
||||
value <<= 1; //shift left
|
||||
value &= 0xFFFFFFFFFFFFFFFE; //reset the corresponding bit (for the 64 bits variable)
|
||||
if (bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[i].first - j] == 1)
|
||||
{
|
||||
value += 1; // insert the bit
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else // we assume we are in a 32 bits system
|
||||
{
|
||||
// read the MSB and perform the sign extension
|
||||
if (bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[0].first] == 1)
|
||||
{
|
||||
value ^= 0xFFFFFFFF;
|
||||
}
|
||||
else
|
||||
{
|
||||
value &= 0;
|
||||
}
|
||||
|
||||
for (int i=0; i<num_of_slices; i++)
|
||||
{
|
||||
for (int j=0; j<parameter[i].second; j++)
|
||||
{
|
||||
value <<= 1; //shift left
|
||||
value &= 0xFFFFFFFE; //reset the corresponding bit
|
||||
if (bits[GALILEO_FNAV_DATA_FRAME_BITS - parameter[i].first - j] == 1)
|
||||
{
|
||||
value += 1; // insert the bit
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
bool Galileo_Fnav_Message::have_new_ephemeris() //Check if we have a new ephemeris stored in the galileo navigation class
|
||||
{
|
||||
if ((flag_ephemeris_1 == true) and (flag_ephemeris_2 == true) and (flag_ephemeris_3 == true) and (flag_iono_and_GST == true))
|
||||
{
|
||||
//if all ephemeris pages have the same IOD, then they belong to the same block
|
||||
if ((FNAV_IODnav_1 == FNAV_IODnav_2) and (FNAV_IODnav_3 == FNAV_IODnav_4) and (FNAV_IODnav_1 == FNAV_IODnav_3))
|
||||
{
|
||||
std::cout << "Ephemeris (1, 2, 3) have been received and belong to the same batch" << std::endl;
|
||||
flag_ephemeris_1 = false;// clear the flag
|
||||
flag_ephemeris_2 = false;// clear the flag
|
||||
flag_ephemeris_3 = false;// clear the flag
|
||||
flag_all_ephemeris = true;
|
||||
IOD_ephemeris = FNAV_IODnav_1;
|
||||
std::cout << "Batch number: "<< IOD_ephemeris << std::endl;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
bool Galileo_Fnav_Message::have_new_iono_and_GST() //Check if we have a new iono data set stored in the galileo navigation class
|
||||
{
|
||||
if ((flag_iono_and_GST == true) and (flag_utc_model == true)) //the condition on flag_utc_model is added to have a time stamp for iono
|
||||
{
|
||||
flag_iono_and_GST = false; // clear the flag
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
bool Galileo_Fnav_Message::have_new_utc_model() // Check if we have a new utc data set stored in the galileo navigation class
|
||||
{
|
||||
if (flag_utc_model == true)
|
||||
{
|
||||
flag_utc_model = false; // clear the flag
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
bool Galileo_Fnav_Message::have_new_almanac() //Check if we have a new almanac data set stored in the galileo navigation class
|
||||
{
|
||||
if ((flag_almanac_1 == true) and (flag_almanac_2 == true))
|
||||
{
|
||||
//All almanac have been received
|
||||
flag_almanac_1 = false;
|
||||
flag_almanac_2 = false;
|
||||
flag_all_almanac = true;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris()
|
||||
{
|
||||
Galileo_Ephemeris ephemeris;
|
||||
ephemeris.flag_all_ephemeris = flag_all_ephemeris;
|
||||
ephemeris.IOD_ephemeris = IOD_ephemeris;
|
||||
ephemeris.SV_ID_PRN_4 = FNAV_SV_ID_PRN_1;
|
||||
ephemeris.i_satellite_PRN = FNAV_SV_ID_PRN_1;
|
||||
ephemeris.M0_1 = FNAV_M0_2; // Mean anomaly at reference time [semi-circles]
|
||||
ephemeris.delta_n_3 = FNAV_deltan_3;// Mean motion difference from computed value [semi-circles/sec]
|
||||
ephemeris.e_1 = FNAV_e_2; // Eccentricity
|
||||
ephemeris.A_1 = FNAV_a12_2; // Square root of the semi-major axis [metres^1/2]
|
||||
ephemeris.OMEGA_0_2 = FNAV_omega0_2;// Longitude of ascending node of orbital plane at weekly epoch [semi-circles]
|
||||
ephemeris.i_0_2 = FNAV_i0_3; // Inclination angle at reference time [semi-circles]
|
||||
ephemeris.omega_2 = FNAV_w_3; // Argument of perigee [semi-circles]
|
||||
ephemeris.OMEGA_dot_3 = FNAV_omegadot_2; // Rate of right ascension [semi-circles/sec]
|
||||
ephemeris.iDot_2 = FNAV_idot_2; // Rate of inclination angle [semi-circles/sec]
|
||||
ephemeris.C_uc_3 = FNAV_Cuc_3; // Amplitude of the cosine harmonic correction term to the argument of latitude [radians]
|
||||
ephemeris.C_us_3 = FNAV_Cus_3; // Amplitude of the sine harmonic correction term to the argument of latitude [radians]
|
||||
ephemeris.C_rc_3 = FNAV_Crc_3; // Amplitude of the cosine harmonic correction term to the orbit radius [meters]
|
||||
ephemeris.C_rs_3 = FNAV_Crs_3; // Amplitude of the sine harmonic correction term to the orbit radius [meters]
|
||||
ephemeris.C_ic_4 = FNAV_Cic_4; // Amplitude of the cosine harmonic correction term to the angle of inclination [radians]
|
||||
ephemeris.C_is_4 = FNAV_Cis_4; // Amplitude of the sine harmonic correction term to the angle of inclination [radians]
|
||||
ephemeris.t0e_1 = FNAV_t0e_3; // Ephemeris reference time [s]
|
||||
|
||||
/*Clock correction parameters*/
|
||||
ephemeris.t0c_4 = FNAV_t0c_1; // Clock correction data reference Time of Week [sec]
|
||||
ephemeris.af0_4 = FNAV_af0_1; // SV clock bias correction coefficient [s]
|
||||
ephemeris.af1_4 = FNAV_af1_1; // SV clock drift correction coefficient [s/s]
|
||||
ephemeris.af2_4 = FNAV_af2_1; // SV clock drift rate correction coefficient [s/s^2]
|
||||
|
||||
/*GST*/
|
||||
ephemeris.WN_5 = FNAV_WN_3; // Week number
|
||||
ephemeris.TOW_5 = FNAV_TOW_3; // Time of Week
|
||||
return ephemeris;
|
||||
}
|
||||
|
||||
Galileo_Iono Galileo_Fnav_Message::get_iono()
|
||||
{
|
||||
Galileo_Iono iono;
|
||||
/*Ionospheric correction*/
|
||||
/*Az*/
|
||||
iono.ai0_5 = FNAV_ai0_1; // Effective Ionisation Level 1st order parameter [sfu]
|
||||
iono.ai1_5 = FNAV_ai1_1; // Effective Ionisation Level 2st order parameter [sfu/degree]
|
||||
iono.ai2_5 = FNAV_ai2_1; // Effective Ionisation Level 3st order parameter [sfu/degree]
|
||||
|
||||
/*Ionospheric disturbance flag*/
|
||||
iono.Region1_flag_5 = FNAV_region1_1; // Ionospheric Disturbance Flag for region 1
|
||||
iono.Region2_flag_5 = FNAV_region2_1; // Ionospheric Disturbance Flag for region 2
|
||||
iono.Region3_flag_5 = FNAV_region3_1; // Ionospheric Disturbance Flag for region 3
|
||||
iono.Region4_flag_5 = FNAV_region4_1; // Ionospheric Disturbance Flag for region 4
|
||||
iono.Region5_flag_5 = FNAV_region5_1; // Ionospheric Disturbance Flag for region 5
|
||||
|
||||
/*GST*/
|
||||
iono.TOW_5 = FNAV_TOW_1;
|
||||
iono.WN_5 = FNAV_WN_1;
|
||||
return iono;
|
||||
}
|
||||
|
||||
Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model()
|
||||
{
|
||||
Galileo_Utc_Model utc_model;
|
||||
//Gal_utc_model.valid = flag_utc_model_valid;
|
||||
/*Word type 6: GST-UTC conversion parameters*/
|
||||
utc_model.A0_6 = FNAV_A0_4;
|
||||
utc_model.A1_6 = FNAV_A1_4;
|
||||
utc_model.Delta_tLS_6 = FNAV_deltatls_4;
|
||||
utc_model.t0t_6 = FNAV_t0t_4;
|
||||
utc_model.WNot_6 = FNAV_WNot_4;
|
||||
utc_model.WN_LSF_6 = FNAV_WNlsf_4;
|
||||
utc_model.DN_6 = FNAV_DN_4;
|
||||
utc_model.Delta_tLSF_6 = FNAV_deltatlsf_4;
|
||||
utc_model.flag_utc_model = flag_utc_model;
|
||||
/*GST*/
|
||||
//utc_model.WN_5 = WN_5; //Week number
|
||||
//utc_model.TOW_5 = WN_5; //Time of Week
|
||||
return utc_model;
|
||||
}
|
||||
|
||||
Galileo_Almanac Galileo_Fnav_Message::get_almanac()
|
||||
{
|
||||
Galileo_Almanac almanac;
|
||||
/*FNAV equivalent of INAV Word type 7: Almanac for SVID1 (1/2), almanac reference time and almanac reference week number*/
|
||||
almanac.IOD_a_7 = FNAV_IODa_5;
|
||||
almanac.WN_a_7 = FNAV_WNa_5;
|
||||
almanac.t0a_7 = FNAV_t0a_5;
|
||||
almanac.SVID1_7 = FNAV_SVID1_5;
|
||||
almanac.DELTA_A_7 = FNAV_Deltaa12_1_5;
|
||||
almanac.e_7 = FNAV_e_1_5;
|
||||
almanac.omega_7 = FNAV_w_1_5;
|
||||
almanac.delta_i_7 = FNAV_deltai_1_5;
|
||||
almanac.Omega0_7 = FNAV_Omega0_1_5;
|
||||
almanac.Omega_dot_7 = FNAV_Omegadot_1_5;
|
||||
almanac.M0_7 = FNAV_M0_1_5;
|
||||
|
||||
/*FNAV equivalent of INAV Word type 8: Almanac for SVID1 (2/2) and SVID2 (1/2)*/
|
||||
almanac.IOD_a_8 = FNAV_IODa_5;
|
||||
almanac.af0_8 = FNAV_af0_1_5;
|
||||
almanac.af1_8 = FNAV_af1_1_5;
|
||||
almanac.E5a_HS_8 = FNAV_E5ahs_1_5;
|
||||
almanac.SVID2_8 = FNAV_SVID2_5;
|
||||
almanac.DELTA_A_8 = FNAV_Deltaa12_2_5;
|
||||
almanac.e_8 = FNAV_e_2_5;
|
||||
almanac.omega_8 = FNAV_w_2_5;
|
||||
almanac.delta_i_8 = FNAV_deltai_2_5;
|
||||
almanac.Omega0_8 = FNAV_Omega0_2_6;
|
||||
almanac.Omega_dot_8 = FNAV_Omegadot_2_6;
|
||||
|
||||
/*FNAV equivalent of INAV Word type 9: Almanac for SVID2 (2/2) and SVID3 (1/2)*/
|
||||
almanac.IOD_a_9 = FNAV_IODa_6;
|
||||
almanac.WN_a_9 = FNAV_WNa_5;
|
||||
almanac.t0a_9 = FNAV_t0a_5;
|
||||
almanac.M0_9 = FNAV_M0_2_6;
|
||||
almanac.af0_9 = FNAV_af0_2_6;
|
||||
almanac.af1_9 = FNAV_af1_2_6;
|
||||
almanac.E5a_HS_9 = FNAV_E5ahs_2_6;
|
||||
almanac.SVID3_9 = FNAV_SVID3_6;
|
||||
almanac.DELTA_A_9 = FNAV_Deltaa12_3_6;
|
||||
almanac.e_9 = FNAV_e_3_6;
|
||||
almanac.omega_9 = FNAV_w_3_6;
|
||||
almanac.delta_i_9 = FNAV_deltai_3_6;
|
||||
|
||||
/*FNAV equivalent of INAV Word type 10: Almanac for SVID3 (2/2)*/
|
||||
almanac.IOD_a_10 = FNAV_IODa_6;
|
||||
almanac.Omega0_10 = FNAV_Omega0_3_6;
|
||||
almanac.Omega_dot_10 = FNAV_Omegadot_3_6;
|
||||
almanac.M0_10 = FNAV_M0_3_6;
|
||||
almanac.af0_10 = FNAV_af0_3_6;
|
||||
almanac.af1_10 = FNAV_af1_3_6;
|
||||
almanac.E5a_HS_10 = FNAV_E5ahs_3_6;
|
||||
return almanac;
|
||||
}
|
||||
212
src/core/system_parameters/galileo_fnav_message.h
Normal file
212
src/core/system_parameters/galileo_fnav_message.h
Normal file
|
|
@ -0,0 +1,212 @@
|
|||
/*!
|
||||
* \file galileo_fnav_message.h
|
||||
* \brief Implementation of a Galileo F/NAV Data message
|
||||
* as described in Galileo OS SIS ICD Issue 1.1 (Sept. 2010)
|
||||
* \author Marc Sales 2014. marcsales92(at)gmail.com
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2014 (see AUTHORS file for a list of contributors)
|
||||
*
|
||||
* GNSS-SDR is a software defined Global Navigation
|
||||
* Satellite Systems receiver
|
||||
*
|
||||
* This file is part of GNSS-SDR.
|
||||
*
|
||||
* GNSS-SDR is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* at your option) any later version.
|
||||
*
|
||||
* GNSS-SDR is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with GNSS-SDR. If not, see <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*/
|
||||
|
||||
#ifndef GNSS_SDR_GALILEO_FNAV_MESSAGE_H_
|
||||
#define GNSS_SDR_GALILEO_FNAV_MESSAGE_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
#include <bitset>
|
||||
#include <boost/assign.hpp>
|
||||
#include <boost/cstdint.hpp> // for boost::uint16_t
|
||||
#include <cmath>
|
||||
#include <utility>
|
||||
#include "galileo_ephemeris.h"
|
||||
#include "galileo_iono.h"
|
||||
#include "galileo_almanac.h"
|
||||
#include "galileo_utc_model.h"
|
||||
|
||||
/*!
|
||||
* \brief This class handles the Galileo F/NAV Data message, as described in the
|
||||
* Galileo Open Service Signal in Space Interface Control Document (OS SIS ICD), Issue 1.1 (Sept 2010).
|
||||
* See http://ec.europa.eu/enterprise/policies/satnav/galileo/files/galileo-os-sis-icd-issue1-revision1_en.pdf
|
||||
*/
|
||||
class Galileo_Fnav_Message
|
||||
{
|
||||
public:
|
||||
void Galileo_Fnav_Message::split_page(std::string page_string);
|
||||
void Galileo_Fnav_Message::reset();
|
||||
bool Galileo_Fnav_Message::have_new_ephemeris();
|
||||
bool Galileo_Fnav_Message::have_new_iono_and_GST();
|
||||
bool Galileo_Fnav_Message::have_new_utc_model();
|
||||
bool Galileo_Fnav_Message::have_new_almanac();
|
||||
Galileo_Ephemeris Galileo_Fnav_Message::get_ephemeris();
|
||||
Galileo_Iono Galileo_Fnav_Message::get_iono();
|
||||
Galileo_Utc_Model Galileo_Fnav_Message::get_utc_model();
|
||||
Galileo_Almanac Galileo_Fnav_Message::get_almanac();
|
||||
|
||||
Galileo_Fnav_Message();
|
||||
|
||||
bool flag_CRC_test;
|
||||
bool flag_all_ephemeris; //!< Flag indicating that all words containing ephemeris have been received
|
||||
bool flag_ephemeris_1; //!< Flag indicating that ephemeris 1/3 (word 2) have been received
|
||||
bool flag_ephemeris_2; //!< Flag indicating that ephemeris 2/3 (word 3) have been received
|
||||
bool flag_ephemeris_3; //!< Flag indicating that ephemeris 3/3 (word 4) have been received
|
||||
|
||||
bool flag_iono_and_GST; //!< Flag indicating that ionospheric and GST parameters (word 1) have been received
|
||||
bool flag_TOW_1;
|
||||
bool flag_TOW_2;
|
||||
bool flag_TOW_3;
|
||||
bool flag_TOW_4;
|
||||
bool flag_TOW_set; //!< it is true when page 1,2,3 or 4 arrives
|
||||
bool flag_utc_model; //!< Flag indicating that utc model parameters (word 4) have been received
|
||||
|
||||
bool flag_all_almanac; //!< Flag indicating that all almanac have been received
|
||||
bool flag_almanac_1; //!< Flag indicating that almanac 1/2 (word 5) have been received
|
||||
bool flag_almanac_2; //!< Flag indicating that almanac 2/2 (word 6) have been received
|
||||
|
||||
int IOD_ephemeris;
|
||||
|
||||
int page_type;
|
||||
/* WORD 1 SVID, Clock correction, SISA, Ionospheric correction, BGD, GST, Signal
|
||||
* health and Data validity status*/
|
||||
int FNAV_SV_ID_PRN_1;
|
||||
int FNAV_IODnav_1;
|
||||
double FNAV_t0c_1;
|
||||
double FNAV_af0_1;
|
||||
double FNAV_af1_1;
|
||||
double FNAV_af2_1;
|
||||
double FNAV_SISA_1;
|
||||
double FNAV_ai0_1;
|
||||
double FNAV_ai1_1;
|
||||
double FNAV_ai2_1;
|
||||
bool FNAV_region1_1;
|
||||
bool FNAV_region2_1;
|
||||
bool FNAV_region3_1;
|
||||
bool FNAV_region4_1;
|
||||
bool FNAV_region5_1;
|
||||
double FNAV_BGD_1;
|
||||
double FNAV_E5ahs_1;
|
||||
double FNAV_WN_1;
|
||||
double FNAV_TOW_1;
|
||||
double FNAV_E5advs_1;
|
||||
|
||||
// WORD 2 Ephemeris (1/3) and GST
|
||||
int FNAV_IODnav_2;
|
||||
double FNAV_M0_2;
|
||||
double FNAV_omegadot_2;
|
||||
double FNAV_e_2;
|
||||
double FNAV_a12_2;
|
||||
double FNAV_omega0_2;
|
||||
double FNAV_idot_2;
|
||||
double FNAV_WN_2;
|
||||
double FNAV_TOW_2;
|
||||
|
||||
// WORD 3 Ephemeris (2/3) and GST
|
||||
int FNAV_IODnav_3;
|
||||
double FNAV_i0_3;
|
||||
double FNAV_w_3;
|
||||
double FNAV_deltan_3;
|
||||
double FNAV_Cuc_3;
|
||||
double FNAV_Cus_3;
|
||||
double FNAV_Crc_3;
|
||||
double FNAV_Crs_3;
|
||||
double FNAV_t0e_3;
|
||||
double FNAV_WN_3;
|
||||
double FNAV_TOW_3;
|
||||
|
||||
/* WORD 4 Ephemeris (3/3), GST-UTC conversion, GST-GPS conversion and TOW.
|
||||
Note that the clock is repeated in this page type*/
|
||||
int FNAV_IODnav_4;
|
||||
double FNAV_Cic_4;
|
||||
double FNAV_Cis_4;
|
||||
double FNAV_A0_4;
|
||||
double FNAV_A1_4;
|
||||
double FNAV_deltatls_4;
|
||||
double FNAV_t0t_4;
|
||||
double FNAV_WNot_4;
|
||||
double FNAV_WNlsf_4;
|
||||
double FNAV_DN_4;
|
||||
double FNAV_deltatlsf_4;
|
||||
double FNAV_t0g_4;
|
||||
double FNAV_A0g_4;
|
||||
double FNAV_A1g_4;
|
||||
double FNAV_WN0g_4;
|
||||
double FNAV_TOW_4;
|
||||
|
||||
// WORD 5 Almanac (SVID1 and SVID2(1/2)), Week Number and almanac reference time
|
||||
int FNAV_IODa_5;
|
||||
double FNAV_WNa_5;
|
||||
double FNAV_t0a_5;
|
||||
int FNAV_SVID1_5;
|
||||
double FNAV_Deltaa12_1_5;
|
||||
double FNAV_e_1_5;
|
||||
double FNAV_w_1_5;
|
||||
double FNAV_deltai_1_5;
|
||||
double FNAV_Omega0_1_5;
|
||||
double FNAV_Omegadot_1_5;
|
||||
double FNAV_M0_1_5;
|
||||
double FNAV_af0_1_5;
|
||||
double FNAV_af1_1_5;
|
||||
double FNAV_E5ahs_1_5;
|
||||
int FNAV_SVID2_5;
|
||||
double FNAV_Deltaa12_2_5;
|
||||
double FNAV_e_2_5;
|
||||
double FNAV_w_2_5;
|
||||
double FNAV_deltai_2_5;
|
||||
|
||||
// WORD 6 Almanac (SVID2(2/2) and SVID3)
|
||||
int FNAV_IODa_6;
|
||||
double FNAV_Omega0_2_6;
|
||||
double FNAV_Omegadot_2_6;
|
||||
double FNAV_M0_2_6;
|
||||
double FNAV_af0_2_6;
|
||||
double FNAV_af1_2_6;
|
||||
double FNAV_E5ahs_2_6;
|
||||
int FNAV_SVID3_6;
|
||||
double FNAV_Deltaa12_3_6;
|
||||
double FNAV_e_3_6;
|
||||
double FNAV_w_3_6;
|
||||
double FNAV_deltai_3_6;
|
||||
double FNAV_Omega0_3_6;
|
||||
double FNAV_Omegadot_3_6;
|
||||
double FNAV_M0_3_6;
|
||||
double FNAV_af0_3_6;
|
||||
double FNAV_af1_3_6;
|
||||
double FNAV_E5ahs_3_6;
|
||||
|
||||
|
||||
|
||||
private:
|
||||
bool Galileo_Fnav_Message::CRC_test(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits,boost::uint32_t checksum);
|
||||
void Galileo_Fnav_Message::decode_page(std::string data);
|
||||
unsigned long int Galileo_Fnav_Message::read_navigation_unsigned(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int,int>> parameter);
|
||||
signed long int Galileo_Fnav_Message::read_navigation_signed(std::bitset<GALILEO_FNAV_DATA_FRAME_BITS> bits, const std::vector<std::pair<int,int>> parameter);
|
||||
|
||||
std::string omega0_1;
|
||||
//std::string omega0_2;
|
||||
//bool omega_flag;
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_GALILEO_FNAV_MESSAGE_H_ */
|
||||
Loading…
Reference in New Issue
Block a user