mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-10-31 15:23:04 +00:00
Code Issues Releases Wiki Activity

Merge branch 'next' of git+ssh://github.com/gnss-sdr/gnss-sdr into next

Browse Source 
# Please enter a commit message to explain why this merge is necessary,
# especially if it merges an updated upstream into a topic branch.
#
# Lines starting with '#' will be ignored, and an empty message aborts
# the commit.
This commit is contained in:
Carles Fernandez
2015-05-19 07:58:23 +02:00
parent 7dcffd3401 c23654f7a2
commit ed06936992
6 changed files with 507 additions and 365 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
conf/gnss-sdr_Hybrid_short.conf
View File

@@ -12,12 +12,26 @@ GNSS-SDR.internal_fs_hz=4000000
;######### CONTROL_THREAD CONFIG ############
ControlThread.wait_for_flowgraph=false
;######### SUPL RRLP GPS assistance configuration #####
GNSS-SDR.SUPL_gps_enabled=false
GNSS-SDR.SUPL_read_gps_assistance_xml=true
GNSS-SDR.SUPL_gps_ephemeris_server=supl.nokia.com
GNSS-SDR.SUPL_gps_ephemeris_port=7275
GNSS-SDR.SUPL_gps_acquisition_server=supl.google.com
GNSS-SDR.SUPL_gps_acquisition_port=7275
GNSS-SDR.SUPL_MCC=244
GNSS-SDR.SUPL_MNS=5
GNSS-SDR.SUPL_LAC=0x59e2
GNSS-SDR.SUPL_CI=0x31b0
;######### SIGNAL_SOURCE CONFIG ############
;#implementation: Use [File_Signal_Source] or [UHD_Signal_Source] or [GN3S_Signal_Source] (experimental)
SignalSource.implementation=File_Signal_Source
;#filename: path to file with the captured GNSS signal samples to be processed
SignalSource.filename=/datalogger/signals/CTTC/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN.dat
SignalSource.filename=../../../Documents/workspace/code2/trunk/data/2013_04_04_GNSS_SIGNAL_at_CTTC_SPAIN.dat
;#item_type: Type and resolution for each of the signal samples. Use only gr_complex in this version.
SignalSource.item_type=short

9
src/algorithms/telemetry_decoder/adapters/gps_l2_m_telemetry_decoder.cc
View File

@@ -38,6 +38,7 @@
#include "gps_iono.h"
#include "gps_utc_model.h"
#include "configuration_interface.h"
#include "concurrent_queue.h"
extern concurrent_queue<Gps_Ephemeris> global_gps_ephemeris_queue;
extern concurrent_queue<Gps_Iono> global_gps_iono_queue;
@@ -70,10 +71,10 @@ GpsL2MTelemetryDecoder::GpsL2MTelemetryDecoder(ConfigurationInterface* configura
telemetry_decoder_ = gps_l2_m_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_gps_ephemeris_queue);
telemetry_decoder_->set_iono_queue(&global_gps_iono_queue);
telemetry_decoder_->set_almanac_queue(&global_gps_almanac_queue);
telemetry_decoder_->set_utc_model_queue(&global_gps_utc_model_queue);
//telemetry_decoder_->set_ephemeris_queue(&global_gps_ephemeris_queue);
//telemetry_decoder_->set_iono_queue(&global_gps_iono_queue);
//telemetry_decoder_->set_almanac_queue(&global_gps_almanac_queue);
//telemetry_decoder_->set_utc_model_queue(&global_gps_utc_model_queue);
//decimation factor
int decimation_factor = configuration->property(role + ".decimation_factor", 1);

706
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.cc
View File

@@ -29,28 +29,25 @@
* -------------------------------------------------------------------------
*/
/*!
* \todo Clean this code and move the telemetry definitions to GPS_L1_CA system definitions file
*/
#include "gps_l2_m_telemetry_decoder_cc.h"
#include <iostream>
#include <sstream>
#include <boost/lexical_cast.hpp>
#include <gnuradio/io_signature.h>
#include <glog/logging.h>
#include <boost/lexical_cast.hpp>
#include "control_message_factory.h"
#include "gnss_synchro.h"
#ifndef _rotl
#define _rotl(X,N) ((X << N) ^ (X >> (32-N))) // Used in the parity check algorithm
#endif
#include "gps_l2_m_telemetry_decoder_cc.h"
using google::LogMessage;
/*!
* \todo name and move the magic numbers to GPS_L1_CA.h
*/
// logging levels
#define EVENT 2 // logs important events which don't occur every block
#define FLOW 3 // logs the function calls of block processing functions
#define SAMP_SYNC 4 // about 1 log entry per sample -> high output
#define LMORE 5 //
gps_l2_m_telemetry_decoder_cc_sptr
gps_l2_m_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)
@@ -61,16 +58,6 @@ gps_l2_m_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long
void gps_l2_m_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
for (unsigned i = 0; i < 3; i++)
{
ninput_items_required[i] = d_samples_per_bit * 8; //set the required sample history
}
}
gps_l2_m_telemetry_decoder_cc::gps_l2_m_telemetry_decoder_cc(
Gnss_Satellite satellite,
long if_freq,
@@ -79,283 +66,184 @@ gps_l2_m_telemetry_decoder_cc::gps_l2_m_telemetry_decoder_cc(
int vector_length,
boost::shared_ptr<gr::msg_queue> queue,
bool dump) :
gr::block("gps_navigation_cc", gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)),
gr::io_signature::make(1, 1, sizeof(Gnss_Synchro)))
gr::block("gps_l2_m_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());
d_vector_length = vector_length;
d_samples_per_bit = ( GPS_L1_CA_CODE_RATE_HZ / GPS_L1_CA_CODE_LENGTH_CHIPS ) / GPS_CA_TELEMETRY_RATE_BITS_SECOND;
LOG(INFO) << "GPS L2C M TELEMETRY PROCESSING: satellite " << d_satellite;
d_fs_in = fs_in;
//d_preamble_duration_seconds = (1.0 / GPS_CA_TELEMETRY_RATE_BITS_SECOND) * GPS_CA_PREAMBLE_LENGTH_BITS;
//std::cout<<"d_preamble_duration_seconds="<<d_preamble_duration_seconds<<"\r\n";
// set the preamble
unsigned short int preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS] = GPS_PREAMBLE;
memcpy((unsigned short int*)this->d_preambles_bits, (unsigned short int*)preambles_bits, GPS_CA_PREAMBLE_LENGTH_BITS*sizeof(unsigned short int));
// preamble bits to sampled symbols
d_preambles_symbols = (signed int*)malloc(sizeof(signed int) * GPS_CA_PREAMBLE_LENGTH_BITS * d_samples_per_bit);
int n = 0;
for (int i = 0; i < GPS_CA_PREAMBLE_LENGTH_BITS; i++)
{
for (unsigned int j = 0; j < d_samples_per_bit; j++)
{
if (d_preambles_bits[i] == 1)
{
d_preambles_symbols[n] = 1;
}
else
{
d_preambles_symbols[n] = -1;
}
n++;
}
}
d_sample_counter = 0;
//d_preamble_code_phase_seconds = 0;
d_stat = 0;
d_preamble_index = 0;
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 0;
d_preamble_time_seconds = 0;
d_flag_frame_sync = false;
d_GPS_frame_4bytes = 0;
d_prev_GPS_frame_4bytes = 0;
d_flag_parity = false;
d_TOW_at_Preamble = 0;
d_TOW_at_current_symbol = 0;
flag_TOW_set = false;
d_average_count = 0;
d_block_size = d_samples_per_symbol * d_symbols_per_bit * d_block_size_in_bits*2; // two CNAV frames
d_decimation_output_factor=0;
//set_output_multiple (1);
d_average_count=0;
d_flag_invert_buffer_symbols=false;
d_flag_invert_input_symbols=false;
d_channel=0;
//set_history(d_samples_per_bit*8); // At least a history of 8 bits are needed to correlate with the preamble
}
gps_l2_m_telemetry_decoder_cc::~gps_l2_m_telemetry_decoder_cc()
{
delete d_preambles_symbols;
d_dump_file.close();
}
bool gps_l2_m_telemetry_decoder_cc::gps_word_parityCheck(unsigned int gpsword)
void gps_l2_m_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
{
unsigned int d1, d2, d3, d4, d5, d6, d7, t, parity;
/* XOR as many bits in parallel as possible. The magic constants pick
up bits which are to be XOR'ed together to implement the GPS parity
check algorithm described in IS-GPS-200E. This avoids lengthy shift-
and-xor loops. */
d1 = gpsword & 0xFBFFBF00;
d2 = _rotl(gpsword,1) & 0x07FFBF01;
d3 = _rotl(gpsword,2) & 0xFC0F8100;
d4 = _rotl(gpsword,3) & 0xF81FFE02;
d5 = _rotl(gpsword,4) & 0xFC00000E;
d6 = _rotl(gpsword,5) & 0x07F00001;
d7 = _rotl(gpsword,6) & 0x00003000;
t = d1 ^ d2 ^ d3 ^ d4 ^ d5 ^ d6 ^ d7;
// Now XOR the 5 6-bit fields together to produce the 6-bit final result.
parity = t ^ _rotl(t,6) ^ _rotl(t,12) ^ _rotl(t,18) ^ _rotl(t,24);
parity = parity & 0x3F;
if (parity == (gpsword & 0x3F)) return(true);
else return(false);
unsigned ninputs = ninput_items_required.size ();
for (unsigned i = 0; i < ninputs; i++)
ninput_items_required[i] = noutput_items;
//LOG(INFO) << "forecast(): " << "noutput_items=" << noutput_items << "\tninput_items_required ninput_items_required.size()=" << ninput_items_required.size();
}
void gps_l2_m_telemetry_decoder_cc::set_decimation(int decimation)
{
d_decimation_output_factor = decimation;
}
int gps_l2_m_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;
//LOG(INFO) << "general_work(): " << "noutput_items=" << noutput_items << "\toutput_items real size=" << output_items.size() << "\tninput_items size=" << ninput_items.size() << "\tinput_items real size=" << input_items.size() << "\tninput_items[0]=" << ninput_items[0];
// get pointers on in- and output gnss-synchro objects
const Gnss_Synchro *in = (const Gnss_Synchro *) input_items[0]; // input
Gnss_Synchro *out = (Gnss_Synchro *) output_items[0]; // output
Gnss_Synchro **out = (Gnss_Synchro **) &output_items[0];
d_sample_counter++; //count for the processed samples
// store the time stamp of the first sample in the processed sample block
double sample_stamp = in[0].Tracking_timestamp_secs;
// ########### Output the tracking data to navigation and PVT ##########
const Gnss_Synchro **in = (const Gnss_Synchro **) &input_items[0]; //Get the input samples pointer
// copy correlation samples into samples vector
//for (int i = 0; i < noutput_items; i++)
/// {
// check if channel is in tracking state
// {
d_sample_buf.push_back(in[0].Prompt_I);
// }
// }
// TODO Optimize me!
//******* preamble correlation ********
for (unsigned int i = 0; i < d_samples_per_bit*8; i++)
{
if (in[0][i].Prompt_I < 0) // symbols clipping
{
corr_value -= d_preambles_symbols[i];
}
else
{
corr_value += d_preambles_symbols[i];
}
}
d_flag_preamble = false;
//******* frame sync ******************
if (abs(corr_value) >= 160)
{
//TODO: Rewrite with state machine
if (d_stat == 0)
{
d_GPS_FSM.Event_gps_word_preamble();
d_preamble_index = d_sample_counter;//record the preamble sample stamp
LOG(INFO) << "Preamble detection for SAT " << this->d_satellite;
d_symbol_accumulator = 0; //sync the symbol to bits integrator
d_symbol_accumulator_counter = 0;
d_frame_bit_index = 8;
d_stat = 1; // enter into frame pre-detection status
}
else if (d_stat == 1) //check 6 seconds of preamble separation
{
preamble_diff = d_sample_counter - d_preamble_index;
if (abs(preamble_diff - 6000) < 1)
{
d_GPS_FSM.Event_gps_word_preamble();
d_flag_preamble = true;
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
{
if (d_stat == 1)
{
preamble_diff = d_sample_counter - d_preamble_index;
if (preamble_diff > 6001)
{
LOG(INFO) << "Lost of frame sync SAT " << this->d_satellite << " preamble_diff= " << preamble_diff;
d_stat = 0; //lost of frame sync
d_flag_frame_sync = false;
flag_TOW_set = false;
}
}
}
//******* SYMBOL TO BIT *******
d_symbol_accumulator += in[0][d_samples_per_bit*8 - 1].Prompt_I; // accumulate the input value in d_symbol_accumulator
d_symbol_accumulator_counter++;
if (d_symbol_accumulator_counter == 20)
{
if (d_symbol_accumulator > 0)
{ //symbol to bit
d_GPS_frame_4bytes += 1; //insert the telemetry bit in LSB
}
d_symbol_accumulator = 0;
d_symbol_accumulator_counter = 0;
//******* bits to words ******
d_frame_bit_index++;
if (d_frame_bit_index == 30)
{
d_frame_bit_index = 0;
// parity check
// Each word in wordbuff is composed of:
// Bits 0 to 29 = the GPS data word
// Bits 30 to 31 = 2 LSBs of the GPS word ahead.
// prepare the extended frame [-2 -1 0 ... 30]
if (d_prev_GPS_frame_4bytes & 0x00000001)
{
d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x40000000;
}
if (d_prev_GPS_frame_4bytes & 0x00000002)
{
d_GPS_frame_4bytes = d_GPS_frame_4bytes | 0x80000000;
}
/* Check that the 2 most recently logged words pass parity. Have to first
invert the data bits according to bit 30 of the previous word. */
if(d_GPS_frame_4bytes & 0x40000000)
{
d_GPS_frame_4bytes ^= 0x3FFFFFC0; // invert the data bits (using XOR)
}
if (gps_l2_m_telemetry_decoder_cc::gps_word_parityCheck(d_GPS_frame_4bytes))
{
memcpy(&d_GPS_FSM.d_GPS_frame_4bytes, &d_GPS_frame_4bytes, sizeof(char)*4);
d_GPS_FSM.d_preamble_time_ms = d_preamble_time_seconds*1000.0;
d_GPS_FSM.Event_gps_word_valid();
d_flag_parity = true;
}
else
{
d_GPS_FSM.Event_gps_word_invalid();
d_flag_parity = false;
}
d_prev_GPS_frame_4bytes = d_GPS_frame_4bytes; // save the actual frame
d_GPS_frame_4bytes = d_GPS_frame_4bytes & 0;
}
else
{
d_GPS_frame_4bytes <<= 1; //shift 1 bit left the telemetry word
}
}
// output the frame
consume_each(1); //one by one
// decode only if enough samples in buffer
if(d_sample_buf.size() >= d_block_size)
{
d_flag_invert_buffer_symbols=false;
while (true)
{
if (d_flag_invert_buffer_symbols==true)
{
for (std::vector<double>::iterator symbol_it = d_sample_buf.begin(); symbol_it != d_sample_buf.end(); symbol_it++)
{
*symbol_it = -(*symbol_it);
}
LOG(INFO)<<"Inverting buffer symbols";
}
//debug
std::stringstream ss2;
for (std::vector<double>::const_iterator symbol_it = d_sample_buf.begin(); symbol_it < d_sample_buf.end(); ++symbol_it)
{
if(*symbol_it>=0)
{
ss2<<'1';
}else{
ss2<<'0';
}
}
LOG(INFO)<<"get_symbols="<<ss2.str();
// align symbols in pairs
// and obtain the bits by decoding the symbols (viterbi decoder)
// they can be already aligned or shifted by one position
std::vector<int> bits;
bool symbol_alignment = d_symbol_aligner_and_decoder.get_bits(d_sample_buf, bits);
std::stringstream ss;
for (std::vector<int>::const_iterator bit_it = bits.begin(); bit_it < bits.end(); ++bit_it)
{
ss << *bit_it;
}
LOG(INFO)<<"get_bits="<<ss.str()<<std::endl;
// search for preambles
// and extract the corresponding message candidates
std::vector<msg_candiate_int_t> msg_candidates;
d_frame_detector.get_frame_candidates(bits, msg_candidates);
// verify checksum
// and return the valid messages
std::vector<msg_candiate_char_t> valid_msgs;
d_crc_verifier.get_valid_frames(msg_candidates, valid_msgs);
if (valid_msgs.size()==0)
{
if (d_flag_invert_buffer_symbols==false)
{
d_flag_invert_buffer_symbols=true;
}else{//already tested the symbol inversion but CRC still fail
LOG(INFO)<<"Discarting this buffer, no CNAV frames detected";
break;
}
}else{ //at least one frame has good CRC, keep the invert sign for the next frames
d_flag_invert_input_symbols=d_flag_invert_buffer_symbols;
LOG(INFO)<<valid_msgs.size()<<" GOOD L2C CNAV FRAME DETECTED!";
break;
}
}
// // compute message sample stamp
// // and fill messages in SBAS raw message objects
// std::vector<Sbas_Raw_Msg> sbas_raw_msgs;
// for(std::vector<msg_candiate_char_t>::const_iterator it = valid_msgs.begin();
// it != valid_msgs.end(); ++it)
// {
// int message_sample_offset =
// (sample_alignment ? 0 : -1)
// + d_samples_per_symbol*(symbol_alignment ? -1 : 0)
// + d_samples_per_symbol * d_symbols_per_bit * it->first;
// double message_sample_stamp = sample_stamp + ((double)message_sample_offset)/1000;
// VLOG(EVENT) << "message_sample_stamp=" << message_sample_stamp
// << " (sample_stamp=" << sample_stamp
// << " sample_alignment=" << sample_alignment
// << " symbol_alignment=" << symbol_alignment
// << " relative_preamble_start=" << it->first
// << " message_sample_offset=" << message_sample_offset
// << ")";
// Sbas_Raw_Msg sbas_raw_msg(message_sample_stamp, this->d_satellite.get_PRN(), it->second);
// sbas_raw_msgs.push_back(sbas_raw_msg);
// }
//
// // parse messages
// // and send them to the SBAS raw message queue
// for(std::vector<Sbas_Raw_Msg>::iterator it = sbas_raw_msgs.begin(); it != sbas_raw_msgs.end(); it++)
// {
// std::cout << "SBAS message type " << it->get_msg_type() << " from PRN" << it->get_prn() << " received" << std::endl;
// sbas_telemetry_data.update(*it);
// }
// clear all processed samples in the input buffer
d_sample_buf.clear();
}
// 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_GPS_FSM.d_nav.d_TOW > 0)
//update TOW at the preamble instant (todo: check for valid d_TOW)
// JAVI: 30/06/2014
// TOW, in GPS, is referred to the START of the SUBFRAME, that is, THE FIRST SYMBOL OF THAT SUBFRAME, NOT THE PREAMBLE.
// thus, no correction should be done. d_TOW_at_Preamble should be renamed to d_TOW_at_subframe_start.
// Sice we detected the preable, then, we are in the last symbol of that preamble, or just at the start of the first subframe symbol.
{
d_TOW_at_Preamble = d_GPS_FSM.d_nav.d_TOW + GPS_SUBFRAME_SECONDS; //we decoded the current TOW when the last word of the subframe arrive, so, we have a lag of ONE SUBFRAME
d_TOW_at_current_symbol = d_TOW_at_Preamble;//GPS_L1_CA_CODE_PERIOD;// + (double)GPS_CA_PREAMBLE_LENGTH_BITS/(double)GPS_CA_TELEMETRY_RATE_BITS_SECOND;
Prn_timestamp_at_preamble_ms = in[0][0].Tracking_timestamp_secs * 1000.0;
if (flag_TOW_set == false)
{
flag_TOW_set = true;
}
}
else
{
d_TOW_at_current_symbol = d_TOW_at_current_symbol + GPS_L1_CA_CODE_PERIOD;
}
current_synchro_data.d_TOW = d_TOW_at_Preamble;
current_synchro_data.d_TOW_at_current_symbol = d_TOW_at_current_symbol;
current_synchro_data.d_TOW_hybrid_at_current_symbol = current_synchro_data.d_TOW_at_current_symbol; // to be used in the hybrid configuration
current_synchro_data.Flag_valid_word = (d_flag_frame_sync == true and d_flag_parity == true and flag_TOW_set == true);
current_synchro_data.Flag_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 (std::ifstream::failure e)
{
LOG(WARNING) << "Exception writing observables dump file " << e.what();
}
}
//todo: implement averaging
//1. Copy the current tracking output
current_synchro_data = in[0];
//2. Add the telemetry decoder information
current_synchro_data.Flag_valid_word = false; // indicate to observable block that this synchro object isn't valid for pseudorange computation
d_average_count++;
if (d_average_count == d_decimation_output_factor)
{
d_average_count = 0;
//3. Make the output (copy the object contents to the GNURadio reserved memory)
*out[0] = current_synchro_data;
out[0] = current_synchro_data;
//std::cout<<"GPS L2 TLM output on CH="<<this->d_channel << " SAMPLE STAMP="<<d_sample_counter/d_decimation_output_factor<<std::endl;
return 1;
}
@@ -366,45 +254,275 @@ int gps_l2_m_telemetry_decoder_cc::general_work (int noutput_items, gr_vector_in
}
void gps_l2_m_telemetry_decoder_cc::set_decimation(int decimation)
{
d_decimation_output_factor = decimation;
}
void gps_l2_m_telemetry_decoder_cc::set_satellite(Gnss_Satellite satellite)
{
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
LOG(INFO) << "Setting decoder Finite State Machine to satellite " << d_satellite;
d_GPS_FSM.i_satellite_PRN = d_satellite.get_PRN();
DLOG(INFO) << "Navigation Satellite set to " << d_satellite;
LOG(INFO) << "GPS L2C CNAV telemetry decoder in channel " << this->d_channel << " set to satellite " << d_satellite;
}
void gps_l2_m_telemetry_decoder_cc::set_channel(int channel)
{
d_channel = channel;
d_GPS_FSM.i_channel_ID = channel;
DLOG(INFO) << "Navigation channel set to " << channel;
// ############# ENABLE DATA FILE LOG #################
if (d_dump == true)
LOG(INFO) << "GPS L2C CNAV channel set to " << channel;
}
// ### helper class for symbol alignment and viterbi decoding ###
gps_l2_m_telemetry_decoder_cc::symbol_aligner_and_decoder::symbol_aligner_and_decoder()
{
// convolutional code properties
d_KK = 7;
int nn = 2;
int g_encoder[nn];
g_encoder[0] = 121; //171o
g_encoder[1] = 91; //133o
d_vd1 = new Viterbi_Decoder(g_encoder, d_KK, nn);
d_vd2 = new Viterbi_Decoder(g_encoder, d_KK, nn);
d_past_symbol = 0;
}
gps_l2_m_telemetry_decoder_cc::symbol_aligner_and_decoder::~symbol_aligner_and_decoder()
{
delete d_vd1;
delete d_vd2;
}
void gps_l2_m_telemetry_decoder_cc::symbol_aligner_and_decoder::reset()
{
d_past_symbol = 0;
d_vd1->reset();
d_vd2->reset();
}
bool gps_l2_m_telemetry_decoder_cc::symbol_aligner_and_decoder::get_bits(const std::vector<double> symbols, std::vector<int> &bits)
{
const int traceback_depth = 5*d_KK;
int nbits_requested = symbols.size()/d_symbols_per_bit;
int nbits_decoded;
// fill two vectors with the two possible symbol alignments
std::vector<double> symbols_vd1(symbols); // aligned symbol vector -> copy input symbol vector
std::vector<double> symbols_vd2; // shifted symbol vector -> add past sample in front of input vector
symbols_vd2.push_back(d_past_symbol);
for (std::vector<double>::const_iterator symbol_it = symbols.begin(); symbol_it != symbols.end() - 1; ++symbol_it)
{
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 (std::ifstream::failure e)
{
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what();
}
symbols_vd2.push_back(*symbol_it);
}
// arrays for decoded bits
int * bits_vd1 = new int[nbits_requested];
int * bits_vd2 = new int[nbits_requested];
// decode
float metric_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1, nbits_requested, nbits_decoded);
float metric_vd2 = d_vd2->decode_continuous(symbols_vd2.data(), traceback_depth, bits_vd2, nbits_requested, nbits_decoded);
LOG(INFO)<<"metric_vd1="<<metric_vd1<<" metric_vd2="<<metric_vd2;
// choose the bits with the better metric
for (int i = 0; i < nbits_decoded; i++)
{
if (metric_vd1 > metric_vd2)
{// symbols aligned
bits.push_back(bits_vd1[i]);
}
else
{// symbols shifted
bits.push_back(bits_vd2[i]);
}
}
d_past_symbol = symbols.back();
delete[] bits_vd1;
delete[] bits_vd2;
return metric_vd1 > metric_vd2;
}
// ### helper class for detecting the preamble and collect the corresponding message candidates ###
void gps_l2_m_telemetry_decoder_cc::frame_detector::reset()
{
d_buffer.clear();
}
void gps_l2_m_telemetry_decoder_cc::frame_detector::get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std::vector<int>>> &msg_candidates)
{
std::stringstream ss;
unsigned int sbas_msg_length = 300;
std::vector<std::vector<int>> preambles = {{1, 0, 0, 0, 1, 0, 1 ,1}};
//LOG(INFO) << "get_frame_candidates(): " << "d_buffer.size()=" << d_buffer.size() << "\tbits.size()=" << bits.size();
ss << "copy bits ";
int count = 0;
// copy new bits into the working buffer
for (std::vector<int>::const_iterator bit_it = bits.begin(); bit_it < bits.end(); ++bit_it)
{
d_buffer.push_back(*bit_it);
ss << *bit_it;
count++;
}
//LOG(INFO) << ss.str() << " into working buffer (" << count << " bits)";
int relative_preamble_start = 0;
while(d_buffer.size() >= sbas_msg_length)
{
// compare with all preambles
for (std::vector<std::vector<int>>::iterator preample_it = preambles.begin(); preample_it < preambles.end(); ++preample_it)
{
bool preamble_detected = true;
bool inv_preamble_detected = true;
// compare the buffer bits with the preamble bits
for (std::vector<int>::iterator preample_bit_it = preample_it->begin(); preample_bit_it < preample_it->end(); ++preample_bit_it)
{
preamble_detected = *preample_bit_it == d_buffer[preample_bit_it - preample_it->begin()] ? preamble_detected : false ;
inv_preamble_detected = *preample_bit_it != d_buffer[preample_bit_it - preample_it->begin()] ? inv_preamble_detected : false ;
}
if (preamble_detected || inv_preamble_detected)
{
// copy candidate
std::vector<int> candidate;
std::copy(d_buffer.begin(), d_buffer.begin() + sbas_msg_length, std::back_inserter(candidate));
if(inv_preamble_detected)
{
// invert bits
for (std::vector<int>::iterator candidate_bit_it = candidate.begin(); candidate_bit_it != candidate.end(); candidate_bit_it++)
*candidate_bit_it = *candidate_bit_it == 0 ? 1 : 0;
}
msg_candidates.push_back(std::pair<int,std::vector<int>>(relative_preamble_start, candidate));
ss.str("");
ss << "preamble " << preample_it - preambles.begin() << (inv_preamble_detected?" inverted":" normal") << " detected! candidate=";
for (std::vector<int>::iterator bit_it = candidate.begin(); bit_it < candidate.end(); ++bit_it)
ss << *bit_it;
//LOG(INFO) << ss.str();
}
}
relative_preamble_start++;
// remove bit in front
d_buffer.pop_front();
}
}
// ### helper class for checking the CRC of the message candidates ###
void gps_l2_m_telemetry_decoder_cc::crc_verifier::reset()
{
}
void gps_l2_m_telemetry_decoder_cc::crc_verifier::get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs)
{
std::stringstream ss;
LOG(INFO) << "get_valid_frames(): " << "msg_candidates.size()=" << msg_candidates.size();
// for each candidate
for (std::vector<msg_candiate_int_t>::const_iterator candidate_it = msg_candidates.begin(); candidate_it < msg_candidates.end(); ++candidate_it)
{
// convert to bytes
std::vector<unsigned char> candidate_bytes;
zerropad_back_and_convert_to_bytes(candidate_it->second, candidate_bytes);
// verify CRC
d_checksum_agent.reset(0);
d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size());
unsigned int crc = d_checksum_agent.checksum();
LOG(INFO) << "candidate " << ": final crc remainder= " << std::hex << crc
<< std::setfill(' ') << std::resetiosflags(std::ios::hex);
// the final remainder must be zero for a valid message, because the CRC is done over the received CRC value
if (crc == 0)
{
valid_msgs.push_back(msg_candiate_char_t(candidate_it->first, candidate_bytes));
ss << "Valid message found!";
}
else
{
ss << "Not a valid message.";
}
//ss << " Relbitoffset=" << candidate_it->first << " content=";
for (std::vector<unsigned char>::iterator byte_it = candidate_bytes.begin(); byte_it < candidate_bytes.end(); ++byte_it)
{
ss << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)(*byte_it);
}
LOG(INFO) << ss.str() << std::setfill(' ') << std::resetiosflags(std::ios::hex) << std::endl;
}
}
void gps_l2_m_telemetry_decoder_cc::crc_verifier::zerropad_back_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes)
{
std::stringstream ss;
const size_t bits_per_byte = 8;
unsigned char byte = 0;
//LOG(INFO) << "zerropad_back_and_convert_to_bytes():" << byte;
for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
{
int idx_bit = candidate_bit_it - msg_candidate.begin();
int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte - 1)
{
bytes.push_back(byte);
//LOG(INFO) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
byte = 0;
}
}
bytes.push_back(byte); // implies: insert 6 zeros at the end to fit the 250bits into a multiple of bytes
LOG(INFO) << " -> byte=" << std::setw(2)
<< std::setfill('0') << std::hex << (unsigned int)byte
<< std::setfill(' ') << std::resetiosflags(std::ios::hex);
}
void gps_l2_m_telemetry_decoder_cc::crc_verifier::zerropad_front_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes)
{
std::stringstream ss;
const size_t bits_per_byte = 8;
unsigned char byte = 0;
int idx_bit = 6; // insert 6 zeros at the front to fit the 250bits into a multiple of bytes
LOG(INFO) << "zerropad_front_and_convert_to_bytes():" << byte;
for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
{
int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte - 1)
{
bytes.push_back(byte);
LOG(INFO) << ss.str() << " -> byte=" << std::setw(2)
<< std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
byte = 0;
}
idx_bit++;
}
LOG(INFO) << " -> byte=" << std::setw(2)
<< std::setfill('0') << std::hex << (unsigned int)byte
<< std::setfill(' ') << std::resetiosflags(std::ios::hex);
}
//
//void gps_l2_m_telemetry_decoder_cc::set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue)
//{
// sbas_telemetry_data.set_raw_msg_queue(raw_msg_queue);
//}
//
//
//void gps_l2_m_telemetry_decoder_cc::set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue)
//{
// sbas_telemetry_data.set_iono_queue(iono_queue);
//}
//
//
//void gps_l2_m_telemetry_decoder_cc::set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue)
//{
// sbas_telemetry_data.set_sat_corr_queue(sat_corr_queue);
//}
//
//
//void gps_l2_m_telemetry_decoder_cc::set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue)
//{
// sbas_telemetry_data.set_ephemeris_queue(ephemeris_queue);
//}

134
src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2_m_telemetry_decoder_cc.h
View File

@@ -31,16 +31,18 @@
#ifndef GNSS_SDR_GPS_L2_M_TELEMETRY_DECODER_CC_H
#define GNSS_SDR_GPS_L2_M_TELEMETRY_DECODER_CC_H
#include <algorithm> // for copy
#include <deque>
#include <fstream>
#include <string>
#include <utility> // for pair
#include <vector>
#include <boost/crc.hpp>
#include <gnuradio/block.h>
#include <gnuradio/msg_queue.h>
#include "GPS_L1_CA.h"
#include "gps_l1_ca_subframe_fsm.h"
#include "concurrent_queue.h"
#include "gnss_satellite.h"
#include "viterbi_decoder.h"
//#include "sbas_telemetry_data.h"
class gps_l2_m_telemetry_decoder_cc;
@@ -51,7 +53,7 @@ gps_l2_m_make_telemetry_decoder_cc(Gnss_Satellite satellite, long if_freq, long
int vector_length, boost::shared_ptr<gr::msg_queue> queue, bool dump);
/*!
* \brief This class implements a block that decodes the NAV data defined in IS-GPS-200E
* \brief This class implements a block that decodes the SBAS integrity and corrections data defined in RTCA MOPS DO-229
*
*/
class gps_l2_m_telemetry_decoder_cc : public gr::block
@@ -61,20 +63,12 @@ public:
void set_satellite(Gnss_Satellite satellite); //!< Set satellite PRN
void set_channel(int channel); //!< Set receiver's channel
/*!
* \brief Set decimation factor to average the GPS synchronization estimation output from the tracking module.
*/
// queues to communicate broadcasted SBAS data to other blocks of GNSS-SDR
//void set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue); //!< Set raw msg queue
//void set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue); //!< Set iono queue
//void set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue); //!< Set sat correction queue
//void set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue); //!< Set SBAS ephemeis queue
void set_decimation(int decimation);
/*!
* \brief Set the satellite data queue
*/
void set_ephemeris_queue(concurrent_queue<Gps_Ephemeris> *ephemeris_queue){d_GPS_FSM.d_ephemeris_queue = ephemeris_queue;} //!< Set the ephemeris data queue
void set_iono_queue(concurrent_queue<Gps_Iono> *iono_queue){d_GPS_FSM.d_iono_queue = iono_queue;} //!< Set the iono data queue
void set_almanac_queue(concurrent_queue<Gps_Almanac> *almanac_queue){d_GPS_FSM.d_almanac_queue = almanac_queue;} //!< Set the almanac data queue
void set_utc_model_queue(concurrent_queue<Gps_Utc_Model> *utc_model_queue){d_GPS_FSM.d_utc_model_queue = utc_model_queue;} //!< Set the UTC model data queue
/*!
* \brief This is where all signal processing takes place
*/
@@ -91,62 +85,78 @@ private:
friend gps_l2_m_telemetry_decoder_cc_sptr
gps_l2_m_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);
gps_l2_m_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);
bool gps_word_parityCheck(unsigned int gpsword);
void viterbi_decoder(double *page_part_symbols, int *page_part_bits);
void align_samples();
// constants
unsigned short int d_preambles_bits[GPS_CA_PREAMBLE_LENGTH_BITS];
// class private vars
signed int *d_preambles_symbols;
unsigned int d_samples_per_bit;
long unsigned int d_sample_counter;
long unsigned int d_preamble_index;
unsigned int d_stat;
bool d_flag_frame_sync;
// symbols
double d_symbol_accumulator;
short int d_symbol_accumulator_counter;
//bits and frame
unsigned short int d_frame_bit_index;
unsigned int d_GPS_frame_4bytes;
unsigned int d_prev_GPS_frame_4bytes;
bool d_flag_parity;
bool d_flag_preamble;
int d_word_number;
// output averaging and decimation
int d_average_count;
int d_decimation_output_factor;
static const int d_samples_per_symbol = 1;
static const int d_symbols_per_bit = 2;
static const int d_block_size_in_bits = 300;
long d_fs_in;
//double d_preamble_duration_seconds;
// navigation message vars
Gps_Navigation_Message d_nav;
GpsL1CaSubframeFsm d_GPS_FSM;
boost::shared_ptr<gr::msg_queue> d_queue;
unsigned int d_vector_length;
bool d_dump;
Gnss_Satellite d_satellite;
int d_channel;
//std::deque<double> d_prn_start_sample_history;
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;
bool d_flag_invert_input_symbols;
bool d_flag_invert_buffer_symbols;
int d_decimation_output_factor;
int d_average_count;
size_t d_block_size; //!< number of samples which are processed during one invocation of the algorithms
std::vector<double> d_sample_buf; //!< input buffer holding the samples to be processed in one block
typedef std::pair<int,std::vector<int>> msg_candiate_int_t;
typedef std::pair<int,std::vector<unsigned char>> msg_candiate_char_t;
// helper class for symbol alignment and Viterbi decoding
class symbol_aligner_and_decoder
{
public:
symbol_aligner_and_decoder();
~symbol_aligner_and_decoder();
void reset();
bool get_bits(const std::vector<double> symbols, std::vector<int> &bits);
private:
int d_KK;
Viterbi_Decoder * d_vd1;
Viterbi_Decoder * d_vd2;
double d_past_symbol;
} d_symbol_aligner_and_decoder;
// helper class for detecting the preamble and collect the corresponding message candidates
class frame_detector
{
public:
void reset();
void get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int, std::vector<int>>> &msg_candidates);
private:
std::deque<int> d_buffer;
} d_frame_detector;
// helper class for checking the CRC of the message candidates
class crc_verifier
{
public:
void reset();
void get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs);
private:
typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type;
crc_24_q_type d_checksum_agent;
void zerropad_front_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes);
void zerropad_back_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes);
} d_crc_verifier;
//Sbas_Telemetry_Data sbas_telemetry_data;
};
#endif

2
src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.cc
View File

@@ -327,7 +327,7 @@ bool sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::get_bits(const st
// fill two vectors with the two possible symbol alignments
std::vector<double> symbols_vd1(symbols); // aligned symbol vector -> copy input symbol vector
std::vector<double> symbols_vd2; // shifted symbol vector -> add past sample in front of input vector
symbols_vd1.push_back(d_past_symbol);
symbols_vd2.push_back(d_past_symbol);
for (std::vector<double>::const_iterator symbol_it = symbols.begin(); symbol_it != symbols.end() - 1; ++symbol_it)
{
symbols_vd2.push_back(*symbol_it);

5
src/algorithms/tracking/gnuradio_blocks/gps_l2_m_dll_pll_tracking_cc.cc
View File

@@ -222,7 +222,6 @@ void gps_l2_m_dll_pll_tracking_cc::start_tracking()
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;
@@ -615,7 +614,7 @@ int gps_l2_m_dll_pll_tracking_cc::general_work (int noutput_items, gr_vector_int
tmp_double = static_cast<double>(d_sample_counter + d_current_prn_length_samples);
d_dump_file.write(reinterpret_cast<char*>(&tmp_double), sizeof(double));
}
catch (std::ifstream::failure e)
catch (std::ifstream::failure& e)
{
LOG(WARNING) << "Exception writing trk dump file " << e.what();
}
@@ -645,7 +644,7 @@ void gps_l2_m_dll_pll_tracking_cc::set_channel(unsigned int channel)
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() << std::endl;
}
catch (std::ifstream::failure e)
catch (std::ifstream::failure& e)
{
LOG(WARNING) << "channel " << d_channel << " Exception opening trk dump file " << e.what() << std::endl;
}