mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-06-24 22:13:15 +00:00
SBAS stuff developed by Daniel Fehr during GSOC 2013
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@436 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez
parent
a8619337be
commit
601e5fa2c9
|
|
@ -19,6 +19,7 @@
|
|||
set(TELEMETRY_DECODER_ADAPTER_SOURCES
|
||||
gps_l1_ca_telemetry_decoder.cc
|
||||
galileo_e1b_telemetry_decoder.cc
|
||||
sbas_l1_telemetry_decoder.cc
|
||||
)
|
||||
|
||||
include_directories(
|
||||
|
|
|
|||
|
|
@ -0,0 +1,118 @@
|
|||
/*!
|
||||
* \file sbas_l1_telemetry_decoder.cc
|
||||
* \brief Implementation of an adapter of a SBAS telemtry data decoder block
|
||||
* to a TelemetryDecoderInterface
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 "sbas_l1_telemetry_decoder.h"
|
||||
#include "configuration_interface.h"
|
||||
#include "sbas_l1_telemetry_decoder_cc.h"
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include "sbas_telemetry_data.h"
|
||||
#include "sbas_ionospheric_correction.h"
|
||||
#include "sbas_satellite_correction.h"
|
||||
#include "sbas_ephemeris.h"
|
||||
|
||||
extern concurrent_queue<Sbas_Raw_Msg> global_sbas_raw_msg_queue;
|
||||
extern concurrent_queue<Sbas_Ionosphere_Correction> global_sbas_iono_queue;
|
||||
extern concurrent_queue<Sbas_Satellite_Correction> global_sbas_sat_corr_queue;
|
||||
extern concurrent_queue<Sbas_Ephemeris> global_sbas_ephemeris_queue;
|
||||
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
SbasL1TelemetryDecoder::SbasL1TelemetryDecoder(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_ = sbas_l1_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 queues;
|
||||
telemetry_decoder_->set_raw_msg_queue(&global_sbas_raw_msg_queue);
|
||||
telemetry_decoder_->set_iono_queue(&global_sbas_iono_queue);
|
||||
telemetry_decoder_->set_sat_corr_queue(&global_sbas_sat_corr_queue);
|
||||
telemetry_decoder_->set_ephemeris_queue(&global_sbas_ephemeris_queue);
|
||||
}
|
||||
|
||||
|
||||
SbasL1TelemetryDecoder::~SbasL1TelemetryDecoder()
|
||||
{}
|
||||
|
||||
|
||||
void SbasL1TelemetryDecoder::set_satellite(Gnss_Satellite satellite)
|
||||
{
|
||||
satellite_ = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
telemetry_decoder_->set_satellite(satellite_);
|
||||
DLOG(INFO) << "SBAS TELEMETRY DECODER: satellite set to " << satellite_;
|
||||
}
|
||||
|
||||
|
||||
void SbasL1TelemetryDecoder::connect(gr::top_block_sptr top_block)
|
||||
{
|
||||
// Nothing to connect internally
|
||||
DLOG(INFO) << "nothing to connect internally";
|
||||
}
|
||||
|
||||
|
||||
void SbasL1TelemetryDecoder::disconnect(gr::top_block_sptr top_block)
|
||||
{
|
||||
// Nothing to disconnect
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr SbasL1TelemetryDecoder::get_left_block()
|
||||
{
|
||||
return telemetry_decoder_;
|
||||
}
|
||||
|
||||
|
||||
gr::basic_block_sptr SbasL1TelemetryDecoder::get_right_block()
|
||||
{
|
||||
return telemetry_decoder_;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,94 @@
|
|||
/*!
|
||||
* \file sbas_l1_telemetry_decoder.h
|
||||
* \brief Interface of an adapter of a SBAS telemtry data decoder block
|
||||
* to a TelemetryDecoderInterface
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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_SBAS_L1_TELEMETRY_DECODER_H_
|
||||
#define GNSS_SDR_SBAS_L1_TELEMETRY_DECODER_H_
|
||||
|
||||
#include "telemetry_decoder_interface.h"
|
||||
#include "sbas_l1_telemetry_decoder_cc.h"
|
||||
#include <gnuradio/msg_queue.h>
|
||||
|
||||
|
||||
class ConfigurationInterface;
|
||||
|
||||
/*!
|
||||
* \brief This class implements a NAV data decoder for SBAS frames in L1 radio link
|
||||
*/
|
||||
class SbasL1TelemetryDecoder : public TelemetryDecoderInterface
|
||||
{
|
||||
public:
|
||||
SbasL1TelemetryDecoder(ConfigurationInterface* configuration,
|
||||
std::string role,
|
||||
unsigned int in_streams,
|
||||
unsigned int out_streams,
|
||||
boost::shared_ptr<gr::msg_queue> queue);
|
||||
|
||||
virtual ~SbasL1TelemetryDecoder();
|
||||
std::string role()
|
||||
{
|
||||
return role_;
|
||||
}
|
||||
|
||||
std::string implementation()
|
||||
{
|
||||
return "SBAS_L1_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:
|
||||
sbas_l1_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
|
||||
|
|
@ -19,6 +19,7 @@
|
|||
set(TELEMETRY_DECODER_GR_BLOCKS_SOURCES
|
||||
gps_l1_ca_telemetry_decoder_cc.cc
|
||||
galileo_e1b_telemetry_decoder_cc.cc
|
||||
sbas_l1_telemetry_decoder_cc.cc
|
||||
)
|
||||
|
||||
include_directories(
|
||||
|
|
|
|||
|
|
@ -0,0 +1,563 @@
|
|||
/*!
|
||||
* \file sbas_l1_telemetry_decoder_cc.cc
|
||||
* \brief Implementation of a SBAS telemetry data decoder block
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 <iostream>
|
||||
#include <sstream>
|
||||
#include <gnuradio/io_signature.h>
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include "control_message_factory.h"
|
||||
#include "gnss_synchro.h"
|
||||
#include "sbas_l1_telemetry_decoder_cc.h"
|
||||
|
||||
using google::LogMessage;
|
||||
|
||||
// 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 //
|
||||
|
||||
|
||||
|
||||
sbas_l1_telemetry_decoder_cc_sptr
|
||||
sbas_l1_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 sbas_l1_telemetry_decoder_cc_sptr(new sbas_l1_telemetry_decoder_cc(satellite, if_freq,
|
||||
fs_in, vector_length, queue, dump));
|
||||
}
|
||||
|
||||
sbas_l1_telemetry_decoder_cc::sbas_l1_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("sbas_l1_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_dump = dump;
|
||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
DLOG(INFO) << "SBAS L1 TELEMETRY PROCESSING: satellite " << d_satellite;
|
||||
d_fs_in = fs_in;
|
||||
|
||||
d_block_size = d_samples_per_symbol * d_symbols_per_bit * d_block_size_in_bits;
|
||||
|
||||
set_output_multiple (1);
|
||||
|
||||
}
|
||||
|
||||
sbas_l1_telemetry_decoder_cc::~sbas_l1_telemetry_decoder_cc()
|
||||
{
|
||||
d_dump_file.close();
|
||||
}
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::forecast (int noutput_items, gr_vector_int &ninput_items_required)
|
||||
{
|
||||
unsigned ninputs = ninput_items_required.size ();
|
||||
for (unsigned i = 0; i < ninputs; i++)
|
||||
ninput_items_required[i] = noutput_items;
|
||||
|
||||
VLOG(LMORE) << "forecast(): " << "noutput_items=" << noutput_items << "\tninput_items_required ninput_items_required.size()=" << ninput_items_required.size();
|
||||
}
|
||||
|
||||
int sbas_l1_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)
|
||||
{
|
||||
|
||||
VLOG(FLOW) << "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
|
||||
|
||||
// store the time stamp of the first sample in the processed sample block
|
||||
double sample_stamp = in[0].Tracking_timestamp_secs;
|
||||
|
||||
// copy correlation samples into samples vector
|
||||
for (int i = 0; i < noutput_items; i++)
|
||||
{
|
||||
// check if channel is in tracking state
|
||||
//if(in[i].Prompt_I != in[i].Prompt_Q) // TODO: check for real condition
|
||||
{
|
||||
d_sample_buf.push_back(in[i].Prompt_I);
|
||||
}
|
||||
}
|
||||
|
||||
// decode only if enough samples in buffer
|
||||
if(d_sample_buf.size() >= d_block_size)
|
||||
{
|
||||
// align correlation samples in pairs
|
||||
// and obtain the symbols by summing the paired correlation samples
|
||||
std::vector<double> symbols;
|
||||
bool sample_alignment = d_sample_aligner.get_symbols(d_sample_buf, symbols);
|
||||
|
||||
// align symbols in pairs
|
||||
// and obtain the bits by decoding the symbol pairs
|
||||
std::vector<int> bits;
|
||||
bool symbol_alignment = d_symbol_aligner_and_decoder.get_bits(symbols, bits);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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
|
||||
// actually the SBAS telemetry decoder doesn't support ranging
|
||||
Gnss_Synchro * current_synchro_data = out; //structure to save the synchronization information and send the output object to the next block
|
||||
for (int i = 0; i < noutput_items; i++)
|
||||
{
|
||||
//1. Copy the current tracking output
|
||||
current_synchro_data[i] = in[i];
|
||||
//2. Add the telemetry decoder information
|
||||
current_synchro_data[i].Flag_valid_word = false; // indicate to observable block that this synchro object isn't valid for pseudorange computation
|
||||
}
|
||||
consume_each(noutput_items); // tell scheduler input items consumed
|
||||
return noutput_items; // tell scheduler output items produced
|
||||
}
|
||||
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::set_satellite(Gnss_Satellite satellite)
|
||||
{
|
||||
d_satellite = Gnss_Satellite(satellite.get_system(), satellite.get_PRN());
|
||||
DLOG(INFO) << "SBAS telemetry decoder in channel " << this->d_channel << " set to satellite " << d_satellite;
|
||||
}
|
||||
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::set_channel(int channel)
|
||||
{
|
||||
d_channel = channel;
|
||||
DLOG(INFO) << "SBAS channel set to " << channel;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// ### helper class for sample alignment ###
|
||||
|
||||
sbas_l1_telemetry_decoder_cc::sample_aligner::sample_aligner()
|
||||
{
|
||||
d_n_smpls_in_history = 3;
|
||||
d_iir_par = 0.05;
|
||||
reset();
|
||||
}
|
||||
sbas_l1_telemetry_decoder_cc::sample_aligner::~sample_aligner()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::sample_aligner::reset()
|
||||
{
|
||||
d_past_sample = 0;
|
||||
d_corr_paired = 0;
|
||||
d_corr_shifted = 0;
|
||||
d_aligned = true;
|
||||
}
|
||||
|
||||
/*
|
||||
* samples length must be a multiple of two
|
||||
*/
|
||||
bool sbas_l1_telemetry_decoder_cc::sample_aligner::get_symbols(const std::vector<double> samples, std::vector<double> &symbols)
|
||||
{
|
||||
double smpls[d_n_smpls_in_history];
|
||||
double corr_diff;
|
||||
bool stand_by = true;
|
||||
double sym;
|
||||
|
||||
VLOG(FLOW) << "get_symbols(): " << "d_past_sample=" << d_past_sample << "\tsamples size=" << samples.size();
|
||||
|
||||
for (unsigned int i_sym = 0; i_sym < samples.size()/sbas_l1_telemetry_decoder_cc::d_samples_per_symbol; i_sym++)
|
||||
{
|
||||
|
||||
// get the next samples
|
||||
for (int i = 0; i < d_n_smpls_in_history; i++)
|
||||
{
|
||||
smpls[i] = ((int)i_sym)*sbas_l1_telemetry_decoder_cc::d_samples_per_symbol+i-1 == -1 ? d_past_sample : samples[i_sym*sbas_l1_telemetry_decoder_cc::d_samples_per_symbol+i-1];
|
||||
}
|
||||
|
||||
// update the pseudo correlations (IIR method) of the two possible alignments
|
||||
d_corr_paired = d_iir_par*smpls[1]*smpls[2] + (1-d_iir_par)*d_corr_paired;
|
||||
d_corr_shifted = d_iir_par*smpls[0]*smpls[1] + (1-d_iir_par)*d_corr_shifted;
|
||||
|
||||
// decide which alignment is the correct one
|
||||
corr_diff = std::abs(d_corr_paired-d_corr_shifted);
|
||||
stand_by = d_aligned ? corr_diff < d_corr_paired/2 : corr_diff < d_corr_shifted/2;
|
||||
if (!stand_by)
|
||||
{
|
||||
d_aligned = d_corr_paired >= d_corr_shifted;
|
||||
}
|
||||
|
||||
// sum the correct pair of samples to a symbol, depending on the current alignment d_align
|
||||
sym = smpls[0+int(d_aligned)*2] + smpls[1];
|
||||
symbols.push_back(sym);
|
||||
|
||||
// sample alignment debug output
|
||||
VLOG(SAMP_SYNC) << std::setprecision(5)
|
||||
<< "smplp: " << std::setw(6) << smpls[0] << " " << "smpl0: " << std::setw(6) << smpls[1] << " " << "smpl1: " << std::setw(6) << smpls[2] << "\t"
|
||||
//<< "Flag_valid_tracking: " << std::setw(1) << in[0][0].Flag_valid_tracking << " " << std::setw(1) << in[0][0].Flag_valid_tracking << "\t"
|
||||
<< "d_corr_paired: " << std::setw(10) << d_corr_paired << "\t"
|
||||
<< "d_corr_shifted: " << std::setw(10) << d_corr_shifted << "\t"
|
||||
<< "corr_diff: " << std::setw(10) << corr_diff << "\t"
|
||||
<< "stand_by: " << std::setw(1) << stand_by << "\t"
|
||||
<< "d_aligned: " << std::setw(1) << d_aligned << "\t"
|
||||
<< "sym: " << std::setw(10) << sym << "\t";
|
||||
}
|
||||
|
||||
// save last sample for next block
|
||||
double temp;
|
||||
temp = samples.back();
|
||||
d_past_sample = (temp);
|
||||
|
||||
return d_aligned;
|
||||
}
|
||||
|
||||
|
||||
// ### helper class for symbol alignment and viterbi decoding ###
|
||||
|
||||
sbas_l1_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;
|
||||
g_encoder[1] = 91;
|
||||
|
||||
d_vd1 = new Viterbi_Decoder(g_encoder, d_KK, nn);
|
||||
d_vd2 = new Viterbi_Decoder(g_encoder, d_KK, nn);
|
||||
d_past_symbol = 0;
|
||||
}
|
||||
sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::~symbol_aligner_and_decoder()
|
||||
{
|
||||
delete d_vd1;
|
||||
delete d_vd2;
|
||||
}
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::reset()
|
||||
{
|
||||
d_past_symbol = 0;
|
||||
d_vd1->reset();
|
||||
d_vd2->reset();
|
||||
}
|
||||
|
||||
bool sbas_l1_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_vd1.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);
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
// 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 sbas_l1_telemetry_decoder_cc::frame_detector::reset()
|
||||
{
|
||||
d_buffer.clear();
|
||||
}
|
||||
|
||||
void sbas_l1_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 = 250;
|
||||
std::vector<std::vector<int>> preambles = {{0, 1, 0, 1, 0, 0, 1 ,1},
|
||||
{1, 0, 0, 1, 1, 0, 1, 0},
|
||||
{1, 1, 0, 0, 0, 1, 1, 0}};
|
||||
|
||||
VLOG(FLOW) << "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++;
|
||||
}
|
||||
VLOG(SAMP_SYNC) << 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;
|
||||
VLOG(EVENT) << ss.str();
|
||||
}
|
||||
}
|
||||
relative_preamble_start++;
|
||||
|
||||
// remove bit in front
|
||||
d_buffer.pop_front();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// ### helper class for checking the CRC of the message candidates ###
|
||||
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::crc_verifier::reset()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void sbas_l1_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;
|
||||
|
||||
VLOG(FLOW) << "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();
|
||||
|
||||
VLOG(SAMP_SYNC) << "candidate " << candidate_it - msg_candidates.begin() << ": 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);
|
||||
}
|
||||
VLOG(SAMP_SYNC) << ss.str() << std::setfill(' ') << std::resetiosflags(std::ios::hex) << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void sbas_l1_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;
|
||||
|
||||
VLOG(LMORE) << "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);
|
||||
VLOG(LMORE) << 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
|
||||
VLOG(LMORE) << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte
|
||||
<< std::setfill(' ') << std::resetiosflags(std::ios::hex);
|
||||
}
|
||||
|
||||
void sbas_l1_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
|
||||
|
||||
VLOG(LMORE) << "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);
|
||||
VLOG(LMORE) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
|
||||
byte = 0;
|
||||
}
|
||||
|
||||
idx_bit++;
|
||||
}
|
||||
VLOG(LMORE) << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte
|
||||
<< std::setfill(' ') << std::resetiosflags(std::ios::hex);
|
||||
}
|
||||
|
||||
void sbas_l1_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 sbas_l1_telemetry_decoder_cc::set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue)
|
||||
{
|
||||
sbas_telemetry_data.set_iono_queue(iono_queue);
|
||||
}
|
||||
|
||||
void sbas_l1_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 sbas_l1_telemetry_decoder_cc::set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue)
|
||||
{
|
||||
sbas_telemetry_data.set_ephemeris_queue(ephemeris_queue);
|
||||
}
|
||||
|
|
@ -0,0 +1,174 @@
|
|||
/*!
|
||||
* \file sbas_l1_telemetry_decoder_cc.h
|
||||
* \brief Interface of a SBAS telemetry data decoder block
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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_SBAS_L1_TELEMETRY_DECODER_CC_H
|
||||
#define GNSS_SDR_SBAS_L1_TELEMETRY_DECODER_CC_H
|
||||
|
||||
#include <fstream>
|
||||
#include <gnuradio/block.h>
|
||||
#include <gnuradio/msg_queue.h>
|
||||
#include <boost/crc.hpp>
|
||||
#include "gnss_satellite.h"
|
||||
#include "viterbi_decoder.h"
|
||||
#include "sbas_telemetry_data.h"
|
||||
|
||||
class sbas_l1_telemetry_decoder_cc;
|
||||
|
||||
typedef boost::shared_ptr<sbas_l1_telemetry_decoder_cc> sbas_l1_telemetry_decoder_cc_sptr;
|
||||
|
||||
sbas_l1_telemetry_decoder_cc_sptr
|
||||
sbas_l1_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 SBAS integrity and corrections data defined in RTCA MOPS DO-229
|
||||
*
|
||||
*/
|
||||
class sbas_l1_telemetry_decoder_cc : public gr::block
|
||||
{
|
||||
public:
|
||||
~sbas_l1_telemetry_decoder_cc();
|
||||
void set_satellite(Gnss_Satellite satellite); //!< Set satellite PRN
|
||||
void set_channel(int channel); //!< Set receiver's channel
|
||||
|
||||
// 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);
|
||||
void set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue);
|
||||
void set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue);
|
||||
void set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue);
|
||||
|
||||
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 sbas_l1_telemetry_decoder_cc_sptr
|
||||
sbas_l1_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);
|
||||
sbas_l1_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 align_samples();
|
||||
|
||||
static const int d_samples_per_symbol = 2;
|
||||
static const int d_symbols_per_bit = 2;
|
||||
static const int d_block_size_in_bits = 30;
|
||||
|
||||
long d_fs_in;
|
||||
|
||||
bool d_dump;
|
||||
Gnss_Satellite d_satellite;
|
||||
int d_channel;
|
||||
|
||||
std::string d_dump_filename;
|
||||
std::ofstream d_dump_file;
|
||||
|
||||
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 sample alignment
|
||||
class sample_aligner
|
||||
{
|
||||
public:
|
||||
sample_aligner();
|
||||
~sample_aligner();
|
||||
|
||||
void reset();
|
||||
|
||||
/*
|
||||
* samples length must be a multiple of two
|
||||
* for block operation the
|
||||
*/
|
||||
bool get_symbols(const std::vector<double> samples, std::vector<double> &symbols);
|
||||
|
||||
private:
|
||||
int d_n_smpls_in_history ;
|
||||
double d_iir_par;
|
||||
|
||||
double d_corr_paired;
|
||||
double d_corr_shifted;
|
||||
bool d_aligned;
|
||||
double d_past_sample;
|
||||
} d_sample_aligner;
|
||||
|
||||
// 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
|
||||
|
|
@ -17,7 +17,8 @@
|
|||
#
|
||||
|
||||
set(TELEMETRY_DECODER_LIB_SOURCES
|
||||
gps_l1_ca_subframe_fsm.cc
|
||||
gps_l1_ca_subframe_fsm.cc
|
||||
viterbi_decoder.cc
|
||||
)
|
||||
|
||||
include_directories(
|
||||
|
|
|
|||
598
src/algorithms/telemetry_decoder/libs/viterbi_decoder.cc
Normal file
598
src/algorithms/telemetry_decoder/libs/viterbi_decoder.cc
Normal file
|
|
@ -0,0 +1,598 @@
|
|||
/*!
|
||||
* \file viterbi_decoder.cc
|
||||
* \brief Implementation of a Viterbi decoder class based on the Iterative Solutions
|
||||
* Coded Modulation Library by Matthew C. Valenti
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 "viterbi_decoder.h"
|
||||
#include <iostream>
|
||||
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
// logging
|
||||
#define EVENT 2 // logs important events which don't occur every block
|
||||
#define FLOW 3 // logs the function calls of block processing functions
|
||||
#define BLOCK 4 // once per block
|
||||
#define SAMPLE 5 // about one log entry per sample
|
||||
#define LMORE 6 // many entries per sample / very specific stuff
|
||||
|
||||
|
||||
#define MAXLOG 1e7 /* Define infinity */
|
||||
|
||||
Viterbi_Decoder::Viterbi_Decoder(const int g_encoder[], const int KK, const int nn)
|
||||
{
|
||||
|
||||
d_nn = nn; //Coding rate 1/n
|
||||
d_KK = KK; //Constraint Length
|
||||
|
||||
// derived code properties
|
||||
d_mm = d_KK - 1;
|
||||
d_states = 1 << d_mm; /* 2^mm */
|
||||
d_number_symbols = 1 << d_nn; /* 2^nn */
|
||||
|
||||
/* create appropriate transition matrices (trellis) */
|
||||
d_out0 = new int[d_states];
|
||||
d_out1 = new int[d_states];
|
||||
d_state0 = new int[d_states];
|
||||
d_state1 = new int[d_states];
|
||||
|
||||
nsc_transit(d_out0, d_state0, 0, g_encoder, d_KK, d_nn);
|
||||
nsc_transit(d_out1, d_state1, 1, g_encoder, d_KK, d_nn);
|
||||
|
||||
// initialise trellis state
|
||||
d_trellis_state_is_initialised = false;
|
||||
Viterbi_Decoder::init_trellis_state();
|
||||
}
|
||||
|
||||
Viterbi_Decoder::~Viterbi_Decoder()
|
||||
{
|
||||
// trellis definition
|
||||
delete[] d_out0;
|
||||
delete[] d_out1;
|
||||
delete[] d_state0;
|
||||
delete[] d_state1;
|
||||
|
||||
// init trellis state
|
||||
delete[] d_pm_t;
|
||||
delete[] d_rec_array;
|
||||
delete[] d_metric_c;
|
||||
}
|
||||
|
||||
void
|
||||
Viterbi_Decoder::reset()
|
||||
{
|
||||
init_trellis_state();
|
||||
}
|
||||
|
||||
/* Function decode_block()
|
||||
|
||||
Description: Uses the Viterbi algorithm to perform hard-decision decoding of a convolutional code.
|
||||
|
||||
Input parameters:
|
||||
r[] The received signal in LLR-form. For BPSK, must be in form r = 2*a*y/(sigma^2).
|
||||
LL The number of data bits to be decoded (doen't inlcude the mm zero-tail-bits)
|
||||
|
||||
Output parameters:
|
||||
output_u_int[] Hard decisions on the data bits (without the mm zero-tail-bits)
|
||||
|
||||
*/
|
||||
|
||||
float
|
||||
Viterbi_Decoder::decode_block(const double input_c[], int output_u_int[], const int LL)
|
||||
{
|
||||
int state;
|
||||
int decoding_length_mismatch;
|
||||
|
||||
VLOG(FLOW) << "decode_block(): LL=" << LL;
|
||||
|
||||
// init
|
||||
init_trellis_state();
|
||||
// do add compare select
|
||||
do_acs(input_c, LL+d_mm);
|
||||
// tail, no need to output -> traceback, but don't decode
|
||||
state = do_traceback(d_mm);
|
||||
// traceback and decode
|
||||
decoding_length_mismatch = do_tb_and_decode(d_mm, LL, state, output_u_int, d_indicator_metric);
|
||||
|
||||
VLOG(FLOW) << "decoding length mismatch: " << decoding_length_mismatch;
|
||||
|
||||
return d_indicator_metric;
|
||||
}
|
||||
|
||||
float
|
||||
Viterbi_Decoder::decode_continuous(const double sym[], const int traceback_depth, int bits[],
|
||||
const int nbits_requested, int &nbits_decoded)
|
||||
{
|
||||
int state;
|
||||
int decoding_length_mismatch;
|
||||
|
||||
VLOG(FLOW) << "decode_continuous(): nbits_requested=" << nbits_requested;
|
||||
|
||||
// do add compare select
|
||||
do_acs(sym, nbits_requested);
|
||||
// the ML sequence in the newest part of the trellis can not be decoded
|
||||
// since it depends on the future values -> traceback, but don't decode
|
||||
state = do_traceback(traceback_depth);
|
||||
// traceback and decode
|
||||
decoding_length_mismatch = do_tb_and_decode(traceback_depth, nbits_requested, state, bits,
|
||||
d_indicator_metric);
|
||||
nbits_decoded = nbits_requested + decoding_length_mismatch;
|
||||
|
||||
VLOG(FLOW) << "decoding length mismatch (continuous decoding): "
|
||||
<< decoding_length_mismatch;
|
||||
|
||||
return d_indicator_metric;
|
||||
}
|
||||
|
||||
void
|
||||
Viterbi_Decoder::init_trellis_state()
|
||||
{
|
||||
int state;
|
||||
|
||||
// if trellis state has been initialised, free old state memory
|
||||
if(d_trellis_state_is_initialised)
|
||||
{
|
||||
// init trellis state
|
||||
delete[] d_pm_t;
|
||||
delete[] d_rec_array;
|
||||
delete[] d_metric_c;
|
||||
}
|
||||
|
||||
// reserve new trellis state memory
|
||||
d_pm_t = new float[d_states];
|
||||
d_trellis_paths = std::deque<Prev>();
|
||||
d_rec_array = new float[d_nn];
|
||||
d_metric_c = new float[d_number_symbols];
|
||||
d_trellis_state_is_initialised = true;
|
||||
|
||||
/* initialize trellis */
|
||||
for (state = 0; state < d_states; state++)
|
||||
{
|
||||
d_pm_t[state] = -MAXLOG;
|
||||
//d_pm_t_next[state] = -MAXLOG;
|
||||
}
|
||||
d_pm_t[0] = 0; /* start in all-zeros state */
|
||||
|
||||
d_indicator_metric = 0;
|
||||
}
|
||||
|
||||
int
|
||||
Viterbi_Decoder::do_acs(const double sym[], int nbits)
|
||||
{
|
||||
int t, i, state_at_t;
|
||||
float metric;
|
||||
float max_val;
|
||||
float * pm_t_next = new float[d_states];
|
||||
|
||||
/* t:
|
||||
* - state: state at t
|
||||
* - d_prev_section[state_at_t]: path metric at t for state state_at_t
|
||||
* - d_out0[state_at_t]: sent symbols for a data bit 0 if state is state_at_t at time t
|
||||
*
|
||||
*/
|
||||
|
||||
for (state_at_t = 0; state_at_t < d_states; state_at_t++)
|
||||
{
|
||||
pm_t_next[state_at_t] = -MAXLOG;
|
||||
}
|
||||
|
||||
|
||||
/* go through trellis */
|
||||
for (t = 0; t < nbits; t++)
|
||||
{
|
||||
|
||||
/* Temporarily store the received symbols current decoding step */
|
||||
for (i = 0; i < d_nn; i++)
|
||||
d_rec_array[i] = (float) sym[d_nn * t + i];
|
||||
|
||||
/* precompute all possible branch metrics */
|
||||
for (i = 0; i < d_number_symbols; i++)
|
||||
{
|
||||
d_metric_c[i] = gamma(d_rec_array, i, d_nn);
|
||||
VLOG(LMORE) << "metric for (tx_sym=" << i << "|ry_sym=(" << d_rec_array[0] << ", " << d_rec_array[1] << ") = " << d_metric_c[i];
|
||||
}
|
||||
|
||||
// find the survivor branches leading the trellis states at t+1
|
||||
Prev next_trellis_states(d_states, t+1);
|
||||
/* step through all states */
|
||||
for (state_at_t = 0; state_at_t < d_states; state_at_t++)
|
||||
{
|
||||
|
||||
int next_state_if_0 = d_state0[state_at_t];
|
||||
int next_state_if_1 = d_state1[state_at_t];
|
||||
|
||||
/* hypothesis: info bit is a zero */
|
||||
int bm_0 = d_metric_c[d_out0[state_at_t]];
|
||||
metric = d_pm_t[state_at_t] + bm_0; // path metric + zerobranch metric
|
||||
|
||||
/* store new metric if more than metric in storage */
|
||||
if (metric > pm_t_next[next_state_if_0])
|
||||
{
|
||||
pm_t_next[next_state_if_0] = metric;
|
||||
next_trellis_states.set_current_state_as_ancestor_of_next_state(next_state_if_0, state_at_t);
|
||||
next_trellis_states.set_decoded_bit_for_next_state(next_state_if_0, 0);
|
||||
next_trellis_states.set_survivor_branch_metric_of_next_state(next_state_if_0, bm_0);
|
||||
}
|
||||
|
||||
/* hypothesis: info bit is a one */
|
||||
int bm_1 = d_metric_c[d_out1[state_at_t]];
|
||||
metric = d_pm_t[state_at_t] + bm_1; // path metric + onebranch metric
|
||||
|
||||
/* store new metric if more than metric in storage */
|
||||
if (metric > pm_t_next[next_state_if_1])
|
||||
{
|
||||
pm_t_next[next_state_if_1] = metric;
|
||||
next_trellis_states.set_current_state_as_ancestor_of_next_state(next_state_if_1, state_at_t);
|
||||
next_trellis_states.set_decoded_bit_for_next_state(next_state_if_1, 1);
|
||||
next_trellis_states.set_survivor_branch_metric_of_next_state(next_state_if_1, bm_1);
|
||||
}
|
||||
}
|
||||
|
||||
d_trellis_paths.push_front(next_trellis_states);
|
||||
|
||||
|
||||
/* normalize -> afterwards, the largest metric value is always 0 */
|
||||
//max_val = 0;
|
||||
max_val = -MAXLOG;
|
||||
for (state_at_t = 0; state_at_t < d_states; state_at_t++)
|
||||
{
|
||||
if (pm_t_next[state_at_t] > max_val)
|
||||
{
|
||||
max_val = pm_t_next[state_at_t];
|
||||
}
|
||||
}
|
||||
VLOG(LMORE) << "max_val at t=" << t << ": " << max_val;
|
||||
for (state_at_t = 0; state_at_t < d_states; state_at_t++)
|
||||
{
|
||||
d_pm_t[state_at_t] = pm_t_next[state_at_t] - max_val;
|
||||
pm_t_next[state_at_t] = -MAXLOG;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
delete[] pm_t_next;
|
||||
|
||||
return t;
|
||||
}
|
||||
|
||||
int
|
||||
Viterbi_Decoder::do_traceback(size_t traceback_length)
|
||||
{
|
||||
// traceback_length is in bits
|
||||
int state;
|
||||
std::deque<Prev>::iterator it;
|
||||
|
||||
VLOG(FLOW) << "do_traceback(): traceback_length=" << traceback_length << std::endl;
|
||||
|
||||
if (d_trellis_paths.size() < traceback_length)
|
||||
{
|
||||
traceback_length = d_trellis_paths.size();
|
||||
}
|
||||
|
||||
state = 0; // maybe start not at state 0, but at state with best metric
|
||||
for (it = d_trellis_paths.begin(); it < d_trellis_paths.begin() + traceback_length; ++it)
|
||||
{
|
||||
state = it->get_anchestor_state_of_current_state(state);
|
||||
}
|
||||
return state;
|
||||
}
|
||||
|
||||
int
|
||||
Viterbi_Decoder::do_tb_and_decode(int traceback_length, int requested_decoding_length, int state, int output_u_int[], float& indicator_metric)
|
||||
{
|
||||
int n_of_branches_for_indicator_metric = 500;
|
||||
|
||||
int t_out;
|
||||
std::deque<Prev>::iterator it;
|
||||
int decoding_length_mismatch;
|
||||
int overstep_length;
|
||||
|
||||
int n_im=0;
|
||||
|
||||
VLOG(FLOW) << "do_tb_and_decode(): requested_decoding_length=" << requested_decoding_length;
|
||||
|
||||
// decode only decode_length bits -> overstep newer bits which are too much
|
||||
decoding_length_mismatch = d_trellis_paths.size() - (traceback_length + requested_decoding_length);
|
||||
VLOG(BLOCK) << "decoding_length_mismatch=" << decoding_length_mismatch;
|
||||
overstep_length = decoding_length_mismatch >= 0 ? decoding_length_mismatch : 0;
|
||||
VLOG(BLOCK) << "overstep_length=" << overstep_length;
|
||||
|
||||
for (it = d_trellis_paths.begin() + traceback_length; it < d_trellis_paths.begin()+traceback_length + overstep_length; ++it)
|
||||
{
|
||||
state = it->get_anchestor_state_of_current_state(state);
|
||||
}
|
||||
|
||||
t_out = d_trellis_paths.end()-(d_trellis_paths.begin() + traceback_length + overstep_length)-1;//requested_decoding_length-1;
|
||||
indicator_metric = 0;
|
||||
for (it = d_trellis_paths.begin() + traceback_length + overstep_length; it < d_trellis_paths.end(); ++it)
|
||||
{
|
||||
//VLOG(SAMPLE)<< "@t_out=" << t_out;
|
||||
//VLOG(SAMPLE) << "tb&dec: @t=" << it->get_t() << " bit=" << it->get_bit_of_current_state(state) << " bm=" << it->get_metric_of_current_state(state);
|
||||
if(it - (d_trellis_paths.begin() + traceback_length + overstep_length) < n_of_branches_for_indicator_metric)
|
||||
{
|
||||
n_im++;
|
||||
indicator_metric += it->get_metric_of_current_state(state);
|
||||
VLOG(SAMPLE) << "@t=" << it->get_t() << " b=" << it->get_bit_of_current_state(state) << " sm=" << indicator_metric << " d=" << it->get_metric_of_current_state(state);
|
||||
}
|
||||
output_u_int[t_out] = it->get_bit_of_current_state(state);
|
||||
state = it->get_anchestor_state_of_current_state(state);
|
||||
|
||||
t_out--;
|
||||
}
|
||||
indicator_metric/=n_im;
|
||||
VLOG(BLOCK) << "indicator metric: " << indicator_metric;
|
||||
// remove old states
|
||||
if (d_trellis_paths.begin() + traceback_length+overstep_length <= d_trellis_paths.end())
|
||||
{
|
||||
d_trellis_paths.erase(d_trellis_paths.begin()+traceback_length+overstep_length, d_trellis_paths.end());
|
||||
}
|
||||
return decoding_length_mismatch;
|
||||
}
|
||||
|
||||
/* function Gamma()
|
||||
|
||||
Description: Computes the branch metric used for decoding.
|
||||
|
||||
Output parameters:
|
||||
(returned float) The metric between the hypothetical symbol and the recevieved vector
|
||||
|
||||
Input parameters:
|
||||
rec_array The received vector, of length nn
|
||||
symbol The hypothetical symbol
|
||||
nn The length of the received vector
|
||||
|
||||
This function is used by siso() */
|
||||
float
|
||||
Viterbi_Decoder::gamma(float rec_array[], int symbol, int nn)
|
||||
{
|
||||
float rm = 0;
|
||||
int i;
|
||||
int mask;
|
||||
float txsym;
|
||||
|
||||
mask = 1;
|
||||
for (i = 0; i < nn; i++)
|
||||
{
|
||||
//if (symbol & mask) rm += rec_array[nn - i - 1];
|
||||
txsym = symbol & mask ? 1 : -1;
|
||||
rm += txsym * rec_array[nn - i - 1];
|
||||
mask = mask << 1;
|
||||
}
|
||||
//rm = rm > 50 ? rm : -1000;
|
||||
|
||||
return (rm);
|
||||
}
|
||||
|
||||
/* function that creates the transit and output vectors */
|
||||
void
|
||||
Viterbi_Decoder::nsc_transit(int output_p[], int trans_p[], int input, const int g[],
|
||||
int KK, int nn)
|
||||
{
|
||||
int nextstate[1];
|
||||
int state, states;
|
||||
states = (1 << (KK - 1)); /* The number of states: 2^mm */
|
||||
|
||||
/* Determine the output and next state for each possible starting state */
|
||||
for (state = 0; state < states; state++)
|
||||
{
|
||||
output_p[state] = nsc_enc_bit(nextstate, input, state, g, KK, nn);
|
||||
trans_p[state] = nextstate[0];
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
/* Function nsc_enc_bit()
|
||||
|
||||
Description: Convolutionally encodes a single bit using a rate 1/n encoder.
|
||||
Takes in one input bit at a time, and produces a n-bit output.
|
||||
|
||||
Input parameters:
|
||||
input The input data bit (i.e. a 0 or 1).
|
||||
state_in The starting state of the encoder (an int from 0 to 2^m-1).
|
||||
g[] An n-element vector containing the code generators in binary form.
|
||||
KK The constraint length of the convolutional code.
|
||||
nn number of symbols bits per input bits (rate 1/nn)
|
||||
|
||||
Output parameters:
|
||||
output_p[] An n-element vector containing the encoded bits.
|
||||
state_out_p[] An integer containing the final state of the encoder
|
||||
(i.e. the state after encoding this bit)
|
||||
|
||||
This function is used by rsc_encode(), nsc_transit(), rsc_transit(), and nsc_transit() */
|
||||
|
||||
int
|
||||
Viterbi_Decoder::nsc_enc_bit(int state_out_p[], int input, int state_in,
|
||||
const int g[], int KK, int nn)
|
||||
{
|
||||
/* declare variables */
|
||||
int state, i;
|
||||
int out = 0;
|
||||
|
||||
/* create a word made up of state and new input */
|
||||
state = (input << (KK - 1)) ^ state_in;
|
||||
|
||||
/* AND the word with the generators */
|
||||
for (i = 0; i < nn; i++)
|
||||
{
|
||||
/* update output symbol */
|
||||
out = (out << 1) + parity_counter(state & g[i], KK);
|
||||
}
|
||||
|
||||
/* shift the state to make the new state */
|
||||
state_out_p[0] = state >> 1;
|
||||
return (out);
|
||||
}
|
||||
|
||||
/* function parity_counter()
|
||||
|
||||
Description: Determines if a symbol has odd (1) or even (0) parity
|
||||
|
||||
Output parameters:
|
||||
(returned int): The symbol's parity = 1 for odd and 0 for even
|
||||
|
||||
Input parameters:
|
||||
symbol: The integer-valued symbol
|
||||
length: The highest bit position in the symbol
|
||||
|
||||
This function is used by nsc_enc_bit(), rsc_enc_bit(), and rsc_tail() */
|
||||
|
||||
int
|
||||
Viterbi_Decoder::parity_counter(int symbol, int length)
|
||||
{
|
||||
int counter;
|
||||
int temp_parity = 0;
|
||||
|
||||
for (counter = 0; counter < length; counter++)
|
||||
{
|
||||
temp_parity = temp_parity ^ (symbol & 1);
|
||||
symbol = symbol >> 1;
|
||||
}
|
||||
|
||||
return (temp_parity);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// prev helper class
|
||||
Viterbi_Decoder::Prev::Prev(int states, int t)
|
||||
{
|
||||
this->t = t;
|
||||
state = new int[states];
|
||||
bit = new int[states];
|
||||
metric = new float[states];
|
||||
|
||||
refcount = new int;
|
||||
*refcount = 1;
|
||||
|
||||
//std::cout << "Prev(" << states << ", " << t << ")" << " constructor" << std::endl;
|
||||
}
|
||||
|
||||
// copy constructor
|
||||
Viterbi_Decoder::Prev::Prev(const Prev& prev)
|
||||
{
|
||||
refcount = prev.refcount;
|
||||
(*refcount)++;
|
||||
|
||||
t = prev.t;
|
||||
state = prev.state;
|
||||
bit = prev.bit;
|
||||
metric = prev.metric;
|
||||
|
||||
VLOG(LMORE) << "Prev(" << "?" << ", " << t << ")" << " copy, new refcount = " << *refcount;
|
||||
}
|
||||
|
||||
// assignment constructor
|
||||
Viterbi_Decoder::Prev& Viterbi_Decoder::Prev::operator=(const Prev& other)
|
||||
{
|
||||
// check for self-assignment
|
||||
if(&other == this)
|
||||
{
|
||||
return *this;
|
||||
}
|
||||
|
||||
// handle old resources
|
||||
if(*refcount==1)
|
||||
{ // if they are not used anymore -> unallocate them
|
||||
delete[] state;
|
||||
delete[] bit;
|
||||
delete[] metric;
|
||||
delete refcount;
|
||||
}
|
||||
else
|
||||
{ // this object is not anymore using them
|
||||
(*refcount)--;
|
||||
}
|
||||
|
||||
// increase ref counter for this resource set
|
||||
refcount = other.refcount;
|
||||
(*refcount)++;
|
||||
|
||||
// take over resources
|
||||
t = other.t;
|
||||
state = other.state;
|
||||
bit = other.bit;
|
||||
metric = other.metric;
|
||||
|
||||
VLOG(LMORE) << "Prev(" << "?" << ", " << t << ")" << " assignment, new refcount = " << *refcount;
|
||||
return *this;
|
||||
}
|
||||
|
||||
Viterbi_Decoder::Prev::~Prev()
|
||||
{
|
||||
if (*refcount == 1)
|
||||
{
|
||||
delete[] state;
|
||||
delete[] bit;
|
||||
delete[] metric;
|
||||
delete refcount;
|
||||
//std::cout << "~Prev(" << "?" << ", " << t << ")" << " destructor with delete" << std::endl;
|
||||
}
|
||||
else
|
||||
{
|
||||
(*refcount)--;
|
||||
VLOG(LMORE) << "~Prev(" << "?" << ", " << t << ")" << " destructor after copy, new refcount = " << *refcount;
|
||||
}
|
||||
}
|
||||
|
||||
int Viterbi_Decoder::Prev::get_anchestor_state_of_current_state(int current_state)
|
||||
{
|
||||
//std::cout << "get prev state: for state " << current_state << " at time " << t << ", the prev state at time " << t-1 << " is " << state[current_state] << std::endl;
|
||||
return state[current_state];
|
||||
}
|
||||
|
||||
int Viterbi_Decoder::Prev::get_bit_of_current_state(int current_state)
|
||||
{
|
||||
//std::cout << "get prev bit : for state " << current_state << " at time " << t << ", the send bit is " << bit[current_state] << std::endl;
|
||||
return bit[current_state];
|
||||
}
|
||||
|
||||
float Viterbi_Decoder::Prev::get_metric_of_current_state(int current_state)
|
||||
{
|
||||
return metric[current_state];
|
||||
}
|
||||
|
||||
int Viterbi_Decoder::Prev::get_t()
|
||||
{
|
||||
return t;
|
||||
}
|
||||
|
||||
void Viterbi_Decoder::Prev::set_current_state_as_ancestor_of_next_state(int next_state, int current_state)
|
||||
{
|
||||
state[next_state] = current_state;
|
||||
}
|
||||
|
||||
void Viterbi_Decoder::Prev::set_decoded_bit_for_next_state(int next_state, int bit)
|
||||
{
|
||||
this->bit[next_state] = bit;
|
||||
}
|
||||
|
||||
void Viterbi_Decoder::Prev::set_survivor_branch_metric_of_next_state(int next_state, float metric)
|
||||
{
|
||||
this->metric[next_state] = metric;
|
||||
}
|
||||
114
src/algorithms/telemetry_decoder/libs/viterbi_decoder.h
Normal file
114
src/algorithms/telemetry_decoder/libs/viterbi_decoder.h
Normal file
|
|
@ -0,0 +1,114 @@
|
|||
/*!
|
||||
* \file viterbi_decoder.h
|
||||
* \brief Interface of a Viterbi decoder class based on the Iterative Solutions
|
||||
* Coded Modulation Library by Matthew C. Valenti
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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_VITERBIDECODER_H_
|
||||
#define GNSS_SDR_VITERBIDECODER_H_
|
||||
|
||||
#include <deque>
|
||||
#include <iostream>
|
||||
|
||||
class Viterbi_Decoder
|
||||
{
|
||||
public:
|
||||
Viterbi_Decoder(const int g_encoder[], const int KK, const int nn);
|
||||
~Viterbi_Decoder();
|
||||
void reset();
|
||||
float decode_block(const double input_c[], int* output_u_int, const int LL);
|
||||
float decode_continuous(const double sym[], const int traceback_depth, int output_u_int[],
|
||||
const int nbits_requested, int &nbits_decoded);
|
||||
|
||||
private:
|
||||
|
||||
class Prev
|
||||
{
|
||||
public:
|
||||
Prev(int states, int t);
|
||||
Prev(const Prev& prev);
|
||||
Prev& operator=(const Prev& other);
|
||||
~Prev();
|
||||
|
||||
int get_anchestor_state_of_current_state(int current_state);
|
||||
int get_bit_of_current_state(int current_state);
|
||||
float get_metric_of_current_state(int current_state);
|
||||
int get_t();
|
||||
void set_current_state_as_ancestor_of_next_state(int next_state, int current_state);
|
||||
void set_decoded_bit_for_next_state(int next_state, int bit);
|
||||
void set_survivor_branch_metric_of_next_state(int next_state, float metric);
|
||||
|
||||
private:
|
||||
int t;
|
||||
int * state;
|
||||
int * bit;
|
||||
float * metric;
|
||||
int * refcount;
|
||||
};
|
||||
|
||||
// code properties
|
||||
int d_KK;
|
||||
int d_nn;
|
||||
|
||||
// derived code properties
|
||||
int d_mm;
|
||||
int d_states;
|
||||
int d_number_symbols;
|
||||
|
||||
// trellis definition
|
||||
int* d_out0;
|
||||
int* d_state0;
|
||||
int* d_out1;
|
||||
int* d_state1;
|
||||
|
||||
// trellis state
|
||||
float *d_pm_t;
|
||||
std::deque<Prev> d_trellis_paths;
|
||||
float *d_metric_c; /* Set of all possible branch metrics */
|
||||
float *d_rec_array; /* Received values for one trellis section */
|
||||
bool d_trellis_state_is_initialised;
|
||||
|
||||
// measures
|
||||
float d_indicator_metric;
|
||||
|
||||
// operations on the trellis (change decoder state)
|
||||
void init_trellis_state();
|
||||
int do_acs(const double sym[], int nbits);
|
||||
int do_traceback(size_t traceback_length);
|
||||
int do_tb_and_decode(int traceback_length, int requested_decoding_length, int state, int bits[], float& indicator_metric);
|
||||
|
||||
// branch metric function
|
||||
float gamma(float rec_array[], int symbol, int nn);
|
||||
|
||||
// trellis generation
|
||||
void nsc_transit(int output_p[], int trans_p[], int input, const int g[], int KK, int nn);
|
||||
int nsc_enc_bit(int state_out_p[], int input, int state_in, const int g[], int KK, int nn);
|
||||
int parity_counter(int symbol, int length);
|
||||
};
|
||||
|
||||
#endif /* GNSS_SDR_VITERBIDECODER_H_ */
|
||||
|
|
@ -32,10 +32,15 @@ set(SYSTEM_PARAMETERS_SOURCES
|
|||
galileo_almanac.cc
|
||||
galileo_iono.cc
|
||||
galileo_navigation_message.cc
|
||||
sbas_ephemeris.cc
|
||||
sbas_ionospheric_correction.cc
|
||||
sbas_satellite_correction.cc
|
||||
sbas_telemetry_data.cc
|
||||
)
|
||||
|
||||
include_directories(
|
||||
$(CMAKE_CURRENT_SOURCE_DIR)
|
||||
${CMAKE_SOURCE_DIR}/src/core/receiver
|
||||
${GLOG_INCLUDE_DIRS}
|
||||
${Boost_INCLUDE_DIRS}
|
||||
${GFlags_INCLUDE_DIRS}
|
||||
|
|
|
|||
51
src/core/system_parameters/sbas_ephemeris.cc
Normal file
51
src/core/system_parameters/sbas_ephemeris.cc
Normal file
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
* \file sbas_ephemeris.cc
|
||||
* \brief Interface of a SBAS REFERENCE LOCATION storage
|
||||
*
|
||||
* \author Daniel Fehr, 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 <stdio.h>
|
||||
#include <math.h>
|
||||
#include <iostream>
|
||||
|
||||
#include "sbas_ephemeris.h"
|
||||
|
||||
void Sbas_Ephemeris::print(std::ostream &out)
|
||||
{
|
||||
out << "<<E>> PRN" << i_prn << ":";
|
||||
out << " d_t0=" << i_t0;
|
||||
out << " d_tof=" << d_tof;
|
||||
out << " i_sv_ura=" << i_sv_ura;
|
||||
out << " b_sv_do_not_use=" << b_sv_do_not_use;
|
||||
out << " d_pos=(x=" << d_pos[0] << ", y=" << d_pos[1] << ", z=" << d_pos[2] << ")";
|
||||
out << " d_vel=(x=" << d_vel[0] << ", y=" << d_vel[1] << ", z=" << d_vel[2] << ")";
|
||||
out << " d_acc=(x=" << d_acc[0] << ", y=" << d_acc[1] << ", z=" << d_acc[2] << ")";
|
||||
out << " d_af0=" << d_af0;
|
||||
out << " d_af1=" << d_af1;
|
||||
}
|
||||
56
src/core/system_parameters/sbas_ephemeris.h
Normal file
56
src/core/system_parameters/sbas_ephemeris.h
Normal file
|
|
@ -0,0 +1,56 @@
|
|||
/*
|
||||
* \file sbas_ephemeris.h
|
||||
* \brief Implementation of a SBAS REFERENCE LOCATION storage
|
||||
*
|
||||
* \author Daniel Fehr, 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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_SBAS_EPHEMERIS_H_
|
||||
#define GNSS_SDR_SBAS_EPHEMERIS_H_
|
||||
|
||||
|
||||
class Sbas_Ephemeris
|
||||
{
|
||||
public:
|
||||
void print(std::ostream &out);
|
||||
int i_prn; // PRN number */
|
||||
//gtime_t t0; // reference epoch time (GPST)
|
||||
int i_t0;
|
||||
//gtime_t tof; // time of message frame (GPST)
|
||||
double d_tof;
|
||||
int i_sv_ura; // SV accuracy (URA index), not standardized
|
||||
bool b_sv_do_not_use; // health status (false:don't use / true:usable)
|
||||
double d_pos[3]; // satellite position (m) (ecef)
|
||||
double d_vel[3]; // satellite velocity (m/s) (ecef)
|
||||
double d_acc[3]; // satellite acceleration (m/s^2) (ecef)
|
||||
double d_af0; // satellite clock-offset (s)
|
||||
double d_af1; // satellite drift (s/s)
|
||||
};
|
||||
|
||||
|
||||
#endif /* GNSS_SDR_SBAS_EPHEMERIS_H_ */
|
||||
450
src/core/system_parameters/sbas_ionospheric_correction.cc
Normal file
450
src/core/system_parameters/sbas_ionospheric_correction.cc
Normal file
|
|
@ -0,0 +1,450 @@
|
|||
/*!
|
||||
* \file sbas_ionospheric_correction.cc
|
||||
* \brief Implementation of the SBAS ionosphere correction set based on SBAS RTKLIB functions
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 <stdarg.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <iostream>
|
||||
#include <boost/serialization/map.hpp>
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include "sbas_ionospheric_correction.h"
|
||||
|
||||
enum V_Log_Level {EVENT = 2, // logs important events which don't occur every update() call
|
||||
FLOW = 3, // logs the function calls of block processing functions
|
||||
MORE = 4}; // very detailed stuff
|
||||
|
||||
|
||||
void
|
||||
Sbas_Ionosphere_Correction::print(std::ostream &out)
|
||||
{
|
||||
for(std::vector<Igp_Band>::const_iterator it_band = d_bands.begin(); it_band != d_bands.end(); ++it_band)
|
||||
{
|
||||
int band = it_band-d_bands.begin();
|
||||
out << "<<I>> Band" << band << ":" << std::endl;
|
||||
for(std::vector<Igp>::const_iterator it_igp = it_band->d_igps.begin(); it_igp != it_band->d_igps.end(); ++it_igp)
|
||||
{
|
||||
int igp = it_igp-it_band->d_igps.begin();
|
||||
out << "<<I>> -IGP" << igp << ":";
|
||||
//std::cout << " valid=" << it_igp->d_valid;
|
||||
out << " t0=" << it_igp->t0;
|
||||
out << " lat=" << it_igp->d_latitude;
|
||||
out << " lon=" << it_igp->d_longitude;
|
||||
out << " give=" << it_igp->d_give;
|
||||
out << " delay=" << it_igp->d_delay;
|
||||
out << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* -receiver position (degree) is in terms of WGS84
|
||||
* -azimuth is the angle of the satellite from the userÕs location measured clockwise from north
|
||||
* -elevation is the angle of the satellite from the user's location measured with respect to the local-tangent-plane
|
||||
*/
|
||||
bool Sbas_Ionosphere_Correction::apply(double sample_stamp, double latitude_d, double longitude_d,
|
||||
double azimut_d, double evaluation_d, double &delay, double &var)
|
||||
{
|
||||
const double GPS_PI = 3.1415926535898; //!< Pi as defined in IS-GPS-200E
|
||||
int result;
|
||||
double pos[3];
|
||||
double azel[2];
|
||||
|
||||
// convert receiver position from degrees to rad
|
||||
pos[0] = latitude_d * GPS_PI / 180.0;
|
||||
pos[1] = longitude_d * GPS_PI / 180.0;
|
||||
pos[2] = 0; // is not used by sbsioncorr, for ionocorrection is a fixed earth radius assumed
|
||||
|
||||
// convert satellite azimut and evaluation from degrees to rad , use topocent to obtain it in pvt block
|
||||
azel[0] = azimut_d * GPS_PI / 180.0;
|
||||
azel[1] = evaluation_d * GPS_PI / 180.0;
|
||||
|
||||
result = sbsioncorr(sample_stamp, pos, azel, &delay, &var);
|
||||
return (bool)result;
|
||||
}
|
||||
|
||||
|
||||
#define PI 3.1415926535897932 /* pi */
|
||||
#define D2R (PI/180.0) /* deg to rad */
|
||||
#define R2D (180.0/PI) /* rad to deg */
|
||||
#define MAXBAND 10 /* max SBAS band of IGP */
|
||||
#define RE_WGS84 6378137.0 /* earth semimajor axis (WGS84) (m) */
|
||||
|
||||
/* geometric distance ----------------------------------------------------------
|
||||
* compute geometric distance and receiver-to-satellite unit vector
|
||||
* args : double *rs I satellilte position (ecef at transmission) (m)
|
||||
* double *rr I receiver position (ecef at reception) (m)
|
||||
* double *e O line-of-sight vector (ecef)
|
||||
* return : geometric distance (m) (0>:error/no satellite position)
|
||||
* notes : distance includes sagnac effect correction
|
||||
*-----------------------------------------------------------------------------*/
|
||||
//extern double geodist(const double *rs, const double *rr, double *e)
|
||||
//{
|
||||
// double r;
|
||||
// int i;
|
||||
//
|
||||
// if (norm(rs,3)<RE_WGS84) return -1.0;
|
||||
// for (i=0;i<3;i++) e[i]=rs[i]-rr[i];
|
||||
// r=norm(e,3);
|
||||
// for (i=0;i<3;i++) e[i]/=r;
|
||||
// return r+OMGE*(rs[0]*rr[1]-rs[1]*rr[0])/CLIGHT;
|
||||
//}
|
||||
|
||||
/* inner product ---------------------------------------------------------------
|
||||
* inner product of vectors
|
||||
* args : double *a,*b I vector a,b (n x 1)
|
||||
* int n I size of vector a,b
|
||||
* return : a'*b
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double Sbas_Ionosphere_Correction::dot(const double *a, const double *b, int n)
|
||||
{
|
||||
double c = 0.0;
|
||||
while (--n >= 0) c += a[n]*b[n];
|
||||
return c;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/* multiply matrix -----------------------------------------------------------*/
|
||||
void Sbas_Ionosphere_Correction::matmul(const char *tr, int n, int k, int m, double alpha,
|
||||
const double *A, const double *B, double beta, double *C)
|
||||
{
|
||||
double d;
|
||||
int i, j, x, f = tr[0]=='N'?(tr[1]=='N'?1:2):(tr[1]=='N'?3:4);
|
||||
|
||||
for (i=0; i<n; i++) for (j=0;j<k;j++)
|
||||
{
|
||||
d = 0.0;
|
||||
switch (f)
|
||||
{
|
||||
case 1: for (x=0; x<m; x++) d += A[i+x*n]*B[x+j*m]; break;
|
||||
case 2: for (x=0; x<m; x++) d += A[i+x*n]*B[j+x*k]; break;
|
||||
case 3: for (x=0; x<m; x++) d += A[x+i*m]*B[x+j*m]; break;
|
||||
case 4: for (x=0; x<m; x++) d += A[x+i*m]*B[j+x*k]; break;
|
||||
}
|
||||
if (beta==0.0) C[i+j*n] = alpha*d; else C[i+j*n] = alpha*d + beta*C[i+j*n];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* ecef to local coordinate transfromation matrix ------------------------------
|
||||
* compute ecef to local coordinate transfromation matrix
|
||||
* args : double *pos I geodetic position {lat,lon} (rad)
|
||||
* double *E O ecef to local coord transformation matrix (3x3)
|
||||
* return : none
|
||||
* notes : matirix stored by column-major order (fortran convention)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
void Sbas_Ionosphere_Correction::xyz2enu(const double *pos, double *E)
|
||||
{
|
||||
double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
|
||||
|
||||
E[0] = -sinl; E[3] = cosl; E[6] = 0.0;
|
||||
E[1] = -sinp*cosl; E[4] = -sinp*sinl; E[7] = cosp;
|
||||
E[2] = cosp*cosl; E[5] = cosp*sinl; E[8] = sinp;
|
||||
}
|
||||
|
||||
|
||||
/* transform ecef vector to local tangental coordinate -------------------------
|
||||
* transform ecef vector to local tangental coordinate
|
||||
* args : double *pos I geodetic position {lat,lon} (rad)
|
||||
* double *r I vector in ecef coordinate {x,y,z}
|
||||
* double *e O vector in local tangental coordinate {e,n,u}
|
||||
* return : none
|
||||
*-----------------------------------------------------------------------------*/
|
||||
void Sbas_Ionosphere_Correction::ecef2enu(const double *pos, const double *r, double *e)
|
||||
{
|
||||
double E[9];
|
||||
|
||||
xyz2enu(pos, E);
|
||||
matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* satellite azimuth/elevation angle -------------------------------------------
|
||||
* compute satellite azimuth/elevation angle
|
||||
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
|
||||
* double *e I receiver-to-satellilte unit vevtor (ecef)
|
||||
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
|
||||
* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
|
||||
* return : elevation angle (rad)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double Sbas_Ionosphere_Correction::satazel(const double *pos, const double *e, double *azel)
|
||||
{
|
||||
double az = 0.0, el=PI/2.0, enu[3];
|
||||
|
||||
if (pos[2] > -RE_WGS84)
|
||||
{
|
||||
ecef2enu(pos,e,enu);
|
||||
az = dot(enu,enu,2) < 1E-12?0.0:atan2(enu[0], enu[1]);
|
||||
if (az<0.0) az += 2*PI;
|
||||
el = asin(enu[2]);
|
||||
}
|
||||
if (azel)
|
||||
{
|
||||
azel[0]=az;
|
||||
azel[1]=el;
|
||||
}
|
||||
return el;
|
||||
}
|
||||
|
||||
/* debug trace function -----------------------------------------------------*/
|
||||
void Sbas_Ionosphere_Correction::trace(int level, const char *format, ...)
|
||||
{
|
||||
va_list ap;
|
||||
char str[1000];
|
||||
|
||||
va_start(ap,format); vsprintf(str,format,ap); va_end(ap);
|
||||
VLOG(FLOW) << "<<I>> " << std::string(str);
|
||||
}
|
||||
|
||||
/* ionospheric pierce point position -------------------------------------------
|
||||
* compute ionospheric pierce point (ipp) position and slant factor
|
||||
* args : double *pos I receiver position {lat,lon,h} (rad,m)
|
||||
* double *azel I azimuth/elevation angle {az,el} (rad)
|
||||
* double re I earth radius (km)
|
||||
* double hion I altitude of ionosphere (km)
|
||||
* double *posp O pierce point position {lat,lon,h} (rad,m)
|
||||
* return : slant factor
|
||||
* notes : see ref [2], only valid on the earth surface
|
||||
* fixing bug on ref [2] A.4.4.10.1 A-22,23
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double Sbas_Ionosphere_Correction::ionppp(const double *pos, const double *azel,
|
||||
double re, double hion, double *posp)
|
||||
{
|
||||
double cosaz, rp, ap, sinap, tanap;
|
||||
|
||||
rp = re/(re+hion)*cos(azel[1]);
|
||||
ap = PI/2.0-azel[1]-asin(rp);
|
||||
sinap = sin(ap);
|
||||
tanap = tan(ap);
|
||||
cosaz = cos(azel[0]);
|
||||
posp[0] = asin(sin(pos[0])*cos(ap) + cos(pos[0])*sinap*cosaz);
|
||||
|
||||
if ((pos[0] > 70.0*D2R && tanap*cosaz > tan(PI/2.0-pos[0]))||
|
||||
(pos[0] < -70.0*D2R && - tanap*cosaz > tan(PI/2.0+pos[0])))
|
||||
{
|
||||
posp[1] = pos[1] + PI - asin(sinap*sin(azel[0])/cos(posp[0]));
|
||||
}
|
||||
else
|
||||
{
|
||||
posp[1] = pos[1] + asin(sinap*sin(azel[0])/cos(posp[0]));
|
||||
}
|
||||
return 1.0/sqrt(1.0-rp*rp);
|
||||
}
|
||||
|
||||
|
||||
/* variance of ionosphere correction (give=GIVEI) --------------------------*/
|
||||
double Sbas_Ionosphere_Correction::varicorr(int give)
|
||||
{
|
||||
const double var[15]={
|
||||
0.0084, 0.0333, 0.0749, 0.1331, 0.2079, 0.2994, 0.4075, 0.5322, 0.6735, 0.8315,
|
||||
1.1974, 1.8709, 3.326, 20.787, 187.0826
|
||||
};
|
||||
return 0<=give&&give<15?var[give]:0.0;
|
||||
}
|
||||
|
||||
|
||||
/* search igps ---------------------------------------------------------------*/
|
||||
void Sbas_Ionosphere_Correction::searchigp(const double *pos, const Igp **igp, double *x, double *y)
|
||||
{
|
||||
int i;
|
||||
int latp[2];
|
||||
int lonp[4];
|
||||
double lat = pos[0]*R2D;
|
||||
double lon = pos[1]*R2D;
|
||||
|
||||
trace(4,"searchigp: pos=%.3f %.3f",pos[0]*R2D, pos[1]*R2D);
|
||||
|
||||
// round the pierce point position to the next IGP grid point
|
||||
if (lon >= 180.0) lon -= 360.0;
|
||||
if (-55.0 <= lat && lat < 55.0)
|
||||
{
|
||||
latp[0] = (int)floor(lat/5.0)*5;
|
||||
latp[1] = latp[0]+5;
|
||||
lonp[0] = lonp[1] = (int)floor(lon/5.0)*5;
|
||||
lonp[2] = lonp[3] = lonp[0] + 5;
|
||||
*x = (lon-lonp[0])/5.0;
|
||||
*y = (lat-latp[0])/5.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
latp[0] = (int)floor((lat-5.0)/10.0)*10+5;
|
||||
latp[1] = latp[0]+10;
|
||||
lonp[0] = lonp[1] = (int)floor(lon/10.0)*10;
|
||||
lonp[2] = lonp[3] = lonp[0]+10;
|
||||
*x = (lon - lonp[0])/10.0;
|
||||
*y = (lat - latp[0])/10.0;
|
||||
if (75.0 <= lat && lat < 85.0)
|
||||
{
|
||||
lonp[1] = (int)floor(lon/90.0)*90;
|
||||
lonp[3] = lonp[1]+90;
|
||||
}
|
||||
else if (-85.0 <= lat && lat < -75.0)
|
||||
{
|
||||
lonp[0] = (int)floor((lon - 50.0)/90.0)*90 + 40;
|
||||
lonp[2] = lonp[0] + 90;
|
||||
}
|
||||
else if (lat >= 85.0)
|
||||
{
|
||||
for (i=0; i<4; i++) lonp[i] = (int)floor(lon/90.0)*90;
|
||||
}
|
||||
else if (lat <- 85.0)
|
||||
{
|
||||
for (i=0; i<4 ;i++) lonp[i] = (int)floor((lon - 50.0)/90.0)*90 + 40;
|
||||
}
|
||||
}
|
||||
|
||||
for (i=0; i<4; i++) if (lonp[i] == 180) lonp[i] = -180;
|
||||
|
||||
// find the correction data for the grid points in latp[] and lonp[]
|
||||
// iterate over bands
|
||||
for (std::vector<Igp_Band>::const_iterator band_it = this->d_bands.begin(); band_it != d_bands.end(); ++band_it)
|
||||
{
|
||||
//VLOG(MORE) << "band=" << band_it-d_bands.begin() << std::endl;
|
||||
// iterate over IGPs in band_it
|
||||
for (std::vector<Igp>::const_iterator igp_it = band_it->d_igps.begin(); igp_it != band_it->d_igps.end(); ++igp_it)
|
||||
{
|
||||
std::stringstream ss;
|
||||
int give = igp_it->d_give;
|
||||
//ss << "IGP: give=" << give;
|
||||
if(give < 15) // test if valid correction data is sent for current IGP
|
||||
{
|
||||
int lat = igp_it->d_latitude;
|
||||
int lon = igp_it->d_longitude;
|
||||
//ss << " lat=" << lat << " lon=" << lon;
|
||||
if (lat==latp[0] && lon==lonp[0]) igp[0] = igp_it.base();
|
||||
else if (lat==latp[1] && lon==lonp[1]) igp[1] = igp_it.base();
|
||||
else if (lat==latp[0] && lon==lonp[2]) igp[2] = igp_it.base();
|
||||
else if (lat==latp[1] && lon==lonp[3]) igp[3] = igp_it.base();
|
||||
}
|
||||
//VLOG(MORE) << ss.str();
|
||||
}
|
||||
}
|
||||
//VLOG(MORE) << "igp[0:3]={" << igp[0] << "," << igp[1] << "," << igp[2] << "," << igp[3] << "}";
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* sbas ionospheric delay correction -------------------------------------------
|
||||
* compute sbas ionosphric delay correction
|
||||
* args : long sample_stamp I sample stamp of observable on which the correction will be applied
|
||||
* sbsion_t *ion I ionospheric correction data (implicit)
|
||||
* double *pos I receiver position {lat,lon,height} (rad/m)
|
||||
* double *azel I satellite azimuth/elavation angle (rad)
|
||||
* double *delay O slant ionospheric delay (L1) (m)
|
||||
* double *var O variance of ionospheric delay (m^2)
|
||||
* return : status (1:ok, 0:no correction)
|
||||
* notes : before calling the function, sbas ionosphere correction parameters
|
||||
* in navigation data (nav->sbsion) must be set by calling
|
||||
* sbsupdatecorr()
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int Sbas_Ionosphere_Correction::sbsioncorr(const double sample_stamp, const double *pos,
|
||||
const double *azel, double *delay, double *var)
|
||||
{
|
||||
const double re = 6378.1363;
|
||||
const double hion = 350.0;
|
||||
int err = 0;
|
||||
double fp;
|
||||
double posp[2];
|
||||
double x = 0.0;
|
||||
double y = 0.0;
|
||||
double t;
|
||||
double w[4] = {0};
|
||||
const Igp *igp[4] = {0}; /* {ws,wn,es,en} */
|
||||
|
||||
trace(4, "sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f", pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D);
|
||||
|
||||
*delay = *var = 0.0;
|
||||
if (pos[2] < -100.0 || azel[1] <= 0) return 1;
|
||||
|
||||
/* ipp (ionospheric pierce point) position */
|
||||
fp = ionppp(pos, azel, re, hion, posp);
|
||||
|
||||
/* search igps around ipp */
|
||||
searchigp(posp, igp, &x, &y);
|
||||
|
||||
VLOG(FLOW) << "<<I>> SBAS iono correction:" << " igp[0]=" << igp[0] << " igp[1]=" << igp[1]
|
||||
<< " igp[2]=" << igp[2] << " igp[3]=" << igp[3] << " x=" << x << " y=" << y;
|
||||
|
||||
/* weight of igps */
|
||||
if (igp[0] && igp[1] && igp[2] && igp[3])
|
||||
{
|
||||
w[0] = (1.0 - x)*(1.0 - y);
|
||||
w[1] = (1.0 - x)*y;
|
||||
w[2] = x*(1.0 - y);
|
||||
w[3] = x*y;
|
||||
}
|
||||
else if (igp[0] && igp[1] && igp[2])
|
||||
{
|
||||
w[1] = y;
|
||||
w[2] = x;
|
||||
if ((w[0] = 1.0 - w[1] - w[2]) < 0.0) err = 1;
|
||||
}
|
||||
else if (igp[0] && igp[2] && igp[3])
|
||||
{
|
||||
w[0] = 1.0 - x;
|
||||
w[3] = y;
|
||||
if ((w[2] = 1.0 - w[0] -w[3]) < 0.0) err = 1;
|
||||
}
|
||||
else if (igp[0] && igp[1] && igp[3])
|
||||
{
|
||||
w[0] = 1.0 - y;
|
||||
w[3] = x;
|
||||
if ((w[1] = 1.0 - w[0] - w[3]) < 0.0) err = 1;
|
||||
}
|
||||
else if (igp[1]&&igp[2]&&igp[3])
|
||||
{
|
||||
w[1] = 1.0 - x;
|
||||
w[2] = 1.0 - y;
|
||||
if ((w[3] = 1.0 - w[1] - w[2]) < 0.0) err = 1;
|
||||
}
|
||||
else err = 1;
|
||||
|
||||
if (err)
|
||||
{
|
||||
trace(2, "no sbas iono correction: lat=%3.0f lon=%4.0f", posp[0]*R2D, posp[1]*R2D);
|
||||
return 0;
|
||||
}
|
||||
for (int i=0; i<4; i++)
|
||||
{
|
||||
if (!igp[i]) continue;
|
||||
t = (sample_stamp - igp[i]->t0); // time diff between now and reception of the igp data in seconds
|
||||
*delay += w[i]*igp[i]->d_delay;
|
||||
*var += w[i] * varicorr(igp[i]->d_give) * 9E-8 * fabs(t);
|
||||
}
|
||||
*delay *= fp;
|
||||
*var *= fp*fp;
|
||||
|
||||
trace(5, "sbsioncorr: dion=%7.2f sig=%7.2f", *delay, sqrt(*var));
|
||||
return 1;
|
||||
}
|
||||
200
src/core/system_parameters/sbas_ionospheric_correction.h
Normal file
200
src/core/system_parameters/sbas_ionospheric_correction.h
Normal file
|
|
@ -0,0 +1,200 @@
|
|||
/*!
|
||||
* \file sbas_ionospheric_correction.h
|
||||
* \brief Interface of the SBAS ionosphere correction set based on SBAS RTKLIB functions
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 SBAS_IONOSPHERIC_CORRECTION_H_
|
||||
#define SBAS_IONOSPHERIC_CORRECTION_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <boost/serialization/serialization.hpp>
|
||||
#include <boost/archive/text_oarchive.hpp>
|
||||
#include <boost/archive/text_iarchive.hpp>
|
||||
#include <boost/serialization/vector.hpp>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
|
||||
|
||||
struct Igp
|
||||
{
|
||||
public:
|
||||
//bool d_valid; // valid==true indicates that the IGP can be used for corrections. it is set to false when a new IGP mask (MT18) has been received but no corresponding delays (MT26)
|
||||
double t0; // time of reception, time of correction
|
||||
int d_latitude;
|
||||
int d_longitude;
|
||||
int d_give;
|
||||
double d_delay;
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive& ar, const unsigned int version)
|
||||
{
|
||||
ar & t0;
|
||||
ar & d_latitude;
|
||||
ar & d_longitude;
|
||||
ar & d_give;
|
||||
ar & d_delay;
|
||||
}
|
||||
};
|
||||
struct Igp_Band
|
||||
{
|
||||
//int d_iodi;
|
||||
//int d_nigp; // number if IGPs in this band (defined by IGP mask from MT18)
|
||||
std::vector<Igp> d_igps;
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive& ar, const unsigned int version)
|
||||
{
|
||||
ar & d_igps;
|
||||
}
|
||||
};
|
||||
|
||||
// valid ionosphere correction for GPS
|
||||
class Sbas_Ionosphere_Correction
|
||||
{
|
||||
private:
|
||||
// /* type definitions ----------------------------------------------------------*/
|
||||
//#define MAXBAND 10 /* max SBAS band of IGP */
|
||||
//#define MAXNIGP 201 /* max number of IGP in SBAS band */
|
||||
//
|
||||
// typedef struct { /* time struct */
|
||||
// time_t time; /* time (s) expressed by standard time_t */
|
||||
// double sec; /* fraction of second under 1 s */
|
||||
// } gtime_t;
|
||||
//
|
||||
// typedef struct { /* SBAS ionospheric correction type */
|
||||
// gtime_t t0; /* correction time */
|
||||
// short lat,lon; /* latitude/longitude (deg) */
|
||||
// short give; /* GIVI+1 */
|
||||
// float delay; /* vertical delay estimate (m) */
|
||||
// } sbsigp_t;
|
||||
//
|
||||
// typedef struct { /* SBAS ionospheric corrections type */
|
||||
// int iodi; /* IODI (issue of date ionos corr) */
|
||||
// int nigp; /* number of igps */
|
||||
// sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
|
||||
// } sbsion_t;
|
||||
|
||||
/* inner product ---------------------------------------------------------------
|
||||
* inner product of vectors
|
||||
* args : double *a,*b I vector a,b (n x 1)
|
||||
* int n I size of vector a,b
|
||||
* return : a'*b
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double dot(const double *a, const double *b, int n);
|
||||
/* multiply matrix -----------------------------------------------------------*/
|
||||
void matmul(const char *tr, int n, int k, int m, double alpha,
|
||||
const double *A, const double *B, double beta, double *C);
|
||||
/* ecef to local coordinate transfromation matrix ------------------------------
|
||||
* compute ecef to local coordinate transfromation matrix
|
||||
* args : double *pos I geodetic position {lat,lon} (rad)
|
||||
* double *E O ecef to local coord transformation matrix (3x3)
|
||||
* return : none
|
||||
* notes : matirix stored by column-major order (fortran convention)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
void xyz2enu(const double *pos, double *E);
|
||||
/* transform ecef vector to local tangental coordinate -------------------------
|
||||
* transform ecef vector to local tangental coordinate
|
||||
* args : double *pos I geodetic position {lat,lon} (rad)
|
||||
* double *r I vector in ecef coordinate {x,y,z}
|
||||
* double *e O vector in local tangental coordinate {e,n,u}
|
||||
* return : none
|
||||
*-----------------------------------------------------------------------------*/
|
||||
void ecef2enu(const double *pos, const double *r, double *e);
|
||||
/* satellite azimuth/elevation angle -------------------------------------------
|
||||
* compute satellite azimuth/elevation angle
|
||||
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
|
||||
* double *e I receiver-to-satellilte unit vevtor (ecef)
|
||||
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
|
||||
* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
|
||||
* return : elevation angle (rad)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double satazel(const double *pos, const double *e, double *azel);
|
||||
|
||||
/* debug trace functions -----------------------------------------------------*/
|
||||
void trace(int level, const char *format, ...);
|
||||
/* time difference -------------------------------------------------------------
|
||||
* difference between gtime_t structs
|
||||
* args : gtime_t t1,t2 I gtime_t structs
|
||||
* return : time difference (t1-t2) (s)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
//double timediff(gtime_t t1, gtime_t t2);
|
||||
/* ionospheric pierce point position -------------------------------------------
|
||||
* compute ionospheric pierce point (ipp) position and slant factor
|
||||
* args : double *pos I receiver position {lat,lon,h} (rad,m)
|
||||
* double *azel I azimuth/elevation angle {az,el} (rad)
|
||||
* double re I earth radius (km)
|
||||
* double hion I altitude of ionosphere (km)
|
||||
* double *posp O pierce point position {lat,lon,h} (rad,m)
|
||||
* return : slant factor
|
||||
* notes : see ref [2], only valid on the earth surface
|
||||
* fixing bug on ref [2] A.4.4.10.1 A-22,23
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double ionppp(const double *pos, const double *azel, double re,
|
||||
double hion, double *posp);
|
||||
/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/
|
||||
double varicorr(int give);
|
||||
/* search igps ---------------------------------------------------------------*/
|
||||
void searchigp(const double *pos, const Igp **igp, double *x, double *y);
|
||||
/* sbas ionospheric delay correction -------------------------------------------
|
||||
* compute sbas ionosphric delay correction
|
||||
* args : long sample_stamp I sample stamp of observable on which the correction will be applied
|
||||
* sbsion_t *ion I ionospheric correction data (implicit)
|
||||
* double *pos I receiver position {lat,lon,height} (rad/m)
|
||||
* double *azel I satellite azimuth/elavation angle (rad)
|
||||
* double *delay O slant ionospheric delay (L1) (m)
|
||||
* double *var O variance of ionospheric delay (m^2)
|
||||
* return : status (1:ok, 0:no correction)
|
||||
* notes : before calling the function, sbas ionosphere correction parameters
|
||||
* in navigation data (nav->sbsion) must be set by callig
|
||||
* sbsupdatecorr()
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int sbsioncorr(const double sample_stamp, const double *pos,
|
||||
const double *azel, double *delay, double *var);
|
||||
|
||||
|
||||
|
||||
public:
|
||||
std::vector<Igp_Band> d_bands;
|
||||
|
||||
void print(std::ostream &out);
|
||||
bool apply(double sample_stamp, double latitude_d, double longitude_d,
|
||||
double azimut_d, double evaluation_d, double &delay, double &var);
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive& ar, const unsigned int version){ar & d_bands;}
|
||||
};
|
||||
|
||||
|
||||
#endif /* SBAS_IONOSPHERIC_CORRECTION_H_ */
|
||||
314
src/core/system_parameters/sbas_satellite_correction.cc
Normal file
314
src/core/system_parameters/sbas_satellite_correction.cc
Normal file
|
|
@ -0,0 +1,314 @@
|
|||
/*!
|
||||
* \file sbas_satellite_correction.cc
|
||||
* \brief Implementation of the SBAS satellite correction set based on SBAS RTKLIB functions
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 <stdarg.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <iostream>
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#include "sbas_satellite_correction.h"
|
||||
|
||||
#define EVENT 2 // logs important events which don't occur every update() call
|
||||
#define FLOW 3 // logs the function calls of block processing functions
|
||||
|
||||
void
|
||||
Sbas_Satellite_Correction::print(std::ostream &out)
|
||||
{
|
||||
out << "<<S>> Sbas satellite corrections for PRN" << d_prn << ":" << std::endl;
|
||||
print_fast_correction(out);
|
||||
print_long_term_correction(out);
|
||||
}
|
||||
|
||||
void
|
||||
Sbas_Satellite_Correction::print_fast_correction(std::ostream &out)
|
||||
{
|
||||
Fast_Correction fcorr = d_fast_correction;
|
||||
out << "<<S>> Fast PRN" << d_prn << ":";
|
||||
if(fcorr.d_tof.is_related())
|
||||
{
|
||||
int gps_week;
|
||||
double gps_sec;
|
||||
fcorr.d_tof.get_gps_time(gps_week, gps_sec);
|
||||
out << " d_t0=(week=" << gps_week << ",sec=" << gps_sec << ")";
|
||||
}
|
||||
else
|
||||
{
|
||||
out << " d_t0=" << fcorr.d_tof.get_time_stamp();
|
||||
}
|
||||
out << " d_prc=" << fcorr.d_prc;
|
||||
out << " d_rrc=" << fcorr.d_rrc;
|
||||
out << " d_dt=" << fcorr.d_dt;
|
||||
out << " d_udre=" << fcorr.d_udre;
|
||||
out << " d_ai=" << fcorr.d_ai;
|
||||
out << " d_tlat=" << fcorr.d_tlat;
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
Sbas_Satellite_Correction::print_long_term_correction(std::ostream &out)
|
||||
{
|
||||
Long_Term_Correction lcorr = d_long_term_correction;
|
||||
out << "<<S>> Long PRN" << d_prn << ":";
|
||||
out << " d_trx=" << lcorr.d_trx;
|
||||
out << " i_tapp=" << lcorr.i_tapp;
|
||||
out << " i_vel=" << lcorr.i_vel;
|
||||
double *p = lcorr.d_dpos;
|
||||
out << " d_dpos=(x=" << p[0] << ", y=" << p[1] << ", z=" << p[2] << ")" ;
|
||||
double *v = lcorr.d_dvel;
|
||||
out << " d_dvel=(x=" << v[0] << ", y=" << v[1] << ", z=" << v[2] << ")" ;
|
||||
out << " d_daf0=" << lcorr.d_daf0;
|
||||
out << " d_daf1=" << lcorr.d_daf1;
|
||||
}
|
||||
|
||||
|
||||
int Sbas_Satellite_Correction::apply_fast(double sample_stamp, double &pr, double &var)
|
||||
{
|
||||
int result;
|
||||
double prc = 0; // pseudo range correction
|
||||
|
||||
result = sbsfastcorr(sample_stamp, &prc, &var);
|
||||
|
||||
pr += prc;
|
||||
|
||||
VLOG(FLOW) << "<<S>> fast correction applied: sample_stamp=" << sample_stamp << " prc=" << prc << " corr. pr=" << pr;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
int Sbas_Satellite_Correction::apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var)
|
||||
{
|
||||
int result;
|
||||
double drs[3] = {0};
|
||||
double ddts = 0;
|
||||
|
||||
result = sbslongcorr(sample_stamp, drs, &ddts);
|
||||
for (int i = 0; i < 3; i++) sv_pos[i] += drs[i];
|
||||
|
||||
VLOG(FLOW) << "<<S>> long term sv pos correction applied: sample_stamp=" << sample_stamp << " drs=(x=" << drs[0] << " y=" << drs[1] << " z=" << drs[2] << ")";
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
int Sbas_Satellite_Correction::apply_long_term_sv_clk(double sample_stamp, double &dts, double &var)
|
||||
{
|
||||
int result;
|
||||
double drs[3] = {0};
|
||||
double ddts = 0;
|
||||
|
||||
result = sbslongcorr(sample_stamp, drs, &ddts);
|
||||
dts += ddts;
|
||||
|
||||
VLOG(FLOW) << "<<S>> long term sv clock correction correction applied: sample_stamp=" << sample_stamp << " ddts=" << ddts;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
bool Sbas_Satellite_Correction::alarm()
|
||||
{
|
||||
return this->d_fast_correction.d_udre == 16;
|
||||
}
|
||||
|
||||
|
||||
#define CLIGHT 299792458.0 /* speed of light (m/s) */
|
||||
#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
|
||||
#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */
|
||||
|
||||
/* debug trace function -----------------------------------------------------*/
|
||||
void Sbas_Satellite_Correction::trace(int level, const char *format, ...)
|
||||
{
|
||||
va_list ap;
|
||||
char str[1000];
|
||||
|
||||
va_start(ap,format);
|
||||
vsprintf(str,format,ap);
|
||||
va_end(ap);
|
||||
VLOG(FLOW) << "<<S>> " << std::string(str);
|
||||
}
|
||||
|
||||
/* variance of fast correction (udre=UDRE+1) ---------------------------------*/
|
||||
double Sbas_Satellite_Correction::varfcorr(int udre)
|
||||
{
|
||||
const double var[14] = {
|
||||
0.052,0.0924,0.1444,0.283,0.4678,0.8315,1.2992,1.8709,2.5465,3.326,
|
||||
5.1968,20.7870,230.9661,2078.695
|
||||
};
|
||||
return 0 < udre && udre <= 14 ? var[udre-1]:0.0;
|
||||
}
|
||||
/* fast correction degradation -----------------------------------------------*/
|
||||
double Sbas_Satellite_Correction::degfcorr(int ai)
|
||||
{
|
||||
const double degf[16] = {
|
||||
0.00000,0.00005,0.00009,0.00012,0.00015,0.00020,0.00030,0.00045,
|
||||
0.00060,0.00090,0.00150,0.00210,0.00270,0.00330,0.00460,0.00580
|
||||
};
|
||||
return 0 < ai && ai <= 15 ? degf[ai]:0.0058;
|
||||
}
|
||||
|
||||
/* long term correction ------------------------------------------------------*/
|
||||
int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, double *ddts)
|
||||
{
|
||||
double t = 0.0;
|
||||
int i;
|
||||
|
||||
Long_Term_Correction lcorr = d_long_term_correction;
|
||||
|
||||
trace(3, "sbslongcorr: prn=%2d", this->d_prn);
|
||||
|
||||
// if (p->sat!=sat||p->lcorr.t0.time==0) continue;
|
||||
|
||||
// compute time of applicability
|
||||
if(d_long_term_correction.i_vel == 1)
|
||||
{ // time of applicability is the one sent, i.e., tapp
|
||||
// TODO: adapt for vel==1 case
|
||||
// t = tow-d_long_term_correction.i_tapp;
|
||||
// vel=1 -> time of applicability is sent in message, needs to be corrected for rollover which can not be done here, since the absolute gps time is unknown. see IS-GPS-200G pdf page 116 for correction
|
||||
/* t = (int)getbitu(msg->msg, p + 90, 13)*16 - (int)msg->tow%86400;
|
||||
if (t <= -43200) t += 86400;
|
||||
else if (t > 43200) t -= 86400;
|
||||
sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/
|
||||
}
|
||||
else
|
||||
{ // time of applicability is time of reception
|
||||
t = time_stamp - lcorr.d_trx; // should not have any impact if vel==0 since d_dvel and d_daf1 are zero, is only used to check outdating
|
||||
}
|
||||
|
||||
//t=time_stamp-lcorr.d_t0;
|
||||
|
||||
if (fabs(t) > MAXSBSAGEL)
|
||||
{
|
||||
trace(2,"sbas long-term correction expired: sat=%2d time_stamp=%5.0f", d_prn, time_stamp);
|
||||
return 0;
|
||||
}
|
||||
|
||||
// sv position correction
|
||||
for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*t;
|
||||
|
||||
// sv clock correction correction
|
||||
*ddts = lcorr.d_daf0 + lcorr.d_daf1*t;
|
||||
|
||||
trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f", d_prn, drs[0], drs[1], drs[2], *ddts*CLIGHT);
|
||||
|
||||
return 1;
|
||||
|
||||
/* if sbas satellite without correction, no correction applied */
|
||||
//if (satsys(sat,NULL)==SYS_SBS) return 1;
|
||||
|
||||
//trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
|
||||
//return 0;
|
||||
}
|
||||
/* fast correction -----------------------------------------------------------*/
|
||||
int Sbas_Satellite_Correction::sbsfastcorr(double time_stamp, double *prc, double *var)
|
||||
#define RRCENA
|
||||
{
|
||||
double t;
|
||||
|
||||
Fast_Correction fcorr = d_fast_correction;
|
||||
|
||||
trace(3, "sbsfastcorr: sat=%2d", this->d_prn);
|
||||
|
||||
//if (p->fcorr.t0.time==0) break; // time==0is only true if t0 hasn't been initialised -> it checks if the correction is valid
|
||||
t = (time_stamp - fcorr.d_tof.get_time_stamp()) + fcorr.d_tlat; // delta t between now and tof
|
||||
|
||||
/* expire age of correction? */
|
||||
if (fabs(t) > MAXSBSAGEF)
|
||||
{
|
||||
trace(2, "no sbas fast correction (expired): time_stamp=%f prn=%2d", time_stamp, d_prn);
|
||||
return 0;
|
||||
}
|
||||
/* UDRE==14 (not monitored)? */
|
||||
else if(fcorr.d_udre == 15)
|
||||
{
|
||||
trace(2,"no sbas fast correction (not monitored): time_stamp=%f prn=%2d", time_stamp, d_prn);
|
||||
return 0;
|
||||
}
|
||||
else if(fcorr.d_udre == 16)
|
||||
{
|
||||
trace(2,"SV is marked as unhealthy: time_stamp=%f prn=%2d", time_stamp, d_prn);
|
||||
return 0;
|
||||
}
|
||||
*prc = fcorr.d_prc;
|
||||
#ifdef RRCENA
|
||||
if (fcorr.d_ai > 0 && fabs(t) <= 8.0*fcorr.d_dt)
|
||||
{
|
||||
*prc += fcorr.d_rrc*t;
|
||||
}
|
||||
#endif
|
||||
*var = varfcorr(fcorr.d_udre) + degfcorr(fcorr.d_ai)*t*t/2.0;
|
||||
|
||||
trace(5, "sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f", d_prn, *prc, sqrt(*var), t);
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* sbas satellite ephemeris and clock correction -------------------------------
|
||||
* correct satellite position and clock bias with sbas satellite corrections
|
||||
* args : long time_stamp I reception time stamp
|
||||
* double *rs IO sat position and corrected {x,y,z} (ecef) (m)
|
||||
* double *dts IO sat clock bias and corrected (s)
|
||||
* double *var O sat position and clock variance (m^2)
|
||||
* return : status (1:ok,0:no correction)
|
||||
* notes : before calling the function, sbas satellite correction parameters
|
||||
* in navigation data (nav->sbssat) must be set by callig
|
||||
* sbsupdatecorr().
|
||||
* satellite clock correction include long-term correction and fast
|
||||
* correction.
|
||||
* sbas clock correction is usually based on L1C/A code. TGD or DCB has
|
||||
* to be considered for other codes
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int Sbas_Satellite_Correction::sbssatcorr(double time_stamp, double *rs, double *dts, double *var)
|
||||
{
|
||||
double drs[3] = {0}, dclk = 0.0, prc = 0.0;
|
||||
int i;
|
||||
|
||||
trace(3,"sbssatcorr : sat=%2d",d_prn);
|
||||
|
||||
/* sbas long term corrections */
|
||||
if (!sbslongcorr(time_stamp, drs, &dclk))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
/* sbas fast corrections */
|
||||
if (!sbsfastcorr(time_stamp, &prc, var))
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
for (i=0; i<3; i++) rs[i] += drs[i];
|
||||
|
||||
dts[0] += dclk + prc/CLIGHT;
|
||||
|
||||
trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f",
|
||||
d_prn,drs[0],drs[1],drs[2],dclk,prc/CLIGHT,*var);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
104
src/core/system_parameters/sbas_satellite_correction.h
Normal file
104
src/core/system_parameters/sbas_satellite_correction.h
Normal file
|
|
@ -0,0 +1,104 @@
|
|||
/*!
|
||||
* \file sbas_satellite_correction.h
|
||||
* \brief Interface of the SBAS satellite correction set based on SBAS RTKLIB functions
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 "sbas_time.h"
|
||||
|
||||
#ifndef GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_
|
||||
#define GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_
|
||||
|
||||
struct Fast_Correction
|
||||
{
|
||||
Sbas_Time d_tof; // for fast corrections the time of applicability (tof) is defined as the time when the corresponding message was send
|
||||
double d_prc;
|
||||
double d_rrc;
|
||||
double d_dt;
|
||||
int d_udre; // UDRE
|
||||
int d_ai;
|
||||
int d_tlat;
|
||||
};
|
||||
|
||||
struct Long_Term_Correction
|
||||
{
|
||||
double d_trx; // time when message was received
|
||||
int i_tapp; // time of applicability (only valid if vel=1, equals the sent t0)
|
||||
int i_vel; // use velocity corrections if vel=1
|
||||
int d_iode;
|
||||
double d_dpos[3]; // position correction
|
||||
double d_dvel[3]; // velocity correction
|
||||
double d_daf0; // clock offset correction
|
||||
double d_daf1; // clock drift correction
|
||||
};
|
||||
|
||||
// valid long and fast term correction for one SV
|
||||
class Sbas_Satellite_Correction
|
||||
{
|
||||
public:
|
||||
int d_prn;
|
||||
Fast_Correction d_fast_correction;
|
||||
Long_Term_Correction d_long_term_correction;
|
||||
void print(std::ostream &out);
|
||||
void print_fast_correction(std::ostream &out);
|
||||
void print_long_term_correction(std::ostream &out);
|
||||
int apply_fast(double sample_stamp, double &pr, double &var);
|
||||
int apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var);
|
||||
int apply_long_term_sv_clk(double sample_stamp, double &dts, double &var);
|
||||
bool alarm();
|
||||
|
||||
private:
|
||||
/* debug trace functions -----------------------------------------------------*/
|
||||
void trace(int level, const char *format, ...);
|
||||
/* variance of fast correction (udre=UDRE+1) ---------------------------------*/
|
||||
double varfcorr(int udre);
|
||||
/* fast correction degradation -----------------------------------------------*/
|
||||
double degfcorr(int ai);
|
||||
/* long term correction ------------------------------------------------------*/
|
||||
int sbslongcorr(double time_stamp, double *drs, double *ddts);
|
||||
/* fast correction -----------------------------------------------------------*/
|
||||
int sbsfastcorr(double time_stamp, double *prc, double *var);
|
||||
/* sbas satellite ephemeris and clock correction -------------------------------
|
||||
* correct satellite position and clock bias with sbas satellite corrections
|
||||
* args : long time_stamp I reception time stamp
|
||||
* double *rs IO sat position and corrected {x,y,z} (ecef) (m)
|
||||
* double *dts IO sat clock bias and corrected (s)
|
||||
* double *var O sat position and clock variance (m^2)
|
||||
* return : status (1:ok,0:no correction)
|
||||
* notes : before calling the function, sbas satellite correction parameters
|
||||
* in navigation data (nav->sbssat) must be set by callig
|
||||
* sbsupdatecorr().
|
||||
* satellite clock correction include long-term correction and fast
|
||||
* correction.
|
||||
* sbas clock correction is usually based on L1C/A code. TGD or DCB has
|
||||
* to be considered for other codes
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int sbssatcorr(double time_stamp, double *rs, double *dts, double *var);
|
||||
};
|
||||
|
||||
|
||||
#endif /* GNSS_SDR_SBAS_SATELLITE_CORRECTION_H_ */
|
||||
1011
src/core/system_parameters/sbas_telemetry_data.cc
Normal file
1011
src/core/system_parameters/sbas_telemetry_data.cc
Normal file
File diff suppressed because it is too large
Load Diff
514
src/core/system_parameters/sbas_telemetry_data.h
Normal file
514
src/core/system_parameters/sbas_telemetry_data.h
Normal file
|
|
@ -0,0 +1,514 @@
|
|||
/*!
|
||||
* \file sbas_telemetry_data.h
|
||||
* \brief Interface of the SBAS telemetry parser based on SBAS RTKLIB functions
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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_SBAS_TELEMETRY_DATA_H_
|
||||
#define GNSS_SDR_SBAS_TELEMETRY_DATA_H_
|
||||
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <algorithm>
|
||||
#include <bitset>
|
||||
#include "boost/assign.hpp"
|
||||
#include <cmath>
|
||||
|
||||
//#include "sbas_satellite_correction.h"
|
||||
//#include "sbas_ionospheric_correction.h"
|
||||
|
||||
#include "concurrent_queue.h"
|
||||
|
||||
#include "sbas_time.h"
|
||||
|
||||
class Sbas_Ionosphere_Correction;
|
||||
class Sbas_Satellite_Correction;
|
||||
struct Fast_Correction;
|
||||
struct Long_Term_Correction;
|
||||
class Sbas_Ephemeris;
|
||||
|
||||
/*
|
||||
* \brief represents a raw SBAS message of 250bits + 6bits padding
|
||||
* (8b preamble + 6b message type + 212b data + 24b CRC + 6b zerro padding)
|
||||
*/
|
||||
class Sbas_Raw_Msg
|
||||
{
|
||||
public:
|
||||
Sbas_Raw_Msg(){rx_time = Sbas_Time(0); i_prn = -1;};
|
||||
//Sbas_Raw_Msg(int week, int tow, int prn, const std::vector<unsigned char> msg) : d_week(week), d_tow(tow), d_prn(prn), d_msg(msg) {}
|
||||
Sbas_Raw_Msg(double sample_stamp, int prn, const std::vector<unsigned char> msg) : rx_time(sample_stamp), i_prn(prn), d_msg(msg) {}
|
||||
//int get_week() {return d_week;}
|
||||
//int get_tow() {return d_tow;}
|
||||
double get_sample_stamp() {return rx_time.get_time_stamp();} // time of reception sample stamp (first sample of preample)
|
||||
void relate(Sbas_Time_Relation time_relation)
|
||||
{
|
||||
rx_time.relate(time_relation);
|
||||
}
|
||||
Sbas_Time get_rx_time_obj(){return rx_time;}
|
||||
int get_prn() {return i_prn;}
|
||||
std::vector<unsigned char> get_msg() const {return d_msg;}
|
||||
int get_preamble()
|
||||
{
|
||||
return d_msg[0];
|
||||
}
|
||||
int get_msg_type()
|
||||
{
|
||||
return d_msg[1] >> 2;
|
||||
}
|
||||
int get_crc()
|
||||
{
|
||||
unsigned char crc_last_byte = (d_msg[30] << 2) && (d_msg[31] >> 6);
|
||||
unsigned char crc_middle_byte = (d_msg[29] << 2) && (d_msg[30] >> 6);
|
||||
unsigned char crc_first_byte = (d_msg[28] << 2) && (d_msg[29] >> 6);
|
||||
return ((unsigned int)(crc_first_byte) << 16) && ((unsigned int)(crc_middle_byte) << 8) && crc_last_byte;
|
||||
}
|
||||
|
||||
private:
|
||||
//int d_week; /* reception time */
|
||||
//int d_tow; /* reception time */
|
||||
Sbas_Time rx_time;
|
||||
int i_prn; /* SBAS satellite PRN number */
|
||||
std::vector<unsigned char> d_msg; /* SBAS message (226bit) padded by 0 */
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* \brief holds an updated set of the telemetry data received from SBAS
|
||||
*/
|
||||
|
||||
class Sbas_Telemetry_Data
|
||||
{
|
||||
public:
|
||||
|
||||
int
|
||||
update(Sbas_Raw_Msg sbas_raw_msg);
|
||||
|
||||
void
|
||||
set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue);
|
||||
void
|
||||
set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue);
|
||||
void
|
||||
set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue);
|
||||
void
|
||||
set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue);
|
||||
|
||||
/*!
|
||||
* Default constructor
|
||||
*/
|
||||
Sbas_Telemetry_Data();
|
||||
/*!
|
||||
* Default deconstructor
|
||||
*/
|
||||
~Sbas_Telemetry_Data();
|
||||
|
||||
private:
|
||||
std::map<int, Fast_Correction> emitted_fast_corrections;
|
||||
std::map<int, Long_Term_Correction> emitted_long_term_corrections;
|
||||
|
||||
Sbas_Time_Relation mt12_time_ref;
|
||||
|
||||
concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue;
|
||||
concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue;
|
||||
concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue;
|
||||
concurrent_queue<Sbas_Ephemeris> *ephemeris_queue;
|
||||
|
||||
int decode_mt12(Sbas_Raw_Msg sbas_raw_msg);
|
||||
|
||||
void updated_sbas_ephemeris(Sbas_Raw_Msg msg);
|
||||
void received_iono_correction();
|
||||
void updated_satellite_corrections();
|
||||
|
||||
/////// rtklib.h
|
||||
|
||||
#define SYS_NONE 0x00 /* navigation system: none */
|
||||
#define SYS_GPS 0x01 /* navigation system: GPS */
|
||||
#define SYS_SBS 0x02 /* navigation system: SBAS */
|
||||
#define SYS_GLO 0x04 /* navigation system: GLONASS */
|
||||
#define SYS_GAL 0x08 /* navigation system: Galileo */
|
||||
#define SYS_QZS 0x10 /* navigation system: QZSS */
|
||||
#define SYS_CMP 0x20 /* navigation system: BeiDou */
|
||||
#define SYS_ALL 0xFF /* navigation system: all */
|
||||
|
||||
#define TSYS_GPS 0 /* time system: GPS time */
|
||||
#define TSYS_UTC 1 /* time system: UTC */
|
||||
#define TSYS_GLO 2 /* time system: GLONASS time */
|
||||
#define TSYS_GAL 3 /* time system: Galileo time */
|
||||
#define TSYS_QZS 4 /* time system: QZSS time */
|
||||
#define TSYS_CMP 5 /* time system: BeiDou time */
|
||||
|
||||
#ifndef NFREQ
|
||||
#define NFREQ 3 /* number of carrier frequencies */
|
||||
#endif
|
||||
#define NFREQGLO 2 /* number of carrier frequencies of GLONASS */
|
||||
|
||||
#ifndef NEXOBS
|
||||
#define NEXOBS 0 /* number of extended obs codes */
|
||||
#endif
|
||||
|
||||
#define MINPRNGPS 1 /* min satellite PRN number of GPS */
|
||||
#define MAXPRNGPS 32 /* max satellite PRN number of GPS */
|
||||
#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */
|
||||
#define NSYSGPS 1
|
||||
|
||||
#ifdef ENAGLO
|
||||
#define MINPRNGLO 1 /* min satellite slot number of GLONASS */
|
||||
#define MAXPRNGLO 24 /* max satellite slot number of GLONASS */
|
||||
#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */
|
||||
#define NSYSGLO 1
|
||||
#else
|
||||
#define MINPRNGLO 0
|
||||
#define MAXPRNGLO 0
|
||||
#define NSATGLO 0
|
||||
#define NSYSGLO 0
|
||||
#endif
|
||||
#ifdef ENAGAL
|
||||
#define MINPRNGAL 1 /* min satellite PRN number of Galileo */
|
||||
#define MAXPRNGAL 27 /* max satellite PRN number of Galileo */
|
||||
#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */
|
||||
#define NSYSGAL 1
|
||||
#else
|
||||
#define MINPRNGAL 0
|
||||
#define MAXPRNGAL 0
|
||||
#define NSATGAL 0
|
||||
#define NSYSGAL 0
|
||||
#endif
|
||||
#ifdef ENAQZS
|
||||
#define MINPRNQZS 193 /* min satellite PRN number of QZSS */
|
||||
#define MAXPRNQZS 195 /* max satellite PRN number of QZSS */
|
||||
#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */
|
||||
#define MAXPRNQZS_S 185 /* max satellite PRN number of QZSS SAIF */
|
||||
#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */
|
||||
#define NSYSQZS 1
|
||||
#else
|
||||
#define MINPRNQZS 0
|
||||
#define MAXPRNQZS 0
|
||||
#define NSATQZS 0
|
||||
#define NSYSQZS 0
|
||||
#endif
|
||||
#ifdef ENACMP
|
||||
#define MINPRNCMP 1 /* min satellite sat number of BeiDou */
|
||||
#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */
|
||||
#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */
|
||||
#define NSYSCMP 1
|
||||
#else
|
||||
#define MINPRNCMP 0
|
||||
#define MAXPRNCMP 0
|
||||
#define NSATCMP 0
|
||||
#define NSYSCMP 0
|
||||
#endif
|
||||
#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP) /* number of systems */
|
||||
|
||||
#define MINPRNSBS 120 /* min satellite PRN number of SBAS */
|
||||
#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */
|
||||
#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */
|
||||
|
||||
#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATSBS)
|
||||
/* max satellite number (1 to MAXSAT) */
|
||||
#ifndef MAXOBS
|
||||
#define MAXOBS 64 /* max number of obs in an epoch */
|
||||
#endif
|
||||
#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */
|
||||
#define MAXOBSTYPE 64 /* max number of obs type in RINEX */
|
||||
#define DTTOL 0.005 /* tolerance of time difference (s) */
|
||||
#if 0
|
||||
#define MAXDTOE 10800.0 /* max time difference to ephem Toe (s) for GPS */
|
||||
#else
|
||||
#define MAXDTOE 7200.0 /* max time difference to ephem Toe (s) for GPS */
|
||||
#endif
|
||||
#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */
|
||||
#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */
|
||||
#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */
|
||||
#define MAXGDOP 300.0 /* max GDOP */
|
||||
|
||||
//#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
|
||||
//#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */
|
||||
//#define MAXSBSURA 8 /* max URA of SBAS satellite */
|
||||
#define MAXBAND 10 /* max SBAS band of IGP */
|
||||
#define MAXNIGP 201 /* max number of IGP in SBAS band */
|
||||
//#define MAXNGEO 4 /* max number of GEO satellites */
|
||||
|
||||
|
||||
#define P2_11 4.882812500000000E-04 /* 2^-11 */
|
||||
#define P2_31 4.656612873077393E-10 /* 2^-31 */
|
||||
#define P2_39 1.818989403545856E-12 /* 2^-39 */
|
||||
|
||||
/* type definitions ----------------------------------------------------------*/
|
||||
|
||||
typedef struct { /* time struct */
|
||||
time_t time; /* time (s) expressed by standard time_t */
|
||||
double sec; /* fraction of second under 1 s */
|
||||
} gtime_t;
|
||||
|
||||
typedef struct { /* SBAS message type */
|
||||
//int week,tow; /* receiption time */
|
||||
double sample_stamp;
|
||||
int prn; /* SBAS satellite PRN number */
|
||||
unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */
|
||||
} sbsmsg_t;
|
||||
|
||||
typedef struct { /* SBAS messages type */
|
||||
int n,nmax; /* number of SBAS messages/allocated */
|
||||
sbsmsg_t *msgs; /* SBAS messages */
|
||||
} sbs_t;
|
||||
|
||||
typedef struct { /* SBAS fast correction type */
|
||||
//gtime_t t0; /* time of applicability (TOF) */
|
||||
double t0;
|
||||
bool valid;
|
||||
double prc; /* pseudorange correction (PRC) (m) */
|
||||
double rrc; /* range-rate correction (RRC) (m/s) */
|
||||
double dt; /* range-rate correction delta-time (s) */
|
||||
int iodf; /* IODF (issue of date fast corr) */
|
||||
short udre; /* UDRE+1 */
|
||||
short ai; /* degradation factor indicator */
|
||||
} sbsfcorr_t;
|
||||
|
||||
typedef struct { /* SBAS long term satellite error correction type */
|
||||
//gtime_t t0; /* correction time */
|
||||
double trx; // time when message was received
|
||||
int tapp; // time of applicability (when vel=1 sent as t0)
|
||||
int vel; // use velocity if vel=1
|
||||
bool valid;
|
||||
int iode; /* IODE (issue of date ephemeris) */
|
||||
double dpos[3]; /* delta position (m) (ecef) */
|
||||
double dvel[3]; /* delta velocity (m/s) (ecef) */
|
||||
double daf0,daf1; /* delta clock-offset/drift (s,s/s) */
|
||||
} sbslcorr_t;
|
||||
|
||||
typedef struct { /* SBAS satellite correction type */
|
||||
int sat; /* satellite number */
|
||||
sbsfcorr_t fcorr; /* fast correction */
|
||||
sbslcorr_t lcorr; /* long term correction */
|
||||
} sbssatp_t;
|
||||
|
||||
typedef struct { /* SBAS satellite corrections type */
|
||||
int iodp; /* IODP (issue of date mask) */
|
||||
int nsat; /* number of satellites */
|
||||
int tlat; /* system latency (s) */
|
||||
sbssatp_t sat[MAXSAT]; /* satellite correction */
|
||||
} sbssat_t;
|
||||
|
||||
typedef struct { /* SBAS ionospheric correction type */
|
||||
//gtime_t t0; /* correction time */
|
||||
double t0;
|
||||
bool valid;
|
||||
short lat,lon; /* latitude/longitude (deg) */
|
||||
short give; /* GIVI+1 */
|
||||
float delay; /* vertical delay estimate (m) */
|
||||
} sbsigp_t;
|
||||
|
||||
typedef struct { /* IGP band type */
|
||||
short x; /* longitude/latitude (deg) */
|
||||
const short *y; /* latitudes/longitudes (deg) */
|
||||
unsigned char bits; /* IGP mask start bit */
|
||||
unsigned char bite; /* IGP mask end bit */
|
||||
} sbsigpband_t;
|
||||
|
||||
typedef struct { /* SBAS ionospheric corrections type */
|
||||
int iodi; /* IODI (issue of date ionos corr) */
|
||||
int nigp; /* number of igps */
|
||||
sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
|
||||
} sbsion_t;
|
||||
|
||||
/*
|
||||
* idicators
|
||||
*/
|
||||
|
||||
typedef struct { /* SBAS ephemeris type */
|
||||
int sat; /* satellite number */
|
||||
//gtime_t t0; /* reference epoch time (GPST) */
|
||||
int t0;
|
||||
//gtime_t tof; /* time of message frame (GPST) */
|
||||
double tof;
|
||||
int sva; /* SV accuracy (URA index) */
|
||||
int svh; /* SV health (0:ok) */
|
||||
double pos[3]; /* satellite position (m) (ecef) */
|
||||
double vel[3]; /* satellite velocity (m/s) (ecef) */
|
||||
double acc[3]; /* satellite acceleration (m/s^2) (ecef) */
|
||||
double af0,af1; /* satellite clock-offset/drift (s,s/s) */
|
||||
} seph_t;
|
||||
|
||||
typedef struct { /* navigation data type */
|
||||
//int n,nmax; /* number of broadcast ephemeris */
|
||||
//int ng,ngmax; /* number of glonass ephemeris */
|
||||
//int ns,nsmax; /* number of sbas ephemeris */
|
||||
//int ne,nemax; /* number of precise ephemeris */
|
||||
//int nc,ncmax; /* number of precise clock */
|
||||
//int na,namax; /* number of almanac data */
|
||||
//int nt,ntmax; /* number of tec grid data */
|
||||
//int nn,nnmax; /* number of stec grid data */
|
||||
//eph_t *eph; /* GPS/QZS/GAL ephemeris */
|
||||
//geph_t *geph; /* GLONASS ephemeris */
|
||||
seph_t seph[2*NSATSBS]; /* SBAS ephemeris */
|
||||
// peph_t *peph; /* precise ephemeris */
|
||||
// pclk_t *pclk; /* precise clock */
|
||||
// alm_t *alm; /* almanac data */
|
||||
// tec_t *tec; /* tec grid data */
|
||||
// stec_t *stec; /* stec grid data */
|
||||
// erp_t erp; /* earth rotation parameters */
|
||||
|
||||
//double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */
|
||||
//double utc_glo[4]; /* GLONASS UTC GPS time parameters */
|
||||
//double utc_gal[4]; /* Galileo UTC GPS time parameters */
|
||||
//double utc_qzs[4]; /* QZS UTC GPS time parameters */
|
||||
//double utc_cmp[4]; /* BeiDou UTC parameters */
|
||||
//double utc_sbs[4]; /* SBAS UTC parameters */
|
||||
//double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
|
||||
//double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */
|
||||
//double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
|
||||
//double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
|
||||
//int leaps; /* leap seconds (s) */
|
||||
//double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
|
||||
//double cbias[MAXSAT][3]; /* code bias (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
|
||||
//double wlbias[MAXSAT]; /* wide-lane bias (cycle) */
|
||||
//double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */
|
||||
//char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
|
||||
// pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
|
||||
sbssat_t sbssat; /* SBAS satellite corrections */
|
||||
sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
|
||||
// dgps_t dgps[MAXSAT]; /* DGPS corrections */
|
||||
// ssr_t ssr[MAXSAT]; /* SSR corrections */
|
||||
// lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
|
||||
// lexion_t lexion; /* LEX ionosphere correction */
|
||||
} nav_t;
|
||||
|
||||
//// common
|
||||
|
||||
static const double gpst0[]; /* gps time reference */
|
||||
|
||||
/* debug trace functions -----------------------------------------------------*/
|
||||
|
||||
FILE *fp_trace; /* file pointer of trace */
|
||||
int level_trace; /* level of trace */
|
||||
unsigned int tick_trace; /* tick time at traceopen (ms) */
|
||||
|
||||
void trace(int level, const char *format, ...);
|
||||
|
||||
/* satellite system+prn/slot number to satellite number ------------------------
|
||||
* convert satellite system+prn/slot number to satellite number
|
||||
* args : int sys I satellite system (SYS_GPS,SYS_GLO,...)
|
||||
* int prn I satellite prn/slot number
|
||||
* return : satellite number (0:error)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int satno(int sys, int prn);
|
||||
/* extract unsigned/signed bits ------------------------------------------------
|
||||
* extract unsigned/signed bits from byte data
|
||||
* args : unsigned char *buff I byte data
|
||||
* int pos I bit position from start of data (bits)
|
||||
* int len I bit length (bits) (len<=32)
|
||||
* return : extracted unsigned/signed bits
|
||||
*-----------------------------------------------------------------------------*/
|
||||
unsigned int getbitu(const unsigned char *buff, int pos, int len);
|
||||
int getbits(const unsigned char *buff, int pos, int len);
|
||||
|
||||
/* convert calendar day/time to time -------------------------------------------
|
||||
* convert calendar day/time to gtime_t struct
|
||||
* args : double *ep I day/time {year,month,day,hour,min,sec}
|
||||
* return : gtime_t struct
|
||||
* notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
gtime_t epoch2time(const double *ep);
|
||||
/* time difference -------------------------------------------------------------
|
||||
* difference between gtime_t structs
|
||||
* args : gtime_t t1,t2 I gtime_t structs
|
||||
* return : time difference (t1-t2) (s)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double timediff(gtime_t t1, gtime_t t2);
|
||||
/* gps time to time ------------------------------------------------------------
|
||||
* convert week and tow in gps time to gtime_t struct
|
||||
* args : int week I week number in gps time
|
||||
* double sec I time of week in gps time (s)
|
||||
* return : gtime_t struct
|
||||
*-----------------------------------------------------------------------------*/
|
||||
gtime_t gpst2time(int week, double sec);
|
||||
|
||||
|
||||
////// sbas.c
|
||||
|
||||
/* sbas igp definition -------------------------------------------------------*/
|
||||
static const short
|
||||
x1[],
|
||||
x2[],
|
||||
x3[],
|
||||
x4[],
|
||||
x5[],
|
||||
x6[],
|
||||
x7[],
|
||||
x8[];
|
||||
|
||||
static const sbsigpband_t igpband1[9][8];/* band 0-8 */
|
||||
static const sbsigpband_t igpband2[2][5];/* band 9-10 */
|
||||
|
||||
/* decode type 1: prn masks --------------------------------------------------*/
|
||||
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat);
|
||||
/* decode type 2-5,0: fast corrections ---------------------------------------*/
|
||||
int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat);
|
||||
/* decode type 6: integrity info ---------------------------------------------*/
|
||||
int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat);
|
||||
/* decode type 7: fast correction degradation factor -------------------------*/
|
||||
int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat);
|
||||
/* decode type 9: geo navigation message -------------------------------------*/
|
||||
int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav);
|
||||
/* decode type 18: ionospheric grid point masks ------------------------------*/
|
||||
int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion);
|
||||
/* decode half long term correction (vel code=0) -----------------------------*/
|
||||
int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
|
||||
/* decode half long term correction (vel code=1) -----------------------------*/
|
||||
int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
|
||||
/* decode half long term correction ------------------------------------------*/
|
||||
int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
|
||||
/* decode type 24: mixed fast/long term correction ---------------------------*/
|
||||
int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat);
|
||||
/* decode type 25: long term satellite error correction ----------------------*/
|
||||
int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat);
|
||||
/* decode type 26: ionospheric deley corrections -----------------------------*/
|
||||
int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion);
|
||||
/* update sbas corrections -----------------------------------------------------
|
||||
* update sbas correction parameters in navigation data with a sbas message
|
||||
* args : sbsmg_t *msg I sbas message
|
||||
* nav_t *nav IO navigation data
|
||||
* return : message type (-1: error or not supported type)
|
||||
* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t)
|
||||
* seph[prn-MINPRNSBS+1] : sat prn current epehmeris
|
||||
* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris
|
||||
*-----------------------------------------------------------------------------*/
|
||||
int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav);
|
||||
|
||||
void prn_mask_changed();
|
||||
bool is_rtklib_sat_correction_valid(int sat);
|
||||
void igp_mask_changed(int band);
|
||||
|
||||
// RTKLIB SBAS telemetry data representation
|
||||
nav_t d_nav;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
193
src/core/system_parameters/sbas_time.h
Normal file
193
src/core/system_parameters/sbas_time.h
Normal file
|
|
@ -0,0 +1,193 @@
|
|||
/*!
|
||||
* \file sbas_time.h
|
||||
* \brief Interface and implementation of classes to handle and relate sample stamp and GPS time based time scales
|
||||
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
|
||||
*
|
||||
* -------------------------------------------------------------------------
|
||||
*
|
||||
* Copyright (C) 2010-2013 (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 <cmath>
|
||||
#include <glog/log_severity.h>
|
||||
#include <glog/logging.h>
|
||||
|
||||
#ifndef GNSS_SDR_SBAS_TIME_H_
|
||||
#define GNSS_SDR_SBAS_TIME_H_
|
||||
|
||||
#define EVENT 2 // logs important events which don't occur every update() call
|
||||
#define FLOW 3 // logs the function calls of block processing functions
|
||||
|
||||
class Sbas_Time_Relation
|
||||
{
|
||||
public:
|
||||
Sbas_Time_Relation()
|
||||
{
|
||||
i_gps_week = 0;
|
||||
d_delta_sec = 0;
|
||||
b_valid = false;
|
||||
VLOG(FLOW) << "<<R>> new invalid time relation: i_gps_week=" << i_gps_week << " d_delta_sec=" << d_delta_sec;
|
||||
}
|
||||
|
||||
Sbas_Time_Relation(double time_stamp_sec, int gps_week, double gps_sec)
|
||||
{
|
||||
i_gps_week = gps_week;
|
||||
d_delta_sec = gps_sec - time_stamp_sec;
|
||||
b_valid = true;
|
||||
|
||||
VLOG(FLOW) << "<<R>> new time relation: i_gps_week=" << i_gps_week << " d_delta_sec=" << d_delta_sec;
|
||||
}
|
||||
|
||||
bool to_gps_time(double time_stamp_sec, int &gps_week, double &gps_sec)
|
||||
{
|
||||
int delta_weeks = int(trunc(time_stamp_sec + d_delta_sec))/604800;
|
||||
gps_sec = time_stamp_sec+d_delta_sec-delta_weeks*604800;
|
||||
gps_week = i_gps_week + delta_weeks;
|
||||
VLOG(FLOW) << "<<R>> to gps time: time_stamp_sec=" << time_stamp_sec << " gps_week=" << gps_week << " gps_sec=" << gps_sec;
|
||||
return b_valid;
|
||||
}
|
||||
|
||||
bool to_sample_stamp(int gps_week, double gps_sec, double &time_stamp_sec)
|
||||
{
|
||||
time_stamp_sec = (gps_sec - d_delta_sec) + (gps_week - i_gps_week)*604800;
|
||||
VLOG(FLOW) << "<<R>> to gps time: gps_week=" << gps_week << " gps_sec=" << gps_sec << " time_stamp_sec=" << time_stamp_sec;
|
||||
return b_valid;
|
||||
}
|
||||
|
||||
bool is_valid()
|
||||
{
|
||||
return b_valid;
|
||||
}
|
||||
|
||||
private:
|
||||
int i_gps_week;
|
||||
double d_delta_sec;
|
||||
bool b_valid;
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
* Sbas_Time relates the relative sample stamp time scale with the absolute GPS time scale.
|
||||
* There are three different states for a Sbas_Time object:
|
||||
* - only relative time (sample stamp) is known
|
||||
* - only absolute time (gps time) is known
|
||||
* - absolute and relative time and their relation is known
|
||||
*/
|
||||
|
||||
class Sbas_Time
|
||||
{
|
||||
public:
|
||||
|
||||
enum Sbas_Time_State {RELATIVE, /*ABSOLUTE,*/ RELATED, UNDEFINED};
|
||||
|
||||
Sbas_Time()
|
||||
{
|
||||
e_state = UNDEFINED;
|
||||
}
|
||||
|
||||
Sbas_Time(double time_stamp_sec)
|
||||
{
|
||||
d_time_stamp_sec = time_stamp_sec;
|
||||
i_gps_week = 0;
|
||||
d_gps_sec = 0;
|
||||
e_state = RELATIVE;
|
||||
}
|
||||
/*
|
||||
Sbas_Time(int gps_week, double gps_sec)
|
||||
{
|
||||
i_gps_week = gps_week;
|
||||
d_gps_sec = gps_sec;
|
||||
d_time_stamp_sec = 0;
|
||||
e_state = ABSOLUTE;
|
||||
}*/
|
||||
Sbas_Time(double time_stamp_sec, Sbas_Time_Relation relation)
|
||||
{
|
||||
if(relation.is_valid())
|
||||
{ // construct a RELATED object
|
||||
d_time_stamp_sec = time_stamp_sec;
|
||||
relation.to_gps_time(d_time_stamp_sec, i_gps_week, d_gps_sec);
|
||||
e_state = RELATED;
|
||||
}
|
||||
else
|
||||
{ // construct a RELATIVE object
|
||||
*this = Sbas_Time(time_stamp_sec);
|
||||
VLOG(FLOW) << "<<R>> create RELATIVE time (invalid relation): time_stamp_sec=" << time_stamp_sec;
|
||||
}
|
||||
}
|
||||
/*Sbas_Time(int gps_week, double gps_sec, Sbas_Time_Relation relation)
|
||||
{
|
||||
i_gps_week = gps_week;
|
||||
d_gps_sec = gps_sec;
|
||||
relation.to_sample_stamp(gps_week, gps_sec, d_time_stamp_sec);
|
||||
e_state = RELATED;
|
||||
}*/
|
||||
|
||||
void relate(Sbas_Time_Relation sbas_time_realtion)
|
||||
{
|
||||
switch (e_state)
|
||||
{
|
||||
case RELATIVE: *this = Sbas_Time(d_time_stamp_sec, sbas_time_realtion);
|
||||
break;
|
||||
|
||||
//case ABSOLUTE: return Sbas_Time(i_gps_week, d_gps_sec, sbas_time_realtion);
|
||||
break;
|
||||
|
||||
case RELATED: LOG(INFO) << "Relating an already related Sbas_Time object is not possible";
|
||||
break;
|
||||
|
||||
case UNDEFINED: std::cerr << "Sbas_Time object state undefined" << std::endl;
|
||||
break;
|
||||
|
||||
default: std::cerr << "Sbas_Time object state not known" << std::endl;
|
||||
break;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
double get_time_stamp()
|
||||
{
|
||||
return d_time_stamp_sec;
|
||||
//return (e_state == RELATIVE || e_state == RELATED);
|
||||
}
|
||||
|
||||
bool get_gps_time(int &gps_week, double &gps_sec)
|
||||
{
|
||||
gps_week = i_gps_week;
|
||||
gps_sec = d_gps_sec;
|
||||
return (/*e_state == ABSOLUTE ||*/ e_state == RELATED);
|
||||
}
|
||||
|
||||
bool is_only_relativ() {return e_state == RELATIVE;}
|
||||
//bool is_only_absolute() {return e_state == ABSOLUTE;}
|
||||
bool is_related() {return e_state == RELATED;}
|
||||
Sbas_Time_State get_state() {return e_state;}
|
||||
|
||||
private:
|
||||
Sbas_Time_State e_state;
|
||||
double d_time_stamp_sec;
|
||||
int i_gps_week;
|
||||
double d_gps_sec;
|
||||
};
|
||||
|
||||
|
||||
#endif /* SBAS_TIME_H_ */
|
||||
Loading…
Reference in New Issue
Block a user