mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-07-16 08:53:11 +00:00
Code cleaning
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
parent
5b2abb62c2
commit
aa5b531cd3
@ -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
|
||||
*
|
||||
|
||||
@ -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";
|
||||
|
||||
@ -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
|
||||
*/
|
||||
@ -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}};
|
||||
|
||||
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)
|
||||
{
|
||||
@ -415,7 +402,6 @@ get_frame_candidates(const std::vector<int> bits, std::vector<std::pair<int,std:
|
||||
*candidate_bit_it = *candidate_bit_it == 0 ? 1:0;
|
||||
}
|
||||
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,24 +459,22 @@ get_valid_frames(const std::vector<msg_candiate_int_t> msg_candidates, std::vect
|
||||
}
|
||||
}
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::crc_verifier::
|
||||
zerropad_back_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes)
|
||||
|
||||
|
||||
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::crc_verifier::zerropad_back_and_convert_to_bytes(const std::vector<int> msg_candidate, std::vector<unsigned char> &bytes)
|
||||
{
|
||||
std::stringstream ss;
|
||||
const size_t bits_per_byte = 8;
|
||||
unsigned char byte = 0;
|
||||
|
||||
VLOG(LMORE) << "zerropad_back_and_convert_to_bytes():" << byte;
|
||||
|
||||
for (std::vector<int>::const_iterator candidate_bit_it = msg_candidate.begin(); candidate_bit_it < msg_candidate.end(); ++candidate_bit_it)
|
||||
{
|
||||
int idx_bit = candidate_bit_it - msg_candidate.begin();
|
||||
int bit_pos_in_current_byte = (bits_per_byte - 1) - (idx_bit % bits_per_byte);
|
||||
|
||||
byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
|
||||
|
||||
ss << *candidate_bit_it;
|
||||
|
||||
if (idx_bit % bits_per_byte == bits_per_byte - 1)
|
||||
{
|
||||
bytes.push_back(byte);
|
||||
@ -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);
|
||||
|
||||
byte |= (unsigned char)(*candidate_bit_it) << bit_pos_in_current_byte;
|
||||
|
||||
ss << *candidate_bit_it;
|
||||
|
||||
if (idx_bit % bits_per_byte == bits_per_byte - 1)
|
||||
{
|
||||
bytes.push_back(byte);
|
||||
VLOG(LMORE) << ss.str() << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte; ss.str("");
|
||||
byte = 0;
|
||||
}
|
||||
|
||||
idx_bit++;
|
||||
}
|
||||
VLOG(LMORE) << " -> byte=" << std::setw(2) << std::setfill('0') << std::hex << (unsigned int)byte
|
||||
<< std::setfill(' ') << std::resetiosflags(std::ios::hex);
|
||||
}
|
||||
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::set_raw_msg_queue(concurrent_queue<Sbas_Raw_Msg> *raw_msg_queue)
|
||||
{
|
||||
sbas_telemetry_data.set_raw_msg_queue(raw_msg_queue);
|
||||
}
|
||||
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::set_iono_queue(concurrent_queue<Sbas_Ionosphere_Correction> *iono_queue)
|
||||
{
|
||||
sbas_telemetry_data.set_iono_queue(iono_queue);
|
||||
}
|
||||
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::set_sat_corr_queue(concurrent_queue<Sbas_Satellite_Correction> *sat_corr_queue)
|
||||
{
|
||||
sbas_telemetry_data.set_sat_corr_queue(sat_corr_queue);
|
||||
}
|
||||
|
||||
|
||||
void sbas_l1_telemetry_decoder_cc::set_ephemeris_queue(concurrent_queue<Sbas_Ephemeris> *ephemeris_queue)
|
||||
{
|
||||
sbas_telemetry_data.set_ephemeris_queue(ephemeris_queue);
|
||||
|
||||
@ -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);
|
||||
|
||||
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;
|
||||
};
|
||||
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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
|
||||
@ -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,6 +207,8 @@ void Sbas_Ionosphere_Correction::ecef2enu(const double *pos, const double *r, do
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double Sbas_Ionosphere_Correction::satazel(const double *pos, const double *e, double *azel)
|
||||
{
|
||||
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)
|
||||
@ -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);
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -63,6 +63,9 @@ private:
|
||||
ar & d_delay;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
struct Igp_Band
|
||||
{
|
||||
//int d_iodi;
|
||||
@ -104,53 +107,66 @@ private:
|
||||
// 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)
|
||||
|
||||
/*!
|
||||
* \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);
|
||||
/* multiply matrix -----------------------------------------------------------*/
|
||||
|
||||
|
||||
|
||||
/*!
|
||||
* \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);
|
||||
/* ecef to local coordinate transfromation matrix ------------------------------
|
||||
* compute ecef to local coordinate transfromation matrix
|
||||
* args : double *pos I geodetic position {lat,lon} (rad)
|
||||
/*!
|
||||
* \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 : matirix stored by column-major order (fortran convention)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
* notes : matrix 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)
|
||||
|
||||
/*!
|
||||
* \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);
|
||||
/* satellite azimuth/elevation angle -------------------------------------------
|
||||
* compute satellite azimuth/elevation angle
|
||||
* args : double *pos I geodetic position {lat,lon,h} (rad,m)
|
||||
|
||||
/*!
|
||||
* \brief Compute satellite azimuth/elevation angle
|
||||
* params : double *pos I geodetic position {lat,lon,h} (rad,m)
|
||||
* double *e I receiver-to-satellilte unit vevtor (ecef)
|
||||
* double *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 debug trace functions
|
||||
*/
|
||||
void trace(int level, const char *format, ...);
|
||||
|
||||
/* time difference -------------------------------------------------------------
|
||||
* difference between gtime_t structs
|
||||
* args : gtime_t t1,t2 I gtime_t structs
|
||||
* return : time difference (t1-t2) (s)
|
||||
*-----------------------------------------------------------------------------*/
|
||||
//double timediff(gtime_t t1, gtime_t t2);
|
||||
/* ionospheric pierce point position -------------------------------------------
|
||||
* compute ionospheric pierce point (ipp) position and slant factor
|
||||
* args : double *pos I receiver position {lat,lon,h} (rad,m)
|
||||
|
||||
/*!
|
||||
* \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)
|
||||
@ -161,13 +177,20 @@ private:
|
||||
*-----------------------------------------------------------------------------*/
|
||||
double ionppp(const double *pos, const double *azel, double re,
|
||||
double hion, double *posp);
|
||||
/* variance of ionosphere correction (give=GIVEI+1) --------------------------*/
|
||||
|
||||
/*!
|
||||
* \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
|
||||
|
||||
/*!
|
||||
* \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)
|
||||
@ -177,23 +200,20 @@ private:
|
||||
* 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);
|
||||
|
||||
|
||||
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);
|
||||
|
||||
private:
|
||||
friend class boost::serialization::access;
|
||||
template<class Archive>
|
||||
void serialize(Archive& ar, const unsigned int version){ar & d_bands;}
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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,66 +96,58 @@ 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;
|
||||
}
|
||||
|
||||
|
||||
#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)
|
||||
{
|
||||
@ -163,6 +157,8 @@ double Sbas_Satellite_Correction::varfcorr(int udre)
|
||||
};
|
||||
return 0 < udre && udre <= 14 ? var[udre - 1] : 0.0;
|
||||
}
|
||||
|
||||
|
||||
/* fast correction degradation -----------------------------------------------*/
|
||||
double Sbas_Satellite_Correction::degfcorr(int ai)
|
||||
{
|
||||
@ -173,21 +169,20 @@ double Sbas_Satellite_Correction::degfcorr(int ai)
|
||||
return 0 < ai && ai <= 15 ? degf[ai] : 0.0058;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* long term correction ------------------------------------------------------*/
|
||||
int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, double *ddts)
|
||||
{
|
||||
double t = 0.0;
|
||||
int i;
|
||||
|
||||
Long_Term_Correction lcorr = d_long_term_correction;
|
||||
|
||||
trace(3, "sbslongcorr: prn=%2d", this->d_prn);
|
||||
|
||||
// if (p->sat!=sat||p->lcorr.t0.time==0) continue;
|
||||
|
||||
// compute time of applicability
|
||||
if(d_long_term_correction.i_vel == 1)
|
||||
{ // time of applicability is the one sent, i.e., tapp
|
||||
{
|
||||
// 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
|
||||
@ -197,47 +192,40 @@ int Sbas_Satellite_Correction::sbslongcorr(double time_stamp, double *drs, doubl
|
||||
sbssat->sat[n-1].lcorr.t0 = gpst2time(msg->week, msg->tow + t);*/
|
||||
}
|
||||
else
|
||||
{ // time of applicability is time of reception
|
||||
{
|
||||
// time of applicability is time of reception
|
||||
t = time_stamp - lcorr.d_trx; // should not have any impact if vel==0 since d_dvel and d_daf1 are zero, is only used to check outdating
|
||||
}
|
||||
|
||||
//t=time_stamp-lcorr.d_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)
|
||||
{
|
||||
@ -263,7 +251,6 @@ int Sbas_Satellite_Correction::sbsfastcorr(double time_stamp, double *prc, doubl
|
||||
}
|
||||
#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;
|
||||
}
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
@ -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) */
|
||||
|
||||
@ -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);
|
||||
@ -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;
|
||||
@ -580,7 +560,7 @@ const Sbas_Telemetry_Data::sbsigpband_t Sbas_Telemetry_Data::igpband2[2][5]={ /*
|
||||
int Sbas_Telemetry_Data::decode_sbstype1(const sbsmsg_t *msg, sbssat_t *sbssat)
|
||||
{
|
||||
int i, n, sat;
|
||||
// see figure A-6: i coresponds to bit number (and for the GPS satellites is identically to the PRN), n to the PRN mask number
|
||||
// see figure A-6: i corresponds to bit number (and for the GPS satellites is identically to the PRN), n to the PRN mask number
|
||||
|
||||
trace(4,"decode_sbstype1:");
|
||||
|
||||
@ -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)
|
||||
{
|
||||
@ -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);
|
||||
|
||||
for (i=0; i < sbssat->nsat && i < MAXSAT; i++)
|
||||
{
|
||||
sbssat->sat[i].fcorr.ai = getbitu(msg->msg, 22 + i*4, 4);
|
||||
@ -763,7 +741,8 @@ int Sbas_Telemetry_Data::decode_sbstype18(const sbsmsg_t *msg, sbsion_t *sbsion)
|
||||
|
||||
short iodi_new = (short)getbitu(msg->msg, 22, 2);
|
||||
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;
|
||||
@ -858,7 +837,8 @@ 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);
|
||||
@ -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;
|
||||
}
|
||||
|
||||
|
||||
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++)
|
||||
|
||||
@ -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
|
||||
{
|
||||
@ -101,24 +101,16 @@ private:
|
||||
|
||||
|
||||
/*
|
||||
* \brief holds an updated set of the telemetry data received from SBAS
|
||||
* \brief Holds an updated set of the telemetry data received from SBAS
|
||||
*/
|
||||
|
||||
class Sbas_Telemetry_Data
|
||||
{
|
||||
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
|
||||
@ -293,9 +285,9 @@ private:
|
||||
|
||||
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
|
||||
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) */
|
||||
@ -339,7 +331,7 @@ private:
|
||||
} sbsion_t;
|
||||
|
||||
/*
|
||||
* idicators
|
||||
* indicators
|
||||
*/
|
||||
|
||||
typedef struct { /* SBAS ephemeris type */
|
||||
@ -508,7 +500,6 @@ private:
|
||||
|
||||
// RTKLIB SBAS telemetry data representation
|
||||
nav_t d_nav;
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -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()
|
||||
@ -190,4 +188,4 @@ private:
|
||||
};
|
||||
|
||||
|
||||
#endif /* SBAS_TIME_H_ */
|
||||
#endif /* GNSS_SDR_SBAS_TIME_H_ */
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user