mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-07-20 19:02:55 +00:00
Code Issues Releases Wiki Activity

Code cleaning

Browse Source 
git-svn-id: https://svn.code.sf.net/p/gnss-sdr/code/trunk@440 64b25241-fba3-4117-9849-534c7e92360d
This commit is contained in:
Carles Fernandez 2013-11-10 18:52:29 +00:00
parent 5b2abb62c2
commit aa5b531cd3
12 changed files with 751 additions and 788 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.cc
View File

@ -1,6 +1,6 @@
/*! /*!
* \file sbas_l1_telemetry_decoder.cc * \file sbas_l1_telemetry_decoder.cc
* \brief Implementation of an adapter of a SBAS telemtry data decoder block * \brief Implementation of an adapter of a SBAS telemetry data decoder block
* to a TelemetryDecoderInterface * to a TelemetryDecoderInterface
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
* *

5
src/algorithms/telemetry_decoder/adapters/sbas_l1_telemetry_decoder.h
View File

@ -1,6 +1,6 @@
/*! /*!
* \file sbas_l1_telemetry_decoder.h * \file sbas_l1_telemetry_decoder.h
* \brief Interface of an adapter of a SBAS telemtry data decoder block * \brief Interface of an adapter of a SBAS telemetry data decoder block
* to a TelemetryDecoderInterface * to a TelemetryDecoderInterface
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch * \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
* *
@ -58,6 +58,9 @@ public:
return role_; return role_;
} }
/*!
* \brief Returns "SBAS_L1_Telemetry_Decoder"
*/
std::string implementation() std::string implementation()
{ {
return "SBAS_L1_Telemetry_Decoder"; return "SBAS_L1_Telemetry_Decoder";

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

@ -212,10 +212,11 @@ sbas_l1_telemetry_decoder_cc::sample_aligner::sample_aligner()
d_iir_par = 0.05; d_iir_par = 0.05;
reset(); reset();
} }
sbas_l1_telemetry_decoder_cc::sample_aligner::~sample_aligner()
{
}
sbas_l1_telemetry_decoder_cc::sample_aligner::~sample_aligner()
{}
void sbas_l1_telemetry_decoder_cc::sample_aligner::reset() void sbas_l1_telemetry_decoder_cc::sample_aligner::reset()
{ {
@ -225,6 +226,7 @@ void sbas_l1_telemetry_decoder_cc::sample_aligner::reset()
d_aligned = true; d_aligned = true;
} }
/* /*
* samples length must be a multiple of two * samples length must be a multiple of two
*/ */
@ -278,13 +280,11 @@ bool sbas_l1_telemetry_decoder_cc::sample_aligner::get_symbols(const std::vector
double temp; double temp;
temp = samples.back(); temp = samples.back();
d_past_sample = (temp); d_past_sample = (temp);
return d_aligned; return d_aligned;
} }
// ### helper class for symbol alignment and viterbi decoding ### // ### helper class for symbol alignment and viterbi decoding ###
sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::symbol_aligner_and_decoder() sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::symbol_aligner_and_decoder()
{ {
// convolutional code properties // convolutional code properties
@ -298,6 +298,8 @@ sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::symbol_aligner_and_dec
d_vd2 = new Viterbi_Decoder(g_encoder, d_KK, nn); d_vd2 = new Viterbi_Decoder(g_encoder, d_KK, nn);
d_past_symbol = 0; d_past_symbol = 0;
} }
sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::~symbol_aligner_and_decoder() sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::~symbol_aligner_and_decoder()
{ {
delete d_vd1; delete d_vd1;
@ -311,14 +313,11 @@ void sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::reset()
d_vd2->reset(); d_vd2->reset();
} }
bool sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder:: bool sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::get_bits(const std::vector<double> symbols, std::vector<int> &bits)
get_bits(const std::vector<double> symbols, std::vector<int> &bits)
{ {
const int traceback_depth = 5*d_KK; const int traceback_depth = 5*d_KK;
int nbits_requested = symbols.size()/d_symbols_per_bit; int nbits_requested = symbols.size()/d_symbols_per_bit;
int nbits_decoded; int nbits_decoded;
// fill two vectors with the two possible symbol alignments // 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_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 std::vector<double> symbols_vd2; // shifted symbol vector -> add past sample in front of input vector
@ -327,15 +326,12 @@ get_bits(const std::vector<double> symbols, std::vector<int> &bits)
{ {
symbols_vd2.push_back(*symbol_it); symbols_vd2.push_back(*symbol_it);
} }
// arrays for decoded bits // arrays for decoded bits
int * bits_vd1 = new int[nbits_requested]; int * bits_vd1 = new int[nbits_requested];
int * bits_vd2 = new int[nbits_requested]; int * bits_vd2 = new int[nbits_requested];
// decode // decode
float metric_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1, nbits_requested, nbits_decoded); 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); 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 // choose the bits with the better metric
for (int i = 0; i<nbits_decoded; i++) for (int i = 0; i<nbits_decoded; i++)
{ {
@ -348,36 +344,28 @@ get_bits(const std::vector<double> symbols, std::vector<int> &bits)
bits.push_back(bits_vd2[i]); bits.push_back(bits_vd2[i]);
} }
} }
d_past_symbol = symbols.back(); d_past_symbol = symbols.back();
delete[] bits_vd1; delete[] bits_vd1;
delete[] bits_vd2; delete[] bits_vd2;
return metric_vd1 > metric_vd2; return metric_vd1 > metric_vd2;
} }
// ### helper class for detecting the preamble and collect the corresponding message candidates ### // ### helper class for detecting the preamble and collect the corresponding message candidates ###
void sbas_l1_telemetry_decoder_cc::frame_detector::reset() void sbas_l1_telemetry_decoder_cc::frame_detector::reset()
{ {
d_buffer.clear(); 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) 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; std::stringstream ss;
unsigned int sbas_msg_length = 250; unsigned int sbas_msg_length = 250;
std::vector<std::vector<int>> preambles = {{0, 1, 0, 1, 0, 0, 1 ,1}, std::vector<std::vector<int>> preambles = {{0, 1, 0, 1, 0, 0, 1 ,1},
{1, 0, 0, 1, 1, 0, 1, 0}, {1, 0, 0, 1, 1, 0, 1, 0},
{1, 1, 0, 0, 0, 1, 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(); VLOG(FLOW) << "get_frame_candidates(): " << "d_buffer.size()=" << d_buffer.size() << "\tbits.size()=" << bits.size();
ss << "copy bits "; ss << "copy bits ";
int count = 0; int count = 0;
// copy new bits into the working buffer // copy new bits into the working buffer
@ -388,7 +376,6 @@ get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std:
count++; count++;
} }
VLOG(SAMP_SYNC) << ss.str() << " into working buffer (" << count << " bits)"; VLOG(SAMP_SYNC) << ss.str() << " into working buffer (" << count << " bits)";
int relative_preamble_start = 0; int relative_preamble_start = 0;
while(d_buffer.size() >= sbas_msg_length) while(d_buffer.size() >= sbas_msg_length)
{ {
@ -415,7 +402,6 @@ get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std:
*candidate_bit_it = *candidate_bit_it == 0 ? 1:0; *candidate_bit_it = *candidate_bit_it == 0 ? 1:0;
} }
msg_candidates.push_back(std::pair<int,std::vector<int>>(relative_preamble_start, candidate)); msg_candidates.push_back(std::pair<int,std::vector<int>>(relative_preamble_start, candidate));
ss.str(""); ss.str("");
ss << "preamble " << preample_it - preambles.begin() << (inv_preamble_detected?" inverted":" normal") << " detected! candidate="; 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) for (std::vector<int>::iterator bit_it = candidate.begin(); bit_it < candidate.end(); ++bit_it)
@ -424,10 +410,8 @@ get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std:
} }
} }
relative_preamble_start++; relative_preamble_start++;
// remove bit in front // remove bit in front
d_buffer.pop_front(); d_buffer.pop_front();
} }
} }
@ -440,28 +424,22 @@ void sbas_l1_telemetry_decoder_cc::crc_verifier::reset()
} }
void sbas_l1_telemetry_decoder_cc::crc_verifier:: 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)
get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs)
{ {
std::stringstream ss; std::stringstream ss;
VLOG(FLOW) << "get_valid_frames(): " << "msg_candidates.size()=" << msg_candidates.size(); VLOG(FLOW) << "get_valid_frames(): " << "msg_candidates.size()=" << msg_candidates.size();
// for each candidate // for each candidate
for (std::vector<msg_candiate_int_t>::const_iterator candidate_it = msg_candidates.begin(); candidate_it < msg_candidates.end(); ++candidate_it) for (std::vector<msg_candiate_int_t>::const_iterator candidate_it = msg_candidates.begin(); candidate_it < msg_candidates.end(); ++candidate_it)
{ {
// convert to bytes // convert to bytes
std::vector<unsigned char> candidate_bytes; std::vector<unsigned char> candidate_bytes;
zerropad_back_and_convert_to_bytes(candidate_it->second, candidate_bytes); zerropad_back_and_convert_to_bytes(candidate_it->second, candidate_bytes);
// verify CRC // verify CRC
d_checksum_agent.reset(0); d_checksum_agent.reset(0);
d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size()); d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size());
unsigned int crc = d_checksum_agent.checksum(); unsigned int crc = d_checksum_agent.checksum();
VLOG(SAMP_SYNC) << "candidate " << candidate_it - msg_candidates.begin() << ": final crc remainder= " << std::hex << crc VLOG(SAMP_SYNC) << "candidate " << candidate_it - msg_candidates.begin() << ": final crc remainder= " << std::hex << crc
<< std::setfill(' ') << std::resetiosflags(std::ios::hex); << 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 // the final remainder must be zero for a valid message, because the CRC is done over the received CRC value
if (crc == 0) if (crc == 0)
{ {
@ -481,24 +459,22 @@ get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vect
} }
} }
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)
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; std::stringstream ss;
const size_t bits_per_byte = 8; const size_t bits_per_byte = 8;
unsigned char byte = 0; unsigned char byte = 0;
VLOG(LMORE) << "zerropad_back_and_convert_to_bytes():" << byte; 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) 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 idx_bit = candidate_bit_it - msg_candidate.begin();
int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte); 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; byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
ss << *candidate_bit_it; ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte - 1) if (idx_bit % bits_per_byte == bits_per_byte - 1)
{ {
bytes.push_back(byte); bytes.push_back(byte);
@ -511,52 +487,51 @@ zerropad_back_and_convert_to_bytes(const std::vector<int> msg_candidate, std::ve
<< std::setfill(' ') << std::resetiosflags(std::ios::hex); << 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)
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; std::stringstream ss;
const size_t bits_per_byte = 8; const size_t bits_per_byte = 8;
unsigned char byte = 0; unsigned char byte = 0;
int idx_bit = 6; // insert 6 zeros at the front to fit the 250bits into a multiple of bytes 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; 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) 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); 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; byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
ss << *candidate_bit_it; ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte - 1) if (idx_bit % bits_per_byte == bits_per_byte - 1)
{ {
bytes.push_back(byte); bytes.push_back(byte);
VLOG(LMORE) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str(""); VLOG(LMORE) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
byte = 0; byte = 0;
} }
idx_bit++; idx_bit++;
} }
VLOG(LMORE) << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte VLOG(LMORE) << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte
<< std::setfill(' ') << std::resetiosflags(std::ios::hex); << 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) 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); 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) void sbas_l1_telemetry_decoder_cc::set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue)
{ {
sbas_telemetry_data.set_iono_queue(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) 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); 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) void sbas_l1_telemetry_decoder_cc::set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue)
{ {
sbas_telemetry_data.set_ephemeris_queue(ephemeris_queue); sbas_telemetry_data.set_ephemeris_queue(ephemeris_queue);

13
src/algorithms/telemetry_decoder/gnuradio_blocks/sbas_l1_telemetry_decoder_cc.h
View File

@ -103,15 +103,12 @@ private:
public: public:
sample_aligner(); sample_aligner();
~sample_aligner(); ~sample_aligner();
void reset(); void reset();
/* /*
* samples length must be a multiple of two * samples length must be a multiple of two
* for block operation the * for block operation the
*/ */
bool get_symbols(const std::vector<double> samples, std::vector<double> &symbols); bool get_symbols(const std::vector<double> samples, std::vector<double> &symbols);
private: private:
int d_n_smpls_in_history ; int d_n_smpls_in_history ;
double d_iir_par; double d_iir_par;
@ -128,11 +125,8 @@ private:
public: public:
symbol_aligner_and_decoder(); symbol_aligner_and_decoder();
~symbol_aligner_and_decoder(); ~symbol_aligner_and_decoder();
void reset(); void reset();
bool get_bits(const std::vector<double> symbols, std::vector<int> &bits); bool get_bits(const std::vector<double> symbols, std::vector<int> &bits);
private: private:
int d_KK; int d_KK;
Viterbi_Decoder * d_vd1; Viterbi_Decoder * d_vd1;
@ -141,33 +135,32 @@ private:
} d_symbol_aligner_and_decoder; } d_symbol_aligner_and_decoder;
// helper class for detecting the preamble and collect the corresponding message candidates // helper class for detecting the preamble and collect the corresponding message candidates
class frame_detector class frame_detector
{ {
public: public:
void reset(); void reset();
void get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int, std::vector<int>>> &msg_candidates); void get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int, std::vector<int>>> &msg_candidates);
private: private:
std::deque<int> d_buffer; std::deque<int> d_buffer;
} d_frame_detector; } d_frame_detector;
// helper class for checking the CRC of the message candidates // helper class for checking the CRC of the message candidates
class crc_verifier class crc_verifier
{ {
public: public:
void reset(); void reset();
void get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs); void get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs);
private: private:
typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type; typedef boost::crc_optimal<24, 0x1864CFBu, 0x0, 0x0, false, false> crc_24_q_type;
crc_24_q_type d_checksum_agent; 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_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); void zerropad_back_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes);
} d_crc_verifier; } d_crc_verifier;
Sbas_Telemetry_Data sbas_telemetry_data; Sbas_Telemetry_Data sbas_telemetry_data;
}; };

3
src/core/system_parameters/sbas_ephemeris.cc
View File

@ -1,6 +1,6 @@
/* /*
* \file sbas_ephemeris.cc * \file sbas_ephemeris.cc
* \brief Interface of a SBAS REFERENCE LOCATION storage * \brief Implementation of a SBAS REFERENCE LOCATION storage
* *
* \author Daniel Fehr, 2013. daniel.co(at)bluewin.ch * \author Daniel Fehr, 2013. daniel.co(at)bluewin.ch
* *
@ -33,7 +33,6 @@
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
#include <iostream> #include <iostream>
#include "sbas_ephemeris.h" #include "sbas_ephemeris.h"
void Sbas_Ephemeris::print(std::ostream &out) void Sbas_Ephemeris::print(std::ostream &out)

25
src/core/system_parameters/sbas_ephemeris.h
View File

@ -1,6 +1,6 @@
/* /*
* \file sbas_ephemeris.h * \file sbas_ephemeris.h
* \brief Implementation of a SBAS REFERENCE LOCATION storage * \brief Interface of a SBAS REFERENCE LOCATION storage
* *
* \author Daniel Fehr, 2013. daniel.co(at)bluewin.ch * \author Daniel Fehr, 2013. daniel.co(at)bluewin.ch
* *
@ -33,23 +33,26 @@
#ifndef GNSS_SDR_SBAS_EPHEMERIS_H_ #ifndef GNSS_SDR_SBAS_EPHEMERIS_H_
#define GNSS_SDR_SBAS_EPHEMERIS_H_ #define GNSS_SDR_SBAS_EPHEMERIS_H_
/*!
* \brief This class stores SBAS SV ephemeris data
*
*/
class Sbas_Ephemeris class Sbas_Ephemeris
{ {
public: public:
void print(std::ostream &out); void print(std::ostream &out);
int i_prn; // PRN number */ int i_prn; //!< PRN number
//gtime_t t0; // reference epoch time (GPST) //gtime_t t0; //!< reference epoch time (GPST)
int i_t0; int i_t0;
//gtime_t tof; // time of message frame (GPST) //gtime_t tof; // time of message frame (GPST)
double d_tof; double d_tof;
int i_sv_ura; // SV accuracy (URA index), not standardized int i_sv_ura; //!< SV accuracy (URA index), not standardized
bool b_sv_do_not_use; // health status (false:don't use / true:usable) bool b_sv_do_not_use; //!< health status (false:do not use / true:usable)
double d_pos[3]; // satellite position (m) (ecef) double d_pos[3]; //!< satellite position (m) (ECEF)
double d_vel[3]; // satellite velocity (m/s) (ecef) double d_vel[3]; //!< satellite velocity (m/s) (ECEF)
double d_acc[3]; // satellite acceleration (m/s^2) (ecef) double d_acc[3]; //!< satellite acceleration (m/s^2) (ECEF)
double d_af0; // satellite clock-offset (s) double d_af0; //!< satellite clock-offset (s)
double d_af1; // satellite drift (s/s) double d_af1; //!< satellite drift (s/s)
}; };

38
src/core/system_parameters/sbas_ionospheric_correction.cc
View File

@ -92,11 +92,6 @@ bool Sbas_Ionosphere_Correction::apply(double sample_stamp, double latitude_d, d
} }
#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 ---------------------------------------------------------- /* geometric distance ----------------------------------------------------------
* compute geometric distance and receiver-to-satellite unit vector * compute geometric distance and receiver-to-satellite unit vector
@ -151,7 +146,14 @@ void Sbas_Ionosphere_Correction::matmul(const char *tr, int n, int k, int m, dou
case 3: for (x=0; x<m; x++) d += A[x+i*m]*B[x+j*m]; 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; 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]; if (beta == 0.0)
{
C[i+j*n] = alpha*d;
}
else
{
C[i+j*n] = alpha*d + beta*C[i+j*n];
}
} }
} }
@ -161,20 +163,19 @@ void Sbas_Ionosphere_Correction::matmul(const char *tr, int n, int k, int m, dou
* args : double *pos I geodetic position {lat,lon} (rad) * args : double *pos I geodetic position {lat,lon} (rad)
* double *E O ecef to local coord transformation matrix (3x3) * double *E O ecef to local coord transformation matrix (3x3)
* return : none * return : none
* notes : matirix stored by column-major order (fortran convention) * notes : matrix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
void Sbas_Ionosphere_Correction::xyz2enu(const double *pos, double *E) 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]); 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[0] = -sinl; E[3] = cosl; E[6] = 0.0;
E[1] = -sinp*cosl; E[4] = -sinp*sinl; E[7] = cosp; E[1] = -sinp*cosl; E[4] = -sinp*sinl; E[7] = cosp;
E[2] = cosp*cosl; E[5] = cosp*sinl; E[8] = sinp; E[2] = cosp*cosl; E[5] = cosp*sinl; E[8] = sinp;
} }
/* transform ecef vector to local tangental coordinate ------------------------- /* transform ecef vector to local tangential coordinate -------------------------
* transform ecef vector to local tangental coordinate * transform ecef vector to local tangential coordinate
* args : double *pos I geodetic position {lat,lon} (rad) * args : double *pos I geodetic position {lat,lon} (rad)
* double *r I vector in ecef coordinate {x,y,z} * double *r I vector in ecef coordinate {x,y,z}
* double *e O vector in local tangental coordinate {e,n,u} * double *e O vector in local tangental coordinate {e,n,u}
@ -183,13 +184,19 @@ void Sbas_Ionosphere_Correction::xyz2enu(const double *pos, double *E)
void Sbas_Ionosphere_Correction::ecef2enu(const double *pos, const double *r, double *e) void Sbas_Ionosphere_Correction::ecef2enu(const double *pos, const double *r, double *e)
{ {
double E[9]; double E[9];
xyz2enu(pos, E); xyz2enu(pos, E);
matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e); matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e);
} }
const double PI = 3.1415926535897932; /* pi */
//const double D2R = (PI/180.0); /* deg to rad */
//const double R2D = (180.0/PI); /* rad to deg */
//const double MAXBAND = 10; /* max SBAS band of IGP */
//const double RE_WGS84 = 6378137.0; /* earth semimajor axis (WGS84) (m) */
/* satellite azimuth/elevation angle ------------------------------------------- /* satellite azimuth/elevation angle -------------------------------------------
* compute satellite azimuth/elevation angle * compute satellite azimuth/elevation angle
* args : double *pos I geodetic position {lat,lon,h} (rad,m) * args : double *pos I geodetic position {lat,lon,h} (rad,m)
@ -200,6 +207,8 @@ void Sbas_Ionosphere_Correction::ecef2enu(const double *pos, const double *r, do
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
double Sbas_Ionosphere_Correction::satazel(const double *pos, const double *e, double *azel) double Sbas_Ionosphere_Correction::satazel(const double *pos, const double *e, double *azel)
{ {
const double RE_WGS84 = 6378137.0; /* earth semimajor axis (WGS84) (m) */
double az = 0.0, el = PI/2.0, enu[3]; double az = 0.0, el = PI/2.0, enu[3];
if (pos[2] > -RE_WGS84) if (pos[2] > -RE_WGS84)
@ -242,6 +251,8 @@ double Sbas_Ionosphere_Correction::ionppp(const double *pos, const double *azel,
double re, double hion, double *posp) double re, double hion, double *posp)
{ {
double cosaz, rp, ap, sinap, tanap; double cosaz, rp, ap, sinap, tanap;
const double D2R = (PI/180.0); /* deg to rad */
//const double R2D = (180.0/PI); /* rad to deg */
rp = re/(re+hion)*cos(azel[1]); rp = re/(re+hion)*cos(azel[1]);
ap = PI/2.0-azel[1]-asin(rp); ap = PI/2.0-azel[1]-asin(rp);
@ -280,6 +291,9 @@ void Sbas_Ionosphere_Correction::searchigp(const double *pos, const Igp **igp, d
int i; int i;
int latp[2]; int latp[2];
int lonp[4]; int lonp[4];
//const double D2R = (PI/180.0); /* deg to rad */
const double R2D = (180.0/PI); /* rad to deg */
double lat = pos[0]*R2D; double lat = pos[0]*R2D;
double lon = pos[1]*R2D; double lon = pos[1]*R2D;
@ -381,6 +395,8 @@ int Sbas_Ionosphere_Correction::sbsioncorr(const double sample_stamp, const doub
double t; double t;
double w[4] = {0}; double w[4] = {0};
const Igp *igp[4] = {0}; /* {ws,wn,es,en} */ const Igp *igp[4] = {0}; /* {ws,wn,es,en} */
//const double D2R = (PI/180.0); /* deg to rad */
const double R2D = (180.0/PI); /* rad to deg */
trace(4, "sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f", pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D); trace(4, "sbsioncorr: pos=%.3f %.3f azel=%.3f %.3f", pos[0]*R2D, pos[1]*R2D, azel[0]*R2D, azel[1]*R2D);

88
src/core/system_parameters/sbas_ionospheric_correction.h
View File

@ -63,6 +63,9 @@ private:
ar & d_delay; ar & d_delay;
} }
}; };
struct Igp_Band struct Igp_Band
{ {
//int d_iodi; //int d_iodi;
@ -104,53 +107,66 @@ private:
// sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ // sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
// } sbsion_t; // } sbsion_t;
/* inner product ---------------------------------------------------------------
* inner product of vectors /*!
* args : double *a,*b I vector a,b (n x 1) * \brief Inner product of vectors
* params : double *a,*b I vector a,b (n x 1)
* int n I size of vector a,b * int n I size of vector a,b
* return : a'*b * return : a'*b
*-----------------------------------------------------------------------------*/ */
double dot(const double *a, const double *b, int n); double dot(const double *a, const double *b, int n);
/* multiply matrix -----------------------------------------------------------*/
/*!
* \brief multiply matrix
*/
void matmul(const char *tr, int n, int k, int m, double alpha, void matmul(const char *tr, int n, int k, int m, double alpha,
const double *A, const double *B, double beta, double *C); const double *A, const double *B, double beta, double *C);
/* ecef to local coordinate transfromation matrix ------------------------------ /*!
* compute ecef to local coordinate transfromation matrix * \brief EFEC to local coordinate transfomartion matrix
* args : double *pos I geodetic position {lat,lon} (rad) * Compute ecef to local coordinate transfomartion matrix
* params : double *pos I geodetic position {lat,lon} (rad)
* double *E O ecef to local coord transformation matrix (3x3) * double *E O ecef to local coord transformation matrix (3x3)
* return : none * return : none
* notes : matirix stored by column-major order (fortran convention) * notes : matrix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/ */
void xyz2enu(const double *pos, double *E); 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) * \brief Transforms ECEF vector into local tangential coordinates
* params : double *pos I geodetic position {lat,lon} (rad)
* double *r I vector in ecef coordinate {x,y,z} * double *r I vector in ecef coordinate {x,y,z}
* double *e O vector in local tangental coordinate {e,n,u} * double *e O vector in local tangental coordinate {e,n,u}
* return : none * return : none
*-----------------------------------------------------------------------------*/ */
void ecef2enu(const double *pos, const double *r, double *e); 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) * \brief Compute satellite azimuth/elevation angle
* params : double *pos I geodetic position {lat,lon,h} (rad,m)
* double *e I receiver-to-satellilte unit vevtor (ecef) * double *e I receiver-to-satellilte unit vevtor (ecef)
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output) * double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
* (0.0 <= azel[0] < 2*pi, -pi/2 <= azel[1] <= pi/2) * (0.0 <= azel[0] < 2*pi, -pi/2 <= azel[1] <= pi/2)
* return : elevation angle (rad) * return : elevation angle (rad)
*-----------------------------------------------------------------------------*/ */
double satazel(const double *pos, const double *e, double *azel); double satazel(const double *pos, const double *e, double *azel);
/* debug trace functions -----------------------------------------------------*/ /*!
* \brief debug trace functions
*/
void trace(int level, const char *format, ...); void trace(int level, const char *format, ...);
/* time difference ------------------------------------------------------------- /* time difference -------------------------------------------------------------
* difference between gtime_t structs * difference between gtime_t structs
* args : gtime_t t1,t2 I gtime_t structs * args : gtime_t t1,t2 I gtime_t structs
* return : time difference (t1-t2) (s) * return : time difference (t1-t2) (s)
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
//double timediff(gtime_t t1, gtime_t t2); //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) * \brief Compute ionospheric pierce point (ipp) position and slant factor
* params : double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad) * double *azel I azimuth/elevation angle {az,el} (rad)
* double re I earth radius (km) * double re I earth radius (km)
* double hion I altitude of ionosphere (km) * double hion I altitude of ionosphere (km)
@ -161,13 +177,20 @@ private:
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
double ionppp(const double *pos, const double *azel, double re, double ionppp(const double *pos, const double *azel, double re,
double hion, double *posp); double hion, double *posp);
/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/
/*!
* \brief Variance of ionosphere correction (give=GIVEI+1)
*/
double varicorr(int give); double varicorr(int give);
/* search igps ---------------------------------------------------------------*/
/*!
* \brief Search igps
*/
void searchigp(const double *pos, const Igp **igp, double *x, double *y); 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 * \brief Compute sbas ionosphric delay correction
* params : long sample_stamp I sample stamp of observable on which the correction will be applied
* sbsion_t *ion I ionospheric correction data (implicit) * sbsion_t *ion I ionospheric correction data (implicit)
* double *pos I receiver position {lat,lon,height} (rad/m) * double *pos I receiver position {lat,lon,height} (rad/m)
* double *azel I satellite azimuth/elavation angle (rad) * double *azel I satellite azimuth/elavation angle (rad)
@ -177,23 +200,20 @@ private:
* notes : before calling the function, sbas ionosphere correction parameters * notes : before calling the function, sbas ionosphere correction parameters
* in navigation data (nav->sbsion) must be set by callig * in navigation data (nav->sbsion) must be set by callig
* sbsupdatecorr() * sbsupdatecorr()
*-----------------------------------------------------------------------------*/ */
int sbsioncorr(const double sample_stamp, const double *pos, int sbsioncorr(const double sample_stamp, const double *pos,
const double *azel, double *delay, double *var); const double *azel, double *delay, double *var);
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const unsigned int version){ar & d_bands;}
public: public:
std::vector<Igp_Band> d_bands; std::vector<Igp_Band> d_bands;
void print(std::ostream &out); void print(std::ostream &out);
bool apply(double sample_stamp, double latitude_d, double longitude_d, bool apply(double sample_stamp, double latitude_d, double longitude_d,
double azimut_d, double evaluation_d, double &delay, double &var); 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;}
}; };

72
src/core/system_parameters/sbas_satellite_correction.cc
View File

@ -34,22 +34,25 @@
#include <iostream> #include <iostream>
#include <glog/log_severity.h> #include <glog/log_severity.h>
#include <glog/logging.h> #include <glog/logging.h>
#include "sbas_satellite_correction.h" #include "sbas_satellite_correction.h"
#define EVENT 2 // logs important events which don't occur every update() call #define EVENT 2 // logs important events which don't occur every update() call
#define FLOW 3 // logs the function calls of block processing functions #define FLOW 3 // logs the function calls of block processing functions
void #define CLIGHT 299792458.0 /* speed of light (m/s) */
Sbas_Satellite_Correction::print(std::ostream &out) #define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */
void Sbas_Satellite_Correction::print(std::ostream &out)
{ {
out << "<<S>> Sbas satellite corrections for PRN" << d_prn << ":" << std::endl; out << "<<S>> Sbas satellite corrections for PRN" << d_prn << ":" << std::endl;
print_fast_correction(out); print_fast_correction(out);
print_long_term_correction(out); print_long_term_correction(out);
} }
void
Sbas_Satellite_Correction::print_fast_correction(std::ostream &out) void Sbas_Satellite_Correction::print_fast_correction(std::ostream &out)
{ {
Fast_Correction fcorr = d_fast_correction; Fast_Correction fcorr = d_fast_correction;
out << "<<S>> Fast PRN" << d_prn << ":"; out << "<<S>> Fast PRN" << d_prn << ":";
@ -73,8 +76,7 @@ Sbas_Satellite_Correction::print_fast_correction(std::ostream &out)
} }
void void Sbas_Satellite_Correction::print_long_term_correction(std::ostream &out)
Sbas_Satellite_Correction::print_long_term_correction(std::ostream &out)
{ {
Long_Term_Correction lcorr = d_long_term_correction; Long_Term_Correction lcorr = d_long_term_correction;
out << "<<S>> Long PRN" << d_prn << ":"; out << "<<S>> Long PRN" << d_prn << ":";
@ -94,66 +96,58 @@ int Sbas_Satellite_Correction::apply_fast(double sample_stamp, double &pr, doubl
{ {
int result; int result;
double prc = 0; // pseudo range correction double prc = 0; // pseudo range correction
result = sbsfastcorr(sample_stamp, &prc, &var); result = sbsfastcorr(sample_stamp, &prc, &var);
pr += prc; pr += prc;
VLOG(FLOW) << "<<S>> fast correction applied: sample_stamp=" << sample_stamp << " prc=" << prc << " corr. pr=" << pr; VLOG(FLOW) << "<<S>> fast correction applied: sample_stamp=" << sample_stamp << " prc=" << prc << " corr. pr=" << pr;
return result; return result;
} }
int Sbas_Satellite_Correction::apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var) int Sbas_Satellite_Correction::apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var)
{ {
int result; int result;
double drs[3] = {0}; double drs[3] = {0};
double ddts = 0; double ddts = 0;
result = sbslongcorr(sample_stamp, drs, &ddts); result = sbslongcorr(sample_stamp, drs, &ddts);
for (int i = 0; i < 3; i++) sv_pos[i] += drs[i]; 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] << ")"; 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; return result;
} }
int Sbas_Satellite_Correction::apply_long_term_sv_clk(double sample_stamp, double &dts, double &var) int Sbas_Satellite_Correction::apply_long_term_sv_clk(double sample_stamp, double &dts, double &var)
{ {
int result; int result;
double drs[3] = {0}; double drs[3] = {0};
double ddts = 0; double ddts = 0;
result = sbslongcorr(sample_stamp, drs, &ddts); result = sbslongcorr(sample_stamp, drs, &ddts);
dts += ddts; dts += ddts;
VLOG(FLOW) << "<<S>> long term sv clock correction correction applied: sample_stamp=" << sample_stamp << " ddts=" << ddts; VLOG(FLOW) << "<<S>> long term sv clock correction correction applied: sample_stamp=" << sample_stamp << " ddts=" << ddts;
return result; return result;
} }
bool Sbas_Satellite_Correction::alarm() bool Sbas_Satellite_Correction::alarm()
{ {
return this->d_fast_correction.d_udre == 16; 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 -----------------------------------------------------*/ /* debug trace function -----------------------------------------------------*/
void Sbas_Satellite_Correction::trace(int level, const char *format, ...) void Sbas_Satellite_Correction::trace(int level, const char *format, ...)
{ {
va_list ap; va_list ap;
char str[1000]; char str[1000];
va_start(ap,format); va_start(ap,format);
vsprintf(str,format,ap); vsprintf(str,format,ap);
va_end(ap); va_end(ap);
VLOG(FLOW) << "<<S>> " << std::string(str); VLOG(FLOW) << "<<S>> " << std::string(str);
} }
/* variance of fast correction (udre=UDRE+1) ---------------------------------*/ /* variance of fast correction (udre=UDRE+1) ---------------------------------*/
double Sbas_Satellite_Correction::varfcorr(int udre) double Sbas_Satellite_Correction::varfcorr(int udre)
{ {
@ -163,6 +157,8 @@ double Sbas_Satellite_Correction::varfcorr(int udre)
}; };
return 0 < udre && udre <= 14 ? var[udre - 1] : 0.0; return 0 < udre && udre <= 14 ? var[udre - 1] : 0.0;
} }
/* fast correction degradation -----------------------------------------------*/ /* fast correction degradation -----------------------------------------------*/
double Sbas_Satellite_Correction::degfcorr(int ai) double Sbas_Satellite_Correction::degfcorr(int ai)
{ {
@ -173,21 +169,20 @@ double Sbas_Satellite_Correction::degfcorr(int ai)
return 0 < ai && ai <= 15 ? degf[ai] : 0.0058; return 0 < ai && ai <= 15 ? degf[ai] : 0.0058;
} }
/* long term correction ------------------------------------------------------*/ /* long term correction ------------------------------------------------------*/
int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, double *ddts) int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, double *ddts)
{ {
double t = 0.0; double t = 0.0;
int i; int i;
Long_Term_Correction lcorr = d_long_term_correction; Long_Term_Correction lcorr = d_long_term_correction;
trace(3, "sbslongcorr: prn=%2d", this->d_prn); trace(3, "sbslongcorr: prn=%2d", this->d_prn);
// if (p->sat!=sat||p->lcorr.t0.time==0) continue; // if (p->sat!=sat||p->lcorr.t0.time==0) continue;
// compute time of applicability // compute time of applicability
if(d_long_term_correction.i_vel == 1) if(d_long_term_correction.i_vel == 1)
{ // time of applicability is the one sent, i.e., tapp {
// time of applicability is the one sent, i.e., tapp
// TODO: adapt for vel==1 case // TODO: adapt for vel==1 case
// t = tow-d_long_term_correction.i_tapp; // 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 // 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
@ -197,47 +192,40 @@ int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, doubl
sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/ sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/
} }
else else
{ // time of applicability is time of reception {
// 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_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; //t=time_stamp-lcorr.d_t0;
if (fabs(t) > MAXSBSAGEL) if (fabs(t) > MAXSBSAGEL)
{ {
trace(2,"sbas long-term correction expired: sat=%2d time_stamp=%5.0f", d_prn, time_stamp); trace(2,"sbas long-term correction expired: sat=%2d time_stamp=%5.0f", d_prn, time_stamp);
return 0; return 0;
} }
// sv position correction // sv position correction
for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*t; for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*t;
// sv clock correction correction // sv clock correction correction
*ddts = lcorr.d_daf0 + lcorr.d_daf1*t; *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); 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; return 1;
/* if sbas satellite without correction, no correction applied */ /* if sbas satellite without correction, no correction applied */
//if (satsys(sat,NULL)==SYS_SBS) return 1; //if (satsys(sat,NULL)==SYS_SBS) return 1;
//trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat); //trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
//return 0; //return 0;
} }
/* fast correction -----------------------------------------------------------*/ /* fast correction -----------------------------------------------------------*/
int Sbas_Satellite_Correction::sbsfastcorr(double time_stamp, double *prc, double *var) int Sbas_Satellite_Correction::sbsfastcorr(double time_stamp, double *prc, double *var)
#define RRCENA #define RRCENA
{ {
double t; double t;
Fast_Correction fcorr = d_fast_correction; Fast_Correction fcorr = d_fast_correction;
trace(3, "sbsfastcorr: sat=%2d", this->d_prn); 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 //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 t = (time_stamp - fcorr.d_tof.get_time_stamp()) + fcorr.d_tlat; // delta t between now and tof
/* expire age of correction? */ /* expire age of correction? */
if (fabs(t) > MAXSBSAGEF) if (fabs(t) > MAXSBSAGEF)
{ {
@ -263,7 +251,6 @@ int Sbas_Satellite_Correction::sbsfastcorr(double time_stamp, double *prc, doubl
} }
#endif #endif
*var = varfcorr(fcorr.d_udre) + degfcorr(fcorr.d_ai) * t * t / 2.0; *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); trace(5, "sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f", d_prn, *prc, sqrt(*var), t);
return 1; return 1;
} }
@ -289,9 +276,7 @@ int Sbas_Satellite_Correction::sbssatcorr(double time_stamp, double *rs, double
{ {
double drs[3] = {0}, dclk = 0.0, prc = 0.0; double drs[3] = {0}, dclk = 0.0, prc = 0.0;
int i; int i;
trace(3,"sbssatcorr : sat=%2d",d_prn); trace(3,"sbssatcorr : sat=%2d",d_prn);
/* sbas long term corrections */ /* sbas long term corrections */
if (!sbslongcorr(time_stamp, drs, &dclk)) if (!sbslongcorr(time_stamp, drs, &dclk))
{ {
@ -303,12 +288,9 @@ int Sbas_Satellite_Correction::sbssatcorr(double time_stamp, double *rs, double
return 0; return 0;
} }
for (i=0; i<3; i++) rs[i] += drs[i]; for (i=0; i<3; i++) rs[i] += drs[i];
dts[0] += dclk + prc/CLIGHT; dts[0] += dclk + prc/CLIGHT;
trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f", 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); d_prn,drs[0],drs[1],drs[2],dclk,prc/CLIGHT,*var);
return 1; return 1;
} }

59
src/core/system_parameters/sbas_telemetry_data.cc
View File

@ -47,7 +47,7 @@
Sbas_Telemetry_Data::Sbas_Telemetry_Data() Sbas_Telemetry_Data::Sbas_Telemetry_Data()
{ {
fp_trace = NULL; /* file pointer of trace */ fp_trace = nullptr;; /* file pointer of trace */
level_trace = 0; /* level of trace */ level_trace = 0; /* level of trace */
tick_trace = 0; /* tick time at traceopen (ms) */ tick_trace = 0; /* tick time at traceopen (ms) */
@ -237,8 +237,9 @@ void Sbas_Telemetry_Data::received_iono_correction()
if(iono_queue != NULL) iono_queue->push(iono_corr); if(iono_queue != NULL) iono_queue->push(iono_corr);
} }
// helper for comparing two POD structures with undefined padding between members // helper for comparing two POD structures with undefined padding between members
// not garanted to work always properly -> don't use it for critical tasks // not guaranteed to work always properly -> don't use it for critical tasks
template <class Struct> template <class Struct>
inline bool are_equal(const Struct &s1, const Struct &s2) inline bool are_equal(const Struct &s1, const Struct &s2)
{ {
@ -254,31 +255,11 @@ inline bool are_equal(const Struct &s1, const Struct &s2)
s1_ = (Struct*) calloc (1, struct_size); s1_ = (Struct*) calloc (1, struct_size);
s2_ = (Struct*) calloc (1, struct_size); s2_ = (Struct*) calloc (1, struct_size);
/* is_equal = !memcmp(s1_, s2_, sizeof(Struct)); // use assignment constructor which doesn't copy paddings
bool is_equal_manual = true;
std::stringstream ss;
ss << "\n<<cmp>> compare original objects of size=" << sizeof(Struct) << ":" << std::endl;
for (size_t i = 0; i < sizeof(Struct); ++i)
{
char byte1 = ((const char *)&s1)[i];
char byte2 = ((const char *)&s2)[i];
if(byte1 != byte2) is_equal_manual = false;
ss << "<<cmp>> s1=" << std::hex << std::setw(4) << std::setfill('0');
ss << (short)byte1;
ss << " s2=" << std::hex << std::setw(4) << std::setfill('0');
ss << (short)byte2;
ss << " equal=" << is_equal_manual;
ss << std::endl;
}*/
// use asignment constructor which doesn't copy paddings
*s1_ = s1; *s1_ = s1;
*s2_ = s2; *s2_ = s2;
// compare struct memory byte-wise // compare struct memory byte-wise
//is_equal = !memcmp(s1_, s2_, sizeof(Struct));
is_equal_manual = true; is_equal_manual = true;
ss.str(); ss.str();
ss << "\n<<cmp>> compare objects of size=" << sizeof(Struct) << " (memcmp says is_equal=" << is_equal << ") :" << std::endl; ss << "\n<<cmp>> compare objects of size=" << sizeof(Struct) << " (memcmp says is_equal=" << is_equal << ") :" << std::endl;
@ -294,7 +275,6 @@ inline bool are_equal(const Struct &s1, const Struct &s2)
ss << " equal=" << is_equal_manual; ss << " equal=" << is_equal_manual;
ss << std::endl; ss << std::endl;
} }
//VLOG(DETAIL) << ss.str();
free(s1_); free(s1_);
free(s2_); free(s2_);
@ -307,7 +287,7 @@ inline bool are_equal(const Struct &s1, const Struct &s2)
void Sbas_Telemetry_Data::updated_satellite_corrections() void Sbas_Telemetry_Data::updated_satellite_corrections()
{ {
VLOG(FLOW) << "<<T>> updated_satellite_corrections():"; VLOG(FLOW) << "<<T>> updated_satellite_corrections():";
// for each satellit in the RTKLIB structure // for each satellite in the RTKLIB structure
for (int i_sat = 0; i_sat < d_nav.sbssat.nsat; i_sat++) for (int i_sat = 0; i_sat < d_nav.sbssat.nsat; i_sat++)
{ {
std::stringstream ss; std::stringstream ss;
@ -404,7 +384,6 @@ void Sbas_Telemetry_Data::trace(int level, const char *format, ...)
{ {
va_list ap; va_list ap;
char str[1000]; char str[1000];
va_start(ap, format); va_start(ap, format);
vsprintf(str, format, ap); vsprintf(str, format, ap);
va_end(ap); va_end(ap);
@ -443,13 +422,14 @@ int Sbas_Telemetry_Data::satno(int sys, int prn)
} }
return 0; return 0;
} }
/* extract unsigned/signed bits ------------------------------------------------
* extract unsigned/signed bits from byte data /*!
* args : unsigned char *buff I byte data * \brief Extracts unsigned/signed bits from byte data
* params : unsigned char *buff I byte data
* int pos I bit position from start of data (bits) * int pos I bit position from start of data (bits)
* int len I bit length (bits) (len<=32) * int len I bit length (bits) (len<=32)
* return : extracted unsigned/signed bits * return : extracted unsigned/signed bits
*-----------------------------------------------------------------------------*/ */
unsigned int Sbas_Telemetry_Data::getbitu(const unsigned char *buff, int pos, int len) unsigned int Sbas_Telemetry_Data::getbitu(const unsigned char *buff, int pos, int len)
{ {
unsigned int bits = 0; unsigned int bits = 0;
@ -580,7 +560,7 @@ const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband2[2][5]={ /*
int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat) int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
int i, n, sat; int i, n, sat;
// see figure A-6: i coresponds to bit number (and for the GPS satellites is identically to the PRN), n to the PRN mask number // see figure A-6: i corresponds to bit number (and for the GPS satellites is identically to the PRN), n to the PRN mask number
trace(4,"decode_sbstype1:"); trace(4,"decode_sbstype1:");
@ -608,6 +588,8 @@ int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(5, "decode_sbstype1: nprn=%d iodp=%d", n, sbssat->iodp); trace(5, "decode_sbstype1: nprn=%d iodp=%d", n, sbssat->iodp);
return 1; return 1;
} }
/* decode type 2-5,0: fast corrections ---------------------------------------*/ /* decode type 2-5,0: fast corrections ---------------------------------------*/
int Sbas_Telemetry_Data::decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat) int Sbas_Telemetry_Data::decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
@ -681,13 +663,9 @@ int Sbas_Telemetry_Data::decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
int Sbas_Telemetry_Data::decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat) int Sbas_Telemetry_Data::decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
int i; int i;
trace(4,"decode_sbstype7"); trace(4,"decode_sbstype7");
if (sbssat->iodp != (int)getbitu(msg->msg, 18, 2)) return 0; if (sbssat->iodp != (int)getbitu(msg->msg, 18, 2)) return 0;
sbssat->tlat = getbitu(msg->msg, 14, 4); sbssat->tlat = getbitu(msg->msg, 14, 4);
for (i=0; i < sbssat->nsat && i < MAXSAT; i++) for (i=0; i < sbssat->nsat && i < MAXSAT; i++)
{ {
sbssat->sat[i].fcorr.ai = getbitu(msg->msg, 22 + i*4, 4); sbssat->sat[i].fcorr.ai = getbitu(msg->msg, 22 + i*4, 4);
@ -763,7 +741,8 @@ int Sbas_Telemetry_Data::decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
short iodi_new = (short)getbitu(msg->msg, 22, 2); short iodi_new = (short)getbitu(msg->msg, 22, 2);
if(sbsion[band].iodi != iodi_new) if(sbsion[band].iodi != iodi_new)
{// IGP mask changed -> invalidate all IGPs in this band {
// IGP mask changed -> invalidate all IGPs in this band
igp_mask_changed(band); igp_mask_changed(band);
} }
sbsion[band].iodi = iodi_new; sbsion[band].iodi = iodi_new;
@ -858,7 +837,8 @@ int Sbas_Telemetry_Data::decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *
trace(4,"decode_longcorrh:"); trace(4,"decode_longcorrh:");
if (getbitu(msg->msg, p, 1) == 0 ) if (getbitu(msg->msg, p, 1) == 0 )
{ /* vel code=0 */ {
/* vel code=0 */
if (sbssat->iodp == (int)getbitu(msg->msg, p + 103, 2)) if (sbssat->iodp == (int)getbitu(msg->msg, p + 103, 2))
{ {
return decode_longcorr0(msg, p + 1, sbssat) && decode_longcorr0(msg, p + 52, sbssat); return decode_longcorr0(msg, p + 1, sbssat) && decode_longcorr0(msg, p + 52, sbssat);
@ -981,11 +961,12 @@ int Sbas_Telemetry_Data::sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
case 26: stat = decode_sbstype26(msg, nav ->sbsion); break; case 26: stat = decode_sbstype26(msg, nav ->sbsion); break;
case 63: break; /* null message */ case 63: break; /* null message */
default: trace(2,"unsupported sbas message: type=%d",type); break; default: trace(2, "Unsupported SBAS message: type=%d", type); break;
} }
return stat ? type : -1; return stat ? type : -1;
} }
void Sbas_Telemetry_Data::prn_mask_changed() void Sbas_Telemetry_Data::prn_mask_changed()
{ {
d_nav.sbssat.tlat = -1; d_nav.sbssat.tlat = -1;
@ -997,6 +978,7 @@ void Sbas_Telemetry_Data::prn_mask_changed()
} }
} }
bool Sbas_Telemetry_Data::is_rtklib_sat_correction_valid(int sat) bool Sbas_Telemetry_Data::is_rtklib_sat_correction_valid(int sat)
{ {
return d_nav.sbssat.tlat > -1 return d_nav.sbssat.tlat > -1
@ -1004,6 +986,7 @@ bool Sbas_Telemetry_Data::is_rtklib_sat_correction_valid(int sat)
&& d_nav.sbssat.sat[sat].lcorr.valid; && d_nav.sbssat.sat[sat].lcorr.valid;
} }
void Sbas_Telemetry_Data::igp_mask_changed(int band) void Sbas_Telemetry_Data::igp_mask_changed(int band)
{ {
for(int i_igp = 0; i_igp < d_nav.sbsion[band].nigp; i_igp++) for(int i_igp = 0; i_igp < d_nav.sbsion[band].nigp; i_igp++)

33
src/core/system_parameters/sbas_telemetry_data.h
View File

@ -54,8 +54,8 @@ struct Long_Term_Correction;
class Sbas_Ephemeris; class Sbas_Ephemeris;
/* /*
* \brief represents a raw SBAS message of 250bits + 6bits padding * \brief Represents a raw SBAS message of 250cbits + 6 bits padding
* (8b preamble + 6b message type + 212b data + 24b CRC + 6b zerro padding) * (8b preamble + 6b message type + 212b data + 24b CRC + 6b zero padding)
*/ */
class Sbas_Raw_Msg class Sbas_Raw_Msg
{ {
@ -101,24 +101,16 @@ private:
/* /*
* \brief holds an updated set of the telemetry data received from SBAS * \brief Holds an updated set of the telemetry data received from SBAS
*/ */
class Sbas_Telemetry_Data class Sbas_Telemetry_Data
{ {
public: public:
int update(Sbas_Raw_Msg sbas_raw_msg);
int void set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue);
update(Sbas_Raw_Msg sbas_raw_msg); 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 void set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue);
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 * Default constructor
@ -293,9 +285,9 @@ private:
typedef struct { /* SBAS long term satellite error correction type */ typedef struct { /* SBAS long term satellite error correction type */
//gtime_t t0; /* correction time */ //gtime_t t0; /* correction time */
double trx; // time when message was received double trx; /* time when message was received */
int tapp; // time of applicability (when vel=1 sent as t0) int tapp; /* time of applicability (when vel=1 sent as t0) */
int vel; // use velocity if vel=1 int vel; /* use velocity if vel=1 */
bool valid; bool valid;
int iode; /* IODE (issue of date ephemeris) */ int iode; /* IODE (issue of date ephemeris) */
double dpos[3]; /* delta position (m) (ecef) */ double dpos[3]; /* delta position (m) (ecef) */
@ -339,7 +331,7 @@ private:
} sbsion_t; } sbsion_t;
/* /*
* idicators * indicators
*/ */
typedef struct { /* SBAS ephemeris type */ typedef struct { /* SBAS ephemeris type */
@ -508,7 +500,6 @@ private:
// RTKLIB SBAS telemetry data representation // RTKLIB SBAS telemetry data representation
nav_t d_nav; nav_t d_nav;
}; };
#endif #endif

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

@ -86,18 +86,16 @@ private:
}; };
/* /*!
* Sbas_Time relates the relative sample stamp time scale with the absolute GPS time scale. * \brief 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: * There are three different states for a Sbas_Time object:
* - only relative time (sample stamp) is known * - only relative time (sample stamp) is known
* - only absolute time (gps time) is known * - only absolute time (gps time) is known
* - absolute and relative time and their relation is known * - absolute and relative time and their relation is known
*/ */
class Sbas_Time class Sbas_Time
{ {
public: public:
enum Sbas_Time_State {RELATIVE, /*ABSOLUTE,*/ RELATED, UNDEFINED}; enum Sbas_Time_State {RELATIVE, /*ABSOLUTE,*/ RELATED, UNDEFINED};
Sbas_Time() Sbas_Time()
@ -190,4 +188,4 @@ private:
}; };
#endif /* SBAS_TIME_H_ */ #endif /* GNSS_SDR_SBAS_TIME_H_ */