mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-04-05 10:17: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
* \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
* \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
* \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
* \author Daniel Fehr 2013. daniel.co(at)bluewin.ch
*
@ -58,6 +58,9 @@ public:
return role_;
}
/*!
* \brief Returns "SBAS_L1_Telemetry_Decoder"
*/
std::string implementation()
{
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;
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()
{
@ -225,6 +226,7 @@ void sbas_l1_telemetry_decoder_cc::sample_aligner::reset()
d_aligned = true;
}
/*
* 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
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
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_paired = d_iir_par*smpls[1]*smpls[2] + (1 - d_iir_par)*d_corr_paired;
d_corr_shifted = d_iir_par*smpls[0]*smpls[1] + (1 - d_iir_par)*d_corr_shifted;
// decide which alignment is the correct one
corr_diff = std::abs(d_corr_paired-d_corr_shifted);
corr_diff = std::abs(d_corr_paired - d_corr_shifted);
stand_by = d_aligned ? corr_diff < d_corr_paired/2 : corr_diff < d_corr_shifted/2;
if (!stand_by)
{
@ -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
sym = smpls[0+int(d_aligned)*2] + smpls[1];
sym = smpls[0 + int(d_aligned)*2] + smpls[1];
symbols.push_back(sym);
// sample alignment debug output
@ -278,13 +280,11 @@ bool sbas_l1_telemetry_decoder_cc::sample_aligner::get_symbols(const std::vector
double temp;
temp = samples.back();
d_past_sample = (temp);
return d_aligned;
}
// ### helper class for symbol alignment and viterbi decoding ###
sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::symbol_aligner_and_decoder()
{
// convolutional code properties
@ -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_past_symbol = 0;
}
sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::~symbol_aligner_and_decoder()
{
delete d_vd1;
@ -311,14 +313,11 @@ void sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::reset()
d_vd2->reset();
}
bool sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::
get_bits(const std::vector<double> symbols, std::vector<int> &bits)
bool sbas_l1_telemetry_decoder_cc::symbol_aligner_and_decoder::get_bits(const std::vector<double> symbols, std::vector<int> &bits)
{
const int traceback_depth = 5*d_KK;
int nbits_requested = symbols.size()/d_symbols_per_bit;
int nbits_decoded;
// fill two vectors with the two possible symbol alignments
std::vector<double> symbols_vd1(symbols); // aligned symbol vector -> copy input symbol vector
std::vector<double> symbols_vd2; // shifted symbol vector -> add past sample in front of input vector
@ -327,15 +326,12 @@ get_bits(const std::vector<double> symbols, std::vector<int> &bits)
{
symbols_vd2.push_back(*symbol_it);
}
// arrays for decoded bits
int * bits_vd1 = new int[nbits_requested];
int * bits_vd2 = new int[nbits_requested];
// decode
float metric_vd1 = d_vd1->decode_continuous(symbols_vd1.data(), traceback_depth, bits_vd1, nbits_requested, nbits_decoded);
float metric_vd2 = d_vd2->decode_continuous(symbols_vd2.data(), traceback_depth, bits_vd2, nbits_requested, nbits_decoded);
// choose the bits with the better metric
for (int i = 0; i<nbits_decoded; i++)
{
@ -348,36 +344,28 @@ get_bits(const std::vector<double> symbols, std::vector<int> &bits)
bits.push_back(bits_vd2[i]);
}
}
d_past_symbol = symbols.back();
delete[] bits_vd1;
delete[] bits_vd2;
return metric_vd1 > metric_vd2;
}
// ### helper class for detecting the preamble and collect the corresponding message candidates ###
void sbas_l1_telemetry_decoder_cc::frame_detector::reset()
{
d_buffer.clear();
}
void sbas_l1_telemetry_decoder_cc::frame_detector::
get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std::vector<int>>> &msg_candidates)
void sbas_l1_telemetry_decoder_cc::frame_detector::get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std::vector<int>>> &msg_candidates)
{
std::stringstream ss;
unsigned int sbas_msg_length = 250;
std::vector<std::vector<int>> preambles = {{0, 1, 0, 1, 0, 0, 1 ,1},
{1, 0, 0, 1, 1, 0, 1, 0},
{1, 1, 0, 0, 0, 1, 1, 0}};
{1, 0, 0, 1, 1, 0, 1, 0},
{1, 1, 0, 0, 0, 1, 1, 0}};
VLOG(FLOW) << "get_frame_candidates(): " << "d_buffer.size()=" << d_buffer.size() << "\tbits.size()=" << bits.size();
ss << "copy bits ";
int count = 0;
// copy new bits into the working buffer
@ -388,7 +376,6 @@ get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std:
count++;
}
VLOG(SAMP_SYNC) << ss.str() << " into working buffer (" << count << " bits)";
int relative_preamble_start = 0;
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
for (std::vector<int>::iterator preample_bit_it = preample_it->begin(); preample_bit_it < preample_it->end(); ++preample_bit_it)
{
preamble_detected = *preample_bit_it == d_buffer[preample_bit_it-preample_it->begin()] ? preamble_detected : false ;
inv_preamble_detected = *preample_bit_it != d_buffer[preample_bit_it-preample_it->begin()] ? inv_preamble_detected : false ;
preamble_detected = *preample_bit_it == d_buffer[preample_bit_it - preample_it->begin()] ? preamble_detected : false ;
inv_preamble_detected = *preample_bit_it != d_buffer[preample_bit_it - preample_it->begin()] ? inv_preamble_detected : false ;
}
if (preamble_detected || inv_preamble_detected)
{
// copy candidate
std::vector<int> candidate;
std::copy(d_buffer.begin(), d_buffer.begin()+sbas_msg_length, std::back_inserter(candidate));
std::copy(d_buffer.begin(), d_buffer.begin() + sbas_msg_length, std::back_inserter(candidate));
if(inv_preamble_detected)
{
// invert bits
for (std::vector<int>::iterator candidate_bit_it = candidate.begin(); candidate_bit_it != candidate.end(); candidate_bit_it++)
*candidate_bit_it = *candidate_bit_it == 0 ? 1:0;
}
msg_candidates.push_back(std::pair<int,std::vector<int>>(relative_preamble_start,candidate));
msg_candidates.push_back(std::pair<int,std::vector<int>>(relative_preamble_start, candidate));
ss.str("");
ss << "preamble " << preample_it - preambles.begin() << (inv_preamble_detected?" inverted":" normal") << " detected! candidate=";
for (std::vector<int>::iterator bit_it = candidate.begin(); bit_it < candidate.end(); ++bit_it)
@ -424,10 +410,8 @@ get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std:
}
}
relative_preamble_start++;
// remove bit in 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::
get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs)
void sbas_l1_telemetry_decoder_cc::crc_verifier::get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vector<msg_candiate_char_t> &valid_msgs)
{
std::stringstream ss;
VLOG(FLOW) << "get_valid_frames(): " << "msg_candidates.size()=" << msg_candidates.size();
// for each candidate
for (std::vector<msg_candiate_int_t>::const_iterator candidate_it = msg_candidates.begin(); candidate_it < msg_candidates.end(); ++candidate_it)
{
// convert to bytes
std::vector<unsigned char> candidate_bytes;
zerropad_back_and_convert_to_bytes(candidate_it->second, candidate_bytes);
// verify CRC
d_checksum_agent.reset(0);
d_checksum_agent.process_bytes(candidate_bytes.data(), candidate_bytes.size());
unsigned int crc = d_checksum_agent.checksum();
VLOG(SAMP_SYNC) << "candidate " << candidate_it - msg_candidates.begin() << ": final crc remainder= " << std::hex << crc
<< std::setfill(' ') << std::resetiosflags(std::ios::hex);
// the final remainder must be zero for a valid message, because the CRC is done over the received CRC value
if (crc == 0)
{
@ -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;
const size_t bits_per_byte = 8;
unsigned char byte = 0;
VLOG(LMORE) << "zerropad_back_and_convert_to_bytes():" << byte;
for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
{
int idx_bit = candidate_bit_it - msg_candidate.begin();
int bit_pos_in_current_byte = (bits_per_byte-1)-(idx_bit % bits_per_byte);
int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte-1)
if (idx_bit % bits_per_byte == bits_per_byte - 1)
{
bytes.push_back(byte);
VLOG(LMORE) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
@ -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);
}
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;
const size_t bits_per_byte = 8;
unsigned char byte = 0;
int idx_bit = 6; // insert 6 zeros at the front to fit the 250bits into a multiple of bytes
VLOG(LMORE) << "zerropad_front_and_convert_to_bytes():" << byte;
for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
{
int bit_pos_in_current_byte = (bits_per_byte-1)-(idx_bit % bits_per_byte);
int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
ss << *candidate_bit_it;
if (idx_bit % bits_per_byte == bits_per_byte-1)
if (idx_bit % bits_per_byte == bits_per_byte - 1)
{
bytes.push_back(byte);
VLOG(LMORE) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
byte = 0;
}
idx_bit++;
}
VLOG(LMORE) << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte
<< std::setfill(' ') << std::resetiosflags(std::ios::hex);
}
void sbas_l1_telemetry_decoder_cc::set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue)
{
sbas_telemetry_data.set_raw_msg_queue(raw_msg_queue);
}
void sbas_l1_telemetry_decoder_cc::set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue)
{
sbas_telemetry_data.set_iono_queue(iono_queue);
}
void sbas_l1_telemetry_decoder_cc::set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue)
{
sbas_telemetry_data.set_sat_corr_queue(sat_corr_queue);
}
void sbas_l1_telemetry_decoder_cc::set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue)
{
sbas_telemetry_data.set_ephemeris_queue(ephemeris_queue);

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

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

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

@ -1,6 +1,6 @@
/*
* \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
*
@ -33,7 +33,6 @@
#include <stdio.h>
#include <math.h>
#include <iostream>
#include "sbas_ephemeris.h"
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
* \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
*
@ -33,23 +33,26 @@
#ifndef GNSS_SDR_SBAS_EPHEMERIS_H_
#define GNSS_SDR_SBAS_EPHEMERIS_H_
/*!
* \brief This class stores SBAS SV ephemeris data
*
*/
class Sbas_Ephemeris
{
public:
void print(std::ostream &out);
int i_prn; // PRN number */
//gtime_t t0; // reference epoch time (GPST)
int i_prn; //!< PRN number
//gtime_t t0; //!< reference epoch time (GPST)
int i_t0;
//gtime_t tof; // time of message frame (GPST)
double d_tof;
int i_sv_ura; // SV accuracy (URA index), not standardized
bool b_sv_do_not_use; // health status (false:don't use / true:usable)
double d_pos[3]; // satellite position (m) (ecef)
double d_vel[3]; // satellite velocity (m/s) (ecef)
double d_acc[3]; // satellite acceleration (m/s^2) (ecef)
double d_af0; // satellite clock-offset (s)
double d_af1; // satellite drift (s/s)
int i_sv_ura; //!< SV accuracy (URA index), not standardized
bool b_sv_do_not_use; //!< health status (false:do not use / true:usable)
double d_pos[3]; //!< satellite position (m) (ECEF)
double d_vel[3]; //!< satellite velocity (m/s) (ECEF)
double d_acc[3]; //!< satellite acceleration (m/s^2) (ECEF)
double d_af0; //!< satellite clock-offset (s)
double d_af1; //!< satellite drift (s/s)
};

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 ----------------------------------------------------------
* 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)
{
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;
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 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)
* double *E O ecef to local coord transformation matrix (3x3)
* 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)
{
double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
E[0] = -sinl; E[3] = cosl; E[6] = 0.0;
E[1] = -sinp*cosl; E[4] = -sinp*sinl; E[7] = cosp;
E[2] = cosp*cosl; E[5] = cosp*sinl; E[8] = sinp;
}
/* transform ecef vector to local tangental coordinate -------------------------
* transform ecef vector to local tangental coordinate
/* transform ecef vector to local tangential coordinate -------------------------
* transform ecef vector to local tangential 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}
@ -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)
{
double E[9];
xyz2enu(pos, 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 -------------------------------------------
* compute satellite azimuth/elevation angle
* 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 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)
{
ecef2enu(pos,e,enu);
az = dot(enu,enu,2) < 1E-12?0.0:atan2(enu[0], enu[1]);
if (az<0.0) az += 2*PI;
ecef2enu(pos, e, enu);
az = dot(enu ,enu, 2) < 1E-12 ? 0.0:atan2(enu[0], enu[1]);
if (az < 0.0) az += 2*PI;
el = asin(enu[2]);
}
if (azel)
{
azel[0]=az;
azel[1]=el;
azel[0] = az;
azel[1] = 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 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]);
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]);
posp[0] = asin(sin(pos[0])*cos(ap) + cos(pos[0])*sinap*cosaz);
if ((pos[0] > 70.0*D2R && tanap*cosaz > tan(PI/2.0-pos[0]))||
(pos[0] < -70.0*D2R && - tanap*cosaz > tan(PI/2.0+pos[0])))
if ((pos[0] > 70.0*D2R && tanap*cosaz > tan(PI/2.0 - pos[0])) ||
(pos[0] < -70.0*D2R && - tanap*cosaz > tan(PI/2.0 + pos[0])))
{
posp[1] = pos[1] + PI - asin(sinap*sin(azel[0])/cos(posp[0]));
}
@ -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]));
}
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,
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 latp[2];
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 lon = pos[1]*R2D;
@ -381,6 +395,8 @@ int Sbas_Ionosphere_Correction::sbsioncorr(const double sample_stamp, const doub
double t;
double w[4] = {0};
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);

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

@ -63,6 +63,9 @@ private:
ar & d_delay;
}
};
struct Igp_Band
{
//int d_iodi;
@ -82,118 +85,135 @@ private:
class Sbas_Ionosphere_Correction
{
private:
// /* type definitions ----------------------------------------------------------*/
//#define MAXBAND 10 /* max SBAS band of IGP */
//#define MAXNIGP 201 /* max number of IGP in SBAS band */
//
// typedef struct { /* time struct */
// time_t time; /* time (s) expressed by standard time_t */
// double sec; /* fraction of second under 1 s */
// } gtime_t;
//
// typedef struct { /* SBAS ionospheric correction type */
// gtime_t t0; /* correction time */
// short lat,lon; /* latitude/longitude (deg) */
// short give; /* GIVI+1 */
// float delay; /* vertical delay estimate (m) */
// } sbsigp_t;
//
// typedef struct { /* SBAS ionospheric corrections type */
// int iodi; /* IODI (issue of date ionos corr) */
// int nigp; /* number of igps */
// sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
// } sbsion_t;
// /* type definitions ----------------------------------------------------------*/
//#define MAXBAND 10 /* max SBAS band of IGP */
//#define MAXNIGP 201 /* max number of IGP in SBAS band */
//
// typedef struct { /* time struct */
// time_t time; /* time (s) expressed by standard time_t */
// double sec; /* fraction of second under 1 s */
// } gtime_t;
//
// typedef struct { /* SBAS ionospheric correction type */
// gtime_t t0; /* correction time */
// short lat,lon; /* latitude/longitude (deg) */
// short give; /* GIVI+1 */
// float delay; /* vertical delay estimate (m) */
// } sbsigp_t;
//
// typedef struct { /* SBAS ionospheric corrections type */
// int iodi; /* IODI (issue of date ionos corr) */
// int nigp; /* number of igps */
// sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
// } sbsion_t;
/* inner product ---------------------------------------------------------------
* inner product of vectors
* args : double *a,*b I vector a,b (n x 1)
* int n I size of vector a,b
* return : a'*b
*-----------------------------------------------------------------------------*/
double dot(const double *a, const double *b, int n);
/* multiply matrix -----------------------------------------------------------*/
void matmul(const char *tr, int n, int k, int m, double alpha,
const double *A, const double *B, double beta, double *C);
/* ecef to local coordinate transfromation matrix ------------------------------
* compute ecef to local coordinate transfromation matrix
* args : double *pos I geodetic position {lat,lon} (rad)
* double *E O ecef to local coord transformation matrix (3x3)
* return : none
* notes : matirix stored by column-major order (fortran convention)
*-----------------------------------------------------------------------------*/
void xyz2enu(const double *pos, double *E);
/* transform ecef vector to local tangental coordinate -------------------------
* transform ecef vector to local tangental coordinate
* args : double *pos I geodetic position {lat,lon} (rad)
* double *r I vector in ecef coordinate {x,y,z}
* double *e O vector in local tangental coordinate {e,n,u}
* return : none
*-----------------------------------------------------------------------------*/
void ecef2enu(const double *pos, const double *r, double *e);
/* satellite azimuth/elevation angle -------------------------------------------
* compute satellite azimuth/elevation angle
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
* double *e I receiver-to-satellilte unit vevtor (ecef)
* double *azel IO azimuth/elevation {az,el} (rad) (NULL: no output)
* (0.0<=azel[0]<2*pi,-pi/2<=azel[1]<=pi/2)
* return : elevation angle (rad)
*-----------------------------------------------------------------------------*/
double satazel(const double *pos, const double *e, double *azel);
/* debug trace functions -----------------------------------------------------*/
/*!
* \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, ...);
/* time difference -------------------------------------------------------------
* difference between gtime_t structs
* args : gtime_t t1,t2 I gtime_t structs
* return : time difference (t1-t2) (s)
*-----------------------------------------------------------------------------*/
* difference between gtime_t structs
* args : gtime_t t1,t2 I gtime_t structs
* return : time difference (t1-t2) (s)
*-----------------------------------------------------------------------------*/
//double timediff(gtime_t t1, gtime_t t2);
/* ionospheric pierce point position -------------------------------------------
* compute ionospheric pierce point (ipp) position and slant factor
* args : double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* double re I earth radius (km)
* double hion I altitude of ionosphere (km)
* double *posp O pierce point position {lat,lon,h} (rad,m)
* return : slant factor
* notes : see ref [2], only valid on the earth surface
* fixing bug on ref [2] A.4.4.10.1 A-22,23
*-----------------------------------------------------------------------------*/
/*!
* \brief Compute ionospheric pierce point (ipp) position and slant factor
* params : double *pos I receiver position {lat,lon,h} (rad,m)
* double *azel I azimuth/elevation angle {az,el} (rad)
* double re I earth radius (km)
* double hion I altitude of ionosphere (km)
* double *posp O pierce point position {lat,lon,h} (rad,m)
* return : slant factor
* notes : see ref [2], only valid on the earth surface
* fixing bug on ref [2] A.4.4.10.1 A-22,23
*-----------------------------------------------------------------------------*/
double ionppp(const double *pos, const double *azel, double re,
double hion, double *posp);
/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/
double hion, double *posp);
/*!
* \brief Variance of ionosphere correction (give=GIVEI+1)
*/
double varicorr(int give);
/* search igps ---------------------------------------------------------------*/
/*!
* \brief Search igps
*/
void searchigp(const double *pos, const Igp **igp, double *x, double *y);
/* sbas ionospheric delay correction -------------------------------------------
* compute sbas ionosphric delay correction
* args : long sample_stamp I sample stamp of observable on which the correction will be applied
* sbsion_t *ion I ionospheric correction data (implicit)
* double *pos I receiver position {lat,lon,height} (rad/m)
* double *azel I satellite azimuth/elavation angle (rad)
* double *delay O slant ionospheric delay (L1) (m)
* double *var O variance of ionospheric delay (m^2)
* return : status (1:ok, 0:no correction)
* notes : before calling the function, sbas ionosphere correction parameters
* in navigation data (nav->sbsion) must be set by callig
* sbsupdatecorr()
*-----------------------------------------------------------------------------*/
/*!
* \brief Compute sbas ionosphric delay correction
* params : long sample_stamp I sample stamp of observable on which the correction will be applied
* sbsion_t *ion I ionospheric correction data (implicit)
* double *pos I receiver position {lat,lon,height} (rad/m)
* double *azel I satellite azimuth/elavation angle (rad)
* double *delay O slant ionospheric delay (L1) (m)
* double *var O variance of ionospheric delay (m^2)
* return : status (1:ok, 0:no correction)
* notes : before calling the function, sbas ionosphere correction parameters
* in navigation data (nav->sbsion) must be set by callig
* sbsupdatecorr()
*/
int sbsioncorr(const double sample_stamp, const double *pos,
const double *azel, double *delay, double *var);
public:
std::vector<Igp_Band> d_bands;
void print(std::ostream &out);
bool apply(double sample_stamp, double latitude_d, double longitude_d,
double azimut_d, double evaluation_d, double &delay, double &var);
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive& ar, const unsigned int version){ar & d_bands;}
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 <glog/log_severity.h>
#include <glog/logging.h>
#include "sbas_satellite_correction.h"
#define EVENT 2 // logs important events which don't occur every update() call
#define FLOW 3 // logs the function calls of block processing functions
void
Sbas_Satellite_Correction::print(std::ostream &out)
#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) */
void Sbas_Satellite_Correction::print(std::ostream &out)
{
out << "<<S>> Sbas satellite corrections for PRN" << d_prn << ":" << std::endl;
print_fast_correction(out);
print_long_term_correction(out);
}
void
Sbas_Satellite_Correction::print_fast_correction(std::ostream &out)
void Sbas_Satellite_Correction::print_fast_correction(std::ostream &out)
{
Fast_Correction fcorr = d_fast_correction;
out << "<<S>> Fast PRN" << d_prn << ":";
@ -73,8 +76,7 @@ Sbas_Satellite_Correction::print_fast_correction(std::ostream &out)
}
void
Sbas_Satellite_Correction::print_long_term_correction(std::ostream &out)
void Sbas_Satellite_Correction::print_long_term_correction(std::ostream &out)
{
Long_Term_Correction lcorr = d_long_term_correction;
out << "<<S>> Long PRN" << d_prn << ":";
@ -94,178 +96,163 @@ int Sbas_Satellite_Correction::apply_fast(double sample_stamp, double &pr, doubl
{
int result;
double prc = 0; // pseudo range correction
result = sbsfastcorr(sample_stamp, &prc, &var);
pr += prc;
VLOG(FLOW) << "<<S>> fast correction applied: sample_stamp=" << sample_stamp << " prc=" << prc << " corr. pr=" << pr;
return result;
}
int Sbas_Satellite_Correction::apply_long_term_sv_pos(double sample_stamp, double sv_pos[], double &var)
{
int result;
double drs[3] = {0};
double ddts = 0;
result = sbslongcorr(sample_stamp, drs, &ddts);
for (int i = 0; i < 3; i++) sv_pos[i] += drs[i];
VLOG(FLOW) << "<<S>> long term sv pos correction applied: sample_stamp=" << sample_stamp << " drs=(x=" << drs[0] << " y=" << drs[1] << " z=" << drs[2] << ")";
return result;
}
int Sbas_Satellite_Correction::apply_long_term_sv_clk(double sample_stamp, double &dts, double &var)
{
int result;
double drs[3] = {0};
double ddts = 0;
result = sbslongcorr(sample_stamp, drs, &ddts);
dts += ddts;
VLOG(FLOW) << "<<S>> long term sv clock correction correction applied: sample_stamp=" << sample_stamp << " ddts=" << ddts;
return result;
}
bool Sbas_Satellite_Correction::alarm()
{
return this->d_fast_correction.d_udre == 16;
return this->d_fast_correction.d_udre == 16;
}
#define CLIGHT 299792458.0 /* speed of light (m/s) */
#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */
/* debug trace function -----------------------------------------------------*/
void Sbas_Satellite_Correction::trace(int level, const char *format, ...)
{
va_list ap;
char str[1000];
va_start(ap,format);
vsprintf(str,format,ap);
va_end(ap);
VLOG(FLOW) << "<<S>> " << std::string(str);
}
/* variance of fast correction (udre=UDRE+1) ---------------------------------*/
double Sbas_Satellite_Correction::varfcorr(int udre)
{
const double var[14] = {
0.052,0.0924,0.1444,0.283,0.4678,0.8315,1.2992,1.8709,2.5465,3.326,
5.1968,20.7870,230.9661,2078.695
0.052, 0.0924, 0.1444, 0.283, 0.4678, 0.8315, 1.2992, 1.8709, 2.5465, 3.326,
5.1968, 20.7870, 230.9661, 2078.695
};
return 0 < udre && udre <= 14 ? var[udre-1]:0.0;
return 0 < udre && udre <= 14 ? var[udre - 1] : 0.0;
}
/* fast correction degradation -----------------------------------------------*/
double Sbas_Satellite_Correction::degfcorr(int ai)
{
const double degf[16] = {
0.00000,0.00005,0.00009,0.00012,0.00015,0.00020,0.00030,0.00045,
0.00060,0.00090,0.00150,0.00210,0.00270,0.00330,0.00460,0.00580
0.00000, 0.00005, 0.00009, 0.00012, 0.00015, 0.00020, 0.00030, 0.00045,
0.00060, 0.00090, 0.00150, 0.00210, 0.00270, 0.00330, 0.00460, 0.00580
};
return 0 < ai && ai <= 15 ? degf[ai]:0.0058;
return 0 < ai && ai <= 15 ? degf[ai] : 0.0058;
}
/* long term correction ------------------------------------------------------*/
int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, double *ddts)
{
double t = 0.0;
int i;
Long_Term_Correction lcorr = d_long_term_correction;
trace(3, "sbslongcorr: prn=%2d", this->d_prn);
// if (p->sat!=sat||p->lcorr.t0.time==0) continue;
// if (p->sat!=sat||p->lcorr.t0.time==0) continue;
// compute time of applicability
if(d_long_term_correction.i_vel == 1)
{ // time of applicability is the one sent, i.e., tapp
// TODO: adapt for vel==1 case
// t = tow-d_long_term_correction.i_tapp;
// vel=1 -> time of applicability is sent in message, needs to be corrected for rollover which can not be done here, since the absolute gps time is unknown. see IS-GPS-200G pdf page 116 for correction
/* t = (int)getbitu(msg->msg, p + 90, 13)*16 - (int)msg->tow%86400;
if (t <= -43200) t += 86400;
else if (t > 43200) t -= 86400;
sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/
}
{
// time of applicability is the one sent, i.e., tapp
// TODO: adapt for vel==1 case
// t = tow-d_long_term_correction.i_tapp;
// vel=1 -> time of applicability is sent in message, needs to be corrected for rollover which can not be done here, since the absolute gps time is unknown. see IS-GPS-200G pdf page 116 for correction
/* t = (int)getbitu(msg->msg, p + 90, 13)*16 - (int)msg->tow%86400;
if (t <= -43200) t += 86400;
else if (t > 43200) t -= 86400;
sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/
}
else
{ // time of applicability is time of reception
t = time_stamp - lcorr.d_trx; // should not have any impact if vel==0 since d_dvel and d_daf1 are zero, is only used to check outdating
}
//t=time_stamp-lcorr.d_t0;
if (fabs(t) > MAXSBSAGEL)
{
trace(2,"sbas long-term correction expired: sat=%2d time_stamp=%5.0f", d_prn, time_stamp);
return 0;
}
// sv position correction
for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*t;
// sv clock correction correction
*ddts = lcorr.d_daf0 + lcorr.d_daf1*t;
trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f", d_prn, drs[0], drs[1], drs[2], *ddts*CLIGHT);
return 1;
{
// time of applicability is time of reception
t = time_stamp - lcorr.d_trx; // should not have any impact if vel==0 since d_dvel and d_daf1 are zero, is only used to check outdating
}
//t=time_stamp-lcorr.d_t0;
if (fabs(t) > MAXSBSAGEL)
{
trace(2,"sbas long-term correction expired: sat=%2d time_stamp=%5.0f", d_prn, time_stamp);
return 0;
}
// sv position correction
for (i=0; i<3; i++) drs[i] = lcorr.d_dpos[i] + lcorr.d_dvel[i]*t;
// sv clock correction correction
*ddts = lcorr.d_daf0 + lcorr.d_daf1*t;
trace(5, "sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f", d_prn, drs[0], drs[1], drs[2], *ddts * CLIGHT);
return 1;
/* if sbas satellite without correction, no correction applied */
//if (satsys(sat,NULL)==SYS_SBS) return 1;
//trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
//return 0;
}
/* fast correction -----------------------------------------------------------*/
int Sbas_Satellite_Correction::sbsfastcorr(double time_stamp, double *prc, double *var)
#define RRCENA
{
double t;
Fast_Correction fcorr = d_fast_correction;
trace(3, "sbsfastcorr: sat=%2d", this->d_prn);
//if (p->fcorr.t0.time==0) break; // time==0is only true if t0 hasn't been initialised -> it checks if the correction is valid
t = (time_stamp - fcorr.d_tof.get_time_stamp()) + fcorr.d_tlat; // delta t between now and tof
/* expire age of correction? */
if (fabs(t) > MAXSBSAGEF)
{
trace(2, "no sbas fast correction (expired): time_stamp=%f prn=%2d", time_stamp, d_prn);
return 0;
}
/* UDRE==14 (not monitored)? */
else if(fcorr.d_udre == 15)
{
trace(2,"no sbas fast correction (not monitored): time_stamp=%f prn=%2d", time_stamp, d_prn);
return 0;
}
else if(fcorr.d_udre == 16)
{
trace(2,"SV is marked as unhealthy: time_stamp=%f prn=%2d", time_stamp, d_prn);
return 0;
}
*prc = fcorr.d_prc;
//if (p->fcorr.t0.time==0) break; // time==0is only true if t0 hasn't been initialised -> it checks if the correction is valid
t = (time_stamp - fcorr.d_tof.get_time_stamp()) + fcorr.d_tlat; // delta t between now and tof
/* expire age of correction? */
if (fabs(t) > MAXSBSAGEF)
{
trace(2, "no sbas fast correction (expired): time_stamp=%f prn=%2d", time_stamp, d_prn);
return 0;
}
/* UDRE==14 (not monitored)? */
else if(fcorr.d_udre == 15)
{
trace(2,"no sbas fast correction (not monitored): time_stamp=%f prn=%2d", time_stamp, d_prn);
return 0;
}
else if(fcorr.d_udre == 16)
{
trace(2,"SV is marked as unhealthy: time_stamp=%f prn=%2d", time_stamp, d_prn);
return 0;
}
*prc = fcorr.d_prc;
#ifdef RRCENA
if (fcorr.d_ai > 0 && fabs(t) <= 8.0*fcorr.d_dt)
{
*prc += fcorr.d_rrc*t;
}
if (fcorr.d_ai > 0 && fabs(t) <= 8.0*fcorr.d_dt)
{
*prc += fcorr.d_rrc*t;
}
#endif
*var = varfcorr(fcorr.d_udre) + degfcorr(fcorr.d_ai)*t*t/2.0;
trace(5, "sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f", d_prn, *prc, sqrt(*var), t);
return 1;
*var = varfcorr(fcorr.d_udre) + degfcorr(fcorr.d_ai) * t * t / 2.0;
trace(5, "sbsfastcorr: sat=%3d prc=%7.2f sig=%7.2f t=%5.0f", d_prn, *prc, sqrt(*var), t);
return 1;
}
@ -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;
int i;
trace(3,"sbssatcorr : sat=%2d",d_prn);
/* sbas long term corrections */
if (!sbslongcorr(time_stamp, drs, &dclk))
{
@ -303,12 +288,9 @@ int Sbas_Satellite_Correction::sbssatcorr(double time_stamp, double *rs, double
return 0;
}
for (i=0; i<3; i++) rs[i] += drs[i];
dts[0] += dclk + prc/CLIGHT;
trace(5,"sbssatcorr: sat=%2d drs=%6.3f %6.3f %6.3f dclk=%.3f %.3f var=%.3f",
d_prn,drs[0],drs[1],drs[2],dclk,prc/CLIGHT,*var);
return 1;
}

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

@ -47,7 +47,7 @@
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 */
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;
int satidx = msg.get_prn()-MINPRNSBS;
int satidx = msg.get_prn() - MINPRNSBS;
seph_t seph_rtklib = d_nav.seph[satidx];
// copy data
@ -237,8 +237,9 @@ void Sbas_Telemetry_Data::received_iono_correction()
if(iono_queue != NULL) iono_queue->push(iono_corr);
}
// 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>
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);
s2_ = (Struct*) calloc (1, struct_size);
/* is_equal = !memcmp(s1_, s2_, sizeof(Struct));
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
// use assignment constructor which doesn't copy paddings
*s1_ = s1;
*s2_ = s2;
// compare struct memory byte-wise
//is_equal = !memcmp(s1_, s2_, sizeof(Struct));
is_equal_manual = true;
ss.str();
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 << std::endl;
}
//VLOG(DETAIL) << ss.str();
free(s1_);
free(s2_);
@ -307,7 +287,7 @@ inline bool are_equal(const Struct &s1, const Struct &s2)
void Sbas_Telemetry_Data::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++)
{
std::stringstream ss;
@ -404,7 +384,6 @@ void Sbas_Telemetry_Data::trace(int level, const char *format, ...)
{
va_list ap;
char str[1000];
va_start(ap, format);
vsprintf(str, format, ap);
va_end(ap);
@ -433,7 +412,7 @@ int Sbas_Telemetry_Data::satno(int sys, int prn)
return NSATGPS + NSATGLO + prn - MINPRNGAL + 1;
case SYS_QZS:
if (prn < MINPRNQZS || MAXPRNQZS < prn) return 0;
return NSATGPS+NSATGLO+NSATGAL+prn-MINPRNQZS+1;
return NSATGPS + NSATGLO + NSATGAL + prn - MINPRNQZS + 1;
case SYS_CMP:
if (prn < MINPRNCMP || MAXPRNCMP < prn) return 0;
return NSATGPS + NSATGLO + NSATGAL + NSATQZS + prn - MINPRNCMP + 1;
@ -443,13 +422,14 @@ int Sbas_Telemetry_Data::satno(int sys, int prn)
}
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 len I bit length (bits) (len<=32)
* return : extracted unsigned/signed bits
*-----------------------------------------------------------------------------*/
*/
unsigned int Sbas_Telemetry_Data::getbitu(const unsigned char *buff, int pos, int len)
{
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};
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 */
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]);
time.time = (time_t)days*86400 + (int)ep[3]*3600 + (int)ep[4]*60 + 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 --------------------------------------------------*/
int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
{
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
int i, n, sat;
// 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:");
@ -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 (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 <= 119) sat = 0; /* 62-119: future gnss */
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 <= 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 sat = 0; /* 203- : reserved */
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 <= 182) sat = 0; /* 139-182: reserved */
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 sat = 0; /* 203- : reserved */
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);
return 1;
}
/* decode type 2-5,0: fast corrections ---------------------------------------*/
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:");
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);
iodf = getbitu(msg->msg, 14, 2);
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);
t0_past = sbssat->sat[j].fcorr.t0;
prc = sbssat->sat[j].fcorr.prc;
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.udre = udrei+1;
sbssat->sat[j].fcorr.udre = udrei + 1;
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)
{
@ -639,7 +621,7 @@ int Sbas_Telemetry_Data::decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat)
}
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.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;
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]);
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 i;
trace(4,"decode_sbstype7");
if (sbssat->iodp!=(int)getbitu(msg->msg,18,2)) return 0;
sbssat->tlat = getbitu(msg->msg,14,4);
if (sbssat->iodp != (int)getbitu(msg->msg, 18, 2)) return 0;
sbssat->tlat = getbitu(msg->msg, 14, 4);
for (i=0; i < sbssat->nsat && i < MAXSAT; i++)
{
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 -------------------------------------*/
int Sbas_Telemetry_Data::decode_sbstype9(const sbsmsg_t *msg, nav_t *nav)
{
seph_t seph={0};
seph_t seph = {0};
int i;
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.tof = msg->sample_stamp;
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[1] = getbits(msg->msg, 69,30)*0.08;
seph.pos[2] = getbits(msg->msg, 99,25)*0.4;
seph.vel[0] = getbits(msg->msg,124,17)*0.000625;
seph.vel[1] = getbits(msg->msg,141,17)*0.000625;
seph.vel[2] = getbits(msg->msg,158,18)*0.004;
seph.acc[0] = getbits(msg->msg,176,10)*0.0000125;
seph.acc[1] = getbits(msg->msg,186,10)*0.0000125;
seph.acc[2] = getbits(msg->msg,196,10)*0.0000625;
seph.pos[0] = getbits(msg->msg, 39, 30)*0.08;
seph.pos[1] = getbits(msg->msg, 69, 30)*0.08;
seph.pos[2] = getbits(msg->msg, 99, 25)*0.4;
seph.vel[0] = getbits(msg->msg, 124, 17)*0.000625;
seph.vel[1] = getbits(msg->msg, 141, 17)*0.000625;
seph.vel[2] = getbits(msg->msg, 158, 18)*0.004;
seph.acc[0] = getbits(msg->msg, 176, 10)*0.0000125;
seph.acc[1] = getbits(msg->msg, 186, 10)*0.0000125;
seph.acc[2] = getbits(msg->msg, 196, 10)*0.0000625;
seph.af0 = getbits(msg->msg, 206, 12)*P2_31;
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";
return 0;
}
nav->seph[NSATSBS+i] = nav->seph[i]; /* previous */
nav->seph[i] = seph; /* current */
nav->seph[NSATSBS + i] = nav->seph[i]; /* previous */
nav->seph[i] = seph; /* current */
trace(5, "decode_sbstype9: prn=%d", msg->prn);
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 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)
{// IGP mask changed -> invalidate all IGPs in this band
{
// IGP mask changed -> invalidate all IGPs in this band
igp_mask_changed(band);
}
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;
for (j=0; j<m; j++)
{
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++].lon = band <= 8?p[j].x:p[j].y[i-p[j].bits];
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++].lon = band <= 8 ? p[j].x : p[j].y[i - p[j].bits];
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) -----------------------------*/
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:");
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.dvel[i] = 0.0;
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.daf0 = getbits(msg->msg,p+41,10)*P2_31;
sbssat->sat[n-1].lcorr.daf1 = 0.0;
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.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.tapp = 0;
sbssat->sat[n-1].lcorr.valid = true;
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.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;
}
@ -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;
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++)
{
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.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.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.daf0 = getbits(msg->msg, p + 47, 11)*P2_31;
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
/*t=(int)getbitu(msg->msg,p+90,13)*16-(int)msg->tow%86400;
if (t<=-43200) t+=86400;
else if (t> 43200) t-=86400;
sbssat->sat[n-1].lcorr.t0=gpst2time(msg->week,msg->tow+t);*/
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.vel = 1;
sbssat->sat[n-1].lcorr.valid = true;
trace(5, "decode_longcorr1: sat=%2d", sbssat->sat[n-1].sat);
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.vel = 1;
sbssat->sat[n - 1].lcorr.valid = true;
trace(5, "decode_longcorr1: sat=%2d", sbssat->sat[n - 1].sat);
return 1;
}
@ -858,15 +837,16 @@ int Sbas_Telemetry_Data::decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *
trace(4,"decode_longcorrh:");
if (getbitu(msg->msg, p, 1) == 0 )
{ /* vel code=0 */
{
/* vel code=0 */
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))
{
return decode_longcorr1(msg, p+1, sbssat);
return decode_longcorr1(msg, p + 1, sbssat);
}
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 = msg->sample_stamp;
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;
}
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:");
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++)
{
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);
delay = getbitu(msg->msg, 22 + i*13, 9);
//sbsion[band].igp[j].t0=gpst2time(msg->week,msg->tow);
sbsion[band].igp[j].t0 = msg->sample_stamp;
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;
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 type=getbitu(msg->msg,8,6),stat=-1;
int type = getbitu(msg->msg, 8, 6), stat = -1;
trace(3,"sbsupdatecorr: type=%d",type);
@ -963,15 +943,15 @@ int Sbas_Telemetry_Data::sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav)
switch (type)
{
case 0: stat = decode_sbstype2 (msg, &nav->sbssat); break;
case 1: stat = decode_sbstype1 (msg, &nav->sbssat); break;
case 0: stat = decode_sbstype2(msg, &nav->sbssat); break;
case 1: stat = decode_sbstype1(msg, &nav->sbssat); break;
case 2:
case 3:
case 4:
case 5: stat = decode_sbstype2 (msg, &nav->sbssat); break;
case 6: stat = decode_sbstype6 (msg, &nav->sbssat); break;
case 7: stat = decode_sbstype7 (msg, &nav->sbssat); break;
case 9: stat = decode_sbstype9 (msg, nav); break;
case 5: stat = decode_sbstype2(msg, &nav->sbssat); break;
case 6: stat = decode_sbstype6(msg, &nav->sbssat); break;
case 7: stat = decode_sbstype7(msg, &nav->sbssat); break;
case 9: stat = decode_sbstype9(msg, nav); break;
case 10: 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;
@ -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 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()
{
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)
{
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;
}
void Sbas_Telemetry_Data::igp_mask_changed(int band)
{
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;
/*
* \brief represents a raw SBAS message of 250bits + 6bits padding
* (8b preamble + 6b message type + 212b data + 24b CRC + 6b zerro padding)
* \brief Represents a raw SBAS message of 250cbits + 6 bits padding
* (8b preamble + 6b message type + 212b data + 24b CRC + 6b zero padding)
*/
class Sbas_Raw_Msg
{
@ -90,425 +90,416 @@ public:
}
private:
//int d_week; /* reception time */
//int d_tow; /* reception time */
//int d_week; /* reception time */
//int d_tow; /* reception time */
Sbas_Time rx_time;
int i_prn; /* SBAS satellite PRN number */
std::vector<unsigned char> d_msg; /* SBAS message (226bit) padded by 0 */
int i_prn; /* SBAS satellite PRN number */
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
{
public:
int
update(Sbas_Raw_Msg sbas_raw_msg);
void
set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue);
void
set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue);
void
set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue);
void
set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue);
int update(Sbas_Raw_Msg sbas_raw_msg);
void set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue);
void set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue);
void set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue);
void set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue);
/*!
* Default constructor
*/
Sbas_Telemetry_Data();
Sbas_Telemetry_Data();
/*!
* Default deconstructor
*/
~Sbas_Telemetry_Data();
~Sbas_Telemetry_Data();
private:
std::map<int, Fast_Correction> emitted_fast_corrections;
std::map<int, Long_Term_Correction> emitted_long_term_corrections;
std::map<int, Fast_Correction> emitted_fast_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_Ionosphere_Correction> *iono_queue;
concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue;
concurrent_queue<Sbas_Ephemeris> *ephemeris_queue;
concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue;
concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue;
concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue;
concurrent_queue<Sbas_Ephemeris> *ephemeris_queue;
int decode_mt12(Sbas_Raw_Msg sbas_raw_msg);
int decode_mt12(Sbas_Raw_Msg sbas_raw_msg);
void updated_sbas_ephemeris(Sbas_Raw_Msg msg);
void received_iono_correction();
void updated_satellite_corrections();
void updated_sbas_ephemeris(Sbas_Raw_Msg msg);
void received_iono_correction();
void updated_satellite_corrections();
/////// rtklib.h
/////// rtklib.h
#define SYS_NONE 0x00 /* navigation system: none */
#define SYS_GPS 0x01 /* navigation system: GPS */
#define SYS_SBS 0x02 /* navigation system: SBAS */
#define SYS_GLO 0x04 /* navigation system: GLONASS */
#define SYS_GAL 0x08 /* navigation system: Galileo */
#define SYS_QZS 0x10 /* navigation system: QZSS */
#define SYS_CMP 0x20 /* navigation system: BeiDou */
#define SYS_ALL 0xFF /* navigation system: all */
#define SYS_NONE 0x00 /* navigation system: none */
#define SYS_GPS 0x01 /* navigation system: GPS */
#define SYS_SBS 0x02 /* navigation system: SBAS */
#define SYS_GLO 0x04 /* navigation system: GLONASS */
#define SYS_GAL 0x08 /* navigation system: Galileo */
#define SYS_QZS 0x10 /* navigation system: QZSS */
#define SYS_CMP 0x20 /* navigation system: BeiDou */
#define SYS_ALL 0xFF /* navigation system: all */
#define TSYS_GPS 0 /* time system: GPS time */
#define TSYS_UTC 1 /* time system: UTC */
#define TSYS_GLO 2 /* time system: GLONASS time */
#define TSYS_GAL 3 /* time system: Galileo time */
#define TSYS_QZS 4 /* time system: QZSS time */
#define TSYS_CMP 5 /* time system: BeiDou time */
#define TSYS_GPS 0 /* time system: GPS time */
#define TSYS_UTC 1 /* time system: UTC */
#define TSYS_GLO 2 /* time system: GLONASS time */
#define TSYS_GAL 3 /* time system: Galileo time */
#define TSYS_QZS 4 /* time system: QZSS time */
#define TSYS_CMP 5 /* time system: BeiDou time */
#ifndef NFREQ
#define NFREQ 3 /* number of carrier frequencies */
#endif
#define NFREQGLO 2 /* number of carrier frequencies of GLONASS */
#ifndef NFREQ
#define NFREQ 3 /* number of carrier frequencies */
#endif
#define NFREQGLO 2 /* number of carrier frequencies of GLONASS */
#ifndef NEXOBS
#define NEXOBS 0 /* number of extended obs codes */
#endif
#ifndef NEXOBS
#define NEXOBS 0 /* number of extended obs codes */
#endif
#define MINPRNGPS 1 /* min satellite PRN number of GPS */
#define MAXPRNGPS 32 /* max satellite PRN number of GPS */
#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */
#define NSYSGPS 1
#define MINPRNGPS 1 /* min satellite PRN number of GPS */
#define MAXPRNGPS 32 /* max satellite PRN number of GPS */
#define NSATGPS (MAXPRNGPS-MINPRNGPS+1) /* number of GPS satellites */
#define NSYSGPS 1
#ifdef ENAGLO
#define MINPRNGLO 1 /* min satellite slot number of GLONASS */
#define MAXPRNGLO 24 /* max satellite slot number of GLONASS */
#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */
#define NSYSGLO 1
#else
#define MINPRNGLO 0
#define MAXPRNGLO 0
#define NSATGLO 0
#define NSYSGLO 0
#endif
#ifdef ENAGAL
#define MINPRNGAL 1 /* min satellite PRN number of Galileo */
#define MAXPRNGAL 27 /* max satellite PRN number of Galileo */
#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */
#define NSYSGAL 1
#else
#define MINPRNGAL 0
#define MAXPRNGAL 0
#define NSATGAL 0
#define NSYSGAL 0
#endif
#ifdef ENAQZS
#define MINPRNQZS 193 /* min satellite PRN number of QZSS */
#define MAXPRNQZS 195 /* max satellite PRN number of QZSS */
#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */
#define MAXPRNQZS_S 185 /* max satellite PRN number of QZSS SAIF */
#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */
#define NSYSQZS 1
#else
#define MINPRNQZS 0
#define MAXPRNQZS 0
#define NSATQZS 0
#define NSYSQZS 0
#endif
#ifdef ENACMP
#define MINPRNCMP 1 /* min satellite sat number of BeiDou */
#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */
#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */
#define NSYSCMP 1
#else
#define MINPRNCMP 0
#define MAXPRNCMP 0
#define NSATCMP 0
#define NSYSCMP 0
#endif
#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP) /* number of systems */
#ifdef ENAGLO
#define MINPRNGLO 1 /* min satellite slot number of GLONASS */
#define MAXPRNGLO 24 /* max satellite slot number of GLONASS */
#define NSATGLO (MAXPRNGLO-MINPRNGLO+1) /* number of GLONASS satellites */
#define NSYSGLO 1
#else
#define MINPRNGLO 0
#define MAXPRNGLO 0
#define NSATGLO 0
#define NSYSGLO 0
#endif
#ifdef ENAGAL
#define MINPRNGAL 1 /* min satellite PRN number of Galileo */
#define MAXPRNGAL 27 /* max satellite PRN number of Galileo */
#define NSATGAL (MAXPRNGAL-MINPRNGAL+1) /* number of Galileo satellites */
#define NSYSGAL 1
#else
#define MINPRNGAL 0
#define MAXPRNGAL 0
#define NSATGAL 0
#define NSYSGAL 0
#endif
#ifdef ENAQZS
#define MINPRNQZS 193 /* min satellite PRN number of QZSS */
#define MAXPRNQZS 195 /* max satellite PRN number of QZSS */
#define MINPRNQZS_S 183 /* min satellite PRN number of QZSS SAIF */
#define MAXPRNQZS_S 185 /* max satellite PRN number of QZSS SAIF */
#define NSATQZS (MAXPRNQZS-MINPRNQZS+1) /* number of QZSS satellites */
#define NSYSQZS 1
#else
#define MINPRNQZS 0
#define MAXPRNQZS 0
#define NSATQZS 0
#define NSYSQZS 0
#endif
#ifdef ENACMP
#define MINPRNCMP 1 /* min satellite sat number of BeiDou */
#define MAXPRNCMP 35 /* max satellite sat number of BeiDou */
#define NSATCMP (MAXPRNCMP-MINPRNCMP+1) /* number of BeiDou satellites */
#define NSYSCMP 1
#else
#define MINPRNCMP 0
#define MAXPRNCMP 0
#define NSATCMP 0
#define NSYSCMP 0
#endif
#define NSYS (NSYSGPS+NSYSGLO+NSYSGAL+NSYSQZS+NSYSCMP) /* number of systems */
#define MINPRNSBS 120 /* min satellite PRN number of SBAS */
#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */
#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */
#define MINPRNSBS 120 /* min satellite PRN number of SBAS */
#define MAXPRNSBS 142 /* max satellite PRN number of SBAS */
#define NSATSBS (MAXPRNSBS-MINPRNSBS+1) /* number of SBAS satellites */
#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATSBS)
/* max satellite number (1 to MAXSAT) */
#ifndef MAXOBS
#define MAXOBS 64 /* max number of obs in an epoch */
#endif
#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */
#define MAXOBSTYPE 64 /* max number of obs type in RINEX */
#define DTTOL 0.005 /* tolerance of time difference (s) */
#if 0
#define MAXDTOE 10800.0 /* max time difference to ephem Toe (s) for GPS */
#else
#define MAXDTOE 7200.0 /* max time difference to ephem Toe (s) for GPS */
#endif
#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */
#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */
#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */
#define MAXGDOP 300.0 /* max GDOP */
#define MAXSAT (NSATGPS+NSATGLO+NSATGAL+NSATQZS+NSATCMP+NSATSBS)
/* max satellite number (1 to MAXSAT) */
#ifndef MAXOBS
#define MAXOBS 64 /* max number of obs in an epoch */
#endif
#define MAXRCV 64 /* max receiver number (1 to MAXRCV) */
#define MAXOBSTYPE 64 /* max number of obs type in RINEX */
#define DTTOL 0.005 /* tolerance of time difference (s) */
#if 0
#define MAXDTOE 10800.0 /* max time difference to ephem Toe (s) for GPS */
#else
#define MAXDTOE 7200.0 /* max time difference to ephem Toe (s) for GPS */
#endif
#define MAXDTOE_GLO 1800.0 /* max time difference to GLONASS Toe (s) */
#define MAXDTOE_SBS 360.0 /* max time difference to SBAS Toe (s) */
#define MAXDTOE_S 86400.0 /* max time difference to ephem toe (s) for other */
#define MAXGDOP 300.0 /* max GDOP */
//#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
//#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */
//#define MAXSBSURA 8 /* max URA of SBAS satellite */
#define MAXBAND 10 /* max SBAS band of IGP */
#define MAXNIGP 201 /* max number of IGP in SBAS band */
//#define MAXNGEO 4 /* max number of GEO satellites */
//#define MAXSBSAGEF 30.0 /* max age of SBAS fast correction (s) */
//#define MAXSBSAGEL 1800.0 /* max age of SBAS long term corr (s) */
//#define MAXSBSURA 8 /* max URA of SBAS satellite */
#define MAXBAND 10 /* max SBAS band of IGP */
#define MAXNIGP 201 /* max number of IGP in SBAS band */
//#define MAXNGEO 4 /* max number of GEO satellites */
#define P2_11 4.882812500000000E-04 /* 2^-11 */
#define P2_31 4.656612873077393E-10 /* 2^-31 */
#define P2_39 1.818989403545856E-12 /* 2^-39 */
#define P2_11 4.882812500000000E-04 /* 2^-11 */
#define P2_31 4.656612873077393E-10 /* 2^-31 */
#define P2_39 1.818989403545856E-12 /* 2^-39 */
/* type definitions ----------------------------------------------------------*/
/* type definitions ----------------------------------------------------------*/
typedef struct { /* time struct */
time_t time; /* time (s) expressed by standard time_t */
double sec; /* fraction of second under 1 s */
} gtime_t;
typedef struct { /* time struct */
time_t time; /* time (s) expressed by standard time_t */
double sec; /* fraction of second under 1 s */
} gtime_t;
typedef struct { /* SBAS message type */
//int week,tow; /* receiption time */
double sample_stamp;
int prn; /* SBAS satellite PRN number */
unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */
} sbsmsg_t;
typedef struct { /* SBAS message type */
//int week,tow; /* receiption time */
double sample_stamp;
int prn; /* SBAS satellite PRN number */
unsigned char msg[29]; /* SBAS message (226bit) padded by 0 */
} sbsmsg_t;
typedef struct { /* SBAS messages type */
int n,nmax; /* number of SBAS messages/allocated */
sbsmsg_t *msgs; /* SBAS messages */
} sbs_t;
typedef struct { /* SBAS messages type */
int n,nmax; /* number of SBAS messages/allocated */
sbsmsg_t *msgs; /* SBAS messages */
} sbs_t;
typedef struct { /* SBAS fast correction type */
//gtime_t t0; /* time of applicability (TOF) */
double t0;
bool valid;
double prc; /* pseudorange correction (PRC) (m) */
double rrc; /* range-rate correction (RRC) (m/s) */
double dt; /* range-rate correction delta-time (s) */
int iodf; /* IODF (issue of date fast corr) */
short udre; /* UDRE+1 */
short ai; /* degradation factor indicator */
} sbsfcorr_t;
typedef struct { /* SBAS fast correction type */
//gtime_t t0; /* time of applicability (TOF) */
double t0;
bool valid;
double prc; /* pseudorange correction (PRC) (m) */
double rrc; /* range-rate correction (RRC) (m/s) */
double dt; /* range-rate correction delta-time (s) */
int iodf; /* IODF (issue of date fast corr) */
short udre; /* UDRE+1 */
short ai; /* degradation factor indicator */
} sbsfcorr_t;
typedef struct { /* SBAS long term satellite error correction type */
//gtime_t t0; /* correction time */
double trx; // time when message was received
int tapp; // time of applicability (when vel=1 sent as t0)
int vel; // use velocity if vel=1
bool valid;
int iode; /* IODE (issue of date ephemeris) */
double dpos[3]; /* delta position (m) (ecef) */
double dvel[3]; /* delta velocity (m/s) (ecef) */
double daf0,daf1; /* delta clock-offset/drift (s,s/s) */
} sbslcorr_t;
typedef struct { /* SBAS long term satellite error correction type */
//gtime_t t0; /* correction time */
double trx; /* time when message was received */
int tapp; /* time of applicability (when vel=1 sent as t0) */
int vel; /* use velocity if vel=1 */
bool valid;
int iode; /* IODE (issue of date ephemeris) */
double dpos[3]; /* delta position (m) (ecef) */
double dvel[3]; /* delta velocity (m/s) (ecef) */
double daf0,daf1; /* delta clock-offset/drift (s,s/s) */
} sbslcorr_t;
typedef struct { /* SBAS satellite correction type */
int sat; /* satellite number */
sbsfcorr_t fcorr; /* fast correction */
sbslcorr_t lcorr; /* long term correction */
} sbssatp_t;
typedef struct { /* SBAS satellite correction type */
int sat; /* satellite number */
sbsfcorr_t fcorr; /* fast correction */
sbslcorr_t lcorr; /* long term correction */
} sbssatp_t;
typedef struct { /* SBAS satellite corrections type */
int iodp; /* IODP (issue of date mask) */
int nsat; /* number of satellites */
int tlat; /* system latency (s) */
sbssatp_t sat[MAXSAT]; /* satellite correction */
} sbssat_t;
typedef struct { /* SBAS satellite corrections type */
int iodp; /* IODP (issue of date mask) */
int nsat; /* number of satellites */
int tlat; /* system latency (s) */
sbssatp_t sat[MAXSAT]; /* satellite correction */
} sbssat_t;
typedef struct { /* SBAS ionospheric correction type */
//gtime_t t0; /* correction time */
double t0;
bool valid;
short lat,lon; /* latitude/longitude (deg) */
short give; /* GIVI+1 */
float delay; /* vertical delay estimate (m) */
} sbsigp_t;
typedef struct { /* SBAS ionospheric correction type */
//gtime_t t0; /* correction time */
double t0;
bool valid;
short lat,lon; /* latitude/longitude (deg) */
short give; /* GIVI+1 */
float delay; /* vertical delay estimate (m) */
} sbsigp_t;
typedef struct { /* IGP band type */
short x; /* longitude/latitude (deg) */
const short *y; /* latitudes/longitudes (deg) */
unsigned char bits; /* IGP mask start bit */
unsigned char bite; /* IGP mask end bit */
} sbsigpband_t;
typedef struct { /* IGP band type */
short x; /* longitude/latitude (deg) */
const short *y; /* latitudes/longitudes (deg) */
unsigned char bits; /* IGP mask start bit */
unsigned char bite; /* IGP mask end bit */
} sbsigpband_t;
typedef struct { /* SBAS ionospheric corrections type */
int iodi; /* IODI (issue of date ionos corr) */
int nigp; /* number of igps */
sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
} sbsion_t;
typedef struct { /* SBAS ionospheric corrections type */
int iodi; /* IODI (issue of date ionos corr) */
int nigp; /* number of igps */
sbsigp_t igp[MAXNIGP]; /* ionospheric correction */
} sbsion_t;
/*
* idicators
*/
/*
* indicators
*/
typedef struct { /* SBAS ephemeris type */
int sat; /* satellite number */
//gtime_t t0; /* reference epoch time (GPST) */
int t0;
//gtime_t tof; /* time of message frame (GPST) */
double tof;
int sva; /* SV accuracy (URA index) */
int svh; /* SV health (0:ok) */
double pos[3]; /* satellite position (m) (ecef) */
double vel[3]; /* satellite velocity (m/s) (ecef) */
double acc[3]; /* satellite acceleration (m/s^2) (ecef) */
double af0,af1; /* satellite clock-offset/drift (s,s/s) */
} seph_t;
typedef struct { /* SBAS ephemeris type */
int sat; /* satellite number */
//gtime_t t0; /* reference epoch time (GPST) */
int t0;
//gtime_t tof; /* time of message frame (GPST) */
double tof;
int sva; /* SV accuracy (URA index) */
int svh; /* SV health (0:ok) */
double pos[3]; /* satellite position (m) (ecef) */
double vel[3]; /* satellite velocity (m/s) (ecef) */
double acc[3]; /* satellite acceleration (m/s^2) (ecef) */
double af0,af1; /* satellite clock-offset/drift (s,s/s) */
} seph_t;
typedef struct { /* navigation data type */
//int n,nmax; /* number of broadcast ephemeris */
//int ng,ngmax; /* number of glonass ephemeris */
//int ns,nsmax; /* number of sbas ephemeris */
//int ne,nemax; /* number of precise ephemeris */
//int nc,ncmax; /* number of precise clock */
//int na,namax; /* number of almanac data */
//int nt,ntmax; /* number of tec grid data */
//int nn,nnmax; /* number of stec grid data */
//eph_t *eph; /* GPS/QZS/GAL ephemeris */
//geph_t *geph; /* GLONASS ephemeris */
seph_t seph[2*NSATSBS]; /* SBAS ephemeris */
// peph_t *peph; /* precise ephemeris */
// pclk_t *pclk; /* precise clock */
// alm_t *alm; /* almanac data */
// tec_t *tec; /* tec grid data */
// stec_t *stec; /* stec grid data */
// erp_t erp; /* earth rotation parameters */
typedef struct { /* navigation data type */
//int n,nmax; /* number of broadcast ephemeris */
//int ng,ngmax; /* number of glonass ephemeris */
//int ns,nsmax; /* number of sbas ephemeris */
//int ne,nemax; /* number of precise ephemeris */
//int nc,ncmax; /* number of precise clock */
//int na,namax; /* number of almanac data */
//int nt,ntmax; /* number of tec grid data */
//int nn,nnmax; /* number of stec grid data */
//eph_t *eph; /* GPS/QZS/GAL ephemeris */
//geph_t *geph; /* GLONASS ephemeris */
seph_t seph[2*NSATSBS]; /* SBAS ephemeris */
// peph_t *peph; /* precise ephemeris */
// pclk_t *pclk; /* precise clock */
// alm_t *alm; /* almanac data */
// tec_t *tec; /* tec grid data */
// stec_t *stec; /* stec grid data */
// erp_t erp; /* earth rotation parameters */
//double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */
//double utc_glo[4]; /* GLONASS UTC GPS time parameters */
//double utc_gal[4]; /* Galileo UTC GPS time parameters */
//double utc_qzs[4]; /* QZS UTC GPS time parameters */
//double utc_cmp[4]; /* BeiDou UTC parameters */
//double utc_sbs[4]; /* SBAS UTC parameters */
//double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
//double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */
//double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
//double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
//int leaps; /* leap seconds (s) */
//double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
//double cbias[MAXSAT][3]; /* code bias (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
//double wlbias[MAXSAT]; /* wide-lane bias (cycle) */
//double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */
//char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
// pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
sbssat_t sbssat; /* SBAS satellite corrections */
sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
// dgps_t dgps[MAXSAT]; /* DGPS corrections */
// ssr_t ssr[MAXSAT]; /* SSR corrections */
// lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
// lexion_t lexion; /* LEX ionosphere correction */
} nav_t;
//double utc_gps[4]; /* GPS delta-UTC parameters {A0,A1,T,W} */
//double utc_glo[4]; /* GLONASS UTC GPS time parameters */
//double utc_gal[4]; /* Galileo UTC GPS time parameters */
//double utc_qzs[4]; /* QZS UTC GPS time parameters */
//double utc_cmp[4]; /* BeiDou UTC parameters */
//double utc_sbs[4]; /* SBAS UTC parameters */
//double ion_gps[8]; /* GPS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
//double ion_gal[4]; /* Galileo iono model parameters {ai0,ai1,ai2,0} */
//double ion_qzs[8]; /* QZSS iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
//double ion_cmp[8]; /* BeiDou iono model parameters {a0,a1,a2,a3,b0,b1,b2,b3} */
//int leaps; /* leap seconds (s) */
//double lam[MAXSAT][NFREQ]; /* carrier wave lengths (m) */
//double cbias[MAXSAT][3]; /* code bias (0:p1-p2,1:p1-c1,2:p2-c2) (m) */
//double wlbias[MAXSAT]; /* wide-lane bias (cycle) */
//double glo_cpbias[4]; /* glonass code-phase bias {1C,1P,2C,2P} (m) */
//char glo_fcn[MAXPRNGLO+1]; /* glonass frequency channel number + 8 */
// pcv_t pcvs[MAXSAT]; /* satellite antenna pcv */
sbssat_t sbssat; /* SBAS satellite corrections */
sbsion_t sbsion[MAXBAND+1]; /* SBAS ionosphere corrections */
// dgps_t dgps[MAXSAT]; /* DGPS corrections */
// ssr_t ssr[MAXSAT]; /* SSR corrections */
// lexeph_t lexeph[MAXSAT]; /* LEX ephemeris */
// lexion_t lexion; /* LEX ionosphere correction */
} nav_t;
//// common
//// 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 */
int level_trace; /* level of trace */
unsigned int tick_trace; /* tick time at traceopen (ms) */
FILE *fp_trace; /* file pointer of trace */
int level_trace; /* level of trace */
unsigned int tick_trace; /* tick time at traceopen (ms) */
void trace(int level, const char *format, ...);
void trace(int level, const char *format, ...);
/* satellite system+prn/slot number to satellite number ------------------------
* convert satellite system+prn/slot number to satellite number
* args : int sys I satellite system (SYS_GPS,SYS_GLO,...)
* int prn I satellite prn/slot number
* return : satellite number (0:error)
*-----------------------------------------------------------------------------*/
int satno(int sys, int prn);
/* extract unsigned/signed bits ------------------------------------------------
* extract unsigned/signed bits from byte data
* args : unsigned char *buff I byte data
* int pos I bit position from start of data (bits)
* int len I bit length (bits) (len<=32)
* return : extracted unsigned/signed bits
*-----------------------------------------------------------------------------*/
unsigned int getbitu(const unsigned char *buff, int pos, int len);
int getbits(const unsigned char *buff, int pos, int len);
/* satellite system+prn/slot number to satellite number ------------------------
* convert satellite system+prn/slot number to satellite number
* args : int sys I satellite system (SYS_GPS,SYS_GLO,...)
* int prn I satellite prn/slot number
* return : satellite number (0:error)
*-----------------------------------------------------------------------------*/
int satno(int sys, int prn);
/* extract unsigned/signed bits ------------------------------------------------
* extract unsigned/signed bits from byte data
* args : unsigned char *buff I byte data
* int pos I bit position from start of data (bits)
* int len I bit length (bits) (len<=32)
* return : extracted unsigned/signed bits
*-----------------------------------------------------------------------------*/
unsigned int getbitu(const unsigned char *buff, int pos, int len);
int getbits(const unsigned char *buff, int pos, int len);
/* convert calendar day/time to time -------------------------------------------
* convert calendar day/time to gtime_t struct
* args : double *ep I day/time {year,month,day,hour,min,sec}
* return : gtime_t struct
* notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
gtime_t epoch2time(const double *ep);
/* time difference -------------------------------------------------------------
* difference between gtime_t structs
* args : gtime_t t1,t2 I gtime_t structs
* return : time difference (t1-t2) (s)
*-----------------------------------------------------------------------------*/
double timediff(gtime_t t1, gtime_t t2);
/* gps time to time ------------------------------------------------------------
* convert week and tow in gps time to gtime_t struct
* args : int week I week number in gps time
* double sec I time of week in gps time (s)
* return : gtime_t struct
*-----------------------------------------------------------------------------*/
gtime_t gpst2time(int week, double sec);
/* convert calendar day/time to time -------------------------------------------
* convert calendar day/time to gtime_t struct
* args : double *ep I day/time {year,month,day,hour,min,sec}
* return : gtime_t struct
* notes : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
gtime_t epoch2time(const double *ep);
/* time difference -------------------------------------------------------------
* difference between gtime_t structs
* args : gtime_t t1,t2 I gtime_t structs
* return : time difference (t1-t2) (s)
*-----------------------------------------------------------------------------*/
double timediff(gtime_t t1, gtime_t t2);
/* gps time to time ------------------------------------------------------------
* convert week and tow in gps time to gtime_t struct
* args : int week I week number in gps time
* double sec I time of week in gps time (s)
* return : gtime_t struct
*-----------------------------------------------------------------------------*/
gtime_t gpst2time(int week, double sec);
////// sbas.c
////// sbas.c
/* sbas igp definition -------------------------------------------------------*/
static const short
x1[],
x2[],
x3[],
x4[],
x5[],
x6[],
x7[],
x8[];
/* sbas igp definition -------------------------------------------------------*/
static const short
x1[],
x2[],
x3[],
x4[],
x5[],
x6[],
x7[],
x8[];
static const sbsigpband_t igpband1[9][8];/* band 0-8 */
static const sbsigpband_t igpband2[2][5];/* band 9-10 */
static const sbsigpband_t igpband1[9][8]; /* band 0-8 */
static const sbsigpband_t igpband2[2][5]; /* band 9-10 */
/* decode type 1: prn masks --------------------------------------------------*/
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 2-5,0: fast corrections ---------------------------------------*/
int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 6: integrity info ---------------------------------------------*/
int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 7: fast correction degradation factor -------------------------*/
int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 9: geo navigation message -------------------------------------*/
int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav);
/* decode type 18: ionospheric grid point masks ------------------------------*/
int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion);
/* decode half long term correction (vel code=0) -----------------------------*/
int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode half long term correction (vel code=1) -----------------------------*/
int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode half long term correction ------------------------------------------*/
int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode type 24: mixed fast/long term correction ---------------------------*/
int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 25: long term satellite error correction ----------------------*/
int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 26: ionospheric deley corrections -----------------------------*/
int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion);
/* update sbas corrections -----------------------------------------------------
* update sbas correction parameters in navigation data with a sbas message
* args : sbsmg_t *msg I sbas message
* nav_t *nav IO navigation data
* return : message type (-1: error or not supported type)
* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t)
* seph[prn-MINPRNSBS+1] : sat prn current epehmeris
* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris
*-----------------------------------------------------------------------------*/
int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav);
/* decode type 1: prn masks --------------------------------------------------*/
int decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 2-5,0: fast corrections ---------------------------------------*/
int decode_sbstype2(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 6: integrity info ---------------------------------------------*/
int decode_sbstype6(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 7: fast correction degradation factor -------------------------*/
int decode_sbstype7(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 9: geo navigation message -------------------------------------*/
int decode_sbstype9(const sbsmsg_t *msg, nav_t *nav);
/* decode type 18: ionospheric grid point masks ------------------------------*/
int decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion);
/* decode half long term correction (vel code=0) -----------------------------*/
int decode_longcorr0(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode half long term correction (vel code=1) -----------------------------*/
int decode_longcorr1(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode half long term correction ------------------------------------------*/
int decode_longcorrh(const sbsmsg_t *msg, int p, sbssat_t *sbssat);
/* decode type 24: mixed fast/long term correction ---------------------------*/
int decode_sbstype24(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 25: long term satellite error correction ----------------------*/
int decode_sbstype25(const sbsmsg_t *msg, sbssat_t *sbssat);
/* decode type 26: ionospheric deley corrections -----------------------------*/
int decode_sbstype26(const sbsmsg_t *msg, sbsion_t *sbsion);
/* update sbas corrections -----------------------------------------------------
* update sbas correction parameters in navigation data with a sbas message
* args : sbsmg_t *msg I sbas message
* nav_t *nav IO navigation data
* return : message type (-1: error or not supported type)
* notes : nav->seph must point to seph[NSATSBS*2] (array of seph_t)
* seph[prn-MINPRNSBS+1] : sat prn current epehmeris
* seph[prn-MINPRNSBS+1+MAXPRNSBS]: sat prn previous epehmeris
*-----------------------------------------------------------------------------*/
int sbsupdatecorr(const sbsmsg_t *msg, nav_t *nav);
void prn_mask_changed();
bool is_rtklib_sat_correction_valid(int sat);
void igp_mask_changed(int band);
// RTKLIB SBAS telemetry data representation
nav_t d_nav;
void prn_mask_changed();
bool is_rtklib_sat_correction_valid(int sat);
void igp_mask_changed(int band);
// RTKLIB SBAS telemetry data representation
nav_t d_nav;
};
#endif

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)
{
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;
VLOG(FLOW) << "<<R>> to gps time: time_stamp_sec=" << time_stamp_sec << " gps_week=" << gps_week << " gps_sec=" << gps_sec;
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:
* - only relative time (sample stamp) is known
* - only absolute time (gps time) is known
* - absolute and relative time and their relation is known
*/
class Sbas_Time
{
public:
enum Sbas_Time_State {RELATIVE, /*ABSOLUTE,*/ RELATED, UNDEFINED};
Sbas_Time()
@ -177,10 +175,10 @@ public:
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_related() {return e_state == RELATED;}
Sbas_Time_State get_state() {return e_state;}
bool is_related() { return e_state == RELATED; }
Sbas_Time_State get_state() { return e_state; }
private:
Sbas_Time_State e_state;
@ -190,4 +188,4 @@ private:
};
#endif /* SBAS_TIME_H_ */
#endif /* GNSS_SDR_SBAS_TIME_H_ */