mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-07-15 08:23:03 +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";

101
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
*/ */
@ -243,15 +245,15 @@ bool sbas_l1_telemetry_decoder_cc::sample_aligner::get_symbols(const std::vector
// get the next samples // get the next samples
for (int i = 0; i < d_n_smpls_in_history; i++) 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]; 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 // 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_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; d_corr_shifted = d_iir_par*smpls[0]*smpls[1] + (1 - d_iir_par)*d_corr_shifted;
// decide which alignment is the correct one // decide which alignment is the correct one
corr_diff = std::abs(d_corr_paired-d_corr_shifted); 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; stand_by = d_aligned ? corr_diff < d_corr_paired/2 : corr_diff < d_corr_shifted/2;
if (!stand_by) if (!stand_by)
{ {
@ -259,7 +261,7 @@ bool sbas_l1_telemetry_decoder_cc::sample_aligner::get_symbols(const std::vector
} }
// sum the correct pair of samples to a symbol, depending on the current alignment d_align // 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]; sym = smpls[0 + int(d_aligned)*2] + smpls[1];
symbols.push_back(sym); symbols.push_back(sym);
// sample alignment debug output // sample alignment debug output
@ -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)
{ {
@ -400,22 +387,21 @@ get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std:
// compare the buffer bits with the preamble bits // 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) 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 ; 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 ; inv_preamble_detected = *preample_bit_it != d_buffer[preample_bit_it - preample_it->begin()] ? inv_preamble_detected : false ;
} }
if (preamble_detected || inv_preamble_detected) if (preamble_detected || inv_preamble_detected)
{ {
// copy candidate // copy candidate
std::vector<int> candidate; std::vector<int> candidate;
std::copy(d_buffer.begin(), d_buffer.begin()+sbas_msg_length, std::back_inserter(candidate)); std::copy(d_buffer.begin(), d_buffer.begin() + sbas_msg_length, std::back_inserter(candidate));
if(inv_preamble_detected) if(inv_preamble_detected)
{ {
// invert bits // invert bits
for (std::vector<int>::iterator candidate_bit_it = candidate.begin(); candidate_bit_it != candidate.end(); candidate_bit_it++) 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; *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,25 +459,23 @@ 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);
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("");
@ -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);

15
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)
}; };

62
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
@ -139,9 +134,9 @@ void Sbas_Ionosphere_Correction::matmul(const char *tr, int n, int k, int m, dou
const double *A, const double *B, double beta, double *C) const double *A, const double *B, double beta, double *C)
{ {
double d; double d;
int i, j, x, f = tr[0]=='N'?(tr[1]=='N'?1:2):(tr[1]=='N'?3:4); 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++) for (i=0; i<n; i++) for (j=0; j<k; j++)
{ {
d = 0.0; d = 0.0;
switch (f) switch (f)
@ -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,19 +207,21 @@ 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)
{ {
double az = 0.0, el=PI/2.0, enu[3]; const double RE_WGS84 = 6378137.0; /* earth semimajor axis (WGS84) (m) */
double az = 0.0, el = PI/2.0, enu[3];
if (pos[2] > -RE_WGS84) if (pos[2] > -RE_WGS84)
{ {
ecef2enu(pos,e,enu); ecef2enu(pos, e, enu);
az = dot(enu,enu,2) < 1E-12?0.0:atan2(enu[0], enu[1]); az = dot(enu ,enu, 2) < 1E-12 ? 0.0:atan2(enu[0], enu[1]);
if (az<0.0) az += 2*PI; if (az < 0.0) az += 2*PI;
el = asin(enu[2]); el = asin(enu[2]);
} }
if (azel) if (azel)
{ {
azel[0]=az; azel[0] = az;
azel[1]=el; azel[1] = el;
} }
return el; return el;
} }
@ -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);
@ -250,8 +261,8 @@ double Sbas_Ionosphere_Correction::ionppp(const double *pos, const double *azel,
cosaz = cos(azel[0]); cosaz = cos(azel[0]);
posp[0] = asin(sin(pos[0])*cos(ap) + cos(pos[0])*sinap*cosaz); 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]))|| 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]))) (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])); posp[1] = pos[1] + PI - asin(sinap*sin(azel[0])/cos(posp[0]));
} }
@ -259,7 +270,7 @@ double Sbas_Ionosphere_Correction::ionppp(const double *pos, const double *azel,
{ {
posp[1] = pos[1] + asin(sinap*sin(azel[0])/cos(posp[0])); posp[1] = pos[1] + asin(sinap*sin(azel[0])/cos(posp[0]));
} }
return 1.0/sqrt(1.0-rp*rp); return 1.0 / sqrt(1.0 - rp*rp);
} }
@ -270,7 +281,7 @@ double Sbas_Ionosphere_Correction::varicorr(int give)
0.0084, 0.0333, 0.0749, 0.1331, 0.2079, 0.2994, 0.4075, 0.5322, 0.6735, 0.8315, 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 1.1974, 1.8709, 3.326, 20.787, 187.0826
}; };
return 0<=give&&give<15?var[give]:0.0; return 0 <= give && give < 15 ? var[give]:0.0;
} }
@ -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);

216
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;
@ -82,118 +85,135 @@ private:
class Sbas_Ionosphere_Correction class Sbas_Ionosphere_Correction
{ {
private: private:
// /* type definitions ----------------------------------------------------------*/ // /* type definitions ----------------------------------------------------------*/
//#define MAXBAND 10 /* max SBAS band of IGP */ //#define MAXBAND 10 /* max SBAS band of IGP */
//#define MAXNIGP 201 /* max number of IGP in SBAS band */ //#define MAXNIGP 201 /* max number of IGP in SBAS band */
// //
// typedef struct { /* time struct */ // typedef struct { /* time struct */
// time_t time; /* time (s) expressed by standard time_t */ // time_t time; /* time (s) expressed by standard time_t */
// double sec; /* fraction of second under 1 s */ // double sec; /* fraction of second under 1 s */
// } gtime_t; // } gtime_t;
// //
// typedef struct { /* SBAS ionospheric correction type */ // typedef struct { /* SBAS ionospheric correction type */
// gtime_t t0; /* correction time */ // gtime_t t0; /* correction time */
// short lat,lon; /* latitude/longitude (deg) */ // short lat,lon; /* latitude/longitude (deg) */
// short give; /* GIVI+1 */ // short give; /* GIVI+1 */
// float delay; /* vertical delay estimate (m) */ // float delay; /* vertical delay estimate (m) */
// } sbsigp_t; // } sbsigp_t;
// //
// typedef struct { /* SBAS ionospheric corrections type */ // typedef struct { /* SBAS ionospheric corrections type */
// int iodi; /* IODI (issue of date ionos corr) */ // int iodi; /* IODI (issue of date ionos corr) */
// int nigp; /* number of igps */ // int nigp; /* number of igps */
// 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)
* 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 -----------------------------------------------------*/ /*!
* \brief Inner product of vectors
* params : 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);
/*!
* \brief 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);
/*!
* \brief EFEC to local coordinate transfomartion matrix
* 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)
* return : none
* notes : matrix stored by column-major order (fortran convention)
*/
void xyz2enu(const double *pos, double *E);
/*!
* \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 *e O vector in local tangental coordinate {e,n,u}
* return : none
*/
void ecef2enu(const double *pos, const double *r, double *e);
/*!
* \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 *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);
/*!
* \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
* double *azel I azimuth/elevation angle {az,el} (rad) * params : double *pos I receiver position {lat,lon,h} (rad,m)
* double re I earth radius (km) * double *azel I azimuth/elevation angle {az,el} (rad)
* double hion I altitude of ionosphere (km) * double re I earth radius (km)
* double *posp O pierce point position {lat,lon,h} (rad,m) * double hion I altitude of ionosphere (km)
* return : slant factor * double *posp O pierce point position {lat,lon,h} (rad,m)
* notes : see ref [2], only valid on the earth surface * return : slant factor
* fixing bug on ref [2] A.4.4.10.1 A-22,23 * 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 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
* sbsion_t *ion I ionospheric correction data (implicit) * params : long sample_stamp I sample stamp of observable on which the correction will be applied
* double *pos I receiver position {lat,lon,height} (rad/m) * sbsion_t *ion I ionospheric correction data (implicit)
* double *azel I satellite azimuth/elavation angle (rad) * double *pos I receiver position {lat,lon,height} (rad/m)
* double *delay O slant ionospheric delay (L1) (m) * double *azel I satellite azimuth/elavation angle (rad)
* double *var O variance of ionospheric delay (m^2) * double *delay O slant ionospheric delay (L1) (m)
* return : status (1:ok, 0:no correction) * double *var O variance of ionospheric delay (m^2)
* notes : before calling the function, sbas ionosphere correction parameters * return : status (1:ok, 0:no correction)
* in navigation data (nav->sbsion) must be set by callig * notes : before calling the function, sbas ionosphere correction parameters
* sbsupdatecorr() * in navigation data (nav->sbsion) must be set by callig
*-----------------------------------------------------------------------------*/ * 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);
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; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive& ar, const unsigned int version){ar & d_bands;} void serialize(Archive& ar, const unsigned int version){ar & d_bands;}
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);
}; };

186
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,178 +96,163 @@ 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)
{ {
const double var[14] = { const double var[14] = {
0.052,0.0924,0.1444,0.283,0.4678,0.8315,1.2992,1.8709,2.5465,3.326, 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 5.1968, 20.7870, 230.9661, 2078.695
}; };
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)
{ {
const double degf[16] = { const double degf[16] = {
0.00000,0.00005,0.00009,0.00012,0.00015,0.00020,0.00030,0.00045, 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 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; 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 {
// TODO: adapt for vel==1 case // time of applicability is the one sent, i.e., tapp
// t = tow-d_long_term_correction.i_tapp; // TODO: adapt for vel==1 case
// 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 = tow-d_long_term_correction.i_tapp;
/* t = (int)getbitu(msg->msg, p + 90, 13)*16 - (int)msg->tow%86400; // 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
if (t <= -43200) t += 86400; /* t = (int)getbitu(msg->msg, p + 90, 13)*16 - (int)msg->tow%86400;
else if (t > 43200) t -= 86400; if (t <= -43200) t += 86400;
sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/ else if (t > 43200) t -= 86400;
} sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/
}
else 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 // 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; //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
for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*t;
// sv position correction // sv clock correction correction
for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*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);
// sv clock correction correction return 1;
*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 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? */
if (fabs(t) > MAXSBSAGEF)
/* 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;
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)
/* 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;
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)
} {
else if(fcorr.d_udre == 16) trace(2,"SV is marked as unhealthy: time_stamp=%f prn=%2d", time_stamp, d_prn);
{ return 0;
trace(2,"SV is marked as unhealthy: time_stamp=%f prn=%2d", time_stamp, d_prn); }
return 0; *prc = fcorr.d_prc;
}
*prc = fcorr.d_prc;
#ifdef RRCENA #ifdef RRCENA
if (fcorr.d_ai > 0 && fabs(t) <= 8.0*fcorr.d_dt) if (fcorr.d_ai > 0 && fabs(t) <= 8.0*fcorr.d_dt)
{ {
*prc += fcorr.d_rrc*t; *prc += fcorr.d_rrc*t;
} }
#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;
} }

205
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) */
@ -174,7 +174,7 @@ void Sbas_Telemetry_Data::updated_sbas_ephemeris(Sbas_Raw_Msg msg)
Sbas_Ephemeris seph; Sbas_Ephemeris seph;
int satidx = msg.get_prn()-MINPRNSBS; int satidx = msg.get_prn() - MINPRNSBS;
seph_t seph_rtklib = d_nav.seph[satidx]; seph_t seph_rtklib = d_nav.seph[satidx];
// copy data // copy data
@ -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);
@ -433,7 +412,7 @@ int Sbas_Telemetry_Data::satno(int sys, int prn)
return NSATGPS + NSATGLO + prn - MINPRNGAL + 1; return NSATGPS + NSATGLO + prn - MINPRNGAL + 1;
case SYS_QZS: case SYS_QZS:
if (prn < MINPRNQZS || MAXPRNQZS < prn) return 0; if (prn < MINPRNQZS || MAXPRNQZS < prn) return 0;
return NSATGPS+NSATGLO+NSATGAL+prn-MINPRNQZS+1; return NSATGPS + NSATGLO + NSATGAL + prn - MINPRNQZS + 1;
case SYS_CMP: case SYS_CMP:
if (prn < MINPRNCMP || MAXPRNCMP < prn) return 0; if (prn < MINPRNCMP || MAXPRNCMP < prn) return 0;
return NSATGPS + NSATGLO + NSATGAL + NSATQZS + prn - MINPRNCMP + 1; return NSATGPS + NSATGLO + NSATGAL + NSATQZS + prn - MINPRNCMP + 1;
@ -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;
@ -479,12 +459,12 @@ Sbas_Telemetry_Data::gtime_t Sbas_Telemetry_Data::epoch2time(const double *ep)
{ {
const int doy[]={1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}; const int doy[]={1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335};
gtime_t time = {0}; gtime_t time = {0};
int days, sec, year = (int)ep[0], mon=(int)ep[1], day=(int)ep[2]; int days, sec, year = (int)ep[0], mon = (int)ep[1], day = (int)ep[2];
if (year<1970 || 2099<year || mon<1 || 12<mon) return time; if (year < 1970 || 2099 < year || mon < 1 || 12 < mon) return time;
/* leap year if year%4==0 in 1901-2099 */ /* leap year if year%4==0 in 1901-2099 */
days = (year - 1970)*365 + (year - 1969)/4 + doy[mon-1] + day -2 + (year%4==0&&mon>=3?1:0); days = (year - 1970)*365 + (year - 1969)/4 + doy[mon - 1] + day - 2 + (year % 4 == 0 && mon >= 3 ? 1 : 0);
sec = (int)floor(ep[5]); sec = (int)floor(ep[5]);
time.time = (time_t)days*86400 + (int)ep[3]*3600 + (int)ep[4]*60 + sec; time.time = (time_t)days*86400 + (int)ep[3]*3600 + (int)ep[4]*60 + sec;
time.sec = ep[5] - sec; time.sec = ep[5] - sec;
@ -579,8 +559,8 @@ const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband2[2][5]={ /*
/* decode type 1: prn masks --------------------------------------------------*/ /* decode type 1: prn masks --------------------------------------------------*/
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:");
@ -588,14 +568,14 @@ int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
if (getbitu(msg->msg, 13+i, 1)) if (getbitu(msg->msg, 13+i, 1))
{ {
if (i <= 37) sat = satno(SYS_GPS, i); /* 0- 37: gps */ if (i <= 37) sat = satno(SYS_GPS, i); /* 0- 37: gps */
else if (i <= 61) sat = satno(SYS_GLO, i - 37); /* 38- 61: glonass */ else if (i <= 61) sat = satno(SYS_GLO, i - 37); /* 38- 61: glonass */
else if (i <= 119) sat = 0; /* 62-119: future gnss */ else if (i <= 119) sat = 0; /* 62-119: future gnss */
else if (i <= 138) sat = satno(SYS_SBS, i); /* 120-138: geo/waas */ else if (i <= 138) sat = satno(SYS_SBS, i); /* 120-138: geo/waas */
else if (i <= 182) sat = 0; /* 139-182: reserved */ else if (i <= 182) sat = 0; /* 139-182: reserved */
else if (i <= 192) sat = satno(SYS_SBS,i+10); /* 183-192: qzss ref [2] */ else if (i <= 192) sat = satno(SYS_SBS, i + 10); /* 183-192: qzss ref [2] */
else if (i <= 202) sat = satno(SYS_QZS, i); /* 193-202: qzss ref [2] */ else if (i <= 202) sat = satno(SYS_QZS, i); /* 193-202: qzss ref [2] */
else sat = 0; /* 203- : reserved */ else sat = 0; /* 203- : reserved */
sbssat->sat[n++].sat = sat; sbssat->sat[n++].sat = sat;
} }
} }
@ -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)
{ {
@ -617,20 +599,20 @@ int Sbas_Telemetry_Data::decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
trace(4,"decode_sbstype2:"); trace(4,"decode_sbstype2:");
if (sbssat->iodp!=(int)getbitu(msg->msg,16,2)) return 0; if (sbssat->iodp != (int)getbitu(msg->msg, 16, 2)) return 0;
type = getbitu(msg->msg, 8, 6); type = getbitu(msg->msg, 8, 6);
iodf = getbitu(msg->msg, 14, 2); iodf = getbitu(msg->msg, 14, 2);
for (i=0; i<13; i++) for (i=0; i<13; i++)
{ {
if ((j = 13*((type==0?2:type)-2)+i) >= sbssat->nsat) break; if ((j = 13*((type == 0 ? 2 : type) - 2) + i) >= sbssat->nsat) break;
udrei = getbitu(msg->msg, 174 + 4*i, 4); udrei = getbitu(msg->msg, 174 + 4*i, 4);
t0_past = sbssat->sat[j].fcorr.t0; t0_past = sbssat->sat[j].fcorr.t0;
prc = sbssat->sat[j].fcorr.prc; prc = sbssat->sat[j].fcorr.prc;
sbssat->sat[j].fcorr.t0 = msg->sample_stamp; sbssat->sat[j].fcorr.t0 = msg->sample_stamp;
sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i*12, 12)*0.125f; sbssat->sat[j].fcorr.prc = getbits(msg->msg, 18 + i*12, 12)*0.125f;
sbssat->sat[j].fcorr.udre = udrei+1; sbssat->sat[j].fcorr.udre = udrei + 1;
dt = sbssat->sat[j].fcorr.t0 - t0_past; dt = sbssat->sat[j].fcorr.t0 - t0_past;
if (!sbssat->sat[j].fcorr.valid || dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0) if (!sbssat->sat[j].fcorr.valid || dt <= 0.0 || 18.0 < dt || sbssat->sat[j].fcorr.ai == 0)
{ {
@ -639,7 +621,7 @@ int Sbas_Telemetry_Data::decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
} }
else else
{ {
sbssat->sat[j].fcorr.rrc = (sbssat->sat[j].fcorr.prc-prc)/dt; sbssat->sat[j].fcorr.rrc = (sbssat->sat[j].fcorr.prc - prc)/dt;
sbssat->sat[j].fcorr.dt = dt; sbssat->sat[j].fcorr.dt = dt;
} }
sbssat->sat[j].fcorr.valid = true; sbssat->sat[j].fcorr.valid = true;
@ -669,7 +651,7 @@ int Sbas_Telemetry_Data::decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
if (!sbssat->sat[i].fcorr.valid || sbssat->sat[i].fcorr.iodf != iodf[i/13]) continue; if (!sbssat->sat[i].fcorr.valid || sbssat->sat[i].fcorr.iodf != iodf[i/13]) continue;
udrei = getbitu(msg->msg, 22 + i*4, 4); udrei = getbitu(msg->msg, 22 + i*4, 4);
sbssat->sat[i].fcorr.udre = udrei+1; sbssat->sat[i].fcorr.udre = udrei + 1;
} }
trace(5, "decode_sbstype6: iodf=%d %d %d %d", iodf[0], iodf[1], iodf[2], iodf[3]); trace(5, "decode_sbstype6: iodf=%d %d %d %d", iodf[0], iodf[1], iodf[2], iodf[3]);
return 1; return 1;
@ -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);
@ -700,7 +678,7 @@ int Sbas_Telemetry_Data::decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat)
/* decode type 9: geo navigation message -------------------------------------*/ /* decode type 9: geo navigation message -------------------------------------*/
int Sbas_Telemetry_Data::decode_sbstype9(const sbsmsg_t *msg, nav_t *nav) int Sbas_Telemetry_Data::decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
{ {
seph_t seph={0}; seph_t seph = {0};
int i; int i;
int sat; int sat;
@ -719,17 +697,17 @@ int Sbas_Telemetry_Data::decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
seph.t0 = getbitu(msg->msg, 22, 13)*16; seph.t0 = getbitu(msg->msg, 22, 13)*16;
seph.tof = msg->sample_stamp; seph.tof = msg->sample_stamp;
seph.sva = getbitu(msg->msg, 35, 4); seph.sva = getbitu(msg->msg, 35, 4);
seph.svh = seph.sva==15?1:0; /* unhealthy if ura==15 */ seph.svh = seph.sva == 15 ? 1 : 0; /* unhealthy if ura==15 */
seph.pos[0] = getbits(msg->msg, 39,30)*0.08; seph.pos[0] = getbits(msg->msg, 39, 30)*0.08;
seph.pos[1] = getbits(msg->msg, 69,30)*0.08; seph.pos[1] = getbits(msg->msg, 69, 30)*0.08;
seph.pos[2] = getbits(msg->msg, 99,25)*0.4; seph.pos[2] = getbits(msg->msg, 99, 25)*0.4;
seph.vel[0] = getbits(msg->msg,124,17)*0.000625; seph.vel[0] = getbits(msg->msg, 124, 17)*0.000625;
seph.vel[1] = getbits(msg->msg,141,17)*0.000625; seph.vel[1] = getbits(msg->msg, 141, 17)*0.000625;
seph.vel[2] = getbits(msg->msg,158,18)*0.004; seph.vel[2] = getbits(msg->msg, 158, 18)*0.004;
seph.acc[0] = getbits(msg->msg,176,10)*0.0000125; seph.acc[0] = getbits(msg->msg, 176, 10)*0.0000125;
seph.acc[1] = getbits(msg->msg,186,10)*0.0000125; seph.acc[1] = getbits(msg->msg, 186, 10)*0.0000125;
seph.acc[2] = getbits(msg->msg,196,10)*0.0000625; seph.acc[2] = getbits(msg->msg, 196, 10)*0.0000625;
seph.af0 = getbits(msg->msg, 206, 12)*P2_31; seph.af0 = getbits(msg->msg, 206, 12)*P2_31;
seph.af1 = getbits(msg->msg, 218, 8)*P2_39/2.0; seph.af1 = getbits(msg->msg, 218, 8)*P2_39/2.0;
@ -740,8 +718,8 @@ int Sbas_Telemetry_Data::decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
VLOG(FLOW) << "<<T>> no change in ephemeris -> won't parse"; VLOG(FLOW) << "<<T>> no change in ephemeris -> won't parse";
return 0; return 0;
} }
nav->seph[NSATSBS+i] = nav->seph[i]; /* previous */ nav->seph[NSATSBS + i] = nav->seph[i]; /* previous */
nav->seph[i] = seph; /* current */ nav->seph[i] = seph; /* current */
trace(5, "decode_sbstype9: prn=%d", msg->prn); trace(5, "decode_sbstype9: prn=%d", msg->prn);
return 1; return 1;
@ -761,9 +739,10 @@ int Sbas_Telemetry_Data::decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
else if (9 <= band && band <= 10) {p = igpband2[band - 9]; m = 5;} else if (9 <= band && band <= 10) {p = igpband2[band - 9]; m = 5;}
else return 0; else return 0;
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;
@ -773,9 +752,9 @@ int Sbas_Telemetry_Data::decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
if (!getbitu(msg->msg, 23+i, 1)) continue; if (!getbitu(msg->msg, 23+i, 1)) continue;
for (j=0; j<m; j++) for (j=0; j<m; j++)
{ {
if (i<p[j].bits||p[j].bite<i) continue; if (i < p[j].bits || p[j].bite < i) continue;
sbsion[band].igp[n].lat = band <= 8?p[j].y[i-p[j].bits]:p[j].x; sbsion[band].igp[n].lat = band <= 8 ? p[j].y[i - p[j].bits] : p[j].x;
sbsion[band].igp[n++].lon = band <= 8?p[j].x:p[j].y[i-p[j].bits]; sbsion[band].igp[n++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits];
break; break;
} }
} }
@ -791,27 +770,27 @@ int Sbas_Telemetry_Data::decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
/* decode half long term correction (vel code=0) -----------------------------*/ /* decode half long term correction (vel code=0) -----------------------------*/
int Sbas_Telemetry_Data::decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat) int Sbas_Telemetry_Data::decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat)
{ {
int i,n = getbitu(msg->msg, p, 6); int i, n = getbitu(msg->msg, p, 6);
trace(4, "decode_longcorr0:"); trace(4, "decode_longcorr0:");
if (n==0 || n>MAXSAT) return 0; if (n == 0 || n > MAXSAT) return 0;
sbssat->sat[n-1].lcorr.iode = getbitu(msg->msg, p+6, 8); sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
for (i=0;i<3;i++) for (i=0; i<3; i++)
{ {
sbssat->sat[n-1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + 9*i, 9)*0.125; sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + 9*i, 9)*0.125;
sbssat->sat[n-1].lcorr.dvel[i] = 0.0; sbssat->sat[n - 1].lcorr.dvel[i] = 0.0;
} }
sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg,p+41,10)*P2_31; sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 41, 10)*P2_31;
sbssat->sat[n-1].lcorr.daf1 = 0.0; sbssat->sat[n - 1].lcorr.daf1 = 0.0;
//sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow); //sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow);
sbssat->sat[n-1].lcorr.trx = msg->sample_stamp; // vel=0 -> time of applicability is reception time sbssat->sat[n - 1].lcorr.trx = msg->sample_stamp; // vel=0 -> time of applicability is reception time
sbssat->sat[n-1].lcorr.tapp = 0; sbssat->sat[n - 1].lcorr.tapp = 0;
sbssat->sat[n-1].lcorr.valid = true; sbssat->sat[n - 1].lcorr.valid = true;
trace(5, "decode_longcorr0:sat=%2d", sbssat->sat[n-1].sat); trace(5, "decode_longcorr0:sat=%2d", sbssat->sat[n - 1].sat);
return 1; return 1;
} }
@ -827,25 +806,25 @@ int Sbas_Telemetry_Data::decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *
if (n==0 || n>MAXSAT) return 0; if (n==0 || n>MAXSAT) return 0;
sbssat->sat[n-1].lcorr.iode = getbitu(msg->msg, p + 6,8); sbssat->sat[n - 1].lcorr.iode = getbitu(msg->msg, p + 6, 8);
for (i=0; i<3; i++) for (i=0; i<3; i++)
{ {
sbssat->sat[n-1].lcorr.dpos[i] = getbits(msg->msg, p + 14 +i*11, 11)*0.125; sbssat->sat[n - 1].lcorr.dpos[i] = getbits(msg->msg, p + 14 + i*11, 11)*0.125;
sbssat->sat[n-1].lcorr.dvel[i] = getbits(msg->msg, p + 58 +i* 8, 8)*P2_11; sbssat->sat[n - 1].lcorr.dvel[i] = getbits(msg->msg, p + 58 + i*8, 8)*P2_11;
} }
sbssat->sat[n-1].lcorr.daf0 = getbits(msg->msg, p + 47, 11)*P2_31; sbssat->sat[n - 1].lcorr.daf0 = getbits(msg->msg, p + 47, 11)*P2_31;
sbssat->sat[n-1].lcorr.daf1 = getbits(msg->msg, p + 82, 8)*P2_39; sbssat->sat[n - 1].lcorr.daf1 = getbits(msg->msg, p + 82, 8)*P2_39;
// 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
/*t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400; /*t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400;
if (t<=-43200) t+=86400; if (t<=-43200) t+=86400;
else if (t> 43200) t-=86400; else if (t> 43200) t-=86400;
sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t);*/ sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t);*/
sbssat->sat[n-1].lcorr.trx = msg->sample_stamp; sbssat->sat[n - 1].lcorr.trx = msg->sample_stamp;
sbssat->sat[n-1].lcorr.tapp = (int)getbitu(msg->msg, p + 90, 13)*16; sbssat->sat[n - 1].lcorr.tapp = (int)getbitu(msg->msg, p + 90, 13)*16;
sbssat->sat[n-1].lcorr.vel = 1; sbssat->sat[n - 1].lcorr.vel = 1;
sbssat->sat[n-1].lcorr.valid = true; sbssat->sat[n - 1].lcorr.valid = true;
trace(5, "decode_longcorr1: sat=%2d", sbssat->sat[n-1].sat); trace(5, "decode_longcorr1: sat=%2d", sbssat->sat[n - 1].sat);
return 1; return 1;
} }
@ -858,15 +837,16 @@ 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);
} }
} }
else if (sbssat->iodp == (int)getbitu(msg->msg, p + 104, 2)) else if (sbssat->iodp == (int)getbitu(msg->msg, p + 104, 2))
{ {
return decode_longcorr1(msg, p+1, sbssat); return decode_longcorr1(msg, p + 1, sbssat);
} }
return 0; return 0;
} }
@ -894,7 +874,7 @@ int Sbas_Telemetry_Data::decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat)
//sbssat->sat[j].fcorr.t0 =gpst2time(msg->week,msg->tow); //sbssat->sat[j].fcorr.t0 =gpst2time(msg->week,msg->tow);
sbssat->sat[j].fcorr.t0 = msg->sample_stamp; sbssat->sat[j].fcorr.t0 = msg->sample_stamp;
sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14 + i*12, 12)*0.125f; sbssat->sat[j].fcorr.prc = getbits(msg->msg, 14 + i*12, 12)*0.125f;
sbssat->sat[j].fcorr.udre = udre+1; sbssat->sat[j].fcorr.udre = udre + 1;
sbssat->sat[j].fcorr.iodf = iodf; sbssat->sat[j].fcorr.iodf = iodf;
} }
return decode_longcorrh(msg, 120, sbssat); return decode_longcorrh(msg, 120, sbssat);
@ -908,7 +888,7 @@ int Sbas_Telemetry_Data::decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat)
{ {
trace(4,"decode_sbstype25:"); trace(4,"decode_sbstype25:");
return decode_longcorrh(msg,14,sbssat)&&decode_longcorrh(msg,120,sbssat); return decode_longcorrh(msg, 14, sbssat) && decode_longcorrh(msg, 120, sbssat);
} }
@ -925,13 +905,13 @@ int Sbas_Telemetry_Data::decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
for (i=0; i<15; i++) for (i=0; i<15; i++)
{ {
if ((j = block*15+i) >= sbsion[band].nigp) continue; if ((j = block*15 + i) >= sbsion[band].nigp) continue;
give = getbitu(msg->msg, 2 + i*13 + 9, 4); give = getbitu(msg->msg, 2 + i*13 + 9, 4);
delay = getbitu(msg->msg, 22 + i*13, 9); delay = getbitu(msg->msg, 22 + i*13, 9);
//sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow); //sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow);
sbsion[band].igp[j].t0 = msg->sample_stamp; sbsion[band].igp[j].t0 = msg->sample_stamp;
sbsion[band].igp[j].valid = true; sbsion[band].igp[j].valid = true;
sbsion[band].igp[j].delay = delay == 0x1FF?0.0f:delay*0.125f; sbsion[band].igp[j].delay = delay == 0x1FF ? 0.0f : delay*0.125f;
sbsion[band].igp[j].give = give; sbsion[band].igp[j].give = give;
if(sbsion[band].igp[j].give > 15) sbsion[band].igp[j].give = 15; // give is not higher than 15, but to be sure if(sbsion[band].igp[j].give > 15) sbsion[band].igp[j].give = 15; // give is not higher than 15, but to be sure
@ -955,7 +935,7 @@ int Sbas_Telemetry_Data::decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion)
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
int Sbas_Telemetry_Data::sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav) int Sbas_Telemetry_Data::sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
{ {
int type=getbitu(msg->msg,8,6),stat=-1; int type = getbitu(msg->msg, 8, 6), stat = -1;
trace(3,"sbsupdatecorr: type=%d",type); trace(3,"sbsupdatecorr: type=%d",type);
@ -963,15 +943,15 @@ int Sbas_Telemetry_Data::sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
switch (type) switch (type)
{ {
case 0: stat = decode_sbstype2 (msg, &nav->sbssat); break; case 0: stat = decode_sbstype2(msg, &nav->sbssat); break;
case 1: stat = decode_sbstype1 (msg, &nav->sbssat); break; case 1: stat = decode_sbstype1(msg, &nav->sbssat); break;
case 2: case 2:
case 3: case 3:
case 4: case 4:
case 5: stat = decode_sbstype2 (msg, &nav->sbssat); break; case 5: stat = decode_sbstype2(msg, &nav->sbssat); break;
case 6: stat = decode_sbstype6 (msg, &nav->sbssat); break; case 6: stat = decode_sbstype6(msg, &nav->sbssat); break;
case 7: stat = decode_sbstype7 (msg, &nav->sbssat); break; case 7: stat = decode_sbstype7(msg, &nav->sbssat); break;
case 9: stat = decode_sbstype9 (msg, nav); break; case 9: stat = decode_sbstype9(msg, nav); break;
case 10: trace(2, "unsupported sbas message: type=%d", type); break; case 10: trace(2, "unsupported sbas message: type=%d", type); break;
case 12: trace(2, "unsupported sbas message: type=%d", type); break; case 12: trace(2, "unsupported sbas message: type=%d", type); break;
case 17: trace(2, "unsupported sbas message: type=%d", type); break; case 17: trace(2, "unsupported sbas message: type=%d", type); break;
@ -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++)

703
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
{ {
@ -90,425 +90,416 @@ public:
} }
private: private:
//int d_week; /* reception time */ //int d_week; /* reception time */
//int d_tow; /* reception time */ //int d_tow; /* reception time */
Sbas_Time rx_time; Sbas_Time rx_time;
int i_prn; /* SBAS satellite PRN number */ int i_prn; /* SBAS satellite PRN number */
std::vector<unsigned char> d_msg; /* SBAS message (226bit) padded by 0 */ std::vector<unsigned char> d_msg; /* SBAS message (226 bit) padded by 0 */
}; };
/* /*
* \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
*/ */
Sbas_Telemetry_Data(); Sbas_Telemetry_Data();
/*! /*!
* Default deconstructor * Default deconstructor
*/ */
~Sbas_Telemetry_Data(); ~Sbas_Telemetry_Data();
private: private:
std::map<int, Fast_Correction> emitted_fast_corrections; std::map<int, Fast_Correction> emitted_fast_corrections;
std::map<int, Long_Term_Correction> emitted_long_term_corrections; std::map<int, Long_Term_Correction> emitted_long_term_corrections;
Sbas_Time_Relation mt12_time_ref; Sbas_Time_Relation mt12_time_ref;
concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue; concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue;
concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue; concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue;
concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue; concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue;
concurrent_queue<Sbas_Ephemeris> *ephemeris_queue; concurrent_queue<Sbas_Ephemeris> *ephemeris_queue;
int decode_mt12(Sbas_Raw_Msg sbas_raw_msg); int decode_mt12(Sbas_Raw_Msg sbas_raw_msg);
void updated_sbas_ephemeris(Sbas_Raw_Msg msg); void updated_sbas_ephemeris(Sbas_Raw_Msg msg);
void received_iono_correction(); void received_iono_correction();
void updated_satellite_corrections(); void updated_satellite_corrections();
/////// rtklib.h /////// rtklib.h
#define SYS_NONE 0x00 /* navigation system: none */ #define SYS_NONE 0x00 /* navigation system: none */
#define SYS_GPS 0x01 /* navigation system: GPS */ #define SYS_GPS 0x01 /* navigation system: GPS */
#define SYS_SBS 0x02 /* navigation system: SBAS */ #define SYS_SBS 0x02 /* navigation system: SBAS */
#define SYS_GLO 0x04 /* navigation system: GLONASS */ #define SYS_GLO 0x04 /* navigation system: GLONASS */
#define SYS_GAL 0x08 /* navigation system: Galileo */ #define SYS_GAL 0x08 /* navigation system: Galileo */
#define SYS_QZS 0x10 /* navigation system: QZSS */ #define SYS_QZS 0x10 /* navigation system: QZSS */
#define SYS_CMP 0x20 /* navigation system: BeiDou */ #define SYS_CMP 0x20 /* navigation system: BeiDou */
#define SYS_ALL 0xFF /* navigation system: all */ #define SYS_ALL 0xFF /* navigation system: all */
#define TSYS_GPS 0 /* time system: GPS time */ #define TSYS_GPS 0 /* time system: GPS time */
#define TSYS_UTC 1 /* time system: UTC */ #define TSYS_UTC 1 /* time system: UTC */
#define TSYS_GLO 2 /* time system: GLONASS time */ #define TSYS_GLO 2 /* time system: GLONASS time */
#define TSYS_GAL 3 /* time system: Galileo time */ #define TSYS_GAL 3 /* time system: Galileo time */
#define TSYS_QZS 4 /* time system: QZSS time */ #define TSYS_QZS 4 /* time system: QZSS time */
#define TSYS_CMP 5 /* time system: BeiDou time */ #define TSYS_CMP 5 /* time system: BeiDou time */
#ifndef NFREQ #ifndef NFREQ
#define NFREQ 3 /* number of carrier frequencies */ #define NFREQ 3 /* number of carrier frequencies */
#endif #endif
#define NFREQGLO 2 /* number of carrier frequencies of GLONASS */ #define NFREQGLO 2 /* number of carrier frequencies of GLONASS */
#ifndef NEXOBS #ifndef NEXOBS
#define NEXOBS 0 /* number of extended obs codes */ #define NEXOBS 0 /* number of extended obs codes */
#endif #endif
#define MINPRNGPS 1 /* min satellite PRN number of GPS */ #define MINPRNGPS 1 /* min satellite PRN number of GPS */
#define MAXPRNGPS 32 /* max satellite PRN number of GPS */ #define MAXPRNGPS 32 /* max satellite PRN number of GPS */
#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */ #define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */
#define NSYSGPS 1 #define NSYSGPS 1
#ifdef ENAGLO #ifdef ENAGLO
#define MINPRNGLO 1 /* min satellite slot number of GLONASS */ #define MINPRNGLO 1 /* min satellite slot number of GLONASS */
#define MAXPRNGLO 24 /* max satellite slot number of GLONASS */ #define MAXPRNGLO 24 /* max satellite slot number of GLONASS */
#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */ #define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */
#define NSYSGLO 1 #define NSYSGLO 1
#else #else
#define MINPRNGLO 0 #define MINPRNGLO 0
#define MAXPRNGLO 0 #define MAXPRNGLO 0
#define NSATGLO 0 #define NSATGLO 0
#define NSYSGLO 0 #define NSYSGLO 0
#endif #endif
#ifdef ENAGAL #ifdef ENAGAL
#define MINPRNGAL 1 /* min satellite PRN number of Galileo */ #define MINPRNGAL 1 /* min satellite PRN number of Galileo */
#define MAXPRNGAL 27 /* max satellite PRN number of Galileo */ #define MAXPRNGAL 27 /* max satellite PRN number of Galileo */
#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */ #define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */
#define NSYSGAL 1 #define NSYSGAL 1
#else #else
#define MINPRNGAL 0 #define MINPRNGAL 0
#define MAXPRNGAL 0 #define MAXPRNGAL 0
#define NSATGAL 0 #define NSATGAL 0
#define NSYSGAL 0 #define NSYSGAL 0
#endif #endif
#ifdef ENAQZS #ifdef ENAQZS
#define MINPRNQZS 193 /* min satellite PRN number of QZSS */ #define MINPRNQZS 193 /* min satellite PRN number of QZSS */
#define MAXPRNQZS 195 /* max 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 MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */
#define MAXPRNQZS_S 185 /* max 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 NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */
#define NSYSQZS 1 #define NSYSQZS 1
#else #else
#define MINPRNQZS 0 #define MINPRNQZS 0
#define MAXPRNQZS 0 #define MAXPRNQZS 0
#define NSATQZS 0 #define NSATQZS 0
#define NSYSQZS 0 #define NSYSQZS 0
#endif #endif
#ifdef ENACMP #ifdef ENACMP
#define MINPRNCMP 1 /* min satellite sat number of BeiDou */ #define MINPRNCMP 1 /* min satellite sat number of BeiDou */
#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */ #define MAXPRNCMP 35 /* max satellite sat number of BeiDou */
#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */ #define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */
#define NSYSCMP 1 #define NSYSCMP 1
#else #else
#define MINPRNCMP 0 #define MINPRNCMP 0
#define MAXPRNCMP 0 #define MAXPRNCMP 0
#define NSATCMP 0 #define NSATCMP 0
#define NSYSCMP 0 #define NSYSCMP 0
#endif #endif
#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP) /* number of systems */ #define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP) /* number of systems */
#define MINPRNSBS 120 /* min satellite PRN number of SBAS */ #define MINPRNSBS 120 /* min satellite PRN number of SBAS */
#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */ #define MAXPRNSBS 142 /* max satellite PRN number of SBAS */
#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */ #define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */
#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATSBS) #define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATSBS)
/* max satellite number (1 to MAXSAT) */ /* max satellite number (1 to MAXSAT) */
#ifndef MAXOBS #ifndef MAXOBS
#define MAXOBS 64 /* max number of obs in an epoch */ #define MAXOBS 64 /* max number of obs in an epoch */
#endif #endif
#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */ #define MAXRCV 64 /* max receiver number (1 to MAXRCV) */
#define MAXOBSTYPE 64 /* max number of obs type in RINEX */ #define MAXOBSTYPE 64 /* max number of obs type in RINEX */
#define DTTOL 0.005 /* tolerance of time difference (s) */ #define DTTOL 0.005 /* tolerance of time difference (s) */
#if 0 #if 0
#define MAXDTOE 10800.0 /* max time difference to ephem Toe (s) for GPS */ #define MAXDTOE 10800.0 /* max time difference to ephem Toe (s) for GPS */
#else #else
#define MAXDTOE 7200.0 /* max time difference to ephem Toe (s) for GPS */ #define MAXDTOE 7200.0 /* max time difference to ephem Toe (s) for GPS */
#endif #endif
#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */ #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_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 MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */
#define MAXGDOP 300.0 /* max GDOP */ #define MAXGDOP 300.0 /* max GDOP */
//#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */ //#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
//#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */ //#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */
//#define MAXSBSURA 8 /* max URA of SBAS satellite */ //#define MAXSBSURA 8 /* max URA of SBAS satellite */
#define MAXBAND 10 /* max SBAS band of IGP */ #define MAXBAND 10 /* max SBAS band of IGP */
#define MAXNIGP 201 /* max number of IGP in SBAS band */ #define MAXNIGP 201 /* max number of IGP in SBAS band */
//#define MAXNGEO 4 /* max number of GEO satellites */ //#define MAXNGEO 4 /* max number of GEO satellites */
#define P2_11 4.882812500000000E-04 /* 2^-11 */ #define P2_11 4.882812500000000E-04 /* 2^-11 */
#define P2_31 4.656612873077393E-10 /* 2^-31 */ #define P2_31 4.656612873077393E-10 /* 2^-31 */
#define P2_39 1.818989403545856E-12 /* 2^-39 */ #define P2_39 1.818989403545856E-12 /* 2^-39 */
/* type definitions ----------------------------------------------------------*/ /* type definitions ----------------------------------------------------------*/
typedef struct { /* time struct */ typedef struct { /* time struct */
time_t time; /* time (s) expressed by standard time_t */ time_t time; /* time (s) expressed by standard time_t */
double sec; /* fraction of second under 1 s */ double sec; /* fraction of second under 1 s */
} gtime_t; } gtime_t;
typedef struct { /* SBAS message type */ typedef struct { /* SBAS message type */
//int week,tow; /* receiption time */ //int week,tow; /* receiption time */
double sample_stamp; double sample_stamp;
int prn; /* SBAS satellite PRN number */ int prn; /* SBAS satellite PRN number */
unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */ unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */
} sbsmsg_t; } sbsmsg_t;
typedef struct { /* SBAS messages type */ typedef struct { /* SBAS messages type */
int n,nmax; /* number of SBAS messages/allocated */ int n,nmax; /* number of SBAS messages/allocated */
sbsmsg_t *msgs; /* SBAS messages */ sbsmsg_t *msgs; /* SBAS messages */
} sbs_t; } sbs_t;
typedef struct { /* SBAS fast correction type */ typedef struct { /* SBAS fast correction type */
//gtime_t t0; /* time of applicability (TOF) */ //gtime_t t0; /* time of applicability (TOF) */
double t0; double t0;
bool valid; bool valid;
double prc; /* pseudorange correction (PRC) (m) */ double prc; /* pseudorange correction (PRC) (m) */
double rrc; /* range-rate correction (RRC) (m/s) */ double rrc; /* range-rate correction (RRC) (m/s) */
double dt; /* range-rate correction delta-time (s) */ double dt; /* range-rate correction delta-time (s) */
int iodf; /* IODF (issue of date fast corr) */ int iodf; /* IODF (issue of date fast corr) */
short udre; /* UDRE+1 */ short udre; /* UDRE+1 */
short ai; /* degradation factor indicator */ short ai; /* degradation factor indicator */
} sbsfcorr_t; } sbsfcorr_t;
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) */
double dvel[3]; /* delta velocity (m/s) (ecef) */ double dvel[3]; /* delta velocity (m/s) (ecef) */
double daf0,daf1; /* delta clock-offset/drift (s,s/s) */ double daf0,daf1; /* delta clock-offset/drift (s,s/s) */
} sbslcorr_t; } sbslcorr_t;
typedef struct { /* SBAS satellite correction type */ typedef struct { /* SBAS satellite correction type */
int sat; /* satellite number */ int sat; /* satellite number */
sbsfcorr_t fcorr; /* fast correction */ sbsfcorr_t fcorr; /* fast correction */
sbslcorr_t lcorr; /* long term correction */ sbslcorr_t lcorr; /* long term correction */
} sbssatp_t; } sbssatp_t;
typedef struct { /* SBAS satellite corrections type */ typedef struct { /* SBAS satellite corrections type */
int iodp; /* IODP (issue of date mask) */ int iodp; /* IODP (issue of date mask) */
int nsat; /* number of satellites */ int nsat; /* number of satellites */
int tlat; /* system latency (s) */ int tlat; /* system latency (s) */
sbssatp_t sat[MAXSAT]; /* satellite correction */ sbssatp_t sat[MAXSAT]; /* satellite correction */
} sbssat_t; } sbssat_t;
typedef struct { /* SBAS ionospheric correction type */ typedef struct { /* SBAS ionospheric correction type */
//gtime_t t0; /* correction time */ //gtime_t t0; /* correction time */
double t0; double t0;
bool valid; bool valid;
short lat,lon; /* latitude/longitude (deg) */ short lat,lon; /* latitude/longitude (deg) */
short give; /* GIVI+1 */ short give; /* GIVI+1 */
float delay; /* vertical delay estimate (m) */ float delay; /* vertical delay estimate (m) */
} sbsigp_t; } sbsigp_t;
typedef struct { /* IGP band type */ typedef struct { /* IGP band type */
short x; /* longitude/latitude (deg) */ short x; /* longitude/latitude (deg) */
const short *y; /* latitudes/longitudes (deg) */ const short *y; /* latitudes/longitudes (deg) */
unsigned char bits; /* IGP mask start bit */ unsigned char bits; /* IGP mask start bit */
unsigned char bite; /* IGP mask end bit */ unsigned char bite; /* IGP mask end bit */
} sbsigpband_t; } sbsigpband_t;
typedef struct { /* SBAS ionospheric corrections type */ typedef struct { /* SBAS ionospheric corrections type */
int iodi; /* IODI (issue of date ionos corr) */ int iodi; /* IODI (issue of date ionos corr) */
int nigp; /* number of igps */ int nigp; /* number of igps */
sbsigp_t igp[MAXNIGP]; /* ionospheric correction */ sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
} sbsion_t; } sbsion_t;
/* /*
* idicators * indicators
*/ */
typedef struct { /* SBAS ephemeris type */ typedef struct { /* SBAS ephemeris type */
int sat; /* satellite number */ int sat; /* satellite number */
//gtime_t t0; /* reference epoch time (GPST) */ //gtime_t t0; /* reference epoch time (GPST) */
int t0; int t0;
//gtime_t tof; /* time of message frame (GPST) */ //gtime_t tof; /* time of message frame (GPST) */
double tof; double tof;
int sva; /* SV accuracy (URA index) */ int sva; /* SV accuracy (URA index) */
int svh; /* SV health (0:ok) */ int svh; /* SV health (0:ok) */
double pos[3]; /* satellite position (m) (ecef) */ double pos[3]; /* satellite position (m) (ecef) */
double vel[3]; /* satellite velocity (m/s) (ecef) */ double vel[3]; /* satellite velocity (m/s) (ecef) */
double acc[3]; /* satellite acceleration (m/s^2) (ecef) */ double acc[3]; /* satellite acceleration (m/s^2) (ecef) */
double af0,af1; /* satellite clock-offset/drift (s,s/s) */ double af0,af1; /* satellite clock-offset/drift (s,s/s) */
} seph_t; } seph_t;
typedef struct { /* navigation data type */ typedef struct { /* navigation data type */
//int n,nmax; /* number of broadcast ephemeris */ //int n,nmax; /* number of broadcast ephemeris */
//int ng,ngmax; /* number of glonass ephemeris */ //int ng,ngmax; /* number of glonass ephemeris */
//int ns,nsmax; /* number of sbas ephemeris */ //int ns,nsmax; /* number of sbas ephemeris */
//int ne,nemax; /* number of precise ephemeris */ //int ne,nemax; /* number of precise ephemeris */
//int nc,ncmax; /* number of precise clock */ //int nc,ncmax; /* number of precise clock */
//int na,namax; /* number of almanac data */ //int na,namax; /* number of almanac data */
//int nt,ntmax; /* number of tec grid data */ //int nt,ntmax; /* number of tec grid data */
//int nn,nnmax; /* number of stec grid data */ //int nn,nnmax; /* number of stec grid data */
//eph_t *eph; /* GPS/QZS/GAL ephemeris */ //eph_t *eph; /* GPS/QZS/GAL ephemeris */
//geph_t *geph; /* GLONASS ephemeris */ //geph_t *geph; /* GLONASS ephemeris */
seph_t seph[2*NSATSBS]; /* SBAS ephemeris */ seph_t seph[2*NSATSBS]; /* SBAS ephemeris */
// peph_t *peph; /* precise ephemeris */ // peph_t *peph; /* precise ephemeris */
// pclk_t *pclk; /* precise clock */ // pclk_t *pclk; /* precise clock */
// alm_t *alm; /* almanac data */ // alm_t *alm; /* almanac data */
// tec_t *tec; /* tec grid data */ // tec_t *tec; /* tec grid data */
// stec_t *stec; /* stec grid data */ // stec_t *stec; /* stec grid data */
// erp_t erp; /* earth rotation parameters */ // erp_t erp; /* earth rotation parameters */
//double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */ //double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */
//double utc_glo[4]; /* GLONASS UTC GPS time parameters */ //double utc_glo[4]; /* GLONASS UTC GPS time parameters */
//double utc_gal[4]; /* Galileo UTC GPS time parameters */ //double utc_gal[4]; /* Galileo UTC GPS time parameters */
//double utc_qzs[4]; /* QZS UTC GPS time parameters */ //double utc_qzs[4]; /* QZS UTC GPS time parameters */
//double utc_cmp[4]; /* BeiDou UTC parameters */ //double utc_cmp[4]; /* BeiDou UTC parameters */
//double utc_sbs[4]; /* SBAS 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_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_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_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} */ //double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
//int leaps; /* leap seconds (s) */ //int leaps; /* leap seconds (s) */
//double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */ //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 cbias[MAXSAT][3]; /* code bias (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
//double wlbias[MAXSAT]; /* wide-lane bias (cycle) */ //double wlbias[MAXSAT]; /* wide-lane bias (cycle) */
//double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */ //double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */
//char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */ //char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
// pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */ // pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
sbssat_t sbssat; /* SBAS satellite corrections */ sbssat_t sbssat; /* SBAS satellite corrections */
sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */ sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
// dgps_t dgps[MAXSAT]; /* DGPS corrections */ // dgps_t dgps[MAXSAT]; /* DGPS corrections */
// ssr_t ssr[MAXSAT]; /* SSR corrections */ // ssr_t ssr[MAXSAT]; /* SSR corrections */
// lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */ // lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
// lexion_t lexion; /* LEX ionosphere correction */ // lexion_t lexion; /* LEX ionosphere correction */
} nav_t; } nav_t;
//// common //// common
static const double gpst0[]; /* gps time reference */ static const double gpst0[]; /* gps time reference */
/* debug trace functions -----------------------------------------------------*/ /* debug trace functions -----------------------------------------------------*/
FILE *fp_trace; /* file pointer of trace */ FILE *fp_trace; /* file pointer of trace */
int level_trace; /* level of trace */ int level_trace; /* level of trace */
unsigned int tick_trace; /* tick time at traceopen (ms) */ unsigned int tick_trace; /* tick time at traceopen (ms) */
void trace(int level, const char *format, ...); void trace(int level, const char *format, ...);
/* satellite system+prn/slot number to satellite number ------------------------ /* satellite system+prn/slot number to satellite number ------------------------
* convert 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,...) * args : int sys I satellite system (SYS_GPS,SYS_GLO,...)
* int prn I satellite prn/slot number * int prn I satellite prn/slot number
* return : satellite number (0:error) * return : satellite number (0:error)
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
int satno(int sys, int prn); int satno(int sys, int prn);
/* extract unsigned/signed bits ------------------------------------------------ /* extract unsigned/signed bits ------------------------------------------------
* extract unsigned/signed bits from byte data * extract unsigned/signed bits from byte data
* args : unsigned char *buff I byte data * args : 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 getbitu(const unsigned char *buff, int pos, int len); unsigned int getbitu(const unsigned char *buff, int pos, int len);
int getbits(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 time -------------------------------------------
* convert calendar day/time to gtime_t struct * convert calendar day/time to gtime_t struct
* args : double *ep I day/time {year,month,day,hour,min,sec} * args : double *ep I day/time {year,month,day,hour,min,sec}
* return : gtime_t struct * return : gtime_t struct
* notes : proper in 1970-2037 or 1970-2099 (64bit time_t) * notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
gtime_t epoch2time(const double *ep); gtime_t epoch2time(const double *ep);
/* 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);
/* gps time to time ------------------------------------------------------------ /* gps time to time ------------------------------------------------------------
* convert week and tow in gps time to gtime_t struct * convert week and tow in gps time to gtime_t struct
* args : int week I week number in gps time * args : int week I week number in gps time
* double sec I time of week in gps time (s) * double sec I time of week in gps time (s)
* return : gtime_t struct * return : gtime_t struct
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
gtime_t gpst2time(int week, double sec); gtime_t gpst2time(int week, double sec);
////// sbas.c ////// sbas.c
/* sbas igp definition -------------------------------------------------------*/ /* sbas igp definition -------------------------------------------------------*/
static const short static const short
x1[], x1[],
x2[], x2[],
x3[], x3[],
x4[], x4[],
x5[], x5[],
x6[], x6[],
x7[], x7[],
x8[]; x8[];
static const sbsigpband_t igpband1[9][8];/* band 0-8 */ static const sbsigpband_t igpband1[9][8]; /* band 0-8 */
static const sbsigpband_t igpband2[2][5];/* band 9-10 */ static const sbsigpband_t igpband2[2][5]; /* band 9-10 */
/* decode type 1: prn masks --------------------------------------------------*/ /* decode type 1: prn masks --------------------------------------------------*/
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 2-5,0: fast corrections ---------------------------------------*/ /* decode type 2-5,0: fast corrections ---------------------------------------*/
int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 6: integrity info ---------------------------------------------*/ /* decode type 6: integrity info ---------------------------------------------*/
int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 7: fast correction degradation factor -------------------------*/ /* decode type 7: fast correction degradation factor -------------------------*/
int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 9: geo navigation message -------------------------------------*/ /* decode type 9: geo navigation message -------------------------------------*/
int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav); int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav);
/* decode type 18: ionospheric grid point masks ------------------------------*/ /* decode type 18: ionospheric grid point masks ------------------------------*/
int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion); int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion);
/* decode half long term correction (vel code=0) -----------------------------*/ /* decode half long term correction (vel code=0) -----------------------------*/
int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat); int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode half long term correction (vel code=1) -----------------------------*/ /* decode half long term correction (vel code=1) -----------------------------*/
int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat); int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode half long term correction ------------------------------------------*/ /* decode half long term correction ------------------------------------------*/
int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat); int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode type 24: mixed fast/long term correction ---------------------------*/ /* decode type 24: mixed fast/long term correction ---------------------------*/
int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 25: long term satellite error correction ----------------------*/ /* decode type 25: long term satellite error correction ----------------------*/
int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat); int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 26: ionospheric deley corrections -----------------------------*/ /* decode type 26: ionospheric deley corrections -----------------------------*/
int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion); int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion);
/* update sbas corrections ----------------------------------------------------- /* update sbas corrections -----------------------------------------------------
* update sbas correction parameters in navigation data with a sbas message * update sbas correction parameters in navigation data with a sbas message
* args : sbsmg_t *msg I sbas message * args : sbsmg_t *msg I sbas message
* nav_t *nav IO navigation data * nav_t *nav IO navigation data
* return : message type (-1: error or not supported type) * return : message type (-1: error or not supported type)
* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t) * 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] : sat prn current epehmeris
* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris * seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris
*-----------------------------------------------------------------------------*/ *-----------------------------------------------------------------------------*/
int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav); int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav);
void prn_mask_changed(); void prn_mask_changed();
bool is_rtklib_sat_correction_valid(int sat); bool is_rtklib_sat_correction_valid(int sat);
void igp_mask_changed(int band); void igp_mask_changed(int band);
// RTKLIB SBAS telemetry data representation
nav_t d_nav;
// RTKLIB SBAS telemetry data representation
nav_t d_nav;
}; };
#endif #endif

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

@ -61,7 +61,7 @@ public:
bool to_gps_time(double time_stamp_sec, int &gps_week, double &gps_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; int delta_weeks = int(trunc(time_stamp_sec + d_delta_sec))/604800;
gps_sec = time_stamp_sec+d_delta_sec-delta_weeks*604800; gps_sec = time_stamp_sec + d_delta_sec - delta_weeks*604800;
gps_week = i_gps_week + delta_weeks; 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; VLOG(FLOW) << "<<R>> to gps time: time_stamp_sec=" << time_stamp_sec << " gps_week=" << gps_week << " gps_sec=" << gps_sec;
return b_valid; return b_valid;
@ -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()
@ -177,10 +175,10 @@ public:
return (/*e_state == ABSOLUTE ||*/ e_state == RELATED); return (/*e_state == ABSOLUTE ||*/ e_state == RELATED);
} }
bool is_only_relativ() {return e_state == RELATIVE;} bool is_only_relativ() { return e_state == RELATIVE; }
//bool is_only_absolute() {return e_state == ABSOLUTE;} //bool is_only_absolute() {return e_state == ABSOLUTE;}
bool is_related() {return e_state == RELATED;} bool is_related() { return e_state == RELATED; }
Sbas_Time_State get_state() {return e_state;} Sbas_Time_State get_state() { return e_state; }
private: private:
Sbas_Time_State e_state; Sbas_Time_State e_state;
@ -190,4 +188,4 @@ private:
}; };
#endif /* SBAS_TIME_H_ */ #endif /* GNSS_SDR_SBAS_TIME_H_ */